arm_fir_sparse_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_sparse_q15.c 00009 * 00010 * Description: Q15 sparse FIR filter processing function. 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 00052 void arm_fir_sparse_q15( 00053 arm_fir_sparse_instance_q15 * S, 00054 q15_t * pSrc, 00055 q15_t * pDst, 00056 q15_t * pScratchIn, 00057 q31_t * pScratchOut, 00058 uint32_t blockSize) 00059 { 00060 00061 q15_t *pState = S->pState; /* State pointer */ 00062 q15_t *pIn = pSrc; /* Working pointer for input */ 00063 q15_t *pOut = pDst; /* Working pointer for output */ 00064 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00065 q15_t *px; /* Temporary pointers for scratch buffer */ 00066 q15_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */ 00067 q15_t *py = pState; /* Temporary pointers for state buffer */ 00068 int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */ 00069 uint32_t delaySize = S->maxDelay + blockSize; /* state length */ 00070 uint16_t numTaps = S->numTaps; /* Filter order */ 00071 int32_t readIndex; /* Read index of the state buffer */ 00072 uint32_t tapCnt, blkCnt; /* loop counters */ 00073 q15_t coeff = *pCoeffs++; /* Read the first coefficient value */ 00074 q31_t *pScr2 = pScratchOut; /* Working pointer for pScratchOut */ 00075 q31_t in1, in2; /* Temporary variables */ 00076 q15_t x0, x1; 00077 q31_t y0, y1; 00078 00079 00080 /* BlockSize of Input samples are copied into the state buffer */ 00081 /* StateIndex points to the starting position to write in the state buffer */ 00082 arm_circularWrite_q15(py, delaySize, &S->stateIndex, 1, pIn, 1, blockSize); 00083 00084 /* Loop over the number of taps. */ 00085 tapCnt = numTaps; 00086 00087 /* Read Index, from where the state buffer should be read, is calculated. */ 00088 readIndex = (S->stateIndex - blockSize) - *pTapDelay++; 00089 00090 /* Wraparound of readIndex */ 00091 if(readIndex < 0) 00092 { 00093 readIndex += (int32_t) delaySize; 00094 } 00095 00096 /* Working pointer for state buffer is updated */ 00097 py = pState; 00098 00099 /* blockSize samples are read from the state buffer */ 00100 arm_circularRead_q15(py, delaySize, &readIndex, 1, 00101 pb, pb, blockSize, 1, blockSize); 00102 00103 /* Working pointer for the scratch buffer of state values */ 00104 px = pb; 00105 00106 /* Working pointer for scratch buffer of output values */ 00107 pScratchOut = pScr2; 00108 00109 /* Loop over the blockSize. Unroll by a factor of 4. 00110 * Compute 4 multiplications at a time. */ 00111 blkCnt = blockSize >> 2; 00112 00113 while(blkCnt > 0u) 00114 { 00115 /* Perform Multiplications and store in scratch buffer */ 00116 00117 /* Read two values from state */ 00118 x0 = *px; 00119 x1 = *(px + 1u); 00120 00121 /* multiply */ 00122 y0 = (q31_t)x0 * coeff; 00123 y1 = (q31_t)x1 * coeff; 00124 00125 /* Write two values into output buffer */ 00126 *pScratchOut++ = y0; 00127 *pScratchOut++ = y1; 00128 00129 /* Read next two values from state */ 00130 x0 = *(px + 2u); 00131 x1 = *(px + 3u); 00132 00133 /* multiply */ 00134 y0 = (q31_t)x0 * coeff; 00135 y1 = (q31_t)x1 * coeff; 00136 00137 /* Write next two values into output buffer */ 00138 *pScratchOut++ = y0; 00139 px += 4u; 00140 *pScratchOut++ = y1; 00141 00142 00143 /* Decrement the loop counter */ 00144 blkCnt--; 00145 } 00146 00147 /* If the blockSize is not a multiple of 4, 00148 * compute the remaining samples */ 00149 blkCnt = blockSize % 0x4u; 00150 00151 while(blkCnt > 0u) 00152 { 00153 /* Perform multiplication and store in the scratch buffer */ 00154 *pScratchOut++ = ((q31_t) * px++ * coeff); 00155 00156 /* Decrement the loop counter */ 00157 blkCnt--; 00158 } 00159 00160 /* Load the coefficient value and 00161 * increment the coefficient buffer for the next set of state values */ 00162 coeff = *pCoeffs++; 00163 00164 /* Read Index, from where the state buffer should be read, is calculated. */ 00165 readIndex = (S->stateIndex - blockSize) - *pTapDelay++; 00166 00167 /* Wraparound of readIndex */ 00168 if(readIndex < 0) 00169 { 00170 readIndex += (int32_t) delaySize; 00171 } 00172 00173 /* Loop over the number of taps. */ 00174 tapCnt = (uint32_t) numTaps - 1u; 00175 00176 while(tapCnt > 0u) 00177 { 00178 /* Working pointer for state buffer is updated */ 00179 py = pState; 00180 00181 /* blockSize samples are read from the state buffer */ 00182 arm_circularRead_q15(py, delaySize, &readIndex, 1, 00183 pb, pb, blockSize, 1, blockSize); 00184 00185 /* Working pointer for the scratch buffer of state values */ 00186 px = pb; 00187 00188 /* Working pointer for scratch buffer of output values */ 00189 pScratchOut = pScr2; 00190 00191 /* Loop over the blockSize. Unroll by a factor of 4. 00192 * Compute 4 MACS at a time. */ 00193 blkCnt = blockSize >> 2; 00194 00195 while(blkCnt > 0u) 00196 { 00197 /* Read two values from scratch */ 00198 x0 = *px; 00199 x1 = *(px + 1u); 00200 00201 /* Read two values from output buffer */ 00202 y0 = *pScratchOut; 00203 y1 = *(pScratchOut + 1u); 00204 00205 /* Perform Multiply-Accumulate */ 00206 y0 = (q31_t)x0 * coeff + y0; 00207 y1 = (q31_t)x1 * coeff + y1; 00208 00209 /* write multiply-Accumlate values into output buffer */ 00210 *pScratchOut = y0; 00211 *(pScratchOut + 1u) = y1; 00212 00213 /* Read next two values from scratch */ 00214 x0 = *(px + 2u); 00215 x1 = *(px + 3u); 00216 /* Read two values from output buffer */ 00217 y0 = *(pScratchOut + 2u); 00218 y1 = *(pScratchOut + 3u); 00219 00220 /* Perform Multiply-Accumulate */ 00221 y0 = (q31_t)x0 * coeff + y0; 00222 y1 = (q31_t)x1 * coeff + y1; 00223 00224 /* write multiply-Accumlate values into output buffer */ 00225 *(pScratchOut + 2u) = y0; 00226 *(pScratchOut + 3u) = y1; 00227 00228 /* update pointers */ 00229 px += 4u; 00230 pScratchOut += 4u; 00231 00232 00233 /* Decrement the loop counter */ 00234 blkCnt--; 00235 } 00236 00237 /* If the blockSize is not a multiple of 4, 00238 * compute the remaining samples */ 00239 blkCnt = blockSize % 0x4u; 00240 00241 while(blkCnt > 0u) 00242 { 00243 00244 /* Perform Multiply-Accumulate */ 00245 *pScratchOut++ += (q31_t) * px++ * coeff; 00246 00247 /* Decrement the loop counter */ 00248 blkCnt--; 00249 } 00250 00251 /* Load the coefficient value and 00252 * increment the coefficient buffer for the next set of state values */ 00253 coeff = *pCoeffs++; 00254 00255 /* Read Index, from where the state buffer should be read, is calculated. */ 00256 readIndex = (S->stateIndex - blockSize) - *pTapDelay++; 00257 00258 /* Wraparound of readIndex */ 00259 if(readIndex < 0) 00260 { 00261 readIndex += (int32_t) delaySize; 00262 } 00263 00264 /* Decrement the tap loop counter */ 00265 tapCnt--; 00266 } 00267 00268 /* All the output values are in pScratchOut buffer. 00269 Convert them into 1.15 format, saturate and store in the destination buffer. */ 00270 /* Loop over the blockSize. */ 00271 blkCnt = blockSize >> 2; 00272 00273 while(blkCnt > 0u) 00274 { 00275 /* Read two values from output buffer */ 00276 in1 = *pScr2++; 00277 in2 = *pScr2++; 00278 00279 #ifndef ARM_MATH_BIG_ENDIAN 00280 00281 *__SIMD32(pOut)++ = 00282 #ifdef CCS 00283 __PKHBT((q15_t) __SSATA(in1, 15, 16), (q15_t) __SSATA(in2, 15, 16), 16); 00284 #else 00285 __PKHBT((q15_t) __SSAT(in1 >> 15, 16), (q15_t) __SSAT(in2 >> 15, 16), 16); 00286 #endif // #ifdef CCS 00287 00288 #else 00289 *__SIMD32(pOut)++ = 00290 #ifdef CCS 00291 __PKHBT((q15_t) __SSATA(in2, 15, 16), (q15_t) __SSATA(in1, 15, 16), 16); 00292 #else 00293 __PKHBT((q15_t) __SSAT(in2 >> 15, 16), (q15_t) __SSAT(in1 >> 15, 16), 16); 00294 #endif // #ifdef CCS 00295 00296 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00297 00298 /* Read two values from output buffer */ 00299 in1 = *pScr2++; 00300 in2 = *pScr2++; 00301 00302 #ifndef ARM_MATH_BIG_ENDIAN 00303 00304 *__SIMD32(pOut)++ = 00305 #ifdef CCS 00306 __PKHBT((q15_t) __SSATA(in1, 15, 16), (q15_t) __SSATA(in2, 15, 16), 16); 00307 #else 00308 __PKHBT((q15_t) __SSAT(in1 >> 15, 16), (q15_t) __SSAT(in2 >> 15, 16), 16); 00309 #endif // #ifdef CCS 00310 00311 #else 00312 00313 *__SIMD32(pOut)++ = 00314 #ifdef CCS 00315 __PKHBT((q15_t) __SSATA(in2, 15, 16), (q15_t) __SSATA(in1, 15, 16), 16); 00316 #else 00317 __PKHBT((q15_t) __SSAT(in2 >> 15, 16), (q15_t) __SSAT(in1 >> 15, 16), 16); 00318 #endif // #ifdef CCS 00319 00320 00321 00322 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00323 00324 blkCnt--; 00325 00326 } 00327 00328 /* If the blockSize is not a multiple of 4, 00329 remaining samples are processed in the below loop */ 00330 blkCnt = blockSize % 0x4u; 00331 00332 while(blkCnt > 0u) 00333 { 00334 #ifdef CCS 00335 *pOut++ = (q15_t) __SSATA(*pScr2++, 15, 16); 00336 #else 00337 *pOut++ = (q15_t) __SSAT(*pScr2++ >> 15, 16); 00338 #endif 00339 /* Decrement the loop counter */ 00340 blkCnt--; 00341 } 00342 } 00343
