in lib/cmsis/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f32.c [155:678]
void arm_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 */
#if defined(ARM_MATH_CM7)
float32_t Xn2a, Xn3a, Xn4a, Xn5a, Xn6a, Xn7a, Xn8a; /* Input State variables */
float32_t Xn2b, Xn3b, Xn4b, Xn5b, Xn6b, Xn7b, Xn8b; /* Input State variables */
float32_t acc2a, acc3a, acc4a, acc5a, acc6a, acc7a, acc8a; /* Simulates the accumulator */
float32_t acc2b, acc3b, acc4b, acc5b, acc6b, acc7b, acc8b; /* Simulates the accumulator */
do
{
/* Reading the coefficients */
b0 = pCoeffs[0];
b1 = pCoeffs[1];
b2 = pCoeffs[2];
a1 = pCoeffs[3];
/* Apply loop unrolling and compute 8 output values simultaneously. */
sample = blockSize >> 3u;
a2 = pCoeffs[4];
/*Reading the state values */
d1a = pState[0];
d2a = pState[1];
d1b = pState[2];
d2b = pState[3];
pCoeffs += 5u;
/* First part of the processing with loop unrolling. Compute 8 outputs at a time.
** a second loop below computes the remaining 1 to 7 samples. */
while(sample > 0u) {
/* y[n] = b0 * x[n] + d1 */
/* d1 = b1 * x[n] + a1 * y[n] + d2 */
/* d2 = b2 * x[n] + a2 * y[n] */
/* Read the first 2 inputs. 2 cycles */
Xn1a = pIn[0 ];
Xn1b = pIn[1 ];
/* Sample 1. 5 cycles */
Xn2a = pIn[2 ];
acc1a = b0 * Xn1a + d1a;
Xn2b = pIn[3 ];
d1a = b1 * Xn1a + d2a;
Xn3a = pIn[4 ];
d2a = b2 * Xn1a;
Xn3b = pIn[5 ];
d1a += a1 * acc1a;
Xn4a = pIn[6 ];
d2a += a2 * acc1a;
/* Sample 2. 5 cycles */
Xn4b = pIn[7 ];
acc1b = b0 * Xn1b + d1b;
Xn5a = pIn[8 ];
d1b = b1 * Xn1b + d2b;
Xn5b = pIn[9 ];
d2b = b2 * Xn1b;
Xn6a = pIn[10];
d1b += a1 * acc1b;
Xn6b = pIn[11];
d2b += a2 * acc1b;
/* Sample 3. 5 cycles */
Xn7a = pIn[12];
acc2a = b0 * Xn2a + d1a;
Xn7b = pIn[13];
d1a = b1 * Xn2a + d2a;
Xn8a = pIn[14];
d2a = b2 * Xn2a;
Xn8b = pIn[15];
d1a += a1 * acc2a;
pIn += 16;
d2a += a2 * acc2a;
/* Sample 4. 5 cycles */
acc2b = b0 * Xn2b + d1b;
d1b = b1 * Xn2b + d2b;
d2b = b2 * Xn2b;
d1b += a1 * acc2b;
d2b += a2 * acc2b;
/* Sample 5. 5 cycles */
acc3a = b0 * Xn3a + d1a;
d1a = b1 * Xn3a + d2a;
d2a = b2 * Xn3a;
d1a += a1 * acc3a;
d2a += a2 * acc3a;
/* Sample 6. 5 cycles */
acc3b = b0 * Xn3b + d1b;
d1b = b1 * Xn3b + d2b;
d2b = b2 * Xn3b;
d1b += a1 * acc3b;
d2b += a2 * acc3b;
/* Sample 7. 5 cycles */
acc4a = b0 * Xn4a + d1a;
d1a = b1 * Xn4a + d2a;
d2a = b2 * Xn4a;
d1a += a1 * acc4a;
d2a += a2 * acc4a;
/* Sample 8. 5 cycles */
acc4b = b0 * Xn4b + d1b;
d1b = b1 * Xn4b + d2b;
d2b = b2 * Xn4b;
d1b += a1 * acc4b;
d2b += a2 * acc4b;
/* Sample 9. 5 cycles */
acc5a = b0 * Xn5a + d1a;
d1a = b1 * Xn5a + d2a;
d2a = b2 * Xn5a;
d1a += a1 * acc5a;
d2a += a2 * acc5a;
/* Sample 10. 5 cycles */
acc5b = b0 * Xn5b + d1b;
d1b = b1 * Xn5b + d2b;
d2b = b2 * Xn5b;
d1b += a1 * acc5b;
d2b += a2 * acc5b;
/* Sample 11. 5 cycles */
acc6a = b0 * Xn6a + d1a;
d1a = b1 * Xn6a + d2a;
d2a = b2 * Xn6a;
d1a += a1 * acc6a;
d2a += a2 * acc6a;
/* Sample 12. 5 cycles */
acc6b = b0 * Xn6b + d1b;
d1b = b1 * Xn6b + d2b;
d2b = b2 * Xn6b;
d1b += a1 * acc6b;
d2b += a2 * acc6b;
/* Sample 13. 5 cycles */
acc7a = b0 * Xn7a + d1a;
d1a = b1 * Xn7a + d2a;
pOut[0 ] = acc1a ;
d2a = b2 * Xn7a;
pOut[1 ] = acc1b ;
d1a += a1 * acc7a;
pOut[2 ] = acc2a ;
d2a += a2 * acc7a;
/* Sample 14. 5 cycles */
pOut[3 ] = acc2b ;
acc7b = b0 * Xn7b + d1b;
pOut[4 ] = acc3a ;
d1b = b1 * Xn7b + d2b;
pOut[5 ] = acc3b ;
d2b = b2 * Xn7b;
pOut[6 ] = acc4a ;
d1b += a1 * acc7b;
pOut[7 ] = acc4b ;
d2b += a2 * acc7b;
/* Sample 15. 5 cycles */
pOut[8 ] = acc5a ;
acc8a = b0 * Xn8a + d1a;
pOut[9 ] = acc5b;
d1a = b1 * Xn8a + d2a;
pOut[10] = acc6a;
d2a = b2 * Xn8a;
pOut[11] = acc6b;
d1a += a1 * acc8a;
pOut[12] = acc7a;
d2a += a2 * acc8a;
/* Sample 16. 5 cycles */
pOut[13] = acc7b;
acc8b = b0 * Xn8b + d1b;
pOut[14] = acc8a;
d1b = b1 * Xn8b + d2b;
pOut[15] = acc8b;
d2b = b2 * Xn8b;
sample--;
d1b += a1 * acc8b;
pOut += 16;
d2b += a2 * acc8b;
}
sample = blockSize & 0x7u;
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);
sample--;
}
/* Store the updated state variables back into the state array */
pState[0] = d1a;
pState[1] = d2a;
pState[2] = d1b;
pState[3] = d2b;
/* The current stage input is given as the output to the next stage */
pIn = pDst;
/* decrement the loop counter */
stage--;
pState += 4u;
/*Reset the output working pointer */
pOut = pDst;
} while(stage > 0u);
#elif defined(ARM_MATH_CM0_FAMILY)
/* Run the below code for Cortex-M0 */
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);
#else
float32_t Xn2a, Xn3a, Xn4a; /* Input State variables */
float32_t Xn2b, Xn3b, Xn4b; /* Input State variables */
float32_t acc2a, acc3a, acc4a; /* accumulator */
float32_t acc2b, acc3b, acc4b; /* accumulator */
float32_t p0a, p1a, p2a, p3a, p4a, A1a;
float32_t p0b, p1b, p2b, p3b, p4b, A1b;
/* Run the below code for Cortex-M4 and Cortex-M3 */
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];
/* Apply loop unrolling and compute 4 output values simultaneously. */
sample = blockSize >> 2u;
/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
** a second loop below computes the remaining 1 to 3 samples. */
while(sample > 0u) {
/* y[n] = b0 * x[n] + d1 */
/* d1 = b1 * x[n] + a1 * y[n] + d2 */
/* d2 = b2 * x[n] + a2 * y[n] */
/* Read the four inputs */
Xn1a = pIn[0];
Xn1b = pIn[1];
Xn2a = pIn[2];
Xn2b = pIn[3];
Xn3a = pIn[4];
Xn3b = pIn[5];
Xn4a = pIn[6];
Xn4b = pIn[7];
pIn += 8;
p0a = b0 * Xn1a;
p0b = b0 * Xn1b;
p1a = b1 * Xn1a;
p1b = b1 * Xn1b;
acc1a = p0a + d1a;
acc1b = p0b + d1b;
p0a = b0 * Xn2a;
p0b = b0 * Xn2b;
p3a = a1 * acc1a;
p3b = a1 * acc1b;
p2a = b2 * Xn1a;
p2b = b2 * Xn1b;
A1a = p1a + p3a;
A1b = p1b + p3b;
p4a = a2 * acc1a;
p4b = a2 * acc1b;
d1a = A1a + d2a;
d1b = A1b + d2b;
d2a = p2a + p4a;
d2b = p2b + p4b;
p1a = b1 * Xn2a;
p1b = b1 * Xn2b;
acc2a = p0a + d1a;
acc2b = p0b + d1b;
p0a = b0 * Xn3a;
p0b = b0 * Xn3b;
p3a = a1 * acc2a;
p3b = a1 * acc2b;
p2a = b2 * Xn2a;
p2b = b2 * Xn2b;
A1a = p1a + p3a;
A1b = p1b + p3b;
p4a = a2 * acc2a;
p4b = a2 * acc2b;
d1a = A1a + d2a;
d1b = A1b + d2b;
d2a = p2a + p4a;
d2b = p2b + p4b;
p1a = b1 * Xn3a;
p1b = b1 * Xn3b;
acc3a = p0a + d1a;
acc3b = p0b + d1b;
p0a = b0 * Xn4a;
p0b = b0 * Xn4b;
p3a = a1 * acc3a;
p3b = a1 * acc3b;
p2a = b2 * Xn3a;
p2b = b2 * Xn3b;
A1a = p1a + p3a;
A1b = p1b + p3b;
p4a = a2 * acc3a;
p4b = a2 * acc3b;
d1a = A1a + d2a;
d1b = A1b + d2b;
d2a = p2a + p4a;
d2b = p2b + p4b;
acc4a = p0a + d1a;
acc4b = p0b + d1b;
p1a = b1 * Xn4a;
p1b = b1 * Xn4b;
p3a = a1 * acc4a;
p3b = a1 * acc4b;
p2a = b2 * Xn4a;
p2b = b2 * Xn4b;
A1a = p1a + p3a;
A1b = p1b + p3b;
p4a = a2 * acc4a;
p4b = a2 * acc4b;
d1a = A1a + d2a;
d1b = A1b + d2b;
d2a = p2a + p4a;
d2b = p2b + p4b;
pOut[0] = acc1a;
pOut[1] = acc1b;
pOut[2] = acc2a;
pOut[3] = acc2b;
pOut[4] = acc3a;
pOut[5] = acc3b;
pOut[6] = acc4a;
pOut[7] = acc4b;
pOut += 8;
sample--;
}
sample = blockSize & 0x3u;
while(sample > 0u) {
Xn1a = *pIn++;
Xn1b = *pIn++;
p0a = b0 * Xn1a;
p0b = b0 * Xn1b;
p1a = b1 * Xn1a;
p1b = b1 * Xn1b;
acc1a = p0a + d1a;
acc1b = p0b + d1b;
p3a = a1 * acc1a;
p3b = a1 * acc1b;
p2a = b2 * Xn1a;
p2b = b2 * Xn1b;
A1a = p1a + p3a;
A1b = p1b + p3b;
p4a = a2 * acc1a;
p4b = a2 * acc1b;
d1a = A1a + d2a;
d1b = A1b + d2b;
d2a = p2a + p4a;
d2b = p2b + p4b;
*pOut++ = acc1a;
*pOut++ = acc1b;
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);
#endif
}