1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2012-2020 Hans Petter Selasky
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28 #include <sys/queue.h>
29 #include <sys/soundcard.h>
30
31 #include <stdint.h>
32 #include <string.h>
33
34 #include "int.h"
35
36 void
format_import(uint32_t fmt,const uint8_t * src,uint32_t len,int64_t * dst)37 format_import(uint32_t fmt, const uint8_t *src, uint32_t len,
38 int64_t *dst)
39 {
40 const uint8_t *end = src + len;
41 int64_t val;
42
43 if (fmt & AFMT_16BIT) {
44 while (src != end) {
45 if (fmt & (AFMT_S16_LE | AFMT_U16_LE))
46 val = src[0] | (src[1] << 8);
47 else
48 val = src[1] | (src[0] << 8);
49
50 src += 2;
51
52 if (fmt & (AFMT_U16_LE | AFMT_U16_BE))
53 val = val ^ 0x8000;
54
55 val <<= (64 - 16);
56 val >>= (64 - 16);
57
58 *dst++ = val;
59 }
60
61 } else if (fmt & AFMT_24BIT) {
62 while (src < end) {
63 if (fmt & (AFMT_S24_LE | AFMT_U24_LE))
64 val = src[0] | (src[1] << 8) | (src[2] << 16);
65 else
66 val = src[2] | (src[1] << 8) | (src[0] << 16);
67
68 src += 3;
69
70 if (fmt & (AFMT_U24_LE | AFMT_U24_BE))
71 val = val ^ 0x800000;
72
73 val <<= (64 - 24);
74 val >>= (64 - 24);
75
76 *dst++ = val;
77 }
78 } else if (fmt & AFMT_32BIT) {
79 while (src < end) {
80 int64_t e, m, s;
81
82 if (fmt & (AFMT_S32_LE | AFMT_U32_LE | AFMT_F32_LE))
83 val = src[0] | (src[1] << 8) | (src[2] << 16) | (src[3] << 24);
84 else
85 val = src[3] | (src[2] << 8) | (src[1] << 16) | (src[0] << 24);
86
87 src += 4;
88
89 if (fmt & (AFMT_U32_LE | AFMT_U32_BE))
90 val = val ^ 0x80000000LL;
91
92 if (fmt & (AFMT_F32_LE | AFMT_F32_BE)) {
93 e = (val >> 23) & 0xff;
94 /* NaN, +/- Inf or too small */
95 if (e == 0xff || e < 96) {
96 val = 0;
97 goto skip;
98 }
99 s = val & 0x80000000U;
100 if (e > 126) {
101 val = s == 0 ? format_max(fmt) :
102 -0x80000000LL;
103 goto skip;
104 }
105 m = 0x800000 | (val & 0x7fffff);
106 e += 8 - 127;
107 if (e < 0)
108 m >>= -e;
109 else
110 m <<= e;
111 val = s == 0 ? m : -m;
112 }
113 skip:
114 val <<= (64 - 32);
115 val >>= (64 - 32);
116
117 *dst++ = val;
118 }
119
120 } else if (fmt & AFMT_8BIT) {
121 while (src < end) {
122 val = src[0];
123
124 src += 1;
125
126 if (fmt & AFMT_U8)
127 val = val ^ 0x80;
128
129 val <<= (64 - 8);
130 val >>= (64 - 8);
131
132 *dst++ = val;
133 }
134 }
135 }
136
137 void
format_export(uint32_t fmt,const int64_t * src,uint8_t * dst,uint32_t len)138 format_export(uint32_t fmt, const int64_t *src, uint8_t *dst, uint32_t len)
139 {
140 const uint8_t *end = dst + len;
141 int64_t val;
142
143 if (fmt & AFMT_16BIT) {
144 while (dst != end) {
145
146 val = *src++;
147
148 if (val > 0x7FFF)
149 val = 0x7FFF;
150 else if (val < -0x7FFF)
151 val = -0x7FFF;
152
153 if (fmt & (AFMT_U16_LE | AFMT_U16_BE))
154 val = val ^ 0x8000;
155
156 if (fmt & (AFMT_S16_LE | AFMT_U16_LE)) {
157 dst[0] = val;
158 dst[1] = val >> 8;
159 } else {
160 dst[1] = val;
161 dst[0] = val >> 8;
162 }
163
164 dst += 2;
165 }
166
167 } else if (fmt & AFMT_24BIT) {
168 while (dst != end) {
169
170 val = *src++;
171
172 if (val > 0x7FFFFF)
173 val = 0x7FFFFF;
174 else if (val < -0x7FFFFF)
175 val = -0x7FFFFF;
176
177 if (fmt & (AFMT_U24_LE | AFMT_U24_BE))
178 val = val ^ 0x800000;
179
180 if (fmt & (AFMT_S24_LE | AFMT_U24_LE)) {
181 dst[0] = val;
182 dst[1] = val >> 8;
183 dst[2] = val >> 16;
184 } else {
185 dst[2] = val;
186 dst[1] = val >> 8;
187 dst[0] = val >> 16;
188 }
189
190 dst += 3;
191 }
192 } else if (fmt & AFMT_32BIT) {
193 while (dst != end) {
194 int64_t r, e;
195
196 val = *src++;
197
198 if (val > 0x7FFFFFFFLL)
199 val = 0x7FFFFFFFLL;
200 else if (val < -0x7FFFFFFFLL)
201 val = -0x7FFFFFFFLL;
202
203 if (fmt & (AFMT_F32_LE | AFMT_F32_BE)) {
204 if (val == 0)
205 r = 0;
206 else if (val == format_max(fmt))
207 r = 0x3f800000;
208 else if (val == -0x80000000LL)
209 r = 0x80000000U | 0x3f800000;
210 else {
211 r = 0;
212 if (val < 0) {
213 r |= 0x80000000U;
214 val = -val;
215 }
216 e = 127 - 8;
217 while ((val & 0x7f000000) != 0) {
218 val >>= 1;
219 e++;
220 }
221 while ((val & 0x7f800000) == 0) {
222 val <<= 1;
223 e--;
224 }
225 r |= (e & 0xff) << 23;
226 r |= val & 0x7fffff;
227 }
228 val = r;
229 }
230
231 if (fmt & (AFMT_U32_LE | AFMT_U32_BE))
232 val = val ^ 0x80000000LL;
233
234 if (fmt & (AFMT_S32_LE | AFMT_U32_LE | AFMT_F32_LE)) {
235 dst[0] = val;
236 dst[1] = val >> 8;
237 dst[2] = val >> 16;
238 dst[3] = val >> 24;
239 } else {
240 dst[3] = val;
241 dst[2] = val >> 8;
242 dst[1] = val >> 16;
243 dst[0] = val >> 24;
244 }
245
246 dst += 4;
247 }
248
249 } else if (fmt & AFMT_8BIT) {
250 while (dst != end) {
251
252 val = *src++;
253
254 if (val > 0x7F)
255 val = 0x7F;
256 else if (val < -0x7F)
257 val = -0x7F;
258
259 if (fmt & (AFMT_U8))
260 val = val ^ 0x80;
261
262 dst[0] = val;
263
264 dst += 1;
265 }
266 }
267 }
268
269 int64_t
format_max(uint32_t fmt)270 format_max(uint32_t fmt)
271 {
272 if (fmt & AFMT_16BIT)
273 return (0x7FFF);
274 else if (fmt & AFMT_24BIT)
275 return (0x7FFFFF);
276 else if (fmt & AFMT_32BIT)
277 return (0x7FFFFFFF);
278 else if (fmt & AFMT_8BIT)
279 return (0x7F);
280 return (0);
281 }
282
283 void
format_maximum(const int64_t * src,int64_t * dst,uint32_t ch,uint32_t samples,int8_t shift)284 format_maximum(const int64_t *src, int64_t *dst, uint32_t ch,
285 uint32_t samples, int8_t shift)
286 {
287 const int64_t *end = src + (samples * ch);
288 int64_t max[ch];
289 int64_t temp;
290 uint32_t x;
291
292 memset(max, 0, sizeof(max));
293
294 while (src != end) {
295 for (x = 0; x != ch; x++) {
296 temp = *src++;
297 if (temp < 0)
298 temp = -temp;
299 if (temp > max[x])
300 max[x] = temp;
301 }
302 }
303
304 for (x = 0; x != ch; x++) {
305 if (shift < 0)
306 max[x] >>= -shift;
307 else
308 max[x] <<= shift;
309 if (dst[x] < max[x])
310 dst[x] = max[x];
311 }
312 }
313
314 void
format_remix(int64_t * buffer_data,uint32_t in_chans,uint32_t out_chans,uint32_t samples)315 format_remix(int64_t *buffer_data, uint32_t in_chans,
316 uint32_t out_chans, uint32_t samples)
317 {
318 uint32_t x;
319
320 if (out_chans > in_chans) {
321 uint32_t dst = out_chans * (samples - 1);
322 uint32_t src = in_chans * (samples - 1);
323 uint32_t fill = out_chans - in_chans;
324
325 for (x = 0; x != samples; x++) {
326 memset(buffer_data + dst + in_chans, 0, 8 * fill);
327 if (src != dst) {
328 memcpy(buffer_data + dst,
329 buffer_data + src,
330 in_chans * 8);
331 }
332 dst -= out_chans;
333 src -= in_chans;
334 }
335 } else if (out_chans < in_chans) {
336 uint32_t dst = 0;
337 uint32_t src = 0;
338
339 for (x = 0; x != samples; x++) {
340 if (src != dst) {
341 memcpy(buffer_data + dst,
342 buffer_data + src,
343 out_chans * 8);
344 }
345 dst += out_chans;
346 src += in_chans;
347 }
348 }
349 }
350
351 void
format_silence(uint32_t fmt,uint8_t * dst,uint32_t len)352 format_silence(uint32_t fmt, uint8_t *dst, uint32_t len)
353 {
354 const uint8_t *end = dst + len;
355
356 if (fmt & AFMT_16BIT) {
357 uint16_t val;
358
359 if (fmt & (AFMT_U16_LE | AFMT_U16_BE))
360 val = 1U << 15;
361 else
362 val = 0;
363
364 while (dst != end) {
365 if (fmt & (AFMT_S16_LE | AFMT_U16_LE)) {
366 dst[0] = val;
367 dst[1] = val >> 8;
368 } else {
369 dst[1] = val;
370 dst[0] = val >> 8;
371 }
372 dst += 2;
373 }
374
375 } else if (fmt & AFMT_24BIT) {
376 uint32_t val;
377
378 if (fmt & (AFMT_U24_LE | AFMT_U24_BE))
379 val = 1U << 23;
380 else
381 val = 0;
382
383 while (dst != end) {
384 if (fmt & (AFMT_S24_LE | AFMT_U24_LE)) {
385 dst[0] = val;
386 dst[1] = val >> 8;
387 dst[2] = val >> 16;
388 } else {
389 dst[2] = val;
390 dst[1] = val >> 8;
391 dst[0] = val >> 16;
392 }
393 dst += 3;
394 }
395 } else if (fmt & AFMT_32BIT) {
396 uint32_t val;
397
398 if (fmt & (AFMT_U32_LE | AFMT_U32_BE))
399 val = 1U << 31;
400 else
401 val = 0;
402
403 while (dst != end) {
404 if (fmt & (AFMT_S32_LE | AFMT_U32_LE | AFMT_F32_LE)) {
405 dst[0] = val;
406 dst[1] = val >> 8;
407 dst[2] = val >> 16;
408 dst[3] = val >> 24;
409 } else {
410 dst[3] = val;
411 dst[2] = val >> 8;
412 dst[1] = val >> 16;
413 dst[0] = val >> 24;
414 }
415 dst += 4;
416 }
417
418 } else if (fmt & AFMT_8BIT) {
419 uint8_t val;
420
421 if (fmt & AFMT_U8)
422 val = 1U << 7;
423 else
424 val = 0;
425
426 while (dst != end) {
427 dst[0] = val;
428 dst += 1;
429 }
430 }
431 }
432