arm_conv_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_conv_q15.c 00009 * 00010 * Description: Convolution of Q15 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 00025 #include "arm_math.h" 00026 00070 void arm_conv_q15( 00071 q15_t * pSrcA, 00072 uint32_t srcALen, 00073 q15_t * pSrcB, 00074 uint32_t srcBLen, 00075 q15_t * pDst, 00076 q15_t * pScratch1, 00077 q15_t *pScratch2) 00078 { 00079 q63_t acc0, acc1, acc2, acc3; /* Accumulator */ 00080 q31_t x1, x2, x3; /* Temporary variables to hold state and coefficient values */ 00081 q31_t y1, y2; /* State variables */ 00082 q15_t *pOut = pDst; /* output pointer */ 00083 q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch1 */ 00084 q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch1 */ 00085 q15_t *pIn1; /* inputA pointer */ 00086 q15_t *pIn2; /* inputB pointer */ 00087 q15_t *px; /* Intermediate inputA pointer */ 00088 q15_t *py; /* Intermediate inputB pointer */ 00089 uint32_t j, k, blkCnt; /* loop counter */ 00090 uint32_t tapCnt; /* loop count */ 00091 00092 /* The algorithm implementation is based on the lengths of the inputs. */ 00093 /* srcB is always made to slide across srcA. */ 00094 /* So srcBLen is always considered as shorter or equal to srcALen */ 00095 if(srcALen >= srcBLen) 00096 { 00097 /* Initialization of inputA pointer */ 00098 pIn1 = pSrcA; 00099 00100 /* Initialization of inputB pointer */ 00101 pIn2 = pSrcB; 00102 00103 } 00104 else 00105 { 00106 /* Initialization of inputA pointer */ 00107 pIn1 = pSrcB; 00108 00109 /* Initialization of inputB pointer */ 00110 pIn2 = pSrcA; 00111 00112 /* srcBLen is always considered as shorter or equal to srcALen */ 00113 j = srcBLen; 00114 srcBLen = srcALen; 00115 srcALen = j; 00116 } 00117 00118 /* pointer to take end of scratch2 buffer */ 00119 pScr2 = pScratch2 + srcBLen - 1; 00120 00121 /* points to smaller length sequence */ 00122 px = pIn2; 00123 00124 /* Apply loop unrolling and do 4 Copies simultaneously. */ 00125 k = srcBLen >> 2u; 00126 00127 /* First part of the processing with loop unrolling copies 4 data points at a time. 00128 ** a second loop below copies for the remaining 1 to 3 samples. */ 00129 /* Copy smaller length input sequence in reverse order into second scratch buffer */ 00130 while(k > 0u) 00131 { 00132 /* copy second buffer in reversal manner */ 00133 *pScr2-- = *px++; 00134 *pScr2-- = *px++; 00135 *pScr2-- = *px++; 00136 *pScr2-- = *px++; 00137 00138 /* Decrement the loop counter */ 00139 k--; 00140 } 00141 00142 /* If the count is not a multiple of 4, copy remaining samples here. 00143 ** No loop unrolling is used. */ 00144 k = srcBLen % 0x4u; 00145 00146 while(k > 0u) 00147 { 00148 /* copy second buffer in reversal manner for remaining samples */ 00149 *pScr2-- = *px++; 00150 00151 /* Decrement the loop counter */ 00152 k--; 00153 } 00154 00155 /* Initialze temporary scratch pointer */ 00156 pScr1 = pScratch1; 00157 00158 /* Assuming scratch1 buffer is aligned by 32-bit */ 00159 /* Fill (srcBLen - 1u) zeros in scratch buffer */ 00160 arm_fill_q15(0, pScr1, (srcBLen - 1u)); 00161 00162 /* Update temporary scratch pointer */ 00163 pScr1 += (srcBLen - 1u); 00164 00165 /* Copy bigger length sequence(srcALen) samples in scratch1 buffer */ 00166 00167 #ifndef UNALIGNED_SUPPORT_DISABLE 00168 00169 /* Copy (srcALen) samples in scratch buffer */ 00170 arm_copy_q15(pIn1, pScr1, srcALen); 00171 00172 /* Update pointers */ 00173 pScr1 += srcALen; 00174 00175 #else 00176 00177 /* Apply loop unrolling and do 4 Copies simultaneously. */ 00178 k = srcALen >> 2u; 00179 00180 /* First part of the processing with loop unrolling copies 4 data points at a time. 00181 ** a second loop below copies for the remaining 1 to 3 samples. */ 00182 while(k > 0u) 00183 { 00184 /* copy second buffer in reversal manner */ 00185 *pScr1++ = *pIn1++; 00186 *pScr1++ = *pIn1++; 00187 *pScr1++ = *pIn1++; 00188 *pScr1++ = *pIn1++; 00189 00190 /* Decrement the loop counter */ 00191 k--; 00192 } 00193 00194 /* If the count is not a multiple of 4, copy remaining samples here. 00195 ** No loop unrolling is used. */ 00196 k = srcALen % 0x4u; 00197 00198 while(k > 0u) 00199 { 00200 /* copy second buffer in reversal manner for remaining samples */ 00201 *pScr1++ = *pIn1++; 00202 00203 /* Decrement the loop counter */ 00204 k--; 00205 } 00206 00207 #endif 00208 00209 00210 #ifndef UNALIGNED_SUPPORT_DISABLE 00211 00212 /* Fill (srcBLen - 1u) zeros at end of scratch buffer */ 00213 arm_fill_q15(0, pScr1, (srcBLen - 1u)); 00214 00215 /* Update pointer */ 00216 pScr1 += (srcBLen - 1u); 00217 00218 #else 00219 00220 /* Apply loop unrolling and do 4 Copies simultaneously. */ 00221 k = (srcBLen - 1u) >> 2u; 00222 00223 /* First part of the processing with loop unrolling copies 4 data points at a time. 00224 ** a second loop below copies for the remaining 1 to 3 samples. */ 00225 while(k > 0u) 00226 { 00227 /* copy second buffer in reversal manner */ 00228 *pScr1++ = 0; 00229 *pScr1++ = 0; 00230 *pScr1++ = 0; 00231 *pScr1++ = 0; 00232 00233 /* Decrement the loop counter */ 00234 k--; 00235 } 00236 00237 /* If the count is not a multiple of 4, copy remaining samples here. 00238 ** No loop unrolling is used. */ 00239 k = (srcBLen - 1u) % 0x4u; 00240 00241 while(k > 0u) 00242 { 00243 /* copy second buffer in reversal manner for remaining samples */ 00244 *pScr1++ = 0; 00245 00246 /* Decrement the loop counter */ 00247 k--; 00248 } 00249 00250 #endif 00251 00252 /* Temporary pointer for scratch2 */ 00253 py = pScratch2; 00254 00255 00256 /* Initialization of pIn2 pointer */ 00257 pIn2 = py; 00258 00259 /* First part of the processing with loop unrolling process 4 data points at a time. 00260 ** a second loop below process for the remaining 1 to 3 samples. */ 00261 00262 /* Actual convolution process starts here */ 00263 blkCnt = (srcALen + srcBLen - 1u) >> 2; 00264 00265 while(blkCnt > 0) 00266 { 00267 /* Initialze temporary scratch pointer as scratch1 */ 00268 pScr1 = pScratch1; 00269 00270 /* Clear Accumlators */ 00271 acc0 = 0; 00272 acc1 = 0; 00273 acc2 = 0; 00274 acc3 = 0; 00275 00276 /* Read two samples from scratch1 buffer */ 00277 x1 = *__SIMD32(pScr1)++; 00278 00279 /* Read next two samples from scratch1 buffer */ 00280 x2 = *__SIMD32(pScr1)++; 00281 00282 tapCnt = (srcBLen) >> 2u; 00283 00284 while(tapCnt > 0u) 00285 { 00286 00287 /* Read four samples from smaller buffer */ 00288 y1 = _SIMD32_OFFSET(pIn2); 00289 y2 = _SIMD32_OFFSET(pIn2 + 2u); 00290 00291 /* multiply and accumlate */ 00292 acc0 = __SMLALD(x1, y1, acc0); 00293 acc2 = __SMLALD(x2, y1, acc2); 00294 00295 /* pack input data */ 00296 #ifndef ARM_MATH_BIG_ENDIAN 00297 x3 = __PKHBT(x2, x1, 0); 00298 #else 00299 x3 = __PKHBT(x1, x2, 0); 00300 #endif 00301 00302 /* multiply and accumlate */ 00303 acc1 = __SMLALDX(x3, y1, acc1); 00304 00305 /* Read next two samples from scratch1 buffer */ 00306 x1 = _SIMD32_OFFSET(pScr1); 00307 00308 /* multiply and accumlate */ 00309 acc0 = __SMLALD(x2, y2, acc0); 00310 acc2 = __SMLALD(x1, y2, acc2); 00311 00312 /* pack input data */ 00313 #ifndef ARM_MATH_BIG_ENDIAN 00314 x3 = __PKHBT(x1, x2, 0); 00315 #else 00316 x3 = __PKHBT(x2, x1, 0); 00317 #endif 00318 00319 acc3 = __SMLALDX(x3, y1, acc3); 00320 acc1 = __SMLALDX(x3, y2, acc1); 00321 00322 x2 = _SIMD32_OFFSET(pScr1 + 2u); 00323 00324 #ifndef ARM_MATH_BIG_ENDIAN 00325 x3 = __PKHBT(x2, x1, 0); 00326 #else 00327 x3 = __PKHBT(x1, x2, 0); 00328 #endif 00329 00330 acc3 = __SMLALDX(x3, y2, acc3); 00331 00332 /* update scratch pointers */ 00333 pIn2 += 4u; 00334 pScr1 += 4u; 00335 00336 00337 /* Decrement the loop counter */ 00338 tapCnt--; 00339 } 00340 00341 /* Update scratch pointer for remaining samples of smaller length sequence */ 00342 pScr1 -= 4u; 00343 00344 /* apply same above for remaining samples of smaller length sequence */ 00345 tapCnt = (srcBLen) & 3u; 00346 00347 while(tapCnt > 0u) 00348 { 00349 00350 /* accumlate the results */ 00351 acc0 += (*pScr1++ * *pIn2); 00352 acc1 += (*pScr1++ * *pIn2); 00353 acc2 += (*pScr1++ * *pIn2); 00354 acc3 += (*pScr1++ * *pIn2++); 00355 00356 pScr1 -= 3u; 00357 00358 /* Decrement the loop counter */ 00359 tapCnt--; 00360 } 00361 00362 blkCnt--; 00363 00364 00365 /* Store the results in the accumulators in the destination buffer. */ 00366 00367 #ifndef ARM_MATH_BIG_ENDIAN 00368 00369 *__SIMD32(pOut)++ = 00370 #ifdef CCS 00371 __PKHBT(__SSATA(acc0, 15, 16), __SSATA(acc1, 15, 16), 16); 00372 00373 *__SIMD32(pOut)++ = 00374 __PKHBT(__SSATA(acc2, 15, 16), __SSATA(acc3, 15, 16), 16); 00375 #else 00376 __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16); 00377 00378 *__SIMD32(pOut)++ = 00379 __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16); 00380 #endif 00381 00382 #else 00383 00384 *__SIMD32(pOut)++ = 00385 #ifdef CCS 00386 __PKHBT(__SSATA(acc1, 15, 16), __SSATA(acc0, 15, 16), 16); 00387 00388 *__SIMD32(pOut)++ = 00389 __PKHBT(__SSATA(acc3, 15, 16), __SSATA(acc2, 15, 16), 16); 00390 #else 00391 __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16); 00392 00393 *__SIMD32(pOut)++ = 00394 __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16); 00395 00396 #endif 00397 00398 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00399 00400 /* Initialization of inputB pointer */ 00401 pIn2 = py; 00402 00403 pScratch1 += 4u; 00404 00405 } 00406 00407 00408 blkCnt = (srcALen + srcBLen - 1u) & 0x3; 00409 00410 /* Calculate convolution for remaining samples of Bigger length sequence */ 00411 while(blkCnt > 0) 00412 { 00413 /* Initialze temporary scratch pointer as scratch1 */ 00414 pScr1 = pScratch1; 00415 00416 /* Clear Accumlators */ 00417 acc0 = 0; 00418 00419 tapCnt = (srcBLen) >> 1u; 00420 00421 while(tapCnt > 0u) 00422 { 00423 00424 /* Read next two samples from scratch1 buffer */ 00425 x1 = *__SIMD32(pScr1)++; 00426 00427 /* Read two samples from smaller buffer */ 00428 y1 = *__SIMD32(pIn2)++; 00429 00430 acc0 = __SMLALD(x1, y1, acc0); 00431 00432 /* Decrement the loop counter */ 00433 tapCnt--; 00434 } 00435 00436 tapCnt = (srcBLen) & 1u; 00437 00438 /* apply same above for remaining samples of smaller length sequence */ 00439 while(tapCnt > 0u) 00440 { 00441 00442 /* accumlate the results */ 00443 acc0 += (*pScr1++ * *pIn2++); 00444 00445 /* Decrement the loop counter */ 00446 tapCnt--; 00447 } 00448 00449 blkCnt--; 00450 00451 /* The result is in 2.30 format. Convert to 1.15 with saturation. 00452 ** Then store the output in the destination buffer. */ 00453 #ifdef CCS 00454 *pOut++ = (q15_t) (__SSATA(acc0, 15, 16)); 00455 #else 00456 *pOut++ = (q15_t) (__SSAT((acc0 >> 15), 16)); 00457 #endif 00458 00459 00460 /* Initialization of inputB pointer */ 00461 pIn2 = py; 00462 00463 pScratch1 += 1u; 00464 00465 } 00466 00467 } 00468
