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
59typedef 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
68static int aurateconv_fetch_to(struct audio_softc *, stream_fetcher_t *,
69 audio_stream_t *, int);
70static void aurateconv_dtor(stream_filter_t *);
71static int aurateconv_slinear16_LE(aurateconv_t *, audio_stream_t *,
72 int, int, int);
73static int aurateconv_slinear24_LE(aurateconv_t *, audio_stream_t *,
74 int, int, int);
75static int aurateconv_slinear32_LE(aurateconv_t *, audio_stream_t *,
76 int, int, int);
77static int aurateconv_slinear16_BE(aurateconv_t *, audio_stream_t *,
78 int, int, int);
79static int aurateconv_slinear24_BE(aurateconv_t *, audio_stream_t *,
80 int, int, int);
81static int aurateconv_slinear32_BE(aurateconv_t *, audio_stream_t *,
82 int, int, int);
83
84static 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
96stream_filter_t *
97aurateconv(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
156static void
157aurateconv_dtor(struct stream_filter *this)
158{
159 if (this != NULL)
160 free(this, M_DEVBUF);
161}
162
163static int
164aurateconv_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) \
331static int \
332aurateconv_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) \
395static int \
396aurateconv_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
461AURATECONV_SLINEAR(16, LE)
462AURATECONV_SLINEAR(24, LE)
463AURATECONV_SLINEAR32(LE)
464AURATECONV_SLINEAR(16, BE)
465AURATECONV_SLINEAR(24, BE)
466AURATECONV_SLINEAR32(BE)
467