1 /* $OpenBSD: dsp.c,v 1.22 2024/12/20 07:35:56 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 <stdio.h> 18 #include <string.h> 19 #include "dsp.h" 20 #include "utils.h" 21 22 const int aparams_ctltovol[128] = { 23 0, 65536, 68109, 70783, 24 73562, 76450, 79451, 82570, 25 85812, 89181, 92682, 96321, 26 100102, 104032, 108116, 112361, 27 116772, 121356, 126121, 131072, 28 136218, 141566, 147123, 152899, 29 158902, 165140, 171624, 178361, 30 185364, 192641, 200204, 208064, 31 216232, 224721, 233544, 242713, 32 252241, 262144, 272436, 283131, 33 294247, 305799, 317804, 330281, 34 343247, 356723, 370728, 385282, 35 400408, 416128, 432465, 449443, 36 467088, 485425, 504482, 524288, 37 544871, 566262, 588493, 611597, 38 635608, 660561, 686495, 713446, 39 741455, 770564, 800816, 832255, 40 864929, 898885, 934175, 970850, 41 1008965, 1048576, 1089742, 1132525, 42 1176987, 1223194, 1271216, 1321123, 43 1372989, 1426892, 1482910, 1541128, 44 1601632, 1664511, 1729858, 1797771, 45 1868350, 1941700, 2017930, 2097152, 46 2179485, 2265049, 2353974, 2446389, 47 2542432, 2642246, 2745978, 2853783, 48 2965821, 3082257, 3203264, 3329021, 49 3459716, 3595542, 3736700, 3883400, 50 4035859, 4194304, 4358969, 4530099, 51 4707947, 4892777, 5084864, 5284492, 52 5491957, 5707567, 5931642, 6164513, 53 6406527, 6658043, 6919432, 7191084, 54 7473400, 7766800, 8071719, 8388608 55 }; 56 57 const int resamp_filt[RESAMP_LENGTH / RESAMP_STEP + 1] = { 58 0, 0, 3, 9, 22, 42, 73, 116, 59 174, 248, 341, 454, 589, 749, 934, 1148, 60 1392, 1666, 1974, 2316, 2693, 3107, 3560, 4051, 61 4582, 5154, 5766, 6420, 7116, 7853, 8632, 9451, 62 10311, 11210, 12148, 13123, 14133, 15178, 16253, 17359, 63 18491, 19647, 20824, 22018, 23226, 24443, 25665, 26888, 64 28106, 29315, 30509, 31681, 32826, 33938, 35009, 36033, 65 37001, 37908, 38744, 39502, 40174, 40750, 41223, 41582, 66 41819, 41925, 41890, 41704, 41358, 40842, 40147, 39261, 67 38176, 36881, 35366, 33623, 31641, 29411, 26923, 24169, 68 21140, 17827, 14222, 10317, 6105, 1580, -3267, -8440, 69 -13944, -19785, -25967, -32492, -39364, -46584, -54153, -62072, 70 -70339, -78953, -87911, -97209, -106843, -116806, -127092, -137692, 71 -148596, -159795, -171276, -183025, -195029, -207271, -219735, -232401, 72 -245249, -258259, -271407, -284670, -298021, -311434, -324880, -338329, 73 -351750, -365111, -378378, -391515, -404485, -417252, -429775, -442015, 74 -453930, -465477, -476613, -487294, -497472, -507102, -516137, -524527, 75 -532225, -539181, -545344, -550664, -555090, -558571, -561055, -562490, 76 -562826, -562010, -559990, -556717, -552139, -546205, -538866, -530074, 77 -519779, -507936, -494496, -479416, -462652, -444160, -423901, -401835, 78 -377923, -352132, -324425, -294772, -263143, -229509, -193847, -156134, 79 -116348, -74474, -30494, 15601, 63822, 114174, 166661, 221283, 80 278037, 336916, 397911, 461009, 526194, 593446, 662741, 734054, 81 807354, 882608, 959779, 1038826, 1119706, 1202370, 1286768, 1372846, 82 1460546, 1549808, 1640566, 1732753, 1826299, 1921130, 2017169, 2114336, 83 2212550, 2311723, 2411770, 2512598, 2614116, 2716228, 2818836, 2921841, 84 3025142, 3128636, 3232218, 3335782, 3439219, 3542423, 3645282, 3747687, 85 3849526, 3950687, 4051059, 4150530, 4248987, 4346320, 4442415, 4537163, 86 4630453, 4722177, 4812225, 4900493, 4986873, 5071263, 5153561, 5233668, 87 5311485, 5386917, 5459872, 5530259, 5597992, 5662986, 5725160, 5784436, 88 5840739, 5893999, 5944148, 5991122, 6034862, 6075313, 6112422, 6146142, 89 6176430, 6203247, 6226559, 6246335, 6262551, 6275185, 6284220, 6289647, 90 6291456, 6289647, 6284220, 6275185, 6262551, 6246335, 6226559, 6203247, 91 6176430, 6146142, 6112422, 6075313, 6034862, 5991122, 5944148, 5893999, 92 5840739, 5784436, 5725160, 5662986, 5597992, 5530259, 5459872, 5386917, 93 5311485, 5233668, 5153561, 5071263, 4986873, 4900493, 4812225, 4722177, 94 4630453, 4537163, 4442415, 4346320, 4248987, 4150530, 4051059, 3950687, 95 3849526, 3747687, 3645282, 3542423, 3439219, 3335782, 3232218, 3128636, 96 3025142, 2921841, 2818836, 2716228, 2614116, 2512598, 2411770, 2311723, 97 2212550, 2114336, 2017169, 1921130, 1826299, 1732753, 1640566, 1549808, 98 1460546, 1372846, 1286768, 1202370, 1119706, 1038826, 959779, 882608, 99 807354, 734054, 662741, 593446, 526194, 461009, 397911, 336916, 100 278037, 221283, 166661, 114174, 63822, 15601, -30494, -74474, 101 -116348, -156134, -193847, -229509, -263143, -294772, -324425, -352132, 102 -377923, -401835, -423901, -444160, -462652, -479416, -494496, -507936, 103 -519779, -530074, -538866, -546205, -552139, -556717, -559990, -562010, 104 -562826, -562490, -561055, -558571, -555090, -550664, -545344, -539181, 105 -532225, -524527, -516137, -507102, -497472, -487294, -476613, -465477, 106 -453930, -442015, -429775, -417252, -404485, -391515, -378378, -365111, 107 -351750, -338329, -324880, -311434, -298021, -284670, -271407, -258259, 108 -245249, -232401, -219735, -207271, -195029, -183025, -171276, -159795, 109 -148596, -137692, -127092, -116806, -106843, -97209, -87911, -78953, 110 -70339, -62072, -54153, -46584, -39364, -32492, -25967, -19785, 111 -13944, -8440, -3267, 1580, 6105, 10317, 14222, 17827, 112 21140, 24169, 26923, 29411, 31641, 33623, 35366, 36881, 113 38176, 39261, 40147, 40842, 41358, 41704, 41890, 41925, 114 41819, 41582, 41223, 40750, 40174, 39502, 38744, 37908, 115 37001, 36033, 35009, 33938, 32826, 31681, 30509, 29315, 116 28106, 26888, 25665, 24443, 23226, 22018, 20824, 19647, 117 18491, 17359, 16253, 15178, 14133, 13123, 12148, 11210, 118 10311, 9451, 8632, 7853, 7116, 6420, 5766, 5154, 119 4582, 4051, 3560, 3107, 2693, 2316, 1974, 1666, 120 1392, 1148, 934, 749, 589, 454, 341, 248, 121 174, 116, 73, 42, 22, 9, 3, 0, 122 0 123 }; 124 125 126 /* 127 * Generate a string corresponding to the encoding in par, 128 * return the length of the resulting string. 129 */ 130 int 131 aparams_enctostr(struct aparams *par, char *ostr) 132 { 133 char *p = ostr; 134 135 *p++ = par->sig ? 's' : 'u'; 136 if (par->bits > 9) 137 *p++ = '0' + par->bits / 10; 138 *p++ = '0' + par->bits % 10; 139 if (par->bps > 1) { 140 *p++ = par->le ? 'l' : 'b'; 141 *p++ = 'e'; 142 if (par->bps != APARAMS_BPS(par->bits) || 143 par->bits < par->bps * 8) { 144 *p++ = par->bps + '0'; 145 if (par->bits < par->bps * 8) { 146 *p++ = par->msb ? 'm' : 'l'; 147 *p++ = 's'; 148 *p++ = 'b'; 149 } 150 } 151 } 152 *p++ = '\0'; 153 return p - ostr - 1; 154 } 155 156 /* 157 * Parse an encoding string, examples: s8, u8, s16, s16le, s24be ... 158 * set *istr to the char following the encoding. Return the number 159 * of bytes consumed. 160 */ 161 int 162 aparams_strtoenc(struct aparams *par, char *istr) 163 { 164 char *p = istr; 165 int i, sig, bits, le, bps, msb; 166 167 #define IS_SEP(c) \ 168 (((c) < 'a' || (c) > 'z') && \ 169 ((c) < 'A' || (c) > 'Z') && \ 170 ((c) < '0' || (c) > '9')) 171 172 /* 173 * get signedness 174 */ 175 if (*p == 's') { 176 sig = 1; 177 } else if (*p == 'u') { 178 sig = 0; 179 } else 180 return 0; 181 p++; 182 183 /* 184 * get number of bits per sample 185 */ 186 bits = 0; 187 for (i = 0; i < 2; i++) { 188 if (*p < '0' || *p > '9') 189 break; 190 bits = (bits * 10) + *p - '0'; 191 p++; 192 } 193 if (bits < BITS_MIN || bits > BITS_MAX) 194 return 0; 195 bps = APARAMS_BPS(bits); 196 msb = 1; 197 le = ADATA_LE; 198 199 /* 200 * get (optional) endianness 201 */ 202 if (p[0] == 'l' && p[1] == 'e') { 203 le = 1; 204 p += 2; 205 } else if (p[0] == 'b' && p[1] == 'e') { 206 le = 0; 207 p += 2; 208 } else if (IS_SEP(*p)) { 209 goto done; 210 } else 211 return 0; 212 213 /* 214 * get (optional) number of bytes 215 */ 216 if (*p >= '0' && *p <= '9') { 217 bps = *p - '0'; 218 if (bps < (bits + 7) / 8 || 219 bps > (BITS_MAX + 7) / 8) 220 return 0; 221 p++; 222 223 /* 224 * get (optional) alignment 225 */ 226 if (p[0] == 'm' && p[1] == 's' && p[2] == 'b') { 227 msb = 1; 228 p += 3; 229 } else if (p[0] == 'l' && p[1] == 's' && p[2] == 'b') { 230 msb = 0; 231 p += 3; 232 } else if (IS_SEP(*p)) { 233 goto done; 234 } else 235 return 0; 236 } else if (!IS_SEP(*p)) 237 return 0; 238 239 done: 240 par->msb = msb; 241 par->sig = sig; 242 par->bits = bits; 243 par->bps = bps; 244 par->le = le; 245 return p - istr; 246 } 247 248 /* 249 * Initialise parameters structure with the defaults natively supported 250 * by the machine. 251 */ 252 void 253 aparams_init(struct aparams *par) 254 { 255 par->bps = sizeof(adata_t); 256 par->bits = ADATA_BITS; 257 par->le = ADATA_LE; 258 par->sig = 1; 259 par->msb = 0; 260 } 261 262 /* 263 * return true if encoding corresponds to what we store in adata_t 264 */ 265 int 266 aparams_native(struct aparams *par) 267 { 268 return par->sig && 269 par->bps == sizeof(adata_t) && 270 par->bits == ADATA_BITS && 271 (par->bps == 1 || par->le == ADATA_LE) && 272 (par->bits == par->bps * 8 || !par->msb); 273 } 274 275 /* 276 * Return the number of input and output frame that would be consumed 277 * by resamp_do(p, *icnt, *ocnt). 278 */ 279 void 280 resamp_getcnt(struct resamp *p, int *icnt, int *ocnt) 281 { 282 long long idiff, odiff; 283 int cdiff; 284 285 cdiff = p->oblksz - p->diff; 286 idiff = (long long)*icnt * p->oblksz; 287 odiff = (long long)*ocnt * p->iblksz; 288 if (odiff - idiff >= cdiff) 289 *ocnt = (idiff + cdiff + p->iblksz - 1) / p->iblksz; 290 else 291 *icnt = (odiff + p->diff) / p->oblksz; 292 } 293 294 /* 295 * Resample the given number of frames. The number of output frames 296 * must match the corresponding number of input frames. Either always 297 * use icnt and ocnt such that: 298 * 299 * icnt * oblksz = ocnt * iblksz 300 * 301 * or use resamp_getcnt() to calculate the proper numbers. 302 */ 303 void 304 resamp_do(struct resamp *p, adata_t *in, adata_t *out, int icnt, int ocnt) 305 { 306 unsigned int nch; 307 adata_t *idata; 308 unsigned int oblksz; 309 unsigned int ifr; 310 int s, ds, diff; 311 adata_t *odata; 312 unsigned int iblksz; 313 unsigned int ofr; 314 unsigned int c; 315 int64_t f[NCHAN_MAX]; 316 adata_t *ctxbuf, *ctx; 317 unsigned int ctx_start; 318 int q, qi, qf, n; 319 320 /* 321 * Partially copy structures into local variables, to avoid 322 * unnecessary indirections; this also allows the compiler to 323 * order local variables more "cache-friendly". 324 */ 325 idata = in; 326 odata = out; 327 diff = p->diff; 328 iblksz = p->iblksz; 329 oblksz = p->oblksz; 330 ctxbuf = p->ctx; 331 ctx_start = p->ctx_start; 332 nch = p->nch; 333 ifr = icnt; 334 ofr = ocnt; 335 336 /* 337 * Start conversion. 338 */ 339 #ifdef DEBUG 340 logx(4, "resamp: copying %d -> %d frames, diff = %d", ifr, ofr, diff); 341 #endif 342 for (;;) { 343 if (diff >= oblksz) { 344 if (ifr == 0) 345 break; 346 ctx_start = (ctx_start - 1) & (RESAMP_NCTX - 1); 347 ctx = ctxbuf + ctx_start; 348 for (c = nch; c > 0; c--) { 349 *ctx = *idata++; 350 ctx += RESAMP_NCTX; 351 } 352 diff -= oblksz; 353 ifr--; 354 } else { 355 if (ofr == 0) 356 break; 357 358 for (c = 0; c < nch; c++) 359 f[c] = 0; 360 361 q = diff * p->filt_step; 362 n = ctx_start; 363 364 while (q < RESAMP_LENGTH) { 365 qi = q >> RESAMP_STEP_BITS; 366 qf = q & (RESAMP_STEP - 1); 367 s = resamp_filt[qi]; 368 ds = resamp_filt[qi + 1] - s; 369 s += (int64_t)qf * ds >> RESAMP_STEP_BITS; 370 ctx = ctxbuf; 371 for (c = 0; c < nch; c++) { 372 f[c] += (int64_t)ctx[n] * s; 373 ctx += RESAMP_NCTX; 374 } 375 q += p->filt_cutoff; 376 n = (n + 1) & (RESAMP_NCTX - 1); 377 } 378 379 for (c = 0; c < nch; c++) { 380 s = f[c] >> RESAMP_BITS; 381 s = (int64_t)s * p->filt_cutoff >> RESAMP_BITS; 382 #if ADATA_BITS == 16 383 /* 384 * In 16-bit mode, we've no room for filter 385 * overshoots, so we need to clip the signal 386 * to avoid 16-bit integers to wrap around. 387 * In 24-bit mode, samples may exceed the 388 * [-1:1] range. Later, cmap_add() will clip 389 * them, so no need to clip them here as well. 390 */ 391 if (s >= ADATA_UNIT) 392 s = ADATA_UNIT - 1; 393 else if (s < -ADATA_UNIT) 394 s = -ADATA_UNIT; 395 #endif 396 *odata++ = s; 397 } 398 399 diff += iblksz; 400 ofr--; 401 } 402 } 403 p->diff = diff; 404 p->ctx_start = ctx_start; 405 #ifdef DEBUG 406 if (ifr != 0) { 407 logx(0, "resamp_do: %d: too many input frames", ifr); 408 panic(); 409 } 410 if (ofr != 0) { 411 logx(0, "resamp_do: %d: too many output frames", ofr); 412 panic(); 413 } 414 #endif 415 } 416 417 static unsigned int 418 uint_gcd(unsigned int a, unsigned int b) 419 { 420 unsigned int r; 421 422 while (b > 0) { 423 r = a % b; 424 a = b; 425 b = r; 426 } 427 return a; 428 } 429 430 /* 431 * initialize resampler with ibufsz/obufsz factor and "nch" channels 432 */ 433 void 434 resamp_init(struct resamp *p, unsigned int iblksz, 435 unsigned int oblksz, int nch) 436 { 437 unsigned int g; 438 439 /* 440 * reduce iblksz/oblksz fraction 441 */ 442 g = uint_gcd(iblksz, oblksz); 443 iblksz /= g; 444 oblksz /= g; 445 446 /* 447 * ensure weird rates don't cause integer overflow 448 */ 449 while (iblksz > ADATA_UNIT || oblksz > ADATA_UNIT) { 450 iblksz >>= 1; 451 oblksz >>= 1; 452 } 453 454 p->iblksz = iblksz; 455 p->oblksz = oblksz; 456 p->diff = 0; 457 p->nch = nch; 458 p->ctx_start = 0; 459 memset(p->ctx, 0, sizeof(p->ctx)); 460 if (p->iblksz < p->oblksz) { 461 p->filt_cutoff = RESAMP_UNIT; 462 p->filt_step = RESAMP_UNIT / p->oblksz; 463 } else { 464 p->filt_cutoff = (int64_t)RESAMP_UNIT * p->oblksz / p->iblksz; 465 p->filt_step = RESAMP_UNIT / p->iblksz; 466 } 467 #ifdef DEBUG 468 logx(3, "resamp_init: %u/%u", iblksz, oblksz); 469 #endif 470 } 471 472 /* 473 * encode "todo" frames from native to foreign encoding 474 */ 475 void 476 enc_do(struct conv *p, unsigned char *in, unsigned char *out, int todo) 477 { 478 unsigned int f; 479 adata_t *idata; 480 unsigned int s; 481 unsigned int oshift; 482 unsigned int obias; 483 unsigned int obps; 484 unsigned int i; 485 unsigned char *odata; 486 int obnext; 487 int osnext; 488 489 #ifdef DEBUG 490 logx(4, "enc: copying %u frames", todo); 491 #endif 492 /* 493 * Partially copy structures into local variables, to avoid 494 * unnecessary indirections; this also allows the compiler to 495 * order local variables more "cache-friendly". 496 */ 497 idata = (adata_t *)in; 498 odata = out; 499 oshift = p->shift; 500 obias = p->bias; 501 obps = p->bps; 502 obnext = p->bnext; 503 osnext = p->snext; 504 505 /* 506 * Start conversion. 507 */ 508 odata += p->bfirst; 509 for (f = todo * p->nch; f > 0; f--) { 510 /* convert adata to u32 */ 511 s = (int)*idata++ + ADATA_UNIT; 512 s <<= 32 - ADATA_BITS; 513 /* convert u32 to uN */ 514 s >>= oshift; 515 /* convert uN to sN */ 516 s -= obias; 517 /* packetize sN */ 518 for (i = obps; i > 0; i--) { 519 *odata = (unsigned char)s; 520 s >>= 8; 521 odata += obnext; 522 } 523 odata += osnext; 524 } 525 } 526 527 /* 528 * store "todo" frames of silence in foreign encoding 529 */ 530 void 531 enc_sil_do(struct conv *p, unsigned char *out, int todo) 532 { 533 unsigned int f; 534 unsigned int s; 535 unsigned int oshift; 536 int obias; 537 unsigned int obps; 538 unsigned int i; 539 unsigned char *odata; 540 int obnext; 541 int osnext; 542 543 #ifdef DEBUG 544 logx(4, "enc: silence %u frames", todo); 545 #endif 546 /* 547 * Partially copy structures into local variables, to avoid 548 * unnecessary indirections; this also allows the compiler to 549 * order local variables more "cache-friendly". 550 */ 551 odata = out; 552 oshift = p->shift; 553 obias = p->bias; 554 obps = p->bps; 555 obnext = p->bnext; 556 osnext = p->snext; 557 558 /* 559 * Start conversion. 560 */ 561 odata += p->bfirst; 562 for (f = todo * p->nch; f > 0; f--) { 563 s = ((1U << 31) >> oshift) - obias; 564 for (i = obps; i > 0; i--) { 565 *odata = (unsigned char)s; 566 s >>= 8; 567 odata += obnext; 568 } 569 odata += osnext; 570 } 571 } 572 573 /* 574 * initialize encoder from native to foreign encoding 575 */ 576 void 577 enc_init(struct conv *p, struct aparams *par, int nch) 578 { 579 #ifdef DEBUG 580 char enc_str[ENCMAX]; 581 #endif 582 583 p->nch = nch; 584 p->bps = par->bps; 585 if (par->msb) { 586 p->shift = 32 - par->bps * 8; 587 } else { 588 p->shift = 32 - par->bits; 589 } 590 if (par->sig) { 591 p->bias = (1U << 31) >> p->shift; 592 } else { 593 p->bias = 0; 594 } 595 if (!par->le) { 596 p->bfirst = par->bps - 1; 597 p->bnext = -1; 598 p->snext = 2 * par->bps; 599 } else { 600 p->bfirst = 0; 601 p->bnext = 1; 602 p->snext = 0; 603 } 604 #ifdef DEBUG 605 logx(3, "enc: %s, %d channels", 606 (aparams_enctostr(par, enc_str), enc_str), p->nch); 607 #endif 608 } 609 610 /* 611 * decode "todo" frames from foreign to native encoding 612 */ 613 void 614 dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo) 615 { 616 unsigned int f; 617 unsigned int ibps; 618 unsigned int i; 619 unsigned int s = 0xdeadbeef; 620 unsigned char *idata; 621 int ibnext; 622 int isnext; 623 unsigned int ibias; 624 unsigned int ishift; 625 adata_t *odata; 626 627 #ifdef DEBUG 628 logx(4, "dec: copying %u frames", todo); 629 #endif 630 /* 631 * Partially copy structures into local variables, to avoid 632 * unnecessary indirections; this also allows the compiler to 633 * order local variables more "cache-friendly". 634 */ 635 idata = in; 636 odata = (adata_t *)out; 637 ibps = p->bps; 638 ibnext = p->bnext; 639 ibias = p->bias; 640 ishift = p->shift; 641 isnext = p->snext; 642 643 /* 644 * Start conversion. 645 */ 646 idata += p->bfirst; 647 for (f = todo * p->nch; f > 0; f--) { 648 for (i = ibps; i > 0; i--) { 649 s <<= 8; 650 s |= *idata; 651 idata += ibnext; 652 } 653 idata += isnext; 654 s += ibias; 655 s <<= ishift; 656 s >>= 32 - ADATA_BITS; 657 *odata++ = s - ADATA_UNIT; 658 } 659 } 660 661 /* 662 * initialize decoder from foreign to native encoding 663 */ 664 void 665 dec_init(struct conv *p, struct aparams *par, int nch) 666 { 667 #ifdef DEBUG 668 char enc_str[ENCMAX]; 669 #endif 670 671 p->bps = par->bps; 672 p->nch = nch; 673 if (par->msb) { 674 p->shift = 32 - par->bps * 8; 675 } else { 676 p->shift = 32 - par->bits; 677 } 678 if (par->sig) { 679 p->bias = (1U << 31) >> p->shift; 680 } else { 681 p->bias = 0; 682 } 683 if (par->le) { 684 p->bfirst = par->bps - 1; 685 p->bnext = -1; 686 p->snext = 2 * par->bps; 687 } else { 688 p->bfirst = 0; 689 p->bnext = 1; 690 p->snext = 0; 691 } 692 #ifdef DEBUG 693 logx(3, "dec: %s, %d channels", 694 (aparams_enctostr(par, enc_str), enc_str), p->nch); 695 #endif 696 } 697 698 /* 699 * mix "todo" input frames on the output with the given volume 700 */ 701 void 702 cmap_add(struct cmap *p, void *in, void *out, int vol, int todo) 703 { 704 adata_t *idata, *odata; 705 int i, j, nch, istart, inext, onext, ostart, y, v; 706 707 #ifdef DEBUG 708 logx(4, "cmap: adding %d frames", todo); 709 #endif 710 idata = in; 711 odata = out; 712 ostart = p->ostart; 713 onext = p->onext; 714 istart = p->istart; 715 inext = p->inext; 716 nch = p->nch; 717 v = vol; 718 719 /* 720 * map/mix input on the output 721 */ 722 for (i = todo; i > 0; i--) { 723 odata += ostart; 724 idata += istart; 725 for (j = nch; j > 0; j--) { 726 y = *odata + ADATA_MUL(*idata, v); 727 if (y >= ADATA_UNIT) 728 y = ADATA_UNIT - 1; 729 else if (y < -ADATA_UNIT) 730 y = -ADATA_UNIT; 731 *odata = y; 732 idata++; 733 odata++; 734 } 735 odata += onext; 736 idata += inext; 737 } 738 } 739 740 /* 741 * overwrite output with "todo" input frames with the given volume 742 */ 743 void 744 cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo) 745 { 746 adata_t *idata, *odata; 747 int i, j, nch, istart, inext, onext, ostart, v; 748 749 #ifdef DEBUG 750 logx(4, "cmap: copying %d frames", todo); 751 #endif 752 idata = in; 753 odata = out; 754 ostart = p->ostart; 755 onext = p->onext; 756 istart = p->istart; 757 inext = p->inext; 758 nch = p->nch; 759 v = vol; 760 761 /* 762 * copy to the output buffer 763 */ 764 for (i = todo; i > 0; i--) { 765 idata += istart; 766 odata += ostart; 767 for (j = nch; j > 0; j--) { 768 *odata = ADATA_MUL(*idata, v); 769 odata++; 770 idata++; 771 } 772 odata += onext; 773 idata += inext; 774 } 775 } 776 777 /* 778 * initialize channel mapper, to map a subset of input channel range 779 * into a subset of the output channel range 780 */ 781 void 782 cmap_init(struct cmap *p, 783 int imin, int imax, int isubmin, int isubmax, 784 int omin, int omax, int osubmin, int osubmax) 785 { 786 int inch, onch, nch; 787 788 /* 789 * Ignore channels outside of the available sets 790 */ 791 if (isubmin < imin) 792 isubmin = imin; 793 if (isubmax > imax) 794 isubmax = imax; 795 if (osubmin < omin) 796 osubmin = omin; 797 if (osubmax > omax) 798 osubmax = omax; 799 800 /* 801 * Shrink the input or the output subset to make both subsets of 802 * the same size 803 */ 804 inch = isubmax - isubmin + 1; 805 onch = osubmax - osubmin + 1; 806 nch = (inch < onch) ? inch : onch; 807 isubmax = isubmin + nch - 1; 808 osubmax = osubmin + nch - 1; 809 810 p->ostart = osubmin - omin; 811 p->onext = omax - osubmax; 812 p->istart = isubmin - imin; 813 p->inext = imax - isubmax; 814 p->nch = nch; 815 #ifdef DEBUG 816 logx(3, "%s: nch = %d, ostart = %d, onext = %d, istart = %d, inext = %d", 817 __func__, p->nch, p->ostart, p->onext, p->istart, p->inext); 818 #endif 819 } 820