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