1 /* $OpenBSD: dsp.c,v 1.3 2012/12/07 08:04:58 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) 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; 572 v = vol; 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; 625 for (j = ostart; j > 0; j--) 626 *odata++ = 0x1111; 627 for (j = nch; j > 0; j--) { 628 *odata = ADATA_MUL(*idata, v); 629 odata++; 630 idata++; 631 } 632 for (j = onext; j > 0; j--) 633 *odata++ = 0x2222; 634 idata += inext; 635 } 636 } 637 638 /* 639 * initialize channel mapper, to map a subset of input channel range 640 * into a subset of the output channel range 641 */ 642 void 643 cmap_init(struct cmap *p, 644 int imin, int imax, int isubmin, int isubmax, 645 int omin, int omax, int osubmin, int osubmax) 646 { 647 int cmin, cmax; 648 649 cmin = -NCHAN_MAX; 650 if (osubmin > cmin) 651 cmin = osubmin; 652 if (omin > cmin) 653 cmin = omin; 654 if (isubmin > cmin) 655 cmin = isubmin; 656 if (imin > cmin) 657 cmin = imin; 658 659 cmax = NCHAN_MAX; 660 if (osubmax < cmax) 661 cmax = osubmax; 662 if (omax < cmax) 663 cmax = omax; 664 if (isubmax < cmax) 665 cmax = isubmax; 666 if (imax < cmax) 667 cmax = imax; 668 669 p->ostart = cmin - omin; 670 p->onext = omax - cmax; 671 p->istart = cmin - imin; 672 p->inext = imax - cmax; 673 p->nch = cmax - cmin + 1; 674 #ifdef DEBUG 675 if (log_level >= 3) { 676 log_puts("cmap: nch = "); 677 log_puti(p->nch); 678 log_puts(", ostart = "); 679 log_puti(p->ostart); 680 log_puts(", onext = "); 681 log_puti(p->onext); 682 log_puts(", istart = "); 683 log_puti(p->istart); 684 log_puts(", inext= "); 685 log_puti(p->inext); 686 log_puts("\n"); 687 } 688 #endif 689 } 690 691 /* 692 * produce a square tone, for instance with: 693 * 694 * period = round / (220 * round / rate) 695 */ 696 int 697 sqrtone(int ctx, adata_t *out, int period, int vol, int todo) 698 { 699 int i; 700 701 for (i = todo; i > 0; i--) { 702 if (ctx == 0) { 703 vol = -vol; 704 ctx = period / 2; 705 } 706 ctx--; 707 *(out++) += vol; 708 } 709 return ctx; 710 } 711