arm_fir_decimate_q31.c
Go to the documentation of this file.
00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2011 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 15. December 2011 00005 * $Revision: V2.0.0 00006 * 00007 * Project: Cortex-R DSP Library 00008 * Title: arm_fir_decimate_q31.c 00009 * 00010 * Description: Q31 FIR Decimator. 00011 * 00012 * Target Processor: Cortex-R4/R5 00013 * 00014 * Version 1.0.0 2011/03/08 00015 * Alpha release. 00016 * 00017 * Version 1.0.1 2011/09/30 00018 * Beta release. 00019 * 00020 * Version 2.0.0 2011/12/15 00021 * Final release. 00022 * 00023 * -------------------------------------------------------------------- */ 00024 #include "arm_math.h" 00025 00057 void arm_fir_decimate_q31( 00058 const arm_fir_decimate_instance_q31 * S, 00059 q31_t * pSrc, 00060 q31_t * pDst, 00061 uint32_t blockSize) 00062 { 00063 q31_t *pState = S->pState; /* State pointer */ 00064 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00065 q31_t *pStateCurnt; /* Points to the current sample of the state */ 00066 q31_t x0, x1, c0, c1; /* Temporary variables to hold state and coefficient values */ 00067 q31_t *px; /* Temporary pointers for state buffer */ 00068 q31_t *pb; /* Temporary pointers for coefficient buffer */ 00069 q63_t sum0; /* Accumulator */ 00070 00071 uint32_t i, tapCnt, blkCnt; /* Loop counters */ 00072 q63_t acc0, acc1; 00073 q31_t *px0, *px1; 00074 uint32_t blkCntN3; 00075 uint32_t numTaps = S->numTaps; /* Number of taps */ 00076 00077 uint32_t outBlockSize = blockSize / S->M; 00078 00079 /* S->pState buffer contains previous frame (numTaps - 1) samples */ 00080 /* pStateCurnt points to the location where the new input data should be written */ 00081 pStateCurnt = S->pState + (numTaps - 1u); 00082 00083 /* Total number of output samples to be computed */ 00084 blkCnt = outBlockSize / 2; 00085 blkCntN3 = outBlockSize - (2*blkCnt); 00086 00087 while(blkCnt > 0u) 00088 { 00089 /* Copy decimation factor number of new input samples into the state buffer */ 00090 i = 2 * S->M; 00091 00092 do 00093 { 00094 *pStateCurnt++ = *pSrc++; 00095 00096 } while(--i); 00097 00098 /* Set accumulator to zero */ 00099 acc0 = 0; 00100 acc1 = 0; 00101 00102 /* Initialize state pointer */ 00103 px0 = pState; 00104 px1 = pState + S->M; 00105 00106 /* Initialize coeff pointer */ 00107 pb = pCoeffs; 00108 00109 /* Loop unrolling. Process 4 taps at a time. */ 00110 tapCnt = numTaps >> 2; 00111 00112 /* Loop over the number of taps. Unroll by a factor of 4. 00113 ** Repeat until we've computed numTaps-4 coefficients. */ 00114 while(tapCnt > 0u) 00115 { 00116 /* Read the b[numTaps-1] coefficient */ 00117 c0 = *(pb); 00118 00119 /* Read x[n-numTaps-1] for sample 0 sample 1 */ 00120 x0 = *(px0); 00121 x1 = *(px1); 00122 00123 00124 /* Perform the multiply-accumulate */ 00125 acc0 += (q63_t) x0 *c0; 00126 acc1 += (q63_t) x1 *c0; 00127 00128 00129 /* Read the b[numTaps-2] coefficient */ 00130 c0 = *(pb + 1u); 00131 00132 /* Read x[n-numTaps-2] for sample 0 sample 1 */ 00133 x0 = *(px0 + 1u); 00134 x1 = *(px1 + 1u); 00135 00136 00137 /* Perform the multiply-accumulate */ 00138 acc0 += (q63_t) x0 *c0; 00139 acc1 += (q63_t) x1 *c0; 00140 00141 00142 /* Read the b[numTaps-3] coefficient */ 00143 c0 = *(pb + 2u); 00144 00145 /* Read x[n-numTaps-3] for sample 0 sample 1 */ 00146 x0 = *(px0 + 2u); 00147 x1 = *(px1 + 2u); 00148 pb += 4u; 00149 00150 00151 /* Perform the multiply-accumulate */ 00152 acc0 += (q63_t) x0 *c0; 00153 acc1 += (q63_t) x1 *c0; 00154 00155 00156 /* Read the b[numTaps-4] coefficient */ 00157 c0 = *(pb - 1u); 00158 00159 /* Read x[n-numTaps-4] for sample 0 sample 1 */ 00160 x0 = *(px0 + 3u); 00161 x1 = *(px1 + 3u); 00162 00163 00164 /* Perform the multiply-accumulate */ 00165 acc0 += (q63_t) x0 *c0; 00166 acc1 += (q63_t) x1 *c0; 00167 00168 /* update state pointers */ 00169 px0 += 4u; 00170 px1 += 4u; 00171 00172 /* Decrement the loop counter */ 00173 tapCnt--; 00174 } 00175 00176 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00177 tapCnt = numTaps % 0x4u; 00178 00179 while(tapCnt > 0u) 00180 { 00181 /* Read coefficients */ 00182 c0 = *(pb++); 00183 00184 /* Fetch 1 state variable */ 00185 x0 = *(px0++); 00186 x1 = *(px1++); 00187 00188 /* Perform the multiply-accumulate */ 00189 acc0 += (q63_t) x0 *c0; 00190 acc1 += (q63_t) x1 *c0; 00191 00192 00193 /* Decrement the loop counter */ 00194 tapCnt--; 00195 } 00196 00197 /* Advance the state pointer by the decimation factor 00198 * to process the next group of decimation factor number samples */ 00199 pState = pState + S->M * 2; 00200 00201 /* The result is in the accumulator, store in the destination buffer. */ 00202 *pDst++ = (q31_t) (acc0 >> 31); 00203 *pDst++ = (q31_t) (acc1 >> 31); 00204 00205 /* Decrement the loop counter */ 00206 blkCnt--; 00207 } 00208 00209 00210 while(blkCntN3 > 0u) 00211 { 00212 /* Copy decimation factor number of new input samples into the state buffer */ 00213 i = S->M; 00214 00215 do 00216 { 00217 *pStateCurnt++ = *pSrc++; 00218 00219 } while(--i); 00220 00221 /* Set accumulator to zero */ 00222 sum0 = 0; 00223 00224 /* Initialize state pointer */ 00225 px = pState; 00226 00227 /* Initialize coeff pointer */ 00228 pb = pCoeffs; 00229 00230 /* Loop unrolling. Process 4 taps at a time. */ 00231 tapCnt = numTaps >> 2; 00232 00233 /* Loop over the number of taps. Unroll by a factor of 4. 00234 ** Repeat until we've computed numTaps-4 coefficients. */ 00235 while(tapCnt > 0u) 00236 { 00237 /* Read the b[numTaps-1] coefficient */ 00238 c0 = *(pb); 00239 00240 /* Read x[n-numTaps-1] sample */ 00241 x0 = *(px); 00242 00243 /* Read the b[numTaps-2] coefficient */ 00244 c1 = *(pb + 1u); 00245 00246 /* Perform the multiply-accumulate */ 00247 sum0 += (q63_t) x0 *c0; 00248 00249 /* Read x[n-numTaps-2] sample */ 00250 x0 = *(px + 1u); 00251 00252 /* Perform the multiply-accumulate */ 00253 sum0 += (q63_t) x0 *c1; 00254 00255 /* Read the b[numTaps-3] coefficient */ 00256 c0 = *(pb + 2u); 00257 00258 /* Read x[n-numTaps-3] sample */ 00259 x0 = *(px + 2u); 00260 00261 /* Read the b[numTaps-4] coefficient */ 00262 c1 = *(pb + 3u); 00263 00264 /* Perform the multiply-accumulate */ 00265 sum0 += (q63_t) x0 *c0; 00266 00267 /* Read x[n-numTaps-4] sample */ 00268 x0 = *(px + 3u); 00269 00270 /* Perform the multiply-accumulate */ 00271 sum0 += (q63_t) x0 *c1; 00272 00273 /* update state pointer and coefficient pointer */ 00274 pb += 4u; 00275 px += 4u; 00276 00277 /* Decrement the loop counter */ 00278 tapCnt--; 00279 } 00280 00281 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00282 tapCnt = numTaps % 0x4u; 00283 00284 while(tapCnt > 0u) 00285 { 00286 /* Read coefficients */ 00287 c0 = *(pb++); 00288 00289 /* Fetch 1 state variable */ 00290 x0 = *(px++); 00291 00292 /* Perform the multiply-accumulate */ 00293 sum0 += (q63_t) x0 *c0; 00294 00295 /* Decrement the loop counter */ 00296 tapCnt--; 00297 } 00298 00299 /* Advance the state pointer by the decimation factor 00300 * to process the next group of decimation factor number samples */ 00301 pState = pState + S->M; 00302 00303 /* The result is in the accumulator, store in the destination buffer. */ 00304 *pDst++ = (q31_t) (sum0 >> 31); 00305 00306 /* Decrement the loop counter */ 00307 blkCntN3--; 00308 } 00309 00310 /* Processing is complete. 00311 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer. 00312 ** This prepares the state buffer for the next function call. */ 00313 00314 /* Points to the start of the state buffer */ 00315 pStateCurnt = S->pState; 00316 00317 i = (numTaps - 1u) >> 2u; 00318 00319 /* copy data */ 00320 while(i > 0u) 00321 { 00322 *pStateCurnt++ = *pState++; 00323 *pStateCurnt++ = *pState++; 00324 *pStateCurnt++ = *pState++; 00325 *pStateCurnt++ = *pState++; 00326 00327 /* Decrement the loop counter */ 00328 i--; 00329 } 00330 00331 i = (numTaps - 1u) % 0x04u; 00332 00333 /* copy data */ 00334 while(i > 0u) 00335 { 00336 *pStateCurnt++ = *pState++; 00337 00338 /* Decrement the loop counter */ 00339 i--; 00340 } 00341 } 00342
