git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_q31.c

   1 /*-----------------------------------------------------------------------------
   2 * Copyright (C) 2010 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        15. July 2011
   5 * $Revision:    V1.0.10
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:                arm_fir_interpolate_q31.c
   9 *
  10 * Description:  Q31 FIR interpolation.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Version 1.0.10 2011/7/15
  15 *    Big Endian support added and Merged M0 and M3/M4 Source code.
  16 *
  17 * Version 1.0.3 2010/11/29
  18 *    Re-organized the CMSIS folders and updated documentation.
  19 *
  20 * Version 1.0.2 2010/11/11
  21 *    Documentation updated.
  22 *
  23 * Version 1.0.1 2010/10/05
  24 *    Production release and review comments incorporated.
  25 *
  26 * Version 1.0.0 2010/09/20
  27 *    Production release and review comments incorporated
  28 *
  29 * Version 0.0.7  2010/06/10
  30 *    Misra-C changes done
  31 * ---------------------------------------------------------------------------*/
  32
  33 #include "arm_math.h"
  34
  35 /**
  36  * @ingroup groupFilters
  37  */
  38
  39 /**
  40  * @addtogroup FIR_Interpolate
  41  * @{
  42  */
  43
  44 /**
  45  * @brief Processing function for the Q31 FIR interpolator.
  46  * @param[in] *S        points to an instance of the Q31 FIR interpolator structure.
  47  * @param[in] *pSrc     points to the block of input data.
  48  * @param[out] *pDst    points to the block of output data.
  49  * @param[in] blockSize number of input samples to process per call.
  50  * @return none.
  51  *
  52  * <b>Scaling and Overflow Behavior:</b>
  53  * \par
  54  * The function is implemented using an internal 64-bit accumulator.
  55  * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
  56  * Thus, if the accumulator result overflows it wraps around rather than clip.
  57  * In order to avoid overflows completely the input signal must be scaled down by <code>1/(numTaps/L)</code>.
  58  * since <code>numTaps/L</code> additions occur per output sample.
  59  * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
  60  */
  61
  62
  63 void arm_fir_interpolate_q31(
  64   const arm_fir_interpolate_instance_q31 * S,
  65   q31_t * pSrc,
  66   q31_t * pDst,
  67   uint32_t blockSize)
  68 {
  69   q31_t *pState = S->pState;                     /* State pointer */
  70   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
  71   q31_t *pStateCurnt;                            /* Points to the current sample of the state */
  72   q31_t *ptr1, *ptr2;                            /* Temporary pointers for state and coefficient buffers */
  73
  74 #ifndef ARM_MATH_CM0
  75
  76   /* Run the below code for Cortex-M4 and Cortex-M3 */
  77
  78   q63_t sum0;                                    /* Accumulators */
  79   q31_t x0, c0;                                  /* Temporary variables to hold state and coefficient values */
  80   uint32_t i, blkCnt, j;                         /* Loop counters */
  81   uint16_t phaseLen = S->phaseLength, tapCnt;    /* Length of each polyphase filter component */
  82
  83
  84   /* S->pState buffer contains previous frame (phaseLen - 1) samples */
  85   /* pStateCurnt points to the location where the new input data should be written */
  86   pStateCurnt = S->pState + ((q31_t) phaseLen - 1);
  87
  88   /* Total number of intput samples */
  89   blkCnt = blockSize;
  90
  91   /* Loop over the blockSize. */
  92   while(blkCnt > 0u)
  93   {
  94     /* Copy new input sample into the state buffer */
  95     *pStateCurnt++ = *pSrc++;
  96
  97     /* Address modifier index of coefficient buffer */
  98     j = 1u;
  99
 100     /* Loop over the Interpolation factor. */
 101     i = S->L;
 102     while(i > 0u)
 103     {
 104       /* Set accumulator to zero */
 105       sum0 = 0;
 106
 107       /* Initialize state pointer */
 108       ptr1 = pState;
 109
 110       /* Initialize coefficient pointer */
 111       ptr2 = pCoeffs + (S->L - j);
 112
 113       /* Loop over the polyPhase length. Unroll by a factor of 4.
 114        ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
 115       tapCnt = phaseLen >> 2;
 116       while(tapCnt > 0u)
 117       {
 118
 119         /* Read the coefficient */
 120         c0 = *(ptr2);
 121
 122         /* Upsampling is done by stuffing L-1 zeros between each sample.
 123          * So instead of multiplying zeros with coefficients,
 124          * Increment the coefficient pointer by interpolation factor times. */
 125         ptr2 += S->L;
 126
 127         /* Read the input sample */
 128         x0 = *(ptr1++);
 129
 130         /* Perform the multiply-accumulate */
 131         sum0 += (q63_t) x0 *c0;
 132
 133         /* Read the coefficient */
 134         c0 = *(ptr2);
 135
 136         /* Increment the coefficient pointer by interpolation factor times. */
 137         ptr2 += S->L;
 138
 139         /* Read the input sample */
 140         x0 = *(ptr1++);
 141
 142         /* Perform the multiply-accumulate */
 143         sum0 += (q63_t) x0 *c0;
 144
 145         /* Read the coefficient */
 146         c0 = *(ptr2);
 147
 148         /* Increment the coefficient pointer by interpolation factor times. */
 149         ptr2 += S->L;
 150
 151         /* Read the input sample */
 152         x0 = *(ptr1++);
 153
 154         /* Perform the multiply-accumulate */
 155         sum0 += (q63_t) x0 *c0;
 156
 157         /* Read the coefficient */
 158         c0 = *(ptr2);
 159
 160         /* Increment the coefficient pointer by interpolation factor times. */
 161         ptr2 += S->L;
 162
 163         /* Read the input sample */
 164         x0 = *(ptr1++);
 165
 166         /* Perform the multiply-accumulate */
 167         sum0 += (q63_t) x0 *c0;
 168
 169         /* Decrement the loop counter */
 170         tapCnt--;
 171       }
 172
 173       /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
 174       tapCnt = phaseLen & 0x3u;
 175
 176       while(tapCnt > 0u)
 177       {
 178         /* Read the coefficient */
 179         c0 = *(ptr2);
 180
 181         /* Increment the coefficient pointer by interpolation factor times. */
 182         ptr2 += S->L;
 183
 184         /* Read the input sample */
 185         x0 = *(ptr1++);
 186
 187         /* Perform the multiply-accumulate */
 188         sum0 += (q63_t) x0 *c0;
 189
 190         /* Decrement the loop counter */
 191         tapCnt--;
 192       }
 193
 194       /* The result is in the accumulator, store in the destination buffer. */
 195       *pDst++ = (q31_t) (sum0 >> 31);
 196
 197       /* Increment the address modifier index of coefficient buffer */
 198       j++;
 199
 200       /* Decrement the loop counter */
 201       i--;
 202     }
 203
 204     /* Advance the state pointer by 1
 205      * to process the next group of interpolation factor number samples */
 206     pState = pState + 1;
 207
 208     /* Decrement the loop counter */
 209     blkCnt--;
 210   }
 211
 212   /* Processing is complete.
 213    ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
 214    ** This prepares the state buffer for the next function call. */
 215
 216   /* Points to the start of the state buffer */
 217   pStateCurnt = S->pState;
 218
 219   tapCnt = (phaseLen - 1u) >> 2u;
 220
 221   /* copy data */
 222   while(tapCnt > 0u)
 223   {
 224     *pStateCurnt++ = *pState++;
 225     *pStateCurnt++ = *pState++;
 226     *pStateCurnt++ = *pState++;
 227     *pStateCurnt++ = *pState++;
 228
 229     /* Decrement the loop counter */
 230     tapCnt--;
 231   }
 232
 233   tapCnt = (phaseLen - 1u) % 0x04u;
 234
 235   /* copy data */
 236   while(tapCnt > 0u)
 237   {
 238     *pStateCurnt++ = *pState++;
 239
 240     /* Decrement the loop counter */
 241     tapCnt--;
 242   }
 243
 244 #else
 245
 246   /* Run the below code for Cortex-M0 */
 247
 248   q63_t sum;                                     /* Accumulator */
 249   q31_t x0, c0;                                  /* Temporary variables to hold state and coefficient values */
 250   uint32_t i, blkCnt;                            /* Loop counters */
 251   uint16_t phaseLen = S->phaseLength, tapCnt;    /* Length of each polyphase filter component */
 252
 253
 254   /* S->pState buffer contains previous frame (phaseLen - 1) samples */
 255   /* pStateCurnt points to the location where the new input data should be written */
 256   pStateCurnt = S->pState + ((q31_t) phaseLen - 1);
 257
 258   /* Total number of intput samples */
 259   blkCnt = blockSize;
 260
 261   /* Loop over the blockSize. */
 262   while(blkCnt > 0u)
 263   {
 264     /* Copy new input sample into the state buffer */
 265     *pStateCurnt++ = *pSrc++;
 266
 267     /* Loop over the Interpolation factor. */
 268     i = S->L;
 269
 270     while(i > 0u)
 271     {
 272       /* Set accumulator to zero */
 273       sum = 0;
 274
 275       /* Initialize state pointer */
 276       ptr1 = pState;
 277
 278       /* Initialize coefficient pointer */
 279       ptr2 = pCoeffs + (i - 1u);
 280
 281       tapCnt = phaseLen;
 282
 283       while(tapCnt > 0u)
 284       {
 285         /* Read the coefficient */
 286         c0 = *(ptr2);
 287
 288         /* Increment the coefficient pointer by interpolation factor times. */
 289         ptr2 += S->L;
 290
 291         /* Read the input sample */
 292         x0 = *ptr1++;
 293
 294         /* Perform the multiply-accumulate */
 295         sum += (q63_t) x0 *c0;
 296
 297         /* Decrement the loop counter */
 298         tapCnt--;
 299       }
 300
 301       /* The result is in the accumulator, store in the destination buffer. */
 302       *pDst++ = (q31_t) (sum >> 31);
 303
 304       /* Decrement the loop counter */
 305       i--;
 306     }
 307
 308     /* Advance the state pointer by 1
 309      * to process the next group of interpolation factor number samples */
 310     pState = pState + 1;
 311
 312     /* Decrement the loop counter */
 313     blkCnt--;
 314   }
 315
 316   /* Processing is complete.
 317    ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
 318    ** This prepares the state buffer for the next function call. */
 319
 320   /* Points to the start of the state buffer */
 321   pStateCurnt = S->pState;
 322
 323   tapCnt = phaseLen - 1u;
 324
 325   /* copy data */
 326   while(tapCnt > 0u)
 327   {
 328     *pStateCurnt++ = *pState++;
 329
 330     /* Decrement the loop counter */
 331     tapCnt--;
 332   }
 333
 334 #endif /*   #ifndef ARM_MATH_CM0 */
 335
 336 }
 337
 338  /**
 339   * @} end of FIR_Interpolate group
 340   */