1 /* $OpenBSD: dsp.c,v 1.20 2024/04/22 12:32:51 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 * log the given format/channels/encoding 333 */ 334 void 335 aparams_log(struct aparams *par) 336 { 337 char enc[ENCMAX]; 338 339 aparams_enctostr(par, enc); 340 log_puts(enc); 341 } 342 343 /* 344 * return true if encoding corresponds to what we store in adata_t 345 */ 346 int 347 aparams_native(struct aparams *par) 348 { 349 return par->sig && 350 par->bps == sizeof(adata_t) && 351 par->bits == ADATA_BITS && 352 (par->bps == 1 || par->le == ADATA_LE) && 353 (par->bits == par->bps * 8 || !par->msb); 354 } 355 356 /* 357 * Return the number of input and output frame that would be consumed 358 * by resamp_do(p, *icnt, *ocnt). 359 */ 360 void 361 resamp_getcnt(struct resamp *p, int *icnt, int *ocnt) 362 { 363 long long idiff, odiff; 364 int cdiff; 365 366 cdiff = p->oblksz - p->diff; 367 idiff = (long long)*icnt * p->oblksz; 368 odiff = (long long)*ocnt * p->iblksz; 369 if (odiff - idiff >= cdiff) 370 *ocnt = (idiff + cdiff + p->iblksz - 1) / p->iblksz; 371 else 372 *icnt = (odiff + p->diff) / p->oblksz; 373 } 374 375 /* 376 * Resample the given number of frames. The number of output frames 377 * must match the corresponding number of input frames. Either always 378 * use icnt and ocnt such that: 379 * 380 * icnt * oblksz = ocnt * iblksz 381 * 382 * or use resamp_getcnt() to calculate the proper numbers. 383 */ 384 void 385 resamp_do(struct resamp *p, adata_t *in, adata_t *out, int icnt, int ocnt) 386 { 387 unsigned int nch; 388 adata_t *idata; 389 unsigned int oblksz; 390 unsigned int ifr; 391 int s, ds, diff; 392 adata_t *odata; 393 unsigned int iblksz; 394 unsigned int ofr; 395 unsigned int c; 396 int64_t f[NCHAN_MAX]; 397 adata_t *ctxbuf, *ctx; 398 unsigned int ctx_start; 399 int q, qi, qf, n; 400 401 /* 402 * Partially copy structures into local variables, to avoid 403 * unnecessary indirections; this also allows the compiler to 404 * order local variables more "cache-friendly". 405 */ 406 idata = in; 407 odata = out; 408 diff = p->diff; 409 iblksz = p->iblksz; 410 oblksz = p->oblksz; 411 ctxbuf = p->ctx; 412 ctx_start = p->ctx_start; 413 nch = p->nch; 414 ifr = icnt; 415 ofr = ocnt; 416 417 /* 418 * Start conversion. 419 */ 420 #ifdef DEBUG 421 if (log_level >= 4) { 422 log_puts("resamp: copying "); 423 log_puti(ifr); 424 log_puts(" -> "); 425 log_putu(ofr); 426 log_puts(" frames, diff = "); 427 log_puti(diff); 428 log_puts("\n"); 429 } 430 #endif 431 for (;;) { 432 if (diff >= oblksz) { 433 if (ifr == 0) 434 break; 435 ctx_start = (ctx_start - 1) & (RESAMP_NCTX - 1); 436 ctx = ctxbuf + ctx_start; 437 for (c = nch; c > 0; c--) { 438 *ctx = *idata++; 439 ctx += RESAMP_NCTX; 440 } 441 diff -= oblksz; 442 ifr--; 443 } else { 444 if (ofr == 0) 445 break; 446 447 for (c = 0; c < nch; c++) 448 f[c] = 0; 449 450 q = diff * p->filt_step; 451 n = ctx_start; 452 453 while (q < RESAMP_LENGTH) { 454 qi = q >> RESAMP_STEP_BITS; 455 qf = q & (RESAMP_STEP - 1); 456 s = resamp_filt[qi]; 457 ds = resamp_filt[qi + 1] - s; 458 s += (int64_t)qf * ds >> RESAMP_STEP_BITS; 459 ctx = ctxbuf; 460 for (c = 0; c < nch; c++) { 461 f[c] += (int64_t)ctx[n] * s; 462 ctx += RESAMP_NCTX; 463 } 464 q += p->filt_cutoff; 465 n = (n + 1) & (RESAMP_NCTX - 1); 466 } 467 468 for (c = 0; c < nch; c++) { 469 s = f[c] >> RESAMP_BITS; 470 s = (int64_t)s * p->filt_cutoff >> RESAMP_BITS; 471 #if ADATA_BITS == 16 472 /* 473 * In 16-bit mode, we've no room for filter 474 * overshoots, so we need to clip the signal 475 * to avoid 16-bit integers to wrap around. 476 * In 24-bit mode, samples may exceed the 477 * [-1:1] range. Later, cmap_add() will clip 478 * them, so no need to clip them here as well. 479 */ 480 if (s >= ADATA_UNIT) 481 s = ADATA_UNIT - 1; 482 else if (s < -ADATA_UNIT) 483 s = -ADATA_UNIT; 484 #endif 485 *odata++ = s; 486 } 487 488 diff += iblksz; 489 ofr--; 490 } 491 } 492 p->diff = diff; 493 p->ctx_start = ctx_start; 494 #ifdef DEBUG 495 if (ifr != 0) { 496 log_puts("resamp_do: "); 497 log_puti(ifr); 498 log_puts(": too many input frames\n"); 499 panic(); 500 } 501 if (ofr != 0) { 502 log_puts("resamp_do: "); 503 log_puti(ofr); 504 log_puts(": too many output frames\n"); 505 panic(); 506 } 507 #endif 508 } 509 510 static unsigned int 511 uint_gcd(unsigned int a, unsigned int b) 512 { 513 unsigned int r; 514 515 while (b > 0) { 516 r = a % b; 517 a = b; 518 b = r; 519 } 520 return a; 521 } 522 523 /* 524 * initialize resampler with ibufsz/obufsz factor and "nch" channels 525 */ 526 void 527 resamp_init(struct resamp *p, unsigned int iblksz, 528 unsigned int oblksz, int nch) 529 { 530 unsigned int g; 531 532 /* 533 * reduce iblksz/oblksz fraction 534 */ 535 g = uint_gcd(iblksz, oblksz); 536 iblksz /= g; 537 oblksz /= g; 538 539 /* 540 * ensure weird rates don't cause integer overflow 541 */ 542 while (iblksz > ADATA_UNIT || oblksz > ADATA_UNIT) { 543 iblksz >>= 1; 544 oblksz >>= 1; 545 } 546 547 p->iblksz = iblksz; 548 p->oblksz = oblksz; 549 p->diff = 0; 550 p->nch = nch; 551 p->ctx_start = 0; 552 memset(p->ctx, 0, sizeof(p->ctx)); 553 if (p->iblksz < p->oblksz) { 554 p->filt_cutoff = RESAMP_UNIT; 555 p->filt_step = RESAMP_UNIT / p->oblksz; 556 } else { 557 p->filt_cutoff = (int64_t)RESAMP_UNIT * p->oblksz / p->iblksz; 558 p->filt_step = RESAMP_UNIT / p->iblksz; 559 } 560 #ifdef DEBUG 561 if (log_level >= 3) { 562 log_puts("resamp: "); 563 log_putu(iblksz); 564 log_puts("/"); 565 log_putu(oblksz); 566 log_puts("\n"); 567 } 568 #endif 569 } 570 571 /* 572 * encode "todo" frames from native to foreign encoding 573 */ 574 void 575 enc_do(struct conv *p, unsigned char *in, unsigned char *out, int todo) 576 { 577 unsigned int f; 578 adata_t *idata; 579 unsigned int s; 580 unsigned int oshift; 581 unsigned int obias; 582 unsigned int obps; 583 unsigned int i; 584 unsigned char *odata; 585 int obnext; 586 int osnext; 587 588 #ifdef DEBUG 589 if (log_level >= 4) { 590 log_puts("enc: copying "); 591 log_putu(todo); 592 log_puts(" frames\n"); 593 } 594 #endif 595 /* 596 * Partially copy structures into local variables, to avoid 597 * unnecessary indirections; this also allows the compiler to 598 * order local variables more "cache-friendly". 599 */ 600 idata = (adata_t *)in; 601 odata = out; 602 oshift = p->shift; 603 obias = p->bias; 604 obps = p->bps; 605 obnext = p->bnext; 606 osnext = p->snext; 607 608 /* 609 * Start conversion. 610 */ 611 odata += p->bfirst; 612 for (f = todo * p->nch; f > 0; f--) { 613 /* convert adata to u32 */ 614 s = (int)*idata++ + ADATA_UNIT; 615 s <<= 32 - ADATA_BITS; 616 /* convert u32 to uN */ 617 s >>= oshift; 618 /* convert uN to sN */ 619 s -= obias; 620 /* packetize sN */ 621 for (i = obps; i > 0; i--) { 622 *odata = (unsigned char)s; 623 s >>= 8; 624 odata += obnext; 625 } 626 odata += osnext; 627 } 628 } 629 630 /* 631 * store "todo" frames of silence in foreign encoding 632 */ 633 void 634 enc_sil_do(struct conv *p, unsigned char *out, int todo) 635 { 636 unsigned int f; 637 unsigned int s; 638 unsigned int oshift; 639 int obias; 640 unsigned int obps; 641 unsigned int i; 642 unsigned char *odata; 643 int obnext; 644 int osnext; 645 646 #ifdef DEBUG 647 if (log_level >= 4) { 648 log_puts("enc: silence "); 649 log_putu(todo); 650 log_puts(" frames\n"); 651 } 652 #endif 653 /* 654 * Partially copy structures into local variables, to avoid 655 * unnecessary indirections; this also allows the compiler to 656 * order local variables more "cache-friendly". 657 */ 658 odata = out; 659 oshift = p->shift; 660 obias = p->bias; 661 obps = p->bps; 662 obnext = p->bnext; 663 osnext = p->snext; 664 665 /* 666 * Start conversion. 667 */ 668 odata += p->bfirst; 669 for (f = todo * p->nch; f > 0; f--) { 670 s = ((1U << 31) >> oshift) - obias; 671 for (i = obps; i > 0; i--) { 672 *odata = (unsigned char)s; 673 s >>= 8; 674 odata += obnext; 675 } 676 odata += osnext; 677 } 678 } 679 680 /* 681 * initialize encoder from native to foreign encoding 682 */ 683 void 684 enc_init(struct conv *p, struct aparams *par, int nch) 685 { 686 p->nch = nch; 687 p->bps = par->bps; 688 if (par->msb) { 689 p->shift = 32 - par->bps * 8; 690 } else { 691 p->shift = 32 - par->bits; 692 } 693 if (par->sig) { 694 p->bias = (1U << 31) >> p->shift; 695 } else { 696 p->bias = 0; 697 } 698 if (!par->le) { 699 p->bfirst = par->bps - 1; 700 p->bnext = -1; 701 p->snext = 2 * par->bps; 702 } else { 703 p->bfirst = 0; 704 p->bnext = 1; 705 p->snext = 0; 706 } 707 #ifdef DEBUG 708 if (log_level >= 3) { 709 log_puts("enc: "); 710 aparams_log(par); 711 log_puts(", "); 712 log_puti(p->nch); 713 log_puts(" channels\n"); 714 } 715 #endif 716 } 717 718 /* 719 * decode "todo" frames from foreign to native encoding 720 */ 721 void 722 dec_do(struct conv *p, unsigned char *in, unsigned char *out, int todo) 723 { 724 unsigned int f; 725 unsigned int ibps; 726 unsigned int i; 727 unsigned int s = 0xdeadbeef; 728 unsigned char *idata; 729 int ibnext; 730 int isnext; 731 unsigned int ibias; 732 unsigned int ishift; 733 adata_t *odata; 734 735 #ifdef DEBUG 736 if (log_level >= 4) { 737 log_puts("dec: copying "); 738 log_putu(todo); 739 log_puts(" frames\n"); 740 } 741 #endif 742 /* 743 * Partially copy structures into local variables, to avoid 744 * unnecessary indirections; this also allows the compiler to 745 * order local variables more "cache-friendly". 746 */ 747 idata = in; 748 odata = (adata_t *)out; 749 ibps = p->bps; 750 ibnext = p->bnext; 751 ibias = p->bias; 752 ishift = p->shift; 753 isnext = p->snext; 754 755 /* 756 * Start conversion. 757 */ 758 idata += p->bfirst; 759 for (f = todo * p->nch; f > 0; f--) { 760 for (i = ibps; i > 0; i--) { 761 s <<= 8; 762 s |= *idata; 763 idata += ibnext; 764 } 765 idata += isnext; 766 s += ibias; 767 s <<= ishift; 768 s >>= 32 - ADATA_BITS; 769 *odata++ = s - ADATA_UNIT; 770 } 771 } 772 773 /* 774 * convert a 32-bit float to adata_t, clipping to -1:1, boundaries 775 * excluded 776 */ 777 static inline int 778 f32_to_adata(unsigned int x) 779 { 780 unsigned int s, e, m, y; 781 782 s = (x >> 31); 783 e = (x >> 23) & 0xff; 784 m = (x << 8) | 0x80000000; 785 786 /* 787 * f32 exponent is (e - 127) and the point is after the 31-th 788 * bit, thus the shift is: 789 * 790 * 31 - (BITS - 1) - (e - 127) 791 * 792 * to ensure output is in the 0..(2^BITS)-1 range, the minimum 793 * shift is 31 - (BITS - 1) + 1, and maximum shift is 31 794 */ 795 if (e < 127 - (ADATA_BITS - 1)) 796 y = 0; 797 else if (e >= 127) 798 y = ADATA_UNIT - 1; 799 else 800 y = m >> (127 + (32 - ADATA_BITS) - e); 801 return (y ^ -s) + s; 802 } 803 804 /* 805 * convert samples from little endian ieee 754 floats to adata_t 806 */ 807 void 808 dec_do_float(struct conv *p, unsigned char *in, unsigned char *out, int todo) 809 { 810 unsigned int f; 811 unsigned int i; 812 unsigned int s = 0xdeadbeef; 813 unsigned char *idata; 814 int ibnext; 815 int isnext; 816 adata_t *odata; 817 818 #ifdef DEBUG 819 if (log_level >= 4) { 820 log_puts("dec_float: copying "); 821 log_putu(todo); 822 log_puts(" frames\n"); 823 } 824 #endif 825 /* 826 * Partially copy structures into local variables, to avoid 827 * unnecessary indirections; this also allows the compiler to 828 * order local variables more "cache-friendly". 829 */ 830 idata = in; 831 odata = (adata_t *)out; 832 ibnext = p->bnext; 833 isnext = p->snext; 834 835 /* 836 * Start conversion. 837 */ 838 idata += p->bfirst; 839 for (f = todo * p->nch; f > 0; f--) { 840 for (i = 4; i > 0; i--) { 841 s <<= 8; 842 s |= *idata; 843 idata += ibnext; 844 } 845 idata += isnext; 846 *odata++ = f32_to_adata(s); 847 } 848 } 849 850 /* 851 * convert samples from ulaw/alaw to adata_t 852 */ 853 void 854 dec_do_ulaw(struct conv *p, unsigned char *in, 855 unsigned char *out, int todo, int is_alaw) 856 { 857 unsigned int f; 858 unsigned char *idata; 859 adata_t *odata; 860 const short *map; 861 862 #ifdef DEBUG 863 if (log_level >= 4) { 864 log_puts("dec_ulaw: copying "); 865 log_putu(todo); 866 log_puts(" frames\n"); 867 } 868 #endif 869 map = is_alaw ? dec_alawmap : dec_ulawmap; 870 idata = in; 871 odata = (adata_t *)out; 872 for (f = todo * p->nch; f > 0; f--) 873 *odata++ = map[*idata++] << (ADATA_BITS - 16); 874 } 875 876 /* 877 * initialize decoder from foreign to native encoding 878 */ 879 void 880 dec_init(struct conv *p, struct aparams *par, int nch) 881 { 882 p->bps = par->bps; 883 p->nch = nch; 884 if (par->msb) { 885 p->shift = 32 - par->bps * 8; 886 } else { 887 p->shift = 32 - par->bits; 888 } 889 if (par->sig) { 890 p->bias = (1U << 31) >> p->shift; 891 } else { 892 p->bias = 0; 893 } 894 if (par->le) { 895 p->bfirst = par->bps - 1; 896 p->bnext = -1; 897 p->snext = 2 * par->bps; 898 } else { 899 p->bfirst = 0; 900 p->bnext = 1; 901 p->snext = 0; 902 } 903 #ifdef DEBUG 904 if (log_level >= 3) { 905 log_puts("dec: "); 906 aparams_log(par); 907 log_puts(", "); 908 log_puti(p->nch); 909 log_puts(" channels\n"); 910 } 911 #endif 912 } 913 914 /* 915 * mix "todo" input frames on the output with the given volume 916 */ 917 void 918 cmap_add(struct cmap *p, void *in, void *out, int vol, int todo) 919 { 920 adata_t *idata, *odata; 921 int i, j, nch, istart, inext, onext, ostart, y, v; 922 923 #ifdef DEBUG 924 if (log_level >= 4) { 925 log_puts("cmap: adding "); 926 log_puti(todo); 927 log_puts(" frames\n"); 928 } 929 #endif 930 idata = in; 931 odata = out; 932 ostart = p->ostart; 933 onext = p->onext; 934 istart = p->istart; 935 inext = p->inext; 936 nch = p->nch; 937 v = vol; 938 939 /* 940 * map/mix input on the output 941 */ 942 for (i = todo; i > 0; i--) { 943 odata += ostart; 944 idata += istart; 945 for (j = nch; j > 0; j--) { 946 y = *odata + ADATA_MUL(*idata, v); 947 if (y >= ADATA_UNIT) 948 y = ADATA_UNIT - 1; 949 else if (y < -ADATA_UNIT) 950 y = -ADATA_UNIT; 951 *odata = y; 952 idata++; 953 odata++; 954 } 955 odata += onext; 956 idata += inext; 957 } 958 } 959 960 /* 961 * overwrite output with "todo" input frames with the given volume 962 */ 963 void 964 cmap_copy(struct cmap *p, void *in, void *out, int vol, int todo) 965 { 966 adata_t *idata, *odata; 967 int i, j, nch, istart, inext, onext, ostart, v; 968 969 #ifdef DEBUG 970 if (log_level >= 4) { 971 log_puts("cmap: copying "); 972 log_puti(todo); 973 log_puts(" frames\n"); 974 } 975 #endif 976 idata = in; 977 odata = out; 978 ostart = p->ostart; 979 onext = p->onext; 980 istart = p->istart; 981 inext = p->inext; 982 nch = p->nch; 983 v = vol; 984 985 /* 986 * copy to the output buffer 987 */ 988 for (i = todo; i > 0; i--) { 989 idata += istart; 990 odata += ostart; 991 for (j = nch; j > 0; j--) { 992 *odata = ADATA_MUL(*idata, v); 993 odata++; 994 idata++; 995 } 996 odata += onext; 997 idata += inext; 998 } 999 } 1000 1001 /* 1002 * initialize channel mapper, to map a subset of input channel range 1003 * into a subset of the output channel range 1004 */ 1005 void 1006 cmap_init(struct cmap *p, 1007 int imin, int imax, int isubmin, int isubmax, 1008 int omin, int omax, int osubmin, int osubmax) 1009 { 1010 int inch, onch, nch; 1011 1012 /* 1013 * Ignore channels outside of the available sets 1014 */ 1015 if (isubmin < imin) 1016 isubmin = imin; 1017 if (isubmax > imax) 1018 isubmax = imax; 1019 if (osubmin < omin) 1020 osubmin = omin; 1021 if (osubmax > omax) 1022 osubmax = omax; 1023 1024 /* 1025 * Shrink the input or the output subset to make both subsets of 1026 * the same size 1027 */ 1028 inch = isubmax - isubmin + 1; 1029 onch = osubmax - osubmin + 1; 1030 nch = (inch < onch) ? inch : onch; 1031 isubmax = isubmin + nch - 1; 1032 osubmax = osubmin + nch - 1; 1033 1034 p->ostart = osubmin - omin; 1035 p->onext = omax - osubmax; 1036 p->istart = isubmin - imin; 1037 p->inext = imax - isubmax; 1038 p->nch = nch; 1039 #ifdef DEBUG 1040 if (log_level >= 3) { 1041 log_puts("cmap: nch = "); 1042 log_puti(p->nch); 1043 log_puts(", ostart = "); 1044 log_puti(p->ostart); 1045 log_puts(", onext = "); 1046 log_puti(p->onext); 1047 log_puts(", istart = "); 1048 log_puti(p->istart); 1049 log_puts(", inext = "); 1050 log_puti(p->inext); 1051 log_puts("\n"); 1052 } 1053 #endif 1054 } 1055