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_biquad_cascade_df1_q31.c 00009 * 00010 * Description: Processing function for the 00011 * Q31 Biquad cascade filter 00012 * 00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 00014 * 00015 * Version 1.0.10 2011/7/15 00016 * Big Endian support added and Merged M0 and M3/M4 Source code. 00017 * 00018 * Version 1.0.3 2010/11/29 00019 * Re-organized the CMSIS folders and updated documentation. 00020 * 00021 * Version 1.0.2 2010/11/11 00022 * Documentation updated. 00023 * 00024 * Version 1.0.1 2010/10/05 00025 * Production release and review comments incorporated. 00026 * 00027 * Version 1.0.0 2010/09/20 00028 * Production release and review comments incorporated. 00029 * 00030 * Version 0.0.5 2010/04/26 00031 * incorporated review comments and updated with latest CMSIS layer 00032 * 00033 * Version 0.0.3 2010/03/10 00034 * Initial version 00035 * -------------------------------------------------------------------- */ 00036 00037 #include "arm_math.h" 00038 00069 void arm_biquad_cascade_df1_q31( 00070 const arm_biquad_casd_df1_inst_q31 * S, 00071 q31_t * pSrc, 00072 q31_t * pDst, 00073 uint32_t blockSize) 00074 { 00075 q31_t *pIn = pSrc; /* input pointer initialization */ 00076 q31_t *pOut = pDst; /* output pointer initialization */ 00077 q31_t *pState = S->pState; /* pState pointer initialization */ 00078 q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */ 00079 q63_t acc; /* accumulator */ 00080 q31_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */ 00081 q31_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00082 q31_t Xn; /* temporary input */ 00083 uint32_t shift = 32u - ((uint32_t) S->postShift + 1u); /* Shift to be applied to the output */ 00084 uint32_t sample, stage = S->numStages; /* loop counters */ 00085 00086 00087 #ifndef ARM_MATH_CM0 00088 00089 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00090 00091 do 00092 { 00093 /* Reading the coefficients */ 00094 b0 = *pCoeffs++; 00095 b1 = *pCoeffs++; 00096 b2 = *pCoeffs++; 00097 a1 = *pCoeffs++; 00098 a2 = *pCoeffs++; 00099 00100 /* Reading the state values */ 00101 Xn1 = pState[0]; 00102 Xn2 = pState[1]; 00103 Yn1 = pState[2]; 00104 Yn2 = pState[3]; 00105 00106 /* Apply loop unrolling and compute 4 output values simultaneously. */ 00107 /* The variable acc hold output values that are being computed: 00108 * 00109 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00110 */ 00111 00112 sample = blockSize >> 2u; 00113 00114 /* First part of the processing with loop unrolling. Compute 4 outputs at a time. 00115 ** a second loop below computes the remaining 1 to 3 samples. */ 00116 while(sample > 0u) 00117 { 00118 /* Read the input */ 00119 Xn = *pIn++; 00120 00121 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00122 00123 /* acc = b0 * x[n] */ 00124 acc = (q63_t) b0 *Xn; 00125 /* acc += b1 * x[n-1] */ 00126 acc += (q63_t) b1 *Xn1; 00127 /* acc += b[2] * x[n-2] */ 00128 acc += (q63_t) b2 *Xn2; 00129 /* acc += a1 * y[n-1] */ 00130 acc += (q63_t) a1 *Yn1; 00131 /* acc += a2 * y[n-2] */ 00132 acc += (q63_t) a2 *Yn2; 00133 00134 /* The result is converted to 1.31 , Yn2 variable is reused */ 00135 Yn2 = (q31_t) (acc >> shift); 00136 00137 /* Store the output in the destination buffer. */ 00138 *pOut++ = Yn2; 00139 00140 /* Read the second input */ 00141 Xn2 = *pIn++; 00142 00143 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00144 00145 /* acc = b0 * x[n] */ 00146 acc = (q63_t) b0 *Xn2; 00147 /* acc += b1 * x[n-1] */ 00148 acc += (q63_t) b1 *Xn; 00149 /* acc += b[2] * x[n-2] */ 00150 acc += (q63_t) b2 *Xn1; 00151 /* acc += a1 * y[n-1] */ 00152 acc += (q63_t) a1 *Yn2; 00153 /* acc += a2 * y[n-2] */ 00154 acc += (q63_t) a2 *Yn1; 00155 00156 00157 /* The result is converted to 1.31, Yn1 variable is reused */ 00158 Yn1 = (q31_t) (acc >> shift); 00159 00160 /* Store the output in the destination buffer. */ 00161 *pOut++ = Yn1; 00162 00163 /* Read the third input */ 00164 Xn1 = *pIn++; 00165 00166 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00167 00168 /* acc = b0 * x[n] */ 00169 acc = (q63_t) b0 *Xn1; 00170 /* acc += b1 * x[n-1] */ 00171 acc += (q63_t) b1 *Xn2; 00172 /* acc += b[2] * x[n-2] */ 00173 acc += (q63_t) b2 *Xn; 00174 /* acc += a1 * y[n-1] */ 00175 acc += (q63_t) a1 *Yn1; 00176 /* acc += a2 * y[n-2] */ 00177 acc += (q63_t) a2 *Yn2; 00178 00179 /* The result is converted to 1.31, Yn2 variable is reused */ 00180 Yn2 = (q31_t) (acc >> shift); 00181 00182 /* Store the output in the destination buffer. */ 00183 *pOut++ = Yn2; 00184 00185 /* Read the forth input */ 00186 Xn = *pIn++; 00187 00188 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00189 00190 /* acc = b0 * x[n] */ 00191 acc = (q63_t) b0 *Xn; 00192 /* acc += b1 * x[n-1] */ 00193 acc += (q63_t) b1 *Xn1; 00194 /* acc += b[2] * x[n-2] */ 00195 acc += (q63_t) b2 *Xn2; 00196 /* acc += a1 * y[n-1] */ 00197 acc += (q63_t) a1 *Yn2; 00198 /* acc += a2 * y[n-2] */ 00199 acc += (q63_t) a2 *Yn1; 00200 00201 /* The result is converted to 1.31, Yn1 variable is reused */ 00202 Yn1 = (q31_t) (acc >> shift); 00203 00204 /* Every time after the output is computed state should be updated. */ 00205 /* The states should be updated as: */ 00206 /* Xn2 = Xn1 */ 00207 /* Xn1 = Xn */ 00208 /* Yn2 = Yn1 */ 00209 /* Yn1 = acc */ 00210 Xn2 = Xn1; 00211 Xn1 = Xn; 00212 00213 /* Store the output in the destination buffer. */ 00214 *pOut++ = Yn1; 00215 00216 /* decrement the loop counter */ 00217 sample--; 00218 } 00219 00220 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00221 ** No loop unrolling is used. */ 00222 sample = (blockSize & 0x3u); 00223 00224 while(sample > 0u) 00225 { 00226 /* Read the input */ 00227 Xn = *pIn++; 00228 00229 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00230 00231 /* acc = b0 * x[n] */ 00232 acc = (q63_t) b0 *Xn; 00233 /* acc += b1 * x[n-1] */ 00234 acc += (q63_t) b1 *Xn1; 00235 /* acc += b[2] * x[n-2] */ 00236 acc += (q63_t) b2 *Xn2; 00237 /* acc += a1 * y[n-1] */ 00238 acc += (q63_t) a1 *Yn1; 00239 /* acc += a2 * y[n-2] */ 00240 acc += (q63_t) a2 *Yn2; 00241 00242 /* The result is converted to 1.31 */ 00243 acc = acc >> shift; 00244 00245 /* Every time after the output is computed state should be updated. */ 00246 /* The states should be updated as: */ 00247 /* Xn2 = Xn1 */ 00248 /* Xn1 = Xn */ 00249 /* Yn2 = Yn1 */ 00250 /* Yn1 = acc */ 00251 Xn2 = Xn1; 00252 Xn1 = Xn; 00253 Yn2 = Yn1; 00254 Yn1 = (q31_t) acc; 00255 00256 /* Store the output in the destination buffer. */ 00257 *pOut++ = (q31_t) acc; 00258 00259 /* decrement the loop counter */ 00260 sample--; 00261 } 00262 00263 /* The first stage goes from the input buffer to the output buffer. */ 00264 /* Subsequent stages occur in-place in the output buffer */ 00265 pIn = pDst; 00266 00267 /* Reset to destination pointer */ 00268 pOut = pDst; 00269 00270 /* Store the updated state variables back into the pState array */ 00271 *pState++ = Xn1; 00272 *pState++ = Xn2; 00273 *pState++ = Yn1; 00274 *pState++ = Yn2; 00275 00276 } while(--stage); 00277 00278 #else 00279 00280 /* Run the below code for Cortex-M0 */ 00281 00282 do 00283 { 00284 /* Reading the coefficients */ 00285 b0 = *pCoeffs++; 00286 b1 = *pCoeffs++; 00287 b2 = *pCoeffs++; 00288 a1 = *pCoeffs++; 00289 a2 = *pCoeffs++; 00290 00291 /* Reading the state values */ 00292 Xn1 = pState[0]; 00293 Xn2 = pState[1]; 00294 Yn1 = pState[2]; 00295 Yn2 = pState[3]; 00296 00297 /* The variables acc holds the output value that is computed: 00298 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00299 */ 00300 00301 sample = blockSize; 00302 00303 while(sample > 0u) 00304 { 00305 /* Read the input */ 00306 Xn = *pIn++; 00307 00308 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00309 /* acc = b0 * x[n] */ 00310 acc = (q63_t) b0 *Xn; 00311 00312 /* acc += b1 * x[n-1] */ 00313 acc += (q63_t) b1 *Xn1; 00314 /* acc += b[2] * x[n-2] */ 00315 acc += (q63_t) b2 *Xn2; 00316 /* acc += a1 * y[n-1] */ 00317 acc += (q63_t) a1 *Yn1; 00318 /* acc += a2 * y[n-2] */ 00319 acc += (q63_t) a2 *Yn2; 00320 00321 /* The result is converted to 1.31 */ 00322 acc = acc >> shift; 00323 00324 /* Every time after the output is computed state should be updated. */ 00325 /* The states should be updated as: */ 00326 /* Xn2 = Xn1 */ 00327 /* Xn1 = Xn */ 00328 /* Yn2 = Yn1 */ 00329 /* Yn1 = acc */ 00330 Xn2 = Xn1; 00331 Xn1 = Xn; 00332 Yn2 = Yn1; 00333 Yn1 = (q31_t) acc; 00334 00335 /* Store the output in the destination buffer. */ 00336 *pOut++ = (q31_t) acc; 00337 00338 /* decrement the loop counter */ 00339 sample--; 00340 } 00341 00342 /* The first stage goes from the input buffer to the output buffer. */ 00343 /* Subsequent stages occur in-place in the output buffer */ 00344 pIn = pDst; 00345 00346 /* Reset to destination pointer */ 00347 pOut = pDst; 00348 00349 /* Store the updated state variables back into the pState array */ 00350 *pState++ = Xn1; 00351 *pState++ = Xn2; 00352 *pState++ = Yn1; 00353 *pState++ = Yn2; 00354 00355 } while(--stage); 00356 00357 #endif /* #ifndef ARM_MATH_CM0 */ 00358 } 00359