arm_rfft_q15.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_rfft_q15.c    
00009 *    
00010 * Description:  RFFT & RIFFT Q15 process 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_radix4_butterfly_q15(
00045   q15_t * pSrc16,
00046   uint32_t fftLen,
00047   q15_t * pCoef16,
00048   uint32_t twidCoefModifier);
00049 
00050 void arm_radix4_butterfly_inverse_q15(
00051   q15_t * pSrc16,
00052   uint32_t fftLen,
00053   q15_t * pCoef16,
00054   uint32_t twidCoefModifier);
00055 
00056 void arm_bitreversal_q15(
00057   q15_t * pSrc,
00058   uint32_t fftLen,
00059   uint16_t bitRevFactor,
00060   uint16_t * pBitRevTab);
00061     
00062     /*--------------------------------------------------------------------    
00063 *       Internal functions prototypes    
00064 --------------------------------------------------------------------*/
00065 
00066 void arm_split_rfft_q15(
00067   q15_t * pSrc,
00068   uint32_t fftLen,
00069   q15_t * pATable,
00070   q15_t * pBTable,
00071   q15_t * pDst,
00072   uint32_t modifier);
00073 
00074 void arm_split_rifft_q15(
00075   q15_t * pSrc,
00076   uint32_t fftLen,
00077   q15_t * pATable,
00078   q15_t * pBTable,
00079   q15_t * pDst,
00080   uint32_t modifier);
00081 
00082 /**    
00083  * @addtogroup RealFFT    
00084  * @{    
00085  */
00086 
00087 /**    
00088  * @brief Processing function for the Q15 RFFT/RIFFT.   
00089  * @param[in]  *S    points to an instance of the Q15 RFFT/RIFFT structure.   
00090  * @param[in]  *pSrc points to the input buffer.   
00091  * @param[out] *pDst points to the output buffer.   
00092  * @return none.   
00093  *    
00094  * \par Input an output formats:   
00095  * \par    
00096  * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.    
00097  * Hence the output format is different for different RFFT sizes.    
00098  * The input and output formats for different RFFT sizes and number of bits to upscale are mentioned in the tables below for RFFT and RIFFT:   
00099  * \par    
00100  * \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT"    
00101  * \par    
00102  * \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT"    
00103  */
00104 
00105 void arm_rfft_q15(
00106   const arm_rfft_instance_q15 * S,
00107   q15_t * pSrc,
00108   q15_t * pDst)
00109 {
00110   const arm_cfft_radix4_instance_q15 *S_CFFT = S->pCfft;
00111 
00112   /* Calculation of RIFFT of input */
00113   if(S->ifftFlagR == 1u)
00114   {
00115     /*  Real IFFT core process */
00116     arm_split_rifft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal,
00117                         S->pTwiddleBReal, pDst, S->twidCoefRModifier);
00118 
00119     /* Complex readix-4 IFFT process */
00120     arm_radix4_butterfly_inverse_q15(pDst, S_CFFT->fftLen,
00121                                      S_CFFT->pTwiddle,
00122                                      S_CFFT->twidCoefModifier);
00123 
00124     /* Bit reversal process */
00125     if(S->bitReverseFlagR == 1u)
00126     {
00127       arm_bitreversal_q15(pDst, S_CFFT->fftLen,
00128                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
00129     }
00130   }
00131   else
00132   {
00133     /* Calculation of RFFT of input */
00134 
00135     /* Complex readix-4 FFT process */
00136     arm_radix4_butterfly_q15(pSrc, S_CFFT->fftLen,
00137                              S_CFFT->pTwiddle, S_CFFT->twidCoefModifier);
00138 
00139     /* Bit reversal process */
00140     if(S->bitReverseFlagR == 1u)
00141     {
00142       arm_bitreversal_q15(pSrc, S_CFFT->fftLen,
00143                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
00144     }
00145 
00146     arm_split_rfft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal,
00147                        S->pTwiddleBReal, pDst, S->twidCoefRModifier);
00148   }
00149 
00150 }
00151 
00152   /**    
00153    * @} end of RealFFT group    
00154    */
00155 
00156 /**    
00157  * @brief  Core Real FFT process    
00158  * @param  *pSrc                points to the input buffer.   
00159  * @param  fftLen               length of FFT.   
00160  * @param  *pATable             points to the A twiddle Coef buffer.    
00161  * @param  *pBTable             points to the B twiddle Coef buffer.   
00162  * @param  *pDst                points to the output buffer.   
00163  * @param  modifier             twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.   
00164  * @return none.    
00165  * The function implements a Real FFT    
00166  */
00167 
00168 void arm_split_rfft_q15(
00169   q15_t * pSrc,
00170   uint32_t fftLen,
00171   q15_t * pATable,
00172   q15_t * pBTable,
00173   q15_t * pDst,
00174   uint32_t modifier)
00175 {
00176   uint32_t i;                                    /* Loop Counter */
00177   q31_t outR, outI;                              /* Temporary variables for output */
00178   q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
00179   q15_t *pSrc1, *pSrc2;
00180 
00181 
00182 //  pSrc[2u * fftLen] = pSrc[0]; 
00183 //  pSrc[(2u * fftLen) + 1u] = pSrc[1]; 
00184 
00185   pCoefA = &pATable[modifier * 2u];
00186   pCoefB = &pBTable[modifier * 2u];
00187 
00188   pSrc1 = &pSrc[2];
00189   pSrc2 = &pSrc[(2u * fftLen) - 2u];
00190 
00191 #ifndef ARM_MATH_CM0_FAMILY
00192 
00193   /* Run the below code for Cortex-M4 and Cortex-M3 */
00194 
00195   i = 1u;
00196 
00197   while(i < fftLen)
00198   {
00199     /*    
00200        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]    
00201        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
00202        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00203      */
00204 
00205     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +    
00206        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00207        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
00208 
00209 
00210 #ifndef ARM_MATH_BIG_ENDIAN
00211 
00212     /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
00213     outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA));
00214 
00215 #else
00216 
00217     /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
00218     outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)));
00219 
00220 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00221 
00222     /* pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
00223        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
00224     outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 15u;
00225 
00226     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00227        pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
00228 
00229 #ifndef ARM_MATH_BIG_ENDIAN
00230 
00231     outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
00232 
00233 #else
00234 
00235     outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--);
00236 
00237 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00238 
00239     /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
00240     outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI);
00241 
00242     /* write output */
00243     pDst[2u * i] = (q15_t) outR;
00244     pDst[(2u * i) + 1u] = outI >> 15u;
00245 
00246     /* write complex conjugate output */
00247     pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
00248     pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 15u);
00249 
00250     /* update coefficient pointer */
00251     pCoefB = pCoefB + (2u * modifier);
00252     pCoefA = pCoefA + (2u * modifier);
00253 
00254     i++;
00255 
00256   }
00257 
00258   pDst[2u * fftLen] = pSrc[0] - pSrc[1];
00259   pDst[(2u * fftLen) + 1u] = 0;
00260 
00261   pDst[0] = pSrc[0] + pSrc[1];
00262   pDst[1] = 0;
00263 
00264 
00265 #else
00266 
00267   /* Run the below code for Cortex-M0 */
00268 
00269   i = 1u;
00270 
00271   while(i < fftLen)
00272   {
00273     /*    
00274        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]    
00275        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
00276        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00277      */
00278 
00279     outR = *pSrc1 * *pCoefA;
00280     outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1));
00281     outR = outR + (*pSrc2 * *pCoefB);
00282     outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 15;
00283 
00284 
00285     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +    
00286        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00287        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);   
00288      */
00289 
00290     outI = *pSrc2 * *(pCoefB + 1);
00291     outI = outI - (*(pSrc2 + 1) * *pCoefB);
00292     outI = outI + (*(pSrc1 + 1) * *pCoefA);
00293     outI = outI + (*pSrc1 * *(pCoefA + 1));
00294 
00295     /* update input pointers */
00296     pSrc1 += 2u;
00297     pSrc2 -= 2u;
00298 
00299     /* write output */
00300     pDst[2u * i] = (q15_t) outR;
00301     pDst[(2u * i) + 1u] = outI >> 15u;
00302 
00303     /* write complex conjugate output */
00304     pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
00305     pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 15u);
00306 
00307     /* update coefficient pointer */
00308     pCoefB = pCoefB + (2u * modifier);
00309     pCoefA = pCoefA + (2u * modifier);
00310 
00311     i++;
00312 
00313   }
00314 
00315   pDst[2u * fftLen] = pSrc[0] - pSrc[1];
00316   pDst[(2u * fftLen) + 1u] = 0;
00317 
00318   pDst[0] = pSrc[0] + pSrc[1];
00319   pDst[1] = 0;
00320 
00321 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
00322 
00323 }
00324 
00325 
00326 /**    
00327  * @brief  Core Real IFFT process    
00328  * @param[in]   *pSrc               points to the input buffer.    
00329  * @param[in]   fftLen              length of FFT.   
00330  * @param[in]   *pATable            points to the twiddle Coef A buffer.   
00331  * @param[in]   *pBTable            points to the twiddle Coef B buffer.    
00332  * @param[out]  *pDst               points to the output buffer.   
00333  * @param[in]   modifier            twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.   
00334  * @return none.    
00335  * The function implements a Real IFFT    
00336  */
00337 void arm_split_rifft_q15(
00338   q15_t * pSrc,
00339   uint32_t fftLen,
00340   q15_t * pATable,
00341   q15_t * pBTable,
00342   q15_t * pDst,
00343   uint32_t modifier)
00344 {
00345   uint32_t i;                                    /* Loop Counter */
00346   q31_t outR, outI;                              /* Temporary variables for output */
00347   q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
00348   q15_t *pSrc1, *pSrc2;
00349   q15_t *pDst1 = &pDst[0];
00350 
00351   pCoefA = &pATable[0];
00352   pCoefB = &pBTable[0];
00353 
00354   pSrc1 = &pSrc[0];
00355   pSrc2 = &pSrc[2u * fftLen];
00356 
00357 #ifndef ARM_MATH_CM0_FAMILY
00358 
00359   /* Run the below code for Cortex-M4 and Cortex-M3 */
00360 
00361   i = fftLen;
00362 
00363   while(i > 0u)
00364   {
00365 
00366     /*    
00367        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
00368        pIn[2 * n - 2 * i] * pBTable[2 * i] -    
00369        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00370 
00371        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -    
00372        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00373        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);    
00374 
00375      */
00376 
00377 
00378 #ifndef ARM_MATH_BIG_ENDIAN
00379 
00380     /* pIn[2 * n - 2 * i] * pBTable[2 * i] -    
00381        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
00382     outR = __SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB));
00383 
00384 #else
00385 
00386     /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +  
00387        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
00388     outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)));
00389 
00390 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00391 
00392     /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
00393        pIn[2 * n - 2 * i] * pBTable[2 * i] */
00394     outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 15u;
00395 
00396     /*    
00397        -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +    
00398        pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
00399     outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
00400 
00401     /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
00402 
00403 #ifndef ARM_MATH_BIG_ENDIAN
00404 
00405     outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI);
00406 
00407 #else
00408 
00409     outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI);
00410 
00411 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00412     /* write output */
00413 
00414 #ifndef ARM_MATH_BIG_ENDIAN
00415 
00416     *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 15u), 16);
00417 
00418 #else
00419 
00420     *__SIMD32(pDst1)++ = __PKHBT((outI >> 15u), outR, 16);
00421 
00422 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00423 
00424     /* update coefficient pointer */
00425     pCoefB = pCoefB + (2u * modifier);
00426     pCoefA = pCoefA + (2u * modifier);
00427 
00428     i--;
00429 
00430   }
00431 
00432 
00433 #else
00434 
00435   /* Run the below code for Cortex-M0 */
00436 
00437   i = fftLen;
00438 
00439   while(i > 0u)
00440   {
00441 
00442     /*    
00443        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
00444        pIn[2 * n - 2 * i] * pBTable[2 * i] -    
00445        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00446      */
00447 
00448     outR = *pSrc2 * *pCoefB;
00449     outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1));
00450     outR = outR + (*pSrc1 * *pCoefA);
00451     outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 15;
00452 
00453     /*   
00454        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -   
00455        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -   
00456        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);   
00457      */
00458 
00459     outI = *(pSrc1 + 1) * *pCoefA;
00460     outI = outI - (*pSrc1 * *(pCoefA + 1));
00461     outI = outI - (*pSrc2 * *(pCoefB + 1));
00462     outI = outI - (*(pSrc2 + 1) * *(pCoefB));
00463 
00464     /* update input pointers */
00465     pSrc1 += 2u;
00466     pSrc2 -= 2u;
00467 
00468     /* write output */
00469     *pDst1++ = (q15_t) outR;
00470     *pDst1++ = (q15_t) (outI >> 15);
00471 
00472     /* update coefficient pointer */
00473     pCoefB = pCoefB + (2u * modifier);
00474     pCoefA = pCoefA + (2u * modifier);
00475 
00476     i--;
00477 
00478   }
00479 
00480 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
00481 
00482 }