arm_rfft_q15.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        12. March 2014  
00005 * $Revision:    V1.4.3  
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   uint32_t i;
00112 
00113   /* Calculation of RIFFT of input */
00114   if(S->ifftFlagR == 1u)
00115   {
00116     /*  Real IFFT core process */
00117     arm_split_rifft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal,
00118                         S->pTwiddleBReal, pDst, S->twidCoefRModifier);
00119 
00120     /* Complex readix-4 IFFT process */
00121     arm_radix4_butterfly_inverse_q15(pDst, S_CFFT->fftLen,
00122                                      S_CFFT->pTwiddle,
00123                                      S_CFFT->twidCoefModifier);
00124 
00125     /* Bit reversal process */
00126     if(S->bitReverseFlagR == 1u)
00127     {
00128       arm_bitreversal_q15(pDst, S_CFFT->fftLen,
00129                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
00130     }
00131     
00132     for(i=0;i<S->fftLenReal;i++)
00133     {
00134       pDst[i] = pDst[i] << 1;
00135     }
00136   }
00137   else
00138   {
00139     /* Calculation of RFFT of input */
00140 
00141     /* Complex readix-4 FFT process */
00142     arm_radix4_butterfly_q15(pSrc, S_CFFT->fftLen,
00143                              S_CFFT->pTwiddle, S_CFFT->twidCoefModifier);
00144 
00145     /* Bit reversal process */
00146     if(S->bitReverseFlagR == 1u)
00147     {
00148       arm_bitreversal_q15(pSrc, S_CFFT->fftLen,
00149                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
00150     }
00151 
00152     arm_split_rfft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal,
00153                        S->pTwiddleBReal, pDst, S->twidCoefRModifier);
00154   }
00155 
00156 }
00157 
00158   /**    
00159    * @} end of RealFFT group    
00160    */
00161 
00162 /**    
00163  * @brief  Core Real FFT process    
00164  * @param  *pSrc                points to the input buffer.   
00165  * @param  fftLen               length of FFT.   
00166  * @param  *pATable             points to the A twiddle Coef buffer.    
00167  * @param  *pBTable             points to the B twiddle Coef buffer.   
00168  * @param  *pDst                points to the output buffer.   
00169  * @param  modifier             twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.   
00170  * @return none.    
00171  * The function implements a Real FFT    
00172  */
00173 
00174 void arm_split_rfft_q15(
00175   q15_t * pSrc,
00176   uint32_t fftLen,
00177   q15_t * pATable,
00178   q15_t * pBTable,
00179   q15_t * pDst,
00180   uint32_t modifier)
00181 {
00182   uint32_t i;                                    /* Loop Counter */
00183   q31_t outR, outI;                              /* Temporary variables for output */
00184   q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
00185   q15_t *pSrc1, *pSrc2;
00186 
00187 
00188 //  pSrc[2u * fftLen] = pSrc[0]; 
00189 //  pSrc[(2u * fftLen) + 1u] = pSrc[1]; 
00190 
00191   pCoefA = &pATable[modifier * 2u];
00192   pCoefB = &pBTable[modifier * 2u];
00193 
00194   pSrc1 = &pSrc[2];
00195   pSrc2 = &pSrc[(2u * fftLen) - 2u];
00196 
00197 #ifndef ARM_MATH_CM0_FAMILY
00198 
00199   /* Run the below code for Cortex-M4 and Cortex-M3 */
00200 
00201   i = 1u;
00202 
00203   while(i < fftLen)
00204   {
00205     /*    
00206        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]    
00207        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
00208        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00209      */
00210 
00211     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +    
00212        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00213        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
00214 
00215 
00216 #ifndef ARM_MATH_BIG_ENDIAN
00217 
00218     /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
00219     outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA));
00220 
00221 #else
00222 
00223     /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
00224     outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)));
00225 
00226 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00227 
00228     /* pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
00229        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
00230     outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 16u;
00231 
00232     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00233        pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
00234 
00235 #ifndef ARM_MATH_BIG_ENDIAN
00236 
00237     outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
00238 
00239 #else
00240 
00241     outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--);
00242 
00243 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00244 
00245     /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
00246     outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI);
00247 
00248     /* write output */
00249     pDst[2u * i] = (q15_t) outR;
00250     pDst[(2u * i) + 1u] = outI >> 16u;
00251 
00252     /* write complex conjugate output */
00253     pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
00254     pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 16u);
00255 
00256     /* update coefficient pointer */
00257     pCoefB = pCoefB + (2u * modifier);
00258     pCoefA = pCoefA + (2u * modifier);
00259 
00260     i++;
00261 
00262   }
00263 
00264   pDst[2u * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
00265   pDst[(2u * fftLen) + 1u] = 0;
00266 
00267   pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
00268   pDst[1] = 0;
00269 
00270 
00271 #else
00272 
00273   /* Run the below code for Cortex-M0 */
00274 
00275   i = 1u;
00276 
00277   while(i < fftLen)
00278   {
00279     /*    
00280        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]    
00281        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +    
00282        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00283      */
00284 
00285     outR = *pSrc1 * *pCoefA;
00286     outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1));
00287     outR = outR + (*pSrc2 * *pCoefB);
00288     outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 16;
00289 
00290 
00291     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +    
00292        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00293        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);   
00294      */
00295 
00296     outI = *pSrc2 * *(pCoefB + 1);
00297     outI = outI - (*(pSrc2 + 1) * *pCoefB);
00298     outI = outI + (*(pSrc1 + 1) * *pCoefA);
00299     outI = outI + (*pSrc1 * *(pCoefA + 1));
00300 
00301     /* update input pointers */
00302     pSrc1 += 2u;
00303     pSrc2 -= 2u;
00304 
00305     /* write output */
00306     pDst[2u * i] = (q15_t) outR;
00307     pDst[(2u * i) + 1u] = outI >> 16u;
00308 
00309     /* write complex conjugate output */
00310     pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
00311     pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 16u);
00312 
00313     /* update coefficient pointer */
00314     pCoefB = pCoefB + (2u * modifier);
00315     pCoefA = pCoefA + (2u * modifier);
00316 
00317     i++;
00318 
00319   }
00320 
00321   pDst[2u * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
00322   pDst[(2u * fftLen) + 1u] = 0;
00323 
00324   pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
00325   pDst[1] = 0;
00326 
00327 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
00328 
00329 }
00330 
00331 
00332 /**    
00333  * @brief  Core Real IFFT process    
00334  * @param[in]   *pSrc               points to the input buffer.    
00335  * @param[in]   fftLen              length of FFT.   
00336  * @param[in]   *pATable            points to the twiddle Coef A buffer.   
00337  * @param[in]   *pBTable            points to the twiddle Coef B buffer.    
00338  * @param[out]  *pDst               points to the output buffer.   
00339  * @param[in]   modifier            twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.   
00340  * @return none.    
00341  * The function implements a Real IFFT    
00342  */
00343 void arm_split_rifft_q15(
00344   q15_t * pSrc,
00345   uint32_t fftLen,
00346   q15_t * pATable,
00347   q15_t * pBTable,
00348   q15_t * pDst,
00349   uint32_t modifier)
00350 {
00351   uint32_t i;                                    /* Loop Counter */
00352   q31_t outR, outI;                              /* Temporary variables for output */
00353   q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
00354   q15_t *pSrc1, *pSrc2;
00355   q15_t *pDst1 = &pDst[0];
00356 
00357   pCoefA = &pATable[0];
00358   pCoefB = &pBTable[0];
00359 
00360   pSrc1 = &pSrc[0];
00361   pSrc2 = &pSrc[2u * fftLen];
00362 
00363 #ifndef ARM_MATH_CM0_FAMILY
00364 
00365   /* Run the below code for Cortex-M4 and Cortex-M3 */
00366 
00367   i = fftLen;
00368 
00369   while(i > 0u)
00370   {
00371 
00372     /*    
00373        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
00374        pIn[2 * n - 2 * i] * pBTable[2 * i] -    
00375        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00376 
00377        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -    
00378        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -    
00379        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);    
00380 
00381      */
00382 
00383 
00384 #ifndef ARM_MATH_BIG_ENDIAN
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 #else
00391 
00392     /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +  
00393        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
00394     outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)));
00395 
00396 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00397 
00398     /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
00399        pIn[2 * n - 2 * i] * pBTable[2 * i] */
00400     outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 16u;
00401 
00402     /*    
00403        -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +    
00404        pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
00405     outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
00406 
00407     /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
00408 
00409 #ifndef ARM_MATH_BIG_ENDIAN
00410 
00411     outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI);
00412 
00413 #else
00414 
00415     outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI);
00416 
00417 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00418     /* write output */
00419 
00420 #ifndef ARM_MATH_BIG_ENDIAN
00421 
00422     *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 16u), 16);
00423 
00424 #else
00425 
00426     *__SIMD32(pDst1)++ = __PKHBT((outI >> 16u), outR, 16);
00427 
00428 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00429 
00430     /* update coefficient pointer */
00431     pCoefB = pCoefB + (2u * modifier);
00432     pCoefA = pCoefA + (2u * modifier);
00433 
00434     i--;
00435 
00436   }
00437 
00438 
00439 #else
00440 
00441   /* Run the below code for Cortex-M0 */
00442 
00443   i = fftLen;
00444 
00445   while(i > 0u)
00446   {
00447 
00448     /*    
00449        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +    
00450        pIn[2 * n - 2 * i] * pBTable[2 * i] -    
00451        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);    
00452      */
00453 
00454     outR = *pSrc2 * *pCoefB;
00455     outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1));
00456     outR = outR + (*pSrc1 * *pCoefA);
00457     outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 16;
00458 
00459     /*   
00460        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -   
00461        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -   
00462        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);   
00463      */
00464 
00465     outI = *(pSrc1 + 1) * *pCoefA;
00466     outI = outI - (*pSrc1 * *(pCoefA + 1));
00467     outI = outI - (*pSrc2 * *(pCoefB + 1));
00468     outI = outI - (*(pSrc2 + 1) * *(pCoefB));
00469 
00470     /* update input pointers */
00471     pSrc1 += 2u;
00472     pSrc2 -= 2u;
00473 
00474     /* write output */
00475     *pDst1++ = (q15_t) outR;
00476     *pDst1++ = (q15_t) (outI >> 16);
00477 
00478     /* update coefficient pointer */
00479     pCoefB = pCoefB + (2u * modifier);
00480     pCoefA = pCoefA + (2u * modifier);
00481 
00482     i--;
00483 
00484   }
00485 
00486 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
00487 
00488 }