- Main Page
- Modules
- Data Structures
- Files
- Examples

- File List
- Globals

# arm_lms_q31.c

Go to the documentation of this file.

    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_q31.c
    00009 *
    00010 * Description:  Processing function for the Q31 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"
    00069 void arm_lms_q31(
    00070   const arm_lms_instance_q31 * S,
    00071   q31_t * pSrc,
    00072   q31_t * pRef,
    00073   q31_t * pOut,
    00074   q31_t * pErr,
    00075   uint32_t blockSize)
    00076 {
    00077   q31_t *pState = S->pState;                     /* State pointer */
    00078   uint32_t numTaps = S->numTaps;                 /* Number of filter coefficients in the filter */
    00079   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
    00080   q31_t *pStateCurnt;                            /* Points to the current sample of the state */
    00081   q31_t mu = S->mu;                              /* Adaptive factor */
    00082   q31_t *px;                                     /* Temporary pointer for state */
    00083   q31_t *pb;                                     /* Temporary pointer for coefficient buffer */
    00084   uint32_t tapCnt, blkCnt;                       /* Loop counters */
    00085   q63_t acc;                                     /* Accumulator */
    00086   q31_t e = 0;                                   /* error of data sample */
    00087   q31_t alpha;                                   /* Intermediate constant for taps update */
    00088   uint8_t shift = (uint8_t) (32u - (S->postShift + 1u));        /* Shift to be applied to the output */
    00089   q31_t coef;                                    /* Temporary variable for coef */
    00090
    00091   /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
    00092   /* pStateCurnt points to the location where the new input data should be written */
    00093   pStateCurnt = &(S->pState[(numTaps - 1u)]);
    00094
    00095   /* Initializing blkCnt with blockSize */
    00096   blkCnt = blockSize;
    00097
    00098
    00099 #ifndef ARM_MATH_CM0
    00100
    00101   /* Run the below code for Cortex-M4 and Cortex-M3 */
    00102
    00103   while(blkCnt > 0u)
    00104   {
    00105     /* Copy the new input sample into the state buffer */
    00106     *pStateCurnt++ = *pSrc++;
    00107
    00108     /* Initialize state pointer */
    00109     px = pState;
    00110
    00111     /* Initialize coefficient pointer */
    00112     pb = pCoeffs;
    00113
    00114     /* Set the accumulator to zero */
    00115     acc = 0;
    00116
    00117     /* Loop unrolling.  Process 4 taps at a time. */
    00118     tapCnt = numTaps >> 2;
    00119
    00120     while(tapCnt > 0u)
    00121     {
    00122       /* Perform the multiply-accumulate */
    00123       /* acc +=  b[N] * x[n-N] */
    00124       acc += ((q63_t) (*px++)) * (*pb++);
    00125
    00126       /* acc +=  b[N-1] * x[n-N-1] */
    00127       acc += ((q63_t) (*px++)) * (*pb++);
    00128
    00129       /* acc +=  b[N-2] * x[n-N-2] */
    00130       acc += ((q63_t) (*px++)) * (*pb++);
    00131
    00132       /* acc +=  b[N-3] * x[n-N-3] */
    00133       acc += ((q63_t) (*px++)) * (*pb++);
    00134
    00135       /* Decrement the loop counter */
    00136       tapCnt--;
    00137     }
    00138
    00139     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
    00140     tapCnt = numTaps % 0x4u;
    00141
    00142     while(tapCnt > 0u)
    00143     {
    00144       /* Perform the multiply-accumulate */
    00145       acc += ((q63_t) (*px++)) * (*pb++);
    00146
    00147       /* Decrement the loop counter */
    00148       tapCnt--;
    00149     }
    00150
    00151     /* Converting the result to 1.31 format */
    00152     /* Store the result from accumulator into the destination buffer. */
    00153     acc = (q31_t) (acc >> shift);
    00154
    00155     *pOut++ = (q31_t) acc;
    00156
    00157     /* Compute and store error */
    00158     e = *pRef++ - (q31_t) acc;
    00159
    00160     *pErr++ = (q31_t) e;
    00161
    00162     /* Compute alpha i.e. intermediate constant for taps update */
    00163     alpha = (q31_t) (((q63_t) e * mu) >> 31);
    00164
    00165     /* Initialize state pointer */
    00166     /* Advance state pointer by 1 for the next sample */
    00167     px = pState++;
    00168
    00169     /* Initialize coefficient pointer */
    00170     pb = pCoeffs;
    00171
    00172     /* Loop unrolling.  Process 4 taps at a time. */
    00173     tapCnt = numTaps >> 2;
    00174
    00175     /* Update filter coefficients */
    00176     while(tapCnt > 0u)
    00177     {
    00178       /* coef is in 2.30 format */
    00179       coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
    00180       /* get coef in 1.31 format by left shifting */
    00181       *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1u));
    00182       /* update coefficient buffer to next coefficient */
    00183       pb++;
    00184
    00185       coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
    00186       *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1u));
    00187       pb++;
    00188
    00189       coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
    00190       *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1u));
    00191       pb++;
    00192
    00193       coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
    00194       *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1u));
    00195       pb++;
    00196
    00197       /* Decrement the loop counter */
    00198       tapCnt--;
    00199     }
    00200
    00201     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
    00202     tapCnt = numTaps % 0x4u;
    00203
    00204     while(tapCnt > 0u)
    00205     {
    00206       /* Perform the multiply-accumulate */
    00207       coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
    00208       *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1u));
    00209       pb++;
    00210
    00211       /* Decrement the loop counter */
    00212       tapCnt--;
    00213     }
    00214
    00215     /* Decrement the loop counter */
    00216     blkCnt--;
    00217   }
    00218
    00219   /* Processing is complete. Now copy the last numTaps - 1 samples to the
    00220      satrt of the state buffer. This prepares the state buffer for the
    00221      next function call. */
    00222
    00223   /* Points to the start of the pState buffer */
    00224   pStateCurnt = S->pState;
    00225
    00226   /* Loop unrolling for (numTaps - 1u) samples copy */
    00227   tapCnt = (numTaps - 1u) >> 2u;
    00228
    00229   /* copy data */
    00230   while(tapCnt > 0u)
    00231   {
    00232     *pStateCurnt++ = *pState++;
    00233     *pStateCurnt++ = *pState++;
    00234     *pStateCurnt++ = *pState++;
    00235     *pStateCurnt++ = *pState++;
    00236
    00237     /* Decrement the loop counter */
    00238     tapCnt--;
    00239   }
    00240
    00241   /* Calculate remaining number of copies */
    00242   tapCnt = (numTaps - 1u) % 0x4u;
    00243
    00244   /* Copy the remaining q31_t data */
    00245   while(tapCnt > 0u)
    00246   {
    00247     *pStateCurnt++ = *pState++;
    00248
    00249     /* Decrement the loop counter */
    00250     tapCnt--;
    00251   }
    00252
    00253 #else
    00254
    00255   /* Run the below code for Cortex-M0 */
    00256
    00257   while(blkCnt > 0u)
    00258   {
    00259     /* Copy the new input sample into the state buffer */
    00260     *pStateCurnt++ = *pSrc++;
    00261
    00262     /* Initialize pState pointer */
    00263     px = pState;
    00264
    00265     /* Initialize pCoeffs pointer */
    00266     pb = pCoeffs;
    00267
    00268     /* Set the accumulator to zero */
    00269     acc = 0;
    00270
    00271     /* Loop over numTaps number of values */
    00272     tapCnt = numTaps;
    00273
    00274     while(tapCnt > 0u)
    00275     {
    00276       /* Perform the multiply-accumulate */
    00277       acc += ((q63_t) (*px++)) * (*pb++);
    00278
    00279       /* Decrement the loop counter */
    00280       tapCnt--;
    00281     }
    00282
    00283     /* Converting the result to 1.31 format */
    00284     /* Store the result from accumulator into the destination buffer. */
    00285     acc = (q31_t) (acc >> shift);
    00286
    00287     *pOut++ = (q31_t) acc;
    00288
    00289     /* Compute and store error */
    00290     e = *pRef++ - (q31_t) acc;
    00291
    00292     *pErr++ = (q31_t) e;
    00293
    00294     /* Weighting factor for the LMS version */
    00295     alpha = (q31_t) (((q63_t) e * mu) >> 31);
    00296
    00297     /* Initialize pState pointer */
    00298     /* Advance state pointer by 1 for the next sample */
    00299     px = pState++;
    00300
    00301     /* Initialize pCoeffs pointer */
    00302     pb = pCoeffs;
    00303
    00304     /* Loop over numTaps number of values */
    00305     tapCnt = numTaps;
    00306
    00307     while(tapCnt > 0u)
    00308     {
    00309       /* Perform the multiply-accumulate */
    00310       coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
    00311       *pb += (coef << 1u);
    00312       pb++;
    00313
    00314       /* Decrement the loop counter */
    00315       tapCnt--;
    00316     }
    00317
    00318     /* Decrement the loop counter */
    00319     blkCnt--;
    00320   }
    00321
    00322   /* Processing is complete. Now copy the last numTaps - 1 samples to the
    00323      start of the state buffer. This prepares the state buffer for the
    00324      next function call. */
    00325
    00326   /* Points to the start of the pState buffer */
    00327   pStateCurnt = S->pState;
    00328
    00329   /*  Copy (numTaps - 1u) samples  */
    00330   tapCnt = (numTaps - 1u);
    00331
    00332   /* Copy the data */
    00333   while(tapCnt > 0u)
    00334   {
    00335     *pStateCurnt++ = *pState++;
    00336
    00337     /* Decrement the loop counter */
    00338     tapCnt--;
    00339   }
    00340
    00341 #endif /*   #ifndef ARM_MATH_CM0 */
    00342
    00343 }
    00344

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines