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