apps/eq.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2006 Thom Johansen
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19
  20 #include <inttypes.h>
  21 #include "config.h"
  22 #include "eq.h"
  23
  24 /* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson.
  25    Slightly faster calculation can be done by deriving forms which use tan()
  26    instead of cos() and sin(), but the latter are far easier to use when doing
  27    fixed point math, and performance is not a big point in the calculation part.
  28    All the 'a' filter coefficients are negated so we can use only additions
  29    in the filtering equation.
  30    We realise the filters as a second order direct form 1 structure. Direct
  31    form 1 was chosen because of better numerical properties for fixed point
  32    implementations.
  33  */
  34
  35 #define DIV64(x, y, z) (long)(((long long)(x) << (z))/(y))
  36 /* This macro requires the EMAC unit to be in fractional mode
  37    when the coef generator routines are called. If this can't be guaranteeed,
  38    then add "&& 0" below. This will use a slower coef calculation on Coldfire.
  39  */
  40 #if defined(CPU_COLDFIRE) && !defined(SIMULATOR)
  41 #define FRACMUL(x, y) \
  42 ({ \
  43     long t; \
  44     asm volatile ("mac.l    %[a], %[b], %%acc0\n\t" \
  45                   "movclr.l %%acc0, %[t]\n\t" \
  46                   : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \
  47     t; \
  48 })
  49 #else
  50 #define FRACMUL(x, y) ((long)(((((long long) (x)) * ((long long) (y))) >> 31)))
  51 #endif
  52
  53 /* TODO: replaygain.c has some fixed point routines. perhaps we could reuse
  54    them? */
  55
  56 /* 128 sixteen bit sine samples + guard point */
  57 short sinetab[] = {
  58     0, 1607, 3211, 4807, 6392, 7961, 9511, 11038, 12539, 14009, 15446, 16845,
  59     18204, 19519, 20787, 22004, 23169, 24278, 25329, 26318, 27244, 28105, 28897,
  60     29621, 30272, 30851, 31356, 31785, 32137, 32412, 32609,32727, 32767, 32727,
  61     32609, 32412, 32137, 31785, 31356, 30851, 30272, 29621, 28897, 28105, 27244,
  62     26318, 25329, 24278, 23169, 22004, 20787, 19519, 18204, 16845, 15446, 14009,
  63     12539, 11038, 9511, 7961, 6392, 4807, 3211, 1607, 0, -1607, -3211, -4807,
  64     -6392, -7961, -9511, -11038, -12539, -14009, -15446, -16845, -18204, -19519,
  65     -20787, -22004, -23169, -24278, -25329, -26318, -27244, -28105, -28897,
  66     -29621, -30272, -30851, -31356, -31785, -32137, -32412, -32609, -32727,
  67     -32767, -32727, -32609, -32412, -32137, -31785, -31356, -30851, -30272,
  68     -29621, -28897, -28105, -27244, -26318, -25329, -24278, -23169, -22004,
  69     -20787, -19519, -18204, -16845, -15446, -14009, -12539, -11038, -9511,
  70     -7961, -6392, -4807, -3211, -1607, 0
  71 };
  72
  73 /* Good quality sine calculated by linearly interpolating
  74  * a 128 sample sine table. First harmonic has amplitude of about -84 dB.
  75  * phase has range from 0 to 0xffffffff, representing 0 and
  76  * 2*pi respectively.
  77  * Return value is a signed value from LONG_MIN to LONG_MAX, representing
  78  * -1 and 1 respectively.
  79  */
  80 static long fsin(unsigned long phase)
  81 {
  82     unsigned int pos = phase >> 25;
  83     unsigned short frac = (phase & 0x01ffffff) >> 9;
  84     short diff = sinetab[pos + 1] - sinetab[pos];
  85
  86     return (sinetab[pos] << 16) + frac*diff;
  87 }
  88
  89 static inline long fcos(unsigned long phase)
  90 {
  91     return fsin(phase + 0xffffffff/4);
  92 }
  93
  94 /* Fixed point square root via Newton-Raphson.
  95  * Output is in same fixed point format as input.
  96  * fracbits specifies number of fractional bits in argument.
  97  */
  98 static long fsqrt(long a, unsigned int fracbits)
  99 {
 100     long b = a/2 + (1 << fracbits); /* initial approximation */
 101     unsigned n;
 102     const unsigned iterations = 4;
 103
 104     for (n = 0; n < iterations; ++n)
 105         b = (b + DIV64(a, b, fracbits))/2;
 106
 107     return b;
 108 }
 109
 110 short dbtoatab[49] = {
 111     2058, 2180, 2309, 2446, 2591, 2744, 2907, 3079, 3261, 3455, 3659, 3876,
 112     4106, 4349, 4607, 4880, 5169, 5475, 5799, 6143, 6507, 6893, 7301, 7734,
 113     8192, 8677, 9192, 9736, 10313, 10924, 11572, 12257, 12983, 13753, 14568,
 114     15431, 16345, 17314, 18340, 19426, 20577, 21797, 23088, 24456, 25905, 27440,
 115     29066, 30789, 32613
 116 };
 117
 118 /* Function for converting dB to squared amplitude factor (A = 10^(dB/40)).
 119    Range is -24 to 24 dB. If gain values outside of this is needed, the above
 120    table needs to be extended.
 121    Parameter format is s15.16 fixed point. Return format is s2.29 fixed point.
 122  */
 123 static long dbtoA(long db)
 124 {
 125     const unsigned long bias = 24 << 16;
 126     unsigned short frac = (db + bias) & 0x0000ffff;
 127     unsigned short pos = (db + bias) >> 16;
 128     short diff = dbtoatab[pos + 1] - dbtoatab[pos];
 129
 130     return (dbtoatab[pos] << 16) + frac*diff;
 131 }
 132
 133 /* Calculate second order section peaking filter coefficients.
 134    cutoff is a value from 0 to 0x80000000, where 0 represents 0 hz and
 135    0x80000000 represents nyquist (samplerate/2).
 136    Q is an unsigned 16.16 fixed point number, lower bound is artificially set
 137    at 0.5.
 138    db is s15.16 fixed point and describes gain/attenuation at peak freq.
 139    c is a pointer where the coefs will be stored.
 140  */
 141 void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, long *c)
 142 {
 143     const long one = 1 << 28; /* s3.28 */
 144     const long A = dbtoA(db);
 145     const long alpha = DIV64(fsin(cutoff), 2*Q, 15); /* s1.30 */
 146     long a0, a1, a2; /* these are all s3.28 format */
 147     long b0, b1, b2;
 148
 149     /* possible numerical ranges listed after each coef */
 150     b0 = one + FRACMUL(alpha, A);     /* [1.25..5] */
 151     b1 = a1 = -2*(fcos(cutoff) >> 3); /* [-2..2] */
 152     b2 = one - FRACMUL(alpha, A);     /* [-3..0.75] */
 153     a0 = one + DIV64(alpha, A, 27);   /* [1.25..5] */
 154     a2 = one - DIV64(alpha, A, 27);   /* [-3..0.75] */
 155
 156     c[0] = DIV64(b0, a0, 28);
 157     c[1] = DIV64(b1, a0, 28);
 158     c[2] = DIV64(b2, a0, 28);
 159     c[3] = DIV64(-a1, a0, 28);
 160     c[4] = DIV64(-a2, a0, 28);
 161 }
 162
 163 /* Calculate coefficients for lowshelf filter */
 164 void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, long *c)
 165 {
 166     const long one = 1 << 24; /* s7.24 */
 167     const long A = dbtoA(db);
 168     const long alpha = DIV64(fsin(cutoff), 2*Q, 15); /* s1.30 */
 169     const long ap1 = (A >> 5) + one;
 170     const long am1 = (A >> 5) - one;
 171     const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1);
 172     long a0, a1, a2; /* these are all s7.24 format */
 173     long b0, b1, b2;
 174     long cs = fcos(cutoff);
 175
 176     b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2;
 177     b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3;
 178     b2 = FRACMUL(A, ap1 - FRACMUL(am1, cs) - twosqrtalpha) << 2;
 179     a0 = ap1 + FRACMUL(am1, cs) + twosqrtalpha;
 180     a1 = -2*((am1 + FRACMUL(ap1, cs)));
 181     a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha;
 182
 183     c[0] = DIV64(b0, a0, 24);
 184     c[1] = DIV64(b1, a0, 24);
 185     c[2] = DIV64(b2, a0, 24);
 186     c[3] = DIV64(-a1, a0, 24);
 187     c[4] = DIV64(-a2, a0, 24);
 188 }
 189
 190 /* Calculate coefficients for highshelf filter */
 191 void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, long *c)
 192 {
 193     const long one = 1 << 24; /* s7.24 */
 194     const long A = dbtoA(db);
 195     const long alpha = DIV64(fsin(cutoff), 2*Q, 15); /* s1.30 */
 196     const long ap1 = (A >> 5) + one;
 197     const long am1 = (A >> 5) - one;
 198     const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1);
 199     long a0, a1, a2; /* these are all s7.24 format */
 200     long b0, b1, b2;
 201     long cs = fcos(cutoff);
 202
 203     b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2;
 204     b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3;
 205     b2 = FRACMUL(A, ap1 + FRACMUL(am1, cs) - twosqrtalpha) << 2;
 206     a0 = ap1 - FRACMUL(am1, cs) + twosqrtalpha;
 207     a1 = 2*((am1 - FRACMUL(ap1, cs)));
 208     a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha;
 209
 210     c[0] = DIV64(b0, a0, 24);
 211     c[1] = DIV64(b1, a0, 24);
 212     c[2] = DIV64(b2, a0, 24);
 213     c[3] = DIV64(-a1, a0, 24);
 214     c[4] = DIV64(-a2, a0, 24);
 215 }
 216
 217 #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) || defined(SIMULATOR)
 218 void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
 219                unsigned channels, unsigned shift)
 220 {
 221     unsigned c, i;
 222     long long acc;
 223
 224     /* Direct form 1 filtering code.
 225        y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
 226        where y[] is output and x[] is input.
 227      */
 228
 229     for (c = 0; c < channels; c++) {
 230         for (i = 0; i < num; i++) {
 231             acc  = (long long) x[c][i] * f->coefs[0];
 232             acc += (long long) f->history[c][0] * f->coefs[1];
 233             acc += (long long) f->history[c][1] * f->coefs[2];
 234             acc += (long long) f->history[c][2] * f->coefs[3];
 235             acc += (long long) f->history[c][3] * f->coefs[4];
 236             f->history[c][1] = f->history[c][0];
 237             f->history[c][0] = x[c][i];
 238             f->history[c][3] = f->history[c][2];
 239             x[c][i] = (acc << shift) >> 32;
 240             f->history[c][2] = x[c][i];
 241         }
 242     }
 243 }
 244 #endif
 245