diff options
Diffstat (limited to 'fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c')
| -rw-r--r-- | fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c | 513 |
1 files changed, 513 insertions, 0 deletions
diff --git a/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c b/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c new file mode 100644 index 0000000..ff1d95b --- /dev/null +++ b/fw/midi-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c @@ -0,0 +1,513 @@ +#include "ref.h"
+
+void ref_correlate_f32(
+ float32_t * pSrcA,
+ uint32_t srcALen,
+ float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst)
+{
+ float32_t *pIn1 = pSrcA; /* inputA pointer */
+ float32_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ float32_t sum; /* Accumulator */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* The algorithm implementation is based on the lengths of the inputs.
+ * srcB is always made to slide across srcA.
+ * So srcBLen is always considered as shorter or equal to srcALen
+ * But CORR(x, y) is reverse of CORR(y, x)
+ * So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer
+ * and a variable, inv is set to 1
+ * If lengths are not equal then zero pad has to be done to make the two
+ * inputs of same length. But to improve the performance, we include zeroes
+ * in the output instead of zero padding either of the the inputs
+ * If srcALen > srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * starting of the output buffer
+ * If srcALen < srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * ending of the output buffer
+ * Once the zero padding is done the remaining of the output is calcualted
+ * using convolution but with the shorter signal time shifted.
+ */
+
+ /* Calculate the length of the remaining sequence */
+ tot = srcALen + srcBLen - 2U;
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ /* Initialise the pointer after zero padding */
+ pDst += srcALen - srcBLen;
+ }
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + srcALen - 1U;
+
+ /* Initialisation of the pointer after zero padding */
+ pDst += tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0.0f;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((i - j < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += pIn1[j] * pIn2[-((int32_t)i - j)];
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = sum;
+ else
+ *pDst++ = sum;
+ }
+}
+
+void ref_correlate_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst)
+{
+ q31_t *pIn1 = pSrcA; /* inputA pointer */
+ q31_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate correlation for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to correlation equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q63_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q31_t)(sum >> 31U);
+ else
+ *pDst++ = (q31_t)(sum >> 31U);
+ }
+}
+
+void ref_correlate_fast_q31(
+ q31_t * pSrcA,
+ uint32_t srcALen,
+ q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst)
+{
+ q31_t *pIn1 = pSrcA; /* inputA pointer */
+ q31_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate correlation for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to correlation equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) pIn1[j] * pIn2[-((int32_t) i - j)])) >> 32);
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q31_t)(sum << 1U);
+ else
+ *pDst++ = (q31_t)(sum << 1U);
+ }
+}
+
+void ref_correlate_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst)
+{
+ q15_t *pIn1 = pSrcA; /* inputA pointer */
+ q15_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q31_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q15_t) ref_sat_q15(sum >> 15U);
+ else
+ *pDst++ = (q15_t) ref_sat_q15(sum >> 15U);
+ }
+}
+
+void ref_correlate_fast_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst)
+{
+ q15_t *pIn1 = pSrcA; /* inputA pointer */
+ q15_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q31_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q15_t)(sum >> 15U);
+ else
+ *pDst++ = (q15_t)(sum >> 15U);
+ }
+}
+
+void ref_correlate_fast_opt_q15(
+ q15_t * pSrcA,
+ uint32_t srcALen,
+ q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch)
+{
+ q15_t *pIn1 = pSrcA; /* inputA pointer */
+ q15_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ q31_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q31_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q15_t) ref_sat_q15(sum >> 15U);
+ else
+ *pDst++ = (q15_t) ref_sat_q15(sum >> 15U);
+ }
+}
+
+void ref_correlate_q7(
+ q7_t * pSrcA,
+ uint32_t srcALen,
+ q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst)
+{
+ q7_t *pIn1 = pSrcA; /* inputA pointer */
+ q7_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ q31_t sum; /* Accumulator */
+ uint32_t i = 0U, j; /* loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q15_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q7_t) __SSAT((sum >> 7U), 8U);
+ else
+ *pDst++ = (q7_t) __SSAT((sum >> 7U), 8U);
+ }
+}
|