arm_cfft_radix2_q31.c

00001 /* ----------------------------------------------------------------------   
00002 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.   
00003 *   
00004 * $Date:        17. January 2013  
00005 * $Revision:    V1.4.1  
00006 *   
00007 * Project:      CMSIS DSP Library   
00008 * Title:        arm_cfft_radix2_q31.c   
00009 *   
00010 * Description:  Radix-2 Decimation in Frequency CFFT & CIFFT Fixed point processing function   
00011 *   
00012 *   
00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00014 *  
00015 * Redistribution and use in source and binary forms, with or without 
00016 * modification, are permitted provided that the following conditions
00017 * are met:
00018 *   - Redistributions of source code must retain the above copyright
00019 *     notice, this list of conditions and the following disclaimer.
00020 *   - Redistributions in binary form must reproduce the above copyright
00021 *     notice, this list of conditions and the following disclaimer in
00022 *     the documentation and/or other materials provided with the 
00023 *     distribution.
00024 *   - Neither the name of ARM LIMITED nor the names of its contributors
00025 *     may be used to endorse or promote products derived from this
00026 *     software without specific prior written permission.
00027 *
00028 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00029 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00030 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00031 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00032 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00033 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00034 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00036 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00037 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00038 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00039 * POSSIBILITY OF SUCH DAMAGE.    
00040 * -------------------------------------------------------------------- */
00041 
00042 #include "arm_math.h"
00043 
00044 void arm_radix2_butterfly_q31(
00045   q31_t * pSrc,
00046   uint32_t fftLen,
00047   q31_t * pCoef,
00048   uint16_t twidCoefModifier);
00049 
00050 void arm_radix2_butterfly_inverse_q31(
00051   q31_t * pSrc,
00052   uint32_t fftLen,
00053   q31_t * pCoef,
00054   uint16_t twidCoefModifier);
00055 
00056 void arm_bitreversal_q31(
00057   q31_t * pSrc,
00058   uint32_t fftLen,
00059   uint16_t bitRevFactor,
00060   uint16_t * pBitRevTab);
00061 
00062 /**   
00063 * @ingroup groupTransforms   
00064 */
00065 
00066 /**   
00067 * @addtogroup ComplexFFT   
00068 * @{   
00069 */
00070 
00071 /**   
00072 * @details   
00073 * @brief Processing function for the fixed-point CFFT/CIFFT.  
00074 * @param[in]      *S    points to an instance of the fixed-point CFFT/CIFFT structure.  
00075 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.  
00076 * @return none.  
00077 */
00078 
00079 void arm_cfft_radix2_q31(
00080 const arm_cfft_radix2_instance_q31 * S,
00081 q31_t * pSrc)
00082 {
00083 
00084    if(S->ifftFlag == 1u)
00085    {
00086       arm_radix2_butterfly_inverse_q31(pSrc, S->fftLen,
00087       S->pTwiddle, S->twidCoefModifier);
00088    }
00089    else
00090    {
00091       arm_radix2_butterfly_q31(pSrc, S->fftLen,
00092       S->pTwiddle, S->twidCoefModifier);
00093    }
00094 
00095    arm_bitreversal_q31(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
00096 }
00097 
00098 /**   
00099 * @} end of ComplexFFT group   
00100 */
00101 
00102 void arm_radix2_butterfly_q31(
00103 q31_t * pSrc,
00104 uint32_t fftLen,
00105 q31_t * pCoef,
00106 uint16_t twidCoefModifier)
00107 {
00108 
00109    unsigned i, j, k, l, m;
00110    unsigned n1, n2, ia;
00111    q31_t xt, yt, cosVal, sinVal;
00112    q31_t p0, p1;
00113 
00114    //N = fftLen; 
00115    n2 = fftLen;
00116 
00117    n1 = n2;
00118    n2 = n2 >> 1;
00119    ia = 0;
00120 
00121    // loop for groups 
00122    for (i = 0; i < n2; i++)
00123    {
00124       cosVal = pCoef[ia * 2];
00125       sinVal = pCoef[(ia * 2) + 1];
00126       ia = ia + twidCoefModifier;
00127 
00128       l = i + n2;
00129       xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
00130       pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
00131       
00132       yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
00133       pSrc[2 * i + 1] =
00134         ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
00135 
00136       mult_32x32_keep32_R(p0, xt, cosVal);
00137       mult_32x32_keep32_R(p1, yt, cosVal);
00138       multAcc_32x32_keep32_R(p0, yt, sinVal); 
00139       multSub_32x32_keep32_R(p1, xt, sinVal);
00140       
00141       pSrc[2u * l] = p0;
00142       pSrc[2u * l + 1u] = p1;
00143 
00144    }                             // groups loop end 
00145 
00146    twidCoefModifier <<= 1u;
00147 
00148    // loop for stage 
00149    for (k = fftLen / 2; k > 2; k = k >> 1)
00150    {
00151       n1 = n2;
00152       n2 = n2 >> 1;
00153       ia = 0;
00154 
00155       // loop for groups 
00156       for (j = 0; j < n2; j++)
00157       {
00158          cosVal = pCoef[ia * 2];
00159          sinVal = pCoef[(ia * 2) + 1];
00160          ia = ia + twidCoefModifier;
00161 
00162          // loop for butterfly 
00163          i = j;
00164          m = fftLen / n1;
00165          do
00166          {
00167             l = i + n2;
00168             xt = pSrc[2 * i] - pSrc[2 * l];
00169             pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
00170             
00171             yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
00172             pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
00173 
00174             mult_32x32_keep32_R(p0, xt, cosVal);
00175             mult_32x32_keep32_R(p1, yt, cosVal);
00176             multAcc_32x32_keep32_R(p0, yt, sinVal);
00177             multSub_32x32_keep32_R(p1, xt, sinVal);
00178             
00179             pSrc[2u * l] = p0;
00180             pSrc[2u * l + 1u] = p1;
00181             i += n1;
00182             m--;
00183          } while( m > 0);                   // butterfly loop end 
00184 
00185       }                           // groups loop end 
00186 
00187       twidCoefModifier <<= 1u;
00188    }                             // stages loop end 
00189 
00190    n1 = n2;
00191    n2 = n2 >> 1;
00192    ia = 0;
00193 
00194    cosVal = pCoef[ia * 2];
00195    sinVal = pCoef[(ia * 2) + 1];
00196    ia = ia + twidCoefModifier;
00197 
00198    // loop for butterfly 
00199    for (i = 0; i < fftLen; i += n1)
00200    {
00201       l = i + n2;
00202       xt = pSrc[2 * i] - pSrc[2 * l];
00203       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
00204 
00205       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
00206       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
00207 
00208       pSrc[2u * l] = xt;
00209 
00210       pSrc[2u * l + 1u] = yt;
00211 
00212       i += n1;
00213       l = i + n2;
00214 
00215       xt = pSrc[2 * i] - pSrc[2 * l];
00216       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
00217 
00218       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
00219       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
00220 
00221       pSrc[2u * l] = xt;
00222 
00223       pSrc[2u * l + 1u] = yt;
00224 
00225    }                             // butterfly loop end 
00226 
00227 }
00228 
00229 
00230 void arm_radix2_butterfly_inverse_q31(
00231 q31_t * pSrc,
00232 uint32_t fftLen,
00233 q31_t * pCoef,
00234 uint16_t twidCoefModifier)
00235 {
00236 
00237    unsigned i, j, k, l;
00238    unsigned n1, n2, ia;
00239    q31_t xt, yt, cosVal, sinVal;
00240    q31_t p0, p1;
00241 
00242    //N = fftLen; 
00243    n2 = fftLen;
00244 
00245    n1 = n2;
00246    n2 = n2 >> 1;
00247    ia = 0;
00248 
00249    // loop for groups 
00250    for (i = 0; i < n2; i++)
00251    {
00252       cosVal = pCoef[ia * 2];
00253       sinVal = pCoef[(ia * 2) + 1];
00254       ia = ia + twidCoefModifier;
00255 
00256       l = i + n2;
00257       xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
00258       pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
00259       
00260       yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
00261       pSrc[2 * i + 1] =
00262         ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
00263 
00264       mult_32x32_keep32_R(p0, xt, cosVal);
00265       mult_32x32_keep32_R(p1, yt, cosVal);
00266       multSub_32x32_keep32_R(p0, yt, sinVal);
00267       multAcc_32x32_keep32_R(p1, xt, sinVal);
00268       
00269       pSrc[2u * l] = p0;
00270       pSrc[2u * l + 1u] = p1;
00271    }                             // groups loop end 
00272 
00273    twidCoefModifier = twidCoefModifier << 1u;
00274 
00275    // loop for stage 
00276    for (k = fftLen / 2; k > 2; k = k >> 1)
00277    {
00278       n1 = n2;
00279       n2 = n2 >> 1;
00280       ia = 0;
00281 
00282       // loop for groups 
00283       for (j = 0; j < n2; j++)
00284       {
00285          cosVal = pCoef[ia * 2];
00286          sinVal = pCoef[(ia * 2) + 1];
00287          ia = ia + twidCoefModifier;
00288 
00289          // loop for butterfly 
00290          for (i = j; i < fftLen; i += n1)
00291          {
00292             l = i + n2;
00293             xt = pSrc[2 * i] - pSrc[2 * l];
00294             pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
00295       
00296             yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
00297             pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
00298 
00299             mult_32x32_keep32_R(p0, xt, cosVal);
00300             mult_32x32_keep32_R(p1, yt, cosVal);
00301             multSub_32x32_keep32_R(p0, yt, sinVal);
00302             multAcc_32x32_keep32_R(p1, xt, sinVal);
00303             
00304             pSrc[2u * l] = p0;
00305             pSrc[2u * l + 1u] = p1;
00306          }                         // butterfly loop end 
00307 
00308       }                           // groups loop end 
00309 
00310       twidCoefModifier = twidCoefModifier << 1u;
00311    }                             // stages loop end 
00312 
00313    n1 = n2;
00314    n2 = n2 >> 1;
00315    ia = 0;
00316 
00317    cosVal = pCoef[ia * 2];
00318    sinVal = pCoef[(ia * 2) + 1];
00319    ia = ia + twidCoefModifier;
00320 
00321    // loop for butterfly 
00322    for (i = 0; i < fftLen; i += n1)
00323    {
00324       l = i + n2;
00325       xt = pSrc[2 * i] - pSrc[2 * l];
00326       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
00327 
00328       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
00329       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
00330 
00331       pSrc[2u * l] = xt;
00332 
00333       pSrc[2u * l + 1u] = yt;
00334 
00335       i += n1;
00336       l = i + n2;
00337 
00338       xt = pSrc[2 * i] - pSrc[2 * l];
00339       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
00340 
00341       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
00342       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
00343 
00344       pSrc[2u * l] = xt;
00345 
00346       pSrc[2u * l + 1u] = yt;
00347 
00348    }                             // butterfly loop end 
00349 
00350 }