Annotation of src/usr.bin/sndiod/dsp.c, Revision 1.4
1.4 ! ratchov 1: /* $OpenBSD: dsp.c,v 1.3 2012/12/07 08:04:58 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 "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: /*
139: * get (optional) alignement
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:
155: par->msb = msb;
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: {
195: return par->bps == sizeof(adata_t) && par->bits == ADATA_BITS &&
196: (par->bps == 1 || par->le == ADATA_LE) &&
197: (par->bits == par->bps * 8 || !par->msb);
198: }
199:
200: /*
201: * resample the given number of frames
202: */
203: int
204: resamp_do(struct resamp *p, adata_t *in, adata_t *out, int todo)
205: {
206: unsigned int nch;
207: adata_t *idata;
208: unsigned int oblksz;
209: unsigned int ifr;
210: int s, ds, diff;
211: adata_t *odata;
212: unsigned int iblksz;
213: unsigned int ofr;
214: unsigned int c;
215: adata_t *ctxbuf, *ctx;
216: unsigned int ctx_start;
217:
218: /*
219: * Partially copy structures into local variables, to avoid
220: * unnecessary indirections; this also allows the compiler to
221: * order local variables more "cache-friendly".
222: */
223: idata = in;
224: odata = out;
225: diff = p->diff;
226: iblksz = p->iblksz;
227: oblksz = p->oblksz;
228: ctxbuf = p->ctx;
229: ctx_start = p->ctx_start;
230: nch = p->nch;
231: ifr = todo;
232: ofr = oblksz;
233:
234: /*
235: * Start conversion.
236: */
237: #ifdef DEBUG
238: if (log_level >= 4) {
239: log_puts("resamp: copying ");
240: log_puti(todo);
241: log_puts(" frames, diff = ");
242: log_putu(diff);
243: log_puts("\n");
244: }
245: #endif
246: for (;;) {
247: if (diff < 0) {
248: if (ifr == 0)
249: break;
250: ctx_start ^= 1;
251: ctx = ctxbuf + ctx_start;
252: for (c = nch; c > 0; c--) {
253: *ctx = *idata++;
254: ctx += RESAMP_NCTX;
255: }
256: diff += oblksz;
257: ifr--;
258: } else if (diff > 0) {
259: if (ofr == 0)
260: break;
261: ctx = ctxbuf;
262: for (c = nch; c > 0; c--) {
263: s = ctx[ctx_start];
264: ds = ctx[ctx_start ^ 1] - s;
265: ctx += RESAMP_NCTX;
266: *odata++ = s + ADATA_MULDIV(ds, diff, oblksz);
267: }
268: diff -= iblksz;
269: ofr--;
270: } else {
271: if (ifr == 0 || ofr == 0)
272: break;
273: ctx = ctxbuf + ctx_start;
274: for (c = nch; c > 0; c--) {
275: *odata++ = *ctx;
276: ctx += RESAMP_NCTX;
277: }
278: ctx_start ^= 1;
279: ctx = ctxbuf + ctx_start;
280: for (c = nch; c > 0; c--) {
281: *ctx = *idata++;
282: ctx += RESAMP_NCTX;
283: }
284: diff -= iblksz;
285: diff += oblksz;
286: ifr--;
287: ofr--;
288: }
289: }
290: p->diff = diff;
291: p->ctx_start = ctx_start;
292: return oblksz - ofr;
293: }
294:
295: /*
296: * initialize resampler with ibufsz/obufsz factor and "nch" channels
297: */
298: void
299: resamp_init(struct resamp *p, unsigned int iblksz, unsigned int oblksz, int nch)
300: {
301: unsigned int i;
302:
303: p->iblksz = iblksz;
304: p->oblksz = oblksz;
305: p->diff = 0;
306: p->idelta = 0;
307: p->odelta = 0;
308: p->nch = nch;
309: p->ctx_start = 0;
310: for (i = 0; i < NCHAN_MAX * RESAMP_NCTX; i++)
311: p->ctx[i] = 0;
312: #ifdef DEBUG
313: if (log_level >= 3) {
314: log_puts("resamp: ");
315: log_putu(iblksz);
316: log_puts("/");
317: log_putu(oblksz);
318: log_puts("\n");
319: }
320: #endif
321: }
322:
323: /*
324: * encode "todo" frames from native to foreign encoding
325: */
326: void
327: enc_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
328: {
329: unsigned int f;
330: adata_t *idata;
331: int s;
332: unsigned int oshift;
333: int osigbit;
334: unsigned int obps;
335: unsigned int i;
336: unsigned char *odata;
337: int obnext;
338: int osnext;
339:
340: #ifdef DEBUG
341: if (log_level >= 4) {
342: log_puts("enc: copying ");
343: log_putu(todo);
344: log_puts(" frames\n");
345: }
346: #endif
347: /*
348: * Partially copy structures into local variables, to avoid
349: * unnecessary indirections; this also allows the compiler to
350: * order local variables more "cache-friendly".
351: */
352: idata = (adata_t *)in;
353: odata = out;
354: oshift = p->shift;
355: osigbit = p->sigbit;
356: obps = p->bps;
357: obnext = p->bnext;
358: osnext = p->snext;
359:
360: /*
361: * Start conversion.
362: */
363: odata += p->bfirst;
364: for (f = todo * p->nch; f > 0; f--) {
365: s = *idata++;
366: s <<= 32 - ADATA_BITS;
367: s >>= oshift;
368: s ^= osigbit;
369: for (i = obps; i > 0; i--) {
370: *odata = (unsigned char)s;
371: s >>= 8;
372: odata += obnext;
373: }
374: odata += osnext;
375: }
376: }
377:
378: /*
379: * store "todo" frames of silence in foreign encoding
380: */
381: void
382: enc_sil_do(struct conv *p, unsigned char *out, int todo)
383: {
384: unsigned int f;
385: int s;
386: int osigbit;
387: unsigned int obps;
388: unsigned int i;
389: unsigned char *odata;
390: int obnext;
391: int osnext;
392:
393: #ifdef DEBUG
394: if (log_level >= 4) {
395: log_puts("enc: silence ");
396: log_putu(todo);
397: log_puts(" frames\n");
398: }
399: #endif
400: /*
401: * Partially copy structures into local variables, to avoid
402: * unnecessary indirections; this also allows the compiler to
403: * order local variables more "cache-friendly".
404: */
405: odata = out;
406: osigbit = p->sigbit;
407: obps = p->bps;
408: obnext = p->bnext;
409: osnext = p->snext;
410:
411: /*
412: * Start conversion.
413: */
414: odata += p->bfirst;
415: for (f = todo * p->nch; f > 0; f--) {
416: s = osigbit;
417: for (i = obps; i > 0; i--) {
418: *odata = (unsigned char)s;
419: s >>= 8;
420: odata += obnext;
421: }
422: odata += osnext;
423: }
424: }
425:
426: /*
427: * initialize encoder from native to foreign encoding
428: */
429: void
430: enc_init(struct conv *p, struct aparams *par, int nch)
431: {
432: p->nch = nch;
433: p->bps = par->bps;
434: p->sigbit = par->sig ? 0 : 1 << (par->bits - 1);
435: if (par->msb) {
436: p->shift = 32 - par->bps * 8;
437: } else {
438: p->shift = 32 - par->bits;
439: }
440: if (!par->le) {
441: p->bfirst = par->bps - 1;
442: p->bnext = -1;
443: p->snext = 2 * par->bps;
444: } else {
445: p->bfirst = 0;
446: p->bnext = 1;
447: p->snext = 0;
448: }
449: #ifdef DEBUG
450: if (log_level >= 3) {
451: log_puts("enc: ");
452: aparams_log(par);
453: log_puts(", ");
454: log_puti(p->nch);
455: log_puts(" channels\n");
456: }
457: #endif
458: }
459:
460: /*
461: * decode "todo" frames from from foreign to native encoding
462: */
463: void
464: dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo)
465: {
466: unsigned int f;
467: unsigned int ibps;
468: unsigned int i;
469: int s = 0xdeadbeef;
470: unsigned char *idata;
471: int ibnext;
472: int isnext;
473: int isigbit;
474: unsigned int ishift;
475: adata_t *odata;
476:
477: #ifdef DEBUG
478: if (log_level >= 4) {
479: log_puts("dec: copying ");
480: log_putu(todo);
481: log_puts(" frames\n");
482: }
483: #endif
484: /*
485: * Partially copy structures into local variables, to avoid
486: * unnecessary indirections; this also allows the compiler to
487: * order local variables more "cache-friendly".
488: */
489: idata = in;
490: odata = (adata_t *)out;
491: ibps = p->bps;
492: ibnext = p->bnext;
493: isigbit = p->sigbit;
494: ishift = p->shift;
495: isnext = p->snext;
496:
497: /*
498: * Start conversion.
499: */
500: idata += p->bfirst;
501: for (f = todo * p->nch; f > 0; f--) {
502: for (i = ibps; i > 0; i--) {
503: s <<= 8;
504: s |= *idata;
505: idata += ibnext;
506: }
507: idata += isnext;
508: s ^= isigbit;
509: s <<= ishift;
510: s >>= 32 - ADATA_BITS;
511: *odata++ = s;
512: }
513: }
514:
515: /*
516: * initialize decoder from foreign to native encoding
517: */
518: void
519: dec_init(struct conv *p, struct aparams *par, int nch)
520: {
521: p->bps = par->bps;
522: p->sigbit = par->sig ? 0 : 1 << (par->bits - 1);
523: p->nch = nch;
524: if (par->msb) {
525: p->shift = 32 - par->bps * 8;
526: } else {
527: p->shift = 32 - par->bits;
528: }
529: if (par->le) {
530: p->bfirst = par->bps - 1;
531: p->bnext = -1;
532: p->snext = 2 * par->bps;
533: } else {
534: p->bfirst = 0;
535: p->bnext = 1;
536: p->snext = 0;
537: }
538: #ifdef DEBUG
539: if (log_level >= 3) {
540: log_puts("dec: ");
541: aparams_log(par);
542: log_puts(", ");
543: log_puti(p->nch);
544: log_puts(" channels\n");
545: }
546: #endif
547: }
548:
549: /*
550: * mix "todo" input frames on the output with the given volume
551: */
552: void
553: cmap_add(struct cmap *p, void *in, void *out, int vol, int todo)
554: {
555: adata_t *idata, *odata;
556: int i, j, nch, istart, inext, onext, ostart, y, v;
557:
558: #ifdef DEBUG
559: if (log_level >= 4) {
560: log_puts("cmap: adding ");
561: log_puti(todo);
562: log_puts(" frames\n");
563: }
564: #endif
565: idata = in;
566: odata = out;
567: ostart = p->ostart;
568: onext = p->onext;
569: istart = p->istart;
570: inext = p->inext;
571: nch = p->nch;
1.2 ratchov 572: v = vol;
1.1 ratchov 573:
574: /*
575: * map/mix input on the output
576: */
577: for (i = todo; i > 0; i--) {
578: odata += ostart;
579: idata += istart;
580: for (j = nch; j > 0; j--) {
581: y = *odata + ADATA_MUL(*idata, v);
582: if (y >= ADATA_UNIT)
583: y = ADATA_UNIT - 1;
584: else if (y < -ADATA_UNIT)
585: y = -ADATA_UNIT;
586: *odata = y;
587: idata++;
588: odata++;
589: }
590: odata += onext;
591: idata += inext;
592: }
593: }
594:
595: /*
596: * overwrite output with "todo" input frames with with the given volume
597: */
598: void
599: cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo)
600: {
601: adata_t *idata, *odata;
602: int i, j, nch, istart, inext, onext, ostart, v;
603:
604: #ifdef DEBUG
605: if (log_level >= 4) {
606: log_puts("cmap: copying ");
607: log_puti(todo);
608: log_puts(" frames\n");
609: }
610: #endif
611: idata = in;
612: odata = out;
613: ostart = p->ostart;
614: onext = p->onext;
615: istart = p->istart;
616: inext = p->inext;
617: nch = p->nch;
618: v = vol;
619:
620: /*
621: * copy to the output buffer
622: */
623: for (i = todo; i > 0; i--) {
624: idata += istart;
1.4 ! ratchov 625: odata += ostart;
1.1 ratchov 626: for (j = nch; j > 0; j--) {
627: *odata = ADATA_MUL(*idata, v);
628: odata++;
629: idata++;
630: }
1.4 ! ratchov 631: odata += onext;
1.1 ratchov 632: idata += inext;
633: }
634: }
635:
636: /*
637: * initialize channel mapper, to map a subset of input channel range
638: * into a subset of the output channel range
639: */
640: void
641: cmap_init(struct cmap *p,
642: int imin, int imax, int isubmin, int isubmax,
643: int omin, int omax, int osubmin, int osubmax)
644: {
645: int cmin, cmax;
646:
647: cmin = -NCHAN_MAX;
648: if (osubmin > cmin)
649: cmin = osubmin;
650: if (omin > cmin)
651: cmin = omin;
652: if (isubmin > cmin)
653: cmin = isubmin;
654: if (imin > cmin)
655: cmin = imin;
656:
657: cmax = NCHAN_MAX;
658: if (osubmax < cmax)
659: cmax = osubmax;
660: if (omax < cmax)
661: cmax = omax;
662: if (isubmax < cmax)
663: cmax = isubmax;
664: if (imax < cmax)
665: cmax = imax;
666:
667: p->ostart = cmin - omin;
668: p->onext = omax - cmax;
669: p->istart = cmin - imin;
670: p->inext = imax - cmax;
671: p->nch = cmax - cmin + 1;
672: #ifdef DEBUG
673: if (log_level >= 3) {
674: log_puts("cmap: nch = ");
675: log_puti(p->nch);
676: log_puts(", ostart = ");
677: log_puti(p->ostart);
678: log_puts(", onext = ");
679: log_puti(p->onext);
680: log_puts(", istart = ");
681: log_puti(p->istart);
682: log_puts(", inext= ");
683: log_puti(p->inext);
684: log_puts("\n");
685: }
686: #endif
687: }
688:
689: /*
690: * produce a square tone, for instance with:
691: *
692: * period = round / (220 * round / rate)
693: */
694: int
695: sqrtone(int ctx, adata_t *out, int period, int vol, int todo)
696: {
697: int i;
698:
699: for (i = todo; i > 0; i--) {
700: if (ctx == 0) {
701: vol = -vol;
702: ctx = period / 2;
703: }
704: ctx--;
705: *(out++) += vol;
706: }
707: return ctx;
708: }