lib/main/CMSIS/DSP/Source/TransformFunctions/arm_cfft_q15.c

   1 /* ----------------------------------------------------------------------
   2  * Project:      CMSIS DSP Library
   3  * Title:        arm_cfft_q15.c
   4  * Description:  Combined Radix Decimation in Q15 Frequency CFFT processing function
   5  *
   6  * $Date:        27. January 2017
   7  * $Revision:    V.1.5.1
   8  *
   9  * Target Processor: Cortex-M cores
  10  * -------------------------------------------------------------------- */
  11 /*
  12  * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
  13  *
  14  * SPDX-License-Identifier: Apache-2.0
  15  *
  16  * Licensed under the Apache License, Version 2.0 (the License); you may
  17  * not use this file except in compliance with the License.
  18  * You may obtain a copy of the License at
  19  *
  20  * www.apache.org/licenses/LICENSE-2.0
  21  *
  22  * Unless required by applicable law or agreed to in writing, software
  23  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
  24  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  25  * See the License for the specific language governing permissions and
  26  * limitations under the License.
  27  */
  28
  29 #include "arm_math.h"
  30
  31 extern void arm_radix4_butterfly_q15(
  32     q15_t * pSrc,
  33     uint32_t fftLen,
  34     q15_t * pCoef,
  35     uint32_t twidCoefModifier);
  36
  37 extern void arm_radix4_butterfly_inverse_q15(
  38     q15_t * pSrc,
  39     uint32_t fftLen,
  40     q15_t * pCoef,
  41     uint32_t twidCoefModifier);
  42
  43 extern void arm_bitreversal_16(
  44     uint16_t * pSrc,
  45     const uint16_t bitRevLen,
  46     const uint16_t * pBitRevTable);
  47
  48 void arm_cfft_radix4by2_q15(
  49     q15_t * pSrc,
  50     uint32_t fftLen,
  51     const q15_t * pCoef);
  52
  53 void arm_cfft_radix4by2_inverse_q15(
  54     q15_t * pSrc,
  55     uint32_t fftLen,
  56     const q15_t * pCoef);
  57
  58 /**
  59 * @ingroup groupTransforms
  60 */
  61
  62 /**
  63 * @addtogroup ComplexFFT
  64 * @{
  65 */
  66
  67 /**
  68 * @details
  69 * @brief       Processing function for the Q15 complex FFT.
  70 * @param[in]      *S    points to an instance of the Q15 CFFT structure.
  71 * @param[in, out] *p1   points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
  72 * @param[in]     ifftFlag       flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.
  73 * @param[in]     bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
  74 * @return none.
  75 */
  76
  77 void arm_cfft_q15(
  78     const arm_cfft_instance_q15 * S,
  79     q15_t * p1,
  80     uint8_t ifftFlag,
  81     uint8_t bitReverseFlag)
  82 {
  83     uint32_t L = S->fftLen;
  84
  85     if (ifftFlag == 1U)
  86     {
  87         switch (L)
  88         {
  89         case 16:
  90         case 64:
  91         case 256:
  92         case 1024:
  93         case 4096:
  94             arm_radix4_butterfly_inverse_q15  ( p1, L, (q15_t*)S->pTwiddle, 1 );
  95             break;
  96
  97         case 32:
  98         case 128:
  99         case 512:
 100         case 2048:
 101             arm_cfft_radix4by2_inverse_q15  ( p1, L, S->pTwiddle );
 102             break;
 103         }
 104     }
 105     else
 106     {
 107         switch (L)
 108         {
 109         case 16:
 110         case 64:
 111         case 256:
 112         case 1024:
 113         case 4096:
 114             arm_radix4_butterfly_q15  ( p1, L, (q15_t*)S->pTwiddle, 1 );
 115             break;
 116
 117         case 32:
 118         case 128:
 119         case 512:
 120         case 2048:
 121             arm_cfft_radix4by2_q15  ( p1, L, S->pTwiddle );
 122             break;
 123         }
 124     }
 125
 126     if ( bitReverseFlag )
 127         arm_bitreversal_16((uint16_t*)p1,S->bitRevLength,S->pBitRevTable);
 128 }
 129
 130 /**
 131 * @} end of ComplexFFT group
 132 */
 133
 134 void arm_cfft_radix4by2_q15(
 135     q15_t * pSrc,
 136     uint32_t fftLen,
 137     const q15_t * pCoef)
 138 {
 139     uint32_t i;
 140     uint32_t n2;
 141     q15_t p0, p1, p2, p3;
 142 #if defined (ARM_MATH_DSP)
 143     q31_t T, S, R;
 144     q31_t coeff, out1, out2;
 145     const q15_t *pC = pCoef;
 146     q15_t *pSi = pSrc;
 147     q15_t *pSl = pSrc + fftLen;
 148 #else
 149     uint32_t ia, l;
 150     q15_t xt, yt, cosVal, sinVal;
 151 #endif
 152
 153     n2 = fftLen >> 1;
 154
 155 #if defined (ARM_MATH_DSP)
 156
 157     for (i = n2; i > 0; i--)
 158     {
 159         coeff = _SIMD32_OFFSET(pC);
 160         pC += 2;
 161
 162         T = _SIMD32_OFFSET(pSi);
 163         T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
 164
 165         S = _SIMD32_OFFSET(pSl);
 166         S = __SHADD16(S, 0); // this is just a SIMD arithmetic shift right by 1
 167
 168         R = __QSUB16(T, S);
 169
 170         _SIMD32_OFFSET(pSi) = __SHADD16(T, S);
 171         pSi += 2;
 172
 173     #ifndef ARM_MATH_BIG_ENDIAN
 174
 175         out1 = __SMUAD(coeff, R) >> 16;
 176         out2 = __SMUSDX(coeff, R);
 177
 178     #else
 179
 180         out1 = __SMUSDX(R, coeff) >> 16U;
 181         out2 = __SMUAD(coeff, R);
 182
 183     #endif //     #ifndef ARM_MATH_BIG_ENDIAN
 184
 185         _SIMD32_OFFSET(pSl) =
 186         (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
 187         pSl += 2;
 188     }
 189
 190 #else //    #if defined (ARM_MATH_DSP)
 191
 192     ia = 0;
 193     for (i = 0; i < n2; i++)
 194     {
 195         cosVal = pCoef[ia * 2];
 196         sinVal = pCoef[(ia * 2) + 1];
 197         ia++;
 198
 199         l = i + n2;
 200
 201         xt = (pSrc[2 * i] >> 1U) - (pSrc[2 * l] >> 1U);
 202         pSrc[2 * i] = ((pSrc[2 * i] >> 1U) + (pSrc[2 * l] >> 1U)) >> 1U;
 203
 204         yt = (pSrc[2 * i + 1] >> 1U) - (pSrc[2 * l + 1] >> 1U);
 205         pSrc[2 * i + 1] =
 206         ((pSrc[2 * l + 1] >> 1U) + (pSrc[2 * i + 1] >> 1U)) >> 1U;
 207
 208         pSrc[2U * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
 209                   ((int16_t) (((q31_t) yt * sinVal) >> 16)));
 210
 211         pSrc[2U * l + 1U] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
 212                        ((int16_t) (((q31_t) xt * sinVal) >> 16)));
 213     }
 214
 215 #endif //    #if defined (ARM_MATH_DSP)
 216
 217     // first col
 218     arm_radix4_butterfly_q15( pSrc, n2, (q15_t*)pCoef, 2U);
 219     // second col
 220     arm_radix4_butterfly_q15( pSrc + fftLen, n2, (q15_t*)pCoef, 2U);
 221
 222     for (i = 0; i < fftLen >> 1; i++)
 223     {
 224         p0 = pSrc[4*i+0];
 225         p1 = pSrc[4*i+1];
 226         p2 = pSrc[4*i+2];
 227         p3 = pSrc[4*i+3];
 228
 229         p0 <<= 1;
 230         p1 <<= 1;
 231         p2 <<= 1;
 232         p3 <<= 1;
 233
 234         pSrc[4*i+0] = p0;
 235         pSrc[4*i+1] = p1;
 236         pSrc[4*i+2] = p2;
 237         pSrc[4*i+3] = p3;
 238     }
 239 }
 240
 241 void arm_cfft_radix4by2_inverse_q15(
 242     q15_t * pSrc,
 243     uint32_t fftLen,
 244     const q15_t * pCoef)
 245 {
 246     uint32_t i;
 247     uint32_t n2;
 248     q15_t p0, p1, p2, p3;
 249 #if defined (ARM_MATH_DSP)
 250     q31_t T, S, R;
 251     q31_t coeff, out1, out2;
 252     const q15_t *pC = pCoef;
 253     q15_t *pSi = pSrc;
 254     q15_t *pSl = pSrc + fftLen;
 255 #else
 256     uint32_t ia, l;
 257     q15_t xt, yt, cosVal, sinVal;
 258 #endif
 259
 260     n2 = fftLen >> 1;
 261
 262 #if defined (ARM_MATH_DSP)
 263
 264     for (i = n2; i > 0; i--)
 265     {
 266         coeff = _SIMD32_OFFSET(pC);
 267         pC += 2;
 268
 269         T = _SIMD32_OFFSET(pSi);
 270         T = __SHADD16(T, 0); // this is just a SIMD arithmetic shift right by 1
 271
 272         S = _SIMD32_OFFSET(pSl);
 273         S = __SHADD16(S, 0); // this is just a SIMD arithmetic shift right by 1
 274
 275         R = __QSUB16(T, S);
 276
 277         _SIMD32_OFFSET(pSi) = __SHADD16(T, S);
 278         pSi += 2;
 279
 280     #ifndef ARM_MATH_BIG_ENDIAN
 281
 282         out1 = __SMUSD(coeff, R) >> 16;
 283         out2 = __SMUADX(coeff, R);
 284     #else
 285
 286         out1 = __SMUADX(R, coeff) >> 16U;
 287         out2 = __SMUSD(__QSUB(0, coeff), R);
 288
 289     #endif //     #ifndef ARM_MATH_BIG_ENDIAN
 290
 291         _SIMD32_OFFSET(pSl) =
 292         (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
 293         pSl += 2;
 294     }
 295
 296 #else //    #if defined (ARM_MATH_DSP)
 297
 298     ia = 0;
 299     for (i = 0; i < n2; i++)
 300     {
 301         cosVal = pCoef[ia * 2];
 302         sinVal = pCoef[(ia * 2) + 1];
 303         ia++;
 304
 305         l = i + n2;
 306         xt = (pSrc[2 * i] >> 1U) - (pSrc[2 * l] >> 1U);
 307         pSrc[2 * i] = ((pSrc[2 * i] >> 1U) + (pSrc[2 * l] >> 1U)) >> 1U;
 308
 309         yt = (pSrc[2 * i + 1] >> 1U) - (pSrc[2 * l + 1] >> 1U);
 310         pSrc[2 * i + 1] =
 311           ((pSrc[2 * l + 1] >> 1U) + (pSrc[2 * i + 1] >> 1U)) >> 1U;
 312
 313         pSrc[2U * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
 314                         ((int16_t) (((q31_t) yt * sinVal) >> 16)));
 315
 316         pSrc[2U * l + 1U] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
 317                            ((int16_t) (((q31_t) xt * sinVal) >> 16)));
 318     }
 319
 320 #endif //    #if defined (ARM_MATH_DSP)
 321
 322     // first col
 323     arm_radix4_butterfly_inverse_q15( pSrc, n2, (q15_t*)pCoef, 2U);
 324     // second col
 325     arm_radix4_butterfly_inverse_q15( pSrc + fftLen, n2, (q15_t*)pCoef, 2U);
 326
 327     for (i = 0; i < fftLen >> 1; i++)
 328     {
 329         p0 = pSrc[4*i+0];
 330         p1 = pSrc[4*i+1];
 331         p2 = pSrc[4*i+2];
 332         p3 = pSrc[4*i+3];
 333
 334         p0 <<= 1;
 335         p1 <<= 1;
 336         p2 <<= 1;
 337         p3 <<= 1;
 338
 339         pSrc[4*i+0] = p0;
 340         pSrc[4*i+1] = p1;
 341         pSrc[4*i+2] = p2;
 342         pSrc[4*i+3] = p3;
 343     }
 344 }
 345