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