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_iir_lattice_f32.c 00009 * 00010 * Description: Floating-point IIR Lattice filter processing function. 00011 * 00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 00013 * 00014 * Version 1.0.10 2011/7/15 00015 * Big Endian support added and Merged M0 and M3/M4 Source code. 00016 * 00017 * Version 1.0.3 2010/11/29 00018 * Re-organized the CMSIS folders and updated documentation. 00019 * 00020 * Version 1.0.2 2010/11/11 00021 * Documentation updated. 00022 * 00023 * Version 1.0.1 2010/10/05 00024 * Production release and review comments incorporated. 00025 * 00026 * Version 1.0.0 2010/09/20 00027 * Production release and review comments incorporated 00028 * 00029 * Version 0.0.7 2010/06/10 00030 * Misra-C changes done 00031 * -------------------------------------------------------------------- */ 00032 00033 #include "arm_math.h" 00034 00120 void arm_iir_lattice_f32( 00121 const arm_iir_lattice_instance_f32 * S, 00122 float32_t * pSrc, 00123 float32_t * pDst, 00124 uint32_t blockSize) 00125 { 00126 float32_t fcurr, fnext = 0, gcurr, gnext; /* Temporary variables for lattice stages */ 00127 float32_t acc; /* Accumlator */ 00128 uint32_t blkCnt, tapCnt; /* temporary variables for counts */ 00129 float32_t *px1, *px2, *pk, *pv; /* temporary pointers for state and coef */ 00130 uint32_t numStages = S->numStages; /* number of stages */ 00131 float32_t *pState; /* State pointer */ 00132 float32_t *pStateCurnt; /* State current pointer */ 00133 00134 00135 #ifndef ARM_MATH_CM0 00136 00137 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00138 00139 gcurr = 0.0f; 00140 blkCnt = blockSize; 00141 00142 pState = &S->pState[0]; 00143 00144 /* Sample processing */ 00145 while(blkCnt > 0u) 00146 { 00147 /* Read Sample from input buffer */ 00148 /* fN(n) = x(n) */ 00149 fcurr = *pSrc++; 00150 00151 /* Initialize state read pointer */ 00152 px1 = pState; 00153 /* Initialize state write pointer */ 00154 px2 = pState; 00155 /* Set accumulator to zero */ 00156 acc = 0.0f; 00157 /* Initialize Ladder coeff pointer */ 00158 pv = &S->pvCoeffs[0]; 00159 /* Initialize Reflection coeff pointer */ 00160 pk = &S->pkCoeffs[0]; 00161 00162 00163 /* Process sample for first tap */ 00164 gcurr = *px1++; 00165 /* fN-1(n) = fN(n) - kN * gN-1(n-1) */ 00166 fnext = fcurr - ((*pk) * gcurr); 00167 /* gN(n) = kN * fN-1(n) + gN-1(n-1) */ 00168 gnext = (fnext * (*pk++)) + gcurr; 00169 /* write gN(n) into state for next sample processing */ 00170 *px2++ = gnext; 00171 /* y(n) += gN(n) * vN */ 00172 acc += (gnext * (*pv++)); 00173 00174 /* Update f values for next coefficient processing */ 00175 fcurr = fnext; 00176 00177 /* Loop unrolling. Process 4 taps at a time. */ 00178 tapCnt = (numStages - 1u) >> 2; 00179 00180 while(tapCnt > 0u) 00181 { 00182 /* Process sample for 2nd, 6th ...taps */ 00183 /* Read gN-2(n-1) from state buffer */ 00184 gcurr = *px1++; 00185 /* Process sample for 2nd, 6th .. taps */ 00186 /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */ 00187 fnext = fcurr - ((*pk) * gcurr); 00188 /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */ 00189 gnext = (fnext * (*pk++)) + gcurr; 00190 /* y(n) += gN-1(n) * vN-1 */ 00191 /* process for gN-5(n) * vN-5, gN-9(n) * vN-9 ... */ 00192 acc += (gnext * (*pv++)); 00193 /* write gN-1(n) into state for next sample processing */ 00194 *px2++ = gnext; 00195 00196 00197 /* Process sample for 3nd, 7th ...taps */ 00198 /* Read gN-3(n-1) from state buffer */ 00199 gcurr = *px1++; 00200 /* Process sample for 3rd, 7th .. taps */ 00201 /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */ 00202 fcurr = fnext - ((*pk) * gcurr); 00203 /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */ 00204 gnext = (fcurr * (*pk++)) + gcurr; 00205 /* y(n) += gN-2(n) * vN-2 */ 00206 /* process for gN-6(n) * vN-6, gN-10(n) * vN-10 ... */ 00207 acc += (gnext * (*pv++)); 00208 /* write gN-2(n) into state for next sample processing */ 00209 *px2++ = gnext; 00210 00211 00212 /* Process sample for 4th, 8th ...taps */ 00213 /* Read gN-4(n-1) from state buffer */ 00214 gcurr = *px1++; 00215 /* Process sample for 4th, 8th .. taps */ 00216 /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */ 00217 fnext = fcurr - ((*pk) * gcurr); 00218 /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */ 00219 gnext = (fnext * (*pk++)) + gcurr; 00220 /* y(n) += gN-3(n) * vN-3 */ 00221 /* process for gN-7(n) * vN-7, gN-11(n) * vN-11 ... */ 00222 acc += (gnext * (*pv++)); 00223 /* write gN-3(n) into state for next sample processing */ 00224 *px2++ = gnext; 00225 00226 00227 /* Process sample for 5th, 9th ...taps */ 00228 /* Read gN-5(n-1) from state buffer */ 00229 gcurr = *px1++; 00230 /* Process sample for 5th, 9th .. taps */ 00231 /* fN-5(n) = fN-4(n) - kN-4 * gN-1(n-1) */ 00232 fcurr = fnext - ((*pk) * gcurr); 00233 /* gN-4(n) = kN-4 * fN-5(n) + gN-5(n-1) */ 00234 gnext = (fcurr * (*pk++)) + gcurr; 00235 /* y(n) += gN-4(n) * vN-4 */ 00236 /* process for gN-8(n) * vN-8, gN-12(n) * vN-12 ... */ 00237 acc += (gnext * (*pv++)); 00238 /* write gN-4(n) into state for next sample processing */ 00239 *px2++ = gnext; 00240 00241 tapCnt--; 00242 00243 } 00244 00245 fnext = fcurr; 00246 00247 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00248 tapCnt = (numStages - 1u) % 0x4u; 00249 00250 while(tapCnt > 0u) 00251 { 00252 gcurr = *px1++; 00253 /* Process sample for last taps */ 00254 fnext = fcurr - ((*pk) * gcurr); 00255 gnext = (fnext * (*pk++)) + gcurr; 00256 /* Output samples for last taps */ 00257 acc += (gnext * (*pv++)); 00258 *px2++ = gnext; 00259 fcurr = fnext; 00260 00261 tapCnt--; 00262 00263 } 00264 00265 00266 /* y(n) += g0(n) * v0 */ 00267 acc += (fnext * (*pv)); 00268 00269 *px2++ = fnext; 00270 00271 /* write out into pDst */ 00272 *pDst++ = acc; 00273 00274 /* Advance the state pointer by 4 to process the next group of 4 samples */ 00275 pState = pState + 1u; 00276 blkCnt--; 00277 00278 } 00279 00280 /* Processing is complete. Now copy last S->numStages samples to start of the buffer 00281 for the preperation of next frame process */ 00282 00283 /* Points to the start of the state buffer */ 00284 pStateCurnt = &S->pState[0]; 00285 pState = &S->pState[blockSize]; 00286 00287 tapCnt = numStages >> 2u; 00288 00289 /* copy data */ 00290 while(tapCnt > 0u) 00291 { 00292 *pStateCurnt++ = *pState++; 00293 *pStateCurnt++ = *pState++; 00294 *pStateCurnt++ = *pState++; 00295 *pStateCurnt++ = *pState++; 00296 00297 /* Decrement the loop counter */ 00298 tapCnt--; 00299 00300 } 00301 00302 /* Calculate remaining number of copies */ 00303 tapCnt = (numStages) % 0x4u; 00304 00305 /* Copy the remaining q31_t data */ 00306 while(tapCnt > 0u) 00307 { 00308 *pStateCurnt++ = *pState++; 00309 00310 /* Decrement the loop counter */ 00311 tapCnt--; 00312 } 00313 00314 #else 00315 00316 /* Run the below code for Cortex-M0 */ 00317 00318 blkCnt = blockSize; 00319 00320 pState = &S->pState[0]; 00321 00322 /* Sample processing */ 00323 while(blkCnt > 0u) 00324 { 00325 /* Read Sample from input buffer */ 00326 /* fN(n) = x(n) */ 00327 fcurr = *pSrc++; 00328 00329 /* Initialize state read pointer */ 00330 px1 = pState; 00331 /* Initialize state write pointer */ 00332 px2 = pState; 00333 /* Set accumulator to zero */ 00334 acc = 0.0f; 00335 /* Initialize Ladder coeff pointer */ 00336 pv = &S->pvCoeffs[0]; 00337 /* Initialize Reflection coeff pointer */ 00338 pk = &S->pkCoeffs[0]; 00339 00340 00341 /* Process sample for numStages */ 00342 tapCnt = numStages; 00343 00344 while(tapCnt > 0u) 00345 { 00346 gcurr = *px1++; 00347 /* Process sample for last taps */ 00348 fnext = fcurr - ((*pk) * gcurr); 00349 gnext = (fnext * (*pk++)) + gcurr; 00350 00351 /* Output samples for last taps */ 00352 acc += (gnext * (*pv++)); 00353 *px2++ = gnext; 00354 fcurr = fnext; 00355 00356 /* Decrementing loop counter */ 00357 tapCnt--; 00358 00359 } 00360 00361 /* y(n) += g0(n) * v0 */ 00362 acc += (fnext * (*pv)); 00363 00364 *px2++ = fnext; 00365 00366 /* write out into pDst */ 00367 *pDst++ = acc; 00368 00369 /* Advance the state pointer by 1 to process the next group of samples */ 00370 pState = pState + 1u; 00371 blkCnt--; 00372 00373 } 00374 00375 /* Processing is complete. Now copy last S->numStages samples to start of the buffer 00376 for the preperation of next frame process */ 00377 00378 /* Points to the start of the state buffer */ 00379 pStateCurnt = &S->pState[0]; 00380 pState = &S->pState[blockSize]; 00381 00382 tapCnt = numStages; 00383 00384 /* Copy the data */ 00385 while(tapCnt > 0u) 00386 { 00387 *pStateCurnt++ = *pState++; 00388 00389 /* Decrement the loop counter */ 00390 tapCnt--; 00391 } 00392 00393 #endif /* #ifndef ARM_MATH_CM0 */ 00394 00395 } 00396 00397 00398 00399