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 @@ -60,16 +60,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; @@ -115,114 +110,48 @@ } 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); \ - 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); \ - pcm_sample_write(dst, v, \ - AFMT_##SIGN##BIT##_##ENDIAN); \ - 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); \ - 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); \ - 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, const uint32_t fmt) +{ + 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, 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; + } -#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], fmt); + 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(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) @@ -397,7 +326,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)) @@ -409,25 +337,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); @@ -505,7 +418,21 @@ if (j == 0) break; - info->apply(info, src, dst, j); + /* Optimize some common formats. */ + 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;