Fixes C89 issue
[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 non-custom modes, m is guaranteed to be a multiple of 4. */
67       for(j=0;j<m;j++)
68       {
69          kiss_fft_cpx t;
70          C_MUL (t,  *Fout2 , *tw1);
71          tw1 += fstride;
72          C_SUB( *Fout2 ,  *Fout , t );
73          C_ADDTO( *Fout ,  t );
74          ++Fout2;
75          ++Fout;
76       }
77    }
78 }
79
80 static void kf_bfly4(
81                      kiss_fft_cpx * Fout,
82                      const size_t fstride,
83                      const kiss_fft_state *st,
84                      int m,
85                      int N,
86                      int mm
87                     )
88 {
89    int i;
90
91    if (m==1)
92    {
93       /* Degenerate case where all the twiddles are 1. */
94       for (i=0;i<N;i++)
95       {
96          kiss_fft_cpx scratch0, scratch1;
97
98          C_SUB( scratch0 , *Fout, Fout[2] );
99          C_ADDTO(*Fout, Fout[2]);
100          C_ADD( scratch1 , Fout[1] , Fout[3] );
101          C_SUB( Fout[2], *Fout, scratch1 );
102          C_ADDTO( *Fout , scratch1 );
103          C_SUB( scratch1 , Fout[1] , Fout[3] );
104
105          Fout[1].r = scratch0.r + scratch1.i;
106          Fout[1].i = scratch0.i - scratch1.r;
107          Fout[3].r = scratch0.r - scratch1.i;
108          Fout[3].i = scratch0.i + scratch1.r;
109          Fout+=4;
110       }
111    } else {
112       int j;
113       kiss_fft_cpx scratch[6];
114       const kiss_twiddle_cpx *tw1,*tw2,*tw3;
115       const int m2=2*m;
116       const int m3=3*m;
117       kiss_fft_cpx * Fout_beg = Fout;
118       for (i=0;i<N;i++)
119       {
120          Fout = Fout_beg + i*mm;
121          tw3 = tw2 = tw1 = st->twiddles;
122          /* For non-custom modes, m=4, otherwise m is guaranteed to be a
123             multiple of 4. */
124          for (j=0;j<m;j++)
125          {
126             C_MUL(scratch[0],Fout[m] , *tw1 );
127             C_MUL(scratch[1],Fout[m2] , *tw2 );
128             C_MUL(scratch[2],Fout[m3] , *tw3 );
129
130             C_SUB( scratch[5] , *Fout, scratch[1] );
131             C_ADDTO(*Fout, scratch[1]);
132             C_ADD( scratch[3] , scratch[0] , scratch[2] );
133             C_SUB( scratch[4] , scratch[0] , scratch[2] );
134             C_SUB( Fout[m2], *Fout, scratch[3] );
135             tw1 += fstride;
136             tw2 += fstride*2;
137             tw3 += fstride*3;
138             C_ADDTO( *Fout , scratch[3] );
139
140             Fout[m].r = scratch[5].r + scratch[4].i;
141             Fout[m].i = scratch[5].i - scratch[4].r;
142             Fout[m3].r = scratch[5].r - scratch[4].i;
143             Fout[m3].i = scratch[5].i + scratch[4].r;
144             ++Fout;
145          }
146       }
147    }
148 }
149
150
151 #ifndef RADIX_TWO_ONLY
152
153 static void kf_bfly3(
154                      kiss_fft_cpx * Fout,
155                      const size_t fstride,
156                      const kiss_fft_state *st,
157                      int m,
158                      int N,
159                      int mm
160                     )
161 {
162    int i;
163    size_t k;
164    const size_t m2 = 2*m;
165    const kiss_twiddle_cpx *tw1,*tw2;
166    kiss_fft_cpx scratch[5];
167    kiss_twiddle_cpx epi3;
168
169    kiss_fft_cpx * Fout_beg = Fout;
170    epi3 = st->twiddles[fstride*m];
171    for (i=0;i<N;i++)
172    {
173       Fout = Fout_beg + i*mm;
174       tw1=tw2=st->twiddles;
175       /* For non-custom modes, m is guaranteed to be a multiple of 4. */
176       k=m;
177       do {
178
179          C_MUL(scratch[1],Fout[m] , *tw1);
180          C_MUL(scratch[2],Fout[m2] , *tw2);
181
182          C_ADD(scratch[3],scratch[1],scratch[2]);
183          C_SUB(scratch[0],scratch[1],scratch[2]);
184          tw1 += fstride;
185          tw2 += fstride*2;
186
187          Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
188          Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
189
190          C_MULBYSCALAR( scratch[0] , epi3.i );
191
192          C_ADDTO(*Fout,scratch[3]);
193
194          Fout[m2].r = Fout[m].r + scratch[0].i;
195          Fout[m2].i = Fout[m].i - scratch[0].r;
196
197          Fout[m].r -= scratch[0].i;
198          Fout[m].i += scratch[0].r;
199
200          ++Fout;
201       } while(--k);
202    }
203 }
204
205
206 static void kf_bfly5(
207                      kiss_fft_cpx * Fout,
208                      const size_t fstride,
209                      const kiss_fft_state *st,
210                      int m,
211                      int N,
212                      int mm
213                     )
214 {
215    kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
216    int i, u;
217    kiss_fft_cpx scratch[13];
218    const kiss_twiddle_cpx * twiddles = st->twiddles;
219    const kiss_twiddle_cpx *tw;
220    kiss_twiddle_cpx ya,yb;
221    kiss_fft_cpx * Fout_beg = Fout;
222
223    ya = twiddles[fstride*m];
224    yb = twiddles[fstride*2*m];
225    tw=st->twiddles;
226
227    for (i=0;i<N;i++)
228    {
229       Fout = Fout_beg + i*mm;
230       Fout0=Fout;
231       Fout1=Fout0+m;
232       Fout2=Fout0+2*m;
233       Fout3=Fout0+3*m;
234       Fout4=Fout0+4*m;
235
236       /* For non-custom modes, m is guaranteed to be a multiple of 4. */
237       for ( u=0; u<m; ++u ) {
238          scratch[0] = *Fout0;
239
240          C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);
241          C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);
242          C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);
243          C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);
244
245          C_ADD( scratch[7],scratch[1],scratch[4]);
246          C_SUB( scratch[10],scratch[1],scratch[4]);
247          C_ADD( scratch[8],scratch[2],scratch[3]);
248          C_SUB( scratch[9],scratch[2],scratch[3]);
249
250          Fout0->r += scratch[7].r + scratch[8].r;
251          Fout0->i += scratch[7].i + scratch[8].i;
252
253          scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
254          scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
255
256          scratch[6].r =  S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);
257          scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);
258
259          C_SUB(*Fout1,scratch[5],scratch[6]);
260          C_ADD(*Fout4,scratch[5],scratch[6]);
261
262          scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
263          scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
264          scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
265          scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);
266
267          C_ADD(*Fout2,scratch[11],scratch[12]);
268          C_SUB(*Fout3,scratch[11],scratch[12]);
269
270          ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
271       }
272    }
273 }
274
275
276 #endif
277
278
279 #ifdef CUSTOM_MODES
280
281 static
282 void compute_bitrev_table(
283          int Fout,
284          opus_int16 *f,
285          const size_t fstride,
286          int in_stride,
287          opus_int16 * factors,
288          const kiss_fft_state *st
289             )
290 {
291    const int p=*factors++; /* the radix  */
292    const int m=*factors++; /* stage's fft length/p */
293
294     /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/
295    if (m==1)
296    {
297       int j;
298       for (j=0;j<p;j++)
299       {
300          *f = Fout+j;
301          f += fstride*in_stride;
302       }
303    } else {
304       int j;
305       for (j=0;j<p;j++)
306       {
307          compute_bitrev_table( Fout , f, fstride*p, in_stride, factors,st);
308          f += fstride*in_stride;
309          Fout += m;
310       }
311    }
312 }
313
314 /*  facbuf is populated by p1,m1,p2,m2, ...
315     where
316     p[i] * m[i] = m[i-1]
317     m0 = n                  */
318 static
319 int kf_factor(int n,opus_int16 * facbuf)
320 {
321     int p=4;
322     int i;
323     int stages=0;
324     int nbak = n;
325
326     /*factor out powers of 4, powers of 2, then any remaining primes */
327     do {
328         while (n % p) {
329             switch (p) {
330                 case 4: p = 2; break;
331                 case 2: p = 3; break;
332                 default: p += 2; break;
333             }
334             if (p>32000 || (opus_int32)p*(opus_int32)p > n)
335                 p = n;          /* no more factors, skip to end */
336         }
337         n /= p;
338 #ifdef RADIX_TWO_ONLY
339         if (p!=2 && p != 4)
340 #else
341         if (p>5)
342 #endif
343         {
344            return 0;
345         }
346         facbuf[2*stages] = p;
347         stages++;
348     } while (n > 1);
349     n = nbak;
350     /* Reverse the order to get the radix 4 at the end, so we can use the
351        fast degenerate case. It turns out that reversing the order also
352        improves the noise behaviour. */
353     for (i=0;i<stages/2;i++)
354     {
355        int tmp;
356        tmp = facbuf[2*i];
357        facbuf[2*i] = facbuf[2*(stages-i-1)];
358        facbuf[2*(stages-i-1)] = tmp;
359     }
360     for (i=0;i<stages;i++)
361     {
362         n /= facbuf[2*i];
363         facbuf[2*i+1] = n;
364     }
365     return 1;
366 }
367
368 static void compute_twiddles(kiss_twiddle_cpx *twiddles, int nfft)
369 {
370    int i;
371 #ifdef FIXED_POINT
372    for (i=0;i<nfft;++i) {
373       opus_val32 phase = -i;
374       kf_cexp2(twiddles+i, DIV32(SHL32(phase,17),nfft));
375    }
376 #else
377    for (i=0;i<nfft;++i) {
378       const double pi=3.14159265358979323846264338327;
379       double phase = ( -2*pi /nfft ) * i;
380       kf_cexp(twiddles+i, phase );
381    }
382 #endif
383 }
384
385 /*
386  *
387  * Allocates all necessary storage space for the fft and ifft.
388  * The return value is a contiguous block of memory.  As such,
389  * It can be freed with free().
390  * */
391 kiss_fft_state *opus_fft_alloc_twiddles(int nfft,void * mem,size_t * lenmem,  const kiss_fft_state *base)
392 {
393     kiss_fft_state *st=NULL;
394     size_t memneeded = sizeof(struct kiss_fft_state); /* twiddle factors*/
395
396     if ( lenmem==NULL ) {
397         st = ( kiss_fft_state*)KISS_FFT_MALLOC( memneeded );
398     }else{
399         if (mem != NULL && *lenmem >= memneeded)
400             st = (kiss_fft_state*)mem;
401         *lenmem = memneeded;
402     }
403     if (st) {
404         opus_int16 *bitrev;
405         kiss_twiddle_cpx *twiddles;
406
407         st->nfft=nfft;
408 #ifndef FIXED_POINT
409         st->scale = 1.f/nfft;
410 #endif
411         if (base != NULL)
412         {
413            st->twiddles = base->twiddles;
414            st->shift = 0;
415            while (nfft<<st->shift != base->nfft && st->shift < 32)
416               st->shift++;
417            if (st->shift>=32)
418               goto fail;
419         } else {
420            st->twiddles = twiddles = (kiss_twiddle_cpx*)KISS_FFT_MALLOC(sizeof(kiss_twiddle_cpx)*nfft);
421            compute_twiddles(twiddles, nfft);
422            st->shift = -1;
423         }
424         if (!kf_factor(nfft,st->factors))
425         {
426            goto fail;
427         }
428
429         /* bitrev */
430         st->bitrev = bitrev = (opus_int16*)KISS_FFT_MALLOC(sizeof(opus_int16)*nfft);
431         if (st->bitrev==NULL)
432             goto fail;
433         compute_bitrev_table(0, bitrev, 1,1, st->factors,st);
434     }
435     return st;
436 fail:
437     opus_fft_free(st);
438     return NULL;
439 }
440
441 kiss_fft_state *opus_fft_alloc(int nfft,void * mem,size_t * lenmem )
442 {
443    return opus_fft_alloc_twiddles(nfft, mem, lenmem, NULL);
444 }
445
446 void opus_fft_free(const kiss_fft_state *cfg)
447 {
448    if (cfg)
449    {
450       opus_free((opus_int16*)cfg->bitrev);
451       if (cfg->shift < 0)
452          opus_free((kiss_twiddle_cpx*)cfg->twiddles);
453       opus_free((kiss_fft_state*)cfg);
454    }
455 }
456
457 #endif /* CUSTOM_MODES */
458
459 void opus_fft_impl(const kiss_fft_state *st,kiss_fft_cpx *fout)
460 {
461     int m2, m;
462     int p;
463     int L;
464     int fstride[MAXFACTORS];
465     int i;
466     int shift;
467
468     /* st->shift can be -1 */
469     shift = st->shift>0 ? st->shift : 0;
470
471     fstride[0] = 1;
472     L=0;
473     do {
474        p = st->factors[2*L];
475        m = st->factors[2*L+1];
476        fstride[L+1] = fstride[L]*p;
477        L++;
478     } while(m!=1);
479     m = st->factors[2*L-1];
480     for (i=L-1;i>=0;i--)
481     {
482        if (i!=0)
483           m2 = st->factors[2*i-1];
484        else
485           m2 = 1;
486        switch (st->factors[2*i])
487        {
488        case 2:
489           kf_bfly2(fout,fstride[i]<<shift,st,m, fstride[i], m2);
490           break;
491        case 4:
492           kf_bfly4(fout,fstride[i]<<shift,st,m, fstride[i], m2);
493           break;
494  #ifndef RADIX_TWO_ONLY
495        case 3:
496           kf_bfly3(fout,fstride[i]<<shift,st,m, fstride[i], m2);
497           break;
498        case 5:
499           kf_bfly5(fout,fstride[i]<<shift,st,m, fstride[i], m2);
500           break;
501  #endif
502        }
503        m = m2;
504     }
505 }
506
507 void opus_fft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
508 {
509    int i;
510 #ifdef FIXED_POINT
511    /* FIXME: This should eventually just go in the state. */
512    opus_val16 scale;
513    int scale_shift;
514    scale_shift = celt_ilog2(st->nfft);
515    if (st->nfft == 1<<scale_shift)
516       scale = Q15ONE;
517    else
518       scale = (1073741824+st->nfft/2)/st->nfft>>(15-scale_shift);
519 #endif
520
521    celt_assert2 (fin != fout, "In-place FFT not supported");
522    /* Bit-reverse the input */
523    for (i=0;i<st->nfft;i++)
524    {
525       kiss_fft_cpx x = fin[i];
526 #ifdef FIXED_POINT
527       fout[st->bitrev[i]].r = SHR32(MULT16_32_Q15(scale, x.r), scale_shift);
528       fout[st->bitrev[i]].i = SHR32(MULT16_32_Q15(scale, x.i), scale_shift);
529 #else
530       fout[st->bitrev[i]].r = st->scale*x.r;
531       fout[st->bitrev[i]].i = st->scale*x.i;
532 #endif
533    }
534    opus_fft_impl(st, fout);
535 }
536
537
538 #ifdef TEST_UNIT_DFT_C
539 void opus_ifft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
540 {
541    int i;
542    celt_assert2 (fin != fout, "In-place FFT not supported");
543    /* Bit-reverse the input */
544    for (i=0;i<st->nfft;i++)
545       fout[st->bitrev[i]] = fin[i];
546    for (i=0;i<st->nfft;i++)
547       fout[i].i = -fout[i].i;
548    opus_fft_impl(st, fout);
549    for (i=0;i<st->nfft;i++)
550       fout[i].i = -fout[i].i;
551 }
552 #endif