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