Update arm_biquad_cascade_df2T_f64.c

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
+
 
 
 /**