CMSIS DSP Software Library: arm_lms_q15.c Source File

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

- File List
- Globals

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

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines