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