lib/lame/fft.c

   1 /*
   2 ** FFT and FHT routines
   3 **  Copyright 1988, 1993; Ron Mayer
   4 **
   5 **  fht(fz,n);
   6 **      Does a hartley transform of "n" points in the array "fz".
   7 **
   8 ** NOTE: This routine uses at least 2 patented algorithms, and may be
   9 **       under the restrictions of a bunch of different organizations.
  10 **       Although I wrote it completely myself; it is kind of a derivative
  11 **       of a routine I once authored and released under the GPL, so it
  12 **       may fall under the free software foundation's restrictions;
  13 **       it was worked on as a Stanford Univ project, so they claim
  14 **       some rights to it; it was further optimized at work here, so
  15 **       I think this company claims parts of it.  The patents are
  16 **       held by R. Bracewell (the FHT algorithm) and O. Buneman (the
  17 **       trig generator), both at Stanford Univ.
  18 **       If it were up to me, I'd say go do whatever you want with it;
  19 **       but it would be polite to give credit to the following people
  20 **       if you use this anywhere:
  21 **           Euler     - probable inventor of the fourier transform.
  22 **           Gauss     - probable inventor of the FFT.
  23 **           Hartley   - probable inventor of the hartley transform.
  24 **           Buneman   - for a really cool trig generator
  25 **           Mayer(me) - for authoring this particular version and
  26 **                       including all the optimizations in one package.
  27 **       Thanks,
  28 **       Ron Mayer; mayer@acuson.com
  29 ** and added some optimization by
  30 **           Mather    - idea of using lookup table
  31 **           Takehiro  - some dirty hack for speed up
  32 */
  33
  34 /* $Id: fft.c,v 1.2 2006/02/09 16:56:23 kramm Exp $ */
  35
  36 #include <stdlib.h>
  37 #include "config_static.h"
  38
  39 #include <math.h>
  40 #include "util.h"
  41 #include "fft.h"
  42
  43
  44
  45
  46 #ifdef WITH_DMALLOC
  47 #include <dmalloc.h>
  48 #endif
  49
  50 #define TRI_SIZE (5-1) /* 1024 =  4**5 */
  51
  52 static const FLOAT costab[TRI_SIZE*2] = {
  53   9.238795325112867e-01, 3.826834323650898e-01,
  54   9.951847266721969e-01, 9.801714032956060e-02,
  55   9.996988186962042e-01, 2.454122852291229e-02,
  56   9.999811752826011e-01, 6.135884649154475e-03
  57 };
  58
  59 static void fht(FLOAT *fz, int n)
  60 {
  61     const FLOAT *tri = costab;
  62     int           k4;
  63     FLOAT *fi, *fn, *gi;
  64
  65     n <<= 1;        /* to get BLKSIZE, because of 3DNow! ASM routine */
  66     fn = fz + n;
  67     k4 = 4;
  68     do {
  69         FLOAT s1, c1;
  70         int   i, k1, k2, k3, kx;
  71         kx  = k4 >> 1;
  72         k1  = k4;
  73         k2  = k4 << 1;
  74         k3  = k2 + k1;
  75         k4  = k2 << 1;
  76         fi  = fz;
  77         gi  = fi + kx;
  78         do {
  79             FLOAT f0,f1,f2,f3;
  80             f1      = fi[0]  - fi[k1];
  81             f0      = fi[0]  + fi[k1];
  82             f3      = fi[k2] - fi[k3];
  83             f2      = fi[k2] + fi[k3];
  84             fi[k2]  = f0     - f2;
  85             fi[0 ]  = f0     + f2;
  86             fi[k3]  = f1     - f3;
  87             fi[k1]  = f1     + f3;
  88             f1      = gi[0]  - gi[k1];
  89             f0      = gi[0]  + gi[k1];
  90             f3      = SQRT2  * gi[k3];
  91             f2      = SQRT2  * gi[k2];
  92             gi[k2]  = f0     - f2;
  93             gi[0 ]  = f0     + f2;
  94             gi[k3]  = f1     - f3;
  95             gi[k1]  = f1     + f3;
  96             gi     += k4;
  97             fi     += k4;
  98         } while (fi<fn);
  99         c1 = tri[0];
 100         s1 = tri[1];
 101         for (i = 1; i < kx; i++) {
 102             FLOAT c2,s2;
 103             c2 = 1 - (2*s1)*s1;
 104             s2 = (2*s1)*c1;
 105             fi = fz + i;
 106             gi = fz + k1 - i;
 107             do {
 108                 FLOAT a,b,g0,f0,f1,g1,f2,g2,f3,g3;
 109                 b       = s2*fi[k1] - c2*gi[k1];
 110                 a       = c2*fi[k1] + s2*gi[k1];
 111                 f1      = fi[0 ]    - a;
 112                 f0      = fi[0 ]    + a;
 113                 g1      = gi[0 ]    - b;
 114                 g0      = gi[0 ]    + b;
 115                 b       = s2*fi[k3] - c2*gi[k3];
 116                 a       = c2*fi[k3] + s2*gi[k3];
 117                 f3      = fi[k2]    - a;
 118                 f2      = fi[k2]    + a;
 119                 g3      = gi[k2]    - b;
 120                 g2      = gi[k2]    + b;
 121                 b       = s1*f2     - c1*g3;
 122                 a       = c1*f2     + s1*g3;
 123                 fi[k2]  = f0        - a;
 124                 fi[0 ]  = f0        + a;
 125                 gi[k3]  = g1        - b;
 126                 gi[k1]  = g1        + b;
 127                 b       = c1*g2     - s1*f3;
 128                 a       = s1*g2     + c1*f3;
 129                 gi[k2]  = g0        - a;
 130                 gi[0 ]  = g0        + a;
 131                 fi[k3]  = f1        - b;
 132                 fi[k1]  = f1        + b;
 133                 gi     += k4;
 134                 fi     += k4;
 135             } while (fi<fn);
 136             c2 = c1;
 137             c1 = c2 * tri[0] - s1 * tri[1];
 138             s1 = c2 * tri[1] + s1 * tri[0];
 139         }
 140         tri += 2;
 141     } while (k4<n);
 142 }
 143
 144 static const unsigned char rv_tbl[] = {
 145     0x00,    0x80,    0x40,    0xc0,    0x20,    0xa0,    0x60,    0xe0,
 146     0x10,    0x90,    0x50,    0xd0,    0x30,    0xb0,    0x70,    0xf0,
 147     0x08,    0x88,    0x48,    0xc8,    0x28,    0xa8,    0x68,    0xe8,
 148     0x18,    0x98,    0x58,    0xd8,    0x38,    0xb8,    0x78,    0xf8,
 149     0x04,    0x84,    0x44,    0xc4,    0x24,    0xa4,    0x64,    0xe4,
 150     0x14,    0x94,    0x54,    0xd4,    0x34,    0xb4,    0x74,    0xf4,
 151     0x0c,    0x8c,    0x4c,    0xcc,    0x2c,    0xac,    0x6c,    0xec,
 152     0x1c,    0x9c,    0x5c,    0xdc,    0x3c,    0xbc,    0x7c,    0xfc,
 153     0x02,    0x82,    0x42,    0xc2,    0x22,    0xa2,    0x62,    0xe2,
 154     0x12,    0x92,    0x52,    0xd2,    0x32,    0xb2,    0x72,    0xf2,
 155     0x0a,    0x8a,    0x4a,    0xca,    0x2a,    0xaa,    0x6a,    0xea,
 156     0x1a,    0x9a,    0x5a,    0xda,    0x3a,    0xba,    0x7a,    0xfa,
 157     0x06,    0x86,    0x46,    0xc6,    0x26,    0xa6,    0x66,    0xe6,
 158     0x16,    0x96,    0x56,    0xd6,    0x36,    0xb6,    0x76,    0xf6,
 159     0x0e,    0x8e,    0x4e,    0xce,    0x2e,    0xae,    0x6e,    0xee,
 160     0x1e,    0x9e,    0x5e,    0xde,    0x3e,    0xbe,    0x7e,    0xfe
 161 };
 162
 163 #define ch01(index)  (buffer[chn][index])
 164
 165 #define ml00(f) (window[i        ] * f(i))
 166 #define ml10(f) (window[i + 0x200] * f(i + 0x200))
 167 #define ml20(f) (window[i + 0x100] * f(i + 0x100))
 168 #define ml30(f) (window[i + 0x300] * f(i + 0x300))
 169
 170 #define ml01(f) (window[i + 0x001] * f(i + 0x001))
 171 #define ml11(f) (window[i + 0x201] * f(i + 0x201))
 172 #define ml21(f) (window[i + 0x101] * f(i + 0x101))
 173 #define ml31(f) (window[i + 0x301] * f(i + 0x301))
 174
 175 #define ms00(f) (window_s[i       ] * f(i + k))
 176 #define ms10(f) (window_s[0x7f - i] * f(i + k + 0x80))
 177 #define ms20(f) (window_s[i + 0x40] * f(i + k + 0x40))
 178 #define ms30(f) (window_s[0x3f - i] * f(i + k + 0xc0))
 179
 180 #define ms01(f) (window_s[i + 0x01] * f(i + k + 0x01))
 181 #define ms11(f) (window_s[0x7e - i] * f(i + k + 0x81))
 182 #define ms21(f) (window_s[i + 0x41] * f(i + k + 0x41))
 183 #define ms31(f) (window_s[0x3e - i] * f(i + k + 0xc1))
 184
 185
 186 void fft_short(lame_internal_flags * const gfc,
 187                 FLOAT x_real[3][BLKSIZE_s], int chn, const sample_t *buffer[2])
 188 {
 189     const FLOAT*  window_s = (const FLOAT *)&gfc->window_s[0];
 190     int           i;
 191     int           j;
 192     int           b;
 193
 194     for (b = 0; b < 3; b++) {
 195         FLOAT *x = &x_real[b][BLKSIZE_s / 2];
 196         short k = (576 / 3) * (b + 1);
 197         j = BLKSIZE_s / 8 - 1;
 198         do {
 199           FLOAT f0,f1,f2,f3, w;
 200
 201           i = rv_tbl[j << 2];
 202
 203           f0 = ms00(ch01); w = ms10(ch01); f1 = f0 - w; f0 = f0 + w;
 204           f2 = ms20(ch01); w = ms30(ch01); f3 = f2 - w; f2 = f2 + w;
 205
 206           x -= 4;
 207           x[0] = f0 + f2;
 208           x[2] = f0 - f2;
 209           x[1] = f1 + f3;
 210           x[3] = f1 - f3;
 211
 212           f0 = ms01(ch01); w = ms11(ch01); f1 = f0 - w; f0 = f0 + w;
 213           f2 = ms21(ch01); w = ms31(ch01); f3 = f2 - w; f2 = f2 + w;
 214
 215           x[BLKSIZE_s / 2 + 0] = f0 + f2;
 216           x[BLKSIZE_s / 2 + 2] = f0 - f2;
 217           x[BLKSIZE_s / 2 + 1] = f1 + f3;
 218           x[BLKSIZE_s / 2 + 3] = f1 - f3;
 219         } while (--j >= 0);
 220
 221         gfc->fft_fht(x, BLKSIZE_s/2);
 222         /* BLKSIZE_s/2 because of 3DNow! ASM routine */
 223     }
 224 }
 225
 226 void fft_long(lame_internal_flags * const gfc,
 227                FLOAT x[BLKSIZE], int chn, const sample_t *buffer[2] )
 228 {
 229     const FLOAT*  window = (const FLOAT *)&gfc->window[0];
 230     int           i;
 231     int           jj = BLKSIZE / 8 - 1;
 232     x += BLKSIZE / 2;
 233
 234     do {
 235       FLOAT f0,f1,f2,f3, w;
 236
 237       i = rv_tbl[jj];
 238       f0 = ml00(ch01); w = ml10(ch01); f1 = f0 - w; f0 = f0 + w;
 239       f2 = ml20(ch01); w = ml30(ch01); f3 = f2 - w; f2 = f2 + w;
 240
 241       x -= 4;
 242       x[0] = f0 + f2;
 243       x[2] = f0 - f2;
 244       x[1] = f1 + f3;
 245       x[3] = f1 - f3;
 246
 247       f0 = ml01(ch01); w = ml11(ch01); f1 = f0 - w; f0 = f0 + w;
 248       f2 = ml21(ch01); w = ml31(ch01); f3 = f2 - w; f2 = f2 + w;
 249
 250       x[BLKSIZE / 2 + 0] = f0 + f2;
 251       x[BLKSIZE / 2 + 2] = f0 - f2;
 252       x[BLKSIZE / 2 + 1] = f1 + f3;
 253       x[BLKSIZE / 2 + 3] = f1 - f3;
 254     } while (--jj >= 0);
 255
 256     gfc->fft_fht(x, BLKSIZE/2);
 257     /* BLKSIZE/2 because of 3DNow! ASM routine */
 258 }
 259
 260
 261 void init_fft(lame_internal_flags * const gfc)
 262 {
 263     FLOAT *window   = &gfc->window[0];
 264     FLOAT *window_s = &gfc->window_s[0];
 265     int i;
 266
 267 #if 0
 268     if (gfc->nsPsy.use) {
 269       for (i = 0; i < BLKSIZE ; i++)
 270         /* blackman window */
 271         window[i] = 0.42-0.5*cos(2*PI*i/(BLKSIZE-1))+0.08*cos(4*PI*i/(BLKSIZE-1));
 272     } else {
 273       /*
 274        * calculate HANN window coefficients
 275        */
 276       for (i = 0; i < BLKSIZE ; i++)
 277         window[i] = 0.5 * (1.0 - cos(2.0 * PI * (i + 0.5) / BLKSIZE));
 278     }
 279 #endif
 280
 281     // The type of window used here will make no real difference, but
 282     // in the interest of merging nspsytune stuff - switch to blackman window
 283     for (i = 0; i < BLKSIZE ; i++)
 284       /* blackman window */
 285       window[i] = 0.42-0.5*cos(2*PI*(i+.5)/BLKSIZE)+
 286         0.08*cos(4*PI*(i+.5)/BLKSIZE);
 287
 288     for (i = 0; i < BLKSIZE_s/2 ; i++)
 289         window_s[i] = 0.5 * (1.0 - cos(2.0 * PI * (i + 0.5) / BLKSIZE_s));
 290
 291 #ifdef HAVE_NASM
 292     if (gfc->CPU_features.AMD_3DNow) {
 293         extern void fht_3DN(FLOAT *fz, int n);
 294         gfc->fft_fht = fht_3DN;
 295     } else
 296 #endif
 297 #ifdef USE_FFTSSE
 298     if (gfc->CPU_features.SIMD) {
 299         extern void fht_SSE(FLOAT *fz, int n);
 300         gfc->fft_fht = fht_SSE;
 301     } else
 302 #endif
 303 #ifdef USE_FFTFPU
 304     if (gfc->CPU_features.i387) {
 305         extern void fht_FPU(FLOAT *fz, int n);
 306         gfc->fft_fht = fht_FPU;
 307     } else
 308 #endif
 309         gfc->fft_fht = fht;
 310 }