tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_iir_lattice_f32.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_iir_lattice_f32.c
   9 *
  10 * Description:  Floating-point IIR Lattice 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
  41 #include "arm_math.h"
  42
  43 /**
  44  * @ingroup groupFilters
  45  */
  46
  47 /**
  48  * @defgroup IIR_Lattice Infinite Impulse Response (IIR) Lattice Filters
  49  *
  50  * This set of functions implements lattice filters
  51  * for Q15, Q31 and floating-point data types.  Lattice filters are used in a
  52  * variety of adaptive filter applications.  The filter structure has feedforward and
  53  * feedback components and the net impulse response is infinite length.
  54  * The functions operate on blocks
  55  * of input and output data and each call to the function processes
  56  * <code>blockSize</code> samples through the filter.  <code>pSrc</code> and
  57  * <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
  58
  59  * \par Algorithm:
  60  * \image html IIRLattice.gif "Infinite Impulse Response Lattice filter"
  61  * <pre>
  62  *    fN(n)   =  x(n)
  63  *    fm-1(n) = fm(n) - km * gm-1(n-1)   for m = N, N-1, ...1
  64  *    gm(n)   = km * fm-1(n) + gm-1(n-1) for m = N, N-1, ...1
  65  *    y(n)    = vN * gN(n) + vN-1 * gN-1(n) + ...+ v0 * g0(n)
  66  * </pre>
  67  * \par
  68  * <code>pkCoeffs</code> points to array of reflection coefficients of size <code>numStages</code>.
  69  * Reflection coefficients are stored in time-reversed order.
  70  * \par
  71  * <pre>
  72  *    {kN, kN-1, ....k1}
  73  * </pre>
  74  * <code>pvCoeffs</code> points to the array of ladder coefficients of size <code>(numStages+1)</code>.
  75  * Ladder coefficients are stored in time-reversed order.
  76  * \par
  77  * <pre>
  78  *    {vN, vN-1, ...v0}
  79  * </pre>
  80  * <code>pState</code> points to a state array of size <code>numStages + blockSize</code>.
  81  * The state variables shown in the figure above (the g values) are stored in the <code>pState</code> array.
  82  * The state variables are updated after each block of data is processed; the coefficients are untouched.
  83  * \par Instance Structure
  84  * The coefficients and state variables for a filter are stored together in an instance data structure.
  85  * A separate instance structure must be defined for each filter.
  86  * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
  87  * There are separate instance structure declarations for each of the 3 supported data types.
  88   *
  89  * \par Initialization Functions
  90  * There is also an associated initialization function for each data type.
  91  * The initialization function performs the following operations:
  92  * - Sets the values of the internal structure fields.
  93  * - Zeros out the values in the state buffer.
  94  * To do this manually without calling the init function, assign the follow subfields of the instance structure:
  95  * numStages, pkCoeffs, pvCoeffs, pState. Also set all of the values in pState to zero.
  96  *
  97  * \par
  98  * Use of the initialization function is optional.
  99  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
 100  * To place an instance structure into a const data section, the instance structure must be manually initialized.
 101  * Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
 102  * <pre>
 103  *arm_iir_lattice_instance_f32 S = {numStages, pState, pkCoeffs, pvCoeffs};
 104  *arm_iir_lattice_instance_q31 S = {numStages, pState, pkCoeffs, pvCoeffs};
 105  *arm_iir_lattice_instance_q15 S = {numStages, pState, pkCoeffs, pvCoeffs};
 106  * </pre>
 107  * \par
 108  * where <code>numStages</code> is the number of stages in the filter; <code>pState</code> points to the state buffer array;
 109  * <code>pkCoeffs</code> points to array of the reflection coefficients; <code>pvCoeffs</code> points to the array of ladder coefficients.
 110  * \par Fixed-Point Behavior
 111  * Care must be taken when using the fixed-point versions of the IIR lattice filter functions.
 112  * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
 113  * Refer to the function specific documentation below for usage guidelines.
 114  */
 115
 116 /**
 117  * @addtogroup IIR_Lattice
 118  * @{
 119  */
 120
 121 /**
 122  * @brief Processing function for the floating-point IIR lattice filter.
 123  * @param[in] *S points to an instance of the floating-point IIR lattice structure.
 124  * @param[in] *pSrc points to the block of input data.
 125  * @param[out] *pDst points to the block of output data.
 126  * @param[in] blockSize number of samples to process.
 127  * @return none.
 128  */
 129
 130 #ifndef ARM_MATH_CM0_FAMILY
 131
 132   /* Run the below code for Cortex-M4 and Cortex-M3 */
 133
 134 void arm_iir_lattice_f32(
 135   const arm_iir_lattice_instance_f32 * S,
 136   float32_t * pSrc,
 137   float32_t * pDst,
 138   uint32_t blockSize)
 139 {
 140   float32_t fnext1, gcurr1, gnext;               /* Temporary variables for lattice stages */
 141   float32_t acc;                                 /* Accumlator */
 142   uint32_t blkCnt, tapCnt;                       /* temporary variables for counts */
 143   float32_t *px1, *px2, *pk, *pv;                /* temporary pointers for state and coef */
 144   uint32_t numStages = S->numStages;             /* number of stages */
 145   float32_t *pState;                             /* State pointer */
 146   float32_t *pStateCurnt;                        /* State current pointer */
 147   float32_t k1, k2;
 148   float32_t v1, v2, v3, v4;
 149   float32_t gcurr2;
 150   float32_t fnext2;
 151
 152   /* initialise loop count */
 153   blkCnt = blockSize;
 154
 155   /* initialise state pointer */
 156   pState = &S->pState[0];
 157
 158   /* Sample processing */
 159   while(blkCnt > 0u)
 160   {
 161     /* Read Sample from input buffer */
 162     /* fN(n) = x(n) */
 163     fnext2 = *pSrc++;
 164
 165     /* Initialize Ladder coeff pointer */
 166     pv = &S->pvCoeffs[0];
 167     /* Initialize Reflection coeff pointer */
 168     pk = &S->pkCoeffs[0];
 169
 170     /* Initialize state read pointer */
 171     px1 = pState;
 172     /* Initialize state write pointer */
 173     px2 = pState;
 174
 175     /* Set accumulator to zero */
 176     acc = 0.0;
 177
 178     /* Loop unrolling.  Process 4 taps at a time. */
 179     tapCnt = (numStages) >> 2;
 180
 181     while(tapCnt > 0u)
 182     {
 183       /* Read gN-1(n-1) from state buffer */
 184       gcurr1 = *px1;
 185
 186       /* read reflection coefficient kN */
 187       k1 = *pk;
 188
 189       /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
 190       fnext1 = fnext2 - (k1 * gcurr1);
 191
 192       /* read ladder coefficient vN */
 193       v1 = *pv;
 194
 195       /* read next reflection coefficient kN-1 */
 196       k2 = *(pk + 1u);
 197
 198       /* Read gN-2(n-1) from state buffer */
 199       gcurr2 = *(px1 + 1u);
 200
 201       /* read next ladder coefficient vN-1 */
 202       v2 = *(pv + 1u);
 203
 204       /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
 205       fnext2 = fnext1 - (k2 * gcurr2);
 206
 207       /* gN(n)   = kN * fN-1(n) + gN-1(n-1) */
 208       gnext = gcurr1 + (k1 * fnext1);
 209
 210       /* read reflection coefficient kN-2 */
 211       k1 = *(pk + 2u);
 212
 213       /* write gN(n) into state for next sample processing */
 214       *px2++ = gnext;
 215
 216       /* Read gN-3(n-1) from state buffer */
 217       gcurr1 = *(px1 + 2u);
 218
 219       /* y(n) += gN(n) * vN  */
 220       acc += (gnext * v1);
 221
 222       /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
 223       fnext1 = fnext2 - (k1 * gcurr1);
 224
 225       /* gN-1(n)   = kN-1 * fN-2(n) + gN-2(n-1) */
 226       gnext = gcurr2 + (k2 * fnext2);
 227
 228       /* Read gN-4(n-1) from state buffer */
 229       gcurr2 = *(px1 + 3u);
 230
 231       /* y(n) += gN-1(n) * vN-1  */
 232       acc += (gnext * v2);
 233
 234       /* read reflection coefficient kN-3 */
 235       k2 = *(pk + 3u);
 236
 237       /* write gN-1(n) into state for next sample processing */
 238       *px2++ = gnext;
 239
 240       /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
 241       fnext2 = fnext1 - (k2 * gcurr2);
 242
 243       /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
 244       gnext = gcurr1 + (k1 * fnext1);
 245
 246       /* read ladder coefficient vN-2 */
 247       v3 = *(pv + 2u);
 248
 249       /* y(n) += gN-2(n) * vN-2  */
 250       acc += (gnext * v3);
 251
 252       /* write gN-2(n) into state for next sample processing */
 253       *px2++ = gnext;
 254
 255       /* update pointer */
 256       pk += 4u;
 257
 258       /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
 259       gnext = (fnext2 * k2) + gcurr2;
 260
 261       /* read next ladder coefficient vN-3 */
 262       v4 = *(pv + 3u);
 263
 264       /* y(n) += gN-4(n) * vN-4  */
 265       acc += (gnext * v4);
 266
 267       /* write gN-3(n) into state for next sample processing */
 268       *px2++ = gnext;
 269
 270       /* update pointers */
 271       px1 += 4u;
 272       pv += 4u;
 273
 274       tapCnt--;
 275
 276     }
 277
 278     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
 279     tapCnt = (numStages) % 0x4u;
 280
 281     while(tapCnt > 0u)
 282     {
 283       gcurr1 = *px1++;
 284       /* Process sample for last taps */
 285       fnext1 = fnext2 - ((*pk) * gcurr1);
 286       gnext = (fnext1 * (*pk++)) + gcurr1;
 287       /* Output samples for last taps */
 288       acc += (gnext * (*pv++));
 289       *px2++ = gnext;
 290       fnext2 = fnext1;
 291
 292       tapCnt--;
 293
 294     }
 295
 296     /* y(n) += g0(n) * v0 */
 297     acc += (fnext2 * (*pv));
 298
 299     *px2++ = fnext2;
 300
 301     /* write out into pDst */
 302     *pDst++ = acc;
 303
 304     /* Advance the state pointer by 4 to process the next group of 4 samples */
 305     pState = pState + 1u;
 306
 307     blkCnt--;
 308
 309   }
 310
 311   /* Processing is complete. Now copy last S->numStages samples to start of the buffer
 312      for the preperation of next frame process */
 313
 314   /* Points to the start of the state buffer */
 315   pStateCurnt = &S->pState[0];
 316   pState = &S->pState[blockSize];
 317
 318   tapCnt = numStages >> 2u;
 319
 320   /* copy data */
 321   while(tapCnt > 0u)
 322   {
 323     *pStateCurnt++ = *pState++;
 324     *pStateCurnt++ = *pState++;
 325     *pStateCurnt++ = *pState++;
 326     *pStateCurnt++ = *pState++;
 327
 328     /* Decrement the loop counter */
 329     tapCnt--;
 330
 331   }
 332
 333   /* Calculate remaining number of copies */
 334   tapCnt = (numStages) % 0x4u;
 335
 336   /* Copy the remaining q31_t data */
 337   while(tapCnt > 0u)
 338   {
 339     *pStateCurnt++ = *pState++;
 340
 341     /* Decrement the loop counter */
 342     tapCnt--;
 343   }
 344 }
 345
 346 #else
 347
 348 void arm_iir_lattice_f32(
 349   const arm_iir_lattice_instance_f32 * S,
 350   float32_t * pSrc,
 351   float32_t * pDst,
 352   uint32_t blockSize)
 353 {
 354   float32_t fcurr, fnext = 0, gcurr, gnext;      /* Temporary variables for lattice stages */
 355   float32_t acc;                                 /* Accumlator */
 356   uint32_t blkCnt, tapCnt;                       /* temporary variables for counts */
 357   float32_t *px1, *px2, *pk, *pv;                /* temporary pointers for state and coef */
 358   uint32_t numStages = S->numStages;             /* number of stages */
 359   float32_t *pState;                             /* State pointer */
 360   float32_t *pStateCurnt;                        /* State current pointer */
 361
 362
 363   /* Run the below code for Cortex-M0 */
 364
 365   blkCnt = blockSize;
 366
 367   pState = &S->pState[0];
 368
 369   /* Sample processing */
 370   while(blkCnt > 0u)
 371   {
 372     /* Read Sample from input buffer */
 373     /* fN(n) = x(n) */
 374     fcurr = *pSrc++;
 375
 376     /* Initialize state read pointer */
 377     px1 = pState;
 378     /* Initialize state write pointer */
 379     px2 = pState;
 380     /* Set accumulator to zero */
 381     acc = 0.0f;
 382     /* Initialize Ladder coeff pointer */
 383     pv = &S->pvCoeffs[0];
 384     /* Initialize Reflection coeff pointer */
 385     pk = &S->pkCoeffs[0];
 386
 387
 388     /* Process sample for numStages */
 389     tapCnt = numStages;
 390
 391     while(tapCnt > 0u)
 392     {
 393       gcurr = *px1++;
 394       /* Process sample for last taps */
 395       fnext = fcurr - ((*pk) * gcurr);
 396       gnext = (fnext * (*pk++)) + gcurr;
 397
 398       /* Output samples for last taps */
 399       acc += (gnext * (*pv++));
 400       *px2++ = gnext;
 401       fcurr = fnext;
 402
 403       /* Decrementing loop counter */
 404       tapCnt--;
 405
 406     }
 407
 408     /* y(n) += g0(n) * v0 */
 409     acc += (fnext * (*pv));
 410
 411     *px2++ = fnext;
 412
 413     /* write out into pDst */
 414     *pDst++ = acc;
 415
 416     /* Advance the state pointer by 1 to process the next group of samples */
 417     pState = pState + 1u;
 418     blkCnt--;
 419
 420   }
 421
 422   /* Processing is complete. Now copy last S->numStages samples to start of the buffer
 423      for the preperation of next frame process */
 424
 425   /* Points to the start of the state buffer */
 426   pStateCurnt = &S->pState[0];
 427   pState = &S->pState[blockSize];
 428
 429   tapCnt = numStages;
 430
 431   /* Copy the data */
 432   while(tapCnt > 0u)
 433   {
 434     *pStateCurnt++ = *pState++;
 435
 436     /* Decrement the loop counter */
 437     tapCnt--;
 438   }
 439
 440 }
 441
 442 #endif /*   #ifndef ARM_MATH_CM0_FAMILY */
 443
 444
 445 /**
 446  * @} end of IIR_Lattice group
 447  */