#ifdef _KERNEL
#ifdef HAVE_KERNEL_OPTION_HEADERS
#include "opt_snd.h"
#endif
#include <dev/sound/pcm/sound.h>
#include <dev/sound/pcm/pcm.h>
#include "feeder_if.h"
#define SND_USE_FXDIV
#include "snd_fxdiv_gen.h"
#endif
#define FEEDMATRIX_RESERVOIR (SND_CHN_MAX * PCM_32_BPS)
#define SND_CHN_T_EOF 0x00e0fe0f
#define SND_CHN_T_NULL 0x0e0e0e0e
struct feed_matrix_info {
uint32_t fmt;
uint32_t bps;
uint32_t ialign, oalign;
uint32_t in, out;
struct {
int chn[SND_CHN_T_MAX + 1];
int mul, shift;
} matrix[SND_CHN_T_MAX + 1];
uint8_t reservoir[FEEDMATRIX_RESERVOIR];
};
static struct pcmchan_matrix feeder_matrix_maps[SND_CHN_MATRIX_MAX] = {
[SND_CHN_MATRIX_1_0] = SND_CHN_MATRIX_MAP_1_0,
[SND_CHN_MATRIX_2_0] = SND_CHN_MATRIX_MAP_2_0,
[SND_CHN_MATRIX_2_1] = SND_CHN_MATRIX_MAP_2_1,
[SND_CHN_MATRIX_3_0] = SND_CHN_MATRIX_MAP_3_0,
[SND_CHN_MATRIX_3_1] = SND_CHN_MATRIX_MAP_3_1,
[SND_CHN_MATRIX_4_0] = SND_CHN_MATRIX_MAP_4_0,
[SND_CHN_MATRIX_4_1] = SND_CHN_MATRIX_MAP_4_1,
[SND_CHN_MATRIX_5_0] = SND_CHN_MATRIX_MAP_5_0,
[SND_CHN_MATRIX_5_1] = SND_CHN_MATRIX_MAP_5_1,
[SND_CHN_MATRIX_6_0] = SND_CHN_MATRIX_MAP_6_0,
[SND_CHN_MATRIX_6_1] = SND_CHN_MATRIX_MAP_6_1,
[SND_CHN_MATRIX_7_0] = SND_CHN_MATRIX_MAP_7_0,
[SND_CHN_MATRIX_7_1] = SND_CHN_MATRIX_MAP_7_1
};
static int feeder_matrix_default_ids[9] = {
[0] = SND_CHN_MATRIX_UNKNOWN,
[1] = SND_CHN_MATRIX_1,
[2] = SND_CHN_MATRIX_2,
[3] = SND_CHN_MATRIX_3,
[4] = SND_CHN_MATRIX_4,
[5] = SND_CHN_MATRIX_5,
[6] = SND_CHN_MATRIX_6,
[7] = SND_CHN_MATRIX_7,
[8] = SND_CHN_MATRIX_8
};
#ifdef _KERNEL
#define FEEDMATRIX_CLIP_CHECK(...)
#else
#define FEEDMATRIX_CLIP_CHECK(v, BIT) do { \
if ((v) < PCM_S##BIT##_MIN || (v) > PCM_S##BIT##_MAX) \
errx(1, "\n\n%s(): Sample clipping: %jd\n", \
__func__, (intmax_t)(v)); \
} while (0)
#endif
__always_inline static void
feed_matrix_apply(struct feed_matrix_info *info, uint8_t *src, uint8_t *dst,
uint32_t count, const uint32_t fmt)
{
intpcm64_t accum;
intpcm_t v;
int i, j;
do {
for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; i++) {
if (info->matrix[i].chn[0] == SND_CHN_T_NULL) {
pcm_sample_write(dst, 0, fmt);
dst += info->bps;
continue;
} else if (info->matrix[i].chn[1] == SND_CHN_T_EOF) {
v = pcm_sample_read(src +
info->matrix[i].chn[0], fmt);
pcm_sample_write(dst, v, fmt);
dst += info->bps;
continue;
}
accum = 0;
for (j = 0; info->matrix[i].chn[j] != SND_CHN_T_EOF;
j++) {
v = pcm_sample_read(src +
info->matrix[i].chn[j], fmt);
accum += v;
}
accum = (accum * info->matrix[i].mul) >>
info->matrix[i].shift;
FEEDMATRIX_CLIP_CHECK(accum, AFMT_BIT(fmt));
v = pcm_clamp(accum, fmt);
pcm_sample_write(dst, v, fmt);
dst += info->bps;
}
src += info->ialign;
} while (--count != 0);
}
static void
feed_matrix_reset(struct feed_matrix_info *info)
{
uint32_t i, j;
for (i = 0; i < nitems(info->matrix); i++) {
for (j = 0;
j < (sizeof(info->matrix[i].chn) /
sizeof(info->matrix[i].chn[0])); j++) {
info->matrix[i].chn[j] = SND_CHN_T_EOF;
}
info->matrix[i].mul = 1;
info->matrix[i].shift = 0;
}
}
static int
feed_matrix_setup(struct feed_matrix_info *info, struct pcmchan_matrix *m_in,
struct pcmchan_matrix *m_out)
{
uint32_t i, j, ch, in_mask, merge_mask;
int mul, shift;
if (info == NULL || m_in == NULL || m_out == NULL ||
AFMT_CHANNEL(info->in) != m_in->channels ||
AFMT_CHANNEL(info->out) != m_out->channels ||
m_in->channels < SND_CHN_MIN || m_in->channels > SND_CHN_MAX ||
m_out->channels < SND_CHN_MIN || m_out->channels > SND_CHN_MAX)
return (EINVAL);
feed_matrix_reset(info);
if (m_in->id == m_out->id && !(m_in->id < SND_CHN_MATRIX_BEGIN ||
m_in->id > SND_CHN_MATRIX_END))
return (0);
if (m_in->id == SND_CHN_MATRIX_1_0) {
if (m_out->id == SND_CHN_MATRIX_1_0)
in_mask = SND_CHN_T_MASK_FL;
else if (m_out->mask & SND_CHN_T_MASK_FC)
in_mask = SND_CHN_T_MASK_FC;
else
in_mask = SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR;
} else
in_mask = m_in->mask;
for (ch = SND_CHN_T_BEGIN; ch <= SND_CHN_T_END &&
m_out->map[ch].type != SND_CHN_T_MAX; ch += SND_CHN_T_STEP) {
merge_mask = m_out->map[ch].members & in_mask;
if (merge_mask == 0) {
info->matrix[ch].chn[0] = SND_CHN_T_NULL;
continue;
}
j = 0;
for (i = SND_CHN_T_BEGIN; i <= SND_CHN_T_END;
i += SND_CHN_T_STEP) {
if (merge_mask & (1 << i)) {
if (m_in->offset[i] >= 0 &&
m_in->offset[i] < (int)m_in->channels)
info->matrix[ch].chn[j++] =
m_in->offset[i] * info->bps;
else {
info->matrix[ch].chn[j++] =
SND_CHN_T_EOF;
break;
}
}
}
#define FEEDMATRIX_ATTN_SHIFT 16
if (j > 1) {
mul = (1 << (FEEDMATRIX_ATTN_SHIFT - 1)) + 143 - j;
shift = FEEDMATRIX_ATTN_SHIFT;
while ((mul & 1) == 0 && shift > 0) {
mul >>= 1;
shift--;
}
info->matrix[ch].mul = mul;
info->matrix[ch].shift = shift;
}
}
#ifndef _KERNEL
fprintf(stderr, "Total: %d\n", ch);
for (i = 0; info->matrix[i].chn[0] != SND_CHN_T_EOF; i++) {
fprintf(stderr, "%d: [", i);
for (j = 0; info->matrix[i].chn[j] != SND_CHN_T_EOF; j++) {
if (j != 0)
fprintf(stderr, ", ");
fprintf(stderr, "%d",
(info->matrix[i].chn[j] == SND_CHN_T_NULL) ?
0xffffffff : info->matrix[i].chn[j] / info->bps);
}
fprintf(stderr, "] attn: (x * %d) >> %d\n",
info->matrix[i].mul, info->matrix[i].shift);
}
#endif
return (0);
}
static int
feed_matrix_init(struct pcm_feeder *f)
{
struct feed_matrix_info *info;
struct pcmchan_matrix *m_in, *m_out;
int ret;
if (AFMT_ENCODING(f->desc.in) != AFMT_ENCODING(f->desc.out))
return (EINVAL);
info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO);
if (info == NULL)
return (ENOMEM);
info->in = f->desc.in;
info->out = f->desc.out;
info->fmt = AFMT_ENCODING(info->in);
info->bps = AFMT_BPS(info->in);
info->ialign = AFMT_ALIGN(info->in);
info->oalign = AFMT_ALIGN(info->out);
m_in = feeder_matrix_format_map(info->in);
m_out = feeder_matrix_format_map(info->out);
ret = feed_matrix_setup(info, m_in, m_out);
if (ret != 0) {
free(info, M_DEVBUF);
return (ret);
}
f->data = info;
return (0);
}
static int
feed_matrix_free(struct pcm_feeder *f)
{
struct feed_matrix_info *info;
info = f->data;
free(info, M_DEVBUF);
f->data = NULL;
return (0);
}
static int
feed_matrix_feed(struct pcm_feeder *f, struct pcm_channel *c, uint8_t *b,
uint32_t count, void *source)
{
struct feed_matrix_info *info;
uint32_t j, inmax;
uint8_t *src, *dst;
info = f->data;
if (info->matrix[0].chn[0] == SND_CHN_T_EOF)
return (FEEDER_FEED(f->source, c, b, count, source));
dst = b;
count = SND_FXROUND(count, info->oalign);
inmax = info->ialign + info->oalign;
do {
if (count < info->oalign)
break;
if (count < inmax) {
src = info->reservoir;
j = info->ialign;
} else {
if (info->ialign == info->oalign)
j = count - info->oalign;
else if (info->ialign > info->oalign)
j = SND_FXROUND(count - info->oalign,
info->ialign);
else
j = (SND_FXDIV(count, info->oalign) - 1) *
info->ialign;
src = dst + count - j;
}
j = SND_FXDIV(FEEDER_FEED(f->source, c, src, j, source),
info->ialign);
if (j == 0)
break;
switch (info->fmt) {
case AFMT_S16_NE:
feed_matrix_apply(info, src, dst, j, AFMT_S16_NE);
break;
case AFMT_S24_NE:
feed_matrix_apply(info, src, dst, j, AFMT_S24_NE);
break;
case AFMT_S32_NE:
feed_matrix_apply(info, src, dst, j, AFMT_S32_NE);
break;
default:
feed_matrix_apply(info, src, dst, j, info->fmt);
break;
}
j *= info->oalign;
dst += j;
count -= j;
} while (count != 0);
return (dst - b);
}
static kobj_method_t feeder_matrix_methods[] = {
KOBJMETHOD(feeder_init, feed_matrix_init),
KOBJMETHOD(feeder_free, feed_matrix_free),
KOBJMETHOD(feeder_feed, feed_matrix_feed),
KOBJMETHOD_END
};
FEEDER_DECLARE(feeder_matrix, FEEDER_MATRIX);
int
feeder_matrix_setup(struct pcm_feeder *f, struct pcmchan_matrix *m_in,
struct pcmchan_matrix *m_out)
{
if (f == NULL || f->class->type != FEEDER_MATRIX || f->data == NULL)
return (EINVAL);
return (feed_matrix_setup(f->data, m_in, m_out));
}
int
feeder_matrix_default_id(uint32_t ch)
{
if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels ||
ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels)
return (SND_CHN_MATRIX_UNKNOWN);
return (feeder_matrix_maps[feeder_matrix_default_ids[ch]].id);
}
struct pcmchan_matrix *
feeder_matrix_default_channel_map(uint32_t ch)
{
if (ch < feeder_matrix_maps[SND_CHN_MATRIX_BEGIN].channels ||
ch > feeder_matrix_maps[SND_CHN_MATRIX_END].channels)
return (NULL);
return (&feeder_matrix_maps[feeder_matrix_default_ids[ch]]);
}
uint32_t
feeder_matrix_default_format(uint32_t format)
{
struct pcmchan_matrix *m;
uint32_t i, ch, ext;
ch = AFMT_CHANNEL(format);
ext = AFMT_EXTCHANNEL(format);
if (ext != 0) {
for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
if (feeder_matrix_maps[i].channels == ch &&
feeder_matrix_maps[i].ext == ext)
return (SND_FORMAT(format, ch, ext));
}
}
m = feeder_matrix_default_channel_map(ch);
if (m == NULL)
return (0x00000000);
return (SND_FORMAT(format, ch, m->ext));
}
int
feeder_matrix_format_id(uint32_t format)
{
uint32_t i, ch, ext;
ch = AFMT_CHANNEL(format);
ext = AFMT_EXTCHANNEL(format);
for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
if (feeder_matrix_maps[i].channels == ch &&
feeder_matrix_maps[i].ext == ext)
return (feeder_matrix_maps[i].id);
}
return (SND_CHN_MATRIX_UNKNOWN);
}
struct pcmchan_matrix *
feeder_matrix_format_map(uint32_t format)
{
uint32_t i, ch, ext;
ch = AFMT_CHANNEL(format);
ext = AFMT_EXTCHANNEL(format);
for (i = SND_CHN_MATRIX_BEGIN; i <= SND_CHN_MATRIX_END; i++) {
if (feeder_matrix_maps[i].channels == ch &&
feeder_matrix_maps[i].ext == ext)
return (&feeder_matrix_maps[i]);
}
return (NULL);
}
struct pcmchan_matrix *
feeder_matrix_id_map(int id)
{
if (id < SND_CHN_MATRIX_BEGIN || id > SND_CHN_MATRIX_END)
return (NULL);
return (&feeder_matrix_maps[id]);
}
int
feeder_matrix_compare(struct pcmchan_matrix *m_in, struct pcmchan_matrix *m_out)
{
uint32_t i;
if (m_in == m_out)
return (0);
if (m_in->channels != m_out->channels || m_in->ext != m_out->ext ||
m_in->mask != m_out->mask)
return (1);
for (i = 0; i < nitems(m_in->map); i++) {
if (m_in->map[i].type != m_out->map[i].type)
return (1);
if (m_in->map[i].type == SND_CHN_T_MAX)
break;
if (m_in->map[i].members != m_out->map[i].members)
return (1);
if (i <= SND_CHN_T_END) {
if (m_in->offset[m_in->map[i].type] !=
m_out->offset[m_out->map[i].type])
return (1);
}
}
return (0);
}
static int snd_chn_to_oss[SND_CHN_T_MAX] = {
[SND_CHN_T_FL] = CHID_L,
[SND_CHN_T_FR] = CHID_R,
[SND_CHN_T_FC] = CHID_C,
[SND_CHN_T_LF] = CHID_LFE,
[SND_CHN_T_SL] = CHID_LS,
[SND_CHN_T_SR] = CHID_RS,
[SND_CHN_T_BL] = CHID_LR,
[SND_CHN_T_BR] = CHID_RR
};
#define SND_CHN_OSS_VALIDMASK \
(SND_CHN_T_MASK_FL | SND_CHN_T_MASK_FR | \
SND_CHN_T_MASK_FC | SND_CHN_T_MASK_LF | \
SND_CHN_T_MASK_SL | SND_CHN_T_MASK_SR | \
SND_CHN_T_MASK_BL | SND_CHN_T_MASK_BR)
#define SND_CHN_OSS_MAX 8
#define SND_CHN_OSS_BEGIN CHID_L
#define SND_CHN_OSS_END CHID_RR
static int oss_to_snd_chn[SND_CHN_OSS_END + 1] = {
[CHID_L] = SND_CHN_T_FL,
[CHID_R] = SND_CHN_T_FR,
[CHID_C] = SND_CHN_T_FC,
[CHID_LFE] = SND_CHN_T_LF,
[CHID_LS] = SND_CHN_T_SL,
[CHID_RS] = SND_CHN_T_SR,
[CHID_LR] = SND_CHN_T_BL,
[CHID_RR] = SND_CHN_T_BR
};
int
feeder_matrix_oss_get_channel_order(struct pcmchan_matrix *m,
unsigned long long *map)
{
unsigned long long tmpmap;
uint32_t i;
if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) ||
m->channels > SND_CHN_OSS_MAX)
return (EINVAL);
tmpmap = 0x0000000000000000ULL;
for (i = 0; i < SND_CHN_OSS_MAX && m->map[i].type != SND_CHN_T_MAX;
i++) {
if ((1 << m->map[i].type) & ~SND_CHN_OSS_VALIDMASK)
return (EINVAL);
tmpmap |=
(unsigned long long)snd_chn_to_oss[m->map[i].type] <<
(i * 4);
}
*map = tmpmap;
return (0);
}
int
feeder_matrix_oss_set_channel_order(struct pcmchan_matrix *m,
unsigned long long *map)
{
struct pcmchan_matrix tmp;
uint32_t chmask, i;
int ch, cheof;
if (m == NULL || map == NULL || (m->mask & ~SND_CHN_OSS_VALIDMASK) ||
m->channels > SND_CHN_OSS_MAX || (*map & 0xffffffff00000000ULL))
return (EINVAL);
tmp = *m;
tmp.channels = 0;
tmp.ext = 0;
tmp.mask = 0;
memset(tmp.offset, -1, sizeof(tmp.offset));
cheof = 0;
for (i = 0; i < SND_CHN_OSS_MAX; i++) {
ch = (*map >> (i * 4)) & 0xf;
if (ch < SND_CHN_OSS_BEGIN) {
if (cheof == 0 && m->map[i].type != SND_CHN_T_MAX)
return (EINVAL);
cheof++;
tmp.map[i] = m->map[i];
continue;
} else if (ch > SND_CHN_OSS_END)
return (EINVAL);
else if (cheof != 0)
return (EINVAL);
ch = oss_to_snd_chn[ch];
chmask = 1 << ch;
if (!(chmask & m->mask))
return (EINVAL);
if (chmask & tmp.mask)
return (EINVAL);
tmp.map[i] = m->map[m->offset[ch]];
if (tmp.map[i].type != ch)
return (EINVAL);
tmp.offset[ch] = i;
tmp.mask |= chmask;
tmp.channels++;
if (chmask & SND_CHN_T_MASK_LF)
tmp.ext++;
}
if (tmp.channels != m->channels || tmp.ext != m->ext ||
tmp.mask != m->mask ||
tmp.map[m->channels].type != SND_CHN_T_MAX)
return (EINVAL);
*m = tmp;
return (0);
}