arm_lms_norm_f32.c

Go to the documentation of this file.

/* ----------------------------------------------------------------------      
* Copyright (C) 2011 ARM Limited. All rights reserved. 
*      
* $Date:        15. December 2011   
* $Revision:    V2.0.0  
*      
* Project:      Cortex-R DSP Library 
* Title:        arm_lms_norm_f32.c      
*      
* Description:  Processing function for the floating-point Normalised LMS.      
*      
* Target Processor:          Cortex-R4/R5
*
* Version 1.0.0 2011/03/08
*     Alpha release.
*
* Version 1.0.1 2011/09/30
*     Beta release.
*
* Version 2.0.0 2011/12/15
*     Final release. 
* 
* -------------------------------------------------------------------- */     
#include "arm_math.h"     
     
void arm_lms_norm_f32(     
  arm_lms_norm_instance_f32 * S,     
  float32_t * pSrc,     
  float32_t * pRef,     
  float32_t * pOut,     
  float32_t * pErr,     
  uint32_t blockSize)     
{     
  float32_t *pState = S->pState;                 /* State pointer */     
  float32_t *pCoeffs = S->pCoeffs;               /* Coefficient pointer */     
  float32_t *pStateCurnt;                        /* Points to the current sample of the state */     
  float32_t *px, *pb;                            /* Temporary pointers for state and coefficient buffers */     
  float32_t mu = S->mu;                          /* Adaptive factor */     
  uint32_t numTaps = S->numTaps;                 /* Number of filter coefficients in the filter */     
  uint32_t tapCnt, blkCnt;                       /* Loop counters */     
  float32_t energy;                              /* Energy of the input */     
  float32_t sum, e, d;                           /* accumulator, error, reference data sample */     
  float32_t w, x0, in;                           /* weight factor, temporary variable to hold input sample and state */     
  float32_t x1, x2;  
  float32_t c0, c1;  
     
  /* Initializations of error,  difference, Coefficient update */     
  e = 0.0f;     
  d = 0.0f;     
  w = 0.0f;     
     
  energy = S->energy;     
  x0 = S->x0;     
     
  /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */     
  /* pStateCurnt points to the location where the new input data should be written */     
  pStateCurnt = &(S->pState[(numTaps - 1u)]);     
     
  blkCnt = blockSize;     
     
  while(blkCnt > 0u)     
  {     
    /* Copy the new input sample into the state buffer */     
    *pStateCurnt++ = *pSrc;     
     
    /* Initialize pState pointer */     
    px = pState;     
     
    /* Initialize coeff pointer */     
    pb = (pCoeffs);     
     
    /* Read the sample from input buffer */     
    in = *pSrc++;     
     
    /* Update the energy calculation */     
    energy -= x0 * x0;     
    energy += in * in;     
     
    /* Set the accumulator to zero */     
    sum = 0.0f;     
     
    /* Loop unrolling.  Process 4 taps at a time. */     
    tapCnt = numTaps >> 2;     
     
    while(tapCnt > 0u)     
    {     
        
      /* Read two values from state and coefficient buffers */  
      x1 = *px;  
      c0 = *pb++;  
      x2 = *(px + 1u);  
      c1 = *pb++;  
           
      /* Perform the multiply-accumulate */     
      sum += x1 * c0;     
      sum += x2 * c1;  
  
      /* Read next two values from state and coefficient buffers */  
      x1 = *(px + 2u);  
      c0 = *pb++;  
      x2 = *(px + 3u);  
      c1 = *pb++;  
  
      /* update state pointer */  
      px += 4u;  
  
      /* Perform the multiply-accumulate */        
      sum += x1 * c0;     
      sum += x2 * c1;     
     
      /* Decrement the loop counter */     
      tapCnt--;     
    }     
     
    /* If the filter length is not a multiple of 4, compute the remaining filter taps */     
    tapCnt = numTaps % 0x4u;     
     
    while(tapCnt > 0u)     
    {     
      /* Perform the multiply-accumulate */     
      sum += (*px++) * (*pb++);     
     
      /* Decrement the loop counter */     
      tapCnt--;     
    }     
     
    /* The result in the accumulator, store in the destination buffer. */     
    *pOut++ = sum;     
     
    /* Compute and store error */     
    d = (float32_t) (*pRef++);     
    e = d - sum;     
    *pErr++ = e;     
     
    /* Calculation of Weighting factor for updating filter coefficients */     
    /* epsilon value 0.000000119209289f */     
    w = (e * mu) / (energy + 0.000000119209289f);     
     
    /* Initialize pState pointer */     
    px = pState;     
     
    /* Initialize coeff pointer */     
    pb = (pCoeffs);     
     
    /* Loop unrolling.  Process 4 taps at a time. */     
    tapCnt = numTaps >> 2;     
     
    /* Update filter coefficients */     
    while(tapCnt > 0u)     
    {     
        /* *pb++ = *pb + (w * (*px++)); */  
  
        /* Read two values from state and coefficient buffers */  
        x1 = *px;  
        x2 = *(px + 1u);  
        c0 = *pb;  
        c1 = *(pb + 1u);  
  
        /* Perform the multiply-accumulate on coefficient pointer */  
        c0 = c0 + x1 * w;   
        c1 = c1 + x2 * w;  
  
        /* update coefficient value and store in coeff  buffer */  
        *pb = c0;  
        *(pb + 1u) = c1;  
  
        /* Read next two values from state and coefficient buffers */  
        x1 = *(px + 2u);  
        x2 = *(px + 3u);  
        c0 = *(pb + 2u);  
        c1 = *(pb + 3u);  
  
        /* Perform the multiply-accumulate on coefficient pointer */  
        c0 = c0 + x1 * w;   
        c1 = c1 + x2 * w;  
  
        /* update coefficient value and store in coeff  buffer */  
        *(pb + 2u) = c0;  
        *(pb + 3u) = c1;  
  
        /* update pointers */  
        px += 4u;  
        pb += 4u;     
             
      /* Decrement the loop counter */     
      tapCnt--;     
    }     
     
    /* If the filter length is not a multiple of 4, compute the remaining filter taps */     
    tapCnt = numTaps % 0x4u;     
     
    while(tapCnt > 0u)     
    {     
      /* Perform the multiply-accumulate */     
      *pb += w * (*px++);     
      pb++;     
     
      /* Decrement the loop counter */     
      tapCnt--;     
    }     
     
    x0 = *pState;     
     
    /* Advance state pointer by 1 for the next sample */     
    pState = pState + 1;     
     
    /* Decrement the loop counter */     
    blkCnt--;     
  }     
     
  S->energy = energy;     
  S->x0 = x0;     
     
  /* Processing is complete. Now copy the last numTaps - 1 samples to the      
     satrt of the state buffer. This prepares the state buffer for the      
     next function call. */     
     
  /* Points to the start of the pState buffer */     
  pStateCurnt = S->pState;     
     
  /* Loop unrolling for (numTaps - 1u)/4 samples copy */     
  tapCnt = (numTaps - 1u) >> 2u;     
     
  /* copy data */     
  while(tapCnt > 0u)     
  {     
    *pStateCurnt++ = *pState++;     
    *pStateCurnt++ = *pState++;     
    *pStateCurnt++ = *pState++;     
    *pStateCurnt++ = *pState++;     
     
    /* Decrement the loop counter */     
    tapCnt--;     
  }     
     
  /* Calculate remaining number of copies */     
  tapCnt = (numTaps - 1u) % 0x4u;     
     
  /* Copy the remaining q31_t data */     
  while(tapCnt > 0u)     
  {     
    *pStateCurnt++ = *pState++;     
     
    /* Decrement the loop counter */     
    tapCnt--;     
  }     
     
     
}