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
