1 | /* $NetBSD: aurateconv.c,v 1.19 2011/11/23 23:07:31 jmcneill Exp $ */ |
2 | |
3 | /*- |
4 | * Copyright (c) 2002 The NetBSD Foundation, Inc. |
5 | * All rights reserved. |
6 | * |
7 | * This code is derived from software contributed to The NetBSD Foundation |
8 | * by TAMURA Kent |
9 | * |
10 | * Redistribution and use in source and binary forms, with or without |
11 | * modification, are permitted provided that the following conditions |
12 | * are met: |
13 | * 1. Redistributions of source code must retain the above copyright |
14 | * notice, this list of conditions and the following disclaimer. |
15 | * 2. Redistributions in binary form must reproduce the above copyright |
16 | * notice, this list of conditions and the following disclaimer in the |
17 | * documentation and/or other materials provided with the distribution. |
18 | * |
19 | * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS |
20 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
21 | * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
22 | * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS |
23 | * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
29 | * POSSIBILITY OF SUCH DAMAGE. |
30 | */ |
31 | |
32 | #include <sys/cdefs.h> |
33 | __KERNEL_RCSID(0, "$NetBSD: aurateconv.c,v 1.19 2011/11/23 23:07:31 jmcneill Exp $" ); |
34 | |
35 | #include <sys/systm.h> |
36 | #include <sys/types.h> |
37 | #include <sys/device.h> |
38 | #include <sys/errno.h> |
39 | #include <sys/malloc.h> |
40 | #include <sys/select.h> |
41 | #include <sys/audioio.h> |
42 | |
43 | #include <dev/audio_if.h> |
44 | #include <dev/audiovar.h> |
45 | #include <dev/auconv.h> |
46 | |
47 | #ifndef _KERNEL |
48 | #include <stdio.h> |
49 | #include <string.h> |
50 | #endif |
51 | |
52 | /* #define AURATECONV_DEBUG */ |
53 | #ifdef AURATECONV_DEBUG |
54 | #define DPRINTF(x) printf x |
55 | #else |
56 | #define DPRINTF(x) |
57 | #endif |
58 | |
59 | typedef struct aurateconv { |
60 | stream_filter_t base; |
61 | audio_params_t from; |
62 | audio_params_t to; |
63 | long count; |
64 | int32_t prev[AUDIO_MAX_CHANNELS]; |
65 | int32_t next[AUDIO_MAX_CHANNELS]; |
66 | } aurateconv_t; |
67 | |
68 | static int aurateconv_fetch_to(struct audio_softc *, stream_fetcher_t *, |
69 | audio_stream_t *, int); |
70 | static void aurateconv_dtor(stream_filter_t *); |
71 | static int aurateconv_slinear16_LE(aurateconv_t *, audio_stream_t *, |
72 | int, int, int); |
73 | static int aurateconv_slinear24_LE(aurateconv_t *, audio_stream_t *, |
74 | int, int, int); |
75 | static int aurateconv_slinear32_LE(aurateconv_t *, audio_stream_t *, |
76 | int, int, int); |
77 | static int aurateconv_slinear16_BE(aurateconv_t *, audio_stream_t *, |
78 | int, int, int); |
79 | static int aurateconv_slinear24_BE(aurateconv_t *, audio_stream_t *, |
80 | int, int, int); |
81 | static int aurateconv_slinear32_BE(aurateconv_t *, audio_stream_t *, |
82 | int, int, int); |
83 | |
84 | static int32_t int32_mask[33] = { |
85 | 0x0, 0x80000000, 0xc0000000, 0xe0000000, |
86 | 0xf0000000, 0xf8000000, 0xfc000000, 0xfe000000, |
87 | 0xff000000, 0xff800000, 0xffc00000, 0xffe00000, |
88 | 0xfff00000, 0xfff80000, 0xfffc0000, 0xfffe0000, |
89 | 0xffff0000, 0xffff8000, 0xffffc000, 0xffffe000, |
90 | 0xfffff000, 0xfffff800, 0xfffffc00, 0xfffffe00, |
91 | 0xffffff00, 0xffffff80, 0xffffffc0, 0xffffffe0, |
92 | 0xfffffff0, 0xfffffff8, 0xfffffffc, 0xfffffffe, |
93 | 0xffffffff |
94 | }; |
95 | |
96 | stream_filter_t * |
97 | aurateconv(struct audio_softc *sc, const audio_params_t *from, |
98 | const audio_params_t *to) |
99 | { |
100 | aurateconv_t *this; |
101 | |
102 | DPRINTF(("Construct '%s' filter: rate=%u:%u chan=%u:%u prec=%u/%u:%u/" |
103 | "%u enc=%u:%u\n" , __func__, from->sample_rate, |
104 | to->sample_rate, from->channels, to->channels, |
105 | from->validbits, from->precision, to->validbits, |
106 | to->precision, from->encoding, to->encoding)); |
107 | #ifdef DIAGNOSTIC |
108 | /* check from/to */ |
109 | if (from->channels == to->channels |
110 | && from->sample_rate == to->sample_rate) |
111 | printf("%s: no conversion\n" , __func__); /* No conversion */ |
112 | |
113 | if (from->encoding != to->encoding |
114 | || from->precision != to->precision |
115 | || from->validbits != to->validbits) { |
116 | printf("%s: encoding/precision must not be changed\n" , __func__); |
117 | return NULL; |
118 | } |
119 | if ((from->encoding != AUDIO_ENCODING_SLINEAR_LE |
120 | && from->encoding != AUDIO_ENCODING_SLINEAR_BE) |
121 | || (from->precision != 16 && from->precision != 24 && from->precision != 32)) { |
122 | printf("%s: encoding/precision must be SLINEAR_LE 16/24/32bit, " |
123 | "or SLINEAR_BE 16/24/32bit" , __func__); |
124 | return NULL; |
125 | } |
126 | |
127 | if (from->channels > AUDIO_MAX_CHANNELS || from->channels <= 0 |
128 | || to->channels > AUDIO_MAX_CHANNELS || to->channels <= 0) { |
129 | printf("%s: invalid channels: from=%u to=%u\n" , |
130 | __func__, from->channels, to->channels); |
131 | return NULL; |
132 | } |
133 | |
134 | if (from->sample_rate <= 0 || to->sample_rate <= 0) { |
135 | printf("%s: invalid sampling rate: from=%u to=%u\n" , |
136 | __func__, from->sample_rate, to->sample_rate); |
137 | return NULL; |
138 | } |
139 | #endif |
140 | |
141 | /* initialize context */ |
142 | this = malloc(sizeof(aurateconv_t), M_DEVBUF, M_WAITOK | M_ZERO); |
143 | this->count = from->sample_rate < to->sample_rate |
144 | ? to->sample_rate + from->sample_rate : 0; |
145 | this->from = *from; |
146 | this->to = *to; |
147 | |
148 | /* initialize vtbl */ |
149 | this->base.base.fetch_to = aurateconv_fetch_to; |
150 | this->base.dtor = aurateconv_dtor; |
151 | this->base.set_fetcher = stream_filter_set_fetcher; |
152 | this->base.set_inputbuffer = stream_filter_set_inputbuffer; |
153 | return &this->base; |
154 | } |
155 | |
156 | static void |
157 | aurateconv_dtor(struct stream_filter *this) |
158 | { |
159 | if (this != NULL) |
160 | free(this, M_DEVBUF); |
161 | } |
162 | |
163 | static int |
164 | aurateconv_fetch_to(struct audio_softc *sc, stream_fetcher_t *self, |
165 | audio_stream_t *dst, int max_used) |
166 | { |
167 | aurateconv_t *this; |
168 | int m, err, frame_dst, frame_src; |
169 | |
170 | this = (aurateconv_t *)self; |
171 | frame_dst = (this->to.precision / 8) * this->to.channels; |
172 | frame_src = (this->from.precision / 8) * this->from.channels; |
173 | max_used = max_used / frame_dst * frame_dst; |
174 | if (max_used <= 0) |
175 | max_used = frame_dst; |
176 | /* calculate required input size for output max_used bytes */ |
177 | m = max_used / frame_dst; |
178 | m *= this->from.sample_rate; |
179 | m /= this->to.sample_rate; |
180 | m *= frame_src; |
181 | if (m <= 0) |
182 | m = frame_src; |
183 | |
184 | if ((err = this->base.prev->fetch_to(sc, this->base.prev, this->base.src, m))) |
185 | return err; |
186 | m = (dst->end - dst->start) / frame_dst * frame_dst; |
187 | m = min(m, max_used); |
188 | |
189 | switch (this->from.encoding) { |
190 | case AUDIO_ENCODING_SLINEAR_LE: |
191 | switch (this->from.precision) { |
192 | case 16: |
193 | return aurateconv_slinear16_LE(this, dst, m, |
194 | frame_src, frame_dst); |
195 | case 24: |
196 | return aurateconv_slinear24_LE(this, dst, m, |
197 | frame_src, frame_dst); |
198 | case 32: |
199 | return aurateconv_slinear32_LE(this, dst, m, |
200 | frame_src, frame_dst); |
201 | } |
202 | break; |
203 | case AUDIO_ENCODING_SLINEAR_BE: |
204 | switch (this->from.precision) { |
205 | case 16: |
206 | return aurateconv_slinear16_BE(this, dst, m, |
207 | frame_src, frame_dst); |
208 | case 24: |
209 | return aurateconv_slinear24_BE(this, dst, m, |
210 | frame_src, frame_dst); |
211 | case 32: |
212 | return aurateconv_slinear32_BE(this, dst, m, |
213 | frame_src, frame_dst); |
214 | } |
215 | break; |
216 | } |
217 | printf("%s: internal error: unsupported encoding: enc=%u prec=%u\n" , |
218 | __func__, this->from.encoding, this->from.precision); |
219 | return 0; |
220 | } |
221 | |
222 | |
223 | #define READ_S8LE(P) *(const int8_t*)(P) |
224 | #define WRITE_S8LE(P, V) *(int8_t*)(P) = V |
225 | #define READ_S8BE(P) *(const int8_t*)(P) |
226 | #define WRITE_S8BE(P, V) *(int8_t*)(P) = V |
227 | #if BYTE_ORDER == LITTLE_ENDIAN |
228 | # define READ_S16LE(P) *(const int16_t*)(P) |
229 | # define WRITE_S16LE(P, V) *(int16_t*)(P) = V |
230 | # define READ_S16BE(P) (int16_t)((P)[0] | ((P)[1]<<8)) |
231 | # define WRITE_S16BE(P, V) \ |
232 | do { \ |
233 | int vv = V; \ |
234 | (P)[0] = vv; \ |
235 | (P)[1] = vv >> 8; \ |
236 | } while (/*CONSTCOND*/ 0) |
237 | # define READ_S32LE(P) *(const int32_t*)(P) |
238 | # define WRITE_S32LE(P, V) *(int32_t*)(P) = V |
239 | # define READ_S32BE(P) (int32_t)((P)[3] | ((P)[2]<<8) | ((P)[1]<<16) | (((int8_t)((P)[0]))<<24)) |
240 | # define WRITE_S32BE(P, V) \ |
241 | do { \ |
242 | int vvv = V; \ |
243 | (P)[0] = vvv >> 24; \ |
244 | (P)[1] = vvv >> 16; \ |
245 | (P)[2] = vvv >> 8; \ |
246 | (P)[3] = vvv; \ |
247 | } while (/*CONSTCOND*/ 0) |
248 | #else /* !LITTLE_ENDIAN */ |
249 | # define READ_S16LE(P) (int16_t)((P)[0] | ((P)[1]<<8)) |
250 | # define WRITE_S16LE(P, V) \ |
251 | do { \ |
252 | int vv = V; \ |
253 | (P)[0] = vv; \ |
254 | (P)[1] = vv >> 8; \ |
255 | } while (/*CONSTCOND*/ 0) |
256 | # define READ_S16BE(P) *(const int16_t*)(P) |
257 | # define WRITE_S16BE(P, V) *(int16_t*)(P) = V |
258 | # define READ_S32LE(P) (int32_t)((P)[0] | ((P)[1]<<8) | ((P)[2]<<16) | (((int8_t)((P)[3]))<<24)) |
259 | # define WRITE_S32LE(P, V) \ |
260 | do { \ |
261 | int vvv = V; \ |
262 | (P)[0] = vvv; \ |
263 | (P)[1] = vvv >> 8; \ |
264 | (P)[2] = vvv >> 16; \ |
265 | (P)[3] = vvv >> 24; \ |
266 | } while (/*CONSTCOND*/ 0) |
267 | # define READ_S32BE(P) *(const int32_t*)(P) |
268 | # define WRITE_S32BE(P, V) *(int32_t*)(P) = V |
269 | #endif /* !LITTLE_ENDIAN */ |
270 | #define READ_S24LE(P) (int32_t)((P)[0] | ((P)[1]<<8) | (((int8_t)((P)[2]))<<16)) |
271 | #define WRITE_S24LE(P, V) \ |
272 | do { \ |
273 | int vvv = V; \ |
274 | (P)[0] = vvv; \ |
275 | (P)[1] = vvv >> 8; \ |
276 | (P)[2] = vvv >> 16; \ |
277 | } while (/*CONSTCOND*/ 0) |
278 | #define READ_S24BE(P) (int32_t)((P)[2] | ((P)[1]<<8) | (((int8_t)((P)[0]))<<16)) |
279 | #define WRITE_S24BE(P, V) \ |
280 | do { \ |
281 | int vvv = V; \ |
282 | (P)[0] = vvv >> 16; \ |
283 | (P)[1] = vvv >> 8; \ |
284 | (P)[2] = vvv; \ |
285 | } while (/*CONSTCOND*/ 0) |
286 | |
287 | #define READ_Sn(BITS, EN, V, STREAM, RP, PAR) \ |
288 | do { \ |
289 | int j; \ |
290 | for (j = 0; j < (PAR)->channels; j++) { \ |
291 | (V)[j] = READ_S##BITS##EN(RP); \ |
292 | RP = audio_stream_add_outp(STREAM, RP, (BITS) / NBBY); \ |
293 | } \ |
294 | } while (/*CONSTCOND*/ 0) |
295 | #define WRITE_Sn(BITS, EN, V, STREAM, WP, FROM, TO) \ |
296 | do { \ |
297 | if ((FROM)->channels == 2 && (TO)->channels == 1) { \ |
298 | WRITE_S##BITS##EN(WP, ((V)[0] + (V)[1]) / 2); \ |
299 | WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \ |
300 | } else if (from->channels <= to->channels) { \ |
301 | int j; \ |
302 | for (j = 0; j < (FROM)->channels; j++) { \ |
303 | WRITE_S##BITS##EN(WP, (V)[j]); \ |
304 | WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \ |
305 | } \ |
306 | if (j == 1 && 1 < (TO)->channels) { \ |
307 | WRITE_S##BITS##EN(WP, (V)[0]); \ |
308 | WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \ |
309 | j++; \ |
310 | } \ |
311 | for (; j < (TO)->channels; j++) { \ |
312 | WRITE_S##BITS##EN(WP, 0); \ |
313 | WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \ |
314 | } \ |
315 | } else { /* from->channels < to->channels */ \ |
316 | int j; \ |
317 | for (j = 0; j < (TO)->channels; j++) { \ |
318 | WRITE_S##BITS##EN(WP, (V)[j]); \ |
319 | WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \ |
320 | } \ |
321 | } \ |
322 | } while (/*CONSTCOND*/ 0) |
323 | |
324 | /* |
325 | * Function template |
326 | * |
327 | * Don't use this for 32bit data because this linear interpolation overflows |
328 | * for 32bit data. |
329 | */ |
330 | #define AURATECONV_SLINEAR(BITS, EN) \ |
331 | static int \ |
332 | aurateconv_slinear##BITS##_##EN (aurateconv_t *this, audio_stream_t *dst, \ |
333 | int m, int frame_src, int frame_dst) \ |
334 | { \ |
335 | uint8_t *w; \ |
336 | const uint8_t *r; \ |
337 | const audio_params_t *from, *to; \ |
338 | audio_stream_t *src; \ |
339 | int32_t v[AUDIO_MAX_CHANNELS]; \ |
340 | int32_t *prev, *next, c256; \ |
341 | int i, values_size; \ |
342 | \ |
343 | src = this->base.src; \ |
344 | w = dst->inp; \ |
345 | r = src->outp; \ |
346 | DPRINTF(("%s: ENTER w=%p r=%p dst->used=%d src->used=%d\n", \ |
347 | __func__, w, r, dst->used, src->used)); \ |
348 | from = &this->from; \ |
349 | to = &this->to; \ |
350 | if (this->from.sample_rate == this->to.sample_rate) { \ |
351 | while (dst->used < m && src->used >= frame_src) { \ |
352 | READ_Sn(BITS, EN, v, src, r, from); \ |
353 | WRITE_Sn(BITS, EN, v, dst, w, from, to); \ |
354 | } \ |
355 | } else if (to->sample_rate < from->sample_rate) { \ |
356 | while (dst->used < m && src->used >= frame_src) { \ |
357 | READ_Sn(BITS, EN, v, src, r, from); \ |
358 | this->count += to->sample_rate; \ |
359 | if (this->count >= from->sample_rate) { \ |
360 | this->count -= from->sample_rate; \ |
361 | WRITE_Sn(BITS, EN, v, dst, w, from, to); \ |
362 | } \ |
363 | } \ |
364 | } else { \ |
365 | /* Initial value of this->count >= to->sample_rate */ \ |
366 | values_size = sizeof(int32_t) * from->channels; \ |
367 | prev = this->prev; \ |
368 | next = this->next; \ |
369 | while (dst->used < m \ |
370 | && ((this->count >= to->sample_rate && src->used >= frame_src) \ |
371 | || this->count < to->sample_rate)) { \ |
372 | if (this->count >= to->sample_rate) { \ |
373 | this->count -= to->sample_rate; \ |
374 | memcpy(prev, next, values_size); \ |
375 | READ_Sn(BITS, EN, next, src, r, from); \ |
376 | } \ |
377 | c256 = this->count * 256 / to->sample_rate; \ |
378 | for (i = 0; i < from->channels; i++) \ |
379 | v[i] = (c256 * next[i] + (256 - c256) * prev[i]) >> 8; \ |
380 | WRITE_Sn(BITS, EN, v, dst, w, from, to); \ |
381 | this->count += from->sample_rate; \ |
382 | } \ |
383 | } \ |
384 | DPRINTF(("%s: LEAVE w=%p r=%p dst->used=%d src->used=%d\n", \ |
385 | __func__, w, r, dst->used, src->used)); \ |
386 | dst->inp = w; \ |
387 | src->outp = r; \ |
388 | return 0; \ |
389 | } |
390 | |
391 | /* |
392 | * Function template for 32bit container |
393 | */ |
394 | #define AURATECONV_SLINEAR32(EN) \ |
395 | static int \ |
396 | aurateconv_slinear32_##EN (aurateconv_t *this, audio_stream_t *dst, \ |
397 | int m, int frame_src, int frame_dst) \ |
398 | { \ |
399 | uint8_t *w; \ |
400 | const uint8_t *r; \ |
401 | const audio_params_t *from, *to; \ |
402 | audio_stream_t *src; \ |
403 | int32_t v[AUDIO_MAX_CHANNELS]; \ |
404 | int32_t *prev, *next; \ |
405 | int64_t c256, mask; \ |
406 | int i, values_size, used_src, used_dst; \ |
407 | \ |
408 | src = this->base.src; \ |
409 | w = dst->inp; \ |
410 | r = src->outp; \ |
411 | used_dst = audio_stream_get_used(dst); \ |
412 | used_src = audio_stream_get_used(src); \ |
413 | from = &this->from; \ |
414 | to = &this->to; \ |
415 | if (this->from.sample_rate == this->to.sample_rate) { \ |
416 | while (used_dst < m && used_src >= frame_src) { \ |
417 | READ_Sn(32, EN, v, src, r, from); \ |
418 | used_src -= frame_src; \ |
419 | WRITE_Sn(32, EN, v, dst, w, from, to); \ |
420 | used_dst += frame_dst; \ |
421 | } \ |
422 | } else if (to->sample_rate < from->sample_rate) { \ |
423 | while (used_dst < m && used_src >= frame_src) { \ |
424 | READ_Sn(32, EN, v, src, r, from); \ |
425 | used_src -= frame_src; \ |
426 | this->count += to->sample_rate; \ |
427 | if (this->count >= from->sample_rate) { \ |
428 | this->count -= from->sample_rate; \ |
429 | WRITE_Sn(32, EN, v, dst, w, from, to); \ |
430 | used_dst += frame_dst; \ |
431 | } \ |
432 | } \ |
433 | } else { \ |
434 | /* Initial value of this->count >= to->sample_rate */ \ |
435 | values_size = sizeof(int32_t) * from->channels; \ |
436 | mask = int32_mask[to->validbits]; \ |
437 | prev = this->prev; \ |
438 | next = this->next; \ |
439 | while (used_dst < m \ |
440 | && ((this->count >= to->sample_rate && used_src >= frame_src) \ |
441 | || this->count < to->sample_rate)) { \ |
442 | if (this->count >= to->sample_rate) { \ |
443 | this->count -= to->sample_rate; \ |
444 | memcpy(prev, next, values_size); \ |
445 | READ_Sn(32, EN, next, src, r, from); \ |
446 | used_src -= frame_src; \ |
447 | } \ |
448 | c256 = this->count * 256 / to->sample_rate; \ |
449 | for (i = 0; i < from->channels; i++) \ |
450 | v[i] = (int32_t)((c256 * next[i] + (INT64_C(256) - c256) * prev[i]) >> 8) & mask; \ |
451 | WRITE_Sn(32, EN, v, dst, w, from, to); \ |
452 | used_dst += frame_dst; \ |
453 | this->count += from->sample_rate; \ |
454 | } \ |
455 | } \ |
456 | dst->inp = w; \ |
457 | src->outp = r; \ |
458 | return 0; \ |
459 | } |
460 | |
461 | AURATECONV_SLINEAR(16, LE) |
462 | AURATECONV_SLINEAR(24, LE) |
463 | AURATECONV_SLINEAR32(LE) |
464 | AURATECONV_SLINEAR(16, BE) |
465 | AURATECONV_SLINEAR(24, BE) |
466 | AURATECONV_SLINEAR32(BE) |
467 | |