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_f32.c 00009 * 00010 * Description: Processing function for the 00011 * floating-point Biquad cascade DirectFormI(DF1) 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 00171 void arm_biquad_cascade_df1_f32( 00172 const arm_biquad_casd_df1_inst_f32 * S, 00173 float32_t * pSrc, 00174 float32_t * pDst, 00175 uint32_t blockSize) 00176 { 00177 float32_t *pIn = pSrc; /* source pointer */ 00178 float32_t *pOut = pDst; /* destination pointer */ 00179 float32_t *pState = S->pState; /* pState pointer */ 00180 float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ 00181 float32_t acc; /* Simulates the accumulator */ 00182 float32_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00183 float32_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */ 00184 float32_t Xn; /* temporary input */ 00185 uint32_t sample, stage = S->numStages; /* loop counters */ 00186 00187 00188 #ifndef ARM_MATH_CM0 00189 00190 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00191 00192 do 00193 { 00194 /* Reading the coefficients */ 00195 b0 = *pCoeffs++; 00196 b1 = *pCoeffs++; 00197 b2 = *pCoeffs++; 00198 a1 = *pCoeffs++; 00199 a2 = *pCoeffs++; 00200 00201 /* Reading the pState values */ 00202 Xn1 = pState[0]; 00203 Xn2 = pState[1]; 00204 Yn1 = pState[2]; 00205 Yn2 = pState[3]; 00206 00207 /* Apply loop unrolling and compute 4 output values simultaneously. */ 00208 /* The variable acc hold output values that are being computed: 00209 * 00210 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00211 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00212 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00213 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00214 */ 00215 00216 sample = blockSize >> 2u; 00217 00218 /* First part of the processing with loop unrolling. Compute 4 outputs at a time. 00219 ** a second loop below computes the remaining 1 to 3 samples. */ 00220 while(sample > 0u) 00221 { 00222 /* Read the first input */ 00223 Xn = *pIn++; 00224 00225 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00226 Yn2 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2); 00227 00228 /* Store the result in the accumulator in the destination buffer. */ 00229 *pOut++ = Yn2; 00230 00231 /* Every time after the output is computed state should be updated. */ 00232 /* The states should be updated as: */ 00233 /* Xn2 = Xn1 */ 00234 /* Xn1 = Xn */ 00235 /* Yn2 = Yn1 */ 00236 /* Yn1 = acc */ 00237 00238 /* Read the second input */ 00239 Xn2 = *pIn++; 00240 00241 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00242 Yn1 = (b0 * Xn2) + (b1 * Xn) + (b2 * Xn1) + (a1 * Yn2) + (a2 * Yn1); 00243 00244 /* Store the result in the accumulator in the destination buffer. */ 00245 *pOut++ = Yn1; 00246 00247 /* Every time after the output is computed state should be updated. */ 00248 /* The states should be updated as: */ 00249 /* Xn2 = Xn1 */ 00250 /* Xn1 = Xn */ 00251 /* Yn2 = Yn1 */ 00252 /* Yn1 = acc */ 00253 00254 /* Read the third input */ 00255 Xn1 = *pIn++; 00256 00257 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00258 Yn2 = (b0 * Xn1) + (b1 * Xn2) + (b2 * Xn) + (a1 * Yn1) + (a2 * Yn2); 00259 00260 /* Store the result in the accumulator in the destination buffer. */ 00261 *pOut++ = Yn2; 00262 00263 /* Every time after the output is computed state should be updated. */ 00264 /* The states should be updated as: */ 00265 /* Xn2 = Xn1 */ 00266 /* Xn1 = Xn */ 00267 /* Yn2 = Yn1 */ 00268 /* Yn1 = acc */ 00269 00270 /* Read the forth input */ 00271 Xn = *pIn++; 00272 00273 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00274 Yn1 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn2) + (a2 * Yn1); 00275 00276 /* Store the result in the accumulator in the destination buffer. */ 00277 *pOut++ = Yn1; 00278 00279 /* Every time after the output is computed state should be updated. */ 00280 /* The states should be updated as: */ 00281 /* Xn2 = Xn1 */ 00282 /* Xn1 = Xn */ 00283 /* Yn2 = Yn1 */ 00284 /* Yn1 = acc */ 00285 Xn2 = Xn1; 00286 Xn1 = Xn; 00287 00288 /* decrement the loop counter */ 00289 sample--; 00290 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 sample = blockSize & 0x3u; 00296 00297 while(sample > 0u) 00298 { 00299 /* Read the input */ 00300 Xn = *pIn++; 00301 00302 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00303 acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2); 00304 00305 /* Store the result in the accumulator in the destination buffer. */ 00306 *pOut++ = acc; 00307 00308 /* Every time after the output is computed state should be updated. */ 00309 /* The states should be updated as: */ 00310 /* Xn2 = Xn1 */ 00311 /* Xn1 = Xn */ 00312 /* Yn2 = Yn1 */ 00313 /* Yn1 = acc */ 00314 Xn2 = Xn1; 00315 Xn1 = Xn; 00316 Yn2 = Yn1; 00317 Yn1 = acc; 00318 00319 /* decrement the loop counter */ 00320 sample--; 00321 00322 } 00323 00324 /* Store the updated state variables back into the pState array */ 00325 *pState++ = Xn1; 00326 *pState++ = Xn2; 00327 *pState++ = Yn1; 00328 *pState++ = Yn2; 00329 00330 /* The first stage goes from the input buffer to the output buffer. */ 00331 /* Subsequent numStages occur in-place in the output buffer */ 00332 pIn = pDst; 00333 00334 /* Reset the output pointer */ 00335 pOut = pDst; 00336 00337 /* decrement the loop counter */ 00338 stage--; 00339 00340 } while(stage > 0u); 00341 00342 #else 00343 00344 /* Run the below code for Cortex-M0 */ 00345 00346 do 00347 { 00348 /* Reading the coefficients */ 00349 b0 = *pCoeffs++; 00350 b1 = *pCoeffs++; 00351 b2 = *pCoeffs++; 00352 a1 = *pCoeffs++; 00353 a2 = *pCoeffs++; 00354 00355 /* Reading the pState values */ 00356 Xn1 = pState[0]; 00357 Xn2 = pState[1]; 00358 Yn1 = pState[2]; 00359 Yn2 = pState[3]; 00360 00361 /* The variables acc holds the output value that is computed: 00362 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00363 */ 00364 00365 sample = blockSize; 00366 00367 while(sample > 0u) 00368 { 00369 /* Read the input */ 00370 Xn = *pIn++; 00371 00372 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00373 acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2); 00374 00375 /* Store the result in the accumulator in the destination buffer. */ 00376 *pOut++ = acc; 00377 00378 /* Every time after the output is computed state should be updated. */ 00379 /* The states should be updated as: */ 00380 /* Xn2 = Xn1 */ 00381 /* Xn1 = Xn */ 00382 /* Yn2 = Yn1 */ 00383 /* Yn1 = acc */ 00384 Xn2 = Xn1; 00385 Xn1 = Xn; 00386 Yn2 = Yn1; 00387 Yn1 = acc; 00388 00389 /* decrement the loop counter */ 00390 sample--; 00391 } 00392 00393 /* Store the updated state variables back into the pState array */ 00394 *pState++ = Xn1; 00395 *pState++ = Xn2; 00396 *pState++ = Yn1; 00397 *pState++ = Yn2; 00398 00399 /* The first stage goes from the input buffer to the output buffer. */ 00400 /* Subsequent numStages occur in-place in the output buffer */ 00401 pIn = pDst; 00402 00403 /* Reset the output pointer */ 00404 pOut = pDst; 00405 00406 /* decrement the loop counter */ 00407 stage--; 00408 00409 } while(stage > 0u); 00410 00411 #endif /* #ifndef ARM_MATH_CM0 */ 00412 00413 } 00414 00415