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