git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_lattice_f32.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_f32.c
   9 *
  10 * Description:  Processing function for the floating-point FIR Lattice filter.
  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  * @defgroup FIR_Lattice Finite Impulse Response (FIR) Lattice Filters
  41  *
  42  * This set of functions implements Finite Impulse Response (FIR) lattice filters
  43  * for Q15, Q31 and floating-point data types.  Lattice filters are used in a
  44  * variety of adaptive filter applications.  The filter structure is feedforward and
  45  * the net impulse response is finite length.
  46  * The functions operate on blocks
  47  * of input and output data and each call to the function processes
  48  * <code>blockSize</code> samples through the filter.  <code>pSrc</code> and
  49  * <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
  50  *
  51  * \par Algorithm:
  52  * \image html FIRLattice.gif "Finite Impulse Response Lattice filter"
  53  * The following difference equation is implemented:
  54  * <pre>
  55  *    f0[n] = g0[n] = x[n]
  56  *    fm[n] = fm-1[n] + km * gm-1[n-1] for m = 1, 2, ...M
  57  *    gm[n] = km * fm-1[n] + gm-1[n-1] for m = 1, 2, ...M
  58  *    y[n] = fM[n]
  59  * </pre>
  60  * \par
  61  * <code>pCoeffs</code> points to tha array of reflection coefficients of size <code>numStages</code>.
  62  * Reflection Coefficients are stored in the following order.
  63  * \par
  64  * <pre>
  65  *    {k1, k2, ..., kM}
  66  * </pre>
  67  * where M is number of stages
  68  * \par
  69  * <code>pState</code> points to a state array of size <code>numStages</code>.
  70  * The state variables (g values) hold previous inputs and are stored in the following order.
  71  * <pre>
  72  *    {g0[n], g1[n], g2[n] ...gM-1[n]}
  73  * </pre>
  74  * The state variables are updated after each block of data is processed; the coefficients are untouched.
  75  * \par Instance Structure
  76  * The coefficients and state variables for a filter are stored together in an instance data structure.
  77  * A separate instance structure must be defined for each filter.
  78  * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
  79  * There are separate instance structure declarations for each of the 3 supported data types.
  80  *
  81  * \par Initialization Functions
  82  * There is also an associated initialization function for each data type.
  83  * The initialization function performs the following operations:
  84  * - Sets the values of the internal structure fields.
  85  * - Zeros out the values in the state buffer.
  86  *
  87  * \par
  88  * Use of the initialization function is optional.
  89  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
  90  * To place an instance structure into a const data section, the instance structure must be manually initialized.
  91  * Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
  92  * <pre>
  93  *arm_fir_lattice_instance_f32 S = {numStages, pState, pCoeffs};
  94  *arm_fir_lattice_instance_q31 S = {numStages, pState, pCoeffs};
  95  *arm_fir_lattice_instance_q15 S = {numStages, pState, pCoeffs};
  96  * </pre>
  97  * \par
  98  * where <code>numStages</code> is the number of stages in the filter; <code>pState</code> is the address of the state buffer;
  99  * <code>pCoeffs</code> is the address of the coefficient buffer.
 100  * \par Fixed-Point Behavior
 101  * Care must be taken when using the fixed-point versions of the FIR Lattice filter functions.
 102  * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
 103  * Refer to the function specific documentation below for usage guidelines.
 104  */
 105
 106 /**
 107  * @addtogroup FIR_Lattice
 108  * @{
 109  */
 110
 111
 112   /**
 113    * @brief Processing function for the floating-point FIR lattice filter.
 114    * @param[in]  *S        points to an instance of the floating-point FIR lattice structure.
 115    * @param[in]  *pSrc     points to the block of input data.
 116    * @param[out] *pDst     points to the block of output data
 117    * @param[in]  blockSize number of samples to process.
 118    * @return none.
 119    */
 120
 121 void arm_fir_lattice_f32(
 122   const arm_fir_lattice_instance_f32 * S,
 123   float32_t * pSrc,
 124   float32_t * pDst,
 125   uint32_t blockSize)
 126 {
 127   float32_t *pState;                             /* State pointer */
 128   float32_t *pCoeffs = S->pCoeffs;               /* Coefficient pointer */
 129   float32_t *px;                                 /* temporary state pointer */
 130   float32_t *pk;                                 /* temporary coefficient pointer */
 131
 132
 133 #ifndef ARM_MATH_CM0
 134
 135   /* Run the below code for Cortex-M4 and Cortex-M3 */
 136
 137   float32_t fcurr1, fnext1, gcurr1, gnext1;      /* temporary variables for first sample in loop unrolling */
 138   float32_t fcurr2, fnext2, gnext2;              /* temporary variables for second sample in loop unrolling */
 139   float32_t fcurr3, fnext3, gnext3;              /* temporary variables for third sample in loop unrolling */
 140   float32_t fcurr4, fnext4, gnext4;              /* temporary variables for fourth sample in loop unrolling */
 141   uint32_t numStages = S->numStages;             /* Number of stages in the filter */
 142   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
 143
 144   gcurr1 = 0.0f;
 145   pState = &S->pState[0];
 146
 147   blkCnt = blockSize >> 2;
 148
 149   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
 150      a second loop below computes the remaining 1 to 3 samples. */
 151   while(blkCnt > 0u)
 152   {
 153
 154     /* Read two samples from input buffer */
 155     /* f0(n) = x(n) */
 156     fcurr1 = *pSrc++;
 157     fcurr2 = *pSrc++;
 158
 159     /* Initialize coeff pointer */
 160     pk = (pCoeffs);
 161
 162     /* Initialize state pointer */
 163     px = pState;
 164
 165     /* Read g0(n-1) from state */
 166     gcurr1 = *px;
 167
 168     /* Process first sample for first tap */
 169     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 170     fnext1 = fcurr1 + ((*pk) * gcurr1);
 171     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 172     gnext1 = (fcurr1 * (*pk)) + gcurr1;
 173
 174     /* Process second sample for first tap */
 175     /* for sample 2 processing */
 176     fnext2 = fcurr2 + ((*pk) * fcurr1);
 177     gnext2 = (fcurr2 * (*pk)) + fcurr1;
 178
 179     /* Read next two samples from input buffer */
 180     /* f0(n+2) = x(n+2) */
 181     fcurr3 = *pSrc++;
 182     fcurr4 = *pSrc++;
 183
 184     /* Copy only last input samples into the state buffer
 185        which will be used for next four samples processing */
 186     *px++ = fcurr4;
 187
 188     /* Process third sample for first tap */
 189     fnext3 = fcurr3 + ((*pk) * fcurr2);
 190     gnext3 = (fcurr3 * (*pk)) + fcurr2;
 191
 192     /* Process fourth sample for first tap */
 193     fnext4 = fcurr4 + ((*pk) * fcurr3);
 194     gnext4 = (fcurr4 * (*pk++)) + fcurr3;
 195
 196     /* Update of f values for next coefficient set processing */
 197     fcurr1 = fnext1;
 198     fcurr2 = fnext2;
 199     fcurr3 = fnext3;
 200     fcurr4 = fnext4;
 201
 202     /* Loop unrolling.  Process 4 taps at a time . */
 203     stageCnt = (numStages - 1u) >> 2u;
 204
 205     /* Loop over the number of taps.  Unroll by a factor of 4.
 206      ** Repeat until we've computed numStages-3 coefficients. */
 207
 208     /* Process 2nd, 3rd, 4th and 5th taps ... here */
 209     while(stageCnt > 0u)
 210     {
 211       /* Read g1(n-1), g3(n-1) .... from state */
 212       gcurr1 = *px;
 213
 214       /* save g1(n) in state buffer */
 215       *px++ = gnext4;
 216
 217       /* Process first sample for 2nd, 6th .. tap */
 218       /* Sample processing for K2, K6.... */
 219       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 220       fnext1 = fcurr1 + ((*pk) * gcurr1);
 221       /* Process second sample for 2nd, 6th .. tap */
 222       /* for sample 2 processing */
 223       fnext2 = fcurr2 + ((*pk) * gnext1);
 224       /* Process third sample for 2nd, 6th .. tap */
 225       fnext3 = fcurr3 + ((*pk) * gnext2);
 226       /* Process fourth sample for 2nd, 6th .. tap */
 227       fnext4 = fcurr4 + ((*pk) * gnext3);
 228
 229       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 230       /* Calculation of state values for next stage */
 231       gnext4 = (fcurr4 * (*pk)) + gnext3;
 232       gnext3 = (fcurr3 * (*pk)) + gnext2;
 233       gnext2 = (fcurr2 * (*pk)) + gnext1;
 234       gnext1 = (fcurr1 * (*pk++)) + gcurr1;
 235
 236
 237       /* Read g2(n-1), g4(n-1) .... from state */
 238       gcurr1 = *px;
 239
 240       /* save g2(n) in state buffer */
 241       *px++ = gnext4;
 242
 243       /* Sample processing for K3, K7.... */
 244       /* Process first sample for 3rd, 7th .. tap */
 245       /* f3(n) = f2(n) +  K3 * g2(n-1) */
 246       fcurr1 = fnext1 + ((*pk) * gcurr1);
 247       /* Process second sample for 3rd, 7th .. tap */
 248       fcurr2 = fnext2 + ((*pk) * gnext1);
 249       /* Process third sample for 3rd, 7th .. tap */
 250       fcurr3 = fnext3 + ((*pk) * gnext2);
 251       /* Process fourth sample for 3rd, 7th .. tap */
 252       fcurr4 = fnext4 + ((*pk) * gnext3);
 253
 254       /* Calculation of state values for next stage */
 255       /* g3(n) = f2(n) * K3  +  g2(n-1) */
 256       gnext4 = (fnext4 * (*pk)) + gnext3;
 257       gnext3 = (fnext3 * (*pk)) + gnext2;
 258       gnext2 = (fnext2 * (*pk)) + gnext1;
 259       gnext1 = (fnext1 * (*pk++)) + gcurr1;
 260
 261
 262       /* Read g1(n-1), g3(n-1) .... from state */
 263       gcurr1 = *px;
 264
 265       /* save g3(n) in state buffer */
 266       *px++ = gnext4;
 267
 268       /* Sample processing for K4, K8.... */
 269       /* Process first sample for 4th, 8th .. tap */
 270       /* f4(n) = f3(n) +  K4 * g3(n-1) */
 271       fnext1 = fcurr1 + ((*pk) * gcurr1);
 272       /* Process second sample for 4th, 8th .. tap */
 273       /* for sample 2 processing */
 274       fnext2 = fcurr2 + ((*pk) * gnext1);
 275       /* Process third sample for 4th, 8th .. tap */
 276       fnext3 = fcurr3 + ((*pk) * gnext2);
 277       /* Process fourth sample for 4th, 8th .. tap */
 278       fnext4 = fcurr4 + ((*pk) * gnext3);
 279
 280       /* g4(n) = f3(n) * K4  +  g3(n-1) */
 281       /* Calculation of state values for next stage */
 282       gnext4 = (fcurr4 * (*pk)) + gnext3;
 283       gnext3 = (fcurr3 * (*pk)) + gnext2;
 284       gnext2 = (fcurr2 * (*pk)) + gnext1;
 285       gnext1 = (fcurr1 * (*pk++)) + gcurr1;
 286
 287       /* Read g2(n-1), g4(n-1) .... from state */
 288       gcurr1 = *px;
 289
 290       /* save g4(n) in state buffer */
 291       *px++ = gnext4;
 292
 293       /* Sample processing for K5, K9.... */
 294       /* Process first sample for 5th, 9th .. tap */
 295       /* f5(n) = f4(n) +  K5 * g4(n-1) */
 296       fcurr1 = fnext1 + ((*pk) * gcurr1);
 297       /* Process second sample for 5th, 9th .. tap */
 298       fcurr2 = fnext2 + ((*pk) * gnext1);
 299       /* Process third sample for 5th, 9th .. tap */
 300       fcurr3 = fnext3 + ((*pk) * gnext2);
 301       /* Process fourth sample for 5th, 9th .. tap */
 302       fcurr4 = fnext4 + ((*pk) * gnext3);
 303
 304       /* Calculation of state values for next stage */
 305       /* g5(n) = f4(n) * K5  +  g4(n-1) */
 306       gnext4 = (fnext4 * (*pk)) + gnext3;
 307       gnext3 = (fnext3 * (*pk)) + gnext2;
 308       gnext2 = (fnext2 * (*pk)) + gnext1;
 309       gnext1 = (fnext1 * (*pk++)) + gcurr1;
 310
 311       stageCnt--;
 312     }
 313
 314     /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
 315     stageCnt = (numStages - 1u) % 0x4u;
 316
 317     while(stageCnt > 0u)
 318     {
 319       gcurr1 = *px;
 320
 321       /* save g value in state buffer */
 322       *px++ = gnext4;
 323
 324       /* Process four samples for last three taps here */
 325       fnext1 = fcurr1 + ((*pk) * gcurr1);
 326       fnext2 = fcurr2 + ((*pk) * gnext1);
 327       fnext3 = fcurr3 + ((*pk) * gnext2);
 328       fnext4 = fcurr4 + ((*pk) * gnext3);
 329
 330       /* g1(n) = f0(n) * K1  +  g0(n-1) */
 331       gnext4 = (fcurr4 * (*pk)) + gnext3;
 332       gnext3 = (fcurr3 * (*pk)) + gnext2;
 333       gnext2 = (fcurr2 * (*pk)) + gnext1;
 334       gnext1 = (fcurr1 * (*pk++)) + gcurr1;
 335
 336       /* Update of f values for next coefficient set processing */
 337       fcurr1 = fnext1;
 338       fcurr2 = fnext2;
 339       fcurr3 = fnext3;
 340       fcurr4 = fnext4;
 341
 342       stageCnt--;
 343
 344     }
 345
 346     /* The results in the 4 accumulators, store in the destination buffer. */
 347     /* y(n) = fN(n) */
 348     *pDst++ = fcurr1;
 349     *pDst++ = fcurr2;
 350     *pDst++ = fcurr3;
 351     *pDst++ = fcurr4;
 352
 353     blkCnt--;
 354   }
 355
 356   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
 357    ** No loop unrolling is used. */
 358   blkCnt = blockSize % 0x4u;
 359
 360   while(blkCnt > 0u)
 361   {
 362     /* f0(n) = x(n) */
 363     fcurr1 = *pSrc++;
 364
 365     /* Initialize coeff pointer */
 366     pk = (pCoeffs);
 367
 368     /* Initialize state pointer */
 369     px = pState;
 370
 371     /* read g2(n) from state buffer */
 372     gcurr1 = *px;
 373
 374     /* for sample 1 processing */
 375     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 376     fnext1 = fcurr1 + ((*pk) * gcurr1);
 377     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 378     gnext1 = (fcurr1 * (*pk++)) + gcurr1;
 379
 380     /* save g1(n) in state buffer */
 381     *px++ = fcurr1;
 382
 383     /* f1(n) is saved in fcurr1
 384        for next stage processing */
 385     fcurr1 = fnext1;
 386
 387     stageCnt = (numStages - 1u);
 388
 389     /* stage loop */
 390     while(stageCnt > 0u)
 391     {
 392       /* read g2(n) from state buffer */
 393       gcurr1 = *px;
 394
 395       /* save g1(n) in state buffer */
 396       *px++ = gnext1;
 397
 398       /* Sample processing for K2, K3.... */
 399       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 400       fnext1 = fcurr1 + ((*pk) * gcurr1);
 401       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 402       gnext1 = (fcurr1 * (*pk++)) + gcurr1;
 403
 404       /* f1(n) is saved in fcurr1
 405          for next stage processing */
 406       fcurr1 = fnext1;
 407
 408       stageCnt--;
 409
 410     }
 411
 412     /* y(n) = fN(n) */
 413     *pDst++ = fcurr1;
 414
 415     blkCnt--;
 416
 417   }
 418
 419 #else
 420
 421   /* Run the below code for Cortex-M0 */
 422
 423   float32_t fcurr, fnext, gcurr, gnext;          /* temporary variables */
 424   uint32_t numStages = S->numStages;             /* Length of the filter */
 425   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
 426
 427   pState = &S->pState[0];
 428
 429   blkCnt = blockSize;
 430
 431   while(blkCnt > 0u)
 432   {
 433     /* f0(n) = x(n) */
 434     fcurr = *pSrc++;
 435
 436     /* Initialize coeff pointer */
 437     pk = pCoeffs;
 438
 439     /* Initialize state pointer */
 440     px = pState;
 441
 442     /* read g0(n-1) from state buffer */
 443     gcurr = *px;
 444
 445     /* for sample 1 processing */
 446     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 447     fnext = fcurr + ((*pk) * gcurr);
 448     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 449     gnext = (fcurr * (*pk++)) + gcurr;
 450
 451     /* save f0(n) in state buffer */
 452     *px++ = fcurr;
 453
 454     /* f1(n) is saved in fcurr
 455        for next stage processing */
 456     fcurr = fnext;
 457
 458     stageCnt = (numStages - 1u);
 459
 460     /* stage loop */
 461     while(stageCnt > 0u)
 462     {
 463       /* read g2(n) from state buffer */
 464       gcurr = *px;
 465
 466       /* save g1(n) in state buffer */
 467       *px++ = gnext;
 468
 469       /* Sample processing for K2, K3.... */
 470       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 471       fnext = fcurr + ((*pk) * gcurr);
 472       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 473       gnext = (fcurr * (*pk++)) + gcurr;
 474
 475       /* f1(n) is saved in fcurr1
 476          for next stage processing */
 477       fcurr = fnext;
 478
 479       stageCnt--;
 480
 481     }
 482
 483     /* y(n) = fN(n) */
 484     *pDst++ = fcurr;
 485
 486     blkCnt--;
 487
 488   }
 489
 490 #endif /*   #ifndef ARM_MATH_CM0 */
 491
 492 }
 493
 494 /**
 495  * @} end of FIR_Lattice group
 496  */