123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297 |
- #ifdef HAVE_CONFIG_H
- #include "config.h"
- #endif
- #include "os_support.h"
- #include "kiss_fftr.h"
- #include "_kiss_fft_guts.h"
- struct kiss_fftr_state{
- kiss_fft_cfg substate;
- kiss_fft_cpx * tmpbuf;
- kiss_fft_cpx * super_twiddles;
- #ifdef USE_SIMD
- long pad;
- #endif
- };
- kiss_fftr_cfg kiss_fftr_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem)
- {
- int i;
- kiss_fftr_cfg st = NULL;
- size_t subsize, memneeded;
- if (nfft & 1) {
- speex_warning("Real FFT optimization must be even.\n");
- return NULL;
- }
- nfft >>= 1;
- kiss_fft_alloc (nfft, inverse_fft, NULL, &subsize);
- memneeded = sizeof(struct kiss_fftr_state) + subsize + sizeof(kiss_fft_cpx) * ( nfft * 2);
- if (lenmem == NULL) {
- st = (kiss_fftr_cfg) KISS_FFT_MALLOC (memneeded);
- } else {
- if (*lenmem >= memneeded)
- st = (kiss_fftr_cfg) mem;
- *lenmem = memneeded;
- }
- if (!st)
- return NULL;
- st->substate = (kiss_fft_cfg) (st + 1);
- st->tmpbuf = (kiss_fft_cpx *) (((char *) st->substate) + subsize);
- st->super_twiddles = st->tmpbuf + nfft;
- kiss_fft_alloc(nfft, inverse_fft, st->substate, &subsize);
- #ifdef FIXED_POINT
- for (i=0;i<nfft;++i) {
- spx_word32_t phase = i+(nfft>>1);
- if (!inverse_fft)
- phase = -phase;
- kf_cexp2(st->super_twiddles+i, DIV32(SHL32(phase,16),nfft));
- }
- #else
- for (i=0;i<nfft;++i) {
- const double pi=3.14159265358979323846264338327;
- double phase = pi*(((double)i) /nfft + .5);
- if (!inverse_fft)
- phase = -phase;
- kf_cexp(st->super_twiddles+i, phase );
- }
- #endif
- return st;
- }
- void kiss_fftr(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_cpx *freqdata)
- {
-
- int k,ncfft;
- kiss_fft_cpx fpnk,fpk,f1k,f2k,tw,tdc;
- if ( st->substate->inverse) {
- speex_fatal("kiss fft usage error: improper alloc\n");
- }
- ncfft = st->substate->nfft;
-
- kiss_fft( st->substate , (const kiss_fft_cpx*)timedata, st->tmpbuf );
-
-
- tdc.r = st->tmpbuf[0].r;
- tdc.i = st->tmpbuf[0].i;
- C_FIXDIV(tdc,2);
- CHECK_OVERFLOW_OP(tdc.r ,+, tdc.i);
- CHECK_OVERFLOW_OP(tdc.r ,-, tdc.i);
- freqdata[0].r = tdc.r + tdc.i;
- freqdata[ncfft].r = tdc.r - tdc.i;
- #ifdef USE_SIMD
- freqdata[ncfft].i = freqdata[0].i = _mm_set1_ps(0);
- #else
- freqdata[ncfft].i = freqdata[0].i = 0;
- #endif
- for ( k=1;k <= ncfft/2 ; ++k ) {
- fpk = st->tmpbuf[k];
- fpnk.r = st->tmpbuf[ncfft-k].r;
- fpnk.i = - st->tmpbuf[ncfft-k].i;
- C_FIXDIV(fpk,2);
- C_FIXDIV(fpnk,2);
- C_ADD( f1k, fpk , fpnk );
- C_SUB( f2k, fpk , fpnk );
- C_MUL( tw , f2k , st->super_twiddles[k]);
- freqdata[k].r = HALF_OF(f1k.r + tw.r);
- freqdata[k].i = HALF_OF(f1k.i + tw.i);
- freqdata[ncfft-k].r = HALF_OF(f1k.r - tw.r);
- freqdata[ncfft-k].i = HALF_OF(tw.i - f1k.i);
- }
- }
- void kiss_fftri(kiss_fftr_cfg st,const kiss_fft_cpx *freqdata, kiss_fft_scalar *timedata)
- {
-
- int k, ncfft;
- if (st->substate->inverse == 0) {
- speex_fatal("kiss fft usage error: improper alloc\n");
- }
- ncfft = st->substate->nfft;
- st->tmpbuf[0].r = freqdata[0].r + freqdata[ncfft].r;
- st->tmpbuf[0].i = freqdata[0].r - freqdata[ncfft].r;
-
- for (k = 1; k <= ncfft / 2; ++k) {
- kiss_fft_cpx fk, fnkc, fek, fok, tmp;
- fk = freqdata[k];
- fnkc.r = freqdata[ncfft - k].r;
- fnkc.i = -freqdata[ncfft - k].i;
-
- C_ADD (fek, fk, fnkc);
- C_SUB (tmp, fk, fnkc);
- C_MUL (fok, tmp, st->super_twiddles[k]);
- C_ADD (st->tmpbuf[k], fek, fok);
- C_SUB (st->tmpbuf[ncfft - k], fek, fok);
- #ifdef USE_SIMD
- st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0);
- #else
- st->tmpbuf[ncfft - k].i *= -1;
- #endif
- }
- kiss_fft (st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata);
- }
- void kiss_fftr2(kiss_fftr_cfg st,const kiss_fft_scalar *timedata,kiss_fft_scalar *freqdata)
- {
-
- int k,ncfft;
- kiss_fft_cpx f2k,tdc;
- spx_word32_t f1kr, f1ki, twr, twi;
- if ( st->substate->inverse) {
- speex_fatal("kiss fft usage error: improper alloc\n");
- }
- ncfft = st->substate->nfft;
-
- kiss_fft( st->substate , (const kiss_fft_cpx*)timedata, st->tmpbuf );
-
-
- tdc.r = st->tmpbuf[0].r;
- tdc.i = st->tmpbuf[0].i;
- C_FIXDIV(tdc,2);
- CHECK_OVERFLOW_OP(tdc.r ,+, tdc.i);
- CHECK_OVERFLOW_OP(tdc.r ,-, tdc.i);
- freqdata[0] = tdc.r + tdc.i;
- freqdata[2*ncfft-1] = tdc.r - tdc.i;
- for ( k=1;k <= ncfft/2 ; ++k )
- {
-
-
- f2k.r = SHR32(SUB32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),1);
- f2k.i = PSHR32(ADD32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),1);
-
- f1kr = SHL32(ADD32(EXTEND32(st->tmpbuf[k].r), EXTEND32(st->tmpbuf[ncfft-k].r)),13);
- f1ki = SHL32(SUB32(EXTEND32(st->tmpbuf[k].i), EXTEND32(st->tmpbuf[ncfft-k].i)),13);
-
- twr = SHR32(SUB32(MULT16_16(f2k.r,st->super_twiddles[k].r),MULT16_16(f2k.i,st->super_twiddles[k].i)), 1);
- twi = SHR32(ADD32(MULT16_16(f2k.i,st->super_twiddles[k].r),MULT16_16(f2k.r,st->super_twiddles[k].i)), 1);
-
- #ifdef FIXED_POINT
- freqdata[2*k-1] = PSHR32(f1kr + twr, 15);
- freqdata[2*k] = PSHR32(f1ki + twi, 15);
- freqdata[2*(ncfft-k)-1] = PSHR32(f1kr - twr, 15);
- freqdata[2*(ncfft-k)] = PSHR32(twi - f1ki, 15);
- #else
- freqdata[2*k-1] = .5f*(f1kr + twr);
- freqdata[2*k] = .5f*(f1ki + twi);
- freqdata[2*(ncfft-k)-1] = .5f*(f1kr - twr);
- freqdata[2*(ncfft-k)] = .5f*(twi - f1ki);
-
- #endif
- }
- }
- void kiss_fftri2(kiss_fftr_cfg st,const kiss_fft_scalar *freqdata,kiss_fft_scalar *timedata)
- {
-
- int k, ncfft;
- if (st->substate->inverse == 0) {
- speex_fatal ("kiss fft usage error: improper alloc\n");
- }
- ncfft = st->substate->nfft;
- st->tmpbuf[0].r = freqdata[0] + freqdata[2*ncfft-1];
- st->tmpbuf[0].i = freqdata[0] - freqdata[2*ncfft-1];
-
- for (k = 1; k <= ncfft / 2; ++k) {
- kiss_fft_cpx fk, fnkc, fek, fok, tmp;
- fk.r = freqdata[2*k-1];
- fk.i = freqdata[2*k];
- fnkc.r = freqdata[2*(ncfft - k)-1];
- fnkc.i = -freqdata[2*(ncfft - k)];
-
- C_ADD (fek, fk, fnkc);
- C_SUB (tmp, fk, fnkc);
- C_MUL (fok, tmp, st->super_twiddles[k]);
- C_ADD (st->tmpbuf[k], fek, fok);
- C_SUB (st->tmpbuf[ncfft - k], fek, fok);
- #ifdef USE_SIMD
- st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0);
- #else
- st->tmpbuf[ncfft - k].i *= -1;
- #endif
- }
- kiss_fft (st->substate, st->tmpbuf, (kiss_fft_cpx *) timedata);
- }
|