arm_fir_interpolate_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_interpolate_q31.c 00009 * 00010 * Description: Q31 FIR interpolation. 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 00054 void arm_fir_interpolate_q31( 00055 const arm_fir_interpolate_instance_q31 * S, 00056 q31_t * pSrc, 00057 q31_t * pDst, 00058 uint32_t blockSize) 00059 { 00060 q31_t *pState = S->pState; /* State pointer */ 00061 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00062 q31_t *pStateCurnt; /* Points to the current sample of the state */ 00063 q31_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */ 00064 q63_t sum0; /* Accumulators */ 00065 q31_t x0, c0; /* Temporary variables to hold state and coefficient values */ 00066 uint32_t i, blkCnt, j; /* Loop counters */ 00067 uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */ 00068 00069 uint32_t blkCntN2; 00070 q63_t acc0, acc1; 00071 q31_t x1; 00072 00073 /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 00074 /* pStateCurnt points to the location where the new input data should be written */ 00075 pStateCurnt = S->pState + ((q31_t) phaseLen - 1); 00076 00077 /* Initialise blkCnt */ 00078 blkCnt = blockSize / 2; 00079 blkCntN2 = blockSize - (2*blkCnt); 00080 00081 /* Samples loop unrolled by 2 */ 00082 while(blkCnt > 0u) 00083 { 00084 /* Copy new input sample into the state buffer */ 00085 *pStateCurnt++ = *pSrc++; 00086 *pStateCurnt++ = *pSrc++; 00087 00088 /* Address modifier index of coefficient buffer */ 00089 j = 1u; 00090 00091 /* Loop over the Interpolation factor. */ 00092 i = (S->L); 00093 00094 while(i > 0u) 00095 { 00096 /* Set accumulator to zero */ 00097 acc0 = 0; 00098 acc1 = 0; 00099 00100 /* Initialize state pointer */ 00101 ptr1 = pState; 00102 00103 /* Initialize coefficient pointer */ 00104 ptr2 = pCoeffs + (S->L - j); 00105 00106 /* Loop over the polyPhase length. Unroll by a factor of 4. 00107 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 00108 tapCnt = phaseLen >> 2u; 00109 00110 x0 = *(ptr1++); 00111 00112 while(tapCnt > 0u) 00113 { 00114 00115 /* Read the input sample */ 00116 x1 = *(ptr1++); 00117 00118 /* Read the coefficient */ 00119 c0 = *(ptr2); 00120 00121 /* Perform the multiply-accumulate */ 00122 acc0 += (q63_t)x0 * c0; 00123 acc1 += (q63_t)x1 * c0; 00124 00125 00126 /* Read the coefficient */ 00127 c0 = *(ptr2 + S->L); 00128 00129 /* Read the input sample */ 00130 x0 = *(ptr1++); 00131 00132 /* Perform the multiply-accumulate */ 00133 acc0 += (q63_t)x1 * c0; 00134 acc1 += (q63_t)x0 * c0; 00135 00136 00137 /* Read the coefficient */ 00138 c0 = *(ptr2 + S->L * 2); 00139 00140 /* Read the input sample */ 00141 x1 = *(ptr1++); 00142 00143 /* Perform the multiply-accumulate */ 00144 acc0 += (q63_t)x0 * c0; 00145 acc1 += (q63_t)x1 * c0; 00146 00147 /* Read the coefficient */ 00148 c0 = *(ptr2 + S->L * 3); 00149 00150 /* Read the input sample */ 00151 x0 = *(ptr1++); 00152 00153 /* Perform the multiply-accumulate */ 00154 acc0 += (q63_t)x1 * c0; 00155 acc1 += (q63_t)x0 * c0; 00156 00157 00158 /* Upsampling is done by stuffing L-1 zeros between each sample. 00159 * So instead of multiplying zeros with coefficients, 00160 * Increment the coefficient pointer by interpolation factor times. */ 00161 ptr2 += 4 * S->L; 00162 00163 /* Decrement the loop counter */ 00164 tapCnt--; 00165 } 00166 00167 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 00168 tapCnt = phaseLen % 0x4u; 00169 00170 while(tapCnt > 0u) 00171 { 00172 00173 /* Read the input sample */ 00174 x1 = *(ptr1++); 00175 00176 /* Read the coefficient */ 00177 c0 = *(ptr2); 00178 00179 /* Perform the multiply-accumulate */ 00180 acc0 += (q63_t)x0 * c0; 00181 acc1 += (q63_t)x1 * c0; 00182 00183 /* Increment the coefficient pointer by interpolation factor times. */ 00184 ptr2 += S->L; 00185 00186 /* update states for next sample processing */ 00187 x0 = x1; 00188 00189 /* Decrement the loop counter */ 00190 tapCnt--; 00191 } 00192 00193 /* The result is in the accumulator, store in the destination buffer. */ 00194 *pDst = (q31_t) (acc0 >> 31); 00195 *(pDst + S->L) = (q31_t) (acc1 >> 31); 00196 00197 00198 pDst++; 00199 00200 /* Increment the address modifier index of coefficient buffer */ 00201 j++; 00202 00203 /* Decrement the loop counter */ 00204 i--; 00205 } 00206 00207 /* Advance the state pointer by 1 00208 * to process the next group of interpolation factor number samples */ 00209 pState = pState + 2; 00210 00211 pDst += S->L; 00212 00213 /* Decrement the loop counter */ 00214 blkCnt--; 00215 } 00216 00217 /* If the blockSize is not a multiple of 2, compute any remaining output samples here. 00218 ** No loop unrolling is used. */ 00219 blkCnt = blkCntN2; 00220 00221 /* Loop over the blockSize. */ 00222 while(blkCnt > 0u) 00223 { 00224 /* Copy new input sample into the state buffer */ 00225 *pStateCurnt++ = *pSrc++; 00226 00227 /* Address modifier index of coefficient buffer */ 00228 j = 1u; 00229 00230 /* Loop over the Interpolation factor. */ 00231 i = S->L; 00232 while(i > 0u) 00233 { 00234 /* Set accumulator to zero */ 00235 sum0 = 0; 00236 00237 /* Initialize state pointer */ 00238 ptr1 = pState; 00239 00240 /* Initialize coefficient pointer */ 00241 ptr2 = pCoeffs + (S->L - j); 00242 00243 /* Loop over the polyPhase length. Unroll by a factor of 4. 00244 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 00245 tapCnt = phaseLen >> 2; 00246 while(tapCnt > 0u) 00247 { 00248 00249 /* Read the coefficient */ 00250 c0 = *(ptr2); 00251 00252 /* Upsampling is done by stuffing L-1 zeros between each sample. 00253 * So instead of multiplying zeros with coefficients, 00254 * Increment the coefficient pointer by interpolation factor times. */ 00255 ptr2 += S->L; 00256 00257 /* Read the input sample */ 00258 x0 = *(ptr1++); 00259 00260 /* Perform the multiply-accumulate */ 00261 sum0 += (q63_t) x0 *c0; 00262 00263 /* Read the coefficient */ 00264 c0 = *(ptr2); 00265 00266 /* Increment the coefficient pointer by interpolation factor times. */ 00267 ptr2 += S->L; 00268 00269 /* Read the input sample */ 00270 x0 = *(ptr1++); 00271 00272 /* Perform the multiply-accumulate */ 00273 sum0 += (q63_t) x0 *c0; 00274 00275 /* Read the coefficient */ 00276 c0 = *(ptr2); 00277 00278 /* Increment the coefficient pointer by interpolation factor times. */ 00279 ptr2 += S->L; 00280 00281 /* Read the input sample */ 00282 x0 = *(ptr1++); 00283 00284 /* Perform the multiply-accumulate */ 00285 sum0 += (q63_t) x0 *c0; 00286 00287 /* Read the coefficient */ 00288 c0 = *(ptr2); 00289 00290 /* Increment the coefficient pointer by interpolation factor times. */ 00291 ptr2 += S->L; 00292 00293 /* Read the input sample */ 00294 x0 = *(ptr1++); 00295 00296 /* Perform the multiply-accumulate */ 00297 sum0 += (q63_t) x0 *c0; 00298 00299 /* Decrement the loop counter */ 00300 tapCnt--; 00301 } 00302 00303 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 00304 tapCnt = phaseLen & 0x3u; 00305 00306 while(tapCnt > 0u) 00307 { 00308 /* Read the coefficient */ 00309 c0 = *(ptr2); 00310 00311 /* Increment the coefficient pointer by interpolation factor times. */ 00312 ptr2 += S->L; 00313 00314 /* Read the input sample */ 00315 x0 = *(ptr1++); 00316 00317 /* Perform the multiply-accumulate */ 00318 sum0 += (q63_t) x0 *c0; 00319 00320 /* Decrement the loop counter */ 00321 tapCnt--; 00322 } 00323 00324 /* The result is in the accumulator, store in the destination buffer. */ 00325 *pDst++ = (q31_t) (sum0 >> 31); 00326 00327 /* Increment the address modifier index of coefficient buffer */ 00328 j++; 00329 00330 /* Decrement the loop counter */ 00331 i--; 00332 } 00333 00334 /* Advance the state pointer by 1 00335 * to process the next group of interpolation factor number samples */ 00336 pState = pState + 1; 00337 00338 /* Decrement the loop counter */ 00339 blkCnt--; 00340 } 00341 00342 /* Processing is complete. 00343 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer. 00344 ** This prepares the state buffer for the next function call. */ 00345 00346 /* Points to the start of the state buffer */ 00347 pStateCurnt = S->pState; 00348 00349 tapCnt = (phaseLen - 1u) >> 2u; 00350 00351 /* copy data */ 00352 while(tapCnt > 0u) 00353 { 00354 *pStateCurnt++ = *pState++; 00355 *pStateCurnt++ = *pState++; 00356 *pStateCurnt++ = *pState++; 00357 *pStateCurnt++ = *pState++; 00358 00359 /* Decrement the loop counter */ 00360 tapCnt--; 00361 } 00362 00363 tapCnt = (phaseLen - 1u) % 0x04u; 00364 00365 /* copy data */ 00366 while(tapCnt > 0u) 00367 { 00368 *pStateCurnt++ = *pState++; 00369 00370 /* Decrement the loop counter */ 00371 tapCnt--; 00372 } 00373 00374 } 00375 00376
