Made pre-computed twiddles the same for forward and inverse FFT
[opus.git] / libcelt / kiss_fft.c
1 /*
2 Copyright (c) 2003-2004, Mark Borgerding
3 Copyright (c) 2005-2007, Jean-Marc Valin
4
5 All rights reserved.
6
7 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
8
9     * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
10     * 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.
11     * 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.
12
13 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.
14 */
15
16
17 #ifdef HAVE_CONFIG_H
18 #include "config.h"
19 #endif
20
21 #include "_kiss_fft_guts.h"
22 #include "arch.h"
23 #include "os_support.h"
24
25 /* The guts header contains all the multiplication and addition macros that are defined for
26  fixed or floating point complex numbers.  It also delares the kf_ internal functions.
27  */
28
29 static void kf_bfly2(
30         kiss_fft_cpx * Fout,
31         const size_t fstride,
32         const kiss_fft_cfg st,
33         int m,
34         int N,
35         int mm
36         )
37 {
38     kiss_fft_cpx * Fout2;
39     kiss_fft_cpx * tw1;
40     kiss_fft_cpx t;
41     if (!st->inverse) {
42        int i,j;
43        kiss_fft_cpx * Fout_beg = Fout;
44        for (i=0;i<N;i++)
45        {
46           Fout = Fout_beg + i*mm;
47           Fout2 = Fout + m;
48           tw1 = st->twiddles;
49           for(j=0;j<m;j++)
50           {
51              /* Almost the same as the code path below, except that we divide the input by two
52               (while keeping the best accuracy possible) */
53              celt_word32_t tr, ti;
54              tr = SHR32(SUB32(MULT16_16(Fout2->r , tw1->r),MULT16_16(Fout2->i , tw1->i)), 1);
55              ti = SHR32(ADD32(MULT16_16(Fout2->i , tw1->r),MULT16_16(Fout2->r , tw1->i)), 1);
56              tw1 += fstride;
57              Fout2->r = PSHR32(SUB32(SHL32(EXTEND32(Fout->r), 14), tr), 15);
58              Fout2->i = PSHR32(SUB32(SHL32(EXTEND32(Fout->i), 14), ti), 15);
59              Fout->r = PSHR32(ADD32(SHL32(EXTEND32(Fout->r), 14), tr), 15);
60              Fout->i = PSHR32(ADD32(SHL32(EXTEND32(Fout->i), 14), ti), 15);
61              ++Fout2;
62              ++Fout;
63           }
64        }
65     } else {
66        int i,j;
67        kiss_fft_cpx * Fout_beg = Fout;
68        for (i=0;i<N;i++)
69        {
70           Fout = Fout_beg + i*mm;
71           Fout2 = Fout + m;
72           tw1 = st->twiddles;
73           for(j=0;j<m;j++)
74           {
75              C_MULC (t,  *Fout2 , *tw1);
76              tw1 += fstride;
77              C_SUB( *Fout2 ,  *Fout , t );
78              C_ADDTO( *Fout ,  t );
79              ++Fout2;
80              ++Fout;
81           }
82        }
83     }
84 }
85
86 static void kf_bfly4(
87         kiss_fft_cpx * Fout,
88         const size_t fstride,
89         const kiss_fft_cfg st,
90         int m,
91         int N,
92         int mm
93         )
94 {
95     kiss_fft_cpx *tw1,*tw2,*tw3;
96     kiss_fft_cpx scratch[6];
97     const size_t m2=2*m;
98     const size_t m3=3*m;
99     int i, j;
100
101     if (st->inverse)
102     {
103        kiss_fft_cpx * Fout_beg = Fout;
104        for (i=0;i<N;i++)
105        {
106           Fout = Fout_beg + i*mm;
107           tw3 = tw2 = tw1 = st->twiddles;
108           for (j=0;j<m;j++)
109           {
110              C_MULC(scratch[0],Fout[m] , *tw1 );
111              C_MULC(scratch[1],Fout[m2] , *tw2 );
112              C_MULC(scratch[2],Fout[m3] , *tw3 );
113              
114              C_SUB( scratch[5] , *Fout, scratch[1] );
115              C_ADDTO(*Fout, scratch[1]);
116              C_ADD( scratch[3] , scratch[0] , scratch[2] );
117              C_SUB( scratch[4] , scratch[0] , scratch[2] );
118              C_SUB( Fout[m2], *Fout, scratch[3] );
119              tw1 += fstride;
120              tw2 += fstride*2;
121              tw3 += fstride*3;
122              C_ADDTO( *Fout , scratch[3] );
123              
124              Fout[m].r = scratch[5].r - scratch[4].i;
125              Fout[m].i = scratch[5].i + scratch[4].r;
126              Fout[m3].r = scratch[5].r + scratch[4].i;
127              Fout[m3].i = scratch[5].i - scratch[4].r;
128              ++Fout;
129           }
130        }
131     } else
132     {
133        kiss_fft_cpx * Fout_beg = Fout;
134        for (i=0;i<N;i++)
135        {
136           Fout = Fout_beg + i*mm;
137           tw3 = tw2 = tw1 = st->twiddles;
138           for (j=0;j<m;j++)
139           {
140              C_MUL4(scratch[0],Fout[m] , *tw1 );
141              C_MUL4(scratch[1],Fout[m2] , *tw2 );
142              C_MUL4(scratch[2],Fout[m3] , *tw3 );
143              
144              Fout->r = PSHR16(Fout->r, 2);
145              Fout->i = PSHR16(Fout->i, 2);
146              C_SUB( scratch[5] , *Fout, scratch[1] );
147              C_ADDTO(*Fout, scratch[1]);
148              C_ADD( scratch[3] , scratch[0] , scratch[2] );
149              C_SUB( scratch[4] , scratch[0] , scratch[2] );
150              Fout[m2].r = PSHR16(Fout[m2].r, 2);
151              Fout[m2].i = PSHR16(Fout[m2].i, 2);
152              C_SUB( Fout[m2], *Fout, scratch[3] );
153              tw1 += fstride;
154              tw2 += fstride*2;
155              tw3 += fstride*3;
156              C_ADDTO( *Fout , scratch[3] );
157              
158              Fout[m].r = scratch[5].r + scratch[4].i;
159              Fout[m].i = scratch[5].i - scratch[4].r;
160              Fout[m3].r = scratch[5].r - scratch[4].i;
161              Fout[m3].i = scratch[5].i + scratch[4].r;
162              ++Fout;
163           }
164        }
165     }
166 }
167
168 static void kf_bfly3(
169          kiss_fft_cpx * Fout,
170          const size_t fstride,
171          const kiss_fft_cfg st,
172          size_t m
173          )
174 {
175      size_t k=m;
176      const size_t m2 = 2*m;
177      kiss_fft_cpx *tw1,*tw2;
178      kiss_fft_cpx scratch[5];
179      kiss_fft_cpx epi3;
180      epi3 = st->twiddles[fstride*m];
181
182      tw1=tw2=st->twiddles;
183      if (st->inverse) {
184         do{
185            if (!st->inverse) {
186               C_FIXDIV(*Fout,3); C_FIXDIV(Fout[m],3); C_FIXDIV(Fout[m2],3);
187            }
188
189            C_MULC(scratch[1],Fout[m] , *tw1);
190            C_MULC(scratch[2],Fout[m2] , *tw2);
191
192            C_ADD(scratch[3],scratch[1],scratch[2]);
193            C_SUB(scratch[0],scratch[1],scratch[2]);
194            tw1 += fstride;
195            tw2 += fstride*2;
196
197            Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
198            Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
199
200            C_MULBYSCALAR( scratch[0] , -epi3.i );
201
202            C_ADDTO(*Fout,scratch[3]);
203
204            Fout[m2].r = Fout[m].r + scratch[0].i;
205            Fout[m2].i = Fout[m].i - scratch[0].r;
206
207            Fout[m].r -= scratch[0].i;
208            Fout[m].i += scratch[0].r;
209
210            ++Fout;
211         }while(--k);
212      } else {
213         do{
214            if (!st->inverse) {
215               C_FIXDIV(*Fout,3); C_FIXDIV(Fout[m],3); C_FIXDIV(Fout[m2],3);
216            }
217
218            C_MUL(scratch[1],Fout[m] , *tw1);
219            C_MUL(scratch[2],Fout[m2] , *tw2);
220
221            C_ADD(scratch[3],scratch[1],scratch[2]);
222            C_SUB(scratch[0],scratch[1],scratch[2]);
223            tw1 += fstride;
224            tw2 += fstride*2;
225
226            Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
227            Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
228
229            C_MULBYSCALAR( scratch[0] , epi3.i );
230
231            C_ADDTO(*Fout,scratch[3]);
232
233            Fout[m2].r = Fout[m].r + scratch[0].i;
234            Fout[m2].i = Fout[m].i - scratch[0].r;
235
236            Fout[m].r -= scratch[0].i;
237            Fout[m].i += scratch[0].r;
238
239            ++Fout;
240         }while(--k);
241      }
242 }
243
244 static void kf_bfly5(
245         kiss_fft_cpx * Fout,
246         const size_t fstride,
247         const kiss_fft_cfg st,
248         int m
249         )
250 {
251     kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
252     int u;
253     kiss_fft_cpx scratch[13];
254     kiss_fft_cpx * twiddles = st->twiddles;
255     kiss_fft_cpx *tw;
256     kiss_fft_cpx ya,yb;
257     ya = twiddles[fstride*m];
258     yb = twiddles[fstride*2*m];
259
260     Fout0=Fout;
261     Fout1=Fout0+m;
262     Fout2=Fout0+2*m;
263     Fout3=Fout0+3*m;
264     Fout4=Fout0+4*m;
265
266     tw=st->twiddles;
267     if (st->inverse) {
268
269        for ( u=0; u<m; ++u ) {
270           if (!st->inverse) {
271              C_FIXDIV( *Fout0,5); C_FIXDIV( *Fout1,5); C_FIXDIV( *Fout2,5); C_FIXDIV( *Fout3,5); C_FIXDIV( *Fout4,5);
272           }
273           scratch[0] = *Fout0;
274
275           C_MULC(scratch[1] ,*Fout1, tw[u*fstride]);
276           C_MULC(scratch[2] ,*Fout2, tw[2*u*fstride]);
277           C_MULC(scratch[3] ,*Fout3, tw[3*u*fstride]);
278           C_MULC(scratch[4] ,*Fout4, tw[4*u*fstride]);
279
280           C_ADD( scratch[7],scratch[1],scratch[4]);
281           C_SUB( scratch[10],scratch[1],scratch[4]);
282           C_ADD( scratch[8],scratch[2],scratch[3]);
283           C_SUB( scratch[9],scratch[2],scratch[3]);
284
285           Fout0->r += scratch[7].r + scratch[8].r;
286           Fout0->i += scratch[7].i + scratch[8].i;
287
288           scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
289           scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
290
291           scratch[6].r = -S_MUL(scratch[10].i,ya.i) - S_MUL(scratch[9].i,yb.i);
292           scratch[6].i =  S_MUL(scratch[10].r,ya.i) + S_MUL(scratch[9].r,yb.i);
293
294           C_SUB(*Fout1,scratch[5],scratch[6]);
295           C_ADD(*Fout4,scratch[5],scratch[6]);
296
297           scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
298           scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
299           scratch[12].r =  S_MUL(scratch[10].i,yb.i) - S_MUL(scratch[9].i,ya.i);
300           scratch[12].i = -S_MUL(scratch[10].r,yb.i) + S_MUL(scratch[9].r,ya.i);
301
302           C_ADD(*Fout2,scratch[11],scratch[12]);
303           C_SUB(*Fout3,scratch[11],scratch[12]);
304
305           ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
306        }
307     } else {
308        for ( u=0; u<m; ++u ) {
309           if (!st->inverse) {
310              C_FIXDIV( *Fout0,5); C_FIXDIV( *Fout1,5); C_FIXDIV( *Fout2,5); C_FIXDIV( *Fout3,5); C_FIXDIV( *Fout4,5);
311           }
312           scratch[0] = *Fout0;
313
314           C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);
315           C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);
316           C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);
317           C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);
318
319           C_ADD( scratch[7],scratch[1],scratch[4]);
320           C_SUB( scratch[10],scratch[1],scratch[4]);
321           C_ADD( scratch[8],scratch[2],scratch[3]);
322           C_SUB( scratch[9],scratch[2],scratch[3]);
323
324           Fout0->r += scratch[7].r + scratch[8].r;
325           Fout0->i += scratch[7].i + scratch[8].i;
326
327           scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
328           scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
329
330           scratch[6].r =  S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);
331           scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);
332
333           C_SUB(*Fout1,scratch[5],scratch[6]);
334           C_ADD(*Fout4,scratch[5],scratch[6]);
335
336           scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
337           scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
338           scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
339           scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);
340
341           C_ADD(*Fout2,scratch[11],scratch[12]);
342           C_SUB(*Fout3,scratch[11],scratch[12]);
343
344           ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
345        }
346     }
347 }
348
349 /* perform the butterfly for one stage of a mixed radix FFT */
350 static void kf_bfly_generic(
351         kiss_fft_cpx * Fout,
352         const size_t fstride,
353         const kiss_fft_cfg st,
354         int m,
355         int p
356         )
357 {
358     int u,k,q1,q;
359     kiss_fft_cpx * twiddles = st->twiddles;
360     kiss_fft_cpx t;
361     kiss_fft_cpx scratchbuf[17];
362     int Norig = st->nfft;
363
364     /*CHECKBUF(scratchbuf,nscratchbuf,p);*/
365     if (p>17)
366        celt_fatal("KissFFT: max radix supported is 17");
367     
368     for ( u=0; u<m; ++u ) {
369         k=u;
370         for ( q1=0 ; q1<p ; ++q1 ) {
371             scratchbuf[q1] = Fout[ k  ];
372         if (!st->inverse) {
373             C_FIXDIV(scratchbuf[q1],p);
374         }
375             k += m;
376         }
377
378         k=u;
379         for ( q1=0 ; q1<p ; ++q1 ) {
380             int twidx=0;
381             Fout[ k ] = scratchbuf[0];
382             for (q=1;q<p;++q ) {
383                 twidx += fstride * k;
384                 if (twidx>=Norig) twidx-=Norig;
385                 if (st->inverse)
386                    C_MULC(t,scratchbuf[q] , twiddles[twidx] );
387                 else
388                    C_MUL(t,scratchbuf[q] , twiddles[twidx] );
389                 C_ADDTO( Fout[ k ] ,t);
390             }
391             k += m;
392         }
393     }
394 }
395
396 static
397 void compute_bitrev_table(
398          int * Fout,
399          int f,
400          const size_t fstride,
401          int in_stride,
402          int * factors,
403          const kiss_fft_cfg st
404             )
405 {
406    const int p=*factors++; /* the radix  */
407    const int m=*factors++; /* stage's fft length/p */
408    
409     /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/
410    if (m==1)
411    {
412       int j;
413       for (j=0;j<p;j++)
414       {
415          Fout[j] = f;
416          f += fstride*in_stride;
417       }
418    } else {
419       int j;
420       for (j=0;j<p;j++)
421       {
422          compute_bitrev_table( Fout , f, fstride*p, in_stride, factors,st);
423          f += fstride*in_stride;
424          Fout += m;
425       }
426    }
427 }
428
429 static
430 void kf_work(
431         kiss_fft_cpx * Fout,
432         const kiss_fft_cpx * f,
433         const size_t fstride,
434         int in_stride,
435         int * factors,
436         const kiss_fft_cfg st,
437         int N,
438         int s2,
439         int m2
440         )
441 {
442     int i;
443     kiss_fft_cpx * Fout_beg=Fout;
444     const int p=*factors++; /* the radix  */
445     const int m=*factors++; /* stage's fft length/p */
446     /*printf ("fft %d %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N, m2);*/
447     if (m!=1) 
448         kf_work( Fout , f, fstride*p, in_stride, factors,st, N*p, fstride*in_stride, m);
449
450     switch (p) {
451         case 2: kf_bfly2(Fout,fstride,st,m, N, m2); break;
452         case 3: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly3(Fout,fstride,st,m);} break; 
453         case 4: kf_bfly4(Fout,fstride,st,m, N, m2); break;
454         case 5: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly5(Fout,fstride,st,m);} break; 
455         default: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly_generic(Fout,fstride,st,m,p);} break;
456     }    
457 }
458
459 /*  facbuf is populated by p1,m1,p2,m2, ...
460     where 
461     p[i] * m[i] = m[i-1]
462     m0 = n                  */
463 static 
464 void kf_factor(int n,int * facbuf)
465 {
466     int p=4;
467
468     /*factor out powers of 4, powers of 2, then any remaining primes */
469     do {
470         while (n % p) {
471             switch (p) {
472                 case 4: p = 2; break;
473                 case 2: p = 3; break;
474                 default: p += 2; break;
475             }
476             if (p>32000 || (celt_int32_t)p*(celt_int32_t)p > n)
477                 p = n;          /* no more factors, skip to end */
478         }
479         n /= p;
480         *facbuf++ = p;
481         *facbuf++ = n;
482     } while (n > 1);
483 }
484 /*
485  *
486  * User-callable function to allocate all necessary storage space for the fft.
487  *
488  * The return value is a contiguous block of memory, allocated with malloc.  As such,
489  * It can be freed with free(), rather than a kiss_fft-specific function.
490  * */
491 kiss_fft_cfg kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem )
492 {
493     kiss_fft_cfg st=NULL;
494     size_t memneeded = sizeof(struct kiss_fft_state)
495         + sizeof(kiss_fft_cpx)*(nfft-1); /* twiddle factors*/
496
497     if ( lenmem==NULL ) {
498         st = ( kiss_fft_cfg)KISS_FFT_MALLOC( memneeded );
499     }else{
500         if (mem != NULL && *lenmem >= memneeded)
501             st = (kiss_fft_cfg)mem;
502         *lenmem = memneeded;
503     }
504     if (st) {
505         int i;
506         st->nfft=nfft;
507         st->inverse = inverse_fft;
508 #ifdef FIXED_POINT
509         for (i=0;i<nfft;++i) {
510             celt_word32_t phase = i;
511             kf_cexp2(st->twiddles+i, DIV32(SHL32(phase,17),nfft));
512         }
513 #else
514         for (i=0;i<nfft;++i) {
515            const double pi=3.14159265358979323846264338327;
516            double phase = ( -2*pi /nfft ) * i;
517            kf_cexp(st->twiddles+i, phase );
518         }
519 #endif
520         kf_factor(nfft,st->factors);
521         
522         /* bitrev */
523         st->bitrev = celt_alloc(sizeof(int)*(nfft));
524         compute_bitrev_table(st->bitrev, 0, 1,1, st->factors,st);
525     }
526     return st;
527 }
528
529
530
531     
532 void kiss_fft_stride(kiss_fft_cfg st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int in_stride)
533 {
534     if (fin == fout) 
535     {
536        celt_fatal("In-place FFT not supported");
537     } else {
538        /* Bit-reverse the input */
539        int i;
540        for (i=0;i<st->nfft;i++)
541           fout[i] = fin[st->bitrev[i]];
542        kf_work( fout, fin, 1,in_stride, st->factors,st, 1, in_stride, 1);
543     }
544 }
545
546 void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
547 {
548     kiss_fft_stride(cfg,fin,fout,1);
549 }
550