9869e9eb86b5db629867c458baa6704a69054d4a
[opus.git] / celt / kiss_fft.c
1 /*Copyright (c) 2003-2004, Mark Borgerding
2   Lots of modifications by Jean-Marc Valin
3   Copyright (c) 2005-2007, Xiph.Org Foundation
4   Copyright (c) 2008,      Xiph.Org Foundation, CSIRO
5
6   All rights reserved.
7
8   Redistribution and use in source and binary forms, with or without
9    modification, are permitted provided that the following conditions are met:
10
11     * Redistributions of source code must retain the above copyright notice,
12        this list of conditions and the following disclaimer.
13     * Redistributions in binary form must reproduce the above copyright notice,
14        this list of conditions and the following disclaimer in the
15        documentation and/or other materials provided with the distribution.
16
17   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27   POSSIBILITY OF SUCH DAMAGE.*/
28
29 /* This code is originally from Mark Borgerding's KISS-FFT but has been
30    heavily modified to better suit Opus */
31
32 #ifndef SKIP_CONFIG_H
33 #  ifdef HAVE_CONFIG_H
34 #    include "config.h"
35 #  endif
36 #endif
37
38 #include "_kiss_fft_guts.h"
39 #include "arch.h"
40 #include "os_support.h"
41 #include "mathops.h"
42 #include "stack_alloc.h"
43
44 /* The guts header contains all the multiplication and addition macros that are defined for
45    complex numbers.  It also delares the kf_ internal functions.
46 */
47
48 static void kf_bfly2(
49                      kiss_fft_cpx * Fout,
50                      const size_t fstride,
51                      const kiss_fft_state *st,
52                      int m,
53                      int N,
54                      int mm
55                     )
56 {
57    kiss_fft_cpx * Fout2;
58    const kiss_twiddle_cpx * tw1;
59    int i,j;
60    kiss_fft_cpx * Fout_beg = Fout;
61    for (i=0;i<N;i++)
62    {
63       Fout = Fout_beg + i*mm;
64       Fout2 = Fout + m;
65       tw1 = st->twiddles;
66       for(j=0;j<m;j++)
67       {
68          kiss_fft_cpx t;
69          C_MUL (t,  *Fout2 , *tw1);
70          tw1 += fstride;
71          C_SUB( *Fout2 ,  *Fout , t );
72          C_ADDTO( *Fout ,  t );
73          ++Fout2;
74          ++Fout;
75       }
76    }
77 }
78
79 static void kf_bfly4(
80                      kiss_fft_cpx * Fout,
81                      const size_t fstride,
82                      const kiss_fft_state *st,
83                      int m,
84                      int N,
85                      int mm
86                     )
87 {
88    int i;
89
90    if (m==1)
91    {
92       /* Degenerate case where all the twiddles are 1. */
93       for (i=0;i<N;i++)
94       {
95          kiss_fft_cpx scratch0, scratch1;
96
97          C_SUB( scratch0 , *Fout, Fout[2] );
98          C_ADDTO(*Fout, Fout[2]);
99          C_ADD( scratch1 , Fout[1] , Fout[3] );
100          C_SUB( Fout[2], *Fout, scratch1 );
101          C_ADDTO( *Fout , scratch1 );
102          C_SUB( scratch1 , Fout[1] , Fout[3] );
103
104          Fout[1].r = scratch0.r + scratch1.i;
105          Fout[1].i = scratch0.i - scratch1.r;
106          Fout[3].r = scratch0.r - scratch1.i;
107          Fout[3].i = scratch0.i + scratch1.r;
108          Fout+=4;
109       }
110    } else {
111       int j;
112       kiss_fft_cpx scratch[6];
113       const kiss_twiddle_cpx *tw1,*tw2,*tw3;
114       const int m2=2*m;
115       const int m3=3*m;
116       kiss_fft_cpx * Fout_beg = Fout;
117       for (i=0;i<N;i++)
118       {
119          Fout = Fout_beg + i*mm;
120          tw3 = tw2 = tw1 = st->twiddles;
121          for (j=0;j<m;j++)
122          {
123             C_MUL(scratch[0],Fout[m] , *tw1 );
124             C_MUL(scratch[1],Fout[m2] , *tw2 );
125             C_MUL(scratch[2],Fout[m3] , *tw3 );
126
127             C_SUB( scratch[5] , *Fout, scratch[1] );
128             C_ADDTO(*Fout, scratch[1]);
129             C_ADD( scratch[3] , scratch[0] , scratch[2] );
130             C_SUB( scratch[4] , scratch[0] , scratch[2] );
131             C_SUB( Fout[m2], *Fout, scratch[3] );
132             tw1 += fstride;
133             tw2 += fstride*2;
134             tw3 += fstride*3;
135             C_ADDTO( *Fout , scratch[3] );
136
137             Fout[m].r = scratch[5].r + scratch[4].i;
138             Fout[m].i = scratch[5].i - scratch[4].r;
139             Fout[m3].r = scratch[5].r - scratch[4].i;
140             Fout[m3].i = scratch[5].i + scratch[4].r;
141             ++Fout;
142          }
143       }
144    }
145 }
146
147
148 #ifndef RADIX_TWO_ONLY
149
150 static void kf_bfly3(
151                      kiss_fft_cpx * Fout,
152                      const size_t fstride,
153                      const kiss_fft_state *st,
154                      int m,
155                      int N,
156                      int mm
157                     )
158 {
159    int i;
160    size_t k;
161    const size_t m2 = 2*m;
162    const kiss_twiddle_cpx *tw1,*tw2;
163    kiss_fft_cpx scratch[5];
164    kiss_twiddle_cpx epi3;
165
166    kiss_fft_cpx * Fout_beg = Fout;
167    epi3 = st->twiddles[fstride*m];
168    for (i=0;i<N;i++)
169    {
170       Fout = Fout_beg + i*mm;
171       tw1=tw2=st->twiddles;
172       k=m;
173       do {
174
175          C_MUL(scratch[1],Fout[m] , *tw1);
176          C_MUL(scratch[2],Fout[m2] , *tw2);
177
178          C_ADD(scratch[3],scratch[1],scratch[2]);
179          C_SUB(scratch[0],scratch[1],scratch[2]);
180          tw1 += fstride;
181          tw2 += fstride*2;
182
183          Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
184          Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
185
186          C_MULBYSCALAR( scratch[0] , epi3.i );
187
188          C_ADDTO(*Fout,scratch[3]);
189
190          Fout[m2].r = Fout[m].r + scratch[0].i;
191          Fout[m2].i = Fout[m].i - scratch[0].r;
192
193          Fout[m].r -= scratch[0].i;
194          Fout[m].i += scratch[0].r;
195
196          ++Fout;
197       } while(--k);
198    }
199 }
200
201
202 static void kf_bfly5(
203                      kiss_fft_cpx * Fout,
204                      const size_t fstride,
205                      const kiss_fft_state *st,
206                      int m,
207                      int N,
208                      int mm
209                     )
210 {
211    kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
212    int i, u;
213    kiss_fft_cpx scratch[13];
214    const kiss_twiddle_cpx * twiddles = st->twiddles;
215    const kiss_twiddle_cpx *tw;
216    kiss_twiddle_cpx ya,yb;
217    kiss_fft_cpx * Fout_beg = Fout;
218
219    ya = twiddles[fstride*m];
220    yb = twiddles[fstride*2*m];
221    tw=st->twiddles;
222
223    for (i=0;i<N;i++)
224    {
225       Fout = Fout_beg + i*mm;
226       Fout0=Fout;
227       Fout1=Fout0+m;
228       Fout2=Fout0+2*m;
229       Fout3=Fout0+3*m;
230       Fout4=Fout0+4*m;
231
232       for ( u=0; u<m; ++u ) {
233          scratch[0] = *Fout0;
234
235          C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);
236          C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);
237          C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);
238          C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);
239
240          C_ADD( scratch[7],scratch[1],scratch[4]);
241          C_SUB( scratch[10],scratch[1],scratch[4]);
242          C_ADD( scratch[8],scratch[2],scratch[3]);
243          C_SUB( scratch[9],scratch[2],scratch[3]);
244
245          Fout0->r += scratch[7].r + scratch[8].r;
246          Fout0->i += scratch[7].i + scratch[8].i;
247
248          scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
249          scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
250
251          scratch[6].r =  S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);
252          scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);
253
254          C_SUB(*Fout1,scratch[5],scratch[6]);
255          C_ADD(*Fout4,scratch[5],scratch[6]);
256
257          scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
258          scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
259          scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
260          scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);
261
262          C_ADD(*Fout2,scratch[11],scratch[12]);
263          C_SUB(*Fout3,scratch[11],scratch[12]);
264
265          ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
266       }
267    }
268 }
269
270
271 #endif
272
273
274 #ifdef CUSTOM_MODES
275
276 static
277 void compute_bitrev_table(
278          int Fout,
279          opus_int16 *f,
280          const size_t fstride,
281          int in_stride,
282          opus_int16 * factors,
283          const kiss_fft_state *st
284             )
285 {
286    const int p=*factors++; /* the radix  */
287    const int m=*factors++; /* stage's fft length/p */
288
289     /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/
290    if (m==1)
291    {
292       int j;
293       for (j=0;j<p;j++)
294       {
295          *f = Fout+j;
296          f += fstride*in_stride;
297       }
298    } else {
299       int j;
300       for (j=0;j<p;j++)
301       {
302          compute_bitrev_table( Fout , f, fstride*p, in_stride, factors,st);
303          f += fstride*in_stride;
304          Fout += m;
305       }
306    }
307 }
308
309 /*  facbuf is populated by p1,m1,p2,m2, ...
310     where
311     p[i] * m[i] = m[i-1]
312     m0 = n                  */
313 static
314 int kf_factor(int n,opus_int16 * facbuf)
315 {
316     int p=4;
317     int i;
318     int stages=0;
319     int nbak = n;
320
321     /*factor out powers of 4, powers of 2, then any remaining primes */
322     do {
323         while (n % p) {
324             switch (p) {
325                 case 4: p = 2; break;
326                 case 2: p = 3; break;
327                 default: p += 2; break;
328             }
329             if (p>32000 || (opus_int32)p*(opus_int32)p > n)
330                 p = n;          /* no more factors, skip to end */
331         }
332         n /= p;
333 #ifdef RADIX_TWO_ONLY
334         if (p!=2 && p != 4)
335 #else
336         if (p>5)
337 #endif
338         {
339            return 0;
340         }
341         facbuf[2*stages] = p;
342         stages++;
343     } while (n > 1);
344     n = nbak;
345     /* Reverse the order to get the radix 4 at the end, so we can use the
346        fast degenerate case. It turns out that reversing the order also
347        improves the noise behaviour. */
348     for (i=0;i<stages/2;i++)
349     {
350        int tmp;
351        tmp = facbuf[2*i];
352        facbuf[2*i] = facbuf[2*(stages-i-1)];
353        facbuf[2*(stages-i-1)] = tmp;
354     }
355     for (i=0;i<stages;i++)
356     {
357         n /= facbuf[2*i];
358         facbuf[2*i+1] = n;
359     }
360     return 1;
361 }
362
363 static void compute_twiddles(kiss_twiddle_cpx *twiddles, int nfft)
364 {
365    int i;
366 #ifdef FIXED_POINT
367    for (i=0;i<nfft;++i) {
368       opus_val32 phase = -i;
369       kf_cexp2(twiddles+i, DIV32(SHL32(phase,17),nfft));
370    }
371 #else
372    for (i=0;i<nfft;++i) {
373       const double pi=3.14159265358979323846264338327;
374       double phase = ( -2*pi /nfft ) * i;
375       kf_cexp(twiddles+i, phase );
376    }
377 #endif
378 }
379
380 /*
381  *
382  * Allocates all necessary storage space for the fft and ifft.
383  * The return value is a contiguous block of memory.  As such,
384  * It can be freed with free().
385  * */
386 kiss_fft_state *opus_fft_alloc_twiddles(int nfft,void * mem,size_t * lenmem,  const kiss_fft_state *base)
387 {
388     kiss_fft_state *st=NULL;
389     size_t memneeded = sizeof(struct kiss_fft_state); /* twiddle factors*/
390
391     if ( lenmem==NULL ) {
392         st = ( kiss_fft_state*)KISS_FFT_MALLOC( memneeded );
393     }else{
394         if (mem != NULL && *lenmem >= memneeded)
395             st = (kiss_fft_state*)mem;
396         *lenmem = memneeded;
397     }
398     if (st) {
399         opus_int16 *bitrev;
400         kiss_twiddle_cpx *twiddles;
401
402         st->nfft=nfft;
403 #ifndef FIXED_POINT
404         st->scale = 1.f/nfft;
405 #endif
406         if (base != NULL)
407         {
408            st->twiddles = base->twiddles;
409            st->shift = 0;
410            while (nfft<<st->shift != base->nfft && st->shift < 32)
411               st->shift++;
412            if (st->shift>=32)
413               goto fail;
414         } else {
415            st->twiddles = twiddles = (kiss_twiddle_cpx*)KISS_FFT_MALLOC(sizeof(kiss_twiddle_cpx)*nfft);
416            compute_twiddles(twiddles, nfft);
417            st->shift = -1;
418         }
419         if (!kf_factor(nfft,st->factors))
420         {
421            goto fail;
422         }
423
424         /* bitrev */
425         st->bitrev = bitrev = (opus_int16*)KISS_FFT_MALLOC(sizeof(opus_int16)*nfft);
426         if (st->bitrev==NULL)
427             goto fail;
428         compute_bitrev_table(0, bitrev, 1,1, st->factors,st);
429     }
430     return st;
431 fail:
432     opus_fft_free(st);
433     return NULL;
434 }
435
436 kiss_fft_state *opus_fft_alloc(int nfft,void * mem,size_t * lenmem )
437 {
438    return opus_fft_alloc_twiddles(nfft, mem, lenmem, NULL);
439 }
440
441 void opus_fft_free(const kiss_fft_state *cfg)
442 {
443    if (cfg)
444    {
445       opus_free((opus_int16*)cfg->bitrev);
446       if (cfg->shift < 0)
447          opus_free((kiss_twiddle_cpx*)cfg->twiddles);
448       opus_free((kiss_fft_state*)cfg);
449    }
450 }
451
452 #endif /* CUSTOM_MODES */
453
454 void opus_fft_impl(const kiss_fft_state *st,kiss_fft_cpx *fout)
455 {
456     int m2, m;
457     int p;
458     int L;
459     int fstride[MAXFACTORS];
460     int i;
461     int shift;
462
463     /* st->shift can be -1 */
464     shift = st->shift>0 ? st->shift : 0;
465
466     fstride[0] = 1;
467     L=0;
468     do {
469        p = st->factors[2*L];
470        m = st->factors[2*L+1];
471        fstride[L+1] = fstride[L]*p;
472        L++;
473     } while(m!=1);
474     m = st->factors[2*L-1];
475     for (i=L-1;i>=0;i--)
476     {
477        if (i!=0)
478           m2 = st->factors[2*i-1];
479        else
480           m2 = 1;
481        switch (st->factors[2*i])
482        {
483        case 2:
484           kf_bfly2(fout,fstride[i]<<shift,st,m, fstride[i], m2);
485           break;
486        case 4:
487           kf_bfly4(fout,fstride[i]<<shift,st,m, fstride[i], m2);
488           break;
489  #ifndef RADIX_TWO_ONLY
490        case 3:
491           kf_bfly3(fout,fstride[i]<<shift,st,m, fstride[i], m2);
492           break;
493        case 5:
494           kf_bfly5(fout,fstride[i]<<shift,st,m, fstride[i], m2);
495           break;
496  #endif
497        }
498        m = m2;
499     }
500 }
501
502 void opus_fft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
503 {
504    int i;
505 #ifdef FIXED_POINT
506    /* FIXME: This should eventually just go in the state. */
507    opus_val16 scale;
508    int scale_shift;
509    scale_shift = celt_ilog2(st->nfft);
510    if (st->nfft == 1<<scale_shift)
511       scale = Q15ONE;
512    else
513       scale = (1073741824+st->nfft/2)/st->nfft>>(15-scale_shift);
514 #endif
515
516    celt_assert2 (fin != fout, "In-place FFT not supported");
517    /* Bit-reverse the input */
518    for (i=0;i<st->nfft;i++)
519    {
520       kiss_fft_cpx x = fin[i];
521 #ifdef FIXED_POINT
522       fout[st->bitrev[i]].r = SHR32(MULT16_32_Q15(scale, x.r), scale_shift);
523       fout[st->bitrev[i]].i = SHR32(MULT16_32_Q15(scale, x.i), scale_shift);
524 #else
525       fout[st->bitrev[i]].r = st->scale*x.r;
526       fout[st->bitrev[i]].i = st->scale*x.i;
527 #endif
528    }
529    opus_fft_impl(st, fout);
530 }
531
532
533 #ifdef TEST_UNIT_DFT_C
534 void opus_ifft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
535 {
536    int i;
537    celt_assert2 (fin != fout, "In-place FFT not supported");
538    /* Bit-reverse the input */
539    for (i=0;i<st->nfft;i++)
540       fout[st->bitrev[i]] = fin[i];
541    for (i=0;i<st->nfft;i++)
542       fout[i].i = -fout[i].i;
543    opus_fft_impl(st, fout);
544    for (i=0;i<st->nfft;i++)
545       fout[i].i = -fout[i].i;
546 }
547 #endif