CMSIS DSP Software Library: arm_lms_norm_q31.c Source File

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

- File List
- Globals

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

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines