tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/TransformFunctions/arm_cfft_radix2_q31.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:            arm_cfft_radix2_q31.c
   9 *
  10 * Description:  Radix-2 Decimation in Frequency CFFT & CIFFT Fixed point processing function
  11 *
  12 *
  13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  14 *
  15 * Redistribution and use in source and binary forms, with or without
  16 * modification, are permitted provided that the following conditions
  17 * are met:
  18 *   - Redistributions of source code must retain the above copyright
  19 *     notice, this list of conditions and the following disclaimer.
  20 *   - Redistributions in binary form must reproduce the above copyright
  21 *     notice, this list of conditions and the following disclaimer in
  22 *     the documentation and/or other materials provided with the
  23 *     distribution.
  24 *   - Neither the name of ARM LIMITED nor the names of its contributors
  25 *     may be used to endorse or promote products derived from this
  26 *     software without specific prior written permission.
  27 *
  28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  39 * POSSIBILITY OF SUCH DAMAGE.
  40 * -------------------------------------------------------------------- */
  41
  42 #include "arm_math.h"
  43
  44 void arm_radix2_butterfly_q31(
  45   q31_t * pSrc,
  46   uint32_t fftLen,
  47   q31_t * pCoef,
  48   uint16_t twidCoefModifier);
  49
  50 void arm_radix2_butterfly_inverse_q31(
  51   q31_t * pSrc,
  52   uint32_t fftLen,
  53   q31_t * pCoef,
  54   uint16_t twidCoefModifier);
  55
  56 void arm_bitreversal_q31(
  57   q31_t * pSrc,
  58   uint32_t fftLen,
  59   uint16_t bitRevFactor,
  60   uint16_t * pBitRevTab);
  61
  62 /**
  63 * @ingroup groupTransforms
  64 */
  65
  66 /**
  67 * @addtogroup ComplexFFT
  68 * @{
  69 */
  70
  71 /**
  72 * @details
  73 * @brief Processing function for the fixed-point CFFT/CIFFT.
  74 * @param[in]      *S    points to an instance of the fixed-point CFFT/CIFFT structure.
  75 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
  76 * @return none.
  77 */
  78
  79 void arm_cfft_radix2_q31(
  80 const arm_cfft_radix2_instance_q31 * S,
  81 q31_t * pSrc)
  82 {
  83
  84    if(S->ifftFlag == 1u)
  85    {
  86       arm_radix2_butterfly_inverse_q31(pSrc, S->fftLen,
  87       S->pTwiddle, S->twidCoefModifier);
  88    }
  89    else
  90    {
  91       arm_radix2_butterfly_q31(pSrc, S->fftLen,
  92       S->pTwiddle, S->twidCoefModifier);
  93    }
  94
  95    arm_bitreversal_q31(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
  96 }
  97
  98 /**
  99 * @} end of ComplexFFT group
 100 */
 101
 102 void arm_radix2_butterfly_q31(
 103 q31_t * pSrc,
 104 uint32_t fftLen,
 105 q31_t * pCoef,
 106 uint16_t twidCoefModifier)
 107 {
 108
 109    unsigned i, j, k, l, m;
 110    unsigned n1, n2, ia;
 111    q31_t xt, yt, cosVal, sinVal;
 112    q31_t p0, p1;
 113
 114    //N = fftLen;
 115    n2 = fftLen;
 116
 117    n1 = n2;
 118    n2 = n2 >> 1;
 119    ia = 0;
 120
 121    // loop for groups
 122    for (i = 0; i < n2; i++)
 123    {
 124       cosVal = pCoef[ia * 2];
 125       sinVal = pCoef[(ia * 2) + 1];
 126       ia = ia + twidCoefModifier;
 127
 128       l = i + n2;
 129       xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
 130       pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
 131
 132       yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
 133       pSrc[2 * i + 1] =
 134         ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
 135
 136       mult_32x32_keep32_R(p0, xt, cosVal);
 137       mult_32x32_keep32_R(p1, yt, cosVal);
 138       multAcc_32x32_keep32_R(p0, yt, sinVal);
 139       multSub_32x32_keep32_R(p1, xt, sinVal);
 140
 141       pSrc[2u * l] = p0;
 142       pSrc[2u * l + 1u] = p1;
 143
 144    }                             // groups loop end
 145
 146    twidCoefModifier <<= 1u;
 147
 148    // loop for stage
 149    for (k = fftLen / 2; k > 2; k = k >> 1)
 150    {
 151       n1 = n2;
 152       n2 = n2 >> 1;
 153       ia = 0;
 154
 155       // loop for groups
 156       for (j = 0; j < n2; j++)
 157       {
 158          cosVal = pCoef[ia * 2];
 159          sinVal = pCoef[(ia * 2) + 1];
 160          ia = ia + twidCoefModifier;
 161
 162          // loop for butterfly
 163          i = j;
 164          m = fftLen / n1;
 165          do
 166          {
 167             l = i + n2;
 168             xt = pSrc[2 * i] - pSrc[2 * l];
 169             pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
 170
 171             yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
 172             pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
 173
 174             mult_32x32_keep32_R(p0, xt, cosVal);
 175             mult_32x32_keep32_R(p1, yt, cosVal);
 176             multAcc_32x32_keep32_R(p0, yt, sinVal);
 177             multSub_32x32_keep32_R(p1, xt, sinVal);
 178
 179             pSrc[2u * l] = p0;
 180             pSrc[2u * l + 1u] = p1;
 181             i += n1;
 182             m--;
 183          } while( m > 0);                   // butterfly loop end
 184
 185       }                           // groups loop end
 186
 187       twidCoefModifier <<= 1u;
 188    }                             // stages loop end
 189
 190    n1 = n2;
 191    n2 = n2 >> 1;
 192    ia = 0;
 193
 194    cosVal = pCoef[ia * 2];
 195    sinVal = pCoef[(ia * 2) + 1];
 196    ia = ia + twidCoefModifier;
 197
 198    // loop for butterfly
 199    for (i = 0; i < fftLen; i += n1)
 200    {
 201       l = i + n2;
 202       xt = pSrc[2 * i] - pSrc[2 * l];
 203       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
 204
 205       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
 206       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
 207
 208       pSrc[2u * l] = xt;
 209
 210       pSrc[2u * l + 1u] = yt;
 211
 212       i += n1;
 213       l = i + n2;
 214
 215       xt = pSrc[2 * i] - pSrc[2 * l];
 216       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
 217
 218       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
 219       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
 220
 221       pSrc[2u * l] = xt;
 222
 223       pSrc[2u * l + 1u] = yt;
 224
 225    }                             // butterfly loop end
 226
 227 }
 228
 229
 230 void arm_radix2_butterfly_inverse_q31(
 231 q31_t * pSrc,
 232 uint32_t fftLen,
 233 q31_t * pCoef,
 234 uint16_t twidCoefModifier)
 235 {
 236
 237    unsigned i, j, k, l;
 238    unsigned n1, n2, ia;
 239    q31_t xt, yt, cosVal, sinVal;
 240    q31_t p0, p1;
 241
 242    //N = fftLen;
 243    n2 = fftLen;
 244
 245    n1 = n2;
 246    n2 = n2 >> 1;
 247    ia = 0;
 248
 249    // loop for groups
 250    for (i = 0; i < n2; i++)
 251    {
 252       cosVal = pCoef[ia * 2];
 253       sinVal = pCoef[(ia * 2) + 1];
 254       ia = ia + twidCoefModifier;
 255
 256       l = i + n2;
 257       xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
 258       pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
 259
 260       yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
 261       pSrc[2 * i + 1] =
 262         ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
 263
 264       mult_32x32_keep32_R(p0, xt, cosVal);
 265       mult_32x32_keep32_R(p1, yt, cosVal);
 266       multSub_32x32_keep32_R(p0, yt, sinVal);
 267       multAcc_32x32_keep32_R(p1, xt, sinVal);
 268
 269       pSrc[2u * l] = p0;
 270       pSrc[2u * l + 1u] = p1;
 271    }                             // groups loop end
 272
 273    twidCoefModifier = twidCoefModifier << 1u;
 274
 275    // loop for stage
 276    for (k = fftLen / 2; k > 2; k = k >> 1)
 277    {
 278       n1 = n2;
 279       n2 = n2 >> 1;
 280       ia = 0;
 281
 282       // loop for groups
 283       for (j = 0; j < n2; j++)
 284       {
 285          cosVal = pCoef[ia * 2];
 286          sinVal = pCoef[(ia * 2) + 1];
 287          ia = ia + twidCoefModifier;
 288
 289          // loop for butterfly
 290          for (i = j; i < fftLen; i += n1)
 291          {
 292             l = i + n2;
 293             xt = pSrc[2 * i] - pSrc[2 * l];
 294             pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
 295
 296             yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
 297             pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
 298
 299             mult_32x32_keep32_R(p0, xt, cosVal);
 300             mult_32x32_keep32_R(p1, yt, cosVal);
 301             multSub_32x32_keep32_R(p0, yt, sinVal);
 302             multAcc_32x32_keep32_R(p1, xt, sinVal);
 303
 304             pSrc[2u * l] = p0;
 305             pSrc[2u * l + 1u] = p1;
 306          }                         // butterfly loop end
 307
 308       }                           // groups loop end
 309
 310       twidCoefModifier = twidCoefModifier << 1u;
 311    }                             // stages loop end
 312
 313    n1 = n2;
 314    n2 = n2 >> 1;
 315    ia = 0;
 316
 317    cosVal = pCoef[ia * 2];
 318    sinVal = pCoef[(ia * 2) + 1];
 319    ia = ia + twidCoefModifier;
 320
 321    // loop for butterfly
 322    for (i = 0; i < fftLen; i += n1)
 323    {
 324       l = i + n2;
 325       xt = pSrc[2 * i] - pSrc[2 * l];
 326       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
 327
 328       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
 329       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
 330
 331       pSrc[2u * l] = xt;
 332
 333       pSrc[2u * l + 1u] = yt;
 334
 335       i += n1;
 336       l = i + n2;
 337
 338       xt = pSrc[2 * i] - pSrc[2 * l];
 339       pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
 340
 341       yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
 342       pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
 343
 344       pSrc[2u * l] = xt;
 345
 346       pSrc[2u * l + 1u] = yt;
 347
 348    }                             // butterfly loop end
 349
 350 }