Annotation of src/usr.bin/sndiod/dsp.c, Revision 1.1
1.1 ! ratchov 1: /* $OpenBSD$ */
! 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;
! 573: v = vol; /* XXX */
! 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: }