arm_cfft_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_cfft_q15.c   
00009 *    
00010 * Description:  Combined Radix Decimation in Q15 Frequency CFFT processing function
00011 *    
00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE.   
00039 * -------------------------------------------------------------------- */
00040 
00041 #include "arm_math.h"
00042 
00043 extern void arm_radix4_butterfly_q15(
00044     q15_t * pSrc,
00045     uint32_t fftLen,
00046     q15_t * pCoef,
00047     uint32_t twidCoefModifier);
00048 
00049 extern void arm_radix4_butterfly_inverse_q15(
00050     q15_t * pSrc,
00051     uint32_t fftLen,
00052     q15_t * pCoef,
00053     uint32_t twidCoefModifier);
00054 
00055 extern void arm_bitreversal_16(
00056     uint16_t * pSrc,
00057     const uint16_t bitRevLen,
00058     const uint16_t * pBitRevTable);
00059     
00060 void arm_cfft_radix4by2_q15(
00061     q15_t * pSrc,
00062     uint32_t fftLen,
00063     const q15_t * pCoef);
00064     
00065 void arm_cfft_radix4by2_inverse_q15(
00066     q15_t * pSrc,
00067     uint32_t fftLen,
00068     const q15_t * pCoef);
00069 
00070 /**   
00071 * @ingroup groupTransforms   
00072 */
00073 
00074 /**
00075 * @addtogroup ComplexFFT   
00076 * @{   
00077 */
00078 
00079 /**   
00080 * @details   
00081 * @brief       Processing function for the Q15 complex FFT.
00082 * @param[in]      *S    points to an instance of the Q15 CFFT structure.  
00083 * @param[in, out] *p1   points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.  
00084 * @param[in]     ifftFlag       flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.  
00085 * @param[in]     bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.  
00086 * @return none.  
00087 */
00088 
00089 void arm_cfft_q15( 
00090     const arm_cfft_instance_q15 * S, 
00091     q15_t * p1,
00092     uint8_t ifftFlag,
00093     uint8_t bitReverseFlag)
00094 {
00095     uint32_t L = S->fftLen;
00096 
00097     if(ifftFlag == 1u)
00098     {
00099         switch (L) 
00100         {
00101         case 16: 
00102         case 64:
00103         case 256:
00104         case 1024:
00105         case 4096:
00106             arm_radix4_butterfly_inverse_q15  ( p1, L, (q15_t*)S->pTwiddle, 1 );
00107             break;
00108             
00109         case 32:
00110         case 128:
00111         case 512:
00112         case 2048:
00113             arm_cfft_radix4by2_inverse_q15  ( p1, L, S->pTwiddle );
00114             break;
00115         }  
00116     }
00117     else
00118     {
00119         switch (L) 
00120         {
00121         case 16: 
00122         case 64:
00123         case 256:
00124         case 1024:
00125         case 4096:
00126             arm_radix4_butterfly_q15  ( p1, L, (q15_t*)S->pTwiddle, 1 );
00127             break;
00128             
00129         case 32:
00130         case 128:
00131         case 512:
00132         case 2048:
00133             arm_cfft_radix4by2_q15  ( p1, L, S->pTwiddle );
00134             break;
00135         }  
00136     }
00137     
00138     if( bitReverseFlag )
00139         arm_bitreversal_16((uint16_t*)p1,S->bitRevLength,S->pBitRevTable);    
00140 }
00141 
00142 /**    
00143 * @} end of ComplexFFT group    
00144 */
00145 
00146 void arm_cfft_radix4by2_q15(
00147     q15_t * pSrc,
00148     uint32_t fftLen,
00149     const q15_t * pCoef) 
00150 {    
00151     uint32_t i;
00152     uint32_t n2;
00153     q15_t p0, p1, p2, p3;
00154 #ifndef ARM_MATH_CM0_FAMILY
00155     q31_t T, S, R;
00156     q31_t coeff, out1, out2;
00157     const q15_t *pC = pCoef;
00158     q15_t *pSi = pSrc;
00159     q15_t *pSl = pSrc + fftLen;
00160 #else
00161     uint32_t ia, l;
00162     q15_t xt, yt, cosVal, sinVal;
00163 #endif
00164     
00165     n2 = fftLen >> 1; 
00166 
00167 #ifndef ARM_MATH_CM0_FAMILY
00168 
00169     for (i = n2; i > 0; i--)
00170     {
00171         coeff = _SIMD32_OFFSET(pC);
00172         pC += 2;
00173 
00174         T = _SIMD32_OFFSET(pSi);
00175         T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
00176 
00177         S = _SIMD32_OFFSET(pSl);
00178         S = __SHADD16(S, 0); // this is just a SIMD arithmetic shift right by 1
00179 
00180         R = __QSUB16(T, S);
00181 
00182         _SIMD32_OFFSET(pSi) = __SHADD16(T, S);
00183         pSi += 2;
00184 
00185     #ifndef ARM_MATH_BIG_ENDIAN
00186 
00187         out1 = __SMUAD(coeff, R) >> 16;
00188         out2 = __SMUSDX(coeff, R);
00189 
00190     #else
00191 
00192         out1 = __SMUSDX(R, coeff) >> 16u;
00193         out2 = __SMUAD(coeff, R);
00194 
00195     #endif //     #ifndef ARM_MATH_BIG_ENDIAN
00196 
00197         _SIMD32_OFFSET(pSl) =
00198         (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
00199         pSl += 2;
00200     } 
00201     
00202 #else //    #ifndef ARM_MATH_CM0_FAMILY
00203     
00204     ia = 0;
00205     for (i = 0; i < n2; i++)
00206     {
00207         cosVal = pCoef[ia * 2];
00208         sinVal = pCoef[(ia * 2) + 1];
00209         ia++;
00210         
00211         l = i + n2;        
00212         
00213         xt = (pSrc[2 * i] >> 1u) - (pSrc[2 * l] >> 1u);
00214         pSrc[2 * i] = ((pSrc[2 * i] >> 1u) + (pSrc[2 * l] >> 1u)) >> 1u;
00215         
00216         yt = (pSrc[2 * i + 1] >> 1u) - (pSrc[2 * l + 1] >> 1u);
00217         pSrc[2 * i + 1] =
00218         ((pSrc[2 * l + 1] >> 1u) + (pSrc[2 * i + 1] >> 1u)) >> 1u;
00219 
00220         pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
00221                   ((int16_t) (((q31_t) yt * sinVal) >> 16)));
00222 
00223         pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
00224                        ((int16_t) (((q31_t) xt * sinVal) >> 16)));
00225     } 
00226     
00227 #endif //    #ifndef ARM_MATH_CM0_FAMILY
00228     
00229     // first col
00230     arm_radix4_butterfly_q15( pSrc, n2, (q15_t*)pCoef, 2u);
00231     // second col
00232     arm_radix4_butterfly_q15( pSrc + fftLen, n2, (q15_t*)pCoef, 2u);
00233             
00234     for (i = 0; i < fftLen >> 1; i++)
00235     {
00236         p0 = pSrc[4*i+0];
00237         p1 = pSrc[4*i+1];
00238         p2 = pSrc[4*i+2];
00239         p3 = pSrc[4*i+3];
00240         
00241         p0 <<= 1;
00242         p1 <<= 1;
00243         p2 <<= 1;
00244         p3 <<= 1;
00245         
00246         pSrc[4*i+0] = p0;
00247         pSrc[4*i+1] = p1;
00248         pSrc[4*i+2] = p2;
00249         pSrc[4*i+3] = p3;
00250     }
00251 }
00252 
00253 void arm_cfft_radix4by2_inverse_q15(
00254     q15_t * pSrc,
00255     uint32_t fftLen,
00256     const q15_t * pCoef) 
00257 {    
00258     uint32_t i;
00259     uint32_t n2;
00260     q15_t p0, p1, p2, p3;
00261 #ifndef ARM_MATH_CM0_FAMILY
00262     q31_t T, S, R;
00263     q31_t coeff, out1, out2;
00264     const q15_t *pC = pCoef;
00265     q15_t *pSi = pSrc;
00266     q15_t *pSl = pSrc + fftLen;
00267 #else
00268     uint32_t ia, l;
00269     q15_t xt, yt, cosVal, sinVal;
00270 #endif
00271     
00272     n2 = fftLen >> 1; 
00273 
00274 #ifndef ARM_MATH_CM0_FAMILY
00275 
00276     for (i = n2; i > 0; i--)
00277     {
00278         coeff = _SIMD32_OFFSET(pC);
00279         pC += 2;
00280 
00281         T = _SIMD32_OFFSET(pSi);
00282         T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
00283 
00284         S = _SIMD32_OFFSET(pSl);
00285         S = __SHADD16(S, 0); // this is just a SIMD arithmetic shift right by 1
00286 
00287         R = __QSUB16(T, S);
00288 
00289         _SIMD32_OFFSET(pSi) = __SHADD16(T, S);
00290         pSi += 2;
00291 
00292     #ifndef ARM_MATH_BIG_ENDIAN
00293 
00294         out1 = __SMUSD(coeff, R) >> 16;
00295         out2 = __SMUADX(coeff, R);
00296     #else
00297 
00298         out1 = __SMUADX(R, coeff) >> 16u;
00299         out2 = __SMUSD(__QSUB(0, coeff), R);
00300 
00301     #endif //     #ifndef ARM_MATH_BIG_ENDIAN
00302 
00303         _SIMD32_OFFSET(pSl) =
00304         (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);        
00305         pSl += 2;
00306     } 
00307     
00308 #else //    #ifndef ARM_MATH_CM0_FAMILY
00309 
00310     ia = 0;
00311     for (i = 0; i < n2; i++)
00312     {
00313         cosVal = pCoef[ia * 2];
00314         sinVal = pCoef[(ia * 2) + 1];
00315         ia++;
00316         
00317         l = i + n2;
00318         xt = (pSrc[2 * i] >> 1u) - (pSrc[2 * l] >> 1u);
00319         pSrc[2 * i] = ((pSrc[2 * i] >> 1u) + (pSrc[2 * l] >> 1u)) >> 1u;
00320         
00321         yt = (pSrc[2 * i + 1] >> 1u) - (pSrc[2 * l + 1] >> 1u);
00322         pSrc[2 * i + 1] =
00323           ((pSrc[2 * l + 1] >> 1u) + (pSrc[2 * i + 1] >> 1u)) >> 1u;
00324         
00325         pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
00326                         ((int16_t) (((q31_t) yt * sinVal) >> 16)));
00327         
00328         pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
00329                            ((int16_t) (((q31_t) xt * sinVal) >> 16)));
00330     } 
00331     
00332 #endif //    #ifndef ARM_MATH_CM0_FAMILY
00333 
00334     // first col
00335     arm_radix4_butterfly_inverse_q15( pSrc, n2, (q15_t*)pCoef, 2u);
00336     // second col
00337     arm_radix4_butterfly_inverse_q15( pSrc + fftLen, n2, (q15_t*)pCoef, 2u);
00338             
00339     for (i = 0; i < fftLen >> 1; i++)
00340     {
00341         p0 = pSrc[4*i+0];
00342         p1 = pSrc[4*i+1];
00343         p2 = pSrc[4*i+2];
00344         p3 = pSrc[4*i+3];
00345         
00346         p0 <<= 1;
00347         p1 <<= 1;
00348         p2 <<= 1;
00349         p3 <<= 1;
00350         
00351         pSrc[4*i+0] = p0;
00352         pSrc[4*i+1] = p1;
00353         pSrc[4*i+2] = p2;
00354         pSrc[4*i+3] = p3;
00355     }
00356 }
00357