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