From a8cc6a74547e6485f3aa9fe597f5d1dc176cca8a Mon Sep 17 00:00:00 2001 From: Thom Johansen Date: Sun, 29 Jan 2006 02:10:28 +0000 Subject: [PATCH] Initial multi-band EQ support for software codec platforms. Now go start making that user interface! git-svn-id: svn://svn.rockbox.org/rockbox/trunk@8478 a1c6a512-1295-4272-9138-f99709370657 --- apps/SOURCES | 4 + apps/dsp.c | 34 ++++++++ apps/eq.c | 226 +++++++++++++++++++++++++++++++++++++++++++++++++++ apps/eq.h | 41 ++++++++++ 4 files changed, 305 insertions(+) create mode 100644 apps/eq.c create mode 100644 apps/eq.h diff --git a/apps/SOURCES b/apps/SOURCES index cf17bbc27c..94c081ec15 100644 --- a/apps/SOURCES +++ b/apps/SOURCES @@ -76,4 +76,8 @@ playback.c metadata.c codecs.c dsp.c +eq.c +#if defined(CPU_COLDFIRE) && !defined(SIMULATOR) +eq_cf.S +#endif #endif diff --git a/apps/dsp.c b/apps/dsp.c index 19cb669a06..b2fc0ce7a2 100644 --- a/apps/dsp.c +++ b/apps/dsp.c @@ -19,6 +19,7 @@ #include #include #include "dsp.h" +#include "eq.h" #include "kernel.h" #include "playback.h" #include "system.h" @@ -166,10 +167,21 @@ struct crossfeed_data int index; }; +/* Current setup is one lowshelf filters, three peaking filters and one + highshelf filter. Varying the number of shelving filters make no sense, + but adding peaking filters are possible. */ +struct eq_state { + char enabled[5]; /* Flags for active filters */ + struct eqfilter ls; + struct eqfilter pk[3]; + struct eqfilter hs; +}; + static struct dsp_config dsp_conf[2] IBSS_ATTR; static struct dither_data dither_data[2] IBSS_ATTR; static struct resample_data resample_data[2] IBSS_ATTR; struct crossfeed_data crossfeed_data IBSS_ATTR; +static struct eq_state eq_data; static int pitch_ratio = 1000; @@ -608,6 +620,25 @@ static void apply_crossfeed(long* src[], int count) } #endif +/* Apply EQ filters to those bands that have got it switched on. */ +void eq_process(long **x, unsigned num) +{ + int i; + unsigned int channels = dsp->stereo_mode != STEREO_MONO ? 2 : 1; + + /* filter configuration currently is 1 low shelf filter, 3 band peaking + filters and 1 high shelf filter, in that order. + */ + if (eq_data.enabled[0]) + eq_filter(x, &eq_data.ls, num, channels, EQ_SHELF_SHIFT); + for (i = 0; i < 3; i++) { + if (eq_data.enabled[1 + i]) + eq_filter(x, &eq_data.pk[i], num, channels, EQ_PEAK_SHIFT); + } + if (eq_data.enabled[4]) + eq_filter(x, &eq_data.hs, num, channels, EQ_SHELF_SHIFT); +} + /* Apply a constant gain to the samples (e.g., for ReplayGain). May update * the src array if gain was applied. * Note that this must be called before the resampler. @@ -713,6 +744,9 @@ long dsp_process(char* dst, char* src[], long size) samples = resample(tmp, samples); if (dsp->crossfeed_enabled && dsp->stereo_mode != STEREO_MONO) apply_crossfeed(tmp, samples); + /* TODO: Might want to wrap this with a generic eq_enabled when the + settings are in place */ + eq_process(tmp, samples); write_samples((short*) dst, tmp, samples); written += samples; dst += samples * sizeof(short) * 2; diff --git a/apps/eq.c b/apps/eq.c new file mode 100644 index 0000000000..8ad886fc0c --- /dev/null +++ b/apps/eq.c @@ -0,0 +1,226 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id$ + * + * Copyright (C) 2006 Thom Johansen + * + * All files in this archive are subject to the GNU General Public License. + * See the file COPYING in the source tree root for full license agreement. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ + +#include "config.h" +#include "eq.h" + +/* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson. + Slightly faster calculation can be done by deriving forms which use tan() + instead of cos() and sin(), but the latter are far easier to use when doing + fixed point math, and performance is not a big point in the calculation part. + We realise the filters as a second order direct form 1 structure. Direct + form 1 was chosen because of better numerical properties for fixed point + implementations. + */ + +#define DIV64(x, y, z) (long)(((long long)(x) << (z))/(y)) +/* TODO: This macro requires the EMAC unit to be in fractional mode + when the coef generator routines are called. If this can be guaranteeed, + then remove the "&& 0" below for faster coef calculation on Coldfire. + */ +#if defined(CPU_COLDFIRE) && !defined(SIMULATOR) && 0 +#define FRACMUL(x, y) \ +({ \ + long t; \ + asm volatile ("mac.l %[a], %[b], %%acc0\n\t" \ + "movclr.l %%acc0, %[t]\n\t" \ + : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \ + t; \ +}) +#else +#define FRACMUL(x, y) ((long)(((((long long) (x)) * ((long long) (y))) >> 31))) +#endif + +/* TODO: replaygain.c has some fixed point routines. perhaps we could reuse + them? */ + +/* 128 sixteen bit sine samples + guard point */ +short sinetab[] = { + 0, 1607, 3211, 4807, 6392, 7961, 9511, 11038, 12539, 14009, 15446, 16845, + 18204, 19519, 20787, 22004, 23169, 24278, 25329, 26318, 27244, 28105, 28897, + 29621, 30272, 30851, 31356, 31785, 32137, 32412, 32609,32727, 32767, 32727, + 32609, 32412, 32137, 31785, 31356, 30851, 30272, 29621, 28897, 28105, 27244, + 26318, 25329, 24278, 23169, 22004, 20787, 19519, 18204, 16845, 15446, 14009, + 12539, 11038, 9511, 7961, 6392, 4807, 3211, 1607, 0, -1607, -3211, -4807, + -6392, -7961, -9511, -11038, -12539, -14009, -15446, -16845, -18204, -19519, + -20787, -22004, -23169, -24278, -25329, -26318, -27244, -28105, -28897, + -29621, -30272, -30851, -31356, -31785, -32137, -32412, -32609, -32727, + -32767, -32727, -32609, -32412, -32137, -31785, -31356, -30851, -30272, + -29621, -28897, -28105, -27244, -26318, -25329, -24278, -23169, -22004, + -20787, -19519, -18204, -16845, -15446, -14009, -12539, -11038, -9511, + -7961, -6392, -4807, -3211, -1607, 0 +}; + +/* Good quality sine calculated by linearly interpolating + * a 128 sample sine table. First harmonic has amplitude of about -84 dB. + * phase has range from 0 to 0xffffffff, representing 0 and + * 2*pi respectively. + * Return value is a signed value from LONG_MIN to LONG_MAX, representing + * -1 and 1 respectively. + */ +static long fsin(unsigned long phase) +{ + unsigned int pos = phase >> 25; + unsigned short frac = (phase & 0x01ffffff) >> 9; + short diff = sinetab[pos + 1] - sinetab[pos]; + + return (sinetab[pos] << 16) + frac*diff; +} + +static inline long fcos(unsigned long phase) +{ + return fsin(phase + 0xffffffff/4); +} + +/* Fixed point square root via Newton-Raphson. + * Output is in same fixed point format as input. + * fracbits specifies number of fractional bits in argument. + */ +static long fsqrt(long a, unsigned int fracbits) +{ + long b = a/2 + (1 << fracbits); /* initial approximation */ + unsigned n; + const unsigned iterations = 4; + + for (n = 0; n < iterations; ++n) + b = (b + DIV64(a, b, fracbits))/2; + + return b; +} + +short dbtoatab[49] = { + 2058, 2180, 2309, 2446, 2591, 2744, 2907, 3079, 3261, 3455, 3659, 3876, + 4106, 4349, 4607, 4880, 5169, 5475, 5799, 6143, 6507, 6893, 7301, 7734, + 8192, 8677, 9192, 9736, 10313, 10924, 11572, 12257, 12983, 13753, 14568, + 15431, 16345, 17314, 18340, 19426, 20577, 21797, 23088, 24456, 25905, 27440, + 29066, 30789, 32613 +}; + +/* Function for converting dB to squared amplitude factor (A = 10^(dB/40)). + Range is -24 to 24 dB. If gain values outside of this is needed, the above + table needs to be extended. + Parameter format is s15.16 fixed point. Return format is s2.29 fixed point. + */ +static long dbtoA(long db) +{ + const unsigned long bias = 24 << 16; + unsigned short frac = (db + bias) & 0x0000ffff; + unsigned short pos = (db + bias) >> 16; + short diff = dbtoatab[pos + 1] - dbtoatab[pos]; + + return (dbtoatab[pos] << 16) + frac*diff; +} + +/* Calculate second order section peaking filter coefficients. + cutoff is a value from 0 to 0xffffffff, where 0 represents 0 hz and + 0xffffffff represents nyquist (samplerate/2). + Q is an unsigned 6.26 fixed point number, lower bound is artificially set + at 0.5. + db is s15.16 fixed point and describes gain/attenuation at peak freq. + c is a pointer where the coefs will be stored. + */ +void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, long *c) +{ + const long one = 1 << 28; /* s3.28 */ + const long A = dbtoA(db); + const long alpha = DIV64(fsin(cutoff), 2*Q, 25); /* s1.30 */ + long a0, a1, a2; /* these are all s3.28 format */ + long b0, b1, b2; + + /* possible numerical ranges listed after each coef */ + b0 = one + FRACMUL(alpha, A); /* [1.25..5] */ + b1 = a1 = -2*(fcos(cutoff) >> 3); /* [-2..2] */ + b2 = one - FRACMUL(alpha, A); /* [-3..0.75] */ + a0 = one + DIV64(alpha, A, 27); /* [1.25..5] */ + a2 = one - DIV64(alpha, A, 27); /* [-3..0.75] */ + + c[0] = DIV64(b0, a0, 28); + c[1] = DIV64(b1, a0, 28); + c[2] = DIV64(b2, a0, 28); + c[3] = DIV64(a1, a0, 28); + c[4] = DIV64(a2, a0, 28); +} + +/* Calculate coefficients for lowshelf filter */ +void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, long *c) +{ + const long one = 1 << 24; /* s7.24 */ + const long A = dbtoA(db); + const long alpha = DIV64(fsin(cutoff), 2*Q, 25); /* s1.30 */ + const long ap1 = (A >> 5) + one; + const long am1 = (A >> 5) - one; + const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); + long a0, a1, a2; /* these are all s7.24 format */ + long b0, b1, b2; + long cs = fcos(cutoff); + + b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2; + b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3; + b2 = FRACMUL(A, ap1 - FRACMUL(am1, cs) - twosqrtalpha) << 2; + a0 = ap1 + FRACMUL(am1, cs) + twosqrtalpha; + a1 = -2*((am1 + FRACMUL(ap1, cs))); + a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha; + + c[0] = DIV64(b0, a0, 24); + c[1] = DIV64(b1, a0, 24); + c[2] = DIV64(b2, a0, 24); + c[3] = DIV64(a1, a0, 24); + c[4] = DIV64(a2, a0, 24); +} + +/* Calculate coefficients for highshelf filter */ +void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, long *c) +{ + const long one = 1 << 24; /* s7.24 */ + const long A = dbtoA(db); + const long alpha = DIV64(fsin(cutoff), 2*Q, 25); /* s1.30 */ + const long ap1 = (A >> 5) + one; + const long am1 = (A >> 5) - one; + const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); + long a0, a1, a2; /* these are all s7.24 format */ + long b0, b1, b2; + long cs = fcos(cutoff); + + b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2; + b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3; + b2 = FRACMUL(A, ap1 + FRACMUL(am1, cs) - twosqrtalpha) << 2; + a0 = ap1 - FRACMUL(am1, cs) + twosqrtalpha; + a1 = 2*((am1 - FRACMUL(ap1, cs))); + a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha; + + c[0] = DIV64(b0, a0, 24); + c[1] = DIV64(b1, a0, 24); + c[2] = DIV64(b2, a0, 24); + c[3] = DIV64(a1, a0, 24); + c[4] = DIV64(a2, a0, 24); +} + +#if !defined(CPU_COLDFIRE) || defined(SIMULATOR) +void eq_filter(long **x, struct eqfilter *f, unsigned num, + unsigned channels, unsigned shift) +{ + /* TODO: Implement generic filtering routine. */ + (void)x; + (void)f; + (void)num; + (void)channels; + (void)shift; +} +#endif + diff --git a/apps/eq.h b/apps/eq.h new file mode 100644 index 0000000000..f7616387b4 --- /dev/null +++ b/apps/eq.h @@ -0,0 +1,41 @@ +/*************************************************************************** + * __________ __ ___. + * Open \______ \ ____ ____ | | _\_ |__ _______ ___ + * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / + * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < + * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ + * \/ \/ \/ \/ \/ + * $Id$ + * + * Copyright (C) 2006 Thom Johansen + * + * All files in this archive are subject to the GNU General Public License. + * See the file COPYING in the source tree root for full license agreement. + * + * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY + * KIND, either express or implied. + * + ****************************************************************************/ + +#ifndef _EQ_H +#define _EQ_H + +/* These depend on the fixed point formats used by the different filter types + and need to be changed when they change. + */ +#define EQ_PEAK_SHIFT 3 +#define EQ_SHELF_SHIFT 7 + +struct eqfilter { + long coefs[5]; /* Order is b0, b1, b2, a1, a2 */ + long history[2][4]; +}; + +void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, long *c); +void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, long *c); +void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, long *c); +void eq_filter(long **x, struct eqfilter *f, unsigned num, + unsigned samples, unsigned shift); + +#endif +