diff options
Diffstat (limited to 'fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions')
10 files changed, 0 insertions, 4270 deletions
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/biquad.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/biquad.c deleted file mode 100644 index 1fe7c54..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/biquad.c +++ /dev/null @@ -1,713 +0,0 @@ -#include "ref.h"
-
-void ref_biquad_cascade_df2T_f32(
- const arm_biquad_cascade_df2T_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pIn = pSrc; /* source pointer */
- float32_t *pOut = pDst; /* destination pointer */
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
- float32_t acc; /* accumulator */
- float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
- float32_t Xn; /* temporary input */
- float32_t d1, d2; /* state variables */
- uint32_t sample, stage = S->numStages; /* loop counters */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /*Reading the state values */
- d1 = pState[0];
- d2 = pState[1];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* y[n] = b0 * x[n] + d1 */
- acc = (b0 * Xn) + d1;
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut++ = acc;
-
- /* Every time after the output is computed state should be updated. */
- /* d1 = b1 * x[n] + a1 * y[n] + d2 */
- d1 = (b1 * Xn + a1 * acc) + d2;
-
- /* d2 = b2 * x[n] + a2 * y[n] */
- d2 = (b2 * Xn) + (a2 * acc);
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* Store the updated state variables back into the state array */
- *pState++ = d1;
- *pState++ = d2;
-
- /* The current stage input is given as the output to the next stage */
- pIn = pDst;
-
- /*Reset the output working pointer */
- pOut = pDst;
-
- /* decrement the loop counter */
- stage--;
-
- } while (stage > 0U);
-}
-
-
-void ref_biquad_cascade_stereo_df2T_f32(
- const arm_biquad_cascade_stereo_df2T_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pIn = pSrc; /* source pointer */
- float32_t *pOut = pDst; /* destination pointer */
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
- float32_t acc1a, acc1b; /* accumulator */
- float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
- float32_t Xn1a, Xn1b; /* temporary input */
- float32_t d1a, d2a, d1b, d2b; /* state variables */
- uint32_t sample, stage = S->numStages; /* loop counters */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /*Reading the state values */
- d1a = pState[0];
- d2a = pState[1];
- d1b = pState[2];
- d2b = pState[3];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn1a = *pIn++; //Channel a
- Xn1b = *pIn++; //Channel b
-
- /* y[n] = b0 * x[n] + d1 */
- acc1a = (b0 * Xn1a) + d1a;
- acc1b = (b0 * Xn1b) + d1b;
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut++ = acc1a;
- *pOut++ = acc1b;
-
- /* Every time after the output is computed state should be updated. */
- /* d1 = b1 * x[n] + a1 * y[n] + d2 */
- d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
- d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
-
- /* d2 = b2 * x[n] + a2 * y[n] */
- d2a = (b2 * Xn1a) + (a2 * acc1a);
- d2b = (b2 * Xn1b) + (a2 * acc1b);
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* Store the updated state variables back into the state array */
- *pState++ = d1a;
- *pState++ = d2a;
- *pState++ = d1b;
- *pState++ = d2b;
-
- /* The current stage input is given as the output to the next stage */
- pIn = pDst;
-
- /*Reset the output working pointer */
- pOut = pDst;
-
- /* decrement the loop counter */
- stage--;
-
- } while (stage > 0U);
-
-}
-
-void ref_biquad_cascade_df2T_f64(
- const arm_biquad_cascade_df2T_instance_f64 * S,
- float64_t * pSrc,
- float64_t * pDst,
- uint32_t blockSize)
-{
- float64_t *pIn = pSrc; /* source pointer */
- float64_t *pOut = pDst; /* destination pointer */
- float64_t *pState = S->pState; /* State pointer */
- float64_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
- float64_t acc; /* accumulator */
- float64_t b0, b1, b2, a1, a2; /* Filter coefficients */
- float64_t Xn; /* temporary input */
- float64_t d1, d2; /* state variables */
- uint32_t sample, stage = S->numStages; /* loop counters */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /*Reading the state values */
- d1 = pState[0];
- d2 = pState[1];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* y[n] = b0 * x[n] + d1 */
- acc = (b0 * Xn) + d1;
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut++ = acc;
-
- /* Every time after the output is computed state should be updated. */
- /* d1 = b1 * x[n] + a1 * y[n] + d2 */
- d1 = (b1 * Xn + a1 * acc) + d2;
-
- /* d2 = b2 * x[n] + a2 * y[n] */
- d2 = (b2 * Xn) + (a2 * acc);
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* Store the updated state variables back into the state array */
- *pState++ = d1;
- *pState++ = d2;
-
- /* The current stage input is given as the output to the next stage */
- pIn = pDst;
-
- /*Reset the output working pointer */
- pOut = pDst;
-
- /* decrement the loop counter */
- stage--;
-
- } while (stage > 0U);
-}
-
-void ref_biquad_cascade_df1_f32(
- const arm_biquad_casd_df1_inst_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pIn = pSrc; /* source pointer */
- float32_t *pOut = pDst; /* destination pointer */
- float32_t *pState = S->pState; /* pState pointer */
- float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
- float32_t acc; /* Simulates the accumulator */
- float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
- float32_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */
- float32_t Xn; /* temporary input */
- uint32_t sample, stage = S->numStages; /* loop counters */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /* Reading the pState values */
- Xn1 = pState[0];
- Xn2 = pState[1];
- Yn1 = pState[2];
- Yn2 = pState[3];
-
- /* The variables acc holds the output value that is computed:
- * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
- */
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
- acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
-
- /* Store the result in the accumulator in the destination buffer. */
- *pOut++ = acc;
-
- /* Every time after the output is computed state should be updated. */
- /* The states should be updated as: */
- /* Xn2 = Xn1 */
- /* Xn1 = Xn */
- /* Yn2 = Yn1 */
- /* Yn1 = acc */
- Xn2 = Xn1;
- Xn1 = Xn;
- Yn2 = Yn1;
- Yn1 = acc;
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* Store the updated state variables back into the pState array */
- *pState++ = Xn1;
- *pState++ = Xn2;
- *pState++ = Yn1;
- *pState++ = Yn2;
-
- /* The first stage goes from the input buffer to the output buffer. */
- /* Subsequent numStages occur in-place in the output buffer */
- pIn = pDst;
-
- /* Reset the output pointer */
- pOut = pDst;
-
- /* decrement the loop counter */
- stage--;
-
- } while (stage > 0U);
-}
-
-void ref_biquad_cas_df1_32x64_q31(
- const arm_biquad_cas_df1_32x64_ins_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pIn = pSrc; /* input pointer initialization */
- q31_t *pOut = pDst; /* output pointer initialization */
- q63_t *pState = S->pState; /* state pointer initialization */
- q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
- q63_t acc; /* accumulator */
- q31_t Xn1, Xn2; /* Input Filter state variables */
- q63_t Yn1, Yn2; /* Output Filter state variables */
- q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
- q31_t Xn; /* temporary input */
- int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
- uint32_t sample, stage = S->numStages; /* loop counters */
- q31_t acc_l, acc_h; /* temporary output */
- uint32_t uShift = ((uint32_t) S->postShift + 1U);
- uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /* Reading the state values */
- Xn1 = pState[0];
- Xn2 = pState[1];
- Yn1 = pState[2];
- Yn2 = pState[3];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
- acc = (q63_t)Xn*b0 + (q63_t)Xn1*b1 + (q63_t)Xn2*b2;
- /* acc += a1 * y[n-1] */
- acc += mult32x64(Yn1, a1);
- /* acc += a2 * y[n-2] */
- acc += mult32x64(Yn2, a2);
-
- /* Every time after the output is computed state should be updated. */
- Xn2 = Xn1;
- Xn1 = Xn;
- Yn2 = Yn1;
-
- /* The result is converted to 1.63, Yn1 variable is reused */
- Yn1 = acc << shift;
-
- /* Calc lower part of acc */
- acc_l = acc & 0xffffffff;
-
- /* Calc upper part of acc */
- acc_h = (acc >> 32) & 0xffffffff;
-
- /* Apply shift for lower part of acc and upper part of acc */
- acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
-
- /* Store the output in the destination buffer in 1.31 format. */
- *pOut++ = acc_h;
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* The first stage output is given as input to the second stage. */
- pIn = pDst;
-
- /* Reset to destination buffer working pointer */
- pOut = pDst;
-
- /* Store the updated state variables back into the pState array */
- *pState++ = (q63_t) Xn1;
- *pState++ = (q63_t) Xn2;
- *pState++ = Yn1;
- *pState++ = Yn2;
-
- } while (--stage);
-}
-
-void ref_biquad_cascade_df1_q31(
- const arm_biquad_casd_df1_inst_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q63_t acc; /* accumulator */
- uint32_t uShift = ((uint32_t) S->postShift + 1U);
- uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
- q31_t *pIn = pSrc; /* input pointer initialization */
- q31_t *pOut = pDst; /* output pointer initialization */
- q31_t *pState = S->pState; /* pState pointer initialization */
- q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
- q31_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */
- q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
- q31_t Xn; /* temporary input */
- uint32_t sample, stage = S->numStages; /* loop counters */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /* Reading the state values */
- Xn1 = pState[0];
- Xn2 = pState[1];
- Yn1 = pState[2];
- Yn2 = pState[3];
-
- /* The variables acc holds the output value that is computed:
- * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
- */
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
- /* acc = b0 * x[n] */
- acc = (q63_t) b0 *Xn;
-
- /* acc += b1 * x[n-1] */
- acc += (q63_t) b1 *Xn1;
- /* acc += b[2] * x[n-2] */
- acc += (q63_t) b2 *Xn2;
- /* acc += a1 * y[n-1] */
- acc += (q63_t) a1 *Yn1;
- /* acc += a2 * y[n-2] */
- acc += (q63_t) a2 *Yn2;
-
- /* The result is converted to 1.31 */
- acc = acc >> lShift;
-
- /* Every time after the output is computed state should be updated. */
- /* The states should be updated as: */
- /* Xn2 = Xn1 */
- /* Xn1 = Xn */
- /* Yn2 = Yn1 */
- /* Yn1 = acc */
- Xn2 = Xn1;
- Xn1 = Xn;
- Yn2 = Yn1;
- Yn1 = (q31_t) acc;
-
- /* Store the output in the destination buffer. */
- *pOut++ = (q31_t) acc;
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* The first stage goes from the input buffer to the output buffer. */
- /* Subsequent stages occur in-place in the output buffer */
- pIn = pDst;
-
- /* Reset to destination pointer */
- pOut = pDst;
-
- /* Store the updated state variables back into the pState array */
- *pState++ = Xn1;
- *pState++ = Xn2;
- *pState++ = Yn1;
- *pState++ = Yn2;
-
- } while (--stage);
-}
-
-
-void ref_biquad_cascade_df1_fast_q31(
- const arm_biquad_casd_df1_inst_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t acc = 0; /* accumulator */
- q31_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */
- q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
- q31_t *pIn = pSrc; /* input pointer initialization */
- q31_t *pOut = pDst; /* output pointer initialization */
- q31_t *pState = S->pState; /* pState pointer initialization */
- q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
- q31_t Xn; /* temporary input */
- int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
- uint32_t sample, stage = S->numStages; /* loop counters */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /* Reading the state values */
- Xn1 = pState[0];
- Xn2 = pState[1];
- Yn1 = pState[2];
- Yn2 = pState[3];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
- mult_32x32_keep32_R(acc, b0, Xn);
- multAcc_32x32_keep32_R(acc, b1, Xn1);
- multAcc_32x32_keep32_R(acc, b2, Xn2);
- multAcc_32x32_keep32_R(acc, a1, Yn1);
- multAcc_32x32_keep32_R(acc, a2, Yn2);
-
- /* The result is converted to 1.31 */
- acc <<= shift;
-
- /* Every time after the output is computed state should be updated. */
- Xn2 = Xn1;
- Xn1 = Xn;
- Yn2 = Yn1;
- Yn1 = acc;
-
- /* Store the output in the destination buffer. */
- *pOut++ = acc;
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* The first stage goes from the input buffer to the output buffer. */
- /* Subsequent stages occur in-place in the output buffer */
- pIn = pDst;
-
- /* Reset to destination pointer */
- pOut = pDst;
-
- /* Store the updated state variables back into the pState array */
- *pState++ = Xn1;
- *pState++ = Xn2;
- *pState++ = Yn1;
- *pState++ = Yn2;
-
- } while (--stage);
-}
-
-void ref_biquad_cascade_df1_fast_q15(
- const arm_biquad_casd_df1_inst_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pIn = pSrc; /* Source pointer */
- q15_t *pOut = pDst; /* Destination pointer */
- q15_t b0, b1, b2, a1, a2; /* Filter coefficients */
- q15_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */
- q15_t Xn; /* temporary input */
- q31_t acc; /* Accumulator */
- int32_t shift = (15 - (int32_t) S->postShift); /* Post shift */
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- uint32_t sample, stage = (uint32_t) S->numStages; /* Stage loop counter */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- pCoeffs++; // skip the 0 coefficient
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /* Reading the state values */
- Xn1 = pState[0];
- Xn2 = pState[1];
- Yn1 = pState[2];
- Yn2 = pState[3];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
- acc = (q31_t)b0*Xn + (q31_t)b1*Xn1 + (q31_t)b2*Xn2 + (q31_t)a1*Yn1 + (q31_t)a2*Yn2;
-
- /* The result is converted to 1.15 */
- acc = ref_sat_q15(acc >> shift);
-
- /* Every time after the output is computed state should be updated. */
- Xn2 = Xn1;
- Xn1 = Xn;
- Yn2 = Yn1;
- Yn1 = (q15_t) acc;
-
- /* Store the output in the destination buffer. */
- *pOut++ = (q15_t) acc;
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* The first stage goes from the input buffer to the output buffer. */
- /* Subsequent stages occur in-place in the output buffer */
- pIn = pDst;
-
- /* Reset to destination pointer */
- pOut = pDst;
-
- /* Store the updated state variables back into the pState array */
- *pState++ = Xn1;
- *pState++ = Xn2;
- *pState++ = Yn1;
- *pState++ = Yn2;
-
- } while (--stage);
-}
-
-void ref_biquad_cascade_df1_q15(
- const arm_biquad_casd_df1_inst_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pIn = pSrc; /* Source pointer */
- q15_t *pOut = pDst; /* Destination pointer */
- q15_t b0, b1, b2, a1, a2; /* Filter coefficients */
- q15_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */
- q15_t Xn; /* temporary input */
- q63_t acc; /* Accumulator */
- int32_t shift = (15 - (int32_t) S->postShift); /* Post shift */
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- uint32_t sample, stage = (uint32_t) S->numStages; /* Stage loop counter */
-
- do
- {
- /* Reading the coefficients */
- b0 = *pCoeffs++;
- pCoeffs++; // skip the 0 coefficient
- b1 = *pCoeffs++;
- b2 = *pCoeffs++;
- a1 = *pCoeffs++;
- a2 = *pCoeffs++;
-
- /* Reading the state values */
- Xn1 = pState[0];
- Xn2 = pState[1];
- Yn1 = pState[2];
- Yn2 = pState[3];
-
- sample = blockSize;
-
- while (sample > 0U)
- {
- /* Read the input */
- Xn = *pIn++;
-
- /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
- acc = (q31_t)b0*Xn + (q31_t)b1*Xn1 + (q31_t)b2*Xn2 + (q31_t)a1*Yn1 + (q31_t)a2*Yn2;
-
- /* The result is converted to 1.15 */
- acc = ref_sat_q15(acc >> shift);
-
- /* Every time after the output is computed state should be updated. */
- Xn2 = Xn1;
- Xn1 = Xn;
- Yn2 = Yn1;
- Yn1 = (q15_t) acc;
-
- /* Store the output in the destination buffer. */
- *pOut++ = (q15_t) acc;
-
- /* decrement the loop counter */
- sample--;
- }
-
- /* The first stage goes from the input buffer to the output buffer. */
- /* Subsequent stages occur in-place in the output buffer */
- pIn = pDst;
-
- /* Reset to destination pointer */
- pOut = pDst;
-
- /* Store the updated state variables back into the pState array */
- *pState++ = Xn1;
- *pState++ = Xn2;
- *pState++ = Yn1;
- *pState++ = Yn2;
-
- } while (--stage);
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/conv.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/conv.c deleted file mode 100644 index dc1b103..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/conv.c +++ /dev/null @@ -1,350 +0,0 @@ -#include "ref.h"
-
-void ref_conv_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst)
-{
- float32_t sum; /* Accumulator */
- uint32_t i, j; /* loop counters */
-
- /* Loop to calculate convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry out MAC operations */
- sum = 0.0f;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += pSrcB[i - j] * pSrcA[j];
- }
- }
- /* Store the output in the destination buffer */
- pDst[i] = sum;
- }
-}
-
-arm_status ref_conv_partial_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints)
-{
- ref_conv_f32(pSrcA,srcALen,pSrcB,srcBLen,pDst);
-
- return ARM_MATH_SUCCESS;
-}
-
-void ref_conv_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst)
-{
- q63_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += (q63_t) pSrcA[j] * (pSrcB[i - j]);
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = (q31_t)(sum >> 31U);
- }
-}
-
-void ref_conv_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst)
-{
- q31_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; 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)pSrcA[j] * pSrcB[i - j])) >> 32);
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = (q31_t)(sum << 1U);
- }
-}
-
-arm_status ref_conv_partial_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints)
-{
- ref_conv_q31(pSrcA,srcALen,pSrcB,srcBLen,pDst);
-
- return ARM_MATH_SUCCESS;
-}
-
-arm_status ref_conv_partial_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints)
-{
- ref_conv_fast_q31(pSrcA,srcALen,pSrcB,srcBLen,pDst);
-
- return ARM_MATH_SUCCESS;
-}
-
-void ref_conv_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst)
-{
- q63_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += (q31_t)pSrcA[j] * pSrcB[i - j];
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = ref_sat_q15(sum >> 15U);
- }
-}
-
-arm_status ref_conv_partial_fast_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints,
- q15_t * pScratch1,
- q15_t * pScratch2)
-{
- q31_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += (q31_t)pSrcA[j] * pSrcB[i - j];
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = ref_sat_q15(sum >> 15U);
- }
-
- return ARM_MATH_SUCCESS;
-}
-
-void ref_conv_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst)
-{
- q31_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += (q31_t)pSrcA[j] * pSrcB[i - j];
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = sum >> 15U;
- }
-}
-
-void ref_conv_fast_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- q15_t * pScratch1,
- q15_t * pScratch2)
-{
- q31_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += (q31_t)pSrcA[j] * pSrcB[i - j];
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = ref_sat_q15(sum >> 15U);
- }
-}
-
-arm_status ref_conv_partial_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints)
-{
- ref_conv_q15(pSrcA,srcALen,pSrcB,srcBLen,pDst);
-
- return ARM_MATH_SUCCESS;
-}
-
-arm_status ref_conv_partial_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints)
-{
- ref_conv_fast_q15(pSrcA,srcALen,pSrcB,srcBLen,pDst);
-
- return ARM_MATH_SUCCESS;
-}
-
-
-void ref_conv_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst)
-{
- q31_t sum; /* Accumulator */
- uint32_t i, j; /* loop counter */
-
- /* Loop to calculate output of convolution for output length number of times */
- for (i = 0; i < srcALen + srcBLen - 1; i++)
- {
- /* Initialize sum with zero to carry on MAC operations */
- sum = 0;
-
- /* Loop to perform MAC operations according to convolution equation */
- for (j = 0; j <= i; j++)
- {
- /* Check the array limitations */
- if ((i - j < srcBLen) && (j < srcALen))
- {
- /* z[i] += x[i-j] * y[j] */
- sum += (q15_t)pSrcA[j] * pSrcB[i - j];
- }
- }
-
- /* Store the output in the destination buffer */
- pDst[i] = (q7_t)ref_sat_q7(sum >> 7);
- }
-}
-
-arm_status ref_conv_partial_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints)
-{
- ref_conv_q7(pSrcA,srcALen,pSrcB,srcBLen,pDst);
-
- return ARM_MATH_SUCCESS;
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c deleted file mode 100644 index ff1d95b..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/correlate.c +++ /dev/null @@ -1,513 +0,0 @@ -#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);
- }
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir.c deleted file mode 100644 index 3e72b87..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir.c +++ /dev/null @@ -1,325 +0,0 @@ -#include "ref.h"
-
-void ref_fir_f32(
- const arm_fir_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *pStateCurnt; /* Points to the current sample of the state */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i; /* Loop counters */
- float32_t acc;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- while (blockSize > 0U)
- {
- /* Copy one sample at a time into state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- acc = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulates */
- acc += pState[i] * pCoeffs[i];
- }
-
- /* The result is store in the destination buffer. */
- *pDst++ = acc;
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- blockSize--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy data */
- for(i=0;i<numTaps-1;i++)
- {
- pStateCurnt[i] = pState[i];
- }
-}
-
-void ref_fir_q31(
- const arm_fir_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i; /* Loop counters */
- q63_t acc;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- while (blockSize > 0U)
- {
- /* Copy one sample at a time into state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- acc = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulates */
- acc += (q63_t)pState[i] * pCoeffs[i];
- }
-
- /* The result is store in the destination buffer. */
- *pDst++ = (q31_t)(acc >> 31);
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- blockSize--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy data */
- for(i=0;i<numTaps-1;i++)
- {
- pStateCurnt[i] = pState[i];
- }
-}
-
-void ref_fir_fast_q31(
- const arm_fir_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i; /* Loop counters */
- q31_t acc;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- while (blockSize > 0U)
- {
- /* Copy one sample at a time into state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- acc = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulates */
- acc = (q31_t) (((((q63_t) acc) << 32) + ((q63_t) pState[i] * pCoeffs[i]) + 0x80000000LL ) >> 32);
- }
-
- /* The result is store in the destination buffer. */
- *pDst++ = (q31_t)(acc << 1);
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- blockSize--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy data */
- for(i=0;i<numTaps-1;i++)
- {
- pStateCurnt[i] = pState[i];
- }
-}
-
-void ref_fir_q15(
- const arm_fir_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i; /* Loop counters */
- q63_t acc;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- while (blockSize > 0U)
- {
- /* Copy one sample at a time into state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- acc = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulates */
- acc += (q31_t)pState[i] * pCoeffs[i];
- }
-
- /* The result is store in the destination buffer. */
- *pDst++ = ref_sat_q15(acc >> 15);
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- blockSize--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy data */
- for(i=0;i<numTaps;i++)
- {
- pStateCurnt[i] = pState[i];
- }
-}
-
-void ref_fir_fast_q15(
- const arm_fir_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i; /* Loop counters */
- q31_t acc;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- while (blockSize > 0U)
- {
- /* Copy one sample at a time into state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- acc = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulates */
- acc += (q31_t)pState[i] * pCoeffs[i];
- }
-
- /* The result is store in the destination buffer. */
- *pDst++ = ref_sat_q15(acc >> 15);
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- blockSize--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy data */
- for(i=0;i<numTaps-1;i++)
- {
- pStateCurnt[i] = pState[i];
- }
-}
-
-void ref_fir_q7(
- const arm_fir_instance_q7 * S,
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize)
-{
- q7_t *pState = S->pState; /* State pointer */
- q7_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q7_t *pStateCurnt; /* Points to the current sample of the state */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i; /* Loop counters */
- q31_t acc;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- while (blockSize > 0U)
- {
- /* Copy one sample at a time into state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- acc = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulates */
- acc += (q31_t)pState[i] * pCoeffs[i];
- }
-
- /* The result is store in the destination buffer. */
- *pDst++ = ref_sat_q7(acc >> 7);
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- blockSize--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the starting of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy data */
- for(i=0;i<numTaps-1;i++)
- {
- pStateCurnt[i] = pState[i];
- }
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_decimate.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_decimate.c deleted file mode 100644 index 7cbf127..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_decimate.c +++ /dev/null @@ -1,386 +0,0 @@ -#include "ref.h"
-
-void ref_fir_decimate_f32(
- const arm_fir_decimate_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *pStateCurnt; /* Points to the current sample of the state */
- float32_t sum0; /* Accumulator */
- float32_t x0, c0; /* Temporary variables to hold state and coefficient values */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i, blkCnt; /* Loop counters */
-
- /* S->pState buffer contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + numTaps - 1U;
-
- /* Total number of output samples to be computed */
- blkCnt = blockSize / S->M;
-
- while (blkCnt > 0U)
- {
- /* Copy decimation factor number of new input samples into the state buffer */
- i = S->M;
-
- do
- {
- *pStateCurnt++ = *pSrc++;
- } while (--i);
-
- /* Set accumulator to zero */
- sum0 = 0.0f;
-
- for(i=0;i<numTaps;i++)
- {
- /* Read coefficients */
- c0 = pCoeffs[i];
-
- /* Fetch 1 state variable */
- x0 = pState[i];
-
- /* Perform the multiply-accumulate */
- sum0 += x0 * c0;
- }
-
- /* Advance the state pointer by the decimation factor
- * to process the next group of decimation factor number samples */
- pState += S->M;
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = sum0;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- /* Copy numTaps number of values */
- i = numTaps - 1U;
-
- /* copy data */
- while (i > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- i--;
- }
-}
-
-void ref_fir_decimate_q31(
- const arm_fir_decimate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
- q63_t sum0; /* Accumulator */
- uint32_t numTaps = S->numTaps; /* Number of taps */
- uint32_t i, blkCnt; /* Loop counters */
-
- /* S->pState buffer contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + numTaps - 1U;
-
- /* Total number of output samples to be computed */
- blkCnt = blockSize / S->M;
-
- while (blkCnt > 0U)
- {
- /* Copy decimation factor number of new input samples into the state buffer */
- i = S->M;
-
- do
- {
- *pStateCurnt++ = *pSrc++;
-
- } while (--i);
-
- /* Set accumulator to zero */
- sum0 = 0;
-
- for(i=0;i<numTaps;i++)
- {
- /* Read coefficients */
- c0 = pCoeffs[i];
-
- /* Fetch 1 state variable */
- x0 = pState[i];
-
- /* Perform the multiply-accumulate */
- sum0 += (q63_t)x0 * c0;
- }
-
- /* Advance the state pointer by the decimation factor
- * to process the next group of decimation factor number samples */
- pState = pState + S->M;
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = (q31_t) (sum0 >> 31);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- i = numTaps - 1U;
-
- /* copy data */
- while (i > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- i--;
- }
-}
-
-void ref_fir_decimate_fast_q31(
- const arm_fir_decimate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
- q31_t sum0; /* Accumulator */
- uint32_t numTaps = S->numTaps; /* Number of taps */
- uint32_t i, blkCnt; /* Loop counters */
-
- /* S->pState buffer contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + numTaps - 1U;
-
- /* Total number of output samples to be computed */
- blkCnt = blockSize / S->M;
-
- while (blkCnt > 0U)
- {
- /* Copy decimation factor number of new input samples into the state buffer */
- i = S->M;
-
- do
- {
- *pStateCurnt++ = *pSrc++;
-
- } while (--i);
-
- /* Set accumulator to zero */
- sum0 = 0;
-
- for(i=0;i<numTaps;i++)
- {
- /* Read coefficients */
- c0 = pCoeffs[i];
-
- /* Fetch 1 state variable */
- x0 = pState[i];
-
- /* Perform the multiply-accumulate */
- sum0 = (q31_t)((((q63_t) sum0 << 32) + ((q63_t) x0 * c0)) >> 32);
- }
-
- /* Advance the state pointer by the decimation factor
- * to process the next group of decimation factor number samples */
- pState = pState + S->M;
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = (q31_t) (sum0 << 1);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- i = numTaps - 1U;
-
- /* copy data */
- while (i > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- i--;
- }
-}
-
-void ref_fir_decimate_q15(
- const arm_fir_decimate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
- q63_t sum0; /* Accumulator */
- uint32_t numTaps = S->numTaps; /* Number of taps */
- uint32_t i, blkCnt; /* Loop counters */
-
- /* S->pState buffer contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + numTaps - 1U;
-
- /* Total number of output samples to be computed */
- blkCnt = blockSize / S->M;
-
- while (blkCnt > 0U)
- {
- /* Copy decimation factor number of new input samples into the state buffer */
- i = S->M;
-
- do
- {
- *pStateCurnt++ = *pSrc++;
-
- } while (--i);
-
- /* Set accumulator to zero */
- sum0 = 0;
-
- for(i=0;i<numTaps;i++)
- {
- /* Read coefficients */
- c0 = pCoeffs[i];
-
- /* Fetch 1 state variable */
- x0 = pState[i];
-
- /* Perform the multiply-accumulate */
- sum0 += (q31_t)x0 * c0;
- }
-
- /* Advance the state pointer by the decimation factor
- * to process the next group of decimation factor number samples */
- pState = pState + S->M;
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = ref_sat_q15(sum0 >> 15);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- i = numTaps - 1U;
-
- /* copy data */
- while (i > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- i--;
- }
-}
-
-void ref_fir_decimate_fast_q15(
- const arm_fir_decimate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- q15_t x0, c0; /* Temporary variables to hold state and coefficient values */
- q31_t sum0; /* Accumulator */
- uint32_t numTaps = S->numTaps; /* Number of taps */
- uint32_t i, blkCnt; /* Loop counters */
-
- /* S->pState buffer contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + numTaps - 1U;
-
- /* Total number of output samples to be computed */
- blkCnt = blockSize / S->M;
-
- while (blkCnt > 0U)
- {
- /* Copy decimation factor number of new input samples into the state buffer */
- i = S->M;
-
- do
- {
- *pStateCurnt++ = *pSrc++;
-
- } while (--i);
-
- /* Set accumulator to zero */
- sum0 = 0;
-
- for(i=0;i<numTaps;i++)
- {
- /* Read coefficients */
- c0 = pCoeffs[i];
-
- /* Fetch 1 state variable */
- x0 = pState[i];
-
- /* Perform the multiply-accumulate */
- sum0 += x0 * c0;
- }
-
- /* Advance the state pointer by the decimation factor
- * to process the next group of decimation factor number samples */
- pState = pState + S->M;
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = ref_sat_q15(sum0 >> 15);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last numTaps - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- i = numTaps - 1U;
-
- /* copy data */
- while (i > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- i--;
- }
-}
-
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_interpolate.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_interpolate.c deleted file mode 100644 index 8abb089..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_interpolate.c +++ /dev/null @@ -1,291 +0,0 @@ -#include "ref.h"
-
-void ref_fir_interpolate_f32(
- const arm_fir_interpolate_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *pStateCurnt; /* Points to the current sample of the state */
- float32_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
- float32_t sum; /* Accumulator */
- uint32_t i, blkCnt; /* Loop counters */
- uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */
-
-
- /* S->pState buffer contains previous frame (phaseLen - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + phaseLen - 1;
-
- /* Total number of intput samples */
- blkCnt = blockSize;
-
- /* Loop over the blockSize. */
- while (blkCnt > 0U)
- {
- /* Copy new input sample into the state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Loop over the Interpolation factor. */
- i = S->L;
-
- while (i > 0U)
- {
- /* Set accumulator to zero */
- sum = 0.0f;
-
- /* Initialize state pointer */
- ptr1 = pState;
-
- /* Initialize coefficient pointer */
- ptr2 = pCoeffs + i - 1;
-
- /* Loop over the polyPhase length */
- tapCnt = phaseLen;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- sum += *ptr1++ * *ptr2;
-
- /* Increment the coefficient pointer by interpolation factor times. */
- ptr2 += S->L;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = sum;
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Advance the state pointer by 1
- * to process the next group of interpolation factor number samples */
- pState = pState + 1;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- tapCnt = phaseLen - 1U;
-
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
-}
-
-void ref_fir_interpolate_q31(
- const arm_fir_interpolate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- q31_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
-
- /* Run the below code for Cortex-M0 */
-
- q63_t sum; /* Accumulator */
- q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
- uint32_t i, blkCnt; /* Loop counters */
- uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */
-
-
- /* S->pState buffer contains previous frame (phaseLen - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + (q31_t)phaseLen - 1;
-
- /* Total number of intput samples */
- blkCnt = blockSize;
-
- /* Loop over the blockSize. */
- while (blkCnt > 0U)
- {
- /* Copy new input sample into the state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Loop over the Interpolation factor. */
- i = S->L;
-
- while (i > 0U)
- {
- /* Set accumulator to zero */
- sum = 0;
-
- /* Initialize state pointer */
- ptr1 = pState;
-
- /* Initialize coefficient pointer */
- ptr2 = pCoeffs + i - 1;
-
- tapCnt = phaseLen;
-
- while (tapCnt > 0U)
- {
- /* Read the coefficient */
- c0 = *(ptr2);
-
- /* Increment the coefficient pointer by interpolation factor times. */
- ptr2 += S->L;
-
- /* Read the input sample */
- x0 = *ptr1++;
-
- /* Perform the multiply-accumulate */
- sum += (q63_t) x0 *c0;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* The result is in the accumulator, store in the destination buffer. */
- *pDst++ = (q31_t)(sum >> 31);
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Advance the state pointer by 1
- * to process the next group of interpolation factor number samples */
- pState = pState + 1;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- tapCnt = phaseLen - 1U;
-
- /* copy data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
-}
-
-void ref_fir_interpolate_q15(
- const arm_fir_interpolate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- q15_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
- q63_t sum; /* Accumulator */
- q15_t x0, c0; /* Temporary variables to hold state and coefficient values */
- uint32_t i, blkCnt, tapCnt; /* Loop counters */
- uint16_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
-
-
- /* S->pState buffer contains previous frame (phaseLen - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = S->pState + phaseLen - 1;
-
- /* Total number of intput samples */
- blkCnt = blockSize;
-
- /* Loop over the blockSize. */
- while (blkCnt > 0U)
- {
- /* Copy new input sample into the state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Loop over the Interpolation factor. */
- i = S->L;
-
- while (i > 0U)
- {
- /* Set accumulator to zero */
- sum = 0;
-
- /* Initialize state pointer */
- ptr1 = pState;
-
- /* Initialize coefficient pointer */
- ptr2 = pCoeffs + i - 1;
-
- /* Loop over the polyPhase length */
- tapCnt = (uint32_t)phaseLen;
-
- while (tapCnt > 0U)
- {
- /* Read the coefficient */
- c0 = *ptr2;
-
- /* Increment the coefficient pointer by interpolation factor times. */
- ptr2 += S->L;
-
- /* Read the input sample */
- x0 = *ptr1++;
-
- /* Perform the multiply-accumulate */
- sum += (q31_t) x0 * c0;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Store the result after converting to 1.15 format in the destination buffer */
- *pDst++ = ref_sat_q15(sum >> 15);
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Advance the state pointer by 1
- * to process the next group of interpolation factor number samples */
- pState = pState + 1;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete.
- ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
- ** This prepares the state buffer for the next function call. */
-
- /* Points to the start of the state buffer */
- pStateCurnt = S->pState;
-
- i = (uint32_t) phaseLen - 1U;
-
- while (i > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- i--;
- }
-
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_lattice.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_lattice.c deleted file mode 100644 index 6466106..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_lattice.c +++ /dev/null @@ -1,241 +0,0 @@ -#include "ref.h"
-
-void ref_fir_lattice_f32(
- const arm_fir_lattice_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t *pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *px; /* temporary state pointer */
- float32_t *pk; /* temporary coefficient pointer */
- float32_t fcurr, fnext, gcurr, gnext; /* temporary variables */
- uint32_t numStages = S->numStages; /* Length of the filter */
- uint32_t blkCnt, stageCnt; /* temporary variables for counts */
-
- pState = &S->pState[0];
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* f0(n) = x(n) */
- fcurr = *pSrc++;
-
- /* Initialize coeff pointer */
- pk = pCoeffs;
-
- /* Initialize state pointer */
- px = pState;
-
- /* read g0(n-1) from state buffer */
- gcurr = *px;
-
- /* for sample 1 processing */
- /* f1(n) = f0(n) + K1 * g0(n-1) */
- fnext = fcurr + ((*pk) * gcurr);
- /* g1(n) = f0(n) * K1 + g0(n-1) */
- gnext = (fcurr * (*pk++)) + gcurr;
-
- /* save f0(n) in state buffer */
- *px++ = fcurr;
-
- /* f1(n) is saved in fcurr
- for next stage processing */
- fcurr = fnext;
-
- stageCnt = (numStages - 1U);
-
- /* stage loop */
- while (stageCnt > 0U)
- {
- /* read g2(n) from state buffer */
- gcurr = *px;
-
- /* save g1(n) in state buffer */
- *px++ = gnext;
-
- /* Sample processing for K2, K3.... */
- /* f2(n) = f1(n) + K2 * g1(n-1) */
- fnext = fcurr + ((*pk) * gcurr);
- /* g2(n) = f1(n) * K2 + g1(n-1) */
- gnext = (fcurr * (*pk++)) + gcurr;
-
- /* f1(n) is saved in fcurr1
- for next stage processing */
- fcurr = fnext;
-
- stageCnt--;
- }
-
- /* y(n) = fN(n) */
- *pDst++ = fcurr;
-
- blkCnt--;
- }
-}
-
-void ref_fir_lattice_q31(
- const arm_fir_lattice_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t *pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *px; /* temporary state pointer */
- q31_t *pk; /* temporary coefficient pointer */
- q31_t fcurr, fnext, gcurr, gnext; /* temporary variables */
- uint32_t numStages = S->numStages; /* Length of the filter */
- uint32_t blkCnt, stageCnt; /* temporary variables for counts */
-
- pState = &S->pState[0];
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* f0(n) = x(n) */
- fcurr = *pSrc++;
-
- /* Initialize coeff pointer */
- pk = pCoeffs;
-
- /* Initialize state pointer */
- px = pState;
-
- /* read g0(n-1) from state buffer */
- gcurr = *px;
-
- /* for sample 1 processing */
- /* f1(n) = f0(n) + K1 * g0(n-1) */
- fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
- /* g1(n) = f0(n) * K1 + g0(n-1) */
- gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
- /* save g1(n) in state buffer */
- *px++ = fcurr;
-
- /* f1(n) is saved in fcurr1
- for next stage processing */
- fcurr = fnext;
-
- stageCnt = (numStages - 1U);
-
- /* stage loop */
- while (stageCnt > 0U)
- {
- /* read g2(n) from state buffer */
- gcurr = *px;
-
- /* save g1(n) in state buffer */
- *px++ = gnext;
-
- /* Sample processing for K2, K3.... */
- /* f2(n) = f1(n) + K2 * g1(n-1) */
- fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
- /* g2(n) = f1(n) * K2 + g1(n-1) */
- gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
-
- /* f1(n) is saved in fcurr1
- for next stage processing */
- fcurr = fnext;
-
- stageCnt--;
-
- }
-
- /* y(n) = fN(n) */
- *pDst++ = fcurr;
-
- blkCnt--;
-
- }
-}
-
-void ref_fir_lattice_q15(
- const arm_fir_lattice_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q15_t *pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *px; /* temporary state pointer */
- q15_t *pk; /* temporary coefficient pointer */
- q31_t fcurnt, fnext, gcurnt, gnext; /* temporary variables */
- uint32_t numStages = S->numStages; /* Length of the filter */
- uint32_t blkCnt, stageCnt; /* temporary variables for counts */
-
- pState = &S->pState[0];
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* f0(n) = x(n) */
- fcurnt = *pSrc++;
-
- /* Initialize coeff pointer */
- pk = (pCoeffs);
-
- /* Initialize state pointer */
- px = pState;
-
- /* read g0(n-1) from state buffer */
- gcurnt = *px;
-
- /* for sample 1 processing */
- /* f1(n) = f0(n) + K1 * g0(n-1) */
- fnext = ((gcurnt * (*pk)) >> 15U) + fcurnt;
- fnext = ref_sat_q15(fnext);
-
-
- /* g1(n) = f0(n) * K1 + g0(n-1) */
- gnext = ((fcurnt * (*pk++)) >> 15U) + gcurnt;
- gnext = ref_sat_q15(gnext);
-
- /* save f0(n) in state buffer */
- *px++ = (q15_t) fcurnt;
-
- /* f1(n) is saved in fcurnt
- for next stage processing */
- fcurnt = fnext;
-
- stageCnt = (numStages - 1U);
-
- /* stage loop */
- while (stageCnt > 0U)
- {
- /* read g1(n-1) from state buffer */
- gcurnt = *px;
-
- /* save g0(n-1) in state buffer */
- *px++ = (q15_t) gnext;
-
- /* Sample processing for K2, K3.... */
- /* f2(n) = f1(n) + K2 * g1(n-1) */
- fnext = ((gcurnt * (*pk)) >> 15U) + fcurnt;
- fnext = ref_sat_q15(fnext);
-
- /* g2(n) = f1(n) * K2 + g1(n-1) */
- gnext = ((fcurnt * (*pk++)) >> 15U) + gcurnt;
- gnext = ref_sat_q15(gnext);
-
-
- /* f1(n) is saved in fcurnt
- for next stage processing */
- fcurnt = fnext;
-
- stageCnt--;
-
- }
-
- /* y(n) = fN(n) */
- *pDst++ = ref_sat_q15(fcurnt);
-
-
- blkCnt--;
-
- }
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_sparse.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_sparse.c deleted file mode 100644 index 0638313..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/fir_sparse.c +++ /dev/null @@ -1,485 +0,0 @@ -#include "ref.h"
-
-void ref_fir_sparse_f32(
- arm_fir_sparse_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- float32_t * pScratchIn,
- uint32_t blockSize)
-{
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *px; /* Scratch buffer pointer */
- float32_t *py = pState; /* Temporary pointers for state buffer */
- float32_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
- float32_t *pOut; /* Destination pointer */
- int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
- uint32_t delaySize = S->maxDelay + blockSize; /* state length */
- uint16_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- int32_t readIndex; /* Read index of the state buffer */
- uint32_t tapCnt, blkCnt; /* loop counters */
- float32_t coeff = *pCoeffs++; /* Read the first coefficient value */
-
-
- /* BlockSize of Input samples are copied into the state buffer */
- /* StateIndex points to the starting position to write in the state buffer */
- arm_circularWrite_f32((int32_t *) py, delaySize, &S->stateIndex, 1,
- (int32_t *) pSrc, 1, blockSize);
-
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = ((int32_t) S->stateIndex - (int32_t) blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
- (int32_t *) pb, (int32_t *) pb, blockSize, 1,
- blockSize);
-
- /* Working pointer for the scratch buffer */
- px = pb;
-
- /* Working pointer for destination buffer */
- pOut = pDst;
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform Multiplications and store in destination buffer */
- *pOut++ = *px++ * coeff;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Loop over the number of taps. */
- tapCnt = (uint32_t) numTaps - 1U;
-
- while (tapCnt > 0U)
- {
- /* Load the coefficient value and
- * increment the coefficient buffer for the next set of state values */
- coeff = *pCoeffs++;
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = ((int32_t) S->stateIndex - (int32_t) blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
- (int32_t *) pb, (int32_t *) pb, blockSize, 1,
- blockSize);
-
- /* Working pointer for the scratch buffer */
- px = pb;
-
- /* Working pointer for destination buffer */
- pOut = pDst;
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform Multiply-Accumulate */
- *pOut++ += *px++ * coeff;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Decrement the tap loop counter */
- tapCnt--;
- }
-}
-
-void ref_fir_sparse_q31(
- arm_fir_sparse_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- q31_t * pScratchIn,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *px; /* Scratch buffer pointer */
- q31_t *py = pState; /* Temporary pointers for state buffer */
- q31_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
- q31_t *pOut; /* Destination pointer */
- q63_t out; /* Temporary output variable */
- int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
- uint32_t delaySize = S->maxDelay + blockSize; /* state length */
- uint16_t numTaps = S->numTaps; /* Filter order */
- int32_t readIndex; /* Read index of the state buffer */
- uint32_t tapCnt, blkCnt; /* loop counters */
- q31_t coeff = *pCoeffs++; /* Read the first coefficient value */
- q31_t in;
-
-
- /* BlockSize of Input samples are copied into the state buffer */
- /* StateIndex points to the starting position to write in the state buffer */
- arm_circularWrite_f32((int32_t *) py, delaySize, &S->stateIndex, 1,
- (int32_t *) pSrc, 1, blockSize);
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
- (int32_t *) pb, (int32_t *) pb, blockSize, 1,
- blockSize);
-
- /* Working pointer for the scratch buffer of state values */
- px = pb;
-
- /* Working pointer for scratch buffer of output values */
- pOut = pDst;
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform Multiplications and store in the destination buffer */
- *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Loop over the number of taps. */
- tapCnt = (uint32_t) numTaps - 1U;
-
- while (tapCnt > 0U)
- {
- /* Load the coefficient value and
- * increment the coefficient buffer for the next set of state values */
- coeff = *pCoeffs++;
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
- (int32_t *) pb, (int32_t *) pb, blockSize, 1,
- blockSize);
-
- /* Working pointer for the scratch buffer of state values */
- px = pb;
-
- /* Working pointer for scratch buffer of output values */
- pOut = pDst;
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform Multiply-Accumulate */
- out = *pOut;
- out += ((q63_t) * px++ * coeff) >> 32;
- *pOut++ = (q31_t) (out);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Decrement the tap loop counter */
- tapCnt--;
- }
-
- /* Working output pointer is updated */
- pOut = pDst;
-
- /* Output is converted into 1.31 format. */
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- in = *pOut << 1;
- *pOut++ = in;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-}
-
-void ref_fir_sparse_q15(
- arm_fir_sparse_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- q15_t * pScratchIn,
- q31_t * pScratchOut,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pIn = pSrc; /* Working pointer for input */
- q15_t *pOut = pDst; /* Working pointer for output */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *px; /* Temporary pointers for scratch buffer */
- q15_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
- q15_t *py = pState; /* Temporary pointers for state buffer */
- int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
- uint32_t delaySize = S->maxDelay + blockSize; /* state length */
- uint16_t numTaps = S->numTaps; /* Filter order */
- int32_t readIndex; /* Read index of the state buffer */
- uint32_t tapCnt, blkCnt; /* loop counters */
- q15_t coeff = *pCoeffs++; /* Read the first coefficient value */
- q31_t *pScr2 = pScratchOut; /* Working pointer for pScratchOut */
-
- /* BlockSize of Input samples are copied into the state buffer */
- /* StateIndex points to the starting position to write in the state buffer */
- arm_circularWrite_q15(py, delaySize, &S->stateIndex, 1, pIn, 1, blockSize);
-
- /* Loop over the number of taps. */
- tapCnt = numTaps;
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = (S->stateIndex - blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_q15(py, delaySize, &readIndex, 1,
- pb, pb, blockSize, 1, blockSize);
-
- /* Working pointer for the scratch buffer of state values */
- px = pb;
-
- /* Working pointer for scratch buffer of output values */
- pScratchOut = pScr2;
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform multiplication and store in the scratch buffer */
- *pScratchOut++ = ((q31_t) * px++ * coeff);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Loop over the number of taps. */
- tapCnt = (uint32_t) numTaps - 1U;
-
- while (tapCnt > 0U)
- {
- /* Load the coefficient value and
- * increment the coefficient buffer for the next set of state values */
- coeff = *pCoeffs++;
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = (S->stateIndex - blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_q15(py, delaySize, &readIndex, 1,
- pb, pb, blockSize, 1, blockSize);
-
- /* Working pointer for the scratch buffer of state values */
- px = pb;
-
- /* Working pointer for scratch buffer of output values */
- pScratchOut = pScr2;
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform Multiply-Accumulate */
- *pScratchOut++ += (q31_t) * px++ * coeff;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Decrement the tap loop counter */
- tapCnt--;
- }
-
- /* All the output values are in pScratchOut buffer.
- Convert them into 1.15 format, saturate and store in the destination buffer. */
- /* Loop over the blockSize. */
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- *pOut++ = (q15_t) __SSAT(*pScr2++ >> 15, 16);
- blkCnt--;
- }
-}
-
-void ref_fir_sparse_q7(
- arm_fir_sparse_instance_q7 * S,
- q7_t *pSrc,
- q7_t *pDst,
- q7_t *pScratchIn,
- q31_t * pScratchOut,
- uint32_t blockSize)
-{
- q7_t *pState = S->pState; /* State pointer */
- q7_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q7_t *px; /* Scratch buffer pointer */
- q7_t *py = pState; /* Temporary pointers for state buffer */
- q7_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
- q7_t *pOut = pDst; /* Destination pointer */
- int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
- uint32_t delaySize = S->maxDelay + blockSize; /* state length */
- uint16_t numTaps = S->numTaps; /* Filter order */
- int32_t readIndex; /* Read index of the state buffer */
- uint32_t tapCnt, blkCnt; /* loop counters */
- q7_t coeff = *pCoeffs++; /* Read the coefficient value */
- q31_t *pScr2 = pScratchOut; /* Working pointer for scratch buffer of output values */
- q31_t in;
-
- /* BlockSize of Input samples are copied into the state buffer */
- /* StateIndex points to the starting position to write in the state buffer */
- arm_circularWrite_q7(py, (int32_t) delaySize, &S->stateIndex, 1, pSrc, 1,
- blockSize);
-
- /* Loop over the number of taps. */
- tapCnt = numTaps;
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = ((int32_t) S->stateIndex - (int32_t) blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_q7(py, (int32_t) delaySize, &readIndex, 1, pb, pb,
- (int32_t) blockSize, 1, blockSize);
-
- /* Working pointer for the scratch buffer of state values */
- px = pb;
-
- /* Working pointer for scratch buffer of output values */
- pScratchOut = pScr2;
-
- /* Loop over the blockSize */
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform multiplication and store in the scratch buffer */
- *pScratchOut++ = ((q31_t) * px++ * coeff);
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Loop over the number of taps. */
- tapCnt = (uint32_t) numTaps - 1U;
-
- while (tapCnt > 0U)
- {
- /* Load the coefficient value and
- * increment the coefficient buffer for the next set of state values */
- coeff = *pCoeffs++;
-
- /* Read Index, from where the state buffer should be read, is calculated. */
- readIndex = ((int32_t) S->stateIndex - (int32_t) blockSize) - *pTapDelay++;
-
- /* Wraparound of readIndex */
- if (readIndex < 0)
- {
- readIndex += (int32_t) delaySize;
- }
-
- /* Working pointer for state buffer is updated */
- py = pState;
-
- /* blockSize samples are read from the state buffer */
- arm_circularRead_q7(py, (int32_t) delaySize, &readIndex, 1, pb, pb,
- (int32_t) blockSize, 1, blockSize);
-
- /* Working pointer for the scratch buffer of state values */
- px = pb;
-
- /* Working pointer for scratch buffer of output values */
- pScratchOut = pScr2;
-
- /* Loop over the blockSize */
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Perform Multiply-Accumulate */
- in = *pScratchOut + ((q31_t) * px++ * coeff);
- *pScratchOut++ = in;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Decrement the tap loop counter */
- tapCnt--;
- }
-
- /* All the output values are in pScratchOut buffer.
- Convert them into 1.15 format, saturate and store in the destination buffer. */
- /* Loop over the blockSize. */
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- *pOut++ = (q7_t) __SSAT(*pScr2++ >> 7, 8);
-
- /* Decrement the blockSize loop counter */
- blkCnt--;
- }
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c deleted file mode 100644 index ab37d5f..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c +++ /dev/null @@ -1,271 +0,0 @@ -#include "ref.h"
-
-void ref_iir_lattice_f32(
- const arm_iir_lattice_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize)
-{
- float32_t fcurr, fnext = 0, gcurr, gnext; /* Temporary variables for lattice stages */
- float32_t acc; /* Accumlator */
- uint32_t blkCnt, tapCnt; /* temporary variables for counts */
- float32_t *px1, *px2, *pk, *pv; /* temporary pointers for state and coef */
- uint32_t numStages = S->numStages; /* number of stages */
- float32_t *pState; /* State pointer */
- float32_t *pStateCurnt; /* State current pointer */
-
- blkCnt = blockSize;
- pState = &S->pState[0];
-
- /* Sample processing */
- while (blkCnt > 0U)
- {
- /* Read Sample from input buffer */
- /* fN(n) = x(n) */
- fcurr = *pSrc++;
-
- /* Initialize state read pointer */
- px1 = pState;
- /* Initialize state write pointer */
- px2 = pState;
- /* Set accumulator to zero */
- acc = 0.0f;
- /* Initialize Ladder coeff pointer */
- pv = &S->pvCoeffs[0];
- /* Initialize Reflection coeff pointer */
- pk = &S->pkCoeffs[0];
-
- /* Process sample for numStages */
- tapCnt = numStages;
-
- while (tapCnt > 0U)
- {
- gcurr = *px1++;
- /* Process sample for last taps */
- fnext = fcurr - (*pk) * gcurr;
- gnext = fnext * (*pk++) + gcurr;
-
- /* Output samples for last taps */
- acc += gnext * (*pv++);
- *px2++ = gnext;
- fcurr = fnext;
-
- /* Decrementing loop counter */
- tapCnt--;
- }
-
- /* y(n) += g0(n) * v0 */
- acc += fnext * (*pv);
-
- *px2++ = fnext;
-
- /* write out into pDst */
- *pDst++ = acc;
-
- /* Advance the state pointer by 1 to process the next group of samples */
- pState = pState + 1U;
- blkCnt--;
- }
-
- /* Processing is complete. Now copy last S->numStages samples to start of the buffer
- for the preperation of next frame process */
-
- /* Points to the start of the state buffer */
- pStateCurnt = &S->pState[0];
- pState = &S->pState[blockSize];
-
- tapCnt = numStages;
-
- /* Copy the data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-}
-
-void ref_iir_lattice_q31(
- const arm_iir_lattice_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize)
-{
- q31_t fcurr, fnext = 0, gcurr = 0, gnext; /* Temporary variables for lattice stages */
- q63_t acc; /* Accumlator */
- uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
- q31_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */
- uint32_t numStages = S->numStages; /* number of stages */
- q31_t *pState; /* State pointer */
- q31_t *pStateCurnt; /* State current pointer */
-
- blkCnt = blockSize;
- pState = &S->pState[0];
-
- /* Sample processing */
- while (blkCnt > 0U)
- {
- /* Read Sample from input buffer */
- /* fN(n) = x(n) */
- fcurr = *pSrc++;
-
- /* Initialize state read pointer */
- px1 = pState;
- /* Initialize state write pointer */
- px2 = pState;
- /* Set accumulator to zero */
- acc = 0;
- /* Initialize Ladder coeff pointer */
- pv = &S->pvCoeffs[0];
- /* Initialize Reflection coeff pointer */
- pk = &S->pkCoeffs[0];
-
- tapCnt = numStages;
-
- while (tapCnt > 0U)
- {
- gcurr = *px1++;
- /* Process sample */
- /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
- fnext =
- ref_sat_q31(((q63_t) fcurr -
- ((q31_t) (((q63_t) gcurr * (*pk)) >> 31))));
- /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
- gnext =
- ref_sat_q31(((q63_t) gcurr +
- ((q31_t) (((q63_t) fnext * (*pk++)) >> 31))));
- /* Output samples */
- /* y(n) += gN(n) * vN */
- acc += ((q63_t) gnext * *pv++);
- /* write gN-1(n-1) into state for next sample processing */
- *px2++ = gnext;
- /* Update f values for next coefficient processing */
- fcurr = fnext;
-
- tapCnt--;
- }
-
- /* y(n) += g0(n) * v0 */
- acc += (q63_t) fnext *(*pv++);
-
- *px2++ = fnext;
-
- /* write out into pDst */
- *pDst++ = (q31_t) (acc >> 31U);
-
- /* Advance the state pointer by 1 to process the next group of samples */
- pState = pState + 1U;
- blkCnt--;
- }
-
- /* Processing is complete. Now copy last S->numStages samples to start of the buffer
- for the preperation of next frame process */
-
- /* Points to the start of the state buffer */
- pStateCurnt = &S->pState[0];
- pState = &S->pState[blockSize];
-
- tapCnt = numStages;
-
- /* Copy the remaining q31_t data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-}
-
-void ref_iir_lattice_q15(
- const arm_iir_lattice_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize)
-{
- q31_t fcurr, fnext = 0, gcurr = 0, gnext; /* Temporary variables for lattice stages */
- uint32_t stgCnt; /* Temporary variables for counts */
- q63_t acc; /* Accumlator */
- uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
- q15_t *px1, *px2, *pk, *pv; /* temporary pointers for state and coef */
- uint32_t numStages = S->numStages; /* number of stages */
- q15_t *pState; /* State pointer */
- q15_t *pStateCurnt; /* State current pointer */
- q15_t out; /* Temporary variable for output */
-
- blkCnt = blockSize;
- pState = &S->pState[0];
-
- /* Sample processing */
- while (blkCnt > 0U)
- {
- /* Read Sample from input buffer */
- /* fN(n) = x(n) */
- fcurr = *pSrc++;
-
- /* Initialize state read pointer */
- px1 = pState;
- /* Initialize state write pointer */
- px2 = pState;
- /* Set accumulator to zero */
- acc = 0;
- /* Initialize Ladder coeff pointer */
- pv = &S->pvCoeffs[0];
- /* Initialize Reflection coeff pointer */
- pk = &S->pkCoeffs[0];
-
- tapCnt = numStages;
-
- while (tapCnt > 0U)
- {
- gcurr = *px1++;
- /* Process sample */
- /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
- fnext = fcurr - ((gcurr * (*pk)) >> 15);
- fnext = ref_sat_q15(fnext);
- /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
- gnext = ((fnext * (*pk++)) >> 15) + gcurr;
- gnext = ref_sat_q15(gnext);
- /* Output samples */
- /* y(n) += gN(n) * vN */
- acc += (q31_t) ((gnext * (*pv++)));
- /* write gN(n) into state for next sample processing */
- *px2++ = (q15_t) gnext;
- /* Update f values for next coefficient processing */
- fcurr = fnext;
-
- tapCnt--;
- }
-
- /* y(n) += g0(n) * v0 */
- acc += (q31_t) ((fnext * (*pv++)));
-
- out = ref_sat_q15(acc >> 15);
- *px2++ = (q15_t) fnext;
-
- /* write out into pDst */
- *pDst++ = out;
-
- /* Advance the state pointer by 1 to process the next group of samples */
- pState = pState + 1U;
- blkCnt--;
- }
-
- /* Processing is complete. Now copy last S->numStages samples to start of the buffer
- for the preperation of next frame process */
- /* Points to the start of the state buffer */
- pStateCurnt = &S->pState[0];
- pState = &S->pState[blockSize];
-
- stgCnt = numStages;
-
- /* copy data */
- while (stgCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- stgCnt--;
- }
-}
diff --git a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/lms.c b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/lms.c deleted file mode 100644 index fee99f9..0000000 --- a/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/lms.c +++ /dev/null @@ -1,695 +0,0 @@ -#include "ref.h"
-
-void ref_lms_f32(
- const arm_lms_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pRef,
- float32_t * pOut,
- float32_t * pErr,
- uint32_t blockSize)
-{
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *pStateCurnt; /* Points to the current sample of the state */
- float32_t mu = S->mu; /* Adaptive factor */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i, blkCnt; /* Loop counters */
- float32_t sum, e, d; /* accumulator, error, reference data sample */
- float32_t w = 0.0f; /* weight factor */
-
- e = 0.0f;
- d = 0.0f;
-
- /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[numTaps - 1U]);
-
- blkCnt = blockSize;
-
- while (blkCnt > 0U)
- {
- /* Copy the new input sample into the state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Set the accumulator to zero */
- sum = 0.0f;
-
- for(i=0;i<numTaps;i++)
- { /* Perform the multiply-accumulate */
- sum += pState[i] * pCoeffs[i];
- }
-
- /* The result is stored in the destination buffer. */
- *pOut++ = sum;
-
- /* Compute and store error */
- d = *pRef++;
- e = d - sum;
- *pErr++ = e;
-
- /* Weighting factor for the LMS version */
- w = e * mu;
-
- for(i=0;i<numTaps;i++)
- { /* Perform the multiply-accumulate */
- pCoeffs[i] += w * pState[i];
- }
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
-
- /* Decrement the loop counter */
- blkCnt--;
- }
-
- /* Processing is complete. Now copy the last numTaps - 1 samples to the
- * start of the state buffer. This prepares the state buffer for the
- * next function call. */
- for(i=0;i<numTaps-1;i++)
- {
- S->pState[i] = pState[i];
- }
-}
-
-void ref_lms_norm_f32(
- arm_lms_norm_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pRef,
- float32_t * pOut,
- float32_t * pErr,
- uint32_t blockSize)
-{
- float32_t *pState = S->pState; /* State pointer */
- float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- float32_t *pStateCurnt; /* Points to the current sample of the state */
- float32_t mu = S->mu; /* Adaptive factor */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t i, blkCnt; /* Loop counters */
- float32_t energy; /* Energy of the input */
- float32_t sum, e, d; /* accumulator, error, reference data sample */
- float32_t w, x0, in; /* weight factor, temporary variable to hold input sample and state */
-
- /* Initializations of error, difference, Coefficient update */
- e = 0.0f;
- d = 0.0f;
- w = 0.0f;
-
- energy = S->energy;
- x0 = S->x0;
-
- /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[numTaps - 1U]);
-
- for(blkCnt = blockSize; blkCnt > 0U; blkCnt--)
- {
- /* Copy the new input sample into the state buffer */
- *pStateCurnt++ = *pSrc;
-
- /* Read the sample from input buffer */
- in = *pSrc++;
-
- /* Update the energy calculation */
- energy -= x0 * x0;
- energy += in * in;
-
- /* Set the accumulator to zero */
- sum = 0.0f;
-
- for(i=0;i<numTaps;i++)
- { /* Perform the multiply-accumulate */
- sum += pState[i] * pCoeffs[i];
- }
-
- /* The result in the accumulator is stored in the destination buffer. */
- *pOut++ = sum;
-
- /* Compute and store error */
- d = *pRef++;
- e = d - sum;
- *pErr++ = e;
-
- /* Calculation of Weighting factor for updating filter coefficients */
- /* epsilon value 0.000000119209289f */
- w = e * mu / (energy + 0.000000119209289f);
-
- for(i=0;i<numTaps;i++)
- {
- /* Perform the multiply-accumulate */
- pCoeffs[i] += w * pState[i];
- }
-
- x0 = *pState;
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
- }
-
- S->energy = energy;
- S->x0 = x0;
-
- /* Processing is complete. Now copy the last numTaps - 1 samples to the
- * start of the state buffer. This prepares the state buffer for the
- * next function call. */
- for(i=0;i<numTaps-1;i++)
- {
- S->pState[i] = pState[i];
- }
-}
-
-void ref_lms_q31(
- const arm_lms_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pRef,
- q31_t * pOut,
- q31_t * pErr,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- q31_t mu = S->mu; /* Adaptive factor */
- q31_t *px; /* Temporary pointer for state */
- q31_t *pb; /* Temporary pointer for coefficient buffer */
- uint32_t tapCnt, blkCnt; /* Loop counters */
- q63_t acc; /* Accumulator */
- q31_t e = 0; /* error of data sample */
- q31_t alpha; /* Intermediate constant for taps update */
- q31_t coef; /* Temporary variable for coef */
- q31_t acc_l, acc_h; /* temporary input */
- uint32_t uShift = (uint32_t)S->postShift + 1;
- uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
-
- /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- for(blkCnt = blockSize; blkCnt > 0U; blkCnt--)
- {
- /* Copy the new input sample into the state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Initialize pState pointer */
- px = pState;
-
- /* Initialize pCoeffs pointer */
- pb = pCoeffs;
-
- /* Set the accumulator to zero */
- acc = 0;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- acc += (q63_t)(*px++) * (*pb++);
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Converting the result to 1.31 format */
- /* Store the result from accumulator into the destination buffer. */
- /* Calc lower part of acc */
- acc_l = acc & 0xffffffff;
-
- /* Calc upper part of acc */
- acc_h = (acc >> 32) & 0xffffffff;
-
- acc = (uint32_t)acc_l >> lShift | acc_h << uShift;
-
- *pOut++ = (q31_t)acc;
-
- /* Compute and store error */
- e = *pRef++ - (q31_t)acc;
-
- *pErr++ = (q31_t)e;
-
- /* Weighting factor for the LMS version */
- alpha = (q31_t)(((q63_t)e * mu) >> 31);
-
- /* Initialize pState pointer */
- /* Advance state pointer by 1 for the next sample */
- px = pState++;
-
- /* Initialize pCoeffs pointer */
- pb = pCoeffs;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- coef = (q31_t)(((q63_t) alpha * (*px++)) >> 32);
- *pb = ref_sat_q31((q63_t)*pb + (coef << 1));
- pb++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
- }
-
- /* Processing is complete. Now copy the last numTaps - 1 samples to the
- start of the state buffer. This prepares the state buffer for the
- next function call. */
-
- /* Points to the start of the pState buffer */
- pStateCurnt = S->pState;
-
- /* Copy (numTaps - 1U) samples */
- tapCnt = numTaps - 1;
-
- /* Copy the data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-}
-
-void ref_lms_norm_q31(
- arm_lms_norm_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pRef,
- q31_t * pOut,
- q31_t * pErr,
- uint32_t blockSize)
-{
- q31_t *pState = S->pState; /* State pointer */
- q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q31_t *pStateCurnt; /* Points to the current sample of the state */
- q31_t *px, *pb; /* Temporary pointers for state and coefficient buffers */
- q31_t mu = S->mu; /* Adaptive factor */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t tapCnt, blkCnt; /* Loop counters */
- q63_t energy; /* Energy of the input */
- q63_t acc; /* Accumulator */
- q31_t e = 0, d = 0; /* error, reference data sample */
- q31_t w = 0, in; /* weight factor and state */
- q31_t x0; /* temporary variable to hold input sample */
- q63_t errorXmu; /* Temporary variables to store error and mu product and reciprocal of energy */
- q31_t coef; /* Temporary variable for coef */
- q31_t acc_l, acc_h; /* temporary input */
- uint32_t uShift = ((uint32_t) S->postShift + 1U);
- uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
-
- energy = S->energy;
- x0 = S->x0;
-
- /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- for(blkCnt = blockSize; blkCnt > 0U; blkCnt--)
- {
-
- /* Copy the new input sample into the state buffer */
- *pStateCurnt++ = *pSrc;
-
- /* Initialize pState pointer */
- px = pState;
-
- /* Initialize pCoeffs pointer */
- pb = pCoeffs;
-
- /* Read the sample from input buffer */
- in = *pSrc++;
-
- /* Update the energy calculation */
- energy = (q31_t)((((q63_t)energy << 32) - (((q63_t)x0 * x0) << 1)) >> 32) & 0xffffffff;
- energy = (q31_t)(((((q63_t)in * in) << 1) + ((q63_t)energy << 32)) >> 32) & 0xffffffff;
-
- /* Set the accumulator to zero */
- acc = 0;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- acc += ((q63_t) (*px++)) * (*pb++);
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Converting the result to 1.31 format */
- /* Calc lower part of acc */
- acc_l = acc & 0xffffffff;
-
- /* Calc upper part of acc */
- acc_h = (acc >> 32) & 0xffffffff;
-
- acc = (uint32_t)acc_l >> lShift | acc_h << uShift;
-
- /* Store the result from accumulator into the destination buffer. */
- *pOut++ = (q31_t)acc;
-
- /* Compute and store error */
- d = *pRef++;
- e = d - (q31_t)acc;
- *pErr++ = e;
-
- /* Calculation of product of (e * mu) */
- errorXmu = (q63_t)e * mu;
-
- /* Weighting factor for the normalized version */
- w = ref_sat_q31(errorXmu / (energy + DELTA_Q31));
-
- /* Initialize pState pointer */
- px = pState;
-
- /* Initialize coeff pointer */
- pb = pCoeffs;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- /* coef is in 2.30 format */
- coef = (q31_t)(((q63_t)w * (*px++)) >> 32);
- /* get coef in 1.31 format by left shifting */
- *pb = ref_sat_q31((q63_t)*pb + (coef << 1U));
- /* update coefficient buffer to next coefficient */
- pb++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Read the sample from state buffer */
- x0 = *pState;
-
- /* Advance state pointer by 1 for the next sample */
- pState++;
- }
-
- /* Save energy and x0 values for the next frame */
- S->energy = (q31_t)energy;
- S->x0 = x0;
-
- /* Processing is complete. Now copy the last numTaps - 1 samples to the
- start of the state buffer. This prepares the state buffer for the
- next function call. */
-
- /* Points to the start of the pState buffer */
- pStateCurnt = S->pState;
-
- /* Loop for (numTaps - 1U) samples copy */
- tapCnt = numTaps - 1;
-
- /* Copy the remaining q31_t data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-}
-
-void ref_lms_q15(
- const arm_lms_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pRef,
- q15_t * pOut,
- q15_t * pErr,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- q15_t mu = S->mu; /* Adaptive factor */
- q15_t *px; /* Temporary pointer for state */
- q15_t *pb; /* Temporary pointer for coefficient buffer */
- uint32_t tapCnt, blkCnt; /* Loop counters */
- q63_t acc; /* Accumulator */
- q15_t e = 0; /* error of data sample */
- q15_t alpha; /* Intermediate constant for taps update */
- q31_t coef; /* Teporary variable for coefficient */
- q31_t acc_l, acc_h;
- int32_t lShift = 15 - (int32_t)S->postShift; /* Post shift */
- int32_t uShift = 32 - lShift;
-
- /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- for(blkCnt = blockSize; blkCnt > 0U; blkCnt--)
- {
- /* Copy the new input sample into the state buffer */
- *pStateCurnt++ = *pSrc++;
-
- /* Initialize pState pointer */
- px = pState;
-
- /* Initialize pCoeffs pointer */
- pb = pCoeffs;
-
- /* Set the accumulator to zero */
- acc = 0;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- acc += (q63_t)((q31_t)(*px++) * (*pb++));
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Calc lower part of acc */
- acc_l = acc & 0xffffffff;
-
- /* Calc upper part of acc */
- acc_h = (acc >> 32) & 0xffffffff;
-
- /* Apply shift for lower part of acc and upper part of acc */
- acc = (uint32_t)acc_l >> lShift | acc_h << uShift;
-
- /* Converting the result to 1.15 format and saturate the output */
- acc = ref_sat_q15(acc);
-
- /* Store the result from accumulator into the destination buffer. */
- *pOut++ = (q15_t)acc;
-
- /* Compute and store error */
- e = *pRef++ - (q15_t)acc;
-
- *pErr++ = (q15_t)e;
-
- /* Compute alpha i.e. intermediate constant for taps update */
- alpha = (q15_t)(((q31_t)e * mu) >> 15);
-
- /* Initialize pState pointer */
- /* Advance state pointer by 1 for the next sample */
- px = pState++;
-
- /* Initialize pCoeffs pointer */
- pb = pCoeffs;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- coef = (q31_t) * pb + (((q31_t) alpha * (*px++)) >> 15);
- *pb++ = (q15_t) ref_sat_q15(coef);
-
- /* Decrement the loop counter */
- tapCnt--;
- }
- }
-
- /* Processing is complete. Now copy the last numTaps - 1 samples to the
- start of the state buffer. This prepares the state buffer for the
- next function call. */
-
- /* Points to the start of the pState buffer */
- pStateCurnt = S->pState;
-
- /* Copy (numTaps - 1U) samples */
- tapCnt = numTaps - 1;
-
- /* Copy the data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-}
-
-void ref_lms_norm_q15(
- arm_lms_norm_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pRef,
- q15_t * pOut,
- q15_t * pErr,
- uint32_t blockSize)
-{
- q15_t *pState = S->pState; /* State pointer */
- q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
- q15_t *pStateCurnt; /* Points to the current sample of the state */
- q15_t *px, *pb; /* Temporary pointers for state and coefficient buffers */
- q15_t mu = S->mu; /* Adaptive factor */
- uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
- uint32_t tapCnt, blkCnt; /* Loop counters */
- q31_t energy; /* Energy of the input */
- q63_t acc; /* Accumulator */
- q15_t e = 0, d = 0; /* error, reference data sample */
- q15_t w = 0, in; /* weight factor and state */
- q15_t x0; /* temporary variable to hold input sample */
- q15_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */
- //q31_t errorXmu; /* Temporary variables to store error and mu product and reciprocal of energy */
- q15_t postShift; /* Post shift to be applied to weight after reciprocal calculation */
- q31_t coef; /* Teporary variable for coefficient */
- q31_t acc_l, acc_h;
- int32_t lShift = 15 - (int32_t)S->postShift; /* Post shift */
- int32_t uShift = 32 - lShift;
-
- energy = S->energy;
- x0 = S->x0;
-
- /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
- /* pStateCurnt points to the location where the new input data should be written */
- pStateCurnt = &(S->pState[(numTaps - 1U)]);
-
- for(blkCnt = blockSize; blkCnt > 0U; blkCnt--)
- {
- /* Copy the new input sample into the state buffer */
- *pStateCurnt++ = *pSrc;
-
- /* Initialize pState pointer */
- px = pState;
-
- /* Initialize pCoeffs pointer */
- pb = pCoeffs;
-
- /* Read the sample from input buffer */
- in = *pSrc++;
-
- /* Update the energy calculation */
- energy -= (((q31_t)x0 * x0) >> 15) & 0xffff;
- energy += (((q31_t)in * in) >> 15) & 0xffff;
-
- /* Set the accumulator to zero */
- acc = 0;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- acc += (q31_t)*px++ * (*pb++);
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Calc lower part of acc */
- acc_l = acc & 0xffffffff;
-
- /* Calc upper part of acc */
- acc_h = (acc >> 32) & 0xffffffff;
-
- /* Apply shift for lower part of acc and upper part of acc */
- acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
-
- /* Converting the result to 1.15 format and saturate the output */
- acc = ref_sat_q15(acc);
-
- /* Store the result from accumulator into the destination buffer. */
- *pOut++ = (q15_t) acc;
-
- /* Compute and store error */
- d = *pRef++;
- e = d - (q15_t) acc;
- *pErr++ = e;
-
-#if 0
- /* Calculation of e * mu value */
- errorXmu = (q31_t) e * mu;
-
- /* Calculation of (e * mu) /energy value */
- acc = errorXmu / (energy + DELTA_Q15);
-#endif
-
- /* Calculation of 1/energy */
- postShift = arm_recip_q15((q15_t) energy + DELTA_Q15,
- &oneByEnergy, S->recipTable);
-
- /* Calculation of e * mu value */
- errorXmu = (q15_t) (((q31_t) e * mu) >> 15);
-
- /* Calculation of (e * mu) * (1/energy) value */
- acc = (((q31_t) errorXmu * oneByEnergy) >> (15 - postShift));
-
- /* Weighting factor for the normalized version */
- w = ref_sat_q15((q31_t)acc);
-
- /* Initialize pState pointer */
- px = pState;
-
- /* Initialize coeff pointer */
- pb = pCoeffs;
-
- /* Loop over numTaps number of values */
- tapCnt = numTaps;
-
- while (tapCnt > 0U)
- {
- /* Perform the multiply-accumulate */
- coef = *pb + (((q31_t)w * (*px++)) >> 15);
- *pb++ = ref_sat_q15(coef);
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-
- /* Read the sample from state buffer */
- x0 = *pState;
-
- /* Advance state pointer by 1 for the next sample */
- pState = pState + 1U;
- }
-
- /* Save energy and x0 values for the next frame */
- S->energy = (q15_t)energy;
- S->x0 = x0;
-
- /* Processing is complete. Now copy the last numTaps - 1 samples to the
- satrt of the state buffer. This prepares the state buffer for the
- next function call. */
-
- /* Points to the start of the pState buffer */
- pStateCurnt = S->pState;
-
- /* copy (numTaps - 1U) data */
- tapCnt = numTaps - 1;
-
- /* copy data */
- while (tapCnt > 0U)
- {
- *pStateCurnt++ = *pState++;
-
- /* Decrement the loop counter */
- tapCnt--;
- }
-}
|