tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_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_fir_q31.c
   9 *
  10 * Description:  Q31 FIR filter processing function.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Redistribution and use in source and binary forms, with or without
  15 * modification, are permitted provided that the following conditions
  16 * are met:
  17 *   - Redistributions of source code must retain the above copyright
  18 *     notice, this list of conditions and the following disclaimer.
  19 *   - Redistributions in binary form must reproduce the above copyright
  20 *     notice, this list of conditions and the following disclaimer in
  21 *     the documentation and/or other materials provided with the
  22 *     distribution.
  23 *   - Neither the name of ARM LIMITED nor the names of its contributors
  24 *     may be used to endorse or promote products derived from this
  25 *     software without specific prior written permission.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38 * POSSIBILITY OF SUCH DAMAGE.
  39 * -------------------------------------------------------------------- */
  40
  41 #include "arm_math.h"
  42
  43 /**
  44  * @ingroup groupFilters
  45  */
  46
  47 /**
  48  * @addtogroup FIR
  49  * @{
  50  */
  51
  52 /**
  53  * @param[in] *S points to an instance of the Q31 FIR filter structure.
  54  * @param[in] *pSrc points to the block of input data.
  55  * @param[out] *pDst points to the block of output data.
  56  * @param[in] blockSize number of samples to process per call.
  57  * @return none.
  58  *
  59  * @details
  60  * <b>Scaling and Overflow Behavior:</b>
  61  * \par
  62  * The function is implemented using an internal 64-bit accumulator.
  63  * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
  64  * Thus, if the accumulator result overflows it wraps around rather than clip.
  65  * In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits.
  66  * After all multiply-accumulates are performed, the 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
  67  *
  68  * \par
  69  * Refer to the function <code>arm_fir_fast_q31()</code> for a faster but less precise implementation of this filter for Cortex-M3 and Cortex-M4.
  70  */
  71
  72 void arm_fir_q31(
  73   const arm_fir_instance_q31 * S,
  74   q31_t * pSrc,
  75   q31_t * pDst,
  76   uint32_t blockSize)
  77 {
  78   q31_t *pState = S->pState;                     /* State pointer */
  79   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
  80   q31_t *pStateCurnt;                            /* Points to the current sample of the state */
  81
  82
  83 #ifndef ARM_MATH_CM0_FAMILY
  84
  85   /* Run the below code for Cortex-M4 and Cortex-M3 */
  86
  87   q31_t x0, x1, x2;                              /* Temporary variables to hold state */
  88   q31_t c0;                                      /* Temporary variable to hold coefficient value */
  89   q31_t *px;                                     /* Temporary pointer for state */
  90   q31_t *pb;                                     /* Temporary pointer for coefficient buffer */
  91   q63_t acc0, acc1, acc2;                        /* Accumulators */
  92   uint32_t numTaps = S->numTaps;                 /* Number of filter coefficients in the filter */
  93   uint32_t i, tapCnt, blkCnt, tapCntN3;          /* Loop counters */
  94
  95   /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
  96   /* pStateCurnt points to the location where the new input data should be written */
  97   pStateCurnt = &(S->pState[(numTaps - 1u)]);
  98
  99   /* Apply loop unrolling and compute 4 output values simultaneously.
 100    * The variables acc0 ... acc3 hold output values that are being computed:
 101    *
 102    *    acc0 =  b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
 103    *    acc1 =  b[numTaps-1] * x[n-numTaps] +   b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
 104    *    acc2 =  b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] +   b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
 105    *    acc3 =  b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps]   +...+ b[0] * x[3]
 106    */
 107   blkCnt = blockSize / 3;
 108   blockSize = blockSize - (3 * blkCnt);
 109
 110   tapCnt = numTaps / 3;
 111   tapCntN3 = numTaps - (3 * tapCnt);
 112
 113   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
 114    ** a second loop below computes the remaining 1 to 3 samples. */
 115   while(blkCnt > 0u)
 116   {
 117     /* Copy three new input samples into the state buffer */
 118     *pStateCurnt++ = *pSrc++;
 119     *pStateCurnt++ = *pSrc++;
 120     *pStateCurnt++ = *pSrc++;
 121
 122     /* Set all accumulators to zero */
 123     acc0 = 0;
 124     acc1 = 0;
 125     acc2 = 0;
 126
 127     /* Initialize state pointer */
 128     px = pState;
 129
 130     /* Initialize coefficient pointer */
 131     pb = pCoeffs;
 132
 133     /* Read the first two samples from the state buffer:
 134      *  x[n-numTaps], x[n-numTaps-1] */
 135     x0 = *(px++);
 136     x1 = *(px++);
 137
 138     /* Loop unrolling.  Process 3 taps at a time. */
 139     i = tapCnt;
 140
 141     while(i > 0u)
 142     {
 143       /* Read the b[numTaps] coefficient */
 144       c0 = *pb;
 145
 146       /* Read x[n-numTaps-2] sample */
 147       x2 = *(px++);
 148
 149       /* Perform the multiply-accumulates */
 150       acc0 += ((q63_t) x0 * c0);
 151       acc1 += ((q63_t) x1 * c0);
 152       acc2 += ((q63_t) x2 * c0);
 153
 154       /* Read the coefficient and state */
 155       c0 = *(pb + 1u);
 156       x0 = *(px++);
 157
 158       /* Perform the multiply-accumulates */
 159       acc0 += ((q63_t) x1 * c0);
 160       acc1 += ((q63_t) x2 * c0);
 161       acc2 += ((q63_t) x0 * c0);
 162
 163       /* Read the coefficient and state */
 164       c0 = *(pb + 2u);
 165       x1 = *(px++);
 166
 167       /* update coefficient pointer */
 168       pb += 3u;
 169
 170       /* Perform the multiply-accumulates */
 171       acc0 += ((q63_t) x2 * c0);
 172       acc1 += ((q63_t) x0 * c0);
 173       acc2 += ((q63_t) x1 * c0);
 174
 175       /* Decrement the loop counter */
 176       i--;
 177     }
 178
 179     /* If the filter length is not a multiple of 3, compute the remaining filter taps */
 180
 181     i = tapCntN3;
 182
 183     while(i > 0u)
 184     {
 185       /* Read coefficients */
 186       c0 = *(pb++);
 187
 188       /* Fetch 1 state variable */
 189       x2 = *(px++);
 190
 191       /* Perform the multiply-accumulates */
 192       acc0 += ((q63_t) x0 * c0);
 193       acc1 += ((q63_t) x1 * c0);
 194       acc2 += ((q63_t) x2 * c0);
 195
 196       /* Reuse the present sample states for next sample */
 197       x0 = x1;
 198       x1 = x2;
 199
 200       /* Decrement the loop counter */
 201       i--;
 202     }
 203
 204     /* Advance the state pointer by 3 to process the next group of 3 samples */
 205     pState = pState + 3;
 206
 207     /* The results in the 3 accumulators are in 2.30 format.  Convert to 1.31
 208      ** Then store the 3 outputs in the destination buffer. */
 209     *pDst++ = (q31_t) (acc0 >> 31u);
 210     *pDst++ = (q31_t) (acc1 >> 31u);
 211     *pDst++ = (q31_t) (acc2 >> 31u);
 212
 213     /* Decrement the samples loop counter */
 214     blkCnt--;
 215   }
 216
 217   /* If the blockSize is not a multiple of 3, compute any remaining output samples here.
 218    ** No loop unrolling is used. */
 219
 220   while(blockSize > 0u)
 221   {
 222     /* Copy one sample at a time into state buffer */
 223     *pStateCurnt++ = *pSrc++;
 224
 225     /* Set the accumulator to zero */
 226     acc0 = 0;
 227
 228     /* Initialize state pointer */
 229     px = pState;
 230
 231     /* Initialize Coefficient pointer */
 232     pb = (pCoeffs);
 233
 234     i = numTaps;
 235
 236     /* Perform the multiply-accumulates */
 237     do
 238     {
 239       acc0 += (q63_t) * (px++) * (*(pb++));
 240       i--;
 241     } while(i > 0u);
 242
 243     /* The result is in 2.62 format.  Convert to 1.31
 244      ** Then store the output in the destination buffer. */
 245     *pDst++ = (q31_t) (acc0 >> 31u);
 246
 247     /* Advance state pointer by 1 for the next sample */
 248     pState = pState + 1;
 249
 250     /* Decrement the samples loop counter */
 251     blockSize--;
 252   }
 253
 254   /* Processing is complete.
 255    ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
 256    ** This prepares the state buffer for the next function call. */
 257
 258   /* Points to the start of the state buffer */
 259   pStateCurnt = S->pState;
 260
 261   tapCnt = (numTaps - 1u) >> 2u;
 262
 263   /* copy data */
 264   while(tapCnt > 0u)
 265   {
 266     *pStateCurnt++ = *pState++;
 267     *pStateCurnt++ = *pState++;
 268     *pStateCurnt++ = *pState++;
 269     *pStateCurnt++ = *pState++;
 270
 271     /* Decrement the loop counter */
 272     tapCnt--;
 273   }
 274
 275   /* Calculate remaining number of copies */
 276   tapCnt = (numTaps - 1u) % 0x4u;
 277
 278   /* Copy the remaining q31_t data */
 279   while(tapCnt > 0u)
 280   {
 281     *pStateCurnt++ = *pState++;
 282
 283     /* Decrement the loop counter */
 284     tapCnt--;
 285   }
 286
 287 #else
 288
 289 /* Run the below code for Cortex-M0 */
 290
 291   q31_t *px;                                     /* Temporary pointer for state */
 292   q31_t *pb;                                     /* Temporary pointer for coefficient buffer */
 293   q63_t acc;                                     /* Accumulator */
 294   uint32_t numTaps = S->numTaps;                 /* Length of the filter */
 295   uint32_t i, tapCnt, blkCnt;                    /* Loop counters */
 296
 297   /* S->pState buffer contains previous frame (numTaps - 1) samples */
 298   /* pStateCurnt points to the location where the new input data should be written */
 299   pStateCurnt = &(S->pState[(numTaps - 1u)]);
 300
 301   /* Initialize blkCnt with blockSize */
 302   blkCnt = blockSize;
 303
 304   while(blkCnt > 0u)
 305   {
 306     /* Copy one sample at a time into state buffer */
 307     *pStateCurnt++ = *pSrc++;
 308
 309     /* Set the accumulator to zero */
 310     acc = 0;
 311
 312     /* Initialize state pointer */
 313     px = pState;
 314
 315     /* Initialize Coefficient pointer */
 316     pb = pCoeffs;
 317
 318     i = numTaps;
 319
 320     /* Perform the multiply-accumulates */
 321     do
 322     {
 323       /* acc =  b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
 324       acc += (q63_t) * px++ * *pb++;
 325       i--;
 326     } while(i > 0u);
 327
 328     /* The result is in 2.62 format.  Convert to 1.31
 329      ** Then store the output in the destination buffer. */
 330     *pDst++ = (q31_t) (acc >> 31u);
 331
 332     /* Advance state pointer by 1 for the next sample */
 333     pState = pState + 1;
 334
 335     /* Decrement the samples loop counter */
 336     blkCnt--;
 337   }
 338
 339   /* Processing is complete.
 340    ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
 341    ** This prepares the state buffer for the next function call. */
 342
 343   /* Points to the start of the state buffer */
 344   pStateCurnt = S->pState;
 345
 346   /* Copy numTaps number of values */
 347   tapCnt = numTaps - 1u;
 348
 349   /* Copy the data */
 350   while(tapCnt > 0u)
 351   {
 352     *pStateCurnt++ = *pState++;
 353
 354     /* Decrement the loop counter */
 355     tapCnt--;
 356   }
 357
 358
 359 #endif /*  #ifndef ARM_MATH_CM0_FAMILY */
 360
 361 }
 362
 363 /**
 364  * @} end of FIR group
 365  */