- 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