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_lms_f32.c 00009 * 00010 * Description: Processing function for the floating-point LMS filter. 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 00170 void arm_lms_f32( 00171 const arm_lms_instance_f32 * S, 00172 float32_t * pSrc, 00173 float32_t * pRef, 00174 float32_t * pOut, 00175 float32_t * pErr, 00176 uint32_t blockSize) 00177 { 00178 float32_t *pState = S->pState; /* State pointer */ 00179 float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00180 float32_t *pStateCurnt; /* Points to the current sample of the state */ 00181 float32_t *px, *pb; /* Temporary pointers for state and coefficient buffers */ 00182 float32_t mu = S->mu; /* Adaptive factor */ 00183 uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */ 00184 uint32_t tapCnt, blkCnt; /* Loop counters */ 00185 float32_t sum, e, d; /* accumulator, error, reference data sample */ 00186 float32_t w = 0.0f; /* weight factor */ 00187 00188 e = 0.0f; 00189 d = 0.0f; 00190 00191 /* S->pState points to state array which contains previous frame (numTaps - 1) samples */ 00192 /* pStateCurnt points to the location where the new input data should be written */ 00193 pStateCurnt = &(S->pState[(numTaps - 1u)]); 00194 00195 blkCnt = blockSize; 00196 00197 00198 #ifndef ARM_MATH_CM0 00199 00200 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00201 00202 while(blkCnt > 0u) 00203 { 00204 /* Copy the new input sample into the state buffer */ 00205 *pStateCurnt++ = *pSrc++; 00206 00207 /* Initialize pState pointer */ 00208 px = pState; 00209 00210 /* Initialize coeff pointer */ 00211 pb = (pCoeffs); 00212 00213 /* Set the accumulator to zero */ 00214 sum = 0.0f; 00215 00216 /* Loop unrolling. Process 4 taps at a time. */ 00217 tapCnt = numTaps >> 2; 00218 00219 while(tapCnt > 0u) 00220 { 00221 /* Perform the multiply-accumulate */ 00222 sum += (*px++) * (*pb++); 00223 sum += (*px++) * (*pb++); 00224 sum += (*px++) * (*pb++); 00225 sum += (*px++) * (*pb++); 00226 00227 /* Decrement the loop counter */ 00228 tapCnt--; 00229 } 00230 00231 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00232 tapCnt = numTaps % 0x4u; 00233 00234 while(tapCnt > 0u) 00235 { 00236 /* Perform the multiply-accumulate */ 00237 sum += (*px++) * (*pb++); 00238 00239 /* Decrement the loop counter */ 00240 tapCnt--; 00241 } 00242 00243 /* The result in the accumulator, store in the destination buffer. */ 00244 *pOut++ = sum; 00245 00246 /* Compute and store error */ 00247 d = (float32_t) (*pRef++); 00248 e = d - sum; 00249 *pErr++ = e; 00250 00251 /* Calculation of Weighting factor for the updating filter coefficients */ 00252 w = e * mu; 00253 00254 /* Initialize pState pointer */ 00255 px = pState; 00256 00257 /* Initialize coeff pointer */ 00258 pb = (pCoeffs); 00259 00260 /* Loop unrolling. Process 4 taps at a time. */ 00261 tapCnt = numTaps >> 2; 00262 00263 /* Update filter coefficients */ 00264 while(tapCnt > 0u) 00265 { 00266 /* Perform the multiply-accumulate */ 00267 *pb = *pb + (w * (*px++)); 00268 pb++; 00269 00270 *pb = *pb + (w * (*px++)); 00271 pb++; 00272 00273 *pb = *pb + (w * (*px++)); 00274 pb++; 00275 00276 *pb = *pb + (w * (*px++)); 00277 pb++; 00278 00279 /* Decrement the loop counter */ 00280 tapCnt--; 00281 } 00282 00283 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00284 tapCnt = numTaps % 0x4u; 00285 00286 while(tapCnt > 0u) 00287 { 00288 /* Perform the multiply-accumulate */ 00289 *pb = *pb + (w * (*px++)); 00290 pb++; 00291 00292 /* Decrement the loop counter */ 00293 tapCnt--; 00294 } 00295 00296 /* Advance state pointer by 1 for the next sample */ 00297 pState = pState + 1; 00298 00299 /* Decrement the loop counter */ 00300 blkCnt--; 00301 } 00302 00303 00304 /* Processing is complete. Now copy the last numTaps - 1 samples to the 00305 satrt of the state buffer. This prepares the state buffer for the 00306 next function call. */ 00307 00308 /* Points to the start of the pState buffer */ 00309 pStateCurnt = S->pState; 00310 00311 /* Loop unrolling for (numTaps - 1u) samples copy */ 00312 tapCnt = (numTaps - 1u) >> 2u; 00313 00314 /* copy data */ 00315 while(tapCnt > 0u) 00316 { 00317 *pStateCurnt++ = *pState++; 00318 *pStateCurnt++ = *pState++; 00319 *pStateCurnt++ = *pState++; 00320 *pStateCurnt++ = *pState++; 00321 00322 /* Decrement the loop counter */ 00323 tapCnt--; 00324 } 00325 00326 /* Calculate remaining number of copies */ 00327 tapCnt = (numTaps - 1u) % 0x4u; 00328 00329 /* Copy the remaining q31_t data */ 00330 while(tapCnt > 0u) 00331 { 00332 *pStateCurnt++ = *pState++; 00333 00334 /* Decrement the loop counter */ 00335 tapCnt--; 00336 } 00337 00338 #else 00339 00340 /* Run the below code for Cortex-M0 */ 00341 00342 while(blkCnt > 0u) 00343 { 00344 /* Copy the new input sample into the state buffer */ 00345 *pStateCurnt++ = *pSrc++; 00346 00347 /* Initialize pState pointer */ 00348 px = pState; 00349 00350 /* Initialize pCoeffs pointer */ 00351 pb = pCoeffs; 00352 00353 /* Set the accumulator to zero */ 00354 sum = 0.0f; 00355 00356 /* Loop over numTaps number of values */ 00357 tapCnt = numTaps; 00358 00359 while(tapCnt > 0u) 00360 { 00361 /* Perform the multiply-accumulate */ 00362 sum += (*px++) * (*pb++); 00363 00364 /* Decrement the loop counter */ 00365 tapCnt--; 00366 } 00367 00368 /* The result is stored in the destination buffer. */ 00369 *pOut++ = sum; 00370 00371 /* Compute and store error */ 00372 d = (float32_t) (*pRef++); 00373 e = d - sum; 00374 *pErr++ = e; 00375 00376 /* Weighting factor for the LMS version */ 00377 w = e * mu; 00378 00379 /* Initialize pState pointer */ 00380 px = pState; 00381 00382 /* Initialize pCoeffs pointer */ 00383 pb = pCoeffs; 00384 00385 /* Loop over numTaps number of values */ 00386 tapCnt = numTaps; 00387 00388 while(tapCnt > 0u) 00389 { 00390 /* Perform the multiply-accumulate */ 00391 *pb = *pb + (w * (*px++)); 00392 pb++; 00393 00394 /* Decrement the loop counter */ 00395 tapCnt--; 00396 } 00397 00398 /* Advance state pointer by 1 for the next sample */ 00399 pState = pState + 1; 00400 00401 /* Decrement the loop counter */ 00402 blkCnt--; 00403 } 00404 00405 00406 /* Processing is complete. Now copy the last numTaps - 1 samples to the 00407 * start of the state buffer. This prepares the state buffer for the 00408 * next function call. */ 00409 00410 /* Points to the start of the pState buffer */ 00411 pStateCurnt = S->pState; 00412 00413 /* Copy (numTaps - 1u) samples */ 00414 tapCnt = (numTaps - 1u); 00415 00416 /* Copy the data */ 00417 while(tapCnt > 0u) 00418 { 00419 *pStateCurnt++ = *pState++; 00420 00421 /* Decrement the loop counter */ 00422 tapCnt--; 00423 } 00424 00425 #endif /* #ifndef ARM_MATH_CM0 */ 00426 00427 } 00428