tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_sparse_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_sparse_q31.c
   9 *
  10 * Description:  Q31 sparse 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 #include "arm_math.h"
  41
  42
  43 /**
  44  * @addtogroup FIR_Sparse
  45  * @{
  46  */
  47
  48 /**
  49  * @brief Processing function for the Q31 sparse FIR filter.
  50  * @param[in]  *S          points to an instance of the Q31 sparse FIR structure.
  51  * @param[in]  *pSrc       points to the block of input data.
  52  * @param[out] *pDst       points to the block of output data
  53  * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.
  54  * @param[in]  blockSize   number of input samples to process per call.
  55  * @return none.
  56  *
  57  * <b>Scaling and Overflow Behavior:</b>
  58  * \par
  59  * The function is implemented using an internal 32-bit accumulator.
  60  * The 1.31 x 1.31 multiplications are truncated to 2.30 format.
  61  * This leads to loss of precision on the intermediate multiplications and provides only a single guard bit.
  62  * If the accumulator result overflows, it wraps around rather than saturate.
  63  * In order to avoid overflows the input signal or coefficients must be scaled down by log2(numTaps) bits.
  64  */
  65
  66 void arm_fir_sparse_q31(
  67   arm_fir_sparse_instance_q31 * S,
  68   q31_t * pSrc,
  69   q31_t * pDst,
  70   q31_t * pScratchIn,
  71   uint32_t blockSize)
  72 {
  73
  74   q31_t *pState = S->pState;                     /* State pointer */
  75   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
  76   q31_t *px;                                     /* Scratch buffer pointer */
  77   q31_t *py = pState;                            /* Temporary pointers for state buffer */
  78   q31_t *pb = pScratchIn;                        /* Temporary pointers for scratch buffer */
  79   q31_t *pOut;                                   /* Destination pointer */
  80   q63_t out;                                     /* Temporary output variable */
  81   int32_t *pTapDelay = S->pTapDelay;             /* Pointer to the array containing offset of the non-zero tap values. */
  82   uint32_t delaySize = S->maxDelay + blockSize;  /* state length */
  83   uint16_t numTaps = S->numTaps;                 /* Filter order */
  84   int32_t readIndex;                             /* Read index of the state buffer */
  85   uint32_t tapCnt, blkCnt;                       /* loop counters */
  86   q31_t coeff = *pCoeffs++;                      /* Read the first coefficient value */
  87   q31_t in;
  88
  89
  90   /* BlockSize of Input samples are copied into the state buffer */
  91   /* StateIndex points to the starting position to write in the state buffer */
  92   arm_circularWrite_f32((int32_t *) py, delaySize, &S->stateIndex, 1,
  93                         (int32_t *) pSrc, 1, blockSize);
  94
  95   /* Read Index, from where the state buffer should be read, is calculated. */
  96   readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
  97
  98   /* Wraparound of readIndex */
  99   if(readIndex < 0)
 100   {
 101     readIndex += (int32_t) delaySize;
 102   }
 103
 104   /* Working pointer for state buffer is updated */
 105   py = pState;
 106
 107   /* blockSize samples are read from the state buffer */
 108   arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
 109                        (int32_t *) pb, (int32_t *) pb, blockSize, 1,
 110                        blockSize);
 111
 112   /* Working pointer for the scratch buffer of state values */
 113   px = pb;
 114
 115   /* Working pointer for scratch buffer of output values */
 116   pOut = pDst;
 117
 118
 119 #ifndef ARM_MATH_CM0_FAMILY
 120
 121   /* Run the below code for Cortex-M4 and Cortex-M3 */
 122
 123   /* Loop over the blockSize. Unroll by a factor of 4.
 124    * Compute 4 Multiplications at a time. */
 125   blkCnt = blockSize >> 2;
 126
 127   while(blkCnt > 0u)
 128   {
 129     /* Perform Multiplications and store in the destination buffer */
 130     *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
 131     *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
 132     *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
 133     *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
 134
 135     /* Decrement the loop counter */
 136     blkCnt--;
 137   }
 138
 139   /* If the blockSize is not a multiple of 4,
 140    * compute the remaining samples */
 141   blkCnt = blockSize % 0x4u;
 142
 143   while(blkCnt > 0u)
 144   {
 145     /* Perform Multiplications and store in the destination buffer */
 146     *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
 147
 148     /* Decrement the loop counter */
 149     blkCnt--;
 150   }
 151
 152   /* Load the coefficient value and
 153    * increment the coefficient buffer for the next set of state values */
 154   coeff = *pCoeffs++;
 155
 156   /* Read Index, from where the state buffer should be read, is calculated. */
 157   readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
 158
 159   /* Wraparound of readIndex */
 160   if(readIndex < 0)
 161   {
 162     readIndex += (int32_t) delaySize;
 163   }
 164
 165   /* Loop over the number of taps. */
 166   tapCnt = (uint32_t) numTaps - 1u;
 167
 168   while(tapCnt > 0u)
 169   {
 170     /* Working pointer for state buffer is updated */
 171     py = pState;
 172
 173     /* blockSize samples are read from the state buffer */
 174     arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
 175                          (int32_t *) pb, (int32_t *) pb, blockSize, 1,
 176                          blockSize);
 177
 178     /* Working pointer for the scratch buffer of state values */
 179     px = pb;
 180
 181     /* Working pointer for scratch buffer of output values */
 182     pOut = pDst;
 183
 184     /* Loop over the blockSize. Unroll by a factor of 4.
 185      * Compute 4 MACS at a time. */
 186     blkCnt = blockSize >> 2;
 187
 188     while(blkCnt > 0u)
 189     {
 190       out = *pOut;
 191       out += ((q63_t) * px++ * coeff) >> 32;
 192       *pOut++ = (q31_t) (out);
 193
 194       out = *pOut;
 195       out += ((q63_t) * px++ * coeff) >> 32;
 196       *pOut++ = (q31_t) (out);
 197
 198       out = *pOut;
 199       out += ((q63_t) * px++ * coeff) >> 32;
 200       *pOut++ = (q31_t) (out);
 201
 202       out = *pOut;
 203       out += ((q63_t) * px++ * coeff) >> 32;
 204       *pOut++ = (q31_t) (out);
 205
 206       /* Decrement the loop counter */
 207       blkCnt--;
 208     }
 209
 210     /* If the blockSize is not a multiple of 4,
 211      * compute the remaining samples */
 212     blkCnt = blockSize % 0x4u;
 213
 214     while(blkCnt > 0u)
 215     {
 216       /* Perform Multiply-Accumulate */
 217       out = *pOut;
 218       out += ((q63_t) * px++ * coeff) >> 32;
 219       *pOut++ = (q31_t) (out);
 220
 221       /* Decrement the loop counter */
 222       blkCnt--;
 223     }
 224
 225     /* Load the coefficient value and
 226      * increment the coefficient buffer for the next set of state values */
 227     coeff = *pCoeffs++;
 228
 229     /* Read Index, from where the state buffer should be read, is calculated. */
 230     readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
 231
 232     /* Wraparound of readIndex */
 233     if(readIndex < 0)
 234     {
 235       readIndex += (int32_t) delaySize;
 236     }
 237
 238     /* Decrement the tap loop counter */
 239     tapCnt--;
 240   }
 241
 242   /* Working output pointer is updated */
 243   pOut = pDst;
 244
 245   /* Output is converted into 1.31 format. */
 246   /* Loop over the blockSize. Unroll by a factor of 4.
 247    * process 4 output samples at a time. */
 248   blkCnt = blockSize >> 2;
 249
 250   while(blkCnt > 0u)
 251   {
 252     in = *pOut << 1;
 253     *pOut++ = in;
 254     in = *pOut << 1;
 255     *pOut++ = in;
 256     in = *pOut << 1;
 257     *pOut++ = in;
 258     in = *pOut << 1;
 259     *pOut++ = in;
 260
 261     /* Decrement the loop counter */
 262     blkCnt--;
 263   }
 264
 265   /* If the blockSize is not a multiple of 4,
 266    * process the remaining output samples */
 267   blkCnt = blockSize % 0x4u;
 268
 269   while(blkCnt > 0u)
 270   {
 271     in = *pOut << 1;
 272     *pOut++ = in;
 273
 274     /* Decrement the loop counter */
 275     blkCnt--;
 276   }
 277
 278 #else
 279
 280   /* Run the below code for Cortex-M0 */
 281   blkCnt = blockSize;
 282
 283   while(blkCnt > 0u)
 284   {
 285     /* Perform Multiplications and store in the destination buffer */
 286     *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
 287
 288     /* Decrement the loop counter */
 289     blkCnt--;
 290   }
 291
 292   /* Load the coefficient value and
 293    * increment the coefficient buffer for the next set of state values */
 294   coeff = *pCoeffs++;
 295
 296   /* Read Index, from where the state buffer should be read, is calculated. */
 297   readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
 298
 299   /* Wraparound of readIndex */
 300   if(readIndex < 0)
 301   {
 302     readIndex += (int32_t) delaySize;
 303   }
 304
 305   /* Loop over the number of taps. */
 306   tapCnt = (uint32_t) numTaps - 1u;
 307
 308   while(tapCnt > 0u)
 309   {
 310     /* Working pointer for state buffer is updated */
 311     py = pState;
 312
 313     /* blockSize samples are read from the state buffer */
 314     arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
 315                          (int32_t *) pb, (int32_t *) pb, blockSize, 1,
 316                          blockSize);
 317
 318     /* Working pointer for the scratch buffer of state values */
 319     px = pb;
 320
 321     /* Working pointer for scratch buffer of output values */
 322     pOut = pDst;
 323
 324     blkCnt = blockSize;
 325
 326     while(blkCnt > 0u)
 327     {
 328       /* Perform Multiply-Accumulate */
 329       out = *pOut;
 330       out += ((q63_t) * px++ * coeff) >> 32;
 331       *pOut++ = (q31_t) (out);
 332
 333       /* Decrement the loop counter */
 334       blkCnt--;
 335     }
 336
 337     /* Load the coefficient value and
 338      * increment the coefficient buffer for the next set of state values */
 339     coeff = *pCoeffs++;
 340
 341     /* Read Index, from where the state buffer should be read, is calculated. */
 342     readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
 343
 344     /* Wraparound of readIndex */
 345     if(readIndex < 0)
 346     {
 347       readIndex += (int32_t) delaySize;
 348     }
 349
 350     /* Decrement the tap loop counter */
 351     tapCnt--;
 352   }
 353
 354   /* Working output pointer is updated */
 355   pOut = pDst;
 356
 357   /* Output is converted into 1.31 format. */
 358   blkCnt = blockSize;
 359
 360   while(blkCnt > 0u)
 361   {
 362     in = *pOut << 1;
 363     *pOut++ = in;
 364
 365     /* Decrement the loop counter */
 366     blkCnt--;
 367   }
 368
 369 #endif /*   #ifndef ARM_MATH_CM0_FAMILY */
 370
 371 }
 372
 373 /**
 374  * @} end of FIR_Sparse group
 375  */