arm_fir_decimate_fast_q15.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_q15.c 00009 * 00010 * Description: Fast Q15 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 00066 #ifndef UNALIGNED_SUPPORT_DISABLE 00067 00068 void arm_fir_decimate_fast_q15( 00069 const arm_fir_decimate_instance_q15 * S, 00070 q15_t * pSrc, 00071 q15_t * pDst, 00072 uint32_t blockSize) 00073 { 00074 q15_t *pState = S->pState; /* State pointer */ 00075 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00076 q15_t *pStateCurnt; /* Points to the current sample of the state */ 00077 q15_t *px; /* Temporary pointer for state buffer */ 00078 q15_t *pb; /* Temporary pointer coefficient buffer */ 00079 q31_t x0, x1, c0, c1; /* Temporary variables to hold state and coefficient values */ 00080 q63_t sum0; /* Accumulators */ 00081 q31_t acc0, acc1; 00082 q15_t *px0, *px1; 00083 uint32_t blkCntN3; 00084 uint32_t numTaps = S->numTaps; /* Number of taps */ 00085 uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */ 00086 00087 00088 /* S->pState buffer contains previous frame (numTaps - 1) samples */ 00089 /* pStateCurnt points to the location where the new input data should be written */ 00090 pStateCurnt = S->pState + (numTaps - 1u); 00091 00092 00093 /* Total number of output samples to be computed */ 00094 blkCnt = outBlockSize / 2; 00095 blkCntN3 = outBlockSize - (2*blkCnt); 00096 00097 00098 while(blkCnt > 0u) 00099 { 00100 /* Copy decimation factor number of new input samples into the state buffer */ 00101 i = 2 * S->M; 00102 00103 do 00104 { 00105 *pStateCurnt++ = *pSrc++; 00106 00107 } while(--i); 00108 00109 /* Set accumulator to zero */ 00110 acc0 = 0; 00111 acc1 = 0; 00112 00113 /* Initialize state pointer */ 00114 px0 = pState; 00115 00116 px1 = pState + S->M; 00117 00118 00119 /* Initialize coeff pointer */ 00120 pb = pCoeffs; 00121 00122 /* Loop unrolling. Process 4 taps at a time. */ 00123 tapCnt = numTaps >> 2; 00124 00125 /* Loop over the number of taps. Unroll by a factor of 4. 00126 ** Repeat until we've computed numTaps-4 coefficients. */ 00127 while(tapCnt > 0u) 00128 { 00129 /* Read the Read b[numTaps-1] and b[numTaps-2] coefficients */ 00130 c0 = *__SIMD32(pb)++; 00131 00132 /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */ 00133 x0 = *__SIMD32(px0)++; 00134 00135 x1 = *__SIMD32(px1)++; 00136 00137 /* Perform the multiply-accumulate */ 00138 acc0 = __SMLAD(x0, c0, acc0); 00139 00140 acc1 = __SMLAD(x1, c0, acc1); 00141 00142 /* Read the b[numTaps-3] and b[numTaps-4] coefficient */ 00143 c0 = *__SIMD32(pb)++; 00144 00145 /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */ 00146 x0 = *__SIMD32(px0)++; 00147 00148 x1 = *__SIMD32(px1)++; 00149 00150 /* Perform the multiply-accumulate */ 00151 acc0 = __SMLAD(x0, c0, acc0); 00152 00153 acc1 = __SMLAD(x1, c0, acc1); 00154 00155 /* Decrement the loop counter */ 00156 tapCnt--; 00157 } 00158 00159 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00160 tapCnt = numTaps % 0x4u; 00161 00162 while(tapCnt > 0u) 00163 { 00164 /* Read coefficients */ 00165 c0 = *pb++; 00166 00167 /* Fetch 1 state variable */ 00168 x0 = *px0++; 00169 00170 x1 = *px1++; 00171 00172 /* Perform the multiply-accumulate */ 00173 acc0 = __SMLAD(x0, c0, acc0); 00174 acc1 = __SMLAD(x1, c0, acc1); 00175 00176 /* Decrement the loop counter */ 00177 tapCnt--; 00178 } 00179 00180 /* Advance the state pointer by the decimation factor 00181 * to process the next group of decimation factor number samples */ 00182 pState = pState + S->M * 2; 00183 00184 /* Store filter output, smlad returns the values in 2.14 format */ 00185 /* so downsacle by 15 to get output in 1.15 */ 00186 #ifdef CCS 00187 *pDst++ = (q15_t) (__SSATA(acc0, 15, 16)); 00188 *pDst++ = (q15_t) (__SSATA(acc1, 15, 16)); 00189 #else 00190 *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16)); 00191 *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16)); 00192 #endif 00193 /* Decrement the loop counter */ 00194 blkCnt--; 00195 } 00196 00197 00198 00199 while(blkCntN3 > 0u) 00200 { 00201 /* Copy decimation factor number of new input samples into the state buffer */ 00202 i = S->M; 00203 00204 do 00205 { 00206 *pStateCurnt++ = *pSrc++; 00207 00208 } while(--i); 00209 00210 /*Set sum to zero */ 00211 sum0 = 0; 00212 00213 /* Initialize state pointer */ 00214 px = pState; 00215 00216 /* Initialize coeff pointer */ 00217 pb = pCoeffs; 00218 00219 /* Loop unrolling. Process 4 taps at a time. */ 00220 tapCnt = numTaps >> 2; 00221 00222 /* Loop over the number of taps. Unroll by a factor of 4. 00223 ** Repeat until we've computed numTaps-4 coefficients. */ 00224 while(tapCnt > 0u) 00225 { 00226 /* Read the Read b[numTaps-1] and b[numTaps-2] coefficients */ 00227 c0 = *__SIMD32(pb)++; 00228 00229 /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */ 00230 x0 = *__SIMD32(px)++; 00231 00232 /* Read the b[numTaps-3] and b[numTaps-4] coefficient */ 00233 c1 = *__SIMD32(pb)++; 00234 00235 /* Perform the multiply-accumulate */ 00236 sum0 = __SMLAD(x0, c0, sum0); 00237 00238 /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */ 00239 x0 = *__SIMD32(px)++; 00240 00241 /* Perform the multiply-accumulate */ 00242 sum0 = __SMLAD(x0, c1, sum0); 00243 00244 /* Decrement the loop counter */ 00245 tapCnt--; 00246 } 00247 00248 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00249 tapCnt = numTaps % 0x4u; 00250 00251 while(tapCnt > 0u) 00252 { 00253 /* Read coefficients */ 00254 c0 = *pb++; 00255 00256 /* Fetch 1 state variable */ 00257 x0 = *px++; 00258 00259 /* Perform the multiply-accumulate */ 00260 sum0 = __SMLAD(x0, c0, sum0); 00261 00262 /* Decrement the loop counter */ 00263 tapCnt--; 00264 } 00265 00266 /* Advance the state pointer by the decimation factor 00267 * to process the next group of decimation factor number samples */ 00268 pState = pState + S->M; 00269 00270 /* Store filter output, smlad returns the values in 2.14 format */ 00271 /* so downsacle by 15 to get output in 1.15 */ 00272 #ifdef CCS 00273 *pDst++ = (q15_t) (__SSATA(sum0, 15, 16)); 00274 #else 00275 *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16)); 00276 #endif 00277 /* Decrement the loop counter */ 00278 blkCntN3--; 00279 } 00280 00281 /* Processing is complete. 00282 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer. 00283 ** This prepares the state buffer for the next function call. */ 00284 00285 /* Points to the start of the state buffer */ 00286 pStateCurnt = S->pState; 00287 00288 i = (numTaps - 1u) >> 2u; 00289 00290 /* copy data */ 00291 while(i > 0u) 00292 { 00293 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++; 00294 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++; 00295 00296 /* Decrement the loop counter */ 00297 i--; 00298 } 00299 00300 i = (numTaps - 1u) % 0x04u; 00301 00302 /* copy data */ 00303 while(i > 0u) 00304 { 00305 *pStateCurnt++ = *pState++; 00306 00307 /* Decrement the loop counter */ 00308 i--; 00309 } 00310 } 00311 00312 #else 00313 00314 00315 void arm_fir_decimate_fast_q15( 00316 const arm_fir_decimate_instance_q15 * S, 00317 q15_t * pSrc, 00318 q15_t * pDst, 00319 uint32_t blockSize) 00320 { 00321 q15_t *pState = S->pState; /* State pointer */ 00322 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00323 q15_t *pStateCurnt; /* Points to the current sample of the state */ 00324 q15_t *px; /* Temporary pointer for state buffer */ 00325 q15_t *pb; /* Temporary pointer coefficient buffer */ 00326 q15_t x0, x1, c0; /* Temporary variables to hold state and coefficient values */ 00327 q31_t sum0; /* Accumulators */ 00328 q31_t acc0, acc1; 00329 q15_t *px0, *px1; 00330 uint32_t blkCntN3; 00331 uint32_t numTaps = S->numTaps; /* Number of taps */ 00332 uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */ 00333 00334 00335 /* S->pState buffer contains previous frame (numTaps - 1) samples */ 00336 /* pStateCurnt points to the location where the new input data should be written */ 00337 pStateCurnt = S->pState + (numTaps - 1u); 00338 00339 00340 /* Total number of output samples to be computed */ 00341 blkCnt = outBlockSize / 2; 00342 blkCntN3 = outBlockSize - (2*blkCnt); 00343 00344 while(blkCnt > 0u) 00345 { 00346 /* Copy decimation factor number of new input samples into the state buffer */ 00347 i = 2 * S->M; 00348 00349 do 00350 { 00351 *pStateCurnt++ = *pSrc++; 00352 00353 } while(--i); 00354 00355 /* Set accumulator to zero */ 00356 acc0 = 0; 00357 acc1 = 0; 00358 00359 /* Initialize state pointer */ 00360 px0 = pState; 00361 00362 px1 = pState + S->M; 00363 00364 00365 /* Initialize coeff pointer */ 00366 pb = pCoeffs; 00367 00368 /* Loop unrolling. Process 4 taps at a time. */ 00369 tapCnt = numTaps >> 2; 00370 00371 /* Loop over the number of taps. Unroll by a factor of 4. 00372 ** Repeat until we've computed numTaps-4 coefficients. */ 00373 while(tapCnt > 0u) 00374 { 00375 /* Read the Read b[numTaps-1] coefficients */ 00376 c0 = *pb++; 00377 00378 /* Read x[n-numTaps-1] for sample 0 and for sample 1 */ 00379 x0 = *px0++; 00380 x1 = *px1++; 00381 00382 /* Perform the multiply-accumulate */ 00383 acc0 += x0 * c0; 00384 acc1 += x1 * c0; 00385 00386 /* Read the b[numTaps-2] coefficient */ 00387 c0 = *pb++; 00388 00389 /* Read x[n-numTaps-2] for sample 0 and sample 1 */ 00390 x0 = *px0++; 00391 x1 = *px1++; 00392 00393 /* Perform the multiply-accumulate */ 00394 acc0 += x0 * c0; 00395 acc1 += x1 * c0; 00396 00397 /* Read the b[numTaps-3] coefficients */ 00398 c0 = *pb++; 00399 00400 /* Read x[n-numTaps-3] for sample 0 and sample 1 */ 00401 x0 = *px0++; 00402 x1 = *px1++; 00403 00404 /* Perform the multiply-accumulate */ 00405 acc0 += x0 * c0; 00406 acc1 += x1 * c0; 00407 00408 /* Read the b[numTaps-4] coefficient */ 00409 c0 = *pb++; 00410 00411 /* Read x[n-numTaps-4] for sample 0 and sample 1 */ 00412 x0 = *px0++; 00413 x1 = *px1++; 00414 00415 /* Perform the multiply-accumulate */ 00416 acc0 += x0 * c0; 00417 acc1 += x1 * c0; 00418 00419 /* Decrement the loop counter */ 00420 tapCnt--; 00421 } 00422 00423 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00424 tapCnt = numTaps % 0x4u; 00425 00426 while(tapCnt > 0u) 00427 { 00428 /* Read coefficients */ 00429 c0 = *pb++; 00430 00431 /* Fetch 1 state variable */ 00432 x0 = *px0++; 00433 x1 = *px1++; 00434 00435 /* Perform the multiply-accumulate */ 00436 acc0 += x0 * c0; 00437 acc1 += x1 * c0; 00438 00439 /* Decrement the loop counter */ 00440 tapCnt--; 00441 } 00442 00443 /* Advance the state pointer by the decimation factor 00444 * to process the next group of decimation factor number samples */ 00445 pState = pState + S->M * 2; 00446 00447 /* Store filter output, smlad returns the values in 2.14 format */ 00448 /* so downsacle by 15 to get output in 1.15 */ 00449 00450 #ifdef CCS 00451 *pDst++ = (q15_t) (__SSATA(acc0, 15, 16)); 00452 *pDst++ = (q15_t) (__SSATA(acc1, 15, 16)); 00453 #else 00454 *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16)); 00455 *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16)); 00456 #endif 00457 00458 /* Decrement the loop counter */ 00459 blkCnt--; 00460 } 00461 00462 while(blkCntN3 > 0u) 00463 { 00464 /* Copy decimation factor number of new input samples into the state buffer */ 00465 i = S->M; 00466 00467 do 00468 { 00469 *pStateCurnt++ = *pSrc++; 00470 00471 } while(--i); 00472 00473 /*Set sum to zero */ 00474 sum0 = 0; 00475 00476 /* Initialize state pointer */ 00477 px = pState; 00478 00479 /* Initialize coeff pointer */ 00480 pb = pCoeffs; 00481 00482 /* Loop unrolling. Process 4 taps at a time. */ 00483 tapCnt = numTaps >> 2; 00484 00485 /* Loop over the number of taps. Unroll by a factor of 4. 00486 ** Repeat until we've computed numTaps-4 coefficients. */ 00487 while(tapCnt > 0u) 00488 { 00489 /* Read the Read b[numTaps-1] coefficients */ 00490 c0 = *pb++; 00491 00492 /* Read x[n-numTaps-1] and sample */ 00493 x0 = *px++; 00494 00495 /* Perform the multiply-accumulate */ 00496 sum0 += x0 * c0; 00497 00498 /* Read the b[numTaps-2] coefficient */ 00499 c0 = *pb++; 00500 00501 /* Read x[n-numTaps-2] and sample */ 00502 x0 = *px++; 00503 00504 /* Perform the multiply-accumulate */ 00505 sum0 += x0 * c0; 00506 00507 /* Read the b[numTaps-3] coefficients */ 00508 c0 = *pb++; 00509 00510 /* Read x[n-numTaps-3] sample */ 00511 x0 = *px++; 00512 00513 /* Perform the multiply-accumulate */ 00514 sum0 += x0 * c0; 00515 00516 /* Read the b[numTaps-4] coefficient */ 00517 c0 = *pb++; 00518 00519 /* Read x[n-numTaps-4] sample */ 00520 x0 = *px++; 00521 00522 /* Perform the multiply-accumulate */ 00523 sum0 += x0 * c0; 00524 00525 /* Decrement the loop counter */ 00526 tapCnt--; 00527 } 00528 00529 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00530 tapCnt = numTaps % 0x4u; 00531 00532 while(tapCnt > 0u) 00533 { 00534 /* Read coefficients */ 00535 c0 = *pb++; 00536 00537 /* Fetch 1 state variable */ 00538 x0 = *px++; 00539 00540 /* Perform the multiply-accumulate */ 00541 sum0 += x0 * c0; 00542 00543 /* Decrement the loop counter */ 00544 tapCnt--; 00545 } 00546 00547 /* Advance the state pointer by the decimation factor 00548 * to process the next group of decimation factor number samples */ 00549 pState = pState + S->M; 00550 00551 /* Store filter output, smlad returns the values in 2.14 format */ 00552 /* so downsacle by 15 to get output in 1.15 */ 00553 #ifdef CCS 00554 *pDst++ = (q15_t) (__SSATA(sum0, 15, 16)); 00555 #else 00556 *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16)); 00557 #endif 00558 /* Decrement the loop counter */ 00559 blkCntN3--; 00560 } 00561 00562 /* Processing is complete. 00563 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer. 00564 ** This prepares the state buffer for the next function call. */ 00565 00566 /* Points to the start of the state buffer */ 00567 pStateCurnt = S->pState; 00568 00569 i = (numTaps - 1u) >> 2u; 00570 00571 /* copy data */ 00572 while(i > 0u) 00573 { 00574 *pStateCurnt++ = *pState++; 00575 *pStateCurnt++ = *pState++; 00576 *pStateCurnt++ = *pState++; 00577 *pStateCurnt++ = *pState++; 00578 00579 /* Decrement the loop counter */ 00580 i--; 00581 } 00582 00583 i = (numTaps - 1u) % 0x04u; 00584 00585 /* copy data */ 00586 while(i > 0u) 00587 { 00588 *pStateCurnt++ = *pState++; 00589 00590 /* Decrement the loop counter */ 00591 i--; 00592 } 00593 } 00594 00595 00596 #endif 00597
