CMSIS DSP Software Library: arm_lms_norm_q15.c Source File

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

- File List
- Globals

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

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines