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