1 /* $OpenBSD: dsp.c,v 1.10 2016/01/09 08:53:08 ratchov Exp $ */ 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) alignment 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, 300 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 unsigned int s; 333 unsigned int oshift; 334 unsigned int obias; 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 obias = p->bias; 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 /* convert adata to u32 */ 367 s = (int)*idata++ + ADATA_UNIT; 368 s <<= 32 - ADATA_BITS; 369 /* convert u32 to uN */ 370 s >>= oshift; 371 /* convert uN to sN */ 372 s -= obias; 373 /* packetize sN */ 374 for (i = obps; i > 0; i--) { 375 *odata = (unsigned char)s; 376 s >>= 8; 377 odata += obnext; 378 } 379 odata += osnext; 380 } 381 } 382 383 /* 384 * store "todo" frames of silence in foreign encoding 385 */ 386 void 387 enc_sil_do(struct conv *p, unsigned char *out, int todo) 388 { 389 unsigned int f; 390 unsigned int s; 391 unsigned int oshift; 392 int obias; 393 unsigned int obps; 394 unsigned int i; 395 unsigned char *odata; 396 int obnext; 397 int osnext; 398 399 #ifdef DEBUG 400 if (log_level >= 4) { 401 log_puts("enc: silence "); 402 log_putu(todo); 403 log_puts(" frames\n"); 404 } 405 #endif 406 /* 407 * Partially copy structures into local variables, to avoid 408 * unnecessary indirections; this also allows the compiler to 409 * order local variables more "cache-friendly". 410 */ 411 odata = out; 412 oshift = p->shift; 413 obias = p->bias; 414 obps = p->bps; 415 obnext = p->bnext; 416 osnext = p->snext; 417 418 /* 419 * Start conversion. 420 */ 421 odata += p->bfirst; 422 for (f = todo * p->nch; f > 0; f--) { 423 s = ((1U << 31) >> oshift) - obias; 424 for (i = obps; i > 0; i--) { 425 *odata = (unsigned char)s; 426 s >>= 8; 427 odata += obnext; 428 } 429 odata += osnext; 430 } 431 } 432 433 /* 434 * initialize encoder from native to foreign encoding 435 */ 436 void 437 enc_init(struct conv *p, struct aparams *par, int nch) 438 { 439 p->nch = nch; 440 p->bps = par->bps; 441 if (par->msb) { 442 p->shift = 32 - par->bps * 8; 443 } else { 444 p->shift = 32 - par->bits; 445 } 446 if (par->sig) { 447 p->bias = (1U << 31) >> p->shift; 448 } else { 449 p->bias = 0; 450 } 451 if (!par->le) { 452 p->bfirst = par->bps - 1; 453 p->bnext = -1; 454 p->snext = 2 * par->bps; 455 } else { 456 p->bfirst = 0; 457 p->bnext = 1; 458 p->snext = 0; 459 } 460 #ifdef DEBUG 461 if (log_level >= 3) { 462 log_puts("enc: "); 463 aparams_log(par); 464 log_puts(", "); 465 log_puti(p->nch); 466 log_puts(" channels\n"); 467 } 468 #endif 469 } 470 471 /* 472 * decode "todo" frames from from foreign to native encoding 473 */ 474 void 475 dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo) 476 { 477 unsigned int f; 478 unsigned int ibps; 479 unsigned int i; 480 unsigned int s = 0xdeadbeef; 481 unsigned char *idata; 482 int ibnext; 483 int isnext; 484 unsigned int ibias; 485 unsigned int ishift; 486 adata_t *odata; 487 488 #ifdef DEBUG 489 if (log_level >= 4) { 490 log_puts("dec: copying "); 491 log_putu(todo); 492 log_puts(" frames\n"); 493 } 494 #endif 495 /* 496 * Partially copy structures into local variables, to avoid 497 * unnecessary indirections; this also allows the compiler to 498 * order local variables more "cache-friendly". 499 */ 500 idata = in; 501 odata = (adata_t *)out; 502 ibps = p->bps; 503 ibnext = p->bnext; 504 ibias = p->bias; 505 ishift = p->shift; 506 isnext = p->snext; 507 508 /* 509 * Start conversion. 510 */ 511 idata += p->bfirst; 512 for (f = todo * p->nch; f > 0; f--) { 513 for (i = ibps; i > 0; i--) { 514 s <<= 8; 515 s |= *idata; 516 idata += ibnext; 517 } 518 idata += isnext; 519 s += ibias; 520 s <<= ishift; 521 s >>= 32 - ADATA_BITS; 522 *odata++ = s - ADATA_UNIT; 523 } 524 } 525 526 /* 527 * initialize decoder from foreign to native encoding 528 */ 529 void 530 dec_init(struct conv *p, struct aparams *par, int nch) 531 { 532 p->bps = par->bps; 533 p->nch = nch; 534 if (par->msb) { 535 p->shift = 32 - par->bps * 8; 536 } else { 537 p->shift = 32 - par->bits; 538 } 539 if (par->sig) { 540 p->bias = (1U << 31) >> p->shift; 541 } else { 542 p->bias = 0; 543 } 544 if (par->le) { 545 p->bfirst = par->bps - 1; 546 p->bnext = -1; 547 p->snext = 2 * par->bps; 548 } else { 549 p->bfirst = 0; 550 p->bnext = 1; 551 p->snext = 0; 552 } 553 #ifdef DEBUG 554 if (log_level >= 3) { 555 log_puts("dec: "); 556 aparams_log(par); 557 log_puts(", "); 558 log_puti(p->nch); 559 log_puts(" channels\n"); 560 } 561 #endif 562 } 563 564 /* 565 * mix "todo" input frames on the output with the given volume 566 */ 567 void 568 cmap_add(struct cmap *p, void *in, void *out, int vol, int todo) 569 { 570 adata_t *idata, *odata; 571 int i, j, nch, istart, inext, onext, ostart, y, v; 572 573 #ifdef DEBUG 574 if (log_level >= 4) { 575 log_puts("cmap: adding "); 576 log_puti(todo); 577 log_puts(" frames\n"); 578 } 579 #endif 580 idata = in; 581 odata = out; 582 ostart = p->ostart; 583 onext = p->onext; 584 istart = p->istart; 585 inext = p->inext; 586 nch = p->nch; 587 v = vol; 588 589 /* 590 * map/mix input on the output 591 */ 592 for (i = todo; i > 0; i--) { 593 odata += ostart; 594 idata += istart; 595 for (j = nch; j > 0; j--) { 596 y = *odata + ADATA_MUL(*idata, v); 597 if (y >= ADATA_UNIT) 598 y = ADATA_UNIT - 1; 599 else if (y < -ADATA_UNIT) 600 y = -ADATA_UNIT; 601 *odata = y; 602 idata++; 603 odata++; 604 } 605 odata += onext; 606 idata += inext; 607 } 608 } 609 610 /* 611 * overwrite output with "todo" input frames with with the given volume 612 */ 613 void 614 cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo) 615 { 616 adata_t *idata, *odata; 617 int i, j, nch, istart, inext, onext, ostart, v; 618 619 #ifdef DEBUG 620 if (log_level >= 4) { 621 log_puts("cmap: copying "); 622 log_puti(todo); 623 log_puts(" frames\n"); 624 } 625 #endif 626 idata = in; 627 odata = out; 628 ostart = p->ostart; 629 onext = p->onext; 630 istart = p->istart; 631 inext = p->inext; 632 nch = p->nch; 633 v = vol; 634 635 /* 636 * copy to the output buffer 637 */ 638 for (i = todo; i > 0; i--) { 639 idata += istart; 640 odata += ostart; 641 for (j = nch; j > 0; j--) { 642 *odata = ADATA_MUL(*idata, v); 643 odata++; 644 idata++; 645 } 646 odata += onext; 647 idata += inext; 648 } 649 } 650 651 /* 652 * initialize channel mapper, to map a subset of input channel range 653 * into a subset of the output channel range 654 */ 655 void 656 cmap_init(struct cmap *p, 657 int imin, int imax, int isubmin, int isubmax, 658 int omin, int omax, int osubmin, int osubmax) 659 { 660 int cmin, cmax; 661 662 cmin = -NCHAN_MAX; 663 if (osubmin > cmin) 664 cmin = osubmin; 665 if (omin > cmin) 666 cmin = omin; 667 if (isubmin > cmin) 668 cmin = isubmin; 669 if (imin > cmin) 670 cmin = imin; 671 672 cmax = NCHAN_MAX; 673 if (osubmax < cmax) 674 cmax = osubmax; 675 if (omax < cmax) 676 cmax = omax; 677 if (isubmax < cmax) 678 cmax = isubmax; 679 if (imax < cmax) 680 cmax = imax; 681 682 p->ostart = cmin - omin; 683 p->onext = omax - cmax; 684 p->istart = cmin - imin; 685 p->inext = imax - cmax; 686 p->nch = cmax - cmin + 1; 687 #ifdef DEBUG 688 if (log_level >= 3) { 689 log_puts("cmap: nch = "); 690 log_puti(p->nch); 691 log_puts(", ostart = "); 692 log_puti(p->ostart); 693 log_puts(", onext = "); 694 log_puti(p->onext); 695 log_puts(", istart = "); 696 log_puti(p->istart); 697 log_puts(", inext = "); 698 log_puti(p->inext); 699 log_puts("\n"); 700 } 701 #endif 702 } 703