diff options
Diffstat (limited to 'fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c')
| -rw-r--r-- | fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c | 271 |
1 files changed, 271 insertions, 0 deletions
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 new file mode 100644 index 0000000..ab37d5f --- /dev/null +++ b/fw/hid-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c @@ -0,0 +1,271 @@ +#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--;
+ }
+}
|