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 /* Inverse gain of circular cordic rotation in s0.31 format. */
  57 static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */
  58
  59 /* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */
  60 static const unsigned long atan_table[] = {
  61     0x1fffffff, /* +0.785398163 (or pi/4) */
  62     0x12e4051d, /* +0.463647609 */
  63     0x09fb385b, /* +0.244978663 */
  64     0x051111d4, /* +0.124354995 */
  65     0x028b0d43, /* +0.062418810 */
  66     0x0145d7e1, /* +0.031239833 */
  67     0x00a2f61e, /* +0.015623729 */
  68     0x00517c55, /* +0.007812341 */
  69     0x0028be53, /* +0.003906230 */
  70     0x00145f2e, /* +0.001953123 */
  71     0x000a2f98, /* +0.000976562 */
  72     0x000517cc, /* +0.000488281 */
  73     0x00028be6, /* +0.000244141 */
  74     0x000145f3, /* +0.000122070 */
  75     0x0000a2f9, /* +0.000061035 */
  76     0x0000517c, /* +0.000030518 */
  77     0x000028be, /* +0.000015259 */
  78     0x0000145f, /* +0.000007629 */
  79     0x00000a2f, /* +0.000003815 */
  80     0x00000517, /* +0.000001907 */
  81     0x0000028b, /* +0.000000954 */
  82     0x00000145, /* +0.000000477 */
  83     0x000000a2, /* +0.000000238 */
  84     0x00000051, /* +0.000000119 */
  85     0x00000028, /* +0.000000060 */
  86     0x00000014, /* +0.000000030 */
  87     0x0000000a, /* +0.000000015 */
  88     0x00000005, /* +0.000000007 */
  89     0x00000002, /* +0.000000004 */
  90     0x00000001, /* +0.000000002 */
  91     0x00000000, /* +0.000000001 */
  92     0x00000000, /* +0.000000000 */
  93 };
  94
  95 /**
  96  * Implements sin and cos using CORDIC rotation.
  97  *
  98  * @param phase has range from 0 to 0xffffffff, representing 0 and
  99  *        2*pi respectively.
 100  * @param cos return address for cos
 101  * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX,
 102  *         representing -1 and 1 respectively.
 103  */
 104 long fsincos(unsigned long phase, long *cos) {
 105     int32_t x, x1, y, y1;
 106     unsigned long z, z1;
 107     int i;
 108
 109     /* Setup initial vector */
 110     x = cordic_circular_gain;
 111     y = 0;
 112     z = phase;
 113
 114     /* The phase has to be somewhere between 0..pi for this to work right */
 115     if (z < 0xffffffff / 4) {
 116         /* z in first quadrant, z += pi/2 to correct */
 117         x = -x;
 118         z += 0xffffffff / 4;
 119     } else if (z < 3 * (0xffffffff / 4)) {
 120         /* z in third quadrant, z -= pi/2 to correct */
 121         z -= 0xffffffff / 4;
 122     } else {
 123         /* z in fourth quadrant, z -= 3pi/2 to correct */
 124         x = -x;
 125         z -= 3 * (0xffffffff / 4);
 126     }
 127
 128     /* Each iteration adds roughly 1-bit of extra precision */
 129     for (i = 0; i < 31; i++) {
 130         x1 = x >> i;
 131         y1 = y >> i;
 132         z1 = atan_table[i];
 133
 134         /* Decided which direction to rotate vector. Pivot point is pi/2 */
 135         if (z >= 0xffffffff / 4) {
 136             x -= y1;
 137             y += x1;
 138             z -= z1;
 139         } else {
 140             x += y1;
 141             y -= x1;
 142             z += z1;
 143         }
 144     }
 145
 146     *cos = x;
 147
 148     return y;
 149 }
 150
 151 /* Fixed point square root via Newton-Raphson.
 152  * Output is in same fixed point format as input.
 153  * fracbits specifies number of fractional bits in argument.
 154  */
 155 static long fsqrt(long a, unsigned int fracbits)
 156 {
 157     long b = a/2 + (1 << fracbits); /* initial approximation */
 158     unsigned n;
 159     const unsigned iterations = 4;
 160
 161     for (n = 0; n < iterations; ++n)
 162         b = (b + DIV64(a, b, fracbits))/2;
 163
 164     return b;
 165 }
 166
 167 short dbtoatab[49] = {
 168     2058, 2180, 2309, 2446, 2591, 2744, 2907, 3079, 3261, 3455, 3659, 3876,
 169     4106, 4349, 4607, 4880, 5169, 5475, 5799, 6143, 6507, 6893, 7301, 7734,
 170     8192, 8677, 9192, 9736, 10313, 10924, 11572, 12257, 12983, 13753, 14568,
 171     15431, 16345, 17314, 18340, 19426, 20577, 21797, 23088, 24456, 25905, 27440,
 172     29066, 30789, 32613
 173 };
 174
 175 /* Function for converting dB to squared amplitude factor (A = 10^(dB/40)).
 176    Range is -24 to 24 dB. If gain values outside of this is needed, the above
 177    table needs to be extended.
 178    Parameter format is s15.16 fixed point. Return format is s2.29 fixed point.
 179  */
 180 static long dbtoA(long db)
 181 {
 182     const unsigned long bias = 24 << 16;
 183     unsigned short frac = (db + bias) & 0x0000ffff;
 184     unsigned short pos = (db + bias) >> 16;
 185     short diff = dbtoatab[pos + 1] - dbtoatab[pos];
 186
 187     return (dbtoatab[pos] << 16) + frac*diff;
 188 }
 189
 190 /* Calculate second order section peaking filter coefficients.
 191    cutoff is a value from 0 to 0x80000000, where 0 represents 0 hz and
 192    0x80000000 represents nyquist (samplerate/2).
 193    Q is an unsigned 16.16 fixed point number, lower bound is artificially set
 194    at 0.5.
 195    db is s15.16 fixed point and describes gain/attenuation at peak freq.
 196    c is a pointer where the coefs will be stored.
 197  */
 198 void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
 199 {
 200     long cc;
 201     const long one = 1 << 28; /* s3.28 */
 202     const long A = dbtoA(db);
 203     const long alpha = DIV64(fsincos(cutoff, &cc), 2*Q, 15); /* s1.30 */
 204     int32_t a0, a1, a2; /* these are all s3.28 format */
 205     int32_t b0, b1, b2;
 206
 207     /* possible numerical ranges listed after each coef */
 208     b0 = one + FRACMUL(alpha, A);     /* [1.25..5] */
 209     b1 = a1 = -2*(cc >> 3);           /* [-2..2] */
 210     b2 = one - FRACMUL(alpha, A);     /* [-3..0.75] */
 211     a0 = one + DIV64(alpha, A, 27);   /* [1.25..5] */
 212     a2 = one - DIV64(alpha, A, 27);   /* [-3..0.75] */
 213
 214     c[0] = DIV64(b0, a0, 28);
 215     c[1] = DIV64(b1, a0, 28);
 216     c[2] = DIV64(b2, a0, 28);
 217     c[3] = DIV64(-a1, a0, 28);
 218     c[4] = DIV64(-a2, a0, 28);
 219 }
 220
 221 /* Calculate coefficients for lowshelf filter */
 222 void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
 223 {
 224     long cs;
 225     const long one = 1 << 24; /* s7.24 */
 226     const long A = dbtoA(db);
 227     const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */
 228     const long ap1 = (A >> 5) + one;
 229     const long am1 = (A >> 5) - one;
 230     const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1);
 231     int32_t a0, a1, a2; /* these are all s7.24 format */
 232     int32_t b0, b1, b2;
 233
 234     b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2;
 235     b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3;
 236     b2 = FRACMUL(A, ap1 - FRACMUL(am1, cs) - twosqrtalpha) << 2;
 237     a0 = ap1 + FRACMUL(am1, cs) + twosqrtalpha;
 238     a1 = -2*((am1 + FRACMUL(ap1, cs)));
 239     a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha;
 240
 241     c[0] = DIV64(b0, a0, 24);
 242     c[1] = DIV64(b1, a0, 24);
 243     c[2] = DIV64(b2, a0, 24);
 244     c[3] = DIV64(-a1, a0, 24);
 245     c[4] = DIV64(-a2, a0, 24);
 246 }
 247
 248 /* Calculate coefficients for highshelf filter */
 249 void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
 250 {
 251     long cs;
 252     const long one = 1 << 24; /* s7.24 */
 253     const long A = dbtoA(db);
 254     const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */
 255     const long ap1 = (A >> 5) + one;
 256     const long am1 = (A >> 5) - one;
 257     const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1);
 258     int32_t a0, a1, a2; /* these are all s7.24 format */
 259     int32_t b0, b1, b2;
 260
 261     b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2;
 262     b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3;
 263     b2 = FRACMUL(A, ap1 + FRACMUL(am1, cs) - twosqrtalpha) << 2;
 264     a0 = ap1 - FRACMUL(am1, cs) + twosqrtalpha;
 265     a1 = 2*((am1 - FRACMUL(ap1, cs)));
 266     a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha;
 267
 268     c[0] = DIV64(b0, a0, 24);
 269     c[1] = DIV64(b1, a0, 24);
 270     c[2] = DIV64(b2, a0, 24);
 271     c[3] = DIV64(-a1, a0, 24);
 272     c[4] = DIV64(-a2, a0, 24);
 273 }
 274
 275 #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) || defined(SIMULATOR)
 276 void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
 277                unsigned channels, unsigned shift)
 278 {
 279     unsigned c, i;
 280     long long acc;
 281
 282     /* Direct form 1 filtering code.
 283        y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
 284        where y[] is output and x[] is input.
 285      */
 286
 287     for (c = 0; c < channels; c++) {
 288         for (i = 0; i < num; i++) {
 289             acc  = (long long) x[c][i] * f->coefs[0];
 290             acc += (long long) f->history[c][0] * f->coefs[1];
 291             acc += (long long) f->history[c][1] * f->coefs[2];
 292             acc += (long long) f->history[c][2] * f->coefs[3];
 293             acc += (long long) f->history[c][3] * f->coefs[4];
 294             f->history[c][1] = f->history[c][0];
 295             f->history[c][0] = x[c][i];
 296             f->history[c][3] = f->history[c][2];
 297             x[c][i] = (acc << shift) >> 32;
 298             f->history[c][2] = x[c][i];
 299         }
 300     }
 301 }
 302 #endif
 303