diff --git a/sys/dev/sound/pcm/feeder_eq.c b/sys/dev/sound/pcm/feeder_eq.c --- a/sys/dev/sound/pcm/feeder_eq.c +++ b/sys/dev/sound/pcm/feeder_eq.c @@ -101,10 +101,6 @@ &feeder_eq_exact_rate, 0, "force exact rate validation"); #endif -struct feed_eq_info; - -typedef void (*feed_eq_t)(struct feed_eq_info *, uint8_t *, uint32_t); - struct feed_eq_tone { intpcm_t o1[SND_CHN_MAX]; intpcm_t o2[SND_CHN_MAX]; @@ -117,7 +113,7 @@ struct feed_eq_tone treble; struct feed_eq_tone bass; struct feed_eq_coeff *coeff; - feed_eq_t biquad; + uint32_t fmt; uint32_t channels; uint32_t rate; uint32_t align; @@ -139,133 +135,74 @@ (((v) < PCM_S32_MIN) ? PCM_S32_MIN : \ (v))) -#define FEEDEQ_DECLARE(SIGN, BIT, ENDIAN) \ -static void \ -feed_eq_biquad_##SIGN##BIT##ENDIAN(struct feed_eq_info *info, \ - uint8_t *dst, uint32_t count) \ -{ \ - struct feed_eq_coeff_tone *treble, *bass; \ - intpcm64_t w; \ - intpcm_t v; \ - uint32_t i, j; \ - int32_t pmul, pshift; \ - \ - pmul = feed_eq_preamp[info->preamp].mul; \ - pshift = feed_eq_preamp[info->preamp].shift; \ - \ - if (info->state == FEEDEQ_DISABLE) { \ - j = count * info->channels; \ - dst += j * PCM_##BIT##_BPS; \ - do { \ - dst -= PCM_##BIT##_BPS; \ - v = pcm_sample_read(dst, AFMT_##SIGN##BIT##_##ENDIAN); \ - v = ((intpcm64_t)pmul * v) >> pshift; \ - pcm_sample_write(dst, v, AFMT_##SIGN##BIT##_##ENDIAN); \ - } while (--j != 0); \ - \ - return; \ - } \ - \ - treble = &(info->coeff[info->treble.gain].treble); \ - bass = &(info->coeff[info->bass.gain].bass); \ - \ - do { \ - i = 0; \ - j = info->channels; \ - do { \ - v = pcm_sample_read_norm(dst, \ - AFMT_##SIGN##BIT##_##ENDIAN); \ - v = ((intpcm64_t)pmul * v) >> pshift; \ - \ - w = (intpcm64_t)v * treble->b0; \ - w += (intpcm64_t)info->treble.i1[i] * treble->b1; \ - w += (intpcm64_t)info->treble.i2[i] * treble->b2; \ - w -= (intpcm64_t)info->treble.o1[i] * treble->a1; \ - w -= (intpcm64_t)info->treble.o2[i] * treble->a2; \ - info->treble.i2[i] = info->treble.i1[i]; \ - info->treble.i1[i] = v; \ - info->treble.o2[i] = info->treble.o1[i]; \ - w >>= FEEDEQ_COEFF_SHIFT; \ - FEEDEQ_ERR_CLIP_CHECK(treble, w); \ - v = FEEDEQ_CLAMP(w); \ - info->treble.o1[i] = v; \ - \ - w = (intpcm64_t)v * bass->b0; \ - w += (intpcm64_t)info->bass.i1[i] * bass->b1; \ - w += (intpcm64_t)info->bass.i2[i] * bass->b2; \ - w -= (intpcm64_t)info->bass.o1[i] * bass->a1; \ - w -= (intpcm64_t)info->bass.o2[i] * bass->a2; \ - info->bass.i2[i] = info->bass.i1[i]; \ - info->bass.i1[i] = v; \ - info->bass.o2[i] = info->bass.o1[i]; \ - w >>= FEEDEQ_COEFF_SHIFT; \ - FEEDEQ_ERR_CLIP_CHECK(bass, w); \ - v = FEEDEQ_CLAMP(w); \ - info->bass.o1[i] = v; \ - \ - pcm_sample_write_norm(dst, v, \ - AFMT_##SIGN##BIT##_##ENDIAN); \ - dst += PCM_##BIT##_BPS; \ - i++; \ - } while (--j != 0); \ - } while (--count != 0); \ -} - -#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) -FEEDEQ_DECLARE(S, 16, LE) -FEEDEQ_DECLARE(S, 32, LE) -#endif -#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) -FEEDEQ_DECLARE(S, 16, BE) -FEEDEQ_DECLARE(S, 32, BE) -#endif -#ifdef SND_FEEDER_MULTIFORMAT -FEEDEQ_DECLARE(S, 8, NE) -FEEDEQ_DECLARE(S, 24, LE) -FEEDEQ_DECLARE(S, 24, BE) -FEEDEQ_DECLARE(U, 8, NE) -FEEDEQ_DECLARE(U, 16, LE) -FEEDEQ_DECLARE(U, 24, LE) -FEEDEQ_DECLARE(U, 32, LE) -FEEDEQ_DECLARE(U, 16, BE) -FEEDEQ_DECLARE(U, 24, BE) -FEEDEQ_DECLARE(U, 32, BE) -#endif - -#define FEEDEQ_ENTRY(SIGN, BIT, ENDIAN) \ - { \ - AFMT_##SIGN##BIT##_##ENDIAN, \ - feed_eq_biquad_##SIGN##BIT##ENDIAN \ +__always_inline static void +feed_eq_biquad(struct feed_eq_info *info, uint8_t *dst, uint32_t count, + const uint32_t fmt) +{ + struct feed_eq_coeff_tone *treble, *bass; + intpcm64_t w; + intpcm_t v; + uint32_t i, j; + int32_t pmul, pshift; + + pmul = feed_eq_preamp[info->preamp].mul; + pshift = feed_eq_preamp[info->preamp].shift; + + if (info->state == FEEDEQ_DISABLE) { + j = count * info->channels; + dst += j * AFMT_BPS(fmt); + do { + dst -= AFMT_BPS(fmt); + v = pcm_sample_read(dst, fmt); + v = ((intpcm64_t)pmul * v) >> pshift; + pcm_sample_write(dst, v, fmt); + } while (--j != 0); + + return; } -static const struct { - uint32_t format; - feed_eq_t biquad; -} feed_eq_biquad_tab[] = { -#if BYTE_ORDER == LITTLE_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) - FEEDEQ_ENTRY(S, 16, LE), - FEEDEQ_ENTRY(S, 32, LE), -#endif -#if BYTE_ORDER == BIG_ENDIAN || defined(SND_FEEDER_MULTIFORMAT) - FEEDEQ_ENTRY(S, 16, BE), - FEEDEQ_ENTRY(S, 32, BE), -#endif -#ifdef SND_FEEDER_MULTIFORMAT - FEEDEQ_ENTRY(S, 8, NE), - FEEDEQ_ENTRY(S, 24, LE), - FEEDEQ_ENTRY(S, 24, BE), - FEEDEQ_ENTRY(U, 8, NE), - FEEDEQ_ENTRY(U, 16, LE), - FEEDEQ_ENTRY(U, 24, LE), - FEEDEQ_ENTRY(U, 32, LE), - FEEDEQ_ENTRY(U, 16, BE), - FEEDEQ_ENTRY(U, 24, BE), - FEEDEQ_ENTRY(U, 32, BE) -#endif -}; + treble = &(info->coeff[info->treble.gain].treble); + bass = &(info->coeff[info->bass.gain].bass); -#define FEEDEQ_BIQUAD_TAB_SIZE \ - ((int32_t)(sizeof(feed_eq_biquad_tab) / sizeof(feed_eq_biquad_tab[0]))) + do { + i = 0; + j = info->channels; + do { + v = pcm_sample_read_norm(dst, fmt); + v = ((intpcm64_t)pmul * v) >> pshift; + + w = (intpcm64_t)v * treble->b0; + w += (intpcm64_t)info->treble.i1[i] * treble->b1; + w += (intpcm64_t)info->treble.i2[i] * treble->b2; + w -= (intpcm64_t)info->treble.o1[i] * treble->a1; + w -= (intpcm64_t)info->treble.o2[i] * treble->a2; + info->treble.i2[i] = info->treble.i1[i]; + info->treble.i1[i] = v; + info->treble.o2[i] = info->treble.o1[i]; + w >>= FEEDEQ_COEFF_SHIFT; + FEEDEQ_ERR_CLIP_CHECK(treble, w); + v = FEEDEQ_CLAMP(w); + info->treble.o1[i] = v; + + w = (intpcm64_t)v * bass->b0; + w += (intpcm64_t)info->bass.i1[i] * bass->b1; + w += (intpcm64_t)info->bass.i2[i] * bass->b2; + w -= (intpcm64_t)info->bass.o1[i] * bass->a1; + w -= (intpcm64_t)info->bass.o2[i] * bass->a2; + info->bass.i2[i] = info->bass.i1[i]; + info->bass.i1[i] = v; + info->bass.o2[i] = info->bass.o1[i]; + w >>= FEEDEQ_COEFF_SHIFT; + FEEDEQ_ERR_CLIP_CHECK(bass, w); + v = FEEDEQ_CLAMP(w); + info->bass.o1[i] = v; + + pcm_sample_write_norm(dst, v, fmt); + dst += AFMT_BPS(fmt); + i++; + } while (--j != 0); + } while (--count != 0); +} static struct feed_eq_coeff * feed_eq_coeff_rate(uint32_t rate) @@ -337,26 +274,15 @@ feed_eq_init(struct pcm_feeder *f) { struct feed_eq_info *info; - feed_eq_t biquad_op; - int i; if (f->desc->in != f->desc->out) return (EINVAL); - biquad_op = NULL; - - for (i = 0; i < FEEDEQ_BIQUAD_TAB_SIZE && biquad_op == NULL; i++) { - if (AFMT_ENCODING(f->desc->in) == feed_eq_biquad_tab[i].format) - biquad_op = feed_eq_biquad_tab[i].biquad; - } - - if (biquad_op == NULL) - return (EINVAL); - info = malloc(sizeof(*info), M_DEVBUF, M_NOWAIT | M_ZERO); if (info == NULL) return (ENOMEM); + info->fmt = AFMT_ENCODING(f->desc->in); info->channels = AFMT_CHANNEL(f->desc->in); info->align = info->channels * AFMT_BPS(f->desc->in); @@ -366,8 +292,6 @@ info->preamp = FEEDEQ_PREAMP2IDX(FEEDEQ_PREAMP_DEFAULT); info->state = FEEDEQ_UNKNOWN; - info->biquad = biquad_op; - f->data = info; return (feed_eq_setup(info)); @@ -470,7 +394,21 @@ if (j == 0) break; - info->biquad(info, dst, j); + /* Optimize some common formats. */ + switch (info->fmt) { + case AFMT_S16_NE: + feed_eq_biquad(info, dst, j, AFMT_S16_NE); + break; + case AFMT_S24_NE: + feed_eq_biquad(info, dst, j, AFMT_S24_NE); + break; + case AFMT_S32_NE: + feed_eq_biquad(info, dst, j, AFMT_S32_NE); + break; + default: + feed_eq_biquad(info, dst, j, info->fmt); + break; + } j *= info->align; dst += j;