From 966097ab7b614c3f0589302baccf06c74fa43621 Mon Sep 17 00:00:00 2001 From: Silfurion Date: Thu, 11 Aug 2022 14:57:47 +0200 Subject: [PATCH] Update arm_biquad_cascade_df2T_f64.c --- .../arm_biquad_cascade_df2T_f64.c | 162 +++++++++++++++++- 1 file changed, 160 insertions(+), 2 deletions(-) diff --git a/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c b/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c index 521f63b3..b81f7cfe 100644 --- a/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c +++ b/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c @@ -3,8 +3,8 @@ * Title: arm_biquad_cascade_df2T_f64.c * Description: Processing function for floating-point transposed direct form II Biquad cascade filter * - * $Date: 23 April 2021 - * $Revision: V1.9.0 + * $Date: 03 June 2022 + * $Revision: V1.9.1 * * Target Processor: Cortex-M and Cortex-A cores * -------------------------------------------------------------------- */ @@ -132,7 +132,163 @@ @param[in] blockSize number of samples to process @return none */ +#if defined(ARM_MATH_NEON) +void arm_biquad_cascade_df2T_f64( + const arm_biquad_cascade_df2T_instance_f64 * S, + const float64_t * pSrc, + float64_t * pDst, + uint32_t blockSize) +{ + + + + const float64_t *pIn = pSrc; /* source pointer */ + float64_t Xn0, Xn1; + float64_t acc0, acc1; + float64_t *pOut = pDst; /* destination pointer */ + float64_t *pState = S->pState; /* State pointer */ + uint32_t sample, stage = S->numStages; /* loop counters */ + float64_t const *pCurCoeffs = /* coefficient pointer */ + (float64_t const *) S->pCoeffs; + float64x2_t b0Coeffs, a0Coeffs; /* Coefficients vector */ + float64x2_t state; /* State vector*/ + + float64_t b0 ; + + + do + { + + /* Reading the coefficients */ + b0 = *pCurCoeffs++ ; + b0Coeffs = vld1q_f64(pCurCoeffs); + pCurCoeffs += 2 ; + a0Coeffs = vld1q_f64(pCurCoeffs); + pCurCoeffs +=2 ; + + state = vld1q_f64(pState); + + sample = blockSize >> 1U; + while (sample > 0U) + { + + /* y[n] = b0 * x[n] + d1 */ + /* d1 = b1 * x[n] + a1 * y[n] + d2 */ + /* d2 = b2 * x[n] + a2 * y[n] */ + + Xn0 = *pIn++ ; + + /* Calculation of acc0*/ + acc0 = b0*Xn0+vgetq_lane_f64(state, 0); + + + /* + * final initial + * state b0Coeffs state a0Coeffs + * | | | | + * __ __ __ __ + * / \ / \ / \ / \ + * | d1 | = | b1 | * Xn0 + | d2 | + | a1 | x acc0 + * | d2 | | b2 | | 0 | | a2 | + * \__/ \__/ \__/ \__/ + */ + + /* state -> initial state (see above) */ + state = vsetq_lane_f64(vgetq_lane_f64(state, 1), state, 0); + state = vsetq_lane_f64(0.0f, state , 1); + + /* Calculation of final state */ + state = vfmaq_n_f64(state, b0Coeffs, Xn0); + state = vfmaq_n_f64(state, a0Coeffs, acc0); + + *pOut++ = acc0 ; + + /* y[n] = b0 * x[n] + d1 */ + /* d1 = b1 * x[n] + a1 * y[n] + d2 */ + /* d2 = b2 * x[n] + a2 * y[n] */ + + Xn0 = *pIn++ ; + + /* Calculation of acc0*/ + acc0 = b0*Xn0+vgetq_lane_f64(state, 0); + + + /* + * final initial + * state b0Coeffs state a0Coeffs + * | | | | + * __ __ __ __ + * / \ / \ / \ / \ + * | d1 | = | b1 | * Xn0 + | d2 | + | a1 | x acc0 + * | d2 | | b2 | | 0 | | a2 | + * \__/ \__/ \__/ \__/ + */ + + /* state -> initial state (see above) */ + state = vsetq_lane_f64(vgetq_lane_f64(state, 1), state, 0); + state = vsetq_lane_f64(0.0f, state , 1); + + /* Calculation of final state */ + state = vfmaq_n_f64(state, b0Coeffs, Xn0); + state = vfmaq_n_f64(state, a0Coeffs, acc0); + + *pOut++ = acc0; + sample--; + } + sample = blockSize & 1 ; + while (sample > 0U) + { + + /* y[n] = b0 * x[n] + d1 */ + /* d1 = b1 * x[n] + a1 * y[n] + d2 */ + /* d2 = b2 * x[n] + a2 * y[n] */ + + Xn0 = *pIn++ ; + + /* Calculation of acc0*/ + acc0 = b0*Xn0+vgetq_lane_f64(state, 0); + + + /* + * final initial + * state b0Coeffs state a0Coeffs + * | | | | + * __ __ __ __ + * / \ / \ / \ / \ + * | d1 | = | b1 | * Xn0 + | d2 | + | a1 | x acc0 + * | d2 | | b2 | | 0 | | a2 | + * \__/ \__/ \__/ \__/ + */ + + /* state -> initial state (see above) */ + state = vsetq_lane_f64(vgetq_lane_f64(state, 1), state, 0); + state = vsetq_lane_f64(0.0f, state , 1); + + /* Calculation of final state */ + state = vfmaq_n_f64(state, b0Coeffs, Xn0); + state = vfmaq_n_f64(state, a0Coeffs, acc0); + + *pOut++ = acc0 ; + sample--; + } + /* Store the updated state variables back into the state array */ + pState[0] = vgetq_lane_f64(state, 0); + pState[1] = vgetq_lane_f64(state, 1); + pState += 2U; + + /* The current stage output is given as the input to the next stage */ + pIn = pDst; + + /* Reset the output working pointer */ + pOut = pDst; + + stage--; + + } while (stage > 0U); + +} +#else void arm_biquad_cascade_df2T_f64( const arm_biquad_cascade_df2T_instance_f64 * S, @@ -436,6 +592,8 @@ void arm_biquad_cascade_df2T_f64( } while (stage > 0U); } +#endif + /**