Debian Codegraph @

common/kiss_fft/kiss_fft.c

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