From 6ab94e0b318884bbcb95e2ea3835f951502e1d99 Mon Sep 17 00:00:00 2001 From: jaseg Date: Wed, 14 Oct 2020 12:47:28 +0200 Subject: Move firmware into subdirectory --- .../RefLibs/src/FilteringFunctions/iir_lattice.c | 271 +++++++++++++++++++++ 1 file changed, 271 insertions(+) create mode 100644 fw/cdc-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c (limited to 'fw/cdc-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c') diff --git a/fw/cdc-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c b/fw/cdc-dials/Drivers/CMSIS/DSP/DSP_Lib_TestSuite/RefLibs/src/FilteringFunctions/iir_lattice.c new file mode 100644 index 0000000..ab37d5f --- /dev/null +++ b/fw/cdc-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--; + } +} -- cgit