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

- File List
- Globals

# arm_fir_lattice_q15.c

Go to the documentation of this file.

    00001 /* ----------------------------------------------------------------------
    00002 * Copyright (C) 2010 ARM Limited. All rights reserved.
    00003 *
    00004 * $Date:        29. November 2010
    00005 * $Revision:    V1.0.3
    00006 *
    00007 * Project:      CMSIS DSP Library
    00008 * Title:        arm_fir_lattice_q15.c
    00009 *
    00010 * Description:  Q15 FIR lattice filter processing function.
    00011 *
    00012 * Target Processor: Cortex-M4/Cortex-M3
    00013 *
    00014 * Version 1.0.3 2010/11/29
    00015 *    Re-organized the CMSIS folders and updated documentation.
    00016 *
    00017 * Version 1.0.2 2010/11/11
    00018 *    Documentation updated.
    00019 *
    00020 * Version 1.0.1 2010/10/05
    00021 *    Production release and review comments incorporated.
    00022 *
    00023 * Version 1.0.0 2010/09/20
    00024 *    Production release and review comments incorporated
    00025 *
    00026 * Version 0.0.7  2010/06/10
    00027 *    Misra-C changes done
    00028 * -------------------------------------------------------------------- */
    00029
    00030 #include "arm_math.h"
    00031
    00051 void arm_fir_lattice_q15(
    00052   const arm_fir_lattice_instance_q15 * S,
    00053   q15_t * pSrc,
    00054   q15_t * pDst,
    00055   uint32_t blockSize)
    00056 {
    00057   q15_t *pState;                                 /* State pointer */
    00058   q15_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
    00059   q15_t *px;                                     /* temporary state pointer */
    00060   q15_t *pk;                                     /* temporary coefficient pointer */
    00061   q31_t fcurnt1, fnext1, gcurnt1 = 0, gnext1;    /* temporary variables for first sample in loop unrolling */
    00062   q31_t fcurnt2, fnext2, gnext2;                 /* temporary variables for second sample in loop unrolling */
    00063   q31_t fcurnt3, fnext3, gnext3;                 /* temporary variables for third sample in loop unrolling */
    00064   q31_t fcurnt4, fnext4, gnext4;                 /* temporary variables for fourth sample in loop unrolling */
    00065   uint32_t numStages = S->numStages;             /* Number of stages in the filter */
    00066   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
    00067
    00068   pState = &S->pState[0];
    00069
    00070   blkCnt = blockSize >> 2u;
    00071
    00072   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
    00073    ** a second loop below computes the remaining 1 to 3 samples. */
    00074   while(blkCnt > 0u)
    00075   {
    00076
    00077     /* Read two samples from input buffer */
    00078     /* f0(n) = x(n) */
    00079     fcurnt1 = *pSrc++;
    00080     fcurnt2 = *pSrc++;
    00081
    00082     /* Initialize coeff pointer */
    00083     pk = (pCoeffs);
    00084
    00085     /* Initialize state pointer */
    00086     px = pState;
    00087
    00088     /* Read g0(n-1) from state */
    00089     gcurnt1 = *px;
    00090
    00091     /* Process first sample for first tap */
    00092     /* f1(n) = f0(n) +  K1 * g0(n-1) */
    00093     fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
    00094     fnext1 = __SSAT(fnext1, 16);
    00095
    00096     /* g1(n) = f0(n) * K1  +  g0(n-1) */
    00097     gnext1 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + gcurnt1;
    00098     gnext1 = __SSAT(gnext1, 16);
    00099
    00100     /* Process second sample for first tap */
    00101     /* for sample 2 processing */
    00102     fnext2 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + fcurnt2;
    00103     fnext2 = __SSAT(fnext2, 16);
    00104
    00105     gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt1;
    00106     gnext2 = __SSAT(gnext2, 16);
    00107
    00108
    00109     /* Read next two samples from input buffer */
    00110     /* f0(n+2) = x(n+2) */
    00111     fcurnt3 = *pSrc++;
    00112     fcurnt4 = *pSrc++;
    00113
    00114     /* Copy only last input samples into the state buffer
    00115        which is used for next four samples processing */
    00116     *px++ = (q15_t) fcurnt4;
    00117
    00118     /* Process third sample for first tap */
    00119     fnext3 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt3;
    00120     fnext3 = __SSAT(fnext3, 16);
    00121     gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt2;
    00122     gnext3 = __SSAT(gnext3, 16);
    00123
    00124     /* Process fourth sample for first tap */
    00125     fnext4 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt4;
    00126     fnext4 = __SSAT(fnext4, 16);
    00127     gnext4 = (q31_t) ((fcurnt4 * (*pk++)) >> 15u) + fcurnt3;
    00128     gnext4 = __SSAT(gnext4, 16);
    00129
    00130     /* Update of f values for next coefficient set processing */
    00131     fcurnt1 = fnext1;
    00132     fcurnt2 = fnext2;
    00133     fcurnt3 = fnext3;
    00134     fcurnt4 = fnext4;
    00135
    00136
    00137     /* Loop unrolling.  Process 4 taps at a time . */
    00138     stageCnt = (numStages - 1u) >> 2;
    00139
    00140
    00141     /* Loop over the number of taps.  Unroll by a factor of 4.
    00142      ** Repeat until we've computed numStages-3 coefficients. */
    00143
    00144     /* Process 2nd, 3rd, 4th and 5th taps ... here */
    00145     while(stageCnt > 0u)
    00146     {
    00147       /* Read g1(n-1), g3(n-1) .... from state */
    00148       gcurnt1 = *px;
    00149
    00150       /* save g1(n) in state buffer */
    00151       *px++ = (q15_t) gnext4;
    00152
    00153       /* Process first sample for 2nd, 6th .. tap */
    00154       /* Sample processing for K2, K6.... */
    00155       /* f1(n) = f0(n) +  K1 * g0(n-1) */
    00156       fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
    00157       fnext1 = __SSAT(fnext1, 16);
    00158
    00159
    00160       /* Process second sample for 2nd, 6th .. tap */
    00161       /* for sample 2 processing */
    00162       fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
    00163       fnext2 = __SSAT(fnext2, 16);
    00164       /* Process third sample for 2nd, 6th .. tap */
    00165       fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
    00166       fnext3 = __SSAT(fnext3, 16);
    00167       /* Process fourth sample for 2nd, 6th .. tap */
    00168       /* fnext4 = fcurnt4 + (*pk) * gnext3; */
    00169       fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
    00170       fnext4 = __SSAT(fnext4, 16);
    00171
    00172       /* g1(n) = f0(n) * K1  +  g0(n-1) */
    00173       /* Calculation of state values for next stage */
    00174       gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
    00175       gnext4 = __SSAT(gnext4, 16);
    00176       gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
    00177       gnext3 = __SSAT(gnext3, 16);
    00178
    00179       gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
    00180       gnext2 = __SSAT(gnext2, 16);
    00181
    00182       gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
    00183       gnext1 = __SSAT(gnext1, 16);
    00184
    00185
    00186       /* Read g2(n-1), g4(n-1) .... from state */
    00187       gcurnt1 = *px;
    00188
    00189       /* save g1(n) in state buffer */
    00190       *px++ = (q15_t) gnext4;
    00191
    00192       /* Sample processing for K3, K7.... */
    00193       /* Process first sample for 3rd, 7th .. tap */
    00194       /* f3(n) = f2(n) +  K3 * g2(n-1) */
    00195       fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
    00196       fcurnt1 = __SSAT(fcurnt1, 16);
    00197
    00198       /* Process second sample for 3rd, 7th .. tap */
    00199       fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
    00200       fcurnt2 = __SSAT(fcurnt2, 16);
    00201
    00202       /* Process third sample for 3rd, 7th .. tap */
    00203       fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
    00204       fcurnt3 = __SSAT(fcurnt3, 16);
    00205
    00206       /* Process fourth sample for 3rd, 7th .. tap */
    00207       fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
    00208       fcurnt4 = __SSAT(fcurnt4, 16);
    00209
    00210       /* Calculation of state values for next stage */
    00211       /* g3(n) = f2(n) * K3  +  g2(n-1) */
    00212       gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
    00213       gnext4 = __SSAT(gnext4, 16);
    00214
    00215       gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
    00216       gnext3 = __SSAT(gnext3, 16);
    00217
    00218       gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
    00219       gnext2 = __SSAT(gnext2, 16);
    00220
    00221       gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
    00222       gnext1 = __SSAT(gnext1, 16);
    00223
    00224       /* Read g1(n-1), g3(n-1) .... from state */
    00225       gcurnt1 = *px;
    00226
    00227       /* save g1(n) in state buffer */
    00228       *px++ = (q15_t) gnext4;
    00229
    00230       /* Sample processing for K4, K8.... */
    00231       /* Process first sample for 4th, 8th .. tap */
    00232       /* f4(n) = f3(n) +  K4 * g3(n-1) */
    00233       fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
    00234       fnext1 = __SSAT(fnext1, 16);
    00235
    00236       /* Process second sample for 4th, 8th .. tap */
    00237       /* for sample 2 processing */
    00238       fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
    00239       fnext2 = __SSAT(fnext2, 16);
    00240
    00241       /* Process third sample for 4th, 8th .. tap */
    00242       fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
    00243       fnext3 = __SSAT(fnext3, 16);
    00244
    00245       /* Process fourth sample for 4th, 8th .. tap */
    00246       fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
    00247       fnext4 = __SSAT(fnext4, 16);
    00248
    00249       /* g4(n) = f3(n) * K4  +  g3(n-1) */
    00250       /* Calculation of state values for next stage */
    00251       gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
    00252       gnext4 = __SSAT(gnext4, 16);
    00253
    00254       gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
    00255       gnext3 = __SSAT(gnext3, 16);
    00256
    00257       gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
    00258       gnext2 = __SSAT(gnext2, 16);
    00259       gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
    00260       gnext1 = __SSAT(gnext1, 16);
    00261
    00262
    00263       /* Read g2(n-1), g4(n-1) .... from state */
    00264       gcurnt1 = *px;
    00265
    00266       /* save g4(n) in state buffer */
    00267       *px++ = (q15_t) gnext4;
    00268
    00269       /* Sample processing for K5, K9.... */
    00270       /* Process first sample for 5th, 9th .. tap */
    00271       /* f5(n) = f4(n) +  K5 * g4(n-1) */
    00272       fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
    00273       fcurnt1 = __SSAT(fcurnt1, 16);
    00274
    00275       /* Process second sample for 5th, 9th .. tap */
    00276       fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
    00277       fcurnt2 = __SSAT(fcurnt2, 16);
    00278
    00279       /* Process third sample for 5th, 9th .. tap */
    00280       fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
    00281       fcurnt3 = __SSAT(fcurnt3, 16);
    00282
    00283       /* Process fourth sample for 5th, 9th .. tap */
    00284       fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
    00285       fcurnt4 = __SSAT(fcurnt4, 16);
    00286
    00287       /* Calculation of state values for next stage */
    00288       /* g5(n) = f4(n) * K5  +  g4(n-1) */
    00289       gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
    00290       gnext4 = __SSAT(gnext4, 16);
    00291       gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
    00292       gnext3 = __SSAT(gnext3, 16);
    00293       gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
    00294       gnext2 = __SSAT(gnext2, 16);
    00295       gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
    00296       gnext1 = __SSAT(gnext1, 16);
    00297
    00298       stageCnt--;
    00299     }
    00300
    00301     /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
    00302     stageCnt = (numStages - 1u) % 0x4u;
    00303
    00304     while(stageCnt > 0u)
    00305     {
    00306       gcurnt1 = *px;
    00307
    00308       /* save g value in state buffer */
    00309       *px++ = (q15_t) gnext4;
    00310
    00311       /* Process four samples for last three taps here */
    00312       fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
    00313       fnext1 = __SSAT(fnext1, 16);
    00314       fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
    00315       fnext2 = __SSAT(fnext2, 16);
    00316
    00317       fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
    00318       fnext3 = __SSAT(fnext3, 16);
    00319
    00320       fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
    00321       fnext4 = __SSAT(fnext4, 16);
    00322
    00323       /* g1(n) = f0(n) * K1  +  g0(n-1) */
    00324       gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
    00325       gnext4 = __SSAT(gnext4, 16);
    00326       gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
    00327       gnext3 = __SSAT(gnext3, 16);
    00328       gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
    00329       gnext2 = __SSAT(gnext2, 16);
    00330       gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
    00331       gnext1 = __SSAT(gnext1, 16);
    00332
    00333       /* Update of f values for next coefficient set processing */
    00334       fcurnt1 = fnext1;
    00335       fcurnt2 = fnext2;
    00336       fcurnt3 = fnext3;
    00337       fcurnt4 = fnext4;
    00338
    00339       stageCnt--;
    00340
    00341     }
    00342
    00343     /* The results in the 4 accumulators, store in the destination buffer. */
    00344     /* y(n) = fN(n) */
    00345     *__SIMD32(pDst)++ = __PKHBT(fcurnt1, fcurnt2, 16);
    00346     *__SIMD32(pDst)++ = __PKHBT(fcurnt3, fcurnt4, 16);
    00347
    00348     blkCnt--;
    00349   }
    00350
    00351   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
    00352    ** No loop unrolling is used. */
    00353   blkCnt = blockSize % 0x4u;
    00354
    00355   while(blkCnt > 0u)
    00356   {
    00357     /* f0(n) = x(n) */
    00358     fcurnt1 = *pSrc++;
    00359
    00360     /* Initialize coeff pointer */
    00361     pk = (pCoeffs);
    00362
    00363     /* Initialize state pointer */
    00364     px = pState;
    00365
    00366     /* read g2(n) from state buffer */
    00367     gcurnt1 = *px;
    00368
    00369     /* for sample 1 processing */
    00370     /* f1(n) = f0(n) +  K1 * g0(n-1) */
    00371     fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
    00372     fnext1 = __SSAT(fnext1, 16);
    00373
    00374
    00375     /* g1(n) = f0(n) * K1  +  g0(n-1) */
    00376     gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
    00377     gnext1 = __SSAT(gnext1, 16);
    00378
    00379     /* save g1(n) in state buffer */
    00380     *px++ = (q15_t) fcurnt1;
    00381
    00382     /* f1(n) is saved in fcurnt1
    00383        for next stage processing */
    00384     fcurnt1 = fnext1;
    00385
    00386     stageCnt = (numStages - 1u);
    00387
    00388     /* stage loop */
    00389     while(stageCnt > 0u)
    00390     {
    00391       /* read g2(n) from state buffer */
    00392       gcurnt1 = *px;
    00393
    00394       /* save g1(n) in state buffer */
    00395       *px++ = (q15_t) gnext1;
    00396
    00397       /* Sample processing for K2, K3.... */
    00398       /* f2(n) = f1(n) +  K2 * g1(n-1) */
    00399       fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
    00400       fnext1 = __SSAT(fnext1, 16);
    00401
    00402       /* g2(n) = f1(n) * K2  +  g1(n-1) */
    00403       gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
    00404       gnext1 = __SSAT(gnext1, 16);
    00405
    00406
    00407       /* f1(n) is saved in fcurnt1
    00408          for next stage processing */
    00409       fcurnt1 = fnext1;
    00410
    00411       stageCnt--;
    00412
    00413     }
    00414
    00415     /* y(n) = fN(n) */
    00416     *pDst++ = __SSAT(fcurnt1, 16);
    00417
    00418
    00419     blkCnt--;
    00420
    00421   }
    00422 }
    00423

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines