Replaced speex_error() by speex_fatal() and speex_assert()
[speexdsp.git] / libspeex / kiss_fftr.c
1 /*
2 Copyright (c) 2003-2004, Mark Borgerding
3
4 All rights reserved.
5
6 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
7
8     * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
9     * 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.
10     * 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.
11
12 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.
13 */
14
15 #ifdef HAVE_CONFIG_H
16 #include "config.h"
17 #endif
18
19 #include "kiss_fftr.h"
20 #include "_kiss_fft_guts.h"
21
22 struct kiss_fftr_state{
23     kiss_fft_cfg substate;
24     kiss_fft_cpx * tmpbuf;
25     kiss_fft_cpx * super_twiddles;
26 #ifdef USE_SIMD    
27     long pad;
28 #endif    
29 };
30
31 kiss_fftr_cfg kiss_fftr_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem)
32 {
33     int i;
34     kiss_fftr_cfg st = NULL;
35     size_t subsize, memneeded;
36
37     if (nfft & 1) {
38         speex_warning("Real FFT optimization must be even.\n");
39         return NULL;
40     }
41     nfft >>= 1;
42
43     kiss_fft_alloc (nfft, inverse_fft, NULL, &subsize);
44     memneeded = sizeof(struct kiss_fftr_state) + subsize + sizeof(kiss_fft_cpx) * ( nfft * 2);
45
46     if (lenmem == NULL) {
47         st = (kiss_fftr_cfg) KISS_FFT_MALLOC (memneeded);
48     } else {
49         if (*lenmem >= memneeded)
50             st = (kiss_fftr_cfg) mem;
51         *lenmem = memneeded;
52     }
53     if (!st)
54         return NULL;
55
56     st->substate = (kiss_fft_cfg) (st + 1); /*just beyond kiss_fftr_state struct */
57     st->tmpbuf = (kiss_fft_cpx *) (((char *) st->substate) + subsize);
58     st->super_twiddles = st->tmpbuf + nfft;
59     kiss_fft_alloc(nfft, inverse_fft, st->substate, &subsize);
60
61 #ifdef FIXED_POINT
62     for (i=0;i<nfft;++i) {
63        spx_word32_t phase = i+(nfft>>1);
64        if (!inverse_fft)
65           phase = -phase;
66        kf_cexp2(st->super_twiddles+i, DIV32(SHL32(phase,16),nfft));
67     }
68 #else
69     for (i=0;i<nfft;++i) {
70        const double pi=3.14159265358979323846264338327;
71        double phase = pi*(((double)i) /nfft + .5);
72        if (!inverse_fft)
73           phase = -phase;
74        kf_cexp(st->super_twiddles+i, phase );
75     }
76 #endif
77     return st;
78 }
79
80 void kiss_fftr(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_cpx *freqdata)
81 {
82     /* input buffer timedata is stored row-wise */
83     int k,ncfft;
84     kiss_fft_cpx fpnk,fpk,f1k,f2k,tw,tdc;
85
86     if ( st->substate->inverse) {
87         speex_fatal("kiss fft usage error: improper alloc\n");
88     }
89
90     ncfft = st->substate->nfft;
91
92     /*perform the parallel fft of two real signals packed in real,imag*/
93     kiss_fft( st->substate , (const kiss_fft_cpx*)timedata, st->tmpbuf );
94     /* The real part of the DC element of the frequency spectrum in st->tmpbuf
95      * contains the sum of the even-numbered elements of the input time sequence
96      * The imag part is the sum of the odd-numbered elements
97      *
98      * The sum of tdc.r and tdc.i is the sum of the input time sequence. 
99      *      yielding DC of input time sequence
100      * The difference of tdc.r - tdc.i is the sum of the input (dot product) [1,-1,1,-1... 
101      *      yielding Nyquist bin of input time sequence
102      */
103  
104     tdc.r = st->tmpbuf[0].r;
105     tdc.i = st->tmpbuf[0].i;
106     C_FIXDIV(tdc,2);
107     CHECK_OVERFLOW_OP(tdc.r ,+, tdc.i);
108     CHECK_OVERFLOW_OP(tdc.r ,-, tdc.i);
109     freqdata[0].r = tdc.r + tdc.i;
110     freqdata[ncfft].r = tdc.r - tdc.i;
111 #ifdef USE_SIMD    
112     freqdata[ncfft].i = freqdata[0].i = _mm_set1_ps(0);
113 #else
114     freqdata[ncfft].i = freqdata[0].i = 0;
115 #endif
116
117     for ( k=1;k <= ncfft/2 ; ++k ) {
118         fpk    = st->tmpbuf[k]; 
119         fpnk.r =   st->tmpbuf[ncfft-k].r;
120         fpnk.i = - st->tmpbuf[ncfft-k].i;
121         C_FIXDIV(fpk,2);
122         C_FIXDIV(fpnk,2);
123
124         C_ADD( f1k, fpk , fpnk );
125         C_SUB( f2k, fpk , fpnk );
126         C_MUL( tw , f2k , st->super_twiddles[k]);
127
128         freqdata[k].r = HALF_OF(f1k.r + tw.r);
129         freqdata[k].i = HALF_OF(f1k.i + tw.i);
130         freqdata[ncfft-k].r = HALF_OF(f1k.r - tw.r);
131         freqdata[ncfft-k].i = HALF_OF(tw.i - f1k.i);
132     }
133 }
134
135 void kiss_fftri(kiss_fftr_cfg st,const kiss_fft_cpx *freqdata, kiss_fft_scalar *timedata)
136 {
137     /* input buffer timedata is stored row-wise */
138     int k, ncfft;
139
140     if (st->substate->inverse == 0) {
141         speex_fatal("kiss fft usage error: improper alloc\n");
142     }
143
144     ncfft = st->substate->nfft;
145
146     st->tmpbuf[0].r = freqdata[0].r + freqdata[ncfft].r;
147     st->tmpbuf[0].i = freqdata[0].r - freqdata[ncfft].r;
148     /*C_FIXDIV(st->tmpbuf[0],2);*/
149
150     for (k = 1; k <= ncfft / 2; ++k) {
151         kiss_fft_cpx fk, fnkc, fek, fok, tmp;
152         fk = freqdata[k];
153         fnkc.r = freqdata[ncfft - k].r;
154         fnkc.i = -freqdata[ncfft - k].i;
155         /*C_FIXDIV( fk , 2 );
156         C_FIXDIV( fnkc , 2 );*/
157
158         C_ADD (fek, fk, fnkc);
159         C_SUB (tmp, fk, fnkc);
160         C_MUL (fok, tmp, st->super_twiddles[k]);
161         C_ADD (st->tmpbuf[k],     fek, fok);
162         C_SUB (st->tmpbuf[ncfft - k], fek, fok);
163 #ifdef USE_SIMD        
164         st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0);
165 #else
166         st->tmpbuf[ncfft - k].i *= -1;
167 #endif
168     }
169     kiss_fft (st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata);
170 }
171
172 void kiss_fftr2(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_scalar *freqdata)
173 {
174    /* input buffer timedata is stored row-wise */
175    int k,ncfft;
176    kiss_fft_cpx f2k,tdc;
177    spx_word32_t f1kr, f1ki, twr, twi;
178
179    if ( st->substate->inverse) {
180       speex_fatal("kiss fft usage error: improper alloc\n");
181    }
182
183    ncfft = st->substate->nfft;
184
185    /*perform the parallel fft of two real signals packed in real,imag*/
186    kiss_fft( st->substate , (const kiss_fft_cpx*)timedata, st->tmpbuf );
187     /* The real part of the DC element of the frequency spectrum in st->tmpbuf
188    * contains the sum of the even-numbered elements of the input time sequence
189    * The imag part is the sum of the odd-numbered elements
190    *
191    * The sum of tdc.r and tdc.i is the sum of the input time sequence. 
192    *      yielding DC of input time sequence
193    * The difference of tdc.r - tdc.i is the sum of the input (dot product) [1,-1,1,-1... 
194    *      yielding Nyquist bin of input time sequence
195     */
196  
197    tdc.r = st->tmpbuf[0].r;
198    tdc.i = st->tmpbuf[0].i;
199    C_FIXDIV(tdc,2);
200    CHECK_OVERFLOW_OP(tdc.r ,+, tdc.i);
201    CHECK_OVERFLOW_OP(tdc.r ,-, tdc.i);
202    freqdata[0] = tdc.r + tdc.i;
203    freqdata[2*ncfft-1] = tdc.r - tdc.i;
204
205    for ( k=1;k <= ncfft/2 ; ++k )
206    {
207       /*fpk    = st->tmpbuf[k]; 
208       fpnk.r =   st->tmpbuf[ncfft-k].r;
209       fpnk.i = - st->tmpbuf[ncfft-k].i;
210       C_FIXDIV(fpk,2);
211       C_FIXDIV(fpnk,2);
212
213       C_ADD( f1k, fpk , fpnk );
214       C_SUB( f2k, fpk , fpnk );
215       
216       C_MUL( tw , f2k , st->super_twiddles[k]);
217
218       freqdata[2*k-1] = HALF_OF(f1k.r + tw.r);
219       freqdata[2*k] = HALF_OF(f1k.i + tw.i);
220       freqdata[2*(ncfft-k)-1] = HALF_OF(f1k.r - tw.r);
221       freqdata[2*(ncfft-k)] = HALF_OF(tw.i - f1k.i);
222       */
223
224       /*f1k.r = PSHR32(ADD32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1);
225       f1k.i = PSHR32(SUB32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1);
226       f2k.r = PSHR32(SUB32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1);
227       f2k.i = SHR32(ADD32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1);
228       
229       C_MUL( tw , f2k , st->super_twiddles[k]);
230
231       freqdata[2*k-1] = HALF_OF(f1k.r + tw.r);
232       freqdata[2*k] = HALF_OF(f1k.i + tw.i);
233       freqdata[2*(ncfft-k)-1] = HALF_OF(f1k.r - tw.r);
234       freqdata[2*(ncfft-k)] = HALF_OF(tw.i - f1k.i);
235    */
236       f2k.r = SHR32(SUB32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1);
237       f2k.i = PSHR32(ADD32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1);
238       
239       f1kr = SHL32(ADD32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),13);
240       f1ki = SHL32(SUB32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),13);
241       
242       twr = SHR32(SUB32(MULT16_16(f2k.r,st->super_twiddles[k].r),MULT16_16(f2k.i,st->super_twiddles[k].i)), 1);
243       twi = SHR32(ADD32(MULT16_16(f2k.i,st->super_twiddles[k].r),MULT16_16(f2k.r,st->super_twiddles[k].i)), 1);
244       
245 #ifdef FIXED_POINT
246       freqdata[2*k-1] = PSHR32(f1kr + twr, 15);
247       freqdata[2*k] = PSHR32(f1ki + twi, 15);
248       freqdata[2*(ncfft-k)-1] = PSHR32(f1kr - twr, 15);
249       freqdata[2*(ncfft-k)] = PSHR32(twi - f1ki, 15);
250 #else
251       freqdata[2*k-1] = .5f*(f1kr + twr);
252       freqdata[2*k] = .5f*(f1ki + twi);
253       freqdata[2*(ncfft-k)-1] = .5f*(f1kr - twr);
254       freqdata[2*(ncfft-k)] = .5f*(twi - f1ki);
255       
256 #endif
257    }
258 }
259
260 void kiss_fftri2(kiss_fftr_cfg st,const kiss_fft_scalar *freqdata,kiss_fft_scalar *timedata)
261 {
262    /* input buffer timedata is stored row-wise */
263    int k, ncfft;
264
265    if (st->substate->inverse == 0) {
266       speex_fatal ("kiss fft usage error: improper alloc\n");
267    }
268
269    ncfft = st->substate->nfft;
270
271    st->tmpbuf[0].r = freqdata[0] + freqdata[2*ncfft-1];
272    st->tmpbuf[0].i = freqdata[0] - freqdata[2*ncfft-1];
273    /*C_FIXDIV(st->tmpbuf[0],2);*/
274
275    for (k = 1; k <= ncfft / 2; ++k) {
276       kiss_fft_cpx fk, fnkc, fek, fok, tmp;
277       fk.r = freqdata[2*k-1];
278       fk.i = freqdata[2*k];
279       fnkc.r = freqdata[2*(ncfft - k)-1];
280       fnkc.i = -freqdata[2*(ncfft - k)];
281         /*C_FIXDIV( fk , 2 );
282       C_FIXDIV( fnkc , 2 );*/
283
284       C_ADD (fek, fk, fnkc);
285       C_SUB (tmp, fk, fnkc);
286       C_MUL (fok, tmp, st->super_twiddles[k]);
287       C_ADD (st->tmpbuf[k],     fek, fok);
288       C_SUB (st->tmpbuf[ncfft - k], fek, fok);
289 #ifdef USE_SIMD        
290       st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0);
291 #else
292       st->tmpbuf[ncfft - k].i *= -1;
293 #endif
294    }
295    kiss_fft (st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata);
296 }