arm_rfft_q15.c

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