diff --git a/sys/dev/sound/pcm/feeder_matrix.c b/sys/dev/sound/pcm/feeder_matrix.c --- a/sys/dev/sound/pcm/feeder_matrix.c +++ b/sys/dev/sound/pcm/feeder_matrix.c @@ -55,16 +55,11 @@ #define SND_CHN_T_EOF 0x00e0fe0f #define SND_CHN_T_NULL 0x0e0e0e0e -struct feed_matrix_info; - -typedef void (*feed_matrix_t)(struct feed_matrix_info *, uint8_t *, - uint8_t *, uint32_t); - struct feed_matrix_info { + uint32_t fmt; uint32_t bps; uint32_t ialign, oalign; uint32_t in, out; - feed_matrix_t apply; struct { int chn[SND_CHN_T_MAX + 1]; int mul, shift; @@ -110,114 +105,61 @@ } while (0) #endif -#define FEEDMATRIX_DECLARE(SIGN, BIT, ENDIAN) \ -static void \ -feed_matrix_##SIGN##BIT##ENDIAN(struct feed_matrix_info *info, \ - uint8_t *src, uint8_t *dst, uint32_t count) \ -{ \ - 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, \ - AFMT_##SIGN##BIT##_##ENDIAN, false);\ - dst += PCM_##BIT##_BPS; \ - continue; \ - } else if (info->matrix[i].chn[1] == \ - SND_CHN_T_EOF) { \ - v = pcm_sample_read( \ - src + info->matrix[i].chn[0], \ - AFMT_##SIGN##BIT##_##ENDIAN, false);\ - pcm_sample_write(dst, v, \ - AFMT_##SIGN##BIT##_##ENDIAN, false);\ - dst += PCM_##BIT##_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], \ - AFMT_##SIGN##BIT##_##ENDIAN, false);\ - accum += v; \ - } \ - \ - accum = (accum * info->matrix[i].mul) >> \ - info->matrix[i].shift; \ - \ - FEEDMATRIX_CLIP_CHECK(accum, BIT); \ - \ - v = (accum > PCM_S##BIT##_MAX) ? \ - PCM_S##BIT##_MAX : \ - ((accum < PCM_S##BIT##_MIN) ? \ - PCM_S##BIT##_MIN : \ - accum); \ - pcm_sample_write(dst, v, \ - AFMT_##SIGN##BIT##_##ENDIAN, false); \ - dst += PCM_##BIT##_BPS; \ - } \ - src += info->ialign; \ - } while (--count != 0); \ -} +__always_inline static void +feed_matrix_apply(struct feed_matrix_info *info, uint8_t *src, uint8_t *dst, + uint32_t count) +{ + intpcm64_t accum; + intpcm_t v; + int i, j; -#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) -FEEDMATRIX_DECLARE(S, 16, LE) -FEEDMATRIX_DECLARE(S, 32, LE) -#endif -#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) -FEEDMATRIX_DECLARE(S, 16, BE) -FEEDMATRIX_DECLARE(S, 32, BE) -#endif -#ifdef SND_FEEDER_MULTIFORMAT -FEEDMATRIX_DECLARE(S, 8, NE) -FEEDMATRIX_DECLARE(S, 24, LE) -FEEDMATRIX_DECLARE(S, 24, BE) -FEEDMATRIX_DECLARE(U, 8, NE) -FEEDMATRIX_DECLARE(U, 16, LE) -FEEDMATRIX_DECLARE(U, 24, LE) -FEEDMATRIX_DECLARE(U, 32, LE) -FEEDMATRIX_DECLARE(U, 16, BE) -FEEDMATRIX_DECLARE(U, 24, BE) -FEEDMATRIX_DECLARE(U, 32, BE) -#endif + 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, info->fmt, false); + 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], info->fmt, false); + pcm_sample_write(dst, v, info->fmt, false); + dst += info->bps; + continue; + } -#define FEEDMATRIX_ENTRY(SIGN, BIT, ENDIAN) \ - { \ - AFMT_##SIGN##BIT##_##ENDIAN, \ - feed_matrix_##SIGN##BIT##ENDIAN \ - } + 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], info->fmt, false); + accum += v; + } -static const struct { - uint32_t format; - feed_matrix_t apply; -} feed_matrix_tab[] = { -#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) - FEEDMATRIX_ENTRY(S, 16, LE), - FEEDMATRIX_ENTRY(S, 32, LE), -#endif -#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) - FEEDMATRIX_ENTRY(S, 16, BE), - FEEDMATRIX_ENTRY(S, 32, BE), -#endif -#ifdef SND_FEEDER_MULTIFORMAT - FEEDMATRIX_ENTRY(S, 8, NE), - FEEDMATRIX_ENTRY(S, 24, LE), - FEEDMATRIX_ENTRY(S, 24, BE), - FEEDMATRIX_ENTRY(U, 8, NE), - FEEDMATRIX_ENTRY(U, 16, LE), - FEEDMATRIX_ENTRY(U, 24, LE), - FEEDMATRIX_ENTRY(U, 32, LE), - FEEDMATRIX_ENTRY(U, 16, BE), - FEEDMATRIX_ENTRY(U, 24, BE), - FEEDMATRIX_ENTRY(U, 32, BE) -#endif -}; + accum = (accum * info->matrix[i].mul) >> + info->matrix[i].shift; + + FEEDMATRIX_CLIP_CHECK(accum, AFMT_BIT(info->fmt)); + + switch (AFMT_BIT(info->fmt)) { + case 8: + v = PCM_CLAMP_S8(v); + break; + case 16: + v = PCM_CLAMP_S16(v); + break; + case 24: + v = PCM_CLAMP_S24(v); + break; + case 32: + v = PCM_CLAMP_S32(v); + break; + } + pcm_sample_write(dst, v, info->fmt, false); + dst += info->bps; + } + src += info->ialign; + } while (--count != 0); +} static void feed_matrix_reset(struct feed_matrix_info *info) @@ -392,7 +334,6 @@ { struct feed_matrix_info *info; struct pcmchan_matrix *m_in, *m_out; - uint32_t i; int ret; if (AFMT_ENCODING(f->desc->in) != AFMT_ENCODING(f->desc->out)) @@ -404,25 +345,10 @@ 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); - info->apply = NULL; - - for (i = 0; info->apply == NULL && - i < (sizeof(feed_matrix_tab) / sizeof(feed_matrix_tab[0])); i++) { - if (AFMT_ENCODING(info->in) == feed_matrix_tab[i].format) - info->apply = feed_matrix_tab[i].apply; - } - - if (info->apply == NULL) { -#ifdef FEEDMATRIX_GENERIC - info->apply = feed_matrix_apply_generic; -#else - free(info, M_DEVBUF); - return (EINVAL); -#endif - } m_in = feeder_matrix_format_map(info->in); m_out = feeder_matrix_format_map(info->out); @@ -500,7 +426,7 @@ if (j == 0) break; - info->apply(info, src, dst, j); + feed_matrix_apply(info, src, dst, j); j *= info->oalign; dst += j;