git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_q15.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_lattice_q15.c
   9 *
  10 * Description:  Q15 FIR lattice 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
  33 #include "arm_math.h"
  34
  35 /**
  36  * @ingroup groupFilters
  37  */
  38
  39 /**
  40  * @addtogroup FIR_Lattice
  41  * @{
  42  */
  43
  44
  45 /**
  46  * @brief Processing function for the Q15 FIR lattice filter.
  47  * @param[in]  *S        points to an instance of the Q15 FIR lattice 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]  blockSize number of samples to process.
  51  * @return none.
  52  */
  53
  54 void arm_fir_lattice_q15(
  55   const arm_fir_lattice_instance_q15 * S,
  56   q15_t * pSrc,
  57   q15_t * pDst,
  58   uint32_t blockSize)
  59 {
  60   q15_t *pState;                                 /* State pointer */
  61   q15_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
  62   q15_t *px;                                     /* temporary state pointer */
  63   q15_t *pk;                                     /* temporary coefficient pointer */
  64
  65
  66 #ifndef ARM_MATH_CM0
  67
  68   /* Run the below code for Cortex-M4 and Cortex-M3 */
  69
  70   q31_t fcurnt1, fnext1, gcurnt1 = 0, gnext1;    /* temporary variables for first sample in loop unrolling */
  71   q31_t fcurnt2, fnext2, gnext2;                 /* temporary variables for second sample in loop unrolling */
  72   q31_t fcurnt3, fnext3, gnext3;                 /* temporary variables for third sample in loop unrolling */
  73   q31_t fcurnt4, fnext4, gnext4;                 /* temporary variables for fourth sample in loop unrolling */
  74   uint32_t numStages = S->numStages;             /* Number of stages in the filter */
  75   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
  76
  77   pState = &S->pState[0];
  78
  79   blkCnt = blockSize >> 2u;
  80
  81   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
  82    ** a second loop below computes the remaining 1 to 3 samples. */
  83   while(blkCnt > 0u)
  84   {
  85
  86     /* Read two samples from input buffer */
  87     /* f0(n) = x(n) */
  88     fcurnt1 = *pSrc++;
  89     fcurnt2 = *pSrc++;
  90
  91     /* Initialize coeff pointer */
  92     pk = (pCoeffs);
  93
  94     /* Initialize state pointer */
  95     px = pState;
  96
  97     /* Read g0(n-1) from state */
  98     gcurnt1 = *px;
  99
 100     /* Process first sample for first tap */
 101     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 102     fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
 103     fnext1 = __SSAT(fnext1, 16);
 104
 105     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 106     gnext1 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + gcurnt1;
 107     gnext1 = __SSAT(gnext1, 16);
 108
 109     /* Process second sample for first tap */
 110     /* for sample 2 processing */
 111     fnext2 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + fcurnt2;
 112     fnext2 = __SSAT(fnext2, 16);
 113
 114     gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt1;
 115     gnext2 = __SSAT(gnext2, 16);
 116
 117
 118     /* Read next two samples from input buffer */
 119     /* f0(n+2) = x(n+2) */
 120     fcurnt3 = *pSrc++;
 121     fcurnt4 = *pSrc++;
 122
 123     /* Copy only last input samples into the state buffer
 124        which is used for next four samples processing */
 125     *px++ = (q15_t) fcurnt4;
 126
 127     /* Process third sample for first tap */
 128     fnext3 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt3;
 129     fnext3 = __SSAT(fnext3, 16);
 130     gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt2;
 131     gnext3 = __SSAT(gnext3, 16);
 132
 133     /* Process fourth sample for first tap */
 134     fnext4 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt4;
 135     fnext4 = __SSAT(fnext4, 16);
 136     gnext4 = (q31_t) ((fcurnt4 * (*pk++)) >> 15u) + fcurnt3;
 137     gnext4 = __SSAT(gnext4, 16);
 138
 139     /* Update of f values for next coefficient set processing */
 140     fcurnt1 = fnext1;
 141     fcurnt2 = fnext2;
 142     fcurnt3 = fnext3;
 143     fcurnt4 = fnext4;
 144
 145
 146     /* Loop unrolling.  Process 4 taps at a time . */
 147     stageCnt = (numStages - 1u) >> 2;
 148
 149
 150     /* Loop over the number of taps.  Unroll by a factor of 4.
 151      ** Repeat until we've computed numStages-3 coefficients. */
 152
 153     /* Process 2nd, 3rd, 4th and 5th taps ... here */
 154     while(stageCnt > 0u)
 155     {
 156       /* Read g1(n-1), g3(n-1) .... from state */
 157       gcurnt1 = *px;
 158
 159       /* save g1(n) in state buffer */
 160       *px++ = (q15_t) gnext4;
 161
 162       /* Process first sample for 2nd, 6th .. tap */
 163       /* Sample processing for K2, K6.... */
 164       /* f1(n) = f0(n) +  K1 * g0(n-1) */
 165       fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
 166       fnext1 = __SSAT(fnext1, 16);
 167
 168
 169       /* Process second sample for 2nd, 6th .. tap */
 170       /* for sample 2 processing */
 171       fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
 172       fnext2 = __SSAT(fnext2, 16);
 173       /* Process third sample for 2nd, 6th .. tap */
 174       fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
 175       fnext3 = __SSAT(fnext3, 16);
 176       /* Process fourth sample for 2nd, 6th .. tap */
 177       /* fnext4 = fcurnt4 + (*pk) * gnext3; */
 178       fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
 179       fnext4 = __SSAT(fnext4, 16);
 180
 181       /* g1(n) = f0(n) * K1  +  g0(n-1) */
 182       /* Calculation of state values for next stage */
 183       gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
 184       gnext4 = __SSAT(gnext4, 16);
 185       gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
 186       gnext3 = __SSAT(gnext3, 16);
 187
 188       gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
 189       gnext2 = __SSAT(gnext2, 16);
 190
 191       gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
 192       gnext1 = __SSAT(gnext1, 16);
 193
 194
 195       /* Read g2(n-1), g4(n-1) .... from state */
 196       gcurnt1 = *px;
 197
 198       /* save g1(n) in state buffer */
 199       *px++ = (q15_t) gnext4;
 200
 201       /* Sample processing for K3, K7.... */
 202       /* Process first sample for 3rd, 7th .. tap */
 203       /* f3(n) = f2(n) +  K3 * g2(n-1) */
 204       fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
 205       fcurnt1 = __SSAT(fcurnt1, 16);
 206
 207       /* Process second sample for 3rd, 7th .. tap */
 208       fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
 209       fcurnt2 = __SSAT(fcurnt2, 16);
 210
 211       /* Process third sample for 3rd, 7th .. tap */
 212       fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
 213       fcurnt3 = __SSAT(fcurnt3, 16);
 214
 215       /* Process fourth sample for 3rd, 7th .. tap */
 216       fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
 217       fcurnt4 = __SSAT(fcurnt4, 16);
 218
 219       /* Calculation of state values for next stage */
 220       /* g3(n) = f2(n) * K3  +  g2(n-1) */
 221       gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
 222       gnext4 = __SSAT(gnext4, 16);
 223
 224       gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
 225       gnext3 = __SSAT(gnext3, 16);
 226
 227       gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
 228       gnext2 = __SSAT(gnext2, 16);
 229
 230       gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
 231       gnext1 = __SSAT(gnext1, 16);
 232
 233       /* Read g1(n-1), g3(n-1) .... from state */
 234       gcurnt1 = *px;
 235
 236       /* save g1(n) in state buffer */
 237       *px++ = (q15_t) gnext4;
 238
 239       /* Sample processing for K4, K8.... */
 240       /* Process first sample for 4th, 8th .. tap */
 241       /* f4(n) = f3(n) +  K4 * g3(n-1) */
 242       fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
 243       fnext1 = __SSAT(fnext1, 16);
 244
 245       /* Process second sample for 4th, 8th .. tap */
 246       /* for sample 2 processing */
 247       fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
 248       fnext2 = __SSAT(fnext2, 16);
 249
 250       /* Process third sample for 4th, 8th .. tap */
 251       fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
 252       fnext3 = __SSAT(fnext3, 16);
 253
 254       /* Process fourth sample for 4th, 8th .. tap */
 255       fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
 256       fnext4 = __SSAT(fnext4, 16);
 257
 258       /* g4(n) = f3(n) * K4  +  g3(n-1) */
 259       /* Calculation of state values for next stage */
 260       gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
 261       gnext4 = __SSAT(gnext4, 16);
 262
 263       gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
 264       gnext3 = __SSAT(gnext3, 16);
 265
 266       gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
 267       gnext2 = __SSAT(gnext2, 16);
 268       gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
 269       gnext1 = __SSAT(gnext1, 16);
 270
 271
 272       /* Read g2(n-1), g4(n-1) .... from state */
 273       gcurnt1 = *px;
 274
 275       /* save g4(n) in state buffer */
 276       *px++ = (q15_t) gnext4;
 277
 278       /* Sample processing for K5, K9.... */
 279       /* Process first sample for 5th, 9th .. tap */
 280       /* f5(n) = f4(n) +  K5 * g4(n-1) */
 281       fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
 282       fcurnt1 = __SSAT(fcurnt1, 16);
 283
 284       /* Process second sample for 5th, 9th .. tap */
 285       fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
 286       fcurnt2 = __SSAT(fcurnt2, 16);
 287
 288       /* Process third sample for 5th, 9th .. tap */
 289       fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
 290       fcurnt3 = __SSAT(fcurnt3, 16);
 291
 292       /* Process fourth sample for 5th, 9th .. tap */
 293       fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
 294       fcurnt4 = __SSAT(fcurnt4, 16);
 295
 296       /* Calculation of state values for next stage */
 297       /* g5(n) = f4(n) * K5  +  g4(n-1) */
 298       gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
 299       gnext4 = __SSAT(gnext4, 16);
 300       gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
 301       gnext3 = __SSAT(gnext3, 16);
 302       gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
 303       gnext2 = __SSAT(gnext2, 16);
 304       gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
 305       gnext1 = __SSAT(gnext1, 16);
 306
 307       stageCnt--;
 308     }
 309
 310     /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
 311     stageCnt = (numStages - 1u) % 0x4u;
 312
 313     while(stageCnt > 0u)
 314     {
 315       gcurnt1 = *px;
 316
 317       /* save g value in state buffer */
 318       *px++ = (q15_t) gnext4;
 319
 320       /* Process four samples for last three taps here */
 321       fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
 322       fnext1 = __SSAT(fnext1, 16);
 323       fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
 324       fnext2 = __SSAT(fnext2, 16);
 325
 326       fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
 327       fnext3 = __SSAT(fnext3, 16);
 328
 329       fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
 330       fnext4 = __SSAT(fnext4, 16);
 331
 332       /* g1(n) = f0(n) * K1  +  g0(n-1) */
 333       gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
 334       gnext4 = __SSAT(gnext4, 16);
 335       gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
 336       gnext3 = __SSAT(gnext3, 16);
 337       gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
 338       gnext2 = __SSAT(gnext2, 16);
 339       gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
 340       gnext1 = __SSAT(gnext1, 16);
 341
 342       /* Update of f values for next coefficient set processing */
 343       fcurnt1 = fnext1;
 344       fcurnt2 = fnext2;
 345       fcurnt3 = fnext3;
 346       fcurnt4 = fnext4;
 347
 348       stageCnt--;
 349
 350     }
 351
 352     /* The results in the 4 accumulators, store in the destination buffer. */
 353     /* y(n) = fN(n) */
 354
 355 #ifndef  ARM_MATH_BIG_ENDIAN
 356
 357     *__SIMD32(pDst)++ = __PKHBT(fcurnt1, fcurnt2, 16);
 358     *__SIMD32(pDst)++ = __PKHBT(fcurnt3, fcurnt4, 16);
 359
 360 #else
 361
 362     *__SIMD32(pDst)++ = __PKHBT(fcurnt2, fcurnt1, 16);
 363     *__SIMD32(pDst)++ = __PKHBT(fcurnt4, fcurnt3, 16);
 364
 365 #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
 366
 367     blkCnt--;
 368   }
 369
 370   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
 371    ** No loop unrolling is used. */
 372   blkCnt = blockSize % 0x4u;
 373
 374   while(blkCnt > 0u)
 375   {
 376     /* f0(n) = x(n) */
 377     fcurnt1 = *pSrc++;
 378
 379     /* Initialize coeff pointer */
 380     pk = (pCoeffs);
 381
 382     /* Initialize state pointer */
 383     px = pState;
 384
 385     /* read g2(n) from state buffer */
 386     gcurnt1 = *px;
 387
 388     /* for sample 1 processing */
 389     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 390     fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
 391     fnext1 = __SSAT(fnext1, 16);
 392
 393
 394     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 395     gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
 396     gnext1 = __SSAT(gnext1, 16);
 397
 398     /* save g1(n) in state buffer */
 399     *px++ = (q15_t) fcurnt1;
 400
 401     /* f1(n) is saved in fcurnt1
 402        for next stage processing */
 403     fcurnt1 = fnext1;
 404
 405     stageCnt = (numStages - 1u);
 406
 407     /* stage loop */
 408     while(stageCnt > 0u)
 409     {
 410       /* read g2(n) from state buffer */
 411       gcurnt1 = *px;
 412
 413       /* save g1(n) in state buffer */
 414       *px++ = (q15_t) gnext1;
 415
 416       /* Sample processing for K2, K3.... */
 417       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 418       fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
 419       fnext1 = __SSAT(fnext1, 16);
 420
 421       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 422       gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
 423       gnext1 = __SSAT(gnext1, 16);
 424
 425
 426       /* f1(n) is saved in fcurnt1
 427          for next stage processing */
 428       fcurnt1 = fnext1;
 429
 430       stageCnt--;
 431
 432     }
 433
 434     /* y(n) = fN(n) */
 435     *pDst++ = __SSAT(fcurnt1, 16);
 436
 437
 438     blkCnt--;
 439
 440   }
 441
 442 #else
 443
 444   /* Run the below code for Cortex-M0 */
 445
 446   q31_t fcurnt, fnext, gcurnt, gnext;            /* temporary variables */
 447   uint32_t numStages = S->numStages;             /* Length of the filter */
 448   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
 449
 450   pState = &S->pState[0];
 451
 452   blkCnt = blockSize;
 453
 454   while(blkCnt > 0u)
 455   {
 456     /* f0(n) = x(n) */
 457     fcurnt = *pSrc++;
 458
 459     /* Initialize coeff pointer */
 460     pk = (pCoeffs);
 461
 462     /* Initialize state pointer */
 463     px = pState;
 464
 465     /* read g0(n-1) from state buffer */
 466     gcurnt = *px;
 467
 468     /* for sample 1 processing */
 469     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 470     fnext = ((gcurnt * (*pk)) >> 15u) + fcurnt;
 471     fnext = __SSAT(fnext, 16);
 472
 473
 474     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 475     gnext = ((fcurnt * (*pk++)) >> 15u) + gcurnt;
 476     gnext = __SSAT(gnext, 16);
 477
 478     /* save f0(n) in state buffer */
 479     *px++ = (q15_t) fcurnt;
 480
 481     /* f1(n) is saved in fcurnt
 482        for next stage processing */
 483     fcurnt = fnext;
 484
 485     stageCnt = (numStages - 1u);
 486
 487     /* stage loop */
 488     while(stageCnt > 0u)
 489     {
 490       /* read g1(n-1) from state buffer */
 491       gcurnt = *px;
 492
 493       /* save g0(n-1) in state buffer */
 494       *px++ = (q15_t) gnext;
 495
 496       /* Sample processing for K2, K3.... */
 497       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 498       fnext = ((gcurnt * (*pk)) >> 15u) + fcurnt;
 499       fnext = __SSAT(fnext, 16);
 500
 501       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 502       gnext = ((fcurnt * (*pk++)) >> 15u) + gcurnt;
 503       gnext = __SSAT(gnext, 16);
 504
 505
 506       /* f1(n) is saved in fcurnt
 507          for next stage processing */
 508       fcurnt = fnext;
 509
 510       stageCnt--;
 511
 512     }
 513
 514     /* y(n) = fN(n) */
 515     *pDst++ = __SSAT(fcurnt, 16);
 516
 517
 518     blkCnt--;
 519
 520   }
 521
 522 #endif /*   #ifndef ARM_MATH_CM0 */
 523
 524 }
 525
 526 /**
 527  * @} end of FIR_Lattice group
 528  */