tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_fast_q15.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_fast_q15.c
   9 *
  10 * Description:  Q15 Fast FIR filter processing function.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3
  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 Q15 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  * <b>Scaling and Overflow Behavior:</b>
  60  * \par
  61  * This fast version uses a 32-bit accumulator with 2.30 format.
  62  * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
  63  * Thus, if the accumulator result overflows it wraps around and distorts the result.
  64  * In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits.
  65  * The 2.30 accumulator is then truncated to 2.15 format and saturated to yield the 1.15 result.
  66  *
  67  * \par
  68  * Refer to the function <code>arm_fir_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.  Both the slow and the fast versions use the same instance structure.
  69  * Use the function <code>arm_fir_init_q15()</code> to initialize the filter structure.
  70  */
  71
  72 void arm_fir_fast_q15(
  73   const arm_fir_instance_q15 * S,
  74   q15_t * pSrc,
  75   q15_t * pDst,
  76   uint32_t blockSize)
  77 {
  78   q15_t *pState = S->pState;                     /* State pointer */
  79   q15_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
  80   q15_t *pStateCurnt;                            /* Points to the current sample of the state */
  81   q31_t acc0, acc1, acc2, acc3;                  /* Accumulators */
  82   q15_t *pb;                                     /* Temporary pointer for coefficient buffer */
  83   q15_t *px;                                     /* Temporary q31 pointer for SIMD state buffer accesses */
  84   q31_t x0, x1, x2, c0;                          /* Temporary variables to hold SIMD state and coefficient values */
  85   uint32_t numTaps = S->numTaps;                 /* Number of taps in the filter */
  86   uint32_t tapCnt, blkCnt;                       /* Loop counters */
  87
  88
  89   /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
  90   /* pStateCurnt points to the location where the new input data should be written */
  91   pStateCurnt = &(S->pState[(numTaps - 1u)]);
  92
  93   /* Apply loop unrolling and compute 4 output values simultaneously.
  94    * The variables acc0 ... acc3 hold output values that are being computed:
  95    *
  96    *    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]
  97    *    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]
  98    *    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]
  99    *    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]
 100    */
 101
 102   blkCnt = blockSize >> 2;
 103
 104   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
 105    ** a second loop below computes the remaining 1 to 3 samples. */
 106   while(blkCnt > 0u)
 107   {
 108     /* Copy four new input samples into the state buffer.
 109      ** Use 32-bit SIMD to move the 16-bit data.  Only requires two copies. */
 110     *pStateCurnt++ = *pSrc++;
 111     *pStateCurnt++ = *pSrc++;
 112     *pStateCurnt++ = *pSrc++;
 113     *pStateCurnt++ = *pSrc++;
 114
 115
 116     /* Set all accumulators to zero */
 117     acc0 = 0;
 118     acc1 = 0;
 119     acc2 = 0;
 120     acc3 = 0;
 121
 122     /* Typecast q15_t pointer to q31_t pointer for state reading in q31_t */
 123     px = pState;
 124
 125     /* Typecast q15_t pointer to q31_t pointer for coefficient reading in q31_t */
 126     pb = pCoeffs;
 127
 128     /* Read the first two samples from the state buffer:  x[n-N], x[n-N-1] */
 129     x0 = *__SIMD32(px)++;
 130
 131     /* Read the third and forth samples from the state buffer: x[n-N-2], x[n-N-3] */
 132     x2 = *__SIMD32(px)++;
 133
 134     /* Loop over the number of taps.  Unroll by a factor of 4.
 135      ** Repeat until we've computed numTaps-(numTaps%4) coefficients. */
 136     tapCnt = numTaps >> 2;
 137
 138     while(tapCnt > 0)
 139     {
 140       /* Read the first two coefficients using SIMD:  b[N] and b[N-1] coefficients */
 141       c0 = *__SIMD32(pb)++;
 142
 143       /* acc0 +=  b[N] * x[n-N] + b[N-1] * x[n-N-1] */
 144       acc0 = __SMLAD(x0, c0, acc0);
 145
 146       /* acc2 +=  b[N] * x[n-N-2] + b[N-1] * x[n-N-3] */
 147       acc2 = __SMLAD(x2, c0, acc2);
 148
 149       /* pack  x[n-N-1] and x[n-N-2] */
 150 #ifndef ARM_MATH_BIG_ENDIAN
 151       x1 = __PKHBT(x2, x0, 0);
 152 #else
 153       x1 = __PKHBT(x0, x2, 0);
 154 #endif
 155
 156       /* Read state x[n-N-4], x[n-N-5] */
 157       x0 = _SIMD32_OFFSET(px);
 158
 159       /* acc1 +=  b[N] * x[n-N-1] + b[N-1] * x[n-N-2] */
 160       acc1 = __SMLADX(x1, c0, acc1);
 161
 162       /* pack  x[n-N-3] and x[n-N-4] */
 163 #ifndef ARM_MATH_BIG_ENDIAN
 164       x1 = __PKHBT(x0, x2, 0);
 165 #else
 166       x1 = __PKHBT(x2, x0, 0);
 167 #endif
 168
 169       /* acc3 +=  b[N] * x[n-N-3] + b[N-1] * x[n-N-4] */
 170       acc3 = __SMLADX(x1, c0, acc3);
 171
 172       /* Read coefficients b[N-2], b[N-3] */
 173       c0 = *__SIMD32(pb)++;
 174
 175       /* acc0 +=  b[N-2] * x[n-N-2] + b[N-3] * x[n-N-3] */
 176       acc0 = __SMLAD(x2, c0, acc0);
 177
 178       /* Read state x[n-N-6], x[n-N-7] with offset */
 179       x2 = _SIMD32_OFFSET(px + 2u);
 180
 181       /* acc2 +=  b[N-2] * x[n-N-4] + b[N-3] * x[n-N-5] */
 182       acc2 = __SMLAD(x0, c0, acc2);
 183
 184       /* acc1 +=  b[N-2] * x[n-N-3] + b[N-3] * x[n-N-4] */
 185       acc1 = __SMLADX(x1, c0, acc1);
 186
 187       /* pack  x[n-N-5] and x[n-N-6] */
 188 #ifndef ARM_MATH_BIG_ENDIAN
 189       x1 = __PKHBT(x2, x0, 0);
 190 #else
 191       x1 = __PKHBT(x0, x2, 0);
 192 #endif
 193
 194       /* acc3 +=  b[N-2] * x[n-N-5] + b[N-3] * x[n-N-6] */
 195       acc3 = __SMLADX(x1, c0, acc3);
 196
 197       /* Update state pointer for next state reading */
 198       px += 4u;
 199
 200       /* Decrement tap count */
 201       tapCnt--;
 202
 203     }
 204
 205     /* If the filter length is not a multiple of 4, compute the remaining filter taps.
 206      ** This is always be 2 taps since the filter length is even. */
 207     if((numTaps & 0x3u) != 0u)
 208     {
 209
 210       /* Read last two coefficients */
 211       c0 = *__SIMD32(pb)++;
 212
 213       /* Perform the multiply-accumulates */
 214       acc0 = __SMLAD(x0, c0, acc0);
 215       acc2 = __SMLAD(x2, c0, acc2);
 216
 217       /* pack state variables */
 218 #ifndef ARM_MATH_BIG_ENDIAN
 219       x1 = __PKHBT(x2, x0, 0);
 220 #else
 221       x1 = __PKHBT(x0, x2, 0);
 222 #endif
 223
 224       /* Read last state variables */
 225       x0 = *__SIMD32(px);
 226
 227       /* Perform the multiply-accumulates */
 228       acc1 = __SMLADX(x1, c0, acc1);
 229
 230       /* pack state variables */
 231 #ifndef ARM_MATH_BIG_ENDIAN
 232       x1 = __PKHBT(x0, x2, 0);
 233 #else
 234       x1 = __PKHBT(x2, x0, 0);
 235 #endif
 236
 237       /* Perform the multiply-accumulates */
 238       acc3 = __SMLADX(x1, c0, acc3);
 239     }
 240
 241     /* The results in the 4 accumulators are in 2.30 format.  Convert to 1.15 with saturation.
 242      ** Then store the 4 outputs in the destination buffer. */
 243
 244 #ifndef ARM_MATH_BIG_ENDIAN
 245
 246     *__SIMD32(pDst)++ =
 247       __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16);
 248
 249     *__SIMD32(pDst)++ =
 250       __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16);
 251
 252 #else
 253
 254     *__SIMD32(pDst)++ =
 255       __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16);
 256
 257     *__SIMD32(pDst)++ =
 258       __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16);
 259
 260
 261 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN       */
 262
 263     /* Advance the state pointer by 4 to process the next group of 4 samples */
 264     pState = pState + 4u;
 265
 266     /* Decrement the loop counter */
 267     blkCnt--;
 268   }
 269
 270   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
 271    ** No loop unrolling is used. */
 272   blkCnt = blockSize % 0x4u;
 273   while(blkCnt > 0u)
 274   {
 275     /* Copy two samples into state buffer */
 276     *pStateCurnt++ = *pSrc++;
 277
 278     /* Set the accumulator to zero */
 279     acc0 = 0;
 280
 281     /* Use SIMD to hold states and coefficients */
 282     px = pState;
 283     pb = pCoeffs;
 284
 285     tapCnt = numTaps >> 1u;
 286
 287     do
 288     {
 289
 290       acc0 += (q31_t) * px++ * *pb++;
 291           acc0 += (q31_t) * px++ * *pb++;
 292
 293       tapCnt--;
 294     }
 295     while(tapCnt > 0u);
 296
 297     /* The result is in 2.30 format.  Convert to 1.15 with saturation.
 298      ** Then store the output in the destination buffer. */
 299     *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
 300
 301     /* Advance state pointer by 1 for the next sample */
 302     pState = pState + 1u;
 303
 304     /* Decrement the loop counter */
 305     blkCnt--;
 306   }
 307
 308   /* Processing is complete.
 309    ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
 310    ** This prepares the state buffer for the next function call. */
 311
 312   /* Points to the start of the state buffer */
 313   pStateCurnt = S->pState;
 314
 315   /* Calculation of count for copying integer writes */
 316   tapCnt = (numTaps - 1u) >> 2;
 317
 318   while(tapCnt > 0u)
 319   {
 320     *pStateCurnt++ = *pState++;
 321     *pStateCurnt++ = *pState++;
 322     *pStateCurnt++ = *pState++;
 323     *pStateCurnt++ = *pState++;
 324
 325     tapCnt--;
 326
 327   }
 328
 329   /* Calculation of count for remaining q15_t data */
 330   tapCnt = (numTaps - 1u) % 0x4u;
 331
 332   /* copy remaining data */
 333   while(tapCnt > 0u)
 334   {
 335     *pStateCurnt++ = *pState++;
 336
 337     /* Decrement the loop counter */
 338     tapCnt--;
 339   }
 340
 341 }
 342
 343 /**
 344  * @} end of FIR group
 345  */