arm_iir_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_iir_lattice_q31.c  
00009 *  
00010 * Description:  Q31 IIR 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 IIR_Lattice  
00038  * @{  
00039  */ 
00040  
00041 /**  
00042  * @brief Processing function for the Q31 IIR lattice filter.  
00043  * @param[in] *S points to an instance of the Q31 IIR lattice structure.  
00044  * @param[in] *pSrc points to the block of input data.  
00045  * @param[out] *pDst points to the block of output data.  
00046  * @param[in] blockSize number of samples to process.  
00047  * @return none.  
00048  *  
00049  * @details  
00050  * <b>Scaling and Overflow Behavior:</b>  
00051  * \par  
00052  * The function is implemented using an internal 64-bit accumulator.  
00053  * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.  
00054  * Thus, if the accumulator result overflows it wraps around rather than clip.  
00055  * In order to avoid overflows completely the input signal must be scaled down by 2*log2(numStages) bits.  
00056  * After all multiply-accumulates are performed, the 2.62 accumulator is saturated to 1.32 format and then truncated to 1.31 format.  
00057  */ 
00058  
00059 void arm_iir_lattice_q31( 
00060   const arm_iir_lattice_instance_q31 * S, 
00061   q31_t * pSrc, 
00062   q31_t * pDst, 
00063   uint32_t blockSize) 
00064 { 
00065   q31_t fcurr, fnext, gcurr = 0, gnext;          /* Temporary variables for lattice stages */ 
00066   q63_t acc;                                     /* Accumlator */ 
00067   uint32_t blkCnt, tapCnt;                       /* Temporary variables for counts */ 
00068   q31_t *px1, *px2, *pk, *pv;                    /* Temporary pointers for state and coef */ 
00069   uint32_t numStages = S->numStages;             /* number of stages */ 
00070   q31_t *pState;                                 /* State pointer */ 
00071   q31_t *pStateCurnt;                            /* State current pointer */ 
00072  
00073   blkCnt = blockSize; 
00074  
00075   pState = &S->pState[0]; 
00076  
00077   /* Sample processing */ 
00078   while(blkCnt > 0u) 
00079   { 
00080     /* Read Sample from input buffer */ 
00081     /* fN(n) = x(n) */ 
00082     fcurr = *pSrc++; 
00083  
00084     /* Initialize state read pointer */ 
00085     px1 = pState; 
00086     /* Initialize state write pointer */ 
00087     px2 = pState; 
00088     /* Set accumulator to zero */ 
00089     acc = 0; 
00090     /* Initialize Ladder coeff pointer */ 
00091     pv = &S->pvCoeffs[0]; 
00092     /* Initialize Reflection coeff pointer */ 
00093     pk = &S->pkCoeffs[0]; 
00094  
00095  
00096     /* Process sample for first tap */ 
00097     gcurr = *px1++; 
00098     /* fN-1(n) = fN(n) - kN * gN-1(n-1) */ 
00099     fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31)); 
00100     /* gN(n) = kN * fN-1(n) + gN-1(n-1) */ 
00101     gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31)); 
00102     /* write gN-1(n-1) into state for next sample processing */ 
00103     *px2++ = gnext; 
00104     /* y(n) += gN(n) * vN  */ 
00105     acc += ((q63_t) gnext * *pv++); 
00106  
00107     /* Update f values for next coefficient processing */ 
00108     fcurr = fnext; 
00109  
00110     /* Loop unrolling.  Process 4 taps at a time. */ 
00111     tapCnt = (numStages - 1u) >> 2; 
00112  
00113     while(tapCnt > 0u) 
00114     { 
00115  
00116       /* Process sample for 2nd, 6th .. taps */ 
00117       /* Read gN-2(n-1) from state buffer */ 
00118       gcurr = *px1++; 
00119       /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */ 
00120       fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31)); 
00121       /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */ 
00122       gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31)); 
00123       /* y(n) += gN-1(n) * vN-1  */ 
00124       /* process for gN-5(n) * vN-5, gN-9(n) * vN-9 ... */ 
00125       acc += ((q63_t) gnext * *pv++); 
00126       /* write gN-1(n) into state for next sample processing */ 
00127       *px2++ = gnext; 
00128  
00129       /* Process sample for 3nd, 7th ...taps */ 
00130       /* Read gN-3(n-1) from state buffer */ 
00131       gcurr = *px1++; 
00132       /* Process sample for 3rd, 7th .. taps */ 
00133       /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */ 
00134       fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk)) >> 31)); 
00135       /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */ 
00136       gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31)); 
00137       /* y(n) += gN-2(n) * vN-2  */ 
00138       /* process for gN-6(n) * vN-6, gN-10(n) * vN-10 ... */ 
00139       acc += ((q63_t) gnext * *pv++); 
00140       /* write gN-2(n) into state for next sample processing */ 
00141       *px2++ = gnext; 
00142  
00143  
00144       /* Process sample for 4th, 8th ...taps */ 
00145       /* Read gN-4(n-1) from state buffer */ 
00146       gcurr = *px1++; 
00147       /* Process sample for 4th, 8th .. taps */ 
00148       /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */ 
00149       fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31)); 
00150       /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */ 
00151       gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31)); 
00152       /* y(n) += gN-3(n) * vN-3  */ 
00153       /* process for gN-7(n) * vN-7, gN-11(n) * vN-11 ... */ 
00154       acc += ((q63_t) gnext * *pv++); 
00155       /* write gN-3(n) into state for next sample processing */ 
00156       *px2++ = gnext; 
00157  
00158  
00159       /* Process sample for 5th, 9th ...taps */ 
00160       /* Read gN-5(n-1) from state buffer */ 
00161       gcurr = *px1++; 
00162       /* Process sample for 5th, 9th .. taps */ 
00163       /* fN-5(n) = fN-4(n) - kN-4 * gN-1(n-1) */ 
00164       fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk)) >> 31)); 
00165       /* gN-4(n) = kN-4 * fN-5(n) + gN-5(n-1) */ 
00166       gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31)); 
00167       /* y(n) += gN-4(n) * vN-4  */ 
00168       /* process for gN-8(n) * vN-8, gN-12(n) * vN-12 ... */ 
00169       acc += ((q63_t) gnext * *pv++); 
00170       /* write gN-4(n) into state for next sample processing */ 
00171       *px2++ = gnext; 
00172  
00173       tapCnt--; 
00174  
00175     } 
00176  
00177     fnext = fcurr; 
00178  
00179     /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 
00180     tapCnt = (numStages - 1u) % 0x4u; 
00181  
00182     while(tapCnt > 0u) 
00183     { 
00184       gcurr = *px1++; 
00185       /* Process sample for last taps */ 
00186       fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31)); 
00187       gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31)); 
00188       /* Output samples for last taps */ 
00189       acc += ((q63_t) gnext * *pv++); 
00190       *px2++ = gnext; 
00191       fcurr = fnext; 
00192  
00193       tapCnt--; 
00194  
00195     } 
00196  
00197     /* y(n) += g0(n) * v0 */ 
00198     acc += (q63_t) fnext *( 
00199   *pv++); 
00200  
00201     *px2++ = fnext; 
00202  
00203     /* write out into pDst */ 
00204     *pDst++ = (q31_t) (acc >> 31u); 
00205  
00206     /* Advance the state pointer by 4 to process the next group of 4 samples */ 
00207     pState = pState + 1u; 
00208     blkCnt--; 
00209  
00210   } 
00211  
00212   /* Processing is complete. Now copy last S->numStages samples to start of the buffer  
00213      for the preperation of next frame process */ 
00214  
00215   /* Points to the start of the state buffer */ 
00216   pStateCurnt = &S->pState[0]; 
00217   pState = &S->pState[blockSize]; 
00218  
00219   tapCnt = numStages >> 2u; 
00220  
00221   /* copy data */ 
00222   while(tapCnt > 0u) 
00223   { 
00224     *pStateCurnt++ = *pState++; 
00225     *pStateCurnt++ = *pState++; 
00226     *pStateCurnt++ = *pState++; 
00227     *pStateCurnt++ = *pState++; 
00228  
00229     /* Decrement the loop counter */ 
00230     tapCnt--; 
00231  
00232   } 
00233  
00234   /* Calculate remaining number of copies */ 
00235   tapCnt = (numStages) % 0x4u; 
00236  
00237   /* Copy the remaining q31_t data */ 
00238   while(tapCnt > 0u) 
00239   { 
00240     *pStateCurnt++ = *pState++; 
00241  
00242     /* Decrement the loop counter */ 
00243     tapCnt--; 
00244   }; 
00245  
00246 } 
00247  
00248  
00249  
00250  
00251 /**  
00252  * @} end of IIR_Lattice group  
00253  */