Annotation of src/usr.bin/sndiod/dsp.c, Revision 1.2
1.2 ! ratchov 1: /* $OpenBSD: dsp.c,v 1.1 2012/11/23 07:03:28 ratchov Exp $ */
1.1 ratchov 2: /*
3: * Copyright (c) 2008-2012 Alexandre Ratchov <alex@caoua.org>
4: *
5: * Permission to use, copy, modify, and distribute this software for any
6: * purpose with or without fee is hereby granted, provided that the above
7: * copyright notice and this permission notice appear in all copies.
8: *
9: * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
10: * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
11: * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
12: * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
13: * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
14: * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
15: * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
16: */
17: #include <string.h>
18: #include "defs.h"
19: #include "dsp.h"
20: #include "utils.h"
21:
22: int aparams_ctltovol[128] = {
23: 0,
24: 256, 266, 276, 287, 299, 310, 323, 335,
25: 348, 362, 376, 391, 406, 422, 439, 456,
26: 474, 493, 512, 532, 553, 575, 597, 621,
27: 645, 670, 697, 724, 753, 782, 813, 845,
28: 878, 912, 948, 985, 1024, 1064, 1106, 1149,
29: 1195, 1241, 1290, 1341, 1393, 1448, 1505, 1564,
30: 1625, 1689, 1756, 1825, 1896, 1971, 2048, 2128,
31: 2212, 2299, 2389, 2483, 2580, 2682, 2787, 2896,
32: 3010, 3128, 3251, 3379, 3511, 3649, 3792, 3941,
33: 4096, 4257, 4424, 4598, 4778, 4966, 5161, 5363,
34: 5574, 5793, 6020, 6256, 6502, 6757, 7023, 7298,
35: 7585, 7883, 8192, 8514, 8848, 9195, 9556, 9931,
36: 10321, 10726, 11148, 11585, 12040, 12513, 13004, 13515,
37: 14045, 14596, 15170, 15765, 16384, 17027, 17696, 18390,
38: 19112, 19863, 20643, 21453, 22295, 23170, 24080, 25025,
39: 26008, 27029, 28090, 29193, 30339, 31530, 32768
40: };
41:
42: /*
43: * Generate a string corresponding to the encoding in par,
44: * return the length of the resulting string.
45: */
46: int
47: aparams_enctostr(struct aparams *par, char *ostr)
48: {
49: char *p = ostr;
50:
51: *p++ = par->sig ? 's' : 'u';
52: if (par->bits > 9)
53: *p++ = '0' + par->bits / 10;
54: *p++ = '0' + par->bits % 10;
55: if (par->bps > 1) {
56: *p++ = par->le ? 'l' : 'b';
57: *p++ = 'e';
58: if (par->bps != APARAMS_BPS(par->bits) ||
59: par->bits < par->bps * 8) {
60: *p++ = par->bps + '0';
61: if (par->bits < par->bps * 8) {
62: *p++ = par->msb ? 'm' : 'l';
63: *p++ = 's';
64: *p++ = 'b';
65: }
66: }
67: }
68: *p++ = '\0';
69: return p - ostr - 1;
70: }
71:
72: /*
73: * Parse an encoding string, examples: s8, u8, s16, s16le, s24be ...
74: * set *istr to the char following the encoding. Return the number
75: * of bytes consumed.
76: */
77: int
78: aparams_strtoenc(struct aparams *par, char *istr)
79: {
80: char *p = istr;
81: int i, sig, bits, le, bps, msb;
82:
83: #define IS_SEP(c) \
84: (((c) < 'a' || (c) > 'z') && \
85: ((c) < 'A' || (c) > 'Z') && \
86: ((c) < '0' || (c) > '9'))
87:
88: /*
89: * get signedness
90: */
91: if (*p == 's') {
92: sig = 1;
93: } else if (*p == 'u') {
94: sig = 0;
95: } else
96: return 0;
97: p++;
98:
99: /*
100: * get number of bits per sample
101: */
102: bits = 0;
103: for (i = 0; i < 2; i++) {
104: if (*p < '0' || *p > '9')
105: break;
106: bits = (bits * 10) + *p - '0';
107: p++;
108: }
109: if (bits < BITS_MIN || bits > BITS_MAX)
110: return 0;
111: bps = APARAMS_BPS(bits);
112: msb = 1;
113: le = ADATA_LE;
114:
115: /*
116: * get (optional) endianness
117: */
118: if (p[0] == 'l' && p[1] == 'e') {
119: le = 1;
120: p += 2;
121: } else if (p[0] == 'b' && p[1] == 'e') {
122: le = 0;
123: p += 2;
124: } else if (IS_SEP(*p)) {
125: goto done;
126: } else
127: return 0;
128:
129: /*
130: * get (optional) number of bytes
131: */
132: if (*p >= '0' && *p <= '9') {
133: bps = *p - '0';
134: if (bps < (bits + 7) / 8 ||
135: bps > (BITS_MAX + 7) / 8)
136: return 0;
137: p++;
138:
139: /*
140: * get (optional) alignement
141: */
142: if (p[0] == 'm' && p[1] == 's' && p[2] == 'b') {
143: msb = 1;
144: p += 3;
145: } else if (p[0] == 'l' && p[1] == 's' && p[2] == 'b') {
146: msb = 0;
147: p += 3;
148: } else if (IS_SEP(*p)) {
149: goto done;
150: } else
151: return 0;
152: } else if (!IS_SEP(*p))
153: return 0;
154:
155: done:
156: par->msb = msb;
157: par->sig = sig;
158: par->bits = bits;
159: par->bps = bps;
160: par->le = le;
161: return p - istr;
162: }
163:
164: /*
165: * Initialise parameters structure with the defaults natively supported
166: * by the machine.
167: */
168: void
169: aparams_init(struct aparams *par)
170: {
171: par->bps = sizeof(adata_t);
172: par->bits = ADATA_BITS;
173: par->le = ADATA_LE;
174: par->sig = 1;
175: par->msb = 0;
176: }
177:
178: /*
179: * log the given format/channels/encoding
180: */
181: void
182: aparams_log(struct aparams *par)
183: {
184: char enc[ENCMAX];
185:
186: aparams_enctostr(par, enc);
187: log_puts(enc);
188: }
189:
190: /*
191: * return true if encoding corresponds to what we store in adata_t
192: */
193: int
194: aparams_native(struct aparams *par)
195: {
196: return par->bps == sizeof(adata_t) && par->bits == ADATA_BITS &&
197: (par->bps == 1 || par->le == ADATA_LE) &&
198: (par->bits == par->bps * 8 || !par->msb);
199: }
200:
201: /*
202: * resample the given number of frames
203: */
204: int
205: resamp_do(struct resamp *p, adata_t *in, adata_t *out, int todo)
206: {
207: unsigned int nch;
208: adata_t *idata;
209: unsigned int oblksz;
210: unsigned int ifr;
211: int s, ds, diff;
212: adata_t *odata;
213: unsigned int iblksz;
214: unsigned int ofr;
215: unsigned int c;
216: adata_t *ctxbuf, *ctx;
217: unsigned int ctx_start;
218:
219: /*
220: * Partially copy structures into local variables, to avoid
221: * unnecessary indirections; this also allows the compiler to
222: * order local variables more "cache-friendly".
223: */
224: idata = in;
225: odata = out;
226: diff = p->diff;
227: iblksz = p->iblksz;
228: oblksz = p->oblksz;
229: ctxbuf = p->ctx;
230: ctx_start = p->ctx_start;
231: nch = p->nch;
232: ifr = todo;
233: ofr = oblksz;
234:
235: /*
236: * Start conversion.
237: */
238: #ifdef DEBUG
239: if (log_level >= 4) {
240: log_puts("resamp: copying ");
241: log_puti(todo);
242: log_puts(" frames, diff = ");
243: log_putu(diff);
244: log_puts("\n");
245: }
246: #endif
247: for (;;) {
248: if (diff < 0) {
249: if (ifr == 0)
250: break;
251: ctx_start ^= 1;
252: ctx = ctxbuf + ctx_start;
253: for (c = nch; c > 0; c--) {
254: *ctx = *idata++;
255: ctx += RESAMP_NCTX;
256: }
257: diff += oblksz;
258: ifr--;
259: } else if (diff > 0) {
260: if (ofr == 0)
261: break;
262: ctx = ctxbuf;
263: for (c = nch; c > 0; c--) {
264: s = ctx[ctx_start];
265: ds = ctx[ctx_start ^ 1] - s;
266: ctx += RESAMP_NCTX;
267: *odata++ = s + ADATA_MULDIV(ds, diff, oblksz);
268: }
269: diff -= iblksz;
270: ofr--;
271: } else {
272: if (ifr == 0 || ofr == 0)
273: break;
274: ctx = ctxbuf + ctx_start;
275: for (c = nch; c > 0; c--) {
276: *odata++ = *ctx;
277: ctx += RESAMP_NCTX;
278: }
279: ctx_start ^= 1;
280: ctx = ctxbuf + ctx_start;
281: for (c = nch; c > 0; c--) {
282: *ctx = *idata++;
283: ctx += RESAMP_NCTX;
284: }
285: diff -= iblksz;
286: diff += oblksz;
287: ifr--;
288: ofr--;
289: }
290: }
291: p->diff = diff;
292: p->ctx_start = ctx_start;
293: return oblksz - ofr;
294: }
295:
296: /*
297: * initialize resampler with ibufsz/obufsz factor and "nch" channels
298: */
299: void
300: resamp_init(struct resamp *p, unsigned int iblksz, unsigned int oblksz, int nch)
301: {
302: unsigned int i;
303:
304: p->iblksz = iblksz;
305: p->oblksz = oblksz;
306: p->diff = 0;
307: p->idelta = 0;
308: p->odelta = 0;
309: p->nch = nch;
310: p->ctx_start = 0;
311: for (i = 0; i < NCHAN_MAX * RESAMP_NCTX; i++)
312: p->ctx[i] = 0;
313: #ifdef DEBUG
314: if (log_level >= 3) {
315: log_puts("resamp: ");
316: log_putu(iblksz);
317: log_puts("/");
318: log_putu(oblksz);
319: log_puts("\n");
320: }
321: #endif
322: }
323:
324: /*
325: * encode "todo" frames from native to foreign encoding
326: */
327: void
328: enc_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
329: {
330: unsigned int f;
331: adata_t *idata;
332: int s;
333: unsigned int oshift;
334: int osigbit;
335: unsigned int obps;
336: unsigned int i;
337: unsigned char *odata;
338: int obnext;
339: int osnext;
340:
341: #ifdef DEBUG
342: if (log_level >= 4) {
343: log_puts("enc: copying ");
344: log_putu(todo);
345: log_puts(" frames\n");
346: }
347: #endif
348: /*
349: * Partially copy structures into local variables, to avoid
350: * unnecessary indirections; this also allows the compiler to
351: * order local variables more "cache-friendly".
352: */
353: idata = (adata_t *)in;
354: odata = out;
355: oshift = p->shift;
356: osigbit = p->sigbit;
357: obps = p->bps;
358: obnext = p->bnext;
359: osnext = p->snext;
360:
361: /*
362: * Start conversion.
363: */
364: odata += p->bfirst;
365: for (f = todo * p->nch; f > 0; f--) {
366: s = *idata++;
367: s <<= 32 - ADATA_BITS;
368: s >>= oshift;
369: s ^= osigbit;
370: for (i = obps; i > 0; i--) {
371: *odata = (unsigned char)s;
372: s >>= 8;
373: odata += obnext;
374: }
375: odata += osnext;
376: }
377: }
378:
379: /*
380: * store "todo" frames of silence in foreign encoding
381: */
382: void
383: enc_sil_do(struct conv *p, unsigned char *out, int todo)
384: {
385: unsigned int f;
386: int s;
387: int osigbit;
388: unsigned int obps;
389: unsigned int i;
390: unsigned char *odata;
391: int obnext;
392: int osnext;
393:
394: #ifdef DEBUG
395: if (log_level >= 4) {
396: log_puts("enc: silence ");
397: log_putu(todo);
398: log_puts(" frames\n");
399: }
400: #endif
401: /*
402: * Partially copy structures into local variables, to avoid
403: * unnecessary indirections; this also allows the compiler to
404: * order local variables more "cache-friendly".
405: */
406: odata = out;
407: osigbit = p->sigbit;
408: obps = p->bps;
409: obnext = p->bnext;
410: osnext = p->snext;
411:
412: /*
413: * Start conversion.
414: */
415: odata += p->bfirst;
416: for (f = todo * p->nch; f > 0; f--) {
417: s = osigbit;
418: for (i = obps; i > 0; i--) {
419: *odata = (unsigned char)s;
420: s >>= 8;
421: odata += obnext;
422: }
423: odata += osnext;
424: }
425: }
426:
427: /*
428: * initialize encoder from native to foreign encoding
429: */
430: void
431: enc_init(struct conv *p, struct aparams *par, int nch)
432: {
433: p->nch = nch;
434: p->bps = par->bps;
435: p->sigbit = par->sig ? 0 : 1 << (par->bits - 1);
436: if (par->msb) {
437: p->shift = 32 - par->bps * 8;
438: } else {
439: p->shift = 32 - par->bits;
440: }
441: if (!par->le) {
442: p->bfirst = par->bps - 1;
443: p->bnext = -1;
444: p->snext = 2 * par->bps;
445: } else {
446: p->bfirst = 0;
447: p->bnext = 1;
448: p->snext = 0;
449: }
450: #ifdef DEBUG
451: if (log_level >= 3) {
452: log_puts("enc: ");
453: aparams_log(par);
454: log_puts(", ");
455: log_puti(p->nch);
456: log_puts(" channels\n");
457: }
458: #endif
459: }
460:
461: /*
462: * decode "todo" frames from from foreign to native encoding
463: */
464: void
465: dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
466: {
467: unsigned int f;
468: unsigned int ibps;
469: unsigned int i;
470: int s = 0xdeadbeef;
471: unsigned char *idata;
472: int ibnext;
473: int isnext;
474: int isigbit;
475: unsigned int ishift;
476: adata_t *odata;
477:
478: #ifdef DEBUG
479: if (log_level >= 4) {
480: log_puts("dec: copying ");
481: log_putu(todo);
482: log_puts(" frames\n");
483: }
484: #endif
485: /*
486: * Partially copy structures into local variables, to avoid
487: * unnecessary indirections; this also allows the compiler to
488: * order local variables more "cache-friendly".
489: */
490: idata = in;
491: odata = (adata_t *)out;
492: ibps = p->bps;
493: ibnext = p->bnext;
494: isigbit = p->sigbit;
495: ishift = p->shift;
496: isnext = p->snext;
497:
498: /*
499: * Start conversion.
500: */
501: idata += p->bfirst;
502: for (f = todo * p->nch; f > 0; f--) {
503: for (i = ibps; i > 0; i--) {
504: s <<= 8;
505: s |= *idata;
506: idata += ibnext;
507: }
508: idata += isnext;
509: s ^= isigbit;
510: s <<= ishift;
511: s >>= 32 - ADATA_BITS;
512: *odata++ = s;
513: }
514: }
515:
516: /*
517: * initialize decoder from foreign to native encoding
518: */
519: void
520: dec_init(struct conv *p, struct aparams *par, int nch)
521: {
522: p->bps = par->bps;
523: p->sigbit = par->sig ? 0 : 1 << (par->bits - 1);
524: p->nch = nch;
525: if (par->msb) {
526: p->shift = 32 - par->bps * 8;
527: } else {
528: p->shift = 32 - par->bits;
529: }
530: if (par->le) {
531: p->bfirst = par->bps - 1;
532: p->bnext = -1;
533: p->snext = 2 * par->bps;
534: } else {
535: p->bfirst = 0;
536: p->bnext = 1;
537: p->snext = 0;
538: }
539: #ifdef DEBUG
540: if (log_level >= 3) {
541: log_puts("dec: ");
542: aparams_log(par);
543: log_puts(", ");
544: log_puti(p->nch);
545: log_puts(" channels\n");
546: }
547: #endif
548: }
549:
550: /*
551: * mix "todo" input frames on the output with the given volume
552: */
553: void
554: cmap_add(struct cmap *p, void *in, void *out, int vol, int todo)
555: {
556: adata_t *idata, *odata;
557: int i, j, nch, istart, inext, onext, ostart, y, v;
558:
559: #ifdef DEBUG
560: if (log_level >= 4) {
561: log_puts("cmap: adding ");
562: log_puti(todo);
563: log_puts(" frames\n");
564: }
565: #endif
566: idata = in;
567: odata = out;
568: ostart = p->ostart;
569: onext = p->onext;
570: istart = p->istart;
571: inext = p->inext;
572: nch = p->nch;
1.2 ! ratchov 573: v = vol;
1.1 ratchov 574:
575: /*
576: * map/mix input on the output
577: */
578: for (i = todo; i > 0; i--) {
579: odata += ostart;
580: idata += istart;
581: for (j = nch; j > 0; j--) {
582: y = *odata + ADATA_MUL(*idata, v);
583: if (y >= ADATA_UNIT)
584: y = ADATA_UNIT - 1;
585: else if (y < -ADATA_UNIT)
586: y = -ADATA_UNIT;
587: *odata = y;
588: idata++;
589: odata++;
590: }
591: odata += onext;
592: idata += inext;
593: }
594: }
595:
596: /*
597: * overwrite output with "todo" input frames with with the given volume
598: */
599: void
600: cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo)
601: {
602: adata_t *idata, *odata;
603: int i, j, nch, istart, inext, onext, ostart, v;
604:
605: #ifdef DEBUG
606: if (log_level >= 4) {
607: log_puts("cmap: copying ");
608: log_puti(todo);
609: log_puts(" frames\n");
610: }
611: #endif
612: idata = in;
613: odata = out;
614: ostart = p->ostart;
615: onext = p->onext;
616: istart = p->istart;
617: inext = p->inext;
618: nch = p->nch;
619: v = vol;
620:
621: /*
622: * copy to the output buffer
623: */
624: for (i = todo; i > 0; i--) {
625: idata += istart;
626: for (j = ostart; j > 0; j--)
627: *odata++ = 0x1111;
628: for (j = nch; j > 0; j--) {
629: *odata = ADATA_MUL(*idata, v);
630: odata++;
631: idata++;
632: }
633: for (j = onext; j > 0; j--)
634: *odata++ = 0x2222;
635: idata += inext;
636: }
637: }
638:
639: /*
640: * initialize channel mapper, to map a subset of input channel range
641: * into a subset of the output channel range
642: */
643: void
644: cmap_init(struct cmap *p,
645: int imin, int imax, int isubmin, int isubmax,
646: int omin, int omax, int osubmin, int osubmax)
647: {
648: int cmin, cmax;
649:
650: cmin = -NCHAN_MAX;
651: if (osubmin > cmin)
652: cmin = osubmin;
653: if (omin > cmin)
654: cmin = omin;
655: if (isubmin > cmin)
656: cmin = isubmin;
657: if (imin > cmin)
658: cmin = imin;
659:
660: cmax = NCHAN_MAX;
661: if (osubmax < cmax)
662: cmax = osubmax;
663: if (omax < cmax)
664: cmax = omax;
665: if (isubmax < cmax)
666: cmax = isubmax;
667: if (imax < cmax)
668: cmax = imax;
669:
670: p->ostart = cmin - omin;
671: p->onext = omax - cmax;
672: p->istart = cmin - imin;
673: p->inext = imax - cmax;
674: p->nch = cmax - cmin + 1;
675: #ifdef DEBUG
676: if (log_level >= 3) {
677: log_puts("cmap: nch = ");
678: log_puti(p->nch);
679: log_puts(", ostart = ");
680: log_puti(p->ostart);
681: log_puts(", onext = ");
682: log_puti(p->onext);
683: log_puts(", istart = ");
684: log_puti(p->istart);
685: log_puts(", inext= ");
686: log_puti(p->inext);
687: log_puts("\n");
688: }
689: #endif
690: }
691:
692: /*
693: * produce a square tone, for instance with:
694: *
695: * period = round / (220 * round / rate)
696: */
697: int
698: sqrtone(int ctx, adata_t *out, int period, int vol, int todo)
699: {
700: int i;
701:
702: for (i = todo; i > 0; i--) {
703: if (ctx == 0) {
704: vol = -vol;
705: ctx = period / 2;
706: }
707: ctx--;
708: *(out++) += vol;
709: }
710: return ctx;
711: }