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