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