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
aparams_enctostr(struct aparams * par,char * ostr)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
aparams_strtoenc(struct aparams * par,char * istr)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
aparams_init(struct aparams * par)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
aparams_native(struct aparams * par)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
resamp_getcnt(struct resamp * p,int * icnt,int * ocnt)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
resamp_do(struct resamp * p,adata_t * in,adata_t * out,int icnt,int ocnt)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
uint_gcd(unsigned int a,unsigned int b)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
resamp_init(struct resamp * p,unsigned int iblksz,unsigned int oblksz,int nch)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
enc_do(struct conv * p,unsigned char * in,unsigned char * out,int todo)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
enc_sil_do(struct conv * p,unsigned char * out,int todo)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
enc_init(struct conv * p,struct aparams * par,int nch)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
dec_do(struct conv * p,unsigned char * in,unsigned char * out,int todo)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
f32_to_adata(unsigned int x)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
dec_do_float(struct conv * p,unsigned char * in,unsigned char * out,int todo)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
dec_do_ulaw(struct conv * p,unsigned char * in,unsigned char * out,int todo,int is_alaw)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
dec_init(struct conv * p,struct aparams * par,int nch)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
cmap_add(struct cmap * p,void * in,void * out,int vol,int todo)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
cmap_copy(struct cmap * p,void * in,void * out,int vol,int todo)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
cmap_init(struct cmap * p,int imin,int imax,int isubmin,int isubmax,int omin,int omax,int osubmin,int osubmax)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