arm_fir_lattice_q31.c

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_q31.c  
00009 *  
00010 * Description:  Q31 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  
00032 /**  
00033  * @ingroup groupFilters  
00034  */ 
00035  
00036 /**  
00037  * @addtogroup FIR_Lattice  
00038  * @{  
00039  */ 
00040  
00041  
00042 /**  
00043  * @brief Processing function for the Q31 FIR lattice filter.  
00044  * @param[in]  *S        points to an instance of the Q31 FIR lattice structure.  
00045  * @param[in]  *pSrc     points to the block of input data.  
00046  * @param[out] *pDst     points to the block of output data  
00047  * @param[in]  blockSize number of samples to process.  
00048  * @return none.  
00049  *  
00050  * @details  
00051  * <b>Scaling and Overflow Behavior:</b>  
00052  * In order to avoid overflows the input signal must be scaled down by 2*log2(numStages) bits.  
00053  */ 
00054  
00055 void arm_fir_lattice_q31( 
00056   const arm_fir_lattice_instance_q31 * S, 
00057   q31_t * pSrc, 
00058   q31_t * pDst, 
00059   uint32_t blockSize) 
00060 { 
00061   q31_t *pState;                                 /* State pointer */ 
00062   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */ 
00063   q31_t *px;                                     /* temporary state pointer */ 
00064   q31_t *pk;                                     /* temporary coefficient pointer */ 
00065   q31_t fcurr1, fnext1, gcurr1 = 0, gnext1;      /* temporary variables for first sample in loop unrolling */ 
00066   q63_t fcurr2, fnext2, gnext2;                  /* temporary variables for second sample in loop unrolling */ 
00067   q63_t fcurr3, fnext3, gnext3;                  /* temporary variables for third sample in loop unrolling */ 
00068   q63_t fcurr4, fnext4, gnext4;                  /* temporary variables for fourth sample in loop unrolling */ 
00069   uint32_t numStages = S->numStages;             /* Length of the filter */ 
00070   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */ 
00071  
00072   pState = &S->pState[0]; 
00073  
00074   blkCnt = blockSize >> 2u; 
00075  
00076   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.  
00077    ** a second loop below computes the remaining 1 to 3 samples. */ 
00078   while(blkCnt > 0u) 
00079   { 
00080  
00081     /* Read two samples from input buffer */ 
00082     /* f0(n) = x(n) */ 
00083     fcurr1 = *pSrc++; 
00084     /* f0(n) = x(n) */ 
00085     fcurr2 = *pSrc++; 
00086  
00087     /* Initialize coeff pointer */ 
00088     pk = (pCoeffs); 
00089  
00090     /* Initialize state pointer */ 
00091     px = pState; 
00092  
00093     /* Read g0(n-1) from state */ 
00094     gcurr1 = *px; 
00095  
00096     /* Process first sample for first tap */ 
00097     /* f1(n) = f0(n) +  K1 * g0(n-1) */ 
00098     fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1; 
00099     /* g1(n) = f0(n) * K1  +  g0(n-1) */ 
00100     gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + gcurr1; 
00101  
00102     /* Process second sample for first tap */ 
00103     /* for sample 2 processing */ 
00104     fnext2 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + fcurr2; 
00105     gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr1; 
00106  
00107  
00108     /* Read next two samples from input buffer */ 
00109     /* f0(n+2) = x(n+2) */ 
00110     fcurr3 = *pSrc++; 
00111     fcurr4 = *pSrc++; 
00112  
00113     /* Copy only last input samples into the state buffer  
00114        which will be used for next four samples processing */ 
00115     *px++ = (q31_t) fcurr4; 
00116  
00117     /* Process third sample for first tap */ 
00118     fnext3 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr3; 
00119     gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr2; 
00120  
00121     /* Process fourth sample for first tap */ 
00122     fnext4 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr4; 
00123     gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk++)) >> 31) + fcurr3; 
00124  
00125     /* save g1(n) in state buffer for next sample processing */ 
00126     /* *px++ = gnext4;       */ 
00127  
00128     /* Update of f values for next coefficient set processing */ 
00129     fcurr1 = fnext1; 
00130     fcurr2 = fnext2; 
00131     fcurr3 = fnext3; 
00132     fcurr4 = fnext4; 
00133  
00134  
00135     /* Loop unrolling.  Process 4 taps at a time . */ 
00136     stageCnt = (numStages - 1u) >> 2u; 
00137  
00138  
00139     /* Loop over the number of taps.  Unroll by a factor of 4.  
00140      ** Repeat until we've computed numStages-3 coefficients. */ 
00141  
00142     /* Process 2nd, 3rd, 4th and 5th taps ... here */ 
00143     while(stageCnt > 0u) 
00144     { 
00145       /* Read g1(n-1), g3(n-1) .... from state */ 
00146       gcurr1 = *px; 
00147  
00148       /* save g1(n) in state buffer */ 
00149       *px++ = (q31_t) gnext4; 
00150  
00151       /* Process first sample for 2nd, 6th .. tap */ 
00152       /* Sample processing for K2, K6.... */ 
00153       /* f2(n) = f1(n) +  K2 * g1(n-1) */ 
00154       fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1; 
00155       /* Process second sample for 2nd, 6th .. tap */ 
00156       /* for sample 2 processing */ 
00157       fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2; 
00158       /* Process third sample for 2nd, 6th .. tap */ 
00159       fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3; 
00160       /* Process fourth sample for 2nd, 6th .. tap */ 
00161       fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4; 
00162  
00163       /* g2(n) = f1(n) * K2  +  g1(n-1) */ 
00164       /* Calculation of state values for next stage */ 
00165       gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3; 
00166       gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2; 
00167       gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1; 
00168       gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1; 
00169  
00170  
00171       /* Read g2(n-1), g4(n-1) .... from state */ 
00172       gcurr1 = *px; 
00173  
00174       /* save g2(n) in state buffer */ 
00175       *px++ = (q31_t) gnext4; 
00176  
00177       /* Sample processing for K3, K7.... */ 
00178       /* Process first sample for 3rd, 7th .. tap */ 
00179       /* f3(n) = f2(n) +  K3 * g2(n-1) */ 
00180       fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1; 
00181       /* Process second sample for 3rd, 7th .. tap */ 
00182       fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2; 
00183       /* Process third sample for 3rd, 7th .. tap */ 
00184       fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3; 
00185       /* Process fourth sample for 3rd, 7th .. tap */ 
00186       fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4; 
00187  
00188       /* Calculation of state values for next stage */ 
00189       /*  gnext4 = fnext4 * (*pk) +  gnext3; */ 
00190       gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3; 
00191       gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2; 
00192       /*  gnext2 = fnext2 * (*pk) +  gnext1; */ 
00193       gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1; 
00194  
00195       /* g1(n) = f0(n) * K1  +  g0(n-1) */ 
00196       /*  gnext1 = fnext1 * (*pk++) +  gcurr1; */ 
00197       gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1; 
00198  
00199       /* Read g1(n-1), g3(n-1) .... from state */ 
00200       gcurr1 = *px; 
00201  
00202       /* save g1(n) in state buffer */ 
00203       *px++ = (q31_t) gnext4; 
00204  
00205       /* Sample processing for K4, K8.... */ 
00206       /* Process first sample for 4th, 8th .. tap */ 
00207       /* f4(n) = f3(n) +  K4 * g3(n-1) */ 
00208       fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1; 
00209       /* Process second sample for 4th, 8th .. tap */ 
00210       /* for sample 2 processing */ 
00211       fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2; 
00212       /* Process third sample for 4th, 8th .. tap */ 
00213       fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3; 
00214       /* Process fourth sample for 4th, 8th .. tap */ 
00215       fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4; 
00216  
00217       /* g4(n) = f3(n) * K4  +  g3(n-1) */ 
00218       /* Calculation of state values for next stage */ 
00219       gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3; 
00220       gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2; 
00221       gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1; 
00222       gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1; 
00223  
00224       /* Read g2(n-1), g4(n-1) .... from state */ 
00225       gcurr1 = *px; 
00226  
00227       /* save g4(n) in state buffer */ 
00228       *px++ = (q31_t) gnext4; 
00229  
00230       /* Sample processing for K5, K9.... */ 
00231       /* Process first sample for 5th, 9th .. tap */ 
00232       /* f5(n) = f4(n) +  K5 * g4(n-1) */ 
00233       fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1; 
00234       /* Process second sample for 5th, 9th .. tap */ 
00235       fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2; 
00236       /* Process third sample for 5th, 9th .. tap */ 
00237       fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3; 
00238       /* Process fourth sample for 5th, 9th .. tap */ 
00239       fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4; 
00240  
00241       /* Calculation of state values for next stage */ 
00242       /* g5(n) = f4(n) * K5  +  g4(n-1) */ 
00243       gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3; 
00244       gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2; 
00245       gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1; 
00246       gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1; 
00247  
00248       stageCnt--; 
00249     } 
00250  
00251     /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */ 
00252     stageCnt = (numStages - 1u) % 0x4u; 
00253  
00254     while(stageCnt > 0u) 
00255     { 
00256       gcurr1 = *px; 
00257  
00258       /* save g value in state buffer */ 
00259       *px++ = (q31_t) gnext4; 
00260  
00261       /* Process four samples for last three taps here */ 
00262       fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1; 
00263       fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2; 
00264       fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3; 
00265       fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4; 
00266  
00267       /* g1(n) = f0(n) * K1  +  g0(n-1) */ 
00268       gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3; 
00269       gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2; 
00270       gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1; 
00271       gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1; 
00272  
00273       /* Update of f values for next coefficient set processing */ 
00274       fcurr1 = fnext1; 
00275       fcurr2 = fnext2; 
00276       fcurr3 = fnext3; 
00277       fcurr4 = fnext4; 
00278  
00279       stageCnt--; 
00280  
00281     } 
00282  
00283     /* The results in the 4 accumulators, store in the destination buffer. */ 
00284     /* y(n) = fN(n) */ 
00285     *pDst++ = fcurr1; 
00286     *pDst++ = (q31_t) fcurr2; 
00287     *pDst++ = (q31_t) fcurr3; 
00288     *pDst++ = (q31_t) fcurr4; 
00289  
00290     blkCnt--; 
00291   } 
00292  
00293   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.  
00294    ** No loop unrolling is used. */ 
00295   blkCnt = blockSize % 0x4u; 
00296  
00297   while(blkCnt > 0u) 
00298   { 
00299     /* f0(n) = x(n) */ 
00300     fcurr1 = *pSrc++; 
00301  
00302     /* Initialize coeff pointer */ 
00303     pk = (pCoeffs); 
00304  
00305     /* Initialize state pointer */ 
00306     px = pState; 
00307  
00308     /* read g2(n) from state buffer */ 
00309     gcurr1 = *px; 
00310  
00311     /* for sample 1 processing */ 
00312     /* f1(n) = f0(n) +  K1 * g0(n-1) */ 
00313     fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1; 
00314     /* g1(n) = f0(n) * K1  +  g0(n-1) */ 
00315     gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1; 
00316     /* save g1(n) in state buffer */ 
00317     *px++ = fcurr1; 
00318  
00319     /* f1(n) is saved in fcurr1  
00320        for next stage processing */ 
00321     fcurr1 = fnext1; 
00322  
00323     stageCnt = (numStages - 1u); 
00324  
00325     /* stage loop */ 
00326     while(stageCnt > 0u) 
00327     { 
00328       /* read g2(n) from state buffer */ 
00329       gcurr1 = *px; 
00330  
00331       /* save g1(n) in state buffer */ 
00332       *px++ = gnext1; 
00333  
00334       /* Sample processing for K2, K3.... */ 
00335       /* f2(n) = f1(n) +  K2 * g1(n-1) */ 
00336       fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1; 
00337       /* g2(n) = f1(n) * K2  +  g1(n-1) */ 
00338       gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1; 
00339  
00340       /* f1(n) is saved in fcurr1  
00341          for next stage processing */ 
00342       fcurr1 = fnext1; 
00343  
00344       stageCnt--; 
00345  
00346     } 
00347  
00348     /* y(n) = fN(n) */ 
00349     *pDst++ = fcurr1; 
00350  
00351     blkCnt--; 
00352  
00353   } 
00354 } 
00355  
00356 /**  
00357  * @} end of FIR_Lattice group  
00358  */