gst-libs/gst/fft/kiss_fft_s16.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_s16.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_s16_cpx *scratchbuf = NULL;
  22 static size_t nscratchbuf = 0;
  23 static kiss_fft_s16_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_s16_cpx*)KISS_FFT_S16_MALLOC(sizeof(kiss_fft_s16_cpx)*(n)); \
  31             nbuf = (size_t)(n); \
  32         } \
  33    }while(0)
  34
  35
  36 static void
  37 kf_bfly2 (kiss_fft_s16_cpx * Fout,
  38     const size_t fstride, const kiss_fft_s16_cfg st, int m)
  39 {
  40   kiss_fft_s16_cpx *Fout2;
  41   kiss_fft_s16_cpx *tw1 = st->twiddles;
  42   kiss_fft_s16_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_s16_cpx * Fout,
  60     const size_t fstride, const kiss_fft_s16_cfg st, const size_t m)
  61 {
  62   kiss_fft_s16_cpx *tw1, *tw2, *tw3;
  63   kiss_fft_s16_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_s16_cpx * Fout,
 107     const size_t fstride, const kiss_fft_s16_cfg st, size_t m)
 108 {
 109   size_t k = m;
 110   const size_t m2 = 2 * m;
 111   kiss_fft_s16_cpx *tw1, *tw2;
 112   kiss_fft_s16_cpx scratch[5];
 113   kiss_fft_s16_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_s16_cpx * Fout,
 151     const size_t fstride, const kiss_fft_s16_cfg st, int m)
 152 {
 153   kiss_fft_s16_cpx *Fout0, *Fout1, *Fout2, *Fout3, *Fout4;
 154   int u;
 155   kiss_fft_s16_cpx scratch[13];
 156   kiss_fft_s16_cpx *twiddles = st->twiddles;
 157   kiss_fft_s16_cpx *tw;
 158   kiss_fft_s16_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_s16_cpx * Fout,
 223     const size_t fstride, const kiss_fft_s16_cfg st, int m, int p)
 224 {
 225   int u, k, q1, q;
 226   kiss_fft_s16_cpx *twiddles = st->twiddles;
 227   kiss_fft_s16_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_s16_cpx * Fout,
 259     const kiss_fft_s16_cpx * f,
 260     const size_t fstride,
 261     int in_stride, int *factors, const kiss_fft_s16_cfg st)
 262 {
 263   kiss_fft_s16_cpx *Fout_beg = Fout;
 264   const int p = *factors++;     /* the radix  */
 265   const int m = *factors++;     /* stage's fft length/p */
 266   const kiss_fft_s16_cpx *Fout_end = Fout + p * m;
 267
 268   if (m == 1) {
 269     do {
 270       *Fout = *f;
 271       f += fstride * in_stride;
 272     } while (++Fout != Fout_end);
 273   } else {
 274     do {
 275       kf_work (Fout, f, fstride * p, in_stride, factors, st);
 276       f += fstride * in_stride;
 277     } while ((Fout += m) != Fout_end);
 278   }
 279
 280   Fout = Fout_beg;
 281
 282   switch (p) {
 283     case 2:
 284       kf_bfly2 (Fout, fstride, st, m);
 285       break;
 286     case 3:
 287       kf_bfly3 (Fout, fstride, st, m);
 288       break;
 289     case 4:
 290       kf_bfly4 (Fout, fstride, st, m);
 291       break;
 292     case 5:
 293       kf_bfly5 (Fout, fstride, st, m);
 294       break;
 295     default:
 296       kf_bfly_generic (Fout, fstride, st, m, p);
 297       break;
 298   }
 299 }
 300
 301 /*  facbuf is populated by p1,m1,p2,m2, ...
 302     where
 303     p[i] * m[i] = m[i-1]
 304     m0 = n                  */
 305 static void
 306 kf_factor (int n, int *facbuf)
 307 {
 308   int p = 4;
 309   double floor_sqrt;
 310
 311   floor_sqrt = floor (sqrt ((double) n));
 312
 313   /*factor out powers of 4, powers of 2, then any remaining primes */
 314   do {
 315     while (n % p) {
 316       switch (p) {
 317         case 4:
 318           p = 2;
 319           break;
 320         case 2:
 321           p = 3;
 322           break;
 323         default:
 324           p += 2;
 325           break;
 326       }
 327       if (p > floor_sqrt)
 328         p = n;                  /* no more factors, skip to end */
 329     }
 330     n /= p;
 331     *facbuf++ = p;
 332     *facbuf++ = n;
 333   } while (n > 1);
 334 }
 335
 336 /*
 337  *
 338  * User-callable function to allocate all necessary storage space for the fft.
 339  *
 340  * The return value is a contiguous block of memory, allocated with malloc.  As such,
 341  * It can be freed with free(), rather than a kiss_fft-specific function.
 342  * */
 343 kiss_fft_s16_cfg
 344 kiss_fft_s16_alloc (int nfft, int inverse_fft, void *mem, size_t * lenmem)
 345 {
 346   kiss_fft_s16_cfg st = NULL;
 347   size_t memneeded = sizeof (struct kiss_fft_s16_state)
 348       + sizeof (kiss_fft_s16_cpx) * (nfft - 1); /* twiddle factors */
 349
 350   if (lenmem == NULL) {
 351     st = (kiss_fft_s16_cfg) KISS_FFT_S16_MALLOC (memneeded);
 352   } else {
 353     if (mem != NULL && *lenmem >= memneeded)
 354       st = (kiss_fft_s16_cfg) mem;
 355     *lenmem = memneeded;
 356   }
 357   if (st) {
 358     int i;
 359
 360     st->nfft = nfft;
 361     st->inverse = inverse_fft;
 362
 363     for (i = 0; i < nfft; ++i) {
 364       const double pi =
 365           3.141592653589793238462643383279502884197169399375105820974944;
 366       double phase = -2 * pi * i / nfft;
 367
 368       if (st->inverse)
 369         phase *= -1;
 370       kf_cexp (st->twiddles + i, phase);
 371     }
 372
 373     kf_factor (nfft, st->factors);
 374   }
 375   return st;
 376 }
 377
 378
 379
 380
 381 void
 382 kiss_fft_s16_stride (kiss_fft_s16_cfg st, const kiss_fft_s16_cpx * fin,
 383     kiss_fft_s16_cpx * fout, int in_stride)
 384 {
 385   if (fin == fout) {
 386     CHECKBUF (tmpbuf, ntmpbuf, st->nfft);
 387     kf_work (tmpbuf, fin, 1, in_stride, st->factors, st);
 388     memcpy (fout, tmpbuf, sizeof (kiss_fft_s16_cpx) * st->nfft);
 389   } else {
 390     kf_work (fout, fin, 1, in_stride, st->factors, st);
 391   }
 392 }
 393
 394 void
 395 kiss_fft_s16 (kiss_fft_s16_cfg cfg, const kiss_fft_s16_cpx * fin,
 396     kiss_fft_s16_cpx * fout)
 397 {
 398   kiss_fft_s16_stride (cfg, fin, fout, 1);
 399 }
 400
 401
 402 /* not really necessary to call, but if someone is doing in-place ffts, they may want to free the
 403    buffers from CHECKBUF
 404  */
 405 void
 406 kiss_fft_s16_cleanup (void)
 407 {
 408   free (scratchbuf);
 409   scratchbuf = NULL;
 410   nscratchbuf = 0;
 411   free (tmpbuf);
 412   tmpbuf = NULL;
 413   ntmpbuf = 0;
 414 }
 415
 416 int
 417 kiss_fft_s16_next_fast_size (int n)
 418 {
 419   while (1) {
 420     int m = n;
 421
 422     while ((m % 2) == 0)
 423       m /= 2;
 424     while ((m % 3) == 0)
 425       m /= 3;
 426     while ((m % 5) == 0)
 427       m /= 5;
 428     if (m <= 1)
 429       break;                    /* n is completely factorable by twos, threes, and fives */
 430     n++;
 431   }
 432   return n;
 433 }