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 guaranteed,
  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 first order shelving filter coefficients.
 191  * Note that the filter is not compatible with the eq_filter routine.
 192  * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and
 193  * 0x80000000 represents the Nyquist frequency (samplerate/2).
 194  * @param ad gain at 0 Hz. s3.27 fixed point.
 195  * @param an gain at Nyquist frequency. s3.27 fixed point.
 196  * @param c pointer to coefficient storage. The coefs are s0.31 format.
 197  */
 198 void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c)
 199 {
 200     const long one = 1 << 27;
 201     long a0, a1;
 202     long b0, b1;
 203     long s, cs;
 204     s = fsincos(cutoff, &cs) >> 4;
 205     cs = one + (cs >> 4);
 206
 207     /* For max A = 4 (24 dB) */
 208     b0 = (FRACMUL(an, cs) << 4) + (FRACMUL(ad, s) << 4);
 209     b1 = (FRACMUL(ad, s) << 4) - (FRACMUL(an, cs) << 4);
 210     a0 = s + cs;
 211     a1 = s - cs;
 212
 213     c[0] = DIV64(b0, a0, 31);
 214     c[1] = DIV64(b1, a0, 31);
 215     c[2] = -DIV64(a1, a0, 31);
 216 }
 217
 218 /**
 219  * Calculate second order section peaking filter coefficients.
 220  * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and
 221  * 0x80000000 represents the Nyquist frequency (samplerate/2).
 222  * @param Q 16.16 fixed point value describing Q factor. Lower bound
 223  * is artificially set at 0.5.
 224  * @param db s15.16 fixed point value describing gain/attenuation at peak freq.
 225  * @param c pointer to coefficient storage. Coefficients are s3.28 format.
 226  */
 227 void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
 228 {
 229     long cc;
 230     const long one = 1 << 28; /* s3.28 */
 231     const long A = dbtoA(db);
 232     const long alpha = DIV64(fsincos(cutoff, &cc), 2*Q, 15); /* s1.30 */
 233     int32_t a0, a1, a2; /* these are all s3.28 format */
 234     int32_t b0, b1, b2;
 235
 236     /* possible numerical ranges are in comments by each coef */
 237     b0 = one + FRACMUL(alpha, A);     /* [1 .. 5] */
 238     b1 = a1 = -2*(cc >> 3);           /* [-2 .. 2] */
 239     b2 = one - FRACMUL(alpha, A);     /* [-3 .. 1] */
 240     a0 = one + DIV64(alpha, A, 27);   /* [1 .. 5] */
 241     a2 = one - DIV64(alpha, A, 27);   /* [-3 .. 1] */
 242
 243     c[0] = DIV64(b0, a0, 28);         /* [0.25 .. 4] */
 244     c[1] = DIV64(b1, a0, 28);         /* [-2 .. 2] */
 245     c[2] = DIV64(b2, a0, 28);         /* [-2.4 .. 1] */
 246     c[3] = DIV64(-a1, a0, 28);        /* [-2 .. 2] */
 247     c[4] = DIV64(-a2, a0, 28);        /* [-0.6 .. 1] */
 248 }
 249
 250 /**
 251  * Calculate coefficients for lowshelf filter. Parameters are as for
 252  * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
 253  */
 254 void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
 255 {
 256     long cs;
 257     const long one = 1 << 25; /* s6.25 */
 258     const long A = dbtoA(db);
 259     const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */
 260     const long ap1 = (A >> 4) + one;
 261     const long am1 = (A >> 4) - one;
 262     const long twosqrtalpha = 2*FRACMUL(fsqrt(A >> 3, 26), alpha);
 263     int32_t a0, a1, a2; /* these are all s6.25 format */
 264     int32_t b0, b1, b2;
 265
 266     /* [0.1 .. 40] */
 267     b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2;
 268     /* [-16 .. 63.4] */
 269     b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3;
 270     /* [0 .. 31.7] */
 271     b2 = FRACMUL(A, ap1 - FRACMUL(am1, cs) - twosqrtalpha) << 2;
 272     /* [0.5 .. 10] */
 273     a0 = ap1 + FRACMUL(am1, cs) + twosqrtalpha;
 274     /* [-16 .. 4] */
 275     a1 = -2*((am1 + FRACMUL(ap1, cs)));
 276     /* [0 .. 8] */
 277     a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha;
 278
 279     c[0] = DIV64(b0, a0, 26);       /* [0.06 .. 15.9] */
 280     c[1] = DIV64(b1, a0, 26);       /* [-2 .. 31.7] */
 281     c[2] = DIV64(b2, a0, 26);       /* [0 .. 15.9] */
 282     c[3] = DIV64(-a1, a0, 26);      /* [-2 .. 2] */
 283     c[4] = DIV64(-a2, a0, 26);      /* [0 .. 1] */
 284 }
 285
 286 /**
 287  * Calculate coefficients for highshelf filter. Parameters are as for
 288  * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
 289  */
 290 void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c)
 291 {
 292     long cs;
 293     const int one = 1 << 25; /* s6.25 */
 294     const int A = dbtoA(db);
 295     const int alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */
 296     const int ap1 = (A >> 4) + one;
 297     const int am1 = (A >> 4) - one;
 298     const int twosqrtalpha = 2*FRACMUL(fsqrt(A >> 3, 26), alpha);
 299     int32_t a0, a1, a2; /* these are all s6.25 format */
 300     int32_t b0, b1, b2;
 301
 302     /* [0.1 .. 40] */
 303     b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2;
 304     /* [-63.5 .. 16] */
 305     b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3;
 306     /* [0 .. 32] */
 307     b2 = FRACMUL(A, ap1 + FRACMUL(am1, cs) - twosqrtalpha) << 2;
 308     /* [0.5 .. 10] */
 309     a0 = ap1 - FRACMUL(am1, cs) + twosqrtalpha;
 310     /* [-4 .. 16] */
 311     a1 = 2*((am1 - FRACMUL(ap1, cs)));
 312     /* [0 .. 8] */
 313     a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha;
 314
 315     c[0] = DIV64(b0, a0, 26);       /* [0 .. 16] */
 316     c[1] = DIV64(b1, a0, 26);       /* [-31.7 .. 2] */
 317     c[2] = DIV64(b2, a0, 26);       /* [0 .. 16] */
 318     c[3] = DIV64(-a1, a0, 26);      /* [-2 .. 2] */
 319     c[4] = DIV64(-a2, a0, 26);      /* [0 .. 1] */
 320 }
 321
 322 #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) || defined(SIMULATOR)
 323 void eq_filter(int32_t **x, struct eqfilter *f, unsigned num,
 324                unsigned channels, unsigned shift)
 325 {
 326     unsigned c, i;
 327     long long acc;
 328
 329     /* Direct form 1 filtering code.
 330        y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
 331        where y[] is output and x[] is input.
 332      */
 333
 334     for (c = 0; c < channels; c++) {
 335         for (i = 0; i < num; i++) {
 336             acc  = (long long) x[c][i] * f->coefs[0];
 337             acc += (long long) f->history[c][0] * f->coefs[1];
 338             acc += (long long) f->history[c][1] * f->coefs[2];
 339             acc += (long long) f->history[c][2] * f->coefs[3];
 340             acc += (long long) f->history[c][3] * f->coefs[4];
 341             f->history[c][1] = f->history[c][0];
 342             f->history[c][0] = x[c][i];
 343             f->history[c][3] = f->history[c][2];
 344             x[c][i] = (acc << shift) >> 32;
 345             f->history[c][2] = x[c][i];
 346         }
 347     }
 348 }
 349 #endif
 350