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