arm_iir_lattice_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_iir_lattice_f32.c      
*      
* Description:  Floating-point IIR Lattice filter processing function.      
*      
* 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_iir_lattice_f32(     
  const arm_iir_lattice_instance_f32 * S,     
  float32_t * pSrc,     
  float32_t * pDst,     
  uint32_t blockSize)     
{     
  float32_t fnext1, gcurr1, gnext;          /* Temporary variables for lattice stages */     
  float32_t acc;                                 /* Accumlator */     
  uint32_t blkCnt, tapCnt;                       /* temporary variables for counts */     
  float32_t *px1, *px2, *pk, *pv;                /* temporary pointers for state and coef */     
  uint32_t numStages = S->numStages;             /* number of stages */     
  float32_t *pState;                             /* State pointer */     
  float32_t *pStateCurnt;                        /* State current pointer */  
  float32_t k1, k2;     
  float32_t  v1, v2, v3, v4;  
  float32_t  gcurr2;   
  float32_t fnext2;  
     
  /* initialise loop count */    
  blkCnt = blockSize;     
    
  /* initialise state pointer */   
  pState = &S->pState[0];     
     
  /* Sample processing */     
  while(blkCnt > 0u)     
  {     
    /* Read Sample from input buffer */     
    /* fN(n) = x(n) */     
    fnext2 = *pSrc++;     
      
    /* Initialize Ladder coeff pointer */     
    pv = &S->pvCoeffs[0];     
    /* Initialize Reflection coeff pointer */     
    pk = &S->pkCoeffs[0];      
     
    /* Initialize state read pointer */     
    px1 = pState;     
    /* Initialize state write pointer */     
    px2 = pState;    
  
    /* Set accumulator to zero */     
    acc = 0.0;     
     
    /* Loop unrolling.  Process 4 taps at a time. */     
    tapCnt = (numStages) >> 2;     
     
    while(tapCnt > 0u)     
    {     
      /* Read gN-1(n-1) from state buffer */     
      gcurr1 = *px1;  
  
      /* read reflection coefficient kN */  
      k1 = *pk;  
  
      /* fN-1(n) = fN(n) - kN * gN-1(n-1) */     
      fnext1 = fnext2 - (k1 * gcurr1);  
  
      /* read ladder coefficient vN */  
      v1 = *pv;  
            
      /* read next reflection coefficient kN-1 */  
      k2 =  *(pk + 1u);        
  
      /* Read gN-2(n-1) from state buffer */     
      gcurr2 = *(px1 + 1u);   
        
      /* read next ladder coefficient vN-1*/  
      v2 = *(pv + 1u);  
  
      /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */  
      fnext2 = fnext1 - (k2 * gcurr2);  
  
      /* gN(n)   = kN * fN-1(n) + gN-1(n-1) */  
      gnext = gcurr1 + (k1 * fnext1);  
     
      /* read reflection coefficient kN-2 */  
      k1 = *(pk + 2u);   
        
      /* write gN(n) into state for next sample processing */     
      *px2++ = gnext;  
  
      /* Read gN-3(n-1) from state buffer */     
      gcurr1 = *(px1 + 2u);  
        
      /* y(n) += gN(n) * vN  */     
      acc += (gnext * v1);     
              
      /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */     
      fnext1 = fnext2 - (k1 * gcurr1);  
  
      /* gN-1(n)   = kN-1 * fN-2(n) + gN-2(n-1) */  
      gnext = gcurr2 + (k2 * fnext2);  
  
      /* Read gN-4(n-1) from state buffer */     
      gcurr2 = *(px1 + 3u);   
        
      /* y(n) += gN-1(n) * vN-1  */     
      acc += (gnext * v2);   
        
      /* read reflection coefficient kN-3 */    
      k2 = *(pk + 3u);  
  
      /* write gN-1(n) into state for next sample processing */     
      *px2++ = gnext;     
  
      /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */     
      fnext2 = fnext1 - (k2 * gcurr2);  
  
      /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */  
      gnext = gcurr1 + (k1 * fnext1);  
        
      /* read ladder coefficient vN-2*/  
      v3 = *(pv + 2u);  
  
      /* y(n) += gN-2(n) * vN-2  */     
      acc += (gnext * v3);   
  
      /* write gN-2(n) into state for next sample processing */     
      *px2++ = gnext;  
        
      /* update pointer */  
      pk += 4u;  
        
       /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */     
      gnext = (fnext2 * k2) + gcurr2;  
        
      /* read next ladder coefficient vN-3*/   
      v4 = *(pv + 3u);  
  
      /* y(n) += gN-4(n) * vN-4  */     
      acc += (gnext * v4);      
           
      /* write gN-3(n) into state for next sample processing */     
      *px2++ = gnext;   
  
      /* update pointers */  
      px1 += 4u;   
      pv += 4u;  
     
      tapCnt--;     
     
    }  
         
    /* If the filter length is not a multiple of 4, compute the remaining filter taps */     
    tapCnt = (numStages) % 0x4u;   
     
    while(tapCnt > 0u)     
    {     
      gcurr1 = *px1++;     
      /* Process sample for last taps */     
      fnext1 = fnext2 - ((*pk) * gcurr1);     
      gnext = (fnext1 * (*pk++)) + gcurr1;     
      /* Output samples for last taps */     
      acc += (gnext * (*pv++));     
      *px2++ = gnext;     
      fnext2 = fnext1;  
     
      tapCnt--;     
     
    }    
     
    /* y(n) += g0(n) * v0 */     
    acc += (fnext2 * (*pv));     
     
    *px2++ = fnext2;     
     
    /* write out into pDst */     
    *pDst++ = acc;     
     
    /* Advance the state pointer by 4 to process the next group of 4 samples */     
    pState = pState + 1u;  
         
    blkCnt--;     
     
  }     
     
  /* Processing is complete. Now copy last S->numStages samples to start of the buffer      
     for the preperation of next frame process */     
     
  /* Points to the start of the state buffer */     
  pStateCurnt = &S->pState[0];     
  pState = &S->pState[blockSize];     
     
  tapCnt = numStages >> 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 = (numStages) % 0x4u;     
     
  /* Copy the remaining q31_t data */     
  while(tapCnt > 0u)     
  {     
    *pStateCurnt++ = *pState++;     
     
    /* Decrement the loop counter */     
    tapCnt--;     
  }     
     
}