arm_fir_decimate_fast_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_fast_q31.c 00009 * 00010 * Description: Fast 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 00059 void arm_fir_decimate_fast_q31( 00060 arm_fir_decimate_instance_q31 * S, 00061 q31_t * pSrc, 00062 q31_t * pDst, 00063 uint32_t blockSize) 00064 { 00065 q31_t *pState = S->pState; /* State pointer */ 00066 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00067 q31_t *pStateCurnt; /* Points to the current sample of the state */ 00068 q31_t x0, c0; /* Temporary variables to hold state and coefficient values */ 00069 q31_t *px; /* Temporary pointers for state buffer */ 00070 q31_t *pb; /* Temporary pointers for coefficient buffer */ 00071 q31_t sum0; /* Accumulator */ 00072 uint32_t numTaps = S->numTaps; /* Number of taps */ 00073 uint32_t i, tapCnt, blkCnt, outBlockSize = blockSize / S->M; /* Loop counters */ 00074 uint32_t blkCntN2; 00075 q31_t x1; 00076 q31_t acc0, acc1; 00077 q31_t *px0, *px1; 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 00085 blkCnt = outBlockSize / 2; 00086 blkCntN2 = outBlockSize - (2*blkCnt); 00087 00088 while(blkCnt > 0u) 00089 { 00090 /* Copy decimation factor number of new input samples into the state buffer */ 00091 i = 2 * S->M; 00092 00093 do 00094 { 00095 *pStateCurnt++ = *pSrc++; 00096 00097 } while(--i); 00098 00099 /* Set accumulator to zero */ 00100 acc0 = 0; 00101 acc1 = 0; 00102 00103 /* Initialize state pointer */ 00104 px0 = pState; 00105 px1 = pState + S->M; 00106 00107 /* Initialize coeff pointer */ 00108 pb = pCoeffs; 00109 00110 /* Loop unrolling. Process 4 taps at a time. */ 00111 tapCnt = numTaps >> 2; 00112 00113 /* Loop over the number of taps. Unroll by a factor of 4. 00114 ** Repeat until we've computed numTaps-4 coefficients. */ 00115 while(tapCnt > 0u) 00116 { 00117 /* Read the b[numTaps-1] coefficient */ 00118 c0 = *(pb); 00119 00120 /* Read x[n-numTaps-1] for sample 0 sample 1 */ 00121 x0 = *(px0); 00122 x1 = *(px1); 00123 00124 /* Perform the multiply-accumulate */ 00125 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32); 00126 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32); 00127 00128 /* Read the b[numTaps-2] coefficient */ 00129 c0 = *(pb + 1u); 00130 00131 /* Read x[n-numTaps-2] for sample 0 sample 1 */ 00132 x0 = *(px0 + 1u); 00133 x1 = *(px1 + 1u); 00134 00135 /* Perform the multiply-accumulate */ 00136 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32); 00137 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32); 00138 00139 /* Read the b[numTaps-3] coefficient */ 00140 c0 = *(pb + 2u); 00141 00142 /* Read x[n-numTaps-3] for sample 0 sample 1 */ 00143 x0 = *(px0 + 2u); 00144 x1 = *(px1 + 2u); 00145 pb += 4u; 00146 00147 /* Perform the multiply-accumulate */ 00148 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32); 00149 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32); 00150 00151 /* Read the b[numTaps-4] coefficient */ 00152 c0 = *(pb - 1u); 00153 00154 /* Read x[n-numTaps-4] for sample 0 sample 1 */ 00155 x0 = *(px0 + 3u); 00156 x1 = *(px1 + 3u); 00157 00158 00159 /* Perform the multiply-accumulate */ 00160 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32); 00161 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32); 00162 00163 /* update state pointers */ 00164 px0 += 4u; 00165 px1 += 4u; 00166 00167 /* Decrement the loop counter */ 00168 tapCnt--; 00169 } 00170 00171 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00172 tapCnt = numTaps % 0x4u; 00173 00174 while(tapCnt > 0u) 00175 { 00176 /* Read coefficients */ 00177 c0 = *(pb++); 00178 00179 /* Fetch 1 state variable */ 00180 x0 = *(px0++); 00181 x1 = *(px1++); 00182 00183 /* Perform the multiply-accumulate */ 00184 acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32); 00185 acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32); 00186 00187 /* Decrement the loop counter */ 00188 tapCnt--; 00189 } 00190 00191 /* Advance the state pointer by the decimation factor 00192 * to process the next group of decimation factor number samples */ 00193 pState = pState + S->M * 2; 00194 00195 /* The result is in the accumulator, store in the destination buffer. */ 00196 *pDst++ = (q31_t) (acc0 << 1); 00197 *pDst++ = (q31_t) (acc1 << 1); 00198 00199 /* Decrement the loop counter */ 00200 blkCnt--; 00201 } 00202 00203 while(blkCntN2 > 0u) 00204 { 00205 /* Copy decimation factor number of new input samples into the state buffer */ 00206 i = S->M; 00207 00208 do 00209 { 00210 *pStateCurnt++ = *pSrc++; 00211 00212 } while(--i); 00213 00214 /* Set accumulator to zero */ 00215 sum0 = 0; 00216 00217 /* Initialize state pointer */ 00218 px = pState; 00219 00220 /* Initialize coeff pointer */ 00221 pb = pCoeffs; 00222 00223 /* Loop unrolling. Process 4 taps at a time. */ 00224 tapCnt = numTaps >> 2; 00225 00226 /* Loop over the number of taps. Unroll by a factor of 4. 00227 ** Repeat until we've computed numTaps-4 coefficients. */ 00228 while(tapCnt > 0u) 00229 { 00230 /* Read the b[numTaps-1] coefficient */ 00231 c0 = *(pb++); 00232 00233 /* Read x[n-numTaps-1] sample */ 00234 x0 = *(px++); 00235 00236 /* Perform the multiply-accumulate */ 00237 sum0 = (q31_t) ((((q63_t) sum0 << 32) + ((q63_t) x0 * c0)) >> 32); 00238 00239 /* Read the b[numTaps-2] coefficient */ 00240 c0 = *(pb++); 00241 00242 /* Read x[n-numTaps-2] sample */ 00243 x0 = *(px++); 00244 00245 /* Perform the multiply-accumulate */ 00246 sum0 = (q31_t) ((((q63_t) sum0 << 32) + ((q63_t) x0 * c0)) >> 32); 00247 00248 /* Read the b[numTaps-3] coefficient */ 00249 c0 = *(pb++); 00250 00251 /* Read x[n-numTaps-3] sample */ 00252 x0 = *(px++); 00253 00254 /* Perform the multiply-accumulate */ 00255 sum0 = (q31_t) ((((q63_t) sum0 << 32) + ((q63_t) x0 * c0)) >> 32); 00256 00257 /* Read the b[numTaps-4] coefficient */ 00258 c0 = *(pb++); 00259 00260 /* Read x[n-numTaps-4] sample */ 00261 x0 = *(px++); 00262 00263 /* Perform the multiply-accumulate */ 00264 sum0 = (q31_t) ((((q63_t) sum0 << 32) + ((q63_t) x0 * c0)) >> 32); 00265 00266 /* Decrement the loop counter */ 00267 tapCnt--; 00268 } 00269 00270 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00271 tapCnt = numTaps % 0x4u; 00272 00273 while(tapCnt > 0u) 00274 { 00275 /* Read coefficients */ 00276 c0 = *(pb++); 00277 00278 /* Fetch 1 state variable */ 00279 x0 = *(px++); 00280 00281 /* Perform the multiply-accumulate */ 00282 sum0 = (q31_t) ((((q63_t) sum0 << 32) + ((q63_t) x0 * c0)) >> 32); 00283 00284 /* Decrement the loop counter */ 00285 tapCnt--; 00286 } 00287 00288 /* Advance the state pointer by the decimation factor 00289 * to process the next group of decimation factor number samples */ 00290 pState = pState + S->M; 00291 00292 /* The result is in the accumulator, store in the destination buffer. */ 00293 *pDst++ = (q31_t) (sum0 << 1); 00294 00295 /* Decrement the loop counter */ 00296 blkCntN2--; 00297 } 00298 00299 /* Processing is complete. 00300 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer. 00301 ** This prepares the state buffer for the next function call. */ 00302 00303 /* Points to the start of the state buffer */ 00304 pStateCurnt = S->pState; 00305 00306 i = (numTaps - 1u) >> 2u; 00307 00308 /* copy data */ 00309 while(i > 0u) 00310 { 00311 *pStateCurnt++ = *pState++; 00312 *pStateCurnt++ = *pState++; 00313 *pStateCurnt++ = *pState++; 00314 *pStateCurnt++ = *pState++; 00315 00316 /* Decrement the loop counter */ 00317 i--; 00318 } 00319 00320 i = (numTaps - 1u) % 0x04u; 00321 00322 /* copy data */ 00323 while(i > 0u) 00324 { 00325 *pStateCurnt++ = *pState++; 00326 00327 /* Decrement the loop counter */ 00328 i--; 00329 } 00330 } 00331
