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