NetBSD-5.0.2/sys/dev/aurateconv.c
/* $NetBSD: aurateconv.c,v 1.18 2008/04/28 20:23:46 martin Exp $ */
/*-
* Copyright (c) 2002 The NetBSD Foundation, Inc.
* All rights reserved.
*
* This code is derived from software contributed to The NetBSD Foundation
* by TAMURA Kent
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
* ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
* BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <sys/cdefs.h>
__KERNEL_RCSID(0, "$NetBSD: aurateconv.c,v 1.18 2008/04/28 20:23:46 martin Exp $");
#include <sys/systm.h>
#include <sys/types.h>
#include <sys/device.h>
#include <sys/errno.h>
#include <sys/malloc.h>
#include <sys/select.h>
#include <sys/audioio.h>
#include <dev/audio_if.h>
#include <dev/audiovar.h>
#include <dev/auconv.h>
#ifndef _KERNEL
#include <stdio.h>
#include <string.h>
#endif
/* #define AURATECONV_DEBUG */
#ifdef AURATECONV_DEBUG
#define DPRINTF(x) printf x
#else
#define DPRINTF(x)
#endif
typedef struct aurateconv {
stream_filter_t base;
audio_params_t from;
audio_params_t to;
long count;
int32_t prev[AUDIO_MAX_CHANNELS];
int32_t next[AUDIO_MAX_CHANNELS];
} aurateconv_t;
static int aurateconv_fetch_to(stream_fetcher_t *, audio_stream_t *, int);
static void aurateconv_dtor(stream_filter_t *);
static int aurateconv_slinear16_LE(aurateconv_t *, audio_stream_t *,
int, int, int);
static int aurateconv_slinear24_LE(aurateconv_t *, audio_stream_t *,
int, int, int);
static int aurateconv_slinear32_LE(aurateconv_t *, audio_stream_t *,
int, int, int);
static int aurateconv_slinear16_BE(aurateconv_t *, audio_stream_t *,
int, int, int);
static int aurateconv_slinear24_BE(aurateconv_t *, audio_stream_t *,
int, int, int);
static int aurateconv_slinear32_BE(aurateconv_t *, audio_stream_t *,
int, int, int);
static int32_t int32_mask[33] = {
0x0, 0x80000000, 0xc0000000, 0xe0000000,
0xf0000000, 0xf8000000, 0xfc000000, 0xfe000000,
0xff000000, 0xff800000, 0xffc00000, 0xffe00000,
0xfff00000, 0xfff80000, 0xfffc0000, 0xfffe0000,
0xffff0000, 0xffff8000, 0xffffc000, 0xffffe000,
0xfffff000, 0xfffff800, 0xfffffc00, 0xfffffe00,
0xffffff00, 0xffffff80, 0xffffffc0, 0xffffffe0,
0xfffffff0, 0xfffffff8, 0xfffffffc, 0xfffffffe,
0xffffffff
};
stream_filter_t *
aurateconv(struct audio_softc *sc, const audio_params_t *from,
const audio_params_t *to)
{
aurateconv_t *this;
DPRINTF(("Construct '%s' filter: rate=%u:%u chan=%u:%u prec=%u/%u:%u/"
"%u enc=%u:%u\n", __func__, from->sample_rate,
to->sample_rate, from->channels, to->channels,
from->validbits, from->precision, to->validbits,
to->precision, from->encoding, to->encoding));
#ifdef DIAGNOSTIC
/* check from/to */
if (from->channels == to->channels
&& from->sample_rate == to->sample_rate)
printf("%s: no conversion\n", __func__); /* No conversion */
if (from->encoding != to->encoding
|| from->precision != to->precision
|| from->validbits != to->validbits) {
printf("%s: encoding/precision must not be changed\n", __func__);
return NULL;
}
if ((from->encoding != AUDIO_ENCODING_SLINEAR_LE
&& from->encoding != AUDIO_ENCODING_SLINEAR_BE)
|| (from->precision != 16 && from->precision != 24 && from->precision != 32)) {
printf("%s: encoding/precision must be SLINEAR_LE 16/24/32bit, "
"or SLINEAR_BE 16/24/32bit", __func__);
return NULL;
}
if (from->channels > AUDIO_MAX_CHANNELS || from->channels <= 0
|| to->channels > AUDIO_MAX_CHANNELS || to->channels <= 0) {
printf("%s: invalid channels: from=%u to=%u\n",
__func__, from->channels, to->channels);
return NULL;
}
if (from->sample_rate <= 0 || to->sample_rate <= 0) {
printf("%s: invalid sampling rate: from=%u to=%u\n",
__func__, from->sample_rate, to->sample_rate);
return NULL;
}
#endif
/* initialize context */
this = malloc(sizeof(aurateconv_t), M_DEVBUF, M_WAITOK | M_ZERO);
this->count = from->sample_rate < to->sample_rate
? to->sample_rate + from->sample_rate : 0;
this->from = *from;
this->to = *to;
/* initialize vtbl */
this->base.base.fetch_to = aurateconv_fetch_to;
this->base.dtor = aurateconv_dtor;
this->base.set_fetcher = stream_filter_set_fetcher;
this->base.set_inputbuffer = stream_filter_set_inputbuffer;
return &this->base;
}
static void
aurateconv_dtor(struct stream_filter *this)
{
if (this != NULL)
free(this, M_DEVBUF);
}
static int
aurateconv_fetch_to(stream_fetcher_t *self, audio_stream_t *dst, int max_used)
{
aurateconv_t *this;
int m, err, frame_dst, frame_src;
this = (aurateconv_t *)self;
frame_dst = (this->to.precision / 8) * this->to.channels;
frame_src = (this->from.precision / 8) * this->from.channels;
max_used = max_used / frame_dst * frame_dst;
if (max_used <= 0)
max_used = frame_dst;
/* calculate required input size for output max_used bytes */
m = max_used / frame_dst;
m *= this->from.sample_rate;
m /= this->to.sample_rate;
m *= frame_src;
if (m <= 0)
m = frame_src;
if ((err = this->base.prev->fetch_to(this->base.prev, this->base.src, m)))
return err;
m = (dst->end - dst->start) / frame_dst * frame_dst;
m = min(m, max_used);
switch (this->from.encoding) {
case AUDIO_ENCODING_SLINEAR_LE:
switch (this->from.precision) {
case 16:
return aurateconv_slinear16_LE(this, dst, m,
frame_src, frame_dst);
case 24:
return aurateconv_slinear24_LE(this, dst, m,
frame_src, frame_dst);
case 32:
return aurateconv_slinear32_LE(this, dst, m,
frame_src, frame_dst);
}
break;
case AUDIO_ENCODING_SLINEAR_BE:
switch (this->from.precision) {
case 16:
return aurateconv_slinear16_BE(this, dst, m,
frame_src, frame_dst);
case 24:
return aurateconv_slinear24_BE(this, dst, m,
frame_src, frame_dst);
case 32:
return aurateconv_slinear32_BE(this, dst, m,
frame_src, frame_dst);
}
break;
}
printf("%s: internal error: unsupported encoding: enc=%u prec=%u\n",
__func__, this->from.encoding, this->from.precision);
return 0;
}
#define READ_S8LE(P) *(const int8_t*)(P)
#define WRITE_S8LE(P, V) *(int8_t*)(P) = V
#define READ_S8BE(P) *(const int8_t*)(P)
#define WRITE_S8BE(P, V) *(int8_t*)(P) = V
#if BYTE_ORDER == LITTLE_ENDIAN
# define READ_S16LE(P) *(const int16_t*)(P)
# define WRITE_S16LE(P, V) *(int16_t*)(P) = V
# define READ_S16BE(P) (int16_t)((P)[0] | ((P)[1]<<8))
# define WRITE_S16BE(P, V) \
do { \
int vv = V; \
(P)[0] = vv; \
(P)[1] = vv >> 8; \
} while (/*CONSTCOND*/ 0)
# define READ_S32LE(P) *(const int32_t*)(P)
# define WRITE_S32LE(P, V) *(int32_t*)(P) = V
# define READ_S32BE(P) (int32_t)((P)[3] | ((P)[2]<<8) | ((P)[1]<<16) | (((int8_t)((P)[0]))<<24))
# define WRITE_S32BE(P, V) \
do { \
int vvv = V; \
(P)[0] = vvv >> 24; \
(P)[1] = vvv >> 16; \
(P)[2] = vvv >> 8; \
(P)[3] = vvv; \
} while (/*CONSTCOND*/ 0)
#else /* !LITTLE_ENDIAN */
# define READ_S16LE(P) (int16_t)((P)[0] | ((P)[1]<<8))
# define WRITE_S16LE(P, V) \
do { \
int vv = V; \
(P)[0] = vv; \
(P)[1] = vv >> 8; \
} while (/*CONSTCOND*/ 0)
# define READ_S16BE(P) *(const int16_t*)(P)
# define WRITE_S16BE(P, V) *(int16_t*)(P) = V
# define READ_S32LE(P) (int32_t)((P)[0] | ((P)[1]<<8) | ((P)[2]<<16) | (((int8_t)((P)[3]))<<24))
# define WRITE_S32LE(P, V) \
do { \
int vvv = V; \
(P)[0] = vvv; \
(P)[1] = vvv >> 8; \
(P)[2] = vvv >> 16; \
(P)[3] = vvv >> 24; \
} while (/*CONSTCOND*/ 0)
# define READ_S32BE(P) *(const int32_t*)(P)
# define WRITE_S32BE(P, V) *(int32_t*)(P) = V
#endif /* !LITTLE_ENDIAN */
#define READ_S24LE(P) (int32_t)((P)[0] | ((P)[1]<<8) | (((int8_t)((P)[2]))<<16))
#define WRITE_S24LE(P, V) \
do { \
int vvv = V; \
(P)[0] = vvv; \
(P)[1] = vvv >> 8; \
(P)[2] = vvv >> 16; \
} while (/*CONSTCOND*/ 0)
#define READ_S24BE(P) (int32_t)((P)[2] | ((P)[1]<<8) | (((int8_t)((P)[0]))<<16))
#define WRITE_S24BE(P, V) \
do { \
int vvv = V; \
(P)[0] = vvv >> 16; \
(P)[1] = vvv >> 8; \
(P)[2] = vvv; \
} while (/*CONSTCOND*/ 0)
#define READ_Sn(BITS, EN, V, STREAM, RP, PAR) \
do { \
int j; \
for (j = 0; j < (PAR)->channels; j++) { \
(V)[j] = READ_S##BITS##EN(RP); \
RP = audio_stream_add_outp(STREAM, RP, (BITS) / NBBY); \
} \
} while (/*CONSTCOND*/ 0)
#define WRITE_Sn(BITS, EN, V, STREAM, WP, FROM, TO) \
do { \
if ((FROM)->channels == 2 && (TO)->channels == 1) { \
WRITE_S##BITS##EN(WP, ((V)[0] + (V)[1]) / 2); \
WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
} else if (from->channels <= to->channels) { \
int j; \
for (j = 0; j < (FROM)->channels; j++) { \
WRITE_S##BITS##EN(WP, (V)[j]); \
WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
} \
if (j == 1 && 1 < (TO)->channels) { \
WRITE_S##BITS##EN(WP, (V)[0]); \
WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
j++; \
} \
for (; j < (TO)->channels; j++) { \
WRITE_S##BITS##EN(WP, 0); \
WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
} \
} else { /* from->channels < to->channels */ \
int j; \
for (j = 0; j < (TO)->channels; j++) { \
WRITE_S##BITS##EN(WP, (V)[j]); \
WP = audio_stream_add_inp(STREAM, WP, (BITS) / NBBY); \
} \
} \
} while (/*CONSTCOND*/ 0)
/*
* Function template
*
* Don't use this for 32bit data because this linear interpolation overflows
* for 32bit data.
*/
#define AURATECONV_SLINEAR(BITS, EN) \
static int \
aurateconv_slinear##BITS##_##EN (aurateconv_t *this, audio_stream_t *dst, \
int m, int frame_src, int frame_dst) \
{ \
uint8_t *w; \
const uint8_t *r; \
const audio_params_t *from, *to; \
audio_stream_t *src; \
int32_t v[AUDIO_MAX_CHANNELS]; \
int32_t *prev, *next, c256; \
int i, values_size; \
\
src = this->base.src; \
w = dst->inp; \
r = src->outp; \
DPRINTF(("%s: ENTER w=%p r=%p dst->used=%d src->used=%d\n", \
__func__, w, r, dst->used, src->used)); \
from = &this->from; \
to = &this->to; \
if (this->from.sample_rate == this->to.sample_rate) { \
while (dst->used < m && src->used >= frame_src) { \
READ_Sn(BITS, EN, v, src, r, from); \
WRITE_Sn(BITS, EN, v, dst, w, from, to); \
} \
} else if (to->sample_rate < from->sample_rate) { \
while (dst->used < m && src->used >= frame_src) { \
READ_Sn(BITS, EN, v, src, r, from); \
this->count += to->sample_rate; \
if (this->count >= from->sample_rate) { \
this->count -= from->sample_rate; \
WRITE_Sn(BITS, EN, v, dst, w, from, to); \
} \
} \
} else { \
/* Initial value of this->count >= to->sample_rate */ \
values_size = sizeof(int32_t) * from->channels; \
prev = this->prev; \
next = this->next; \
while (dst->used < m \
&& ((this->count >= to->sample_rate && src->used >= frame_src) \
|| this->count < to->sample_rate)) { \
if (this->count >= to->sample_rate) { \
this->count -= to->sample_rate; \
memcpy(prev, next, values_size); \
READ_Sn(BITS, EN, next, src, r, from); \
} \
c256 = this->count * 256 / to->sample_rate; \
for (i = 0; i < from->channels; i++) \
v[i] = (c256 * next[i] + (256 - c256) * prev[i]) >> 8; \
WRITE_Sn(BITS, EN, v, dst, w, from, to); \
this->count += from->sample_rate; \
} \
} \
DPRINTF(("%s: LEAVE w=%p r=%p dst->used=%d src->used=%d\n", \
__func__, w, r, dst->used, src->used)); \
dst->inp = w; \
src->outp = r; \
return 0; \
}
/*
* Function template for 32bit container
*/
#define AURATECONV_SLINEAR32(EN) \
static int \
aurateconv_slinear32_##EN (aurateconv_t *this, audio_stream_t *dst, \
int m, int frame_src, int frame_dst) \
{ \
uint8_t *w; \
const uint8_t *r; \
const audio_params_t *from, *to; \
audio_stream_t *src; \
int32_t v[AUDIO_MAX_CHANNELS]; \
int32_t *prev, *next; \
int64_t c256, mask; \
int i, values_size, used_src, used_dst; \
\
src = this->base.src; \
w = dst->inp; \
r = src->outp; \
used_dst = audio_stream_get_used(dst); \
used_src = audio_stream_get_used(src); \
from = &this->from; \
to = &this->to; \
if (this->from.sample_rate == this->to.sample_rate) { \
while (used_dst < m && used_src >= frame_src) { \
READ_Sn(32, EN, v, src, r, from); \
used_src -= frame_src; \
WRITE_Sn(32, EN, v, dst, w, from, to); \
used_dst += frame_dst; \
} \
} else if (to->sample_rate < from->sample_rate) { \
while (used_dst < m && used_src >= frame_src) { \
READ_Sn(32, EN, v, src, r, from); \
used_src -= frame_src; \
this->count += to->sample_rate; \
if (this->count >= from->sample_rate) { \
this->count -= from->sample_rate; \
WRITE_Sn(32, EN, v, dst, w, from, to); \
used_dst += frame_dst; \
} \
} \
} else { \
/* Initial value of this->count >= to->sample_rate */ \
values_size = sizeof(int32_t) * from->channels; \
mask = int32_mask[to->validbits]; \
prev = this->prev; \
next = this->next; \
while (used_dst < m \
&& ((this->count >= to->sample_rate && used_src >= frame_src) \
|| this->count < to->sample_rate)) { \
if (this->count >= to->sample_rate) { \
this->count -= to->sample_rate; \
memcpy(prev, next, values_size); \
READ_Sn(32, EN, next, src, r, from); \
used_src -= frame_src; \
} \
c256 = this->count * 256 / to->sample_rate; \
for (i = 0; i < from->channels; i++) \
v[i] = (int32_t)((c256 * next[i] + (INT64_C(256) - c256) * prev[i]) >> 8) & mask; \
WRITE_Sn(32, EN, v, dst, w, from, to); \
used_dst += frame_dst; \
this->count += from->sample_rate; \
} \
} \
dst->inp = w; \
src->outp = r; \
return 0; \
}
AURATECONV_SLINEAR(16, LE)
AURATECONV_SLINEAR(24, LE)
AURATECONV_SLINEAR32(LE)
AURATECONV_SLINEAR(16, BE)
AURATECONV_SLINEAR(24, BE)
AURATECONV_SLINEAR32(BE)