- Main Page
- Modules
- Data Structures
- Files
- Examples

- File List
- Globals

# arm_iir_lattice_q31.c

Go to the documentation of this file.

    00001 /* ----------------------------------------------------------------------
    00002 * Copyright (C) 2010 ARM Limited. All rights reserved.
    00003 *
    00004 * $Date:        15. July 2011
    00005 * $Revision:    V1.0.10
    00006 *
    00007 * Project:      CMSIS DSP Library
    00008 * Title:        arm_iir_lattice_q31.c
    00009 *
    00010 * Description:  Q31 IIR lattice filter processing function.
    00011 *
    00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
    00013 *
    00014 * Version 1.0.10 2011/7/15
    00015 *    Big Endian support added and Merged M0 and M3/M4 Source code.
    00016 *
    00017 * Version 1.0.3 2010/11/29
    00018 *    Re-organized the CMSIS folders and updated documentation.
    00019 *
    00020 * Version 1.0.2 2010/11/11
    00021 *    Documentation updated.
    00022 *
    00023 * Version 1.0.1 2010/10/05
    00024 *    Production release and review comments incorporated.
    00025 *
    00026 * Version 1.0.0 2010/09/20
    00027 *    Production release and review comments incorporated
    00028 *
    00029 * Version 0.0.7  2010/06/10
    00030 *    Misra-C changes done
    00031 * -------------------------------------------------------------------- */
    00032
    00033 #include "arm_math.h"
    00034
    00062 void arm_iir_lattice_q31(
    00063   const arm_iir_lattice_instance_q31 * S,
    00064   q31_t * pSrc,
    00065   q31_t * pDst,
    00066   uint32_t blockSize)
    00067 {
    00068   q31_t fcurr, fnext = 0, gcurr = 0, gnext;      /* Temporary variables for lattice stages */
    00069   q63_t acc;                                     /* Accumlator */
    00070   uint32_t blkCnt, tapCnt;                       /* Temporary variables for counts */
    00071   q31_t *px1, *px2, *pk, *pv;                    /* Temporary pointers for state and coef */
    00072   uint32_t numStages = S->numStages;             /* number of stages */
    00073   q31_t *pState;                                 /* State pointer */
    00074   q31_t *pStateCurnt;                            /* State current pointer */
    00075
    00076   blkCnt = blockSize;
    00077
    00078   pState = &S->pState[0];
    00079
    00080
    00081 #ifndef ARM_MATH_CM0
    00082
    00083   /* Run the below code for Cortex-M4 and Cortex-M3 */
    00084
    00085   /* Sample processing */
    00086   while(blkCnt > 0u)
    00087   {
    00088     /* Read Sample from input buffer */
    00089     /* fN(n) = x(n) */
    00090     fcurr = *pSrc++;
    00091
    00092     /* Initialize state read pointer */
    00093     px1 = pState;
    00094     /* Initialize state write pointer */
    00095     px2 = pState;
    00096     /* Set accumulator to zero */
    00097     acc = 0;
    00098     /* Initialize Ladder coeff pointer */
    00099     pv = &S->pvCoeffs[0];
    00100     /* Initialize Reflection coeff pointer */
    00101     pk = &S->pkCoeffs[0];
    00102
    00103
    00104     /* Process sample for first tap */
    00105     gcurr = *px1++;
    00106     /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
    00107     fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
    00108     /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
    00109     gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
    00110     /* write gN-1(n-1) into state for next sample processing */
    00111     *px2++ = gnext;
    00112     /* y(n) += gN(n) * vN  */
    00113     acc += ((q63_t) gnext * *pv++);
    00114
    00115     /* Update f values for next coefficient processing */
    00116     fcurr = fnext;
    00117
    00118     /* Loop unrolling.  Process 4 taps at a time. */
    00119     tapCnt = (numStages - 1u) >> 2;
    00120
    00121     while(tapCnt > 0u)
    00122     {
    00123
    00124       /* Process sample for 2nd, 6th .. taps */
    00125       /* Read gN-2(n-1) from state buffer */
    00126       gcurr = *px1++;
    00127       /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
    00128       fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
    00129       /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
    00130       gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
    00131       /* y(n) += gN-1(n) * vN-1  */
    00132       /* process for gN-5(n) * vN-5, gN-9(n) * vN-9 ... */
    00133       acc += ((q63_t) gnext * *pv++);
    00134       /* write gN-1(n) into state for next sample processing */
    00135       *px2++ = gnext;
    00136
    00137       /* Process sample for 3nd, 7th ...taps */
    00138       /* Read gN-3(n-1) from state buffer */
    00139       gcurr = *px1++;
    00140       /* Process sample for 3rd, 7th .. taps */
    00141       /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
    00142       fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
    00143       /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
    00144       gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31));
    00145       /* y(n) += gN-2(n) * vN-2  */
    00146       /* process for gN-6(n) * vN-6, gN-10(n) * vN-10 ... */
    00147       acc += ((q63_t) gnext * *pv++);
    00148       /* write gN-2(n) into state for next sample processing */
    00149       *px2++ = gnext;
    00150
    00151
    00152       /* Process sample for 4th, 8th ...taps */
    00153       /* Read gN-4(n-1) from state buffer */
    00154       gcurr = *px1++;
    00155       /* Process sample for 4th, 8th .. taps */
    00156       /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
    00157       fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
    00158       /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
    00159       gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
    00160       /* y(n) += gN-3(n) * vN-3  */
    00161       /* process for gN-7(n) * vN-7, gN-11(n) * vN-11 ... */
    00162       acc += ((q63_t) gnext * *pv++);
    00163       /* write gN-3(n) into state for next sample processing */
    00164       *px2++ = gnext;
    00165
    00166
    00167       /* Process sample for 5th, 9th ...taps */
    00168       /* Read gN-5(n-1) from state buffer */
    00169       gcurr = *px1++;
    00170       /* Process sample for 5th, 9th .. taps */
    00171       /* fN-5(n) = fN-4(n) - kN-4 * gN-1(n-1) */
    00172       fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
    00173       /* gN-4(n) = kN-4 * fN-5(n) + gN-5(n-1) */
    00174       gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31));
    00175       /* y(n) += gN-4(n) * vN-4  */
    00176       /* process for gN-8(n) * vN-8, gN-12(n) * vN-12 ... */
    00177       acc += ((q63_t) gnext * *pv++);
    00178       /* write gN-4(n) into state for next sample processing */
    00179       *px2++ = gnext;
    00180
    00181       tapCnt--;
    00182
    00183     }
    00184
    00185     fnext = fcurr;
    00186
    00187     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
    00188     tapCnt = (numStages - 1u) % 0x4u;
    00189
    00190     while(tapCnt > 0u)
    00191     {
    00192       gcurr = *px1++;
    00193       /* Process sample for last taps */
    00194       fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
    00195       gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
    00196       /* Output samples for last taps */
    00197       acc += ((q63_t) gnext * *pv++);
    00198       *px2++ = gnext;
    00199       fcurr = fnext;
    00200
    00201       tapCnt--;
    00202
    00203     }
    00204
    00205     /* y(n) += g0(n) * v0 */
    00206     acc += (q63_t) fnext *(
    00207   *pv++);
    00208
    00209     *px2++ = fnext;
    00210
    00211     /* write out into pDst */
    00212     *pDst++ = (q31_t) (acc >> 31u);
    00213
    00214     /* Advance the state pointer by 4 to process the next group of 4 samples */
    00215     pState = pState + 1u;
    00216     blkCnt--;
    00217
    00218   }
    00219
    00220   /* Processing is complete. Now copy last S->numStages samples to start of the buffer
    00221      for the preperation of next frame process */
    00222
    00223   /* Points to the start of the state buffer */
    00224   pStateCurnt = &S->pState[0];
    00225   pState = &S->pState[blockSize];
    00226
    00227   tapCnt = numStages >> 2u;
    00228
    00229   /* copy data */
    00230   while(tapCnt > 0u)
    00231   {
    00232     *pStateCurnt++ = *pState++;
    00233     *pStateCurnt++ = *pState++;
    00234     *pStateCurnt++ = *pState++;
    00235     *pStateCurnt++ = *pState++;
    00236
    00237     /* Decrement the loop counter */
    00238     tapCnt--;
    00239
    00240   }
    00241
    00242   /* Calculate remaining number of copies */
    00243   tapCnt = (numStages) % 0x4u;
    00244
    00245   /* Copy the remaining q31_t data */
    00246   while(tapCnt > 0u)
    00247   {
    00248     *pStateCurnt++ = *pState++;
    00249
    00250     /* Decrement the loop counter */
    00251     tapCnt--;
    00252   };
    00253
    00254 #else
    00255
    00256   /* Run the below code for Cortex-M0 */
    00257   /* Sample processing */
    00258   while(blkCnt > 0u)
    00259   {
    00260     /* Read Sample from input buffer */
    00261     /* fN(n) = x(n) */
    00262     fcurr = *pSrc++;
    00263
    00264     /* Initialize state read pointer */
    00265     px1 = pState;
    00266     /* Initialize state write pointer */
    00267     px2 = pState;
    00268     /* Set accumulator to zero */
    00269     acc = 0;
    00270     /* Initialize Ladder coeff pointer */
    00271     pv = &S->pvCoeffs[0];
    00272     /* Initialize Reflection coeff pointer */
    00273     pk = &S->pkCoeffs[0];
    00274
    00275     tapCnt = numStages;
    00276
    00277     while(tapCnt > 0u)
    00278     {
    00279       gcurr = *px1++;
    00280       /* Process sample */
    00281       /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
    00282       fnext =
    00283         clip_q63_to_q31(((q63_t) fcurr -
    00284                          ((q31_t) (((q63_t) gcurr * (*pk)) >> 31))));
    00285       /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
    00286       gnext =
    00287         clip_q63_to_q31(((q63_t) gcurr +
    00288                          ((q31_t) (((q63_t) fnext * (*pk++)) >> 31))));
    00289       /* Output samples */
    00290       /* y(n) += gN(n) * vN  */
    00291       acc += ((q63_t) gnext * *pv++);
    00292       /* write gN-1(n-1) into state for next sample processing */
    00293       *px2++ = gnext;
    00294       /* Update f values for next coefficient processing */
    00295       fcurr = fnext;
    00296
    00297       tapCnt--;
    00298     }
    00299
    00300     /* y(n) += g0(n) * v0 */
    00301     acc += (q63_t) fnext *(
    00302   *pv++);
    00303
    00304     *px2++ = fnext;
    00305
    00306     /* write out into pDst */
    00307     *pDst++ = (q31_t) (acc >> 31u);
    00308
    00309     /* Advance the state pointer by 1 to process the next group of samples */
    00310     pState = pState + 1u;
    00311     blkCnt--;
    00312
    00313   }
    00314
    00315   /* Processing is complete. Now copy last S->numStages samples to start of the buffer
    00316      for the preperation of next frame process */
    00317
    00318   /* Points to the start of the state buffer */
    00319   pStateCurnt = &S->pState[0];
    00320   pState = &S->pState[blockSize];
    00321
    00322   tapCnt = numStages;
    00323
    00324   /* Copy the remaining q31_t data */
    00325   while(tapCnt > 0u)
    00326   {
    00327     *pStateCurnt++ = *pState++;
    00328
    00329     /* Decrement the loop counter */
    00330     tapCnt--;
    00331   }
    00332
    00333 #endif /*   #ifndef ARM_MATH_CM0 */
    00334
    00335 }
    00336
    00337
    00338
    00339

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines