arm_fir_interpolate_f32.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_f32.c 00009 * 00010 * Description: FIR interpolation for floating-point sequences. 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 00126 void arm_fir_interpolate_f32( 00127 const arm_fir_interpolate_instance_f32 * S, 00128 float32_t * pSrc, 00129 float32_t * pDst, 00130 uint32_t blockSize) 00131 { 00132 float32_t *pState = S->pState; /* State pointer */ 00133 float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00134 float32_t *pStateCurnt; /* Points to the current sample of the state */ 00135 float32_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */ 00136 float32_t sum0; /* Accumulators */ 00137 float32_t x0, c0; /* Temporary variables to hold state and coefficient values */ 00138 uint32_t i, blkCnt, j; /* Loop counters */ 00139 uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */ 00140 float32_t acc0, acc1, acc2, acc3; 00141 float32_t x1, x2, x3; 00142 uint32_t blkCntN4; 00143 float32_t c1, c2, c3; 00144 00145 /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 00146 /* pStateCurnt points to the location where the new input data should be written */ 00147 pStateCurnt = S->pState + (phaseLen - 1u); 00148 00149 /* Initialise blkCnt */ 00150 blkCnt = blockSize / 4; 00151 blkCntN4 = blockSize - (4*blkCnt); 00152 00153 /* Samples loop unrolled by 4 */ 00154 while(blkCnt > 0u) 00155 { 00156 /* Copy new input sample into the state buffer */ 00157 *pStateCurnt++ = *pSrc++; 00158 *pStateCurnt++ = *pSrc++; 00159 *pStateCurnt++ = *pSrc++; 00160 *pStateCurnt++ = *pSrc++; 00161 00162 /* Address modifier index of coefficient buffer */ 00163 j = 1u; 00164 00165 /* Loop over the Interpolation factor. */ 00166 i = (S->L); 00167 00168 while(i > 0u) 00169 { 00170 /* Set accumulator to zero */ 00171 acc0 = 0.0f; 00172 acc1 = 0.0f; 00173 acc2 = 0.0f; 00174 acc3 = 0.0f; 00175 00176 /* Initialize state pointer */ 00177 ptr1 = pState; 00178 00179 /* Initialize coefficient pointer */ 00180 ptr2 = pCoeffs + (S->L - j); 00181 00182 /* Loop over the polyPhase length. Unroll by a factor of 4. 00183 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 00184 tapCnt = phaseLen >> 2u; 00185 00186 x0 = *(ptr1++); 00187 x1 = *(ptr1++); 00188 x2 = *(ptr1++); 00189 00190 while(tapCnt > 0u) 00191 { 00192 00193 /* Read the input sample */ 00194 x3 = *(ptr1++); 00195 00196 /* Read the coefficient */ 00197 c0 = *(ptr2); 00198 00199 /* Perform the multiply-accumulate */ 00200 acc0 += x0 * c0; 00201 acc1 += x1 * c0; 00202 acc2 += x2 * c0; 00203 acc3 += x3 * c0; 00204 00205 /* Read the coefficient */ 00206 c1 = *(ptr2 + S->L); 00207 00208 /* Read the input sample */ 00209 x0 = *(ptr1++); 00210 00211 /* Perform the multiply-accumulate */ 00212 acc0 += x1 * c1; 00213 acc1 += x2 * c1; 00214 acc2 += x3 * c1; 00215 acc3 += x0 * c1; 00216 00217 /* Read the coefficient */ 00218 c2 = *(ptr2 + S->L * 2); 00219 00220 /* Read the input sample */ 00221 x1 = *(ptr1++); 00222 00223 /* Perform the multiply-accumulate */ 00224 acc0 += x2 * c2; 00225 acc1 += x3 * c2; 00226 acc2 += x0 * c2; 00227 acc3 += x1 * c2; 00228 00229 /* Read the coefficient */ 00230 c3 = *(ptr2 + S->L * 3); 00231 00232 /* Read the input sample */ 00233 x2 = *(ptr1++); 00234 00235 /* Perform the multiply-accumulate */ 00236 acc0 += x3 * c3; 00237 acc1 += x0 * c3; 00238 acc2 += x1 * c3; 00239 acc3 += x2 * c3; 00240 00241 00242 /* Upsampling is done by stuffing L-1 zeros between each sample. 00243 * So instead of multiplying zeros with coefficients, 00244 * Increment the coefficient pointer by interpolation factor times. */ 00245 ptr2 += 4 * S->L; 00246 00247 /* Decrement the loop counter */ 00248 tapCnt--; 00249 } 00250 00251 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 00252 tapCnt = phaseLen % 0x4u; 00253 00254 while(tapCnt > 0u) 00255 { 00256 00257 /* Read the input sample */ 00258 x3 = *(ptr1++); 00259 00260 /* Read the coefficient */ 00261 c0 = *(ptr2); 00262 00263 /* Perform the multiply-accumulate */ 00264 acc0 += x0 * c0; 00265 acc1 += x1 * c0; 00266 acc2 += x2 * c0; 00267 acc3 += x3 * c0; 00268 00269 /* Increment the coefficient pointer by interpolation factor times. */ 00270 ptr2 += S->L; 00271 00272 /* update states for next sample processing */ 00273 x0 = x1; 00274 x1 = x2; 00275 x2 = x3; 00276 00277 /* Decrement the loop counter */ 00278 tapCnt--; 00279 } 00280 00281 /* The result is in the accumulator, store in the destination buffer. */ 00282 *pDst = acc0; 00283 *(pDst + S->L) = acc1; 00284 *(pDst + 2 * S->L) = acc2; 00285 *(pDst + 3 * S->L) = acc3; 00286 00287 pDst ++; 00288 00289 /* Increment the address modifier index of coefficient buffer */ 00290 j++; 00291 00292 /* Decrement the loop counter */ 00293 i--; 00294 } 00295 00296 /* Advance the state pointer by 1 00297 * to process the next group of interpolation factor number samples */ 00298 pState = pState + 4; 00299 00300 pDst += S->L * 3; 00301 00302 /* Decrement the loop counter */ 00303 blkCnt--; 00304 } 00305 00306 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00307 ** No loop unrolling is used. */ 00308 00309 while(blkCntN4 > 0u) 00310 { 00311 /* Copy new input sample into the state buffer */ 00312 *pStateCurnt++ = *pSrc++; 00313 00314 /* Address modifier index of coefficient buffer */ 00315 j = 1u; 00316 00317 /* Loop over the Interpolation factor. */ 00318 i = S->L; 00319 while(i > 0u) 00320 { 00321 /* Set accumulator to zero */ 00322 sum0 = 0.0f; 00323 00324 /* Initialize state pointer */ 00325 ptr1 = pState; 00326 00327 /* Initialize coefficient pointer */ 00328 ptr2 = pCoeffs + (S->L - j); 00329 00330 /* Loop over the polyPhase length. Unroll by a factor of 4. 00331 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 00332 tapCnt = phaseLen >> 2u; 00333 while(tapCnt > 0u) 00334 { 00335 00336 /* Read the coefficient */ 00337 c0 = *(ptr2); 00338 00339 /* Upsampling is done by stuffing L-1 zeros between each sample. 00340 * So instead of multiplying zeros with coefficients, 00341 * Increment the coefficient pointer by interpolation factor times. */ 00342 ptr2 += S->L; 00343 00344 /* Read the input sample */ 00345 x0 = *(ptr1++); 00346 00347 /* Perform the multiply-accumulate */ 00348 sum0 += x0 * c0; 00349 00350 /* Read the coefficient */ 00351 c0 = *(ptr2); 00352 00353 /* Increment the coefficient pointer by interpolation factor times. */ 00354 ptr2 += S->L; 00355 00356 /* Read the input sample */ 00357 x0 = *(ptr1++); 00358 00359 /* Perform the multiply-accumulate */ 00360 sum0 += x0 * c0; 00361 00362 /* Read the coefficient */ 00363 c0 = *(ptr2); 00364 00365 /* Increment the coefficient pointer by interpolation factor times. */ 00366 ptr2 += S->L; 00367 00368 /* Read the input sample */ 00369 x0 = *(ptr1++); 00370 00371 /* Perform the multiply-accumulate */ 00372 sum0 += x0 * c0; 00373 00374 /* Read the coefficient */ 00375 c0 = *(ptr2); 00376 00377 /* Increment the coefficient pointer by interpolation factor times. */ 00378 ptr2 += S->L; 00379 00380 /* Read the input sample */ 00381 x0 = *(ptr1++); 00382 00383 /* Perform the multiply-accumulate */ 00384 sum0 += x0 * c0; 00385 00386 /* Decrement the loop counter */ 00387 tapCnt--; 00388 } 00389 00390 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 00391 tapCnt = phaseLen % 0x4u; 00392 00393 while(tapCnt > 0u) 00394 { 00395 /* Perform the multiply-accumulate */ 00396 sum0 += *(ptr1++) * (*ptr2); 00397 00398 /* Increment the coefficient pointer by interpolation factor times. */ 00399 ptr2 += S->L; 00400 00401 /* Decrement the loop counter */ 00402 tapCnt--; 00403 } 00404 00405 /* The result is in the accumulator, store in the destination buffer. */ 00406 *pDst++ = sum0; 00407 00408 /* Increment the address modifier index of coefficient buffer */ 00409 j++; 00410 00411 /* Decrement the loop counter */ 00412 i--; 00413 } 00414 00415 /* Advance the state pointer by 1 00416 * to process the next group of interpolation factor number samples */ 00417 pState = pState + 1; 00418 00419 /* Decrement the loop counter */ 00420 blkCntN4--; 00421 } 00422 00423 /* Processing is complete. 00424 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer. 00425 ** This prepares the state buffer for the next function call. */ 00426 00427 /* Points to the start of the state buffer */ 00428 pStateCurnt = S->pState; 00429 00430 tapCnt = (phaseLen - 1u) >> 2u; 00431 00432 /* copy data */ 00433 while(tapCnt > 0u) 00434 { 00435 *pStateCurnt++ = *pState++; 00436 *pStateCurnt++ = *pState++; 00437 *pStateCurnt++ = *pState++; 00438 *pStateCurnt++ = *pState++; 00439 00440 /* Decrement the loop counter */ 00441 tapCnt--; 00442 } 00443 00444 tapCnt = (phaseLen - 1u) % 0x04u; 00445 00446 /* copy data */ 00447 while(tapCnt > 0u) 00448 { 00449 *pStateCurnt++ = *pState++; 00450 00451 /* Decrement the loop counter */ 00452 tapCnt--; 00453 } 00454 } 00455
