CMSIS DSP Software Library: arm_correlate_q31.c Source File
Main Page
Modules
Data Structures
Files
Examples
File List
Globals
arm_correlate_q31.c
Go to the documentation of this file.00001 /* ----------------------------------------------------------------------
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.
00003 *
00004 * $Date: 29. November 2010
00005 * $Revision: V1.0.3
00006 *
00007 * Project: CMSIS DSP Library
00008 * Title: arm_correlate_q31.c
00009 *
00010 * Description: Q31 Correlation.
00011 *
00012 * Target Processor: Cortex-M4/Cortex-M3
00013 *
00014 * Version 1.0.3 2010/11/29
00015 * Re-organized the CMSIS folders and updated documentation.
00016 *
00017 * Version 1.0.2 2010/11/11
00018 * Documentation updated.
00019 *
00020 * Version 1.0.1 2010/10/05
00021 * Production release and review comments incorporated.
00022 *
00023 * Version 1.0.0 2010/09/20
00024 * Production release and review comments incorporated
00025 *
00026 * Version 0.0.7 2010/06/10
00027 * Misra-C changes done
00028 *
00029 * -------------------------------------------------------------------- */
00030
00031 #include "arm_math.h"
00032
00068 void arm_correlate_q31(
00069 q31_t * pSrcA,
00070 uint32_t srcALen,
00071 q31_t * pSrcB,
00072 uint32_t srcBLen,
00073 q31_t * pDst)
00074 {
00075 q31_t *pIn1; /* inputA pointer */
00076 q31_t *pIn2; /* inputB pointer */
00077 q31_t *pOut = pDst; /* output pointer */
00078 q31_t *px; /* Intermediate inputA pointer */
00079 q31_t *py; /* Intermediate inputB pointer */
00080 q31_t *pSrc1; /* Intermediate pointers */
00081 q63_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
00082 q31_t x0, x1, x2, x3, c0; /* temporary variables for holding input and coefficient values */
00083 uint32_t j, k = 0u, count, blkCnt, outBlockSize, blockSize1, blockSize2, blockSize3; /* loop counter */
00084 int32_t inc = 1; /* Destination address modifier */
00085
00086
00087 /* The algorithm implementation is based on the lengths of the inputs. */
00088 /* srcB is always made to slide across srcA. */
00089 /* So srcBLen is always considered as shorter or equal to srcALen */
00090 /* But CORR(x, y) is reverse of CORR(y, x) */
00091 /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
00092 /* and the destination pointer modifier, inc is set to -1 */
00093 /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
00094 /* But to improve the performance,
00095 * we include zeroes in the output instead of zero padding either of the the inputs*/
00096 /* If srcALen > srcBLen,
00097 * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
00098 /* If srcALen < srcBLen,
00099 * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
00100 if(srcALen >= srcBLen)
00101 {
00102 /* Initialization of inputA pointer */
00103 pIn1 = (pSrcA);
00104
00105 /* Initialization of inputB pointer */
00106 pIn2 = (pSrcB);
00107
00108 /* Number of output samples is calculated */
00109 outBlockSize = (2u * srcALen) - 1u;
00110
00111 /* When srcALen > srcBLen, zero padding is done to srcB
00112 * to make their lengths equal.
00113 * Instead, (outBlockSize - (srcALen + srcBLen - 1))
00114 * number of output samples are made zero */
00115 j = outBlockSize - (srcALen + (srcBLen - 1u));
00116
00117 while(j > 0u)
00118 {
00119 /* Zero is stored in the destination buffer */
00120 *pOut++ = 0;
00121
00122 /* Decrement the loop counter */
00123 j--;
00124 }
00125
00126 }
00127 else
00128 {
00129 /* Initialization of inputA pointer */
00130 pIn1 = (pSrcB);
00131
00132 /* Initialization of inputB pointer */
00133 pIn2 = (pSrcA);
00134
00135 /* srcBLen is always considered as shorter or equal to srcALen */
00136 j = srcBLen;
00137 srcBLen = srcALen;
00138 srcALen = j;
00139
00140 /* CORR(x, y) = Reverse order(CORR(y, x)) */
00141 /* Hence set the destination pointer to point to the last output sample */
00142 pOut = pDst + ((srcALen + srcBLen) - 2u);
00143
00144 /* Destination address modifier is set to -1 */
00145 inc = -1;
00146
00147 }
00148
00149 /* The function is internally
00150 * divided into three parts according to the number of multiplications that has to be
00151 * taken place between inputA samples and inputB samples. In the first part of the
00152 * algorithm, the multiplications increase by one for every iteration.
00153 * In the second part of the algorithm, srcBLen number of multiplications are done.
00154 * In the third part of the algorithm, the multiplications decrease by one
00155 * for every iteration.*/
00156 /* The algorithm is implemented in three stages.
00157 * The loop counters of each stage is initiated here. */
00158 blockSize1 = srcBLen - 1u;
00159 blockSize2 = srcALen - (srcBLen - 1u);
00160 blockSize3 = blockSize1;
00161
00162 /* --------------------------
00163 * Initializations of stage1
00164 * -------------------------*/
00165
00166 /* sum = x[0] * y[srcBlen - 1]
00167 * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
00168 * ....
00169 * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
00170 */
00171
00172 /* In this stage the MAC operations are increased by 1 for every iteration.
00173 The count variable holds the number of MAC operations performed */
00174 count = 1u;
00175
00176 /* Working pointer of inputA */
00177 px = pIn1;
00178
00179 /* Working pointer of inputB */
00180 pSrc1 = pIn2 + (srcBLen - 1u);
00181 py = pSrc1;
00182
00183 /* ------------------------
00184 * Stage1 process
00185 * ----------------------*/
00186
00187 /* The first stage starts here */
00188 while(blockSize1 > 0u)
00189 {
00190 /* Accumulator is made zero for every iteration */
00191 sum = 0;
00192
00193 /* Apply loop unrolling and compute 4 MACs simultaneously. */
00194 k = count >> 2;
00195
00196 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
00197 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
00198 while(k > 0u)
00199 {
00200 /* x[0] * y[srcBLen - 4] */
00201 sum += (q63_t) * px++ * (*py++);
00202 /* x[1] * y[srcBLen - 3] */
00203 sum += (q63_t) * px++ * (*py++);
00204 /* x[2] * y[srcBLen - 2] */
00205 sum += (q63_t) * px++ * (*py++);
00206 /* x[3] * y[srcBLen - 1] */
00207 sum += (q63_t) * px++ * (*py++);
00208
00209 /* Decrement the loop counter */
00210 k--;
00211 }
00212
00213 /* If the count is not a multiple of 4, compute any remaining MACs here.
00214 ** No loop unrolling is used. */
00215 k = count % 0x4u;
00216
00217 while(k > 0u)
00218 {
00219 /* Perform the multiply-accumulates */
00220 /* x[0] * y[srcBLen - 1] */
00221 sum += (q63_t) * px++ * (*py++);
00222
00223 /* Decrement the loop counter */
00224 k--;
00225 }
00226
00227 /* Store the result in the accumulator in the destination buffer. */
00228 *pOut = (q31_t) (sum >> 31);
00229 /* Destination pointer is updated according to the address modifier, inc */
00230 pOut += inc;
00231
00232 /* Update the inputA and inputB pointers for next MAC calculation */
00233 py = pSrc1 - count;
00234 px = pIn1;
00235
00236 /* Increment the MAC count */
00237 count++;
00238
00239 /* Decrement the loop counter */
00240 blockSize1--;
00241 }
00242
00243 /* --------------------------
00244 * Initializations of stage2
00245 * ------------------------*/
00246
00247 /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
00248 * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
00249 * ....
00250 * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
00251 */
00252
00253 /* Working pointer of inputA */
00254 px = pIn1;
00255
00256 /* Working pointer of inputB */
00257 py = pIn2;
00258
00259 /* count is index by which the pointer pIn1 to be incremented */
00260 count = 1u;
00261
00262 /* -------------------
00263 * Stage2 process
00264 * ------------------*/
00265
00266 /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
00267 * So, to loop unroll over blockSize2,
00268 * srcBLen should be greater than or equal to 4 */
00269 if(srcBLen >= 4u)
00270 {
00271 /* Loop unroll over blockSize2, by 4 */
00272 blkCnt = blockSize2 >> 2u;
00273
00274 while(blkCnt > 0u)
00275 {
00276 /* Set all accumulators to zero */
00277 acc0 = 0;
00278 acc1 = 0;
00279 acc2 = 0;
00280 acc3 = 0;
00281
00282 /* read x[0], x[1], x[2] samples */
00283 x0 = *(px++);
00284 x1 = *(px++);
00285 x2 = *(px++);
00286
00287 /* Apply loop unrolling and compute 4 MACs simultaneously. */
00288 k = srcBLen >> 2u;
00289
00290 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
00291 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
00292 do
00293 {
00294 /* Read y[0] sample */
00295 c0 = *(py++);
00296
00297 /* Read x[3] sample */
00298 x3 = *(px++);
00299
00300 /* Perform the multiply-accumulate */
00301 /* acc0 += x[0] * y[0] */
00302 acc0 += ((q63_t) x0 * c0);
00303 /* acc1 += x[1] * y[0] */
00304 acc1 += ((q63_t) x1 * c0);
00305 /* acc2 += x[2] * y[0] */
00306 acc2 += ((q63_t) x2 * c0);
00307 /* acc3 += x[3] * y[0] */
00308 acc3 += ((q63_t) x3 * c0);
00309
00310 /* Read y[1] sample */
00311 c0 = *(py++);
00312
00313 /* Read x[4] sample */
00314 x0 = *(px++);
00315
00316 /* Perform the multiply-accumulates */
00317 /* acc0 += x[1] * y[1] */
00318 acc0 += ((q63_t) x1 * c0);
00319 /* acc1 += x[2] * y[1] */
00320 acc1 += ((q63_t) x2 * c0);
00321 /* acc2 += x[3] * y[1] */
00322 acc2 += ((q63_t) x3 * c0);
00323 /* acc3 += x[4] * y[1] */
00324 acc3 += ((q63_t) x0 * c0);
00325 /* Read y[2] sample */
00326 c0 = *(py++);
00327
00328 /* Read x[5] sample */
00329 x1 = *(px++);
00330
00331 /* Perform the multiply-accumulates */
00332 /* acc0 += x[2] * y[2] */
00333 acc0 += ((q63_t) x2 * c0);
00334 /* acc1 += x[3] * y[2] */
00335 acc1 += ((q63_t) x3 * c0);
00336 /* acc2 += x[4] * y[2] */
00337 acc2 += ((q63_t) x0 * c0);
00338 /* acc3 += x[5] * y[2] */
00339 acc3 += ((q63_t) x1 * c0);
00340
00341 /* Read y[3] sample */
00342 c0 = *(py++);
00343
00344 /* Read x[6] sample */
00345 x2 = *(px++);
00346
00347 /* Perform the multiply-accumulates */
00348 /* acc0 += x[3] * y[3] */
00349 acc0 += ((q63_t) x3 * c0);
00350 /* acc1 += x[4] * y[3] */
00351 acc1 += ((q63_t) x0 * c0);
00352 /* acc2 += x[5] * y[3] */
00353 acc2 += ((q63_t) x1 * c0);
00354 /* acc3 += x[6] * y[3] */
00355 acc3 += ((q63_t) x2 * c0);
00356
00357
00358 } while(--k);
00359
00360 /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
00361 ** No loop unrolling is used. */
00362 k = srcBLen % 0x4u;
00363
00364 while(k > 0u)
00365 {
00366 /* Read y[4] sample */
00367 c0 = *(py++);
00368
00369 /* Read x[7] sample */
00370 x3 = *(px++);
00371
00372 /* Perform the multiply-accumulates */
00373 /* acc0 += x[4] * y[4] */
00374 acc0 += ((q63_t) x0 * c0);
00375 /* acc1 += x[5] * y[4] */
00376 acc1 += ((q63_t) x1 * c0);
00377 /* acc2 += x[6] * y[4] */
00378 acc2 += ((q63_t) x2 * c0);
00379 /* acc3 += x[7] * y[4] */
00380 acc3 += ((q63_t) x3 * c0);
00381
00382 /* Reuse the present samples for the next MAC */
00383 x0 = x1;
00384 x1 = x2;
00385 x2 = x3;
00386
00387 /* Decrement the loop counter */
00388 k--;
00389 }
00390
00391 /* Store the result in the accumulator in the destination buffer. */
00392 *pOut = (q31_t) (acc0 >> 31);
00393 /* Destination pointer is updated according to the address modifier, inc */
00394 pOut += inc;
00395
00396 *pOut = (q31_t) (acc1 >> 31);
00397 pOut += inc;
00398
00399 *pOut = (q31_t) (acc2 >> 31);
00400 pOut += inc;
00401
00402 *pOut = (q31_t) (acc3 >> 31);
00403 pOut += inc;
00404
00405 /* Update the inputA and inputB pointers for next MAC calculation */
00406 px = pIn1 + (count * 4u);
00407 py = pIn2;
00408
00409 /* Increment the pointer pIn1 index, count by 1 */
00410 count++;
00411
00412 /* Decrement the loop counter */
00413 blkCnt--;
00414 }
00415
00416 /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
00417 ** No loop unrolling is used. */
00418 blkCnt = blockSize2 % 0x4u;
00419
00420 while(blkCnt > 0u)
00421 {
00422 /* Accumulator is made zero for every iteration */
00423 sum = 0;
00424
00425 /* Apply loop unrolling and compute 4 MACs simultaneously. */
00426 k = srcBLen >> 2u;
00427
00428 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
00429 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
00430 while(k > 0u)
00431 {
00432 /* Perform the multiply-accumulates */
00433 sum += (q63_t) * px++ * (*py++);
00434 sum += (q63_t) * px++ * (*py++);
00435 sum += (q63_t) * px++ * (*py++);
00436 sum += (q63_t) * px++ * (*py++);
00437
00438 /* Decrement the loop counter */
00439 k--;
00440 }
00441
00442 /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
00443 ** No loop unrolling is used. */
00444 k = srcBLen % 0x4u;
00445
00446 while(k > 0u)
00447 {
00448 /* Perform the multiply-accumulate */
00449 sum += (q63_t) * px++ * (*py++);
00450
00451 /* Decrement the loop counter */
00452 k--;
00453 }
00454
00455 /* Store the result in the accumulator in the destination buffer. */
00456 *pOut = (q31_t) (sum >> 31);
00457 /* Destination pointer is updated according to the address modifier, inc */
00458 pOut += inc;
00459
00460 /* Update the inputA and inputB pointers for next MAC calculation */
00461 px = pIn1 + count;
00462 py = pIn2;
00463
00464 /* Increment the MAC count */
00465 count++;
00466
00467 /* Decrement the loop counter */
00468 blkCnt--;
00469 }
00470 }
00471 else
00472 {
00473 /* If the srcBLen is not a multiple of 4,
00474 * the blockSize2 loop cannot be unrolled by 4 */
00475 blkCnt = blockSize2;
00476
00477 while(blkCnt > 0u)
00478 {
00479 /* Accumulator is made zero for every iteration */
00480 sum = 0;
00481
00482 /* Loop over srcBLen */
00483 k = srcBLen;
00484
00485 while(k > 0u)
00486 {
00487 /* Perform the multiply-accumulate */
00488 sum += (q63_t) * px++ * (*py++);
00489
00490 /* Decrement the loop counter */
00491 k--;
00492 }
00493
00494 /* Store the result in the accumulator in the destination buffer. */
00495 *pOut = (q31_t) (sum >> 31);
00496 /* Destination pointer is updated according to the address modifier, inc */
00497 pOut += inc;
00498
00499 /* Update the inputA and inputB pointers for next MAC calculation */
00500 px = pIn1 + count;
00501 py = pIn2;
00502
00503 /* Increment the MAC count */
00504 count++;
00505
00506 /* Decrement the loop counter */
00507 blkCnt--;
00508 }
00509 }
00510
00511 /* --------------------------
00512 * Initializations of stage3
00513 * -------------------------*/
00514
00515 /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
00516 * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
00517 * ....
00518 * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
00519 * sum += x[srcALen-1] * y[0]
00520 */
00521
00522 /* In this stage the MAC operations are decreased by 1 for every iteration.
00523 The count variable holds the number of MAC operations performed */
00524 count = srcBLen - 1u;
00525
00526 /* Working pointer of inputA */
00527 pSrc1 = pIn1 + (srcALen - (srcBLen - 1u));
00528 px = pSrc1;
00529
00530 /* Working pointer of inputB */
00531 py = pIn2;
00532
00533 /* -------------------
00534 * Stage3 process
00535 * ------------------*/
00536
00537 while(blockSize3 > 0u)
00538 {
00539 /* Accumulator is made zero for every iteration */
00540 sum = 0;
00541
00542 /* Apply loop unrolling and compute 4 MACs simultaneously. */
00543 k = count >> 2u;
00544
00545 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
00546 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
00547 while(k > 0u)
00548 {
00549 /* Perform the multiply-accumulates */
00550 /* sum += x[srcALen - srcBLen + 4] * y[3] */
00551 sum += (q63_t) * px++ * (*py++);
00552 /* sum += x[srcALen - srcBLen + 3] * y[2] */
00553 sum += (q63_t) * px++ * (*py++);
00554 /* sum += x[srcALen - srcBLen + 2] * y[1] */
00555 sum += (q63_t) * px++ * (*py++);
00556 /* sum += x[srcALen - srcBLen + 1] * y[0] */
00557 sum += (q63_t) * px++ * (*py++);
00558
00559 /* Decrement the loop counter */
00560 k--;
00561 }
00562
00563 /* If the count is not a multiple of 4, compute any remaining MACs here.
00564 ** No loop unrolling is used. */
00565 k = count % 0x4u;
00566
00567 while(k > 0u)
00568 {
00569 /* Perform the multiply-accumulates */
00570 sum += (q63_t) * px++ * (*py++);
00571
00572 /* Decrement the loop counter */
00573 k--;
00574 }
00575
00576 /* Store the result in the accumulator in the destination buffer. */
00577 *pOut = (q31_t) (sum >> 31);
00578 /* Destination pointer is updated according to the address modifier, inc */
00579 pOut += inc;
00580
00581 /* Update the inputA and inputB pointers for next MAC calculation */
00582 px = ++pSrc1;
00583 py = pIn2;
00584
00585 /* Decrement the MAC count */
00586 count--;
00587
00588 /* Decrement the loop counter */
00589 blockSize3--;
00590 }
00591
00592 }
00593
All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines
Generated on Mon Nov 29 2010 17:19:56 for CMSIS DSP Software Library by
1.7.2
Wyszukiwarka
Podobne podstrony:
arm mult q31? sourcearm sqrt q31? sourcearm rms q31? sourcearm std q31? sourcearm sub q31? sourcearm rfft q31? sourcearm correlate ?2? sourcearm cos q31? sourcearm ?t4 q31? sourcearm shift q31? sourcearm correlate q7? sourcearm sin q31? sourcearm fill q31? sourcearm conv q31? sourcearm var q31? sourcearm mean q31? sourcearm ?s q31? sourcearm min q31? sourcearm correlate q31?więcej podobnych podstron