arm_correlate_f32.c

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_f32.c  
00009 *  
00010 * Description:   Correlation for floating-point sequences.  
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  
00033 /**  
00034  * @ingroup groupFilters  
00035  */ 
00036  
00037 /**  
00038  * @defgroup Corr Correlation  
00039  *  
00040  * Correlation is a mathematical operation that is similar to convolution.  
00041  * As with convolution, correlation uses two signals to produce a third signal.  
00042  * The underlying algorithms in correlation and convolution are identical except that one of the inputs is flipped in convolution.  
00043  * Correlation is commonly used to measure the similarity between two signals.  
00044  * It has applications in pattern recognition, cryptanalysis, and searching.  
00045  * The CMSIS library provides correlation functions for Q7, Q15, Q31 and floating-point data types.  
00046  * Fast versions of the Q15 and Q31 functions are also provided.  
00047  *  
00048  * \par Algorithm  
00049  * Let <code>a[n]</code> and <code>b[n]</code> be sequences of length <code>srcALen</code> and <code>srcBLen</code> samples respectively.  
00050  * The convolution of the two signals is denoted by  
00051  * <pre>  
00052  *                   c[n] = a[n] * b[n]  
00053  * </pre>  
00054  * In correlation, one of the signals is flipped in time  
00055  * <pre>  
00056  *                   c[n] = a[n] * b[-n]  
00057  * </pre>  
00058  *  
00059  * \par  
00060  * and this is mathematically defined as  
00061  * \image html CorrelateEquation.gif  
00062  * \par  
00063  * The <code>pSrcA</code> points to the first input vector of length <code>srcALen</code> and <code>pSrcB</code> points to the second input vector of length <code>srcBLen</code>.  
00064  * The result <code>c[n]</code> is of length <code>2 * max(srcALen, srcBLen) - 1</code> and is defined over the interval <code>n=0, 1, 2, ..., (2 * max(srcALen, srcBLen) - 2)</code>.  
00065  * The output result is written to <code>pDst</code> and the calling function must allocate <code>2 * max(srcALen, srcBLen) - 1</code> words for the result.  
00066  *  
00067  * <b>Fixed-Point Behavior</b>  
00068  * \par  
00069  * Correlation requires summing up a large number of intermediate products.  
00070  * As such, the Q7, Q15, and Q31 functions run a risk of overflow and saturation.  
00071  * Refer to the function specific documentation below for further details of the particular algorithm used.  
00072  */ 
00073  
00074 /**  
00075  * @addtogroup Corr  
00076  * @{  
00077  */ 
00078 /**  
00079  * @brief Correlation of floating-point sequences  
00080  * @param[in]  *pSrcA points to the first input sequence.  
00081  * @param[in]  srcALen length of the first input sequence.  
00082  * @param[in]  *pSrcB points to the second input sequence.  
00083  * @param[in]  srcBLen length of the second input sequence.  
00084  * @param[out] *pDst points to the location where the output result is written.  Length 2 * max(srcALen, srcBLen) - 1.  
00085  * @return none.  
00086  */ 
00087  
00088 void arm_correlate_f32( 
00089   float32_t * pSrcA, 
00090   uint32_t srcALen, 
00091   float32_t * pSrcB, 
00092   uint32_t srcBLen, 
00093   float32_t * pDst) 
00094 { 
00095   float32_t *pIn1;                               /* inputA pointer */ 
00096   float32_t *pIn2;                               /* inputB pointer */ 
00097   float32_t *pOut = pDst;                        /* output pointer */ 
00098   float32_t *px;                                 /* Intermediate inputA pointer */ 
00099   float32_t *py;                                 /* Intermediate inputB pointer */ 
00100   float32_t *pSrc1;                              /* Intermediate pointers */ 
00101   float32_t sum, acc0, acc1, acc2, acc3;         /* Accumulators */ 
00102   float32_t x0, x1, x2, x3, c0;                  /* temporary variables for holding input and coefficient values */ 
00103   uint32_t j, k = 0u, count, blkCnt, outBlockSize, blockSize1, blockSize2, blockSize3;  /* loop counters */ 
00104   int32_t inc = 1;                               /* Destination address modifier */ 
00105  
00106  
00107   /* The algorithm implementation is based on the lengths of the inputs. */ 
00108   /* srcB is always made to slide across srcA. */ 
00109   /* So srcBLen is always considered as shorter or equal to srcALen */ 
00110   /* But CORR(x, y) is reverse of CORR(y, x) */ 
00111   /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */ 
00112   /* and the destination pointer modifier, inc is set to -1 */ 
00113   /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */ 
00114   /* But to improve the performance,  
00115    * we include zeroes in the output instead of zero padding either of the the inputs*/ 
00116   /* If srcALen > srcBLen,  
00117    * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */ 
00118   /* If srcALen < srcBLen,  
00119    * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */ 
00120   if(srcALen >= srcBLen) 
00121   { 
00122     /* Initialization of inputA pointer */ 
00123     pIn1 = pSrcA; 
00124  
00125     /* Initialization of inputB pointer */ 
00126     pIn2 = pSrcB; 
00127  
00128     /* Number of output samples is calculated */ 
00129     outBlockSize = (2u * srcALen) - 1u; 
00130  
00131     /* When srcALen > srcBLen, zero padding has to be done to srcB  
00132      * to make their lengths equal.  
00133      * Instead, (outBlockSize - (srcALen + srcBLen - 1))  
00134      * number of output samples are made zero */ 
00135     j = outBlockSize - (srcALen + (srcBLen - 1u)); 
00136  
00137     while(j > 0u) 
00138     { 
00139       /* Zero is stored in the destination buffer */ 
00140       *pOut++ = 0.0f; 
00141  
00142       /* Decrement the loop counter */ 
00143       j--; 
00144     } 
00145  
00146   } 
00147   else 
00148   { 
00149     /* Initialization of inputA pointer */ 
00150     pIn1 = pSrcB; 
00151  
00152     /* Initialization of inputB pointer */ 
00153     pIn2 = pSrcA; 
00154  
00155     /* srcBLen is always considered as shorter or equal to srcALen */ 
00156     j = srcBLen; 
00157     srcBLen = srcALen; 
00158     srcALen = j; 
00159  
00160     /* CORR(x, y) = Reverse order(CORR(y, x)) */ 
00161     /* Hence set the destination pointer to point to the last output sample */ 
00162     pOut = pDst + ((srcALen + srcBLen) - 2u); 
00163  
00164     /* Destination address modifier is set to -1 */ 
00165     inc = -1; 
00166  
00167   } 
00168  
00169   /* The function is internally  
00170    * divided into three parts according to the number of multiplications that has to be  
00171    * taken place between inputA samples and inputB samples. In the first part of the  
00172    * algorithm, the multiplications increase by one for every iteration.  
00173    * In the second part of the algorithm, srcBLen number of multiplications are done.  
00174    * In the third part of the algorithm, the multiplications decrease by one  
00175    * for every iteration.*/ 
00176   /* The algorithm is implemented in three stages.  
00177    * The loop counters of each stage is initiated here. */ 
00178   blockSize1 = srcBLen - 1u; 
00179   blockSize2 = srcALen - (srcBLen - 1u); 
00180   blockSize3 = blockSize1; 
00181  
00182   /* --------------------------  
00183    * Initializations of stage1  
00184    * -------------------------*/ 
00185  
00186   /* sum = x[0] * y[srcBlen - 1]  
00187    * sum = x[0] * y[srcBlen-2] + x[1] * y[srcBlen - 1]  
00188    * ....  
00189    * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]  
00190    */ 
00191  
00192   /* In this stage the MAC operations are increased by 1 for every iteration.  
00193      The count variable holds the number of MAC operations performed */ 
00194   count = 1u; 
00195  
00196   /* Working pointer of inputA */ 
00197   px = pIn1; 
00198  
00199   /* Working pointer of inputB */ 
00200   pSrc1 = pIn2 + (srcBLen - 1u); 
00201   py = pSrc1; 
00202  
00203   /* ------------------------  
00204    * Stage1 process  
00205    * ----------------------*/ 
00206  
00207   /* The first stage starts here */ 
00208   while(blockSize1 > 0u) 
00209   { 
00210     /* Accumulator is made zero for every iteration */ 
00211     sum = 0.0f; 
00212  
00213     /* Apply loop unrolling and compute 4 MACs simultaneously. */ 
00214     k = count >> 2u; 
00215  
00216     /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.  
00217      ** a second loop below computes MACs for the remaining 1 to 3 samples. */ 
00218     while(k > 0u) 
00219     { 
00220       /* x[0] * y[srcBLen - 4] */ 
00221       sum += *px++ * *py++; 
00222       /* x[1] * y[srcBLen - 3] */ 
00223       sum += *px++ * *py++; 
00224       /* x[2] * y[srcBLen - 2] */ 
00225       sum += *px++ * *py++; 
00226       /* x[3] * y[srcBLen - 1] */ 
00227       sum += *px++ * *py++; 
00228  
00229       /* Decrement the loop counter */ 
00230       k--; 
00231     } 
00232  
00233     /* If the count is not a multiple of 4, compute any remaining MACs here.  
00234      ** No loop unrolling is used. */ 
00235     k = count % 0x4u; 
00236  
00237     while(k > 0u) 
00238     { 
00239       /* Perform the multiply-accumulate */ 
00240       /* x[0] * y[srcBLen - 1] */ 
00241       sum += *px++ * *py++; 
00242  
00243       /* Decrement the loop counter */ 
00244       k--; 
00245     } 
00246  
00247     /* Store the result in the accumulator in the destination buffer. */ 
00248     *pOut = sum; 
00249     /* Destination pointer is updated according to the address modifier, inc */ 
00250     pOut += inc; 
00251  
00252     /* Update the inputA and inputB pointers for next MAC calculation */ 
00253     py = pSrc1 - count; 
00254     px = pIn1; 
00255  
00256     /* Increment the MAC count */ 
00257     count++; 
00258  
00259     /* Decrement the loop counter */ 
00260     blockSize1--; 
00261   } 
00262  
00263   /* --------------------------  
00264    * Initializations of stage2  
00265    * ------------------------*/ 
00266  
00267   /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]  
00268    * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]  
00269    * ....  
00270    * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]  
00271    */ 
00272  
00273   /* Working pointer of inputA */ 
00274   px = pIn1; 
00275  
00276   /* Working pointer of inputB */ 
00277   py = pIn2; 
00278  
00279   /* count is index by which the pointer pIn1 to be incremented */ 
00280   count = 1u; 
00281  
00282   /* -------------------  
00283    * Stage2 process  
00284    * ------------------*/ 
00285  
00286   /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.  
00287    * So, to loop unroll over blockSize2,  
00288    * srcBLen should be greater than or equal to 4, to loop unroll the srcBLen loop */ 
00289   if(srcBLen >= 4u) 
00290   { 
00291     /* Loop unroll over blockSize2, by 4 */ 
00292     blkCnt = blockSize2 >> 2u; 
00293  
00294     while(blkCnt > 0u) 
00295     { 
00296       /* Set all accumulators to zero */ 
00297       acc0 = 0.0f; 
00298       acc1 = 0.0f; 
00299       acc2 = 0.0f; 
00300       acc3 = 0.0f; 
00301  
00302       /* read x[0], x[1], x[2] samples */ 
00303       x0 = *(px++); 
00304       x1 = *(px++); 
00305       x2 = *(px++); 
00306  
00307       /* Apply loop unrolling and compute 4 MACs simultaneously. */ 
00308       k = srcBLen >> 2u; 
00309  
00310       /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.  
00311        ** a second loop below computes MACs for the remaining 1 to 3 samples. */ 
00312       do 
00313       { 
00314         /* Read y[0] sample */ 
00315         c0 = *(py++); 
00316  
00317         /* Read x[3] sample */ 
00318         x3 = *(px++); 
00319  
00320         /* Perform the multiply-accumulate */ 
00321         /* acc0 +=  x[0] * y[0] */ 
00322         acc0 += x0 * c0; 
00323         /* acc1 +=  x[1] * y[0] */ 
00324         acc1 += x1 * c0; 
00325         /* acc2 +=  x[2] * y[0] */ 
00326         acc2 += x2 * c0; 
00327         /* acc3 +=  x[3] * y[0] */ 
00328         acc3 += x3 * c0; 
00329  
00330         /* Read y[1] sample */ 
00331         c0 = *(py++); 
00332  
00333         /* Read x[4] sample */ 
00334         x0 = *(px++); 
00335  
00336         /* Perform the multiply-accumulate */ 
00337         /* acc0 +=  x[1] * y[1] */ 
00338         acc0 += x1 * c0; 
00339         /* acc1 +=  x[2] * y[1] */ 
00340         acc1 += x2 * c0; 
00341         /* acc2 +=  x[3] * y[1] */ 
00342         acc2 += x3 * c0; 
00343         /* acc3 +=  x[4] * y[1] */ 
00344         acc3 += x0 * c0; 
00345  
00346         /* Read y[2] sample */ 
00347         c0 = *(py++); 
00348  
00349         /* Read x[5] sample */ 
00350         x1 = *(px++); 
00351  
00352         /* Perform the multiply-accumulates */ 
00353         /* acc0 +=  x[2] * y[2] */ 
00354         acc0 += x2 * c0; 
00355         /* acc1 +=  x[3] * y[2] */ 
00356         acc1 += x3 * c0; 
00357         /* acc2 +=  x[4] * y[2] */ 
00358         acc2 += x0 * c0; 
00359         /* acc3 +=  x[5] * y[2] */ 
00360         acc3 += x1 * c0; 
00361  
00362         /* Read y[3] sample */ 
00363         c0 = *(py++); 
00364  
00365         /* Read x[6] sample */ 
00366         x2 = *(px++); 
00367  
00368         /* Perform the multiply-accumulates */ 
00369         /* acc0 +=  x[3] * y[3] */ 
00370         acc0 += x3 * c0; 
00371         /* acc1 +=  x[4] * y[3] */ 
00372         acc1 += x0 * c0; 
00373         /* acc2 +=  x[5] * y[3] */ 
00374         acc2 += x1 * c0; 
00375         /* acc3 +=  x[6] * y[3] */ 
00376         acc3 += x2 * c0; 
00377  
00378  
00379       } while(--k); 
00380  
00381       /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.  
00382        ** No loop unrolling is used. */ 
00383       k = srcBLen % 0x4u; 
00384  
00385       while(k > 0u) 
00386       { 
00387         /* Read y[4] sample */ 
00388         c0 = *(py++); 
00389  
00390         /* Read x[7] sample */ 
00391         x3 = *(px++); 
00392  
00393         /* Perform the multiply-accumulates */ 
00394         /* acc0 +=  x[4] * y[4] */ 
00395         acc0 += x0 * c0; 
00396         /* acc1 +=  x[5] * y[4] */ 
00397         acc1 += x1 * c0; 
00398         /* acc2 +=  x[6] * y[4] */ 
00399         acc2 += x2 * c0; 
00400         /* acc3 +=  x[7] * y[4] */ 
00401         acc3 += x3 * c0; 
00402  
00403         /* Reuse the present samples for the next MAC */ 
00404         x0 = x1; 
00405         x1 = x2; 
00406         x2 = x3; 
00407  
00408         /* Decrement the loop counter */ 
00409         k--; 
00410       } 
00411  
00412       /* Store the result in the accumulator in the destination buffer. */ 
00413       *pOut = acc0; 
00414       /* Destination pointer is updated according to the address modifier, inc */ 
00415       pOut += inc; 
00416  
00417       *pOut = acc1; 
00418       pOut += inc; 
00419  
00420       *pOut = acc2; 
00421       pOut += inc; 
00422  
00423       *pOut = acc3; 
00424       pOut += inc; 
00425  
00426       /* Update the inputA and inputB pointers for next MAC calculation */ 
00427       px = pIn1 + (count * 4u); 
00428       py = pIn2; 
00429  
00430       /* Increment the pointer pIn1 index, count by 1 */ 
00431       count++; 
00432  
00433       /* Decrement the loop counter */ 
00434       blkCnt--; 
00435     } 
00436  
00437     /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.  
00438      ** No loop unrolling is used. */ 
00439     blkCnt = blockSize2 % 0x4u; 
00440  
00441     while(blkCnt > 0u) 
00442     { 
00443       /* Accumulator is made zero for every iteration */ 
00444       sum = 0.0f; 
00445  
00446       /* Apply loop unrolling and compute 4 MACs simultaneously. */ 
00447       k = srcBLen >> 2u; 
00448  
00449       /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.  
00450        ** a second loop below computes MACs for the remaining 1 to 3 samples. */ 
00451       while(k > 0u) 
00452       { 
00453         /* Perform the multiply-accumulates */ 
00454         sum += *px++ * *py++; 
00455         sum += *px++ * *py++; 
00456         sum += *px++ * *py++; 
00457         sum += *px++ * *py++; 
00458  
00459         /* Decrement the loop counter */ 
00460         k--; 
00461       } 
00462  
00463       /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.  
00464        ** No loop unrolling is used. */ 
00465       k = srcBLen % 0x4u; 
00466  
00467       while(k > 0u) 
00468       { 
00469         /* Perform the multiply-accumulate */ 
00470         sum += *px++ * *py++; 
00471  
00472         /* Decrement the loop counter */ 
00473         k--; 
00474       } 
00475  
00476       /* Store the result in the accumulator in the destination buffer. */ 
00477       *pOut = sum; 
00478       /* Destination pointer is updated according to the address modifier, inc */ 
00479       pOut += inc; 
00480  
00481       /* Update the inputA and inputB pointers for next MAC calculation */ 
00482       px = pIn1 + count; 
00483       py = pIn2; 
00484  
00485       /* Increment the pointer pIn1 index, count by 1 */ 
00486       count++; 
00487  
00488       /* Decrement the loop counter */ 
00489       blkCnt--; 
00490     } 
00491   } 
00492   else 
00493   { 
00494     /* If the srcBLen is not a multiple of 4,  
00495      * the blockSize2 loop cannot be unrolled by 4 */ 
00496     blkCnt = blockSize2; 
00497  
00498     while(blkCnt > 0u) 
00499     { 
00500       /* Accumulator is made zero for every iteration */ 
00501       sum = 0.0f; 
00502  
00503       /* Loop over srcBLen */ 
00504       k = srcBLen; 
00505  
00506       while(k > 0u) 
00507       { 
00508         /* Perform the multiply-accumulate */ 
00509         sum += *px++ * *py++; 
00510  
00511         /* Decrement the loop counter */ 
00512         k--; 
00513       } 
00514  
00515       /* Store the result in the accumulator in the destination buffer. */ 
00516       *pOut = sum; 
00517       /* Destination pointer is updated according to the address modifier, inc */ 
00518       pOut += inc; 
00519  
00520       /* Update the inputA and inputB pointers for next MAC calculation */ 
00521       px = pIn1 + count; 
00522       py = pIn2; 
00523  
00524       /* Increment the pointer pIn1 index, count by 1 */ 
00525       count++; 
00526  
00527       /* Decrement the loop counter */ 
00528       blkCnt--; 
00529     } 
00530   } 
00531  
00532   /* --------------------------  
00533    * Initializations of stage3  
00534    * -------------------------*/ 
00535  
00536   /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]  
00537    * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]  
00538    * ....  
00539    * sum +=  x[srcALen-2] * y[0] + x[srcALen-1] * y[1]  
00540    * sum +=  x[srcALen-1] * y[0]  
00541    */ 
00542  
00543   /* In this stage the MAC operations are decreased by 1 for every iteration.  
00544      The count variable holds the number of MAC operations performed */ 
00545   count = srcBLen - 1u; 
00546  
00547   /* Working pointer of inputA */ 
00548   pSrc1 = pIn1 + (srcALen - (srcBLen - 1u)); 
00549   px = pSrc1; 
00550  
00551   /* Working pointer of inputB */ 
00552   py = pIn2; 
00553  
00554   /* -------------------  
00555    * Stage3 process  
00556    * ------------------*/ 
00557  
00558   while(blockSize3 > 0u) 
00559   { 
00560     /* Accumulator is made zero for every iteration */ 
00561     sum = 0.0f; 
00562  
00563     /* Apply loop unrolling and compute 4 MACs simultaneously. */ 
00564     k = count >> 2u; 
00565  
00566     /* First part of the processing with loop unrolling.  Compute 4 MACs at a time.  
00567      ** a second loop below computes MACs for the remaining 1 to 3 samples. */ 
00568     while(k > 0u) 
00569     { 
00570       /* Perform the multiply-accumulates */ 
00571       /* sum += x[srcALen - srcBLen + 4] * y[3] */ 
00572       sum += *px++ * *py++; 
00573       /* sum += x[srcALen - srcBLen + 3] * y[2] */ 
00574       sum += *px++ * *py++; 
00575       /* sum += x[srcALen - srcBLen + 2] * y[1] */ 
00576       sum += *px++ * *py++; 
00577       /* sum += x[srcALen - srcBLen + 1] * y[0] */ 
00578       sum += *px++ * *py++; 
00579  
00580       /* Decrement the loop counter */ 
00581       k--; 
00582     } 
00583  
00584     /* If the count is not a multiple of 4, compute any remaining MACs here.  
00585      ** No loop unrolling is used. */ 
00586     k = count % 0x4u; 
00587  
00588     while(k > 0u) 
00589     { 
00590       /* Perform the multiply-accumulates */ 
00591       sum += *px++ * *py++; 
00592  
00593       /* Decrement the loop counter */ 
00594       k--; 
00595     } 
00596  
00597     /* Store the result in the accumulator in the destination buffer. */ 
00598     *pOut = sum; 
00599     /* Destination pointer is updated according to the address modifier, inc */ 
00600     pOut += inc; 
00601  
00602     /* Update the inputA and inputB pointers for next MAC calculation */ 
00603     px = ++pSrc1; 
00604     py = pIn2; 
00605  
00606     /* Decrement the MAC count */ 
00607     count--; 
00608  
00609     /* Decrement the loop counter */ 
00610     blockSize3--; 
00611   } 
00612  
00613 } 
00614  
00615 /**  
00616  * @} end of Corr group  
00617  */