vcs.maemo.org Git - tunertool/blob - src/kiss_fft.c

   1 /*
   2 Copyright (c) 2003-2004, Mark Borgerding
   3
   4 All rights reserved.
   5
   6 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
   7
   8     * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
   9     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
  10     * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission.
  11
  12 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  13 */
  14
  15
  16 #include "_kiss_fft_guts.h"
  17 /* The guts header contains all the multiplication and addition macros that are defined for
  18  fixed or floating point complex numbers.  It also delares the kf_ internal functions.
  19  */
  20
  21 static kiss_fft_cpx *scratchbuf = NULL;
  22 static size_t nscratchbuf = 0;
  23 static kiss_fft_cpx *tmpbuf = NULL;
  24 static size_t ntmpbuf = 0;
  25
  26 #define CHECKBUF(buf,nbuf,n) \
  27     do { \
  28         if ( nbuf < (size_t)(n) ) {\
  29             free(buf); \
  30             buf = (kiss_fft_cpx*)KISS_FFT_MALLOC(sizeof(kiss_fft_cpx)*(n)); \
  31             nbuf = (size_t)(n); \
  32         } \
  33    }while(0)
  34
  35
  36 static void
  37 kf_bfly2 (kiss_fft_cpx * Fout,
  38     const size_t fstride, const kiss_fft_cfg st, int m)
  39 {
  40   kiss_fft_cpx *Fout2;
  41   kiss_fft_cpx *tw1 = st->twiddles;
  42   kiss_fft_cpx t;
  43
  44   Fout2 = Fout + m;
  45   do {
  46     C_FIXDIV (*Fout, 2);
  47     C_FIXDIV (*Fout2, 2);
  48
  49     C_MUL (t, *Fout2, *tw1);
  50     tw1 += fstride;
  51     C_SUB (*Fout2, *Fout, t);
  52     C_ADDTO (*Fout, t);
  53     ++Fout2;
  54     ++Fout;
  55   } while (--m);
  56 }
  57
  58 static void
  59 kf_bfly4 (kiss_fft_cpx * Fout,
  60     const size_t fstride, const kiss_fft_cfg st, const size_t m)
  61 {
  62   kiss_fft_cpx *tw1, *tw2, *tw3;
  63   kiss_fft_cpx scratch[6];
  64   size_t k = m;
  65   const size_t m2 = 2 * m;
  66   const size_t m3 = 3 * m;
  67
  68   tw3 = tw2 = tw1 = st->twiddles;
  69
  70   do {
  71     C_FIXDIV (*Fout, 4);
  72     C_FIXDIV (Fout[m], 4);
  73     C_FIXDIV (Fout[m2], 4);
  74     C_FIXDIV (Fout[m3], 4);
  75
  76     C_MUL (scratch[0], Fout[m], *tw1);
  77     C_MUL (scratch[1], Fout[m2], *tw2);
  78     C_MUL (scratch[2], Fout[m3], *tw3);
  79
  80     C_SUB (scratch[5], *Fout, scratch[1]);
  81     C_ADDTO (*Fout, scratch[1]);
  82     C_ADD (scratch[3], scratch[0], scratch[2]);
  83     C_SUB (scratch[4], scratch[0], scratch[2]);
  84     C_SUB (Fout[m2], *Fout, scratch[3]);
  85     tw1 += fstride;
  86     tw2 += fstride * 2;
  87     tw3 += fstride * 3;
  88     C_ADDTO (*Fout, scratch[3]);
  89
  90     if (st->inverse) {
  91       Fout[m].r = scratch[5].r - scratch[4].i;
  92       Fout[m].i = scratch[5].i + scratch[4].r;
  93       Fout[m3].r = scratch[5].r + scratch[4].i;
  94       Fout[m3].i = scratch[5].i - scratch[4].r;
  95     } else {
  96       Fout[m].r = scratch[5].r + scratch[4].i;
  97       Fout[m].i = scratch[5].i - scratch[4].r;
  98       Fout[m3].r = scratch[5].r - scratch[4].i;
  99       Fout[m3].i = scratch[5].i + scratch[4].r;
 100     }
 101     ++Fout;
 102   } while (--k);
 103 }
 104
 105 static void
 106 kf_bfly3 (kiss_fft_cpx * Fout,
 107     const size_t fstride, const kiss_fft_cfg st, size_t m)
 108 {
 109   size_t k = m;
 110   const size_t m2 = 2 * m;
 111   kiss_fft_cpx *tw1, *tw2;
 112   kiss_fft_cpx scratch[5];
 113   kiss_fft_cpx epi3;
 114
 115   epi3 = st->twiddles[fstride * m];
 116
 117   tw1 = tw2 = st->twiddles;
 118
 119   do {
 120     C_FIXDIV (*Fout, 3);
 121     C_FIXDIV (Fout[m], 3);
 122     C_FIXDIV (Fout[m2], 3);
 123
 124     C_MUL (scratch[1], Fout[m], *tw1);
 125     C_MUL (scratch[2], Fout[m2], *tw2);
 126
 127     C_ADD (scratch[3], scratch[1], scratch[2]);
 128     C_SUB (scratch[0], scratch[1], scratch[2]);
 129     tw1 += fstride;
 130     tw2 += fstride * 2;
 131
 132     Fout[m].r = Fout->r - HALF_OF (scratch[3].r);
 133     Fout[m].i = Fout->i - HALF_OF (scratch[3].i);
 134
 135     C_MULBYSCALAR (scratch[0], epi3.i);
 136
 137     C_ADDTO (*Fout, scratch[3]);
 138
 139     Fout[m2].r = Fout[m].r + scratch[0].i;
 140     Fout[m2].i = Fout[m].i - scratch[0].r;
 141
 142     Fout[m].r -= scratch[0].i;
 143     Fout[m].i += scratch[0].r;
 144
 145     ++Fout;
 146   } while (--k);
 147 }
 148
 149 static void
 150 kf_bfly5 (kiss_fft_cpx * Fout,
 151     const size_t fstride, const kiss_fft_cfg st, int m)
 152 {
 153   kiss_fft_cpx *Fout0, *Fout1, *Fout2, *Fout3, *Fout4;
 154   int u;
 155   kiss_fft_cpx scratch[13];
 156   kiss_fft_cpx *twiddles = st->twiddles;
 157   kiss_fft_cpx *tw;
 158   kiss_fft_cpx ya, yb;
 159
 160   ya = twiddles[fstride * m];
 161   yb = twiddles[fstride * 2 * m];
 162
 163   Fout0 = Fout;
 164   Fout1 = Fout0 + m;
 165   Fout2 = Fout0 + 2 * m;
 166   Fout3 = Fout0 + 3 * m;
 167   Fout4 = Fout0 + 4 * m;
 168
 169   tw = st->twiddles;
 170   for (u = 0; u < m; ++u) {
 171     C_FIXDIV (*Fout0, 5);
 172     C_FIXDIV (*Fout1, 5);
 173     C_FIXDIV (*Fout2, 5);
 174     C_FIXDIV (*Fout3, 5);
 175     C_FIXDIV (*Fout4, 5);
 176     scratch[0] = *Fout0;
 177
 178     C_MUL (scratch[1], *Fout1, tw[u * fstride]);
 179     C_MUL (scratch[2], *Fout2, tw[2 * u * fstride]);
 180     C_MUL (scratch[3], *Fout3, tw[3 * u * fstride]);
 181     C_MUL (scratch[4], *Fout4, tw[4 * u * fstride]);
 182
 183     C_ADD (scratch[7], scratch[1], scratch[4]);
 184     C_SUB (scratch[10], scratch[1], scratch[4]);
 185     C_ADD (scratch[8], scratch[2], scratch[3]);
 186     C_SUB (scratch[9], scratch[2], scratch[3]);
 187
 188     Fout0->r += scratch[7].r + scratch[8].r;
 189     Fout0->i += scratch[7].i + scratch[8].i;
 190
 191     scratch[5].r =
 192         scratch[0].r + S_MUL (scratch[7].r, ya.r) + S_MUL (scratch[8].r, yb.r);
 193     scratch[5].i =
 194         scratch[0].i + S_MUL (scratch[7].i, ya.r) + S_MUL (scratch[8].i, yb.r);
 195
 196     scratch[6].r = S_MUL (scratch[10].i, ya.i) + S_MUL (scratch[9].i, yb.i);
 197     scratch[6].i = -S_MUL (scratch[10].r, ya.i) - S_MUL (scratch[9].r, yb.i);
 198
 199     C_SUB (*Fout1, scratch[5], scratch[6]);
 200     C_ADD (*Fout4, scratch[5], scratch[6]);
 201
 202     scratch[11].r =
 203         scratch[0].r + S_MUL (scratch[7].r, yb.r) + S_MUL (scratch[8].r, ya.r);
 204     scratch[11].i =
 205         scratch[0].i + S_MUL (scratch[7].i, yb.r) + S_MUL (scratch[8].i, ya.r);
 206     scratch[12].r = -S_MUL (scratch[10].i, yb.i) + S_MUL (scratch[9].i, ya.i);
 207     scratch[12].i = S_MUL (scratch[10].r, yb.i) - S_MUL (scratch[9].r, ya.i);
 208
 209     C_ADD (*Fout2, scratch[11], scratch[12]);
 210     C_SUB (*Fout3, scratch[11], scratch[12]);
 211
 212     ++Fout0;
 213     ++Fout1;
 214     ++Fout2;
 215     ++Fout3;
 216     ++Fout4;
 217   }
 218 }
 219
 220 /* perform the butterfly for one stage of a mixed radix FFT */
 221 static void
 222 kf_bfly_generic (kiss_fft_cpx * Fout,
 223     const size_t fstride, const kiss_fft_cfg st, int m, int p)
 224 {
 225   int u, k, q1, q;
 226   kiss_fft_cpx *twiddles = st->twiddles;
 227   kiss_fft_cpx t;
 228   int Norig = st->nfft;
 229
 230   CHECKBUF (scratchbuf, nscratchbuf, p);
 231
 232   for (u = 0; u < m; ++u) {
 233     k = u;
 234     for (q1 = 0; q1 < p; ++q1) {
 235       scratchbuf[q1] = Fout[k];
 236       C_FIXDIV (scratchbuf[q1], p);
 237       k += m;
 238     }
 239
 240     k = u;
 241     for (q1 = 0; q1 < p; ++q1) {
 242       int twidx = 0;
 243
 244       Fout[k] = scratchbuf[0];
 245       for (q = 1; q < p; ++q) {
 246         twidx += fstride * k;
 247         if (twidx >= Norig)
 248           twidx -= Norig;
 249         C_MUL (t, scratchbuf[q], twiddles[twidx]);
 250         C_ADDTO (Fout[k], t);
 251       }
 252       k += m;
 253     }
 254   }
 255 }
 256
 257 static void
 258 kf_work (kiss_fft_cpx * Fout,
 259     const kiss_fft_cpx * f,
 260     const size_t fstride, int in_stride, int *factors, const kiss_fft_cfg st)
 261 {
 262   kiss_fft_cpx *Fout_beg = Fout;
 263   const int p = *factors++;     /* the radix  */
 264   const int m = *factors++;     /* stage's fft length/p */
 265   const kiss_fft_cpx *Fout_end = Fout + p * m;
 266
 267   if (m == 1) {
 268     do {
 269       *Fout = *f;
 270       f += fstride * in_stride;
 271     } while (++Fout != Fout_end);
 272   } else {
 273     do {
 274       kf_work (Fout, f, fstride * p, in_stride, factors, st);
 275       f += fstride * in_stride;
 276     } while ((Fout += m) != Fout_end);
 277   }
 278
 279   Fout = Fout_beg;
 280
 281   switch (p) {
 282     case 2:
 283       kf_bfly2 (Fout, fstride, st, m);
 284       break;
 285     case 3:
 286       kf_bfly3 (Fout, fstride, st, m);
 287       break;
 288     case 4:
 289       kf_bfly4 (Fout, fstride, st, m);
 290       break;
 291     case 5:
 292       kf_bfly5 (Fout, fstride, st, m);
 293       break;
 294     default:
 295       kf_bfly_generic (Fout, fstride, st, m, p);
 296       break;
 297   }
 298 }
 299
 300 /*  facbuf is populated by p1,m1,p2,m2, ...
 301     where
 302     p[i] * m[i] = m[i-1]
 303     m0 = n                  */
 304 static void
 305 kf_factor (int n, int *facbuf)
 306 {
 307   int p = 4;
 308   double floor_sqrt;
 309
 310   floor_sqrt = floor (sqrt ((double) n));
 311
 312   /*factor out powers of 4, powers of 2, then any remaining primes */
 313   do {
 314     while (n % p) {
 315       switch (p) {
 316         case 4:
 317           p = 2;
 318           break;
 319         case 2:
 320           p = 3;
 321           break;
 322         default:
 323           p += 2;
 324           break;
 325       }
 326       if (p > floor_sqrt)
 327         p = n;                  /* no more factors, skip to end */
 328     }
 329     n /= p;
 330     *facbuf++ = p;
 331     *facbuf++ = n;
 332   } while (n > 1);
 333 }
 334
 335 /*
 336  *
 337  * User-callable function to allocate all necessary storage space for the fft.
 338  *
 339  * The return value is a contiguous block of memory, allocated with malloc.  As such,
 340  * It can be freed with free(), rather than a kiss_fft-specific function.
 341  * */
 342 kiss_fft_cfg
 343 kiss_fft_alloc (int nfft, int inverse_fft, void *mem, size_t * lenmem)
 344 {
 345   kiss_fft_cfg st = NULL;
 346   size_t memneeded = sizeof (struct kiss_fft_state)
 347       + sizeof (kiss_fft_cpx) * (nfft - 1);     /* twiddle factors */
 348
 349   if (lenmem == NULL) {
 350     st = (kiss_fft_cfg) KISS_FFT_MALLOC (memneeded);
 351   } else {
 352     if (mem != NULL && *lenmem >= memneeded)
 353       st = (kiss_fft_cfg) mem;
 354     *lenmem = memneeded;
 355   }
 356   if (st) {
 357     int i;
 358
 359     st->nfft = nfft;
 360     st->inverse = inverse_fft;
 361
 362     for (i = 0; i < nfft; ++i) {
 363       const double pi =
 364           3.141592653589793238462643383279502884197169399375105820974944;
 365       double phase = -2 * pi * i / nfft;
 366
 367       if (st->inverse)
 368         phase *= -1;
 369       kf_cexp (st->twiddles + i, phase);
 370     }
 371
 372     kf_factor (nfft, st->factors);
 373   }
 374   return st;
 375 }
 376
 377
 378
 379
 380 void
 381 kiss_fft_stride (kiss_fft_cfg st, const kiss_fft_cpx * fin, kiss_fft_cpx * fout,
 382     int in_stride)
 383 {
 384   if (fin == fout) {
 385     CHECKBUF (tmpbuf, ntmpbuf, st->nfft);
 386     kf_work (tmpbuf, fin, 1, in_stride, st->factors, st);
 387     memcpy (fout, tmpbuf, sizeof (kiss_fft_cpx) * st->nfft);
 388   } else {
 389     kf_work (fout, fin, 1, in_stride, st->factors, st);
 390   }
 391 }
 392
 393 void
 394 kiss_fft (kiss_fft_cfg cfg, const kiss_fft_cpx * fin, kiss_fft_cpx * fout)
 395 {
 396   kiss_fft_stride (cfg, fin, fout, 1);
 397 }
 398
 399
 400 /* not really necessary to call, but if someone is doing in-place ffts, they may want to free the
 401    buffers from CHECKBUF
 402  */
 403 void
 404 kiss_fft_cleanup (void)
 405 {
 406   free (scratchbuf);
 407   scratchbuf = NULL;
 408   nscratchbuf = 0;
 409   free (tmpbuf);
 410   tmpbuf = NULL;
 411   ntmpbuf = 0;
 412 }
 413
 414 int
 415 kiss_fft_next_fast_size (int n)
 416 {
 417   while (1) {
 418     int m = n;
 419
 420     while ((m % 2) == 0)
 421       m /= 2;
 422     while ((m % 3) == 0)
 423       m /= 3;
 424     while ((m % 5) == 0)
 425       m /= 5;
 426     if (m <= 1)
 427       break;                    /* n is completely factorable by twos, threes, and fives */
 428     n++;
 429   }
 430   return n;
 431 }