1 /* $OpenBSD: dsp.c,v 1.15 2020/12/10 17:30:49 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->sig && 196 par->bps == sizeof(adata_t) && 197 par->bits == ADATA_BITS && 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 */ 205 void 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 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 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; 232 diff = p->oblksz; 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 (;;) { 240 if (diff >= oblksz) { 241 if (todo == 0) 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 } 249 diff -= oblksz; 250 todo--; 251 } else { 252 ctx = ctxbuf; 253 for (c = nch; c > 0; c--) { 254 s = ctx[ctx_start ^ 1]; 255 ds = ctx[ctx_start] - s; 256 ctx += RESAMP_NCTX; 257 *odata++ = s + ADATA_MULDIV(ds, diff, oblksz); 258 } 259 diff += iblksz; 260 } 261 } 262 263 p->ctx_start = ctx_start; 264 } 265 266 /* 267 * initialize resampler with ibufsz/obufsz factor and "nch" channels 268 */ 269 void 270 resamp_init(struct resamp *p, unsigned int iblksz, 271 unsigned int oblksz, int nch) 272 { 273 p->iblksz = iblksz; 274 p->oblksz = oblksz; 275 p->nch = nch; 276 p->ctx_start = 0; 277 memset(p->ctx, 0, sizeof(p->ctx)); 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; 297 unsigned int s; 298 unsigned int oshift; 299 unsigned int obias; 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; 321 obias = p->bias; 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--) { 331 /* convert adata to u32 */ 332 s = (int)*idata++ + ADATA_UNIT; 333 s <<= 32 - ADATA_BITS; 334 /* convert u32 to uN */ 335 s >>= oshift; 336 /* convert uN to sN */ 337 s -= obias; 338 /* packetize sN */ 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; 355 unsigned int s; 356 unsigned int oshift; 357 int obias; 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; 377 oshift = p->shift; 378 obias = p->bias; 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--) { 388 s = ((1U << 31) >> oshift) - obias; 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 } 411 if (par->sig) { 412 p->bias = (1U << 31) >> p->shift; 413 } else { 414 p->bias = 0; 415 } 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 /* 437 * decode "todo" frames from foreign to native encoding 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; 445 unsigned int s = 0xdeadbeef; 446 unsigned char *idata; 447 int ibnext; 448 int isnext; 449 unsigned int ibias; 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; 469 ibias = p->bias; 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; 484 s += ibias; 485 s <<= ishift; 486 s >>= 32 - ADATA_BITS; 487 *odata++ = s - ADATA_UNIT; 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 } 504 if (par->sig) { 505 p->bias = (1U << 31) >> p->shift; 506 } else { 507 p->bias = 0; 508 } 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; 552 v = vol; 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 /* 576 * overwrite output with "todo" input frames with the given volume 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; 605 odata += ostart; 606 for (j = nch; j > 0; j--) { 607 *odata = ADATA_MUL(*idata, v); 608 odata++; 609 idata++; 610 } 611 odata += onext; 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); 662 log_puts(", inext = "); 663 log_puti(p->inext); 664 log_puts("\n"); 665 } 666 #endif 667 } 668