arm_cfft_q15.c

00001 /* ----------------------------------------------------------------------
00002  * Project:      CMSIS DSP Library
00003  * Title:        arm_cfft_q15.c
00004  * Description:  Combined Radix Decimation in Q15 Frequency CFFT processing function
00005  *
00006  * $Date:        27. January 2017
00007  * $Revision:    V.1.5.1
00008  *
00009  * Target Processor: Cortex-M cores
00010  * -------------------------------------------------------------------- */
00011 /*
00012  * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
00013  *
00014  * SPDX-License-Identifier: Apache-2.0
00015  *
00016  * Licensed under the Apache License, Version 2.0 (the License); you may
00017  * not use this file except in compliance with the License.
00018  * You may obtain a copy of the License at
00019  *
00020  * www.apache.org/licenses/LICENSE-2.0
00021  *
00022  * Unless required by applicable law or agreed to in writing, software
00023  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00024  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00025  * See the License for the specific language governing permissions and
00026  * limitations under the License.
00027  */
00028 
00029 #include "arm_math.h"
00030 
00031 extern void arm_radix4_butterfly_q15(
00032     q15_t * pSrc,
00033     uint32_t fftLen,
00034     q15_t * pCoef,
00035     uint32_t twidCoefModifier);
00036 
00037 extern void arm_radix4_butterfly_inverse_q15(
00038     q15_t * pSrc,
00039     uint32_t fftLen,
00040     q15_t * pCoef,
00041     uint32_t twidCoefModifier);
00042 
00043 extern void arm_bitreversal_16(
00044     uint16_t * pSrc,
00045     const uint16_t bitRevLen,
00046     const uint16_t * pBitRevTable);
00047 
00048 void arm_cfft_radix4by2_q15(
00049     q15_t * pSrc,
00050     uint32_t fftLen,
00051     const q15_t * pCoef);
00052 
00053 void arm_cfft_radix4by2_inverse_q15(
00054     q15_t * pSrc,
00055     uint32_t fftLen,
00056     const q15_t * pCoef);
00057 
00058 /**
00059 * @ingroup groupTransforms
00060 */
00061 
00062 /**
00063 * @addtogroup ComplexFFT
00064 * @{
00065 */
00066 
00067 /**
00068 * @details
00069 * @brief       Processing function for the Q15 complex FFT.
00070 * @param[in]      *S    points to an instance of the Q15 CFFT structure.
00071 * @param[in, out] *p1   points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
00072 * @param[in]     ifftFlag       flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.
00073 * @param[in]     bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
00074 * @return none.
00075 */
00076 
00077 void arm_cfft_q15(
00078     const arm_cfft_instance_q15 * S,
00079     q15_t * p1,
00080     uint8_t ifftFlag,
00081     uint8_t bitReverseFlag)
00082 {
00083     uint32_t L = S->fftLen;
00084 
00085     if (ifftFlag == 1U)
00086     {
00087         switch (L)
00088         {
00089         case 16:
00090         case 64:
00091         case 256:
00092         case 1024:
00093         case 4096:
00094             arm_radix4_butterfly_inverse_q15  ( p1, L, (q15_t*)S->pTwiddle, 1 );
00095             break;
00096 
00097         case 32:
00098         case 128:
00099         case 512:
00100         case 2048:
00101             arm_cfft_radix4by2_inverse_q15  ( p1, L, S->pTwiddle );
00102             break;
00103         }
00104     }
00105     else
00106     {
00107         switch (L)
00108         {
00109         case 16:
00110         case 64:
00111         case 256:
00112         case 1024:
00113         case 4096:
00114             arm_radix4_butterfly_q15  ( p1, L, (q15_t*)S->pTwiddle, 1 );
00115             break;
00116 
00117         case 32:
00118         case 128:
00119         case 512:
00120         case 2048:
00121             arm_cfft_radix4by2_q15  ( p1, L, S->pTwiddle );
00122             break;
00123         }
00124     }
00125 
00126     if ( bitReverseFlag )
00127         arm_bitreversal_16((uint16_t*)p1,S->bitRevLength,S->pBitRevTable);
00128 }
00129 
00130 /**
00131 * @} end of ComplexFFT group
00132 */
00133 
00134 void arm_cfft_radix4by2_q15(
00135     q15_t * pSrc,
00136     uint32_t fftLen,
00137     const q15_t * pCoef)
00138 {
00139     uint32_t i;
00140     uint32_t n2;
00141     q15_t p0, p1, p2, p3;
00142 #if defined (ARM_MATH_DSP)
00143     q31_t T, S, R;
00144     q31_t coeff, out1, out2;
00145     const q15_t *pC = pCoef;
00146     q15_t *pSi = pSrc;
00147     q15_t *pSl = pSrc + fftLen;
00148 #else
00149     uint32_t ia, l;
00150     q15_t xt, yt, cosVal, sinVal;
00151 #endif
00152 
00153     n2 = fftLen >> 1;
00154 
00155 #if defined (ARM_MATH_DSP)
00156 
00157     for (i = n2; i > 0; i--)
00158     {
00159         coeff = _SIMD32_OFFSET(pC);
00160         pC += 2;
00161 
00162         T = _SIMD32_OFFSET(pSi);
00163         T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
00164 
00165         S = _SIMD32_OFFSET(pSl);
00166         S = __SHADD16(S, 0); // this is just a SIMD arithmetic shift right by 1
00167 
00168         R = __QSUB16(T, S);
00169 
00170         _SIMD32_OFFSET(pSi) = __SHADD16(T, S);
00171         pSi += 2;
00172 
00173     #ifndef ARM_MATH_BIG_ENDIAN
00174 
00175         out1 = __SMUAD(coeff, R) >> 16;
00176         out2 = __SMUSDX(coeff, R);
00177 
00178     #else
00179 
00180         out1 = __SMUSDX(R, coeff) >> 16U;
00181         out2 = __SMUAD(coeff, R);
00182 
00183     #endif //     #ifndef ARM_MATH_BIG_ENDIAN
00184 
00185         _SIMD32_OFFSET(pSl) =
00186         (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
00187         pSl += 2;
00188     }
00189 
00190 #else //    #if defined (ARM_MATH_DSP)
00191 
00192     ia = 0;
00193     for (i = 0; i < n2; i++)
00194     {
00195         cosVal = pCoef[ia * 2];
00196         sinVal = pCoef[(ia * 2) + 1];
00197         ia++;
00198 
00199         l = i + n2;
00200 
00201         xt = (pSrc[2 * i] >> 1U) - (pSrc[2 * l] >> 1U);
00202         pSrc[2 * i] = ((pSrc[2 * i] >> 1U) + (pSrc[2 * l] >> 1U)) >> 1U;
00203 
00204         yt = (pSrc[2 * i + 1] >> 1U) - (pSrc[2 * l + 1] >> 1U);
00205         pSrc[2 * i + 1] =
00206         ((pSrc[2 * l + 1] >> 1U) + (pSrc[2 * i + 1] >> 1U)) >> 1U;
00207 
00208         pSrc[2U * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
00209                   ((int16_t) (((q31_t) yt * sinVal) >> 16)));
00210 
00211         pSrc[2U * l + 1U] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
00212                        ((int16_t) (((q31_t) xt * sinVal) >> 16)));
00213     }
00214 
00215 #endif //    #if defined (ARM_MATH_DSP)
00216 
00217     // first col
00218     arm_radix4_butterfly_q15( pSrc, n2, (q15_t*)pCoef, 2U);
00219     // second col
00220     arm_radix4_butterfly_q15( pSrc + fftLen, n2, (q15_t*)pCoef, 2U);
00221 
00222     for (i = 0; i < fftLen >> 1; i++)
00223     {
00224         p0 = pSrc[4*i+0];
00225         p1 = pSrc[4*i+1];
00226         p2 = pSrc[4*i+2];
00227         p3 = pSrc[4*i+3];
00228 
00229         p0 <<= 1;
00230         p1 <<= 1;
00231         p2 <<= 1;
00232         p3 <<= 1;
00233 
00234         pSrc[4*i+0] = p0;
00235         pSrc[4*i+1] = p1;
00236         pSrc[4*i+2] = p2;
00237         pSrc[4*i+3] = p3;
00238     }
00239 }
00240 
00241 void arm_cfft_radix4by2_inverse_q15(
00242     q15_t * pSrc,
00243     uint32_t fftLen,
00244     const q15_t * pCoef)
00245 {
00246     uint32_t i;
00247     uint32_t n2;
00248     q15_t p0, p1, p2, p3;
00249 #if defined (ARM_MATH_DSP)
00250     q31_t T, S, R;
00251     q31_t coeff, out1, out2;
00252     const q15_t *pC = pCoef;
00253     q15_t *pSi = pSrc;
00254     q15_t *pSl = pSrc + fftLen;
00255 #else
00256     uint32_t ia, l;
00257     q15_t xt, yt, cosVal, sinVal;
00258 #endif
00259 
00260     n2 = fftLen >> 1;
00261 
00262 #if defined (ARM_MATH_DSP)
00263 
00264     for (i = n2; i > 0; i--)
00265     {
00266         coeff = _SIMD32_OFFSET(pC);
00267         pC += 2;
00268 
00269         T = _SIMD32_OFFSET(pSi);
00270         T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
00271 
00272         S = _SIMD32_OFFSET(pSl);
00273         S = __SHADD16(S, 0); // this is just a SIMD arithmetic shift right by 1
00274 
00275         R = __QSUB16(T, S);
00276 
00277         _SIMD32_OFFSET(pSi) = __SHADD16(T, S);
00278         pSi += 2;
00279 
00280     #ifndef ARM_MATH_BIG_ENDIAN
00281 
00282         out1 = __SMUSD(coeff, R) >> 16;
00283         out2 = __SMUADX(coeff, R);
00284     #else
00285 
00286         out1 = __SMUADX(R, coeff) >> 16U;
00287         out2 = __SMUSD(__QSUB(0, coeff), R);
00288 
00289     #endif //     #ifndef ARM_MATH_BIG_ENDIAN
00290 
00291         _SIMD32_OFFSET(pSl) =
00292         (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
00293         pSl += 2;
00294     }
00295 
00296 #else //    #if defined (ARM_MATH_DSP)
00297 
00298     ia = 0;
00299     for (i = 0; i < n2; i++)
00300     {
00301         cosVal = pCoef[ia * 2];
00302         sinVal = pCoef[(ia * 2) + 1];
00303         ia++;
00304 
00305         l = i + n2;
00306         xt = (pSrc[2 * i] >> 1U) - (pSrc[2 * l] >> 1U);
00307         pSrc[2 * i] = ((pSrc[2 * i] >> 1U) + (pSrc[2 * l] >> 1U)) >> 1U;
00308 
00309         yt = (pSrc[2 * i + 1] >> 1U) - (pSrc[2 * l + 1] >> 1U);
00310         pSrc[2 * i + 1] =
00311           ((pSrc[2 * l + 1] >> 1U) + (pSrc[2 * i + 1] >> 1U)) >> 1U;
00312 
00313         pSrc[2U * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
00314                         ((int16_t) (((q31_t) yt * sinVal) >> 16)));
00315 
00316         pSrc[2U * l + 1U] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
00317                            ((int16_t) (((q31_t) xt * sinVal) >> 16)));
00318     }
00319 
00320 #endif //    #if defined (ARM_MATH_DSP)
00321 
00322     // first col
00323     arm_radix4_butterfly_inverse_q15( pSrc, n2, (q15_t*)pCoef, 2U);
00324     // second col
00325     arm_radix4_butterfly_inverse_q15( pSrc + fftLen, n2, (q15_t*)pCoef, 2U);
00326 
00327     for (i = 0; i < fftLen >> 1; i++)
00328     {
00329         p0 = pSrc[4*i+0];
00330         p1 = pSrc[4*i+1];
00331         p2 = pSrc[4*i+2];
00332         p3 = pSrc[4*i+3];
00333 
00334         p0 <<= 1;
00335         p1 <<= 1;
00336         p2 <<= 1;
00337         p3 <<= 1;
00338 
00339         pSrc[4*i+0] = p0;
00340         pSrc[4*i+1] = p1;
00341         pSrc[4*i+2] = p2;
00342         pSrc[4*i+3] = p3;
00343     }
00344 }
00345 
00346