#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--;
|
}
|
}
|
|
