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