arm_biquad_cascade_df1_q15.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_biquad_cascade_df1_q15.c  
00009 *  
00010 * Description:  Processing function for the  
00011 *               Q15 Biquad cascade DirectFormI(DF1) filter.  
00012 *  
00013 * Target Processor: Cortex-M4/Cortex-M3
00014 *  
00015 * Version 1.0.3 2010/11/29 
00016 *    Re-organized the CMSIS folders and updated documentation.  
00017 *   
00018 * Version 1.0.2 2010/11/11  
00019 *    Documentation updated.   
00020 *  
00021 * Version 1.0.1 2010/10/05   
00022 *    Production release and review comments incorporated.  
00023 *  
00024 * Version 1.0.0 2010/09/20   
00025 *    Production release and review comments incorporated.  
00026 *  
00027 * Version 0.0.5  2010/04/26   
00028 *    incorporated review comments and updated with latest CMSIS layer  
00029 *  
00030 * Version 0.0.3  2010/03/10   
00031 *    Initial version  
00032 * -------------------------------------------------------------------- */ 
00033  
00034 #include "arm_math.h" 
00035  
00036 /**  
00037  * @ingroup groupFilters  
00038  */ 
00039  
00040 /**  
00041  * @addtogroup BiquadCascadeDF1  
00042  * @{  
00043  */ 
00044  
00045 /**  
00046  * @brief Processing function for the Q15 Biquad cascade filter.  
00047  * @param[in]  *S points to an instance of the Q15 Biquad cascade structure.  
00048  * @param[in]  *pSrc points to the block of input data.  
00049  * @param[out] *pDst points to the location where the output result is written.  
00050  * @param[in]  blockSize number of samples to process per call.  
00051  * @return none.  
00052  *  
00053  *  
00054  * <b>Scaling and Overflow Behavior:</b>  
00055  * \par  
00056  * The function is implemented using a 64-bit internal accumulator.  
00057  * Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.  
00058  * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.  
00059  * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.  
00060  * The accumulator is then shifted by <code>postShift</code> bits to truncate the result to 1.15 format by discarding the low 16 bits.  
00061  * Finally, the result is saturated to 1.15 format.  
00062  *  
00063  * \par  
00064  * Refer to the function <code>arm_biquad_cascade_df1_fast_q15()</code> for a faster but less precise implementation of this filter.  
00065  */ 
00066  
00067 void arm_biquad_cascade_df1_q15( 
00068   const arm_biquad_casd_df1_inst_q15 * S, 
00069   q15_t * pSrc, 
00070   q15_t * pDst, 
00071   uint32_t blockSize) 
00072 { 
00073   q15_t *pIn = pSrc;                             /*  Source pointer                               */ 
00074   q15_t *pOut = pDst;                            /*  Destination pointer                          */ 
00075   q31_t in;                                      /*  Temporary variable to hold input value       */ 
00076   q31_t out;                                     /*  Temporary variable to hold output value      */ 
00077   q15_t b0;                                      /*  Temporary variable to hold bo value          */ 
00078   q31_t b1, a1;                                  /*  Filter coefficients                          */ 
00079   q31_t state_in, state_out;                     /*  Filter state variables                       */ 
00080   q63_t acc;                                     /*  Accumulator                                  */ 
00081   int32_t shift = (15 - (int32_t) S->postShift); /*  Post shift                                   */ 
00082   q15_t *pState = S->pState;                     /*  State pointer                                */ 
00083   q15_t *pCoeffs = S->pCoeffs;                   /*  Coefficient pointer                          */ 
00084   q31_t *pState_q31;                             /*  32-bit state pointer for SIMD implementation */ 
00085   uint32_t sample, stage = (uint32_t) S->numStages;     /*  Stage loop counter                           */ 
00086  
00087  
00088  
00089   do 
00090   { 
00091     /* Initialize state pointer of type q31 */ 
00092     pState_q31 = (q31_t *) (pState); 
00093  
00094     /* Read the b0 and 0 coefficients using SIMD  */ 
00095     b0 = *__SIMD32(pCoeffs)++; 
00096  
00097     /* Read the b1 and b2 coefficients using SIMD */ 
00098     b1 = *__SIMD32(pCoeffs)++; 
00099  
00100     /* Read the a1 and a2 coefficients using SIMD */ 
00101     a1 = *__SIMD32(pCoeffs)++; 
00102  
00103     /* Read the input state values from the state buffer:  x[n-1], x[n-2] */ 
00104     state_in = (q31_t) (*pState_q31++); 
00105  
00106     /* Read the output state values from the state buffer:  y[n-1], y[n-2] */ 
00107     state_out = (q31_t) (*pState_q31); 
00108  
00109     /* Apply loop unrolling and compute 2 output values simultaneously. */ 
00110     /*      The variable acc hold output values that are being computed:  
00111      *  
00112      *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]  
00113      *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]  
00114      */ 
00115     sample = blockSize >> 1u; 
00116  
00117     /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.  
00118      ** a second loop below computes the remaining 1 sample. */ 
00119     while(sample > 0u) 
00120     { 
00121  
00122       /* Read the input */ 
00123       in = *__SIMD32(pIn)++; 
00124  
00125       /* out =  b0 * x[n] + 0 * 0 */ 
00126       out = (q31_t) b0 * ((q15_t) in); 
00127       /* acc +=  b1 * x[n-1] +  b2 * x[n-2] + out */ 
00128       acc = __SMLALD(b1, state_in, out); 
00129       /* acc +=  a1 * y[n-1] +  a2 * y[n-2] */ 
00130       acc = __SMLALD(a1, state_out, acc); 
00131  
00132       /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */ 
00133       out = __SSAT((acc >> shift), 16); 
00134  
00135       /* Every time after the output is computed state should be updated. */ 
00136       /* The states should be updated as:  */ 
00137       /* Xn2 = Xn1    */ 
00138       /* Xn1 = Xn     */ 
00139       /* Yn2 = Yn1    */ 
00140       /* Yn1 = acc   */ 
00141       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */ 
00142       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */ 
00143       state_in = __PKHBT(in, state_in, 16); 
00144       state_out = __PKHBT(out, state_out, 16); 
00145  
00146       /* out =  b0 * x[n] + 0 * 0 */ 
00147       out = (q31_t) b0 * ((q15_t) (in >> 16)); 
00148       /* acc +=  b1 * x[n-1] +  b2 * x[n-2] + out */ 
00149       acc = __SMLALD(b1, state_in, out); 
00150       /* acc +=  a1 * y[n-1] + a2 * y[n-2] */ 
00151       acc = __SMLALD(a1, state_out, acc); 
00152  
00153       /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */ 
00154       out = __SSAT((acc >> shift), 16); 
00155  
00156       /* Store the output in the destination buffer. */ 
00157       *__SIMD32(pOut)++ = __PKHBT(state_out, out, 16); 
00158  
00159       /* Every time after the output is computed state should be updated. */ 
00160       /* The states should be updated as:  */ 
00161       /* Xn2 = Xn1    */ 
00162       /* Xn1 = Xn     */ 
00163       /* Yn2 = Yn1    */ 
00164       /* Yn1 = acc   */ 
00165       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */ 
00166       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */ 
00167       state_in = __PKHBT(in >> 16, state_in, 16); 
00168       state_out = __PKHBT(out, state_out, 16); 
00169  
00170       /* Decrement the loop counter */ 
00171       sample--; 
00172  
00173     } 
00174  
00175     /* If the blockSize is not a multiple of 2, compute any remaining output samples here.  
00176      ** No loop unrolling is used. */ 
00177  
00178     if((blockSize & 0x1u) != 0u) 
00179     { 
00180       /* Read the input */ 
00181       in = *pIn++; 
00182  
00183       /* out =  b0 * x[n] + 0 * 0 */ 
00184       out = (q31_t) in *b0; 
00185       /* acc =  b1 * x[n-1] + b2 * x[n-2] + out */ 
00186       acc = __SMLALD(b1, state_in, out); 
00187       /* acc +=  a1 * y[n-1] + a2 * y[n-2] */ 
00188       acc = __SMLALD(a1, state_out, acc); 
00189  
00190       /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */ 
00191       out = __SSAT((acc >> shift), 16); 
00192  
00193       /* Store the output in the destination buffer. */ 
00194       *pOut++ = (q15_t) out; 
00195  
00196       /* Every time after the output is computed state should be updated. */ 
00197       /* The states should be updated as:  */ 
00198       /* Xn2 = Xn1    */ 
00199       /* Xn1 = Xn     */ 
00200       /* Yn2 = Yn1    */ 
00201       /* Yn1 = acc   */ 
00202       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */ 
00203       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */ 
00204       state_in = __PKHBT(in, state_in, 16); 
00205       state_out = __PKHBT(out, state_out, 16); 
00206  
00207     } 
00208  
00209     /*  The first stage goes from the input wire to the output wire.  */ 
00210     /*  Subsequent numStages occur in-place in the output wire  */ 
00211     pIn = pDst; 
00212  
00213     /* Reset the output pointer */ 
00214     pOut = pDst; 
00215  
00216     /*  Store the updated state variables back into the state array */ 
00217     *__SIMD32(pState)++ = __PKHBT(state_in, (state_in >> 16), 16); 
00218     *__SIMD32(pState)++ = __PKHBT(state_out, (state_out >> 16), 16); 
00219  
00220     /* Decrement the loop counter */ 
00221     stage--; 
00222  
00223   } while(stage > 0u); 
00224 } 
00225  
00226  
00227 /**  
00228  * @} end of BiquadCascadeDF1 group  
00229  */