kiss_fft.c

一个开源的sip源代码

        scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
        scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
        scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
        scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);

        C_ADD(*Fout2,scratch[11],scratch[12]);
        C_SUB(*Fout3,scratch[11],scratch[12]);

        ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
    }
}

/* perform the butterfly for one stage of a mixed radix FFT */
static void kf_bfly_generic(
        kiss_fft_cpx * Fout,
        const size_t fstride,
        const kiss_fft_cfg st,
        int m,
        int p
        )
{
    int u,k,q1,q;
    kiss_fft_cpx * twiddles = st->twiddles;
    kiss_fft_cpx t;
    kiss_fft_cpx scratchbuf[17];
    int Norig = st->nfft;

    /*CHECKBUF(scratchbuf,nscratchbuf,p);*/
    if (p>17)
       speex_error("KissFFT: max radix supported is 17");
    
    for ( u=0; u<m; ++u ) {
        k=u;
        for ( q1=0 ; q1<p ; ++q1 ) {
            scratchbuf[q1] = Fout[ k  ];
        if (!st->inverse) {
            C_FIXDIV(scratchbuf[q1],p);
	}
            k += m;
        }

        k=u;
        for ( q1=0 ; q1<p ; ++q1 ) {
            int twidx=0;
            Fout[ k ] = scratchbuf[0];
            for (q=1;q<p;++q ) {
                twidx += fstride * k;
                if (twidx>=Norig) twidx-=Norig;
                C_MUL(t,scratchbuf[q] , twiddles[twidx] );
                C_ADDTO( Fout[ k ] ,t);
            }
            k += m;
        }
    }
}
               
static
void kf_shuffle(
         kiss_fft_cpx * Fout,
         const kiss_fft_cpx * f,
         const size_t fstride,
         int in_stride,
         int * factors,
         const kiss_fft_cfg st
            )
{
   const int p=*factors++; /* the radix  */
   const int m=*factors++; /* stage's fft length/p */
   
    /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/
   if (m==1)
   {
      int j;
      for (j=0;j<p;j++)
      {
         Fout[j] = *f;
         f += fstride*in_stride;
      }
   } else {
      int j;
      for (j=0;j<p;j++)
      {
         kf_shuffle( Fout , f, fstride*p, in_stride, factors,st);
         f += fstride*in_stride;
         Fout += m;
      }
   }
}

static
void kf_work(
        kiss_fft_cpx * Fout,
        const kiss_fft_cpx * f,
        const size_t fstride,
        int in_stride,
        int * factors,
        const kiss_fft_cfg st,
        int N,
        int s2,
        int m2
        )
{
   int i;
    kiss_fft_cpx * Fout_beg=Fout;
    const int p=*factors++; /* the radix  */
    const int m=*factors++; /* stage's fft length/p */
#if 0
    /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/
    if (m==1)
    {
    /*   int j;
       for (j=0;j<p;j++)
       {
          Fout[j] = *f;
          f += fstride*in_stride;
       }*/
    } else {
       int j;
       for (j=0;j<p;j++)
       {
          kf_work( Fout , f, fstride*p, in_stride, factors,st, N*p, fstride*in_stride, m);
          f += fstride*in_stride;
          Fout += m;
       }
    }

    Fout=Fout_beg;

    switch (p) {
        case 2: kf_bfly2(Fout,fstride,st,m); break;
        case 3: kf_bfly3(Fout,fstride,st,m); break; 
        case 4: kf_bfly4(Fout,fstride,st,m); break;
        case 5: kf_bfly5(Fout,fstride,st,m); break; 
        default: kf_bfly_generic(Fout,fstride,st,m,p); break;
    }
#else
    /*printf ("fft %d %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N, m2);*/
    if (m==1) 
    {
       /*for (i=0;i<N;i++)
       {
          int j;
          Fout = Fout_beg+i*m2;
          const kiss_fft_cpx * f2 = f+i*s2;
          for (j=0;j<p;j++)
          {
             *Fout++ = *f2;
             f2 += fstride*in_stride;
          }
       }*/
    }else{
       kf_work( Fout , f, fstride*p, in_stride, factors,st, N*p, fstride*in_stride, m);
    }

    
       
       
       switch (p) {
          case 2: kf_bfly2(Fout,fstride,st,m, N, m2); break;
          case 3: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly3(Fout,fstride,st,m);} break; 
          case 4: kf_bfly4(Fout,fstride,st,m, N, m2); break;
          case 5: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly5(Fout,fstride,st,m);} break; 
          default: for (i=0;i<N;i++){Fout=Fout_beg+i*m2; kf_bfly_generic(Fout,fstride,st,m,p);} break;
    }    
#endif
}

/*  facbuf is populated by p1,m1,p2,m2, ...
    where 
    p[i] * m[i] = m[i-1]
    m0 = n                  */
static 
void kf_factor(int n,int * facbuf)
{
    int p=4;

    /*factor out powers of 4, powers of 2, then any remaining primes */
    do {
        while (n % p) {
            switch (p) {
                case 4: p = 2; break;
                case 2: p = 3; break;
                default: p += 2; break;
            }
            if (p>32000 || (spx_int32_t)p*(spx_int32_t)p > n)
                p = n;          /* no more factors, skip to end */
        }
        n /= p;
        *facbuf++ = p;
        *facbuf++ = n;
    } while (n > 1);
}
/*
 *
 * User-callable function to allocate all necessary storage space for the fft.
 *
 * The return value is a contiguous block of memory, allocated with malloc.  As such,
 * It can be freed with free(), rather than a kiss_fft-specific function.
 * */
kiss_fft_cfg kiss_fft_alloc(int nfft,int inverse_fft,void * mem,size_t * lenmem )
{
    kiss_fft_cfg st=NULL;
    size_t memneeded = sizeof(struct kiss_fft_state)
        + sizeof(kiss_fft_cpx)*(nfft-1); /* twiddle factors*/

    if ( lenmem==NULL ) {
        st = ( kiss_fft_cfg)KISS_FFT_MALLOC( memneeded );
    }else{
        if (mem != NULL && *lenmem >= memneeded)
            st = (kiss_fft_cfg)mem;
        *lenmem = memneeded;
    }
    if (st) {
        int i;
        st->nfft=nfft;
        st->inverse = inverse_fft;
#ifdef FIXED_POINT
        for (i=0;i<nfft;++i) {
            spx_word32_t phase = i;
            if (!st->inverse)
                phase = -phase;
            kf_cexp2(st->twiddles+i, DIV32(SHL32(phase,17),nfft));
        }
#else
        for (i=0;i<nfft;++i) {
           const double pi=3.14159265358979323846264338327;
           double phase = ( -2*pi /nfft ) * i;
           if (st->inverse)
              phase *= -1;
           kf_cexp(st->twiddles+i, phase );
        }
#endif
        kf_factor(nfft,st->factors);
    }
    return st;
}



    
void kiss_fft_stride(kiss_fft_cfg st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout,int in_stride)
{
    if (fin == fout) 
    {
       speex_error("In-place FFT not supported");
       /*CHECKBUF(tmpbuf,ntmpbuf,st->nfft);
       kf_work(tmpbuf,fin,1,in_stride, st->factors,st);
       speex_move(fout,tmpbuf,sizeof(kiss_fft_cpx)*st->nfft);*/
    } else {
       kf_shuffle( fout, fin, 1,in_stride, st->factors,st);
       kf_work( fout, fin, 1,in_stride, st->factors,st, 1, in_stride, 1);
    }
}

void kiss_fft(kiss_fft_cfg cfg,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
{
    kiss_fft_stride(cfg,fin,fout,1);
}