apps/plugins/fft/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 SHAREDBSS_ATTR = NULL;
  22 static size_t nscratchbuf SHAREDBSS_ATTR = 0;
  23 static kiss_fft_cpx *tmpbuf SHAREDBSS_ATTR = NULL;
  24 static size_t ntmpbuf SHAREDBSS_ATTR = 0;
  25
  26 #define CHECKBUF(buf,nbuf,n) \
  27     do { \
  28         if ( nbuf < (size_t)(n) ) {\
  29                 DEBUGF("CHECKBUF NOT IMPLEMENTED!");\
  30                 break;\
  31         } \
  32    }while(0)
  33
  34
  35 static void kf_bfly2(
  36         kiss_fft_cpx * Fout,
  37         const size_t fstride,
  38         const kiss_fft_cfg st,
  39         int m
  40         )
  41 {
  42     kiss_fft_cpx * Fout2;
  43     kiss_fft_cpx * tw1 = st->twiddles;
  44     kiss_fft_cpx t;
  45     Fout2 = Fout + m;
  46     do{
  47         C_FIXDIV(*Fout,2); 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 kf_bfly4(
  59         kiss_fft_cpx * Fout,
  60         const size_t fstride,
  61         const kiss_fft_cfg st,
  62         const size_t m
  63         )
  64 {
  65     kiss_fft_cpx *tw1,*tw2,*tw3;
  66     kiss_fft_cpx scratch[6];
  67     size_t k=m;
  68     const size_t m2=2*m;
  69     const size_t m3=3*m;
  70
  71     tw3 = tw2 = tw1 = st->twiddles;
  72
  73     do {
  74         C_FIXDIV(*Fout,4); C_FIXDIV(Fout[m],4); C_FIXDIV(Fout[m2],4); 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 kf_bfly3(
 106          kiss_fft_cpx * Fout,
 107          const size_t fstride,
 108          const kiss_fft_cfg st,
 109          size_t m
 110          )
 111 {
 112      size_t k=m;
 113      const size_t m2 = 2*m;
 114      kiss_fft_cpx *tw1,*tw2;
 115      kiss_fft_cpx scratch[5];
 116      kiss_fft_cpx epi3;
 117      epi3 = st->twiddles[fstride*m];
 118
 119      tw1=tw2=st->twiddles;
 120
 121      do{
 122          C_FIXDIV(*Fout,3); C_FIXDIV(Fout[m],3); 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 kf_bfly5(
 150         kiss_fft_cpx * Fout,
 151         const size_t fstride,
 152         const kiss_fft_cfg st,
 153         int m
 154         )
 155 {
 156     kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
 157     int u;
 158     kiss_fft_cpx scratch[13];
 159     kiss_fft_cpx * twiddles = st->twiddles;
 160     kiss_fft_cpx *tw;
 161     kiss_fft_cpx ya,yb;
 162     ya = twiddles[fstride*m];
 163     yb = twiddles[fstride*2*m];
 164
 165     Fout0=Fout;
 166     Fout1=Fout0+m;
 167     Fout2=Fout0+2*m;
 168     Fout3=Fout0+3*m;
 169     Fout4=Fout0+4*m;
 170
 171     tw=st->twiddles;
 172     for ( u=0; u<m; ++u ) {
 173         C_FIXDIV( *Fout0,5); C_FIXDIV( *Fout1,5); C_FIXDIV( *Fout2,5); C_FIXDIV( *Fout3,5); C_FIXDIV( *Fout4,5);
 174         scratch[0] = *Fout0;
 175
 176         C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);
 177         C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);
 178         C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);
 179         C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);
 180
 181         C_ADD( scratch[7],scratch[1],scratch[4]);
 182         C_SUB( scratch[10],scratch[1],scratch[4]);
 183         C_ADD( scratch[8],scratch[2],scratch[3]);
 184         C_SUB( scratch[9],scratch[2],scratch[3]);
 185
 186         Fout0->r += scratch[7].r + scratch[8].r;
 187         Fout0->i += scratch[7].i + scratch[8].i;
 188
 189         scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
 190         scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
 191
 192         scratch[6].r =  S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);
 193         scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);
 194
 195         C_SUB(*Fout1,scratch[5],scratch[6]);
 196         C_ADD(*Fout4,scratch[5],scratch[6]);
 197
 198         scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
 199         scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
 200         scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
 201         scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);
 202
 203         C_ADD(*Fout2,scratch[11],scratch[12]);
 204         C_SUB(*Fout3,scratch[11],scratch[12]);
 205
 206         ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
 207     }
 208 }
 209
 210 /* perform the butterfly for one stage of a mixed radix FFT */
 211 static void kf_bfly_generic(
 212         kiss_fft_cpx * Fout,
 213         const size_t fstride,
 214         const kiss_fft_cfg st,
 215         int m,
 216         int p
 217         )
 218 {
 219     int u,k,q1,q;
 220     kiss_fft_cpx * twiddles = st->twiddles;
 221     kiss_fft_cpx t;
 222     int Norig = st->nfft;
 223
 224     CHECKBUF(scratchbuf,nscratchbuf,p);
 225
 226     for ( u=0; u<m; ++u ) {
 227         k=u;
 228         for ( q1=0 ; q1<p ; ++q1 ) {
 229             scratchbuf[q1] = Fout[ k  ];
 230             C_FIXDIV(scratchbuf[q1],p);
 231             k += m;
 232         }
 233
 234         k=u;
 235         for ( q1=0 ; q1<p ; ++q1 ) {
 236             int twidx=0;
 237             Fout[ k ] = scratchbuf[0];
 238             for (q=1;q<p;++q ) {
 239                 twidx += fstride * k;
 240                 if (twidx>=Norig) twidx-=Norig;
 241                 C_MUL(t,scratchbuf[q] , twiddles[twidx] );
 242                 C_ADDTO( Fout[ k ] ,t);
 243             }
 244             k += m;
 245         }
 246     }
 247 }
 248
 249 static
 250 void kf_work(
 251         kiss_fft_cpx * Fout,
 252         const kiss_fft_cpx * f,
 253         const size_t fstride,
 254         int in_stride,
 255         int * factors,
 256         const kiss_fft_cfg st
 257         )
 258 {
 259     kiss_fft_cpx * Fout_beg=Fout;
 260     const int p=*factors++; /* the radix  */
 261     const int m=*factors++; /* stage's fft length/p */
 262     const kiss_fft_cpx * Fout_end = Fout + p*m;
 263
 264 #ifdef _OPENMP
 265     // use openmp extensions at the
 266     // top-level (not recursive)
 267     if (fstride==1) {
 268         int k;
 269
 270         // execute the p different work units in different threads
 271 #       pragma omp parallel for
 272         for (k=0;k<p;++k)
 273             kf_work( Fout +k*m, f+ fstride*in_stride*k,fstride*p,in_stride,factors,st);
 274         // all threads have joined by this point
 275
 276         switch (p) {
 277             case 2: kf_bfly2(Fout,fstride,st,m); break;
 278             case 3: kf_bfly3(Fout,fstride,st,m); break;
 279             case 4: kf_bfly4(Fout,fstride,st,m); break;
 280             case 5: kf_bfly5(Fout,fstride,st,m); break;
 281             default: kf_bfly_generic(Fout,fstride,st,m,p); break;
 282         }
 283         return;
 284     }
 285 #endif
 286
 287     if (m==1) {
 288         do{
 289             *Fout = *f;
 290             f += fstride*in_stride;
 291         }while(++Fout != Fout_end );
 292     }else{
 293         do{
 294             // recursive call:
 295             // DFT of size m*p performed by doing
 296             // p instances of smaller DFTs of size m,
 297             // each one takes a decimated version of the input
 298             kf_work( Fout , f, fstride*p, in_stride, factors,st);
 299             f += fstride*in_stride;
 300         }while( (Fout += m) != Fout_end );
 301     }
 302
 303     Fout=Fout_beg;
 304
 305     // recombine the p smaller DFTs
 306     switch (p) {
 307         case 2: kf_bfly2(Fout,fstride,st,m); break;
 308         case 3: kf_bfly3(Fout,fstride,st,m); break;
 309         case 4: kf_bfly4(Fout,fstride,st,m); break;
 310         case 5: kf_bfly5(Fout,fstride,st,m); break;
 311         default: kf_bfly_generic(Fout,fstride,st,m,p); break;
 312     }
 313 }
 314
 315 /*  facbuf is populated by p1,m1,p2,m2, ...
 316     where
 317     p[i] * m[i] = m[i-1]
 318     m0 = n                  */
 319 static
 320 void kf_factor(int n,int * facbuf)
 321 {
 322     int p=4;
 323     int32_t floor_sqrt = fp_sqrt(n, 15) >> 15;
 324
 325     /*factor out powers of 4, powers of 2, then any remaining primes */
 326     do {
 327         while (n % p) {
 328             switch (p) {
 329                 case 4: p = 2; break;
 330                 case 2: p = 3; break;
 331                 default: p += 2; break;
 332             }
 333             if (p > floor_sqrt)
 334                 p = n;          /* no more factors, skip to end */
 335         }
 336         n /= p;
 337         *facbuf++ = p;
 338         *facbuf++ = n;
 339     } while (n > 1);
 340 }
 341
 342 /*
 343  *
 344  * User-callable function to allocate all necessary storage space for the fft.
 345  *
 346  * The return value is a contiguous block of memory, allocated with malloc.  As such,
 347  * It can be freed with free(), rather than a kiss_fft-specific function.
 348  * */
 349 kiss_fft_cfg kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem )
 350 {
 351     kiss_fft_cfg st=NULL;
 352     size_t memneeded = sizeof(struct kiss_fft_state)
 353         + sizeof(kiss_fft_cpx)*(nfft-1); /* twiddle factors*/
 354
 355     if ( lenmem==NULL ) {
 356         DEBUGF("This version of kiss fft can't use malloc");
 357         return st;
 358         /* st = ( kiss_fft_cfg)KISS_FFT_MALLOC( memneeded ); */
 359     }else{
 360         if (mem != NULL && *lenmem >= memneeded)
 361             st = (kiss_fft_cfg)mem;
 362         *lenmem = memneeded;
 363     }
 364     if (st) {
 365                 int i;
 366                 st->nfft=nfft;
 367                 st->inverse = inverse_fft;
 368
 369                 for (i=0;i<nfft;++i) {
 370                         /* const double pi=3.141592653589793238462643383279502884197169399375105820974944;
 371                         double phase = -2*pi*i / nfft; */
 372                         if (st->inverse)
 373                                 DEBUGF("Inverse FFT not implemented!"); /* kf_cexp(st->twiddles+i, -1*i, nfft ); */
 374                         else
 375                                 kf_cexp( st->twiddles+i, i, nfft );
 376                 }
 377
 378                 kf_factor(nfft,st->factors);
 379         }
 380     return st;
 381 }
 382
 383
 384
 385
 386 void kiss_fft_stride(kiss_fft_cfg st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int in_stride)
 387 {
 388     if (fin == fout) {
 389         CHECKBUF(tmpbuf,ntmpbuf,st->nfft);
 390         kf_work(tmpbuf,fin,1,in_stride, st->factors,st);
 391         memcpy(fout,tmpbuf,sizeof(kiss_fft_cpx)*st->nfft);
 392     }else{
 393         kf_work( fout, fin, 1,in_stride, st->factors,st );
 394     }
 395 }
 396
 397 void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
 398 {
 399     kiss_fft_stride(cfg,fin,fout,1);
 400 }
 401
 402
 403 /* not really necessary to call, but if someone is doing in-place ffts, they may want to free the
 404    buffers from CHECKBUF
 405  */
 406 void kiss_fft_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 kiss_fft_next_fast_size(int n)
 417 {
 418     while(1) {
 419         int m=n;
 420         while ( (m%2) == 0 ) m/=2;
 421         while ( (m%3) == 0 ) m/=3;
 422         while ( (m%5) == 0 ) m/=5;
 423         if (m<=1)
 424             break; /* n is completely factorable by twos, threes, and fives */
 425         n++;
 426     }
 427     return n;
 428 }