CMSIS DSP Software Library: arm_correlate_fast_q15.c Source File

- Main Page
- Modules
- Data Structures
- Files
- Examples

- File List
- Globals

# arm_correlate_fast_q15.c

Go to the documentation of this file.

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

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines