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