diff options
Diffstat (limited to 'Drivers/CMSIS/DSP/Source/FilteringFunctions')
101 files changed, 35566 insertions, 0 deletions
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/CMakeLists.txt b/Drivers/CMSIS/DSP/Source/FilteringFunctions/CMakeLists.txt new file mode 100644 index 0000000..039a7a4 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/CMakeLists.txt @@ -0,0 +1,128 @@ +cmake_minimum_required (VERSION 3.6)
+
+project(CMSISDSPFiltering)
+
+
+add_library(CMSISDSPFiltering STATIC)
+
+include(interpol)
+interpol(CMSISDSPFiltering)
+
+configdsp(CMSISDSPFiltering ..)
+
+if (CONFIGTABLE AND ALLFAST)
+target_compile_definitions(CMSISDSPFiltering PUBLIC ARM_ALL_FAST_TABLES)
+endif()
+
+if (NOT CONFIGTABLE OR ALLFAST OR ARM_LMS_NORM_Q31)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_norm_init_q31.c)
+endif()
+
+if (NOT CONFIGTABLE OR ALLFAST OR ARM_LMS_NORM_Q15)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_norm_init_q15.c)
+endif()
+
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_32x64_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_32x64_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_fast_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_fast_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df2T_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df2T_f64.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df2T_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df2T_init_f64.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_stereo_df2T_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_stereo_df2T_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_fast_opt_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_fast_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_fast_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_opt_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_opt_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_fast_opt_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_fast_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_fast_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_opt_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_opt_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_partial_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_conv_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_fast_opt_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_fast_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_fast_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_opt_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_opt_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_correlate_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_fast_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_fast_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_decimate_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_fast_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_fast_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_init_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_interpolate_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_interpolate_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_interpolate_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_interpolate_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_interpolate_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_interpolate_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_lattice_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_lattice_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_lattice_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_lattice_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_lattice_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_lattice_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_init_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_fir_sparse_q7.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_iir_lattice_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_iir_lattice_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_iir_lattice_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_iir_lattice_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_iir_lattice_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_iir_lattice_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_init_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_init_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_norm_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_norm_init_f32.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_norm_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_norm_q31.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_q15.c)
+target_sources(CMSISDSPFiltering PRIVATE arm_lms_q31.c)
+
+
+### Includes
+target_include_directories(CMSISDSPFiltering PUBLIC "${DSP}/../../Include")
+
+
+
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/FilteringFunctions.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/FilteringFunctions.c new file mode 100644 index 0000000..aa2947a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/FilteringFunctions.c @@ -0,0 +1,127 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: FilteringFunctions.c
+ * Description: Combination of all filtering function source files.
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.0.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_biquad_cascade_df1_32x64_init_q31.c"
+#include "arm_biquad_cascade_df1_32x64_q31.c"
+#include "arm_biquad_cascade_df1_f32.c"
+#include "arm_biquad_cascade_df1_fast_q15.c"
+#include "arm_biquad_cascade_df1_fast_q31.c"
+#include "arm_biquad_cascade_df1_init_f32.c"
+#include "arm_biquad_cascade_df1_init_q15.c"
+#include "arm_biquad_cascade_df1_init_q31.c"
+#include "arm_biquad_cascade_df1_q15.c"
+#include "arm_biquad_cascade_df1_q31.c"
+#include "arm_biquad_cascade_df2T_f32.c"
+#include "arm_biquad_cascade_df2T_f64.c"
+#include "arm_biquad_cascade_df2T_init_f32.c"
+#include "arm_biquad_cascade_df2T_init_f64.c"
+#include "arm_biquad_cascade_stereo_df2T_f32.c"
+#include "arm_biquad_cascade_stereo_df2T_init_f32.c"
+#include "arm_conv_f32.c"
+#include "arm_conv_fast_opt_q15.c"
+#include "arm_conv_fast_q15.c"
+#include "arm_conv_fast_q31.c"
+#include "arm_conv_opt_q15.c"
+#include "arm_conv_opt_q7.c"
+#include "arm_conv_partial_f32.c"
+#include "arm_conv_partial_fast_opt_q15.c"
+#include "arm_conv_partial_fast_q15.c"
+#include "arm_conv_partial_fast_q31.c"
+#include "arm_conv_partial_opt_q15.c"
+#include "arm_conv_partial_opt_q7.c"
+#include "arm_conv_partial_q15.c"
+#include "arm_conv_partial_q31.c"
+#include "arm_conv_partial_q7.c"
+#include "arm_conv_q15.c"
+#include "arm_conv_q31.c"
+#include "arm_conv_q7.c"
+#include "arm_correlate_f32.c"
+#include "arm_correlate_fast_opt_q15.c"
+#include "arm_correlate_fast_q15.c"
+#include "arm_correlate_fast_q31.c"
+#include "arm_correlate_opt_q15.c"
+#include "arm_correlate_opt_q7.c"
+#include "arm_correlate_q15.c"
+#include "arm_correlate_q31.c"
+#include "arm_correlate_q7.c"
+#include "arm_fir_decimate_f32.c"
+#include "arm_fir_decimate_fast_q15.c"
+#include "arm_fir_decimate_fast_q31.c"
+#include "arm_fir_decimate_init_f32.c"
+#include "arm_fir_decimate_init_q15.c"
+#include "arm_fir_decimate_init_q31.c"
+#include "arm_fir_decimate_q15.c"
+#include "arm_fir_decimate_q31.c"
+#include "arm_fir_f32.c"
+#include "arm_fir_fast_q15.c"
+#include "arm_fir_fast_q31.c"
+#include "arm_fir_init_f32.c"
+#include "arm_fir_init_q15.c"
+#include "arm_fir_init_q31.c"
+#include "arm_fir_init_q7.c"
+#include "arm_fir_interpolate_f32.c"
+#include "arm_fir_interpolate_init_f32.c"
+#include "arm_fir_interpolate_init_q15.c"
+#include "arm_fir_interpolate_init_q31.c"
+#include "arm_fir_interpolate_q15.c"
+#include "arm_fir_interpolate_q31.c"
+#include "arm_fir_lattice_f32.c"
+#include "arm_fir_lattice_init_f32.c"
+#include "arm_fir_lattice_init_q15.c"
+#include "arm_fir_lattice_init_q31.c"
+#include "arm_fir_lattice_q15.c"
+#include "arm_fir_lattice_q31.c"
+#include "arm_fir_q15.c"
+#include "arm_fir_q31.c"
+#include "arm_fir_q7.c"
+#include "arm_fir_sparse_f32.c"
+#include "arm_fir_sparse_init_f32.c"
+#include "arm_fir_sparse_init_q15.c"
+#include "arm_fir_sparse_init_q31.c"
+#include "arm_fir_sparse_init_q7.c"
+#include "arm_fir_sparse_q15.c"
+#include "arm_fir_sparse_q31.c"
+#include "arm_fir_sparse_q7.c"
+#include "arm_iir_lattice_f32.c"
+#include "arm_iir_lattice_init_f32.c"
+#include "arm_iir_lattice_init_q15.c"
+#include "arm_iir_lattice_init_q31.c"
+#include "arm_iir_lattice_q15.c"
+#include "arm_iir_lattice_q31.c"
+#include "arm_lms_f32.c"
+#include "arm_lms_init_f32.c"
+#include "arm_lms_init_q15.c"
+#include "arm_lms_init_q31.c"
+#include "arm_lms_norm_f32.c"
+#include "arm_lms_norm_init_f32.c"
+#include "arm_lms_norm_init_q15.c"
+#include "arm_lms_norm_init_q31.c"
+#include "arm_lms_norm_q15.c"
+#include "arm_lms_norm_q31.c"
+#include "arm_lms_q15.c"
+#include "arm_lms_q31.c"
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c new file mode 100644 index 0000000..80e8f58 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_init_q31.c @@ -0,0 +1,94 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_32x64_init_q31.c
+ * Description: High precision Q31 Biquad cascade filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1_32x64
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 Biquad cascade 32x64 filter.
+ @param[in,out] S points to an instance of the high precision Q31 Biquad cascade filter structure
+ @param[in] numStages number of 2nd order stages in the filter
+ @param[in] pCoeffs points to the filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] postShift Shift to be applied after the accumulator. Varies according to the coefficients format
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order:
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
+ @par
+ The <code>pState</code> points to state variables array and size of each state variable is 1.63 format.
+ Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
+ The state variables are arranged in the state array as:
+ <pre>
+ {x[n-1], x[n-2], y[n-1], y[n-2]}
+ </pre>
+ The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
+ The state array has a total length of <code>4*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+void arm_biquad_cas_df1_32x64_init_q31(
+ arm_biquad_cas_df1_32x64_ins_q31 * S,
+ uint8_t numStages,
+ const q31_t * pCoeffs,
+ q63_t * pState,
+ uint8_t postShift)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign postShift to be applied to the output */
+ S->postShift = postShift;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 4 * numStages */
+ memset(pState, 0, (4U * (uint32_t) numStages) * sizeof(q63_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF1_32x64 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c new file mode 100644 index 0000000..e7e41ed --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c @@ -0,0 +1,458 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_32x64_q31.c
+ * Description: High precision Q31 Biquad cascade filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup BiquadCascadeDF1_32x64 High Precision Q31 Biquad Cascade Filter
+
+ This function implements a high precision Biquad cascade filter which operates on
+ Q31 data values. The filter coefficients are in 1.31 format and the state variables
+ are in 1.63 format. The double precision state variables reduce quantization noise
+ in the filter and provide a cleaner output.
+ These filters are particularly useful when implementing filters in which the
+ singularities are close to the unit circle. This is common for low pass or high
+ pass filters with very low cutoff frequencies.
+
+ The function operates on blocks of input and output data
+ and each call to the function processes <code>blockSize</code> samples through
+ the filter. <code>pSrc</code> and <code>pDst</code> points to input and output arrays
+ containing <code>blockSize</code> Q31 values.
+
+ @par Algorithm
+ Each Biquad stage implements a second order filter using the difference equation:
+ <pre>
+ y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ </pre>
+ A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.
+ \image html Biquad.gif "Single Biquad filter stage"
+ Coefficients <code>b0, b1 and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
+ Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
+ Pay careful attention to the sign of the feedback coefficients.
+ Some design tools use the difference equation
+ <pre>
+ y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
+ </pre>
+ In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
+ @par
+ Higher order filters are realized as a cascade of second order sections.
+ <code>numStages</code> refers to the number of second order stages used.
+ For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
+ \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"
+ A 9th order filter would be realized with <code>numStages=5</code> second order stages
+ with the coefficients for one of the stages configured as a first order filter
+ (<code>b2=0</code> and <code>a2=0</code>).
+ @par
+ The <code>pState</code> points to state variables array.
+ Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code> and each state variable in 1.63 format to improve precision.
+ The state variables are arranged in the array as:
+ <pre>
+ {x[n-1], x[n-2], y[n-1], y[n-2]}
+ </pre>
+ @par
+ The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
+ The state array has a total length of <code>4*numStages</code> values of data in 1.63 format.
+ The state variables are updated after each block of data is processed, the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
+
+ @par Init Function
+ There is also an associated initialization function which performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numStages, pCoeffs, postShift, pState. Also set all of the values in pState to zero.
+
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros before static initialization.
+ For example, to statically initialize the filter instance structure use
+ <pre>
+ arm_biquad_cas_df1_32x64_ins_q31 S1 = {numStages, pState, pCoeffs, postShift};
+ </pre>
+ where <code>numStages</code> is the number of Biquad stages in the filter;
+ <code>pState</code> is the address of the state buffer;
+ <code>pCoeffs</code> is the address of the coefficient buffer;
+ <code>postShift</code> shift to be applied which is described in detail below.
+ @par Fixed-Point Behavior
+ Care must be taken while using Biquad Cascade 32x64 filter function.
+ Following issues must be considered:
+ - Scaling of coefficients
+ - Filter gain
+ - Overflow and saturation
+
+ @par
+ Filter coefficients are represented as fractional values and
+ restricted to lie in the range <code>[-1 +1)</code>.
+ The processing function has an additional scaling parameter <code>postShift</code>
+ which allows the filter coefficients to exceed the range <code>[+1 -1)</code>.
+ At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
+ \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"
+ This essentially scales the filter coefficients by <code>2^postShift</code>.
+ For example, to realize the coefficients
+ <pre>
+ {1.5, -0.8, 1.2, 1.6, -0.9}
+ </pre>
+ set the Coefficient array to:
+ <pre>
+ {0.75, -0.4, 0.6, 0.8, -0.45}
+ </pre>
+ and set <code>postShift=1</code>
+ @par
+ The second thing to keep in mind is the gain through the filter.
+ The frequency response of a Biquad filter is a function of its coefficients.
+ It is possible for the gain through the filter to exceed 1.0 meaning that the
+ filter increases the amplitude of certain frequencies.
+ This means that an input signal with amplitude < 1.0 may result in an output > 1.0
+ and these are saturated or overflowed based on the implementation of the filter.
+ To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0
+ or the input signal must be scaled down so that the combination of input and filter are never overflowed.
+ @par
+ The third item to consider is the overflow and saturation behavior of the fixed-point Q31 version.
+ This is described in the function specific documentation below.
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1_32x64
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 Biquad cascade 32x64 filter.
+ @param[in] S points to an instance of the high precision Q31 Biquad cascade filter
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Details
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by 2 bits and lie in the range [-0.25 +0.25).
+ After all 5 multiply-accumulates are performed, the 2.62 accumulator is shifted by <code>postShift</code> bits and the result truncated to
+ 1.31 format by discarding the low 32 bits.
+ @par
+ Two related functions are provided in the CMSIS DSP library.
+ - \ref arm_biquad_cascade_df1_q31() implements a Biquad cascade with 32-bit coefficients and state variables with a Q63 accumulator.
+ - \ref arm_biquad_cascade_df1_fast_q31() implements a Biquad cascade with 32-bit coefficients and state variables with a Q31 accumulator.
+ */
+
+void arm_biquad_cas_df1_32x64_q31(
+ const arm_biquad_cas_df1_32x64_ins_q31 * S,
+ q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pIn = pSrc; /* input pointer initialization */
+ q31_t *pOut = pDst; /* output pointer initialization */
+ q63_t *pState = S->pState; /* state pointer initialization */
+ const q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
+ q63_t acc; /* accumulator */
+ q31_t Xn1, Xn2; /* Input Filter state variables */
+ q63_t Yn1, Yn2; /* Output Filter state variables */
+ q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ q31_t Xn; /* temporary input */
+ int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
+ uint32_t sample, stage = S->numStages; /* loop counters */
+ q31_t acc_l, acc_h; /* temporary output */
+ uint32_t uShift = ((uint32_t) S->postShift + 1U);
+ uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = *pCoeffs++;
+ b1 = *pCoeffs++;
+ b2 = *pCoeffs++;
+ a1 = *pCoeffs++;
+ a2 = *pCoeffs++;
+
+ /* Reading the state values */
+ Xn1 = (q31_t) (pState[0]);
+ Xn2 = (q31_t) (pState[1]);
+ Yn1 = pState[2];
+ Yn2 = pState[3];
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Apply loop unrolling and compute 4 output values simultaneously. */
+ /* Variable acc hold output value that is being computed and stored in destination buffer
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ sample = blockSize >> 2U;
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+ /* acc = b0 * x[n] */
+ acc = (q63_t) Xn * b0;
+
+ /* acc += b1 * x[n-1] */
+ acc += (q63_t) Xn1 * b1;
+
+ /* acc += b[2] * x[n-2] */
+ acc += (q63_t) Xn2 * b2;
+
+ /* acc += a1 * y[n-1] */
+ acc += mult32x64(Yn1, a1);
+
+ /* acc += a2 * y[n-2] */
+ acc += mult32x64(Yn2, a2);
+
+ /* The result is converted to 1.63 , Yn2 variable is reused */
+ Yn2 = acc << shift;
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store the output in the destination buffer in 1.31 format. */
+ *pOut = acc_h;
+
+ /* Read the second input into Xn2, to reuse the value */
+ Xn2 = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+ /* acc += b1 * x[n-1] */
+ acc = (q63_t) Xn * b1;
+
+ /* acc = b0 * x[n] */
+ acc += (q63_t) Xn2 * b0;
+
+ /* acc += b[2] * x[n-2] */
+ acc += (q63_t) Xn1 * b2;
+
+ /* acc += a1 * y[n-1] */
+ acc += mult32x64(Yn2, a1);
+
+ /* acc += a2 * y[n-2] */
+ acc += mult32x64(Yn1, a2);
+
+ /* The result is converted to 1.63, Yn1 variable is reused */
+ Yn1 = acc << shift;
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Read the third input into Xn1, to reuse the value */
+ Xn1 = *pIn++;
+
+ /* The result is converted to 1.31 */
+ /* Store the output in the destination buffer. */
+ *(pOut + 1U) = acc_h;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+ /* acc = b0 * x[n] */
+ acc = (q63_t) Xn1 * b0;
+
+ /* acc += b1 * x[n-1] */
+ acc += (q63_t) Xn2 * b1;
+
+ /* acc += b[2] * x[n-2] */
+ acc += (q63_t) Xn * b2;
+
+ /* acc += a1 * y[n-1] */
+ acc += mult32x64(Yn1, a1);
+
+ /* acc += a2 * y[n-2] */
+ acc += mult32x64(Yn2, a2);
+
+ /* The result is converted to 1.63, Yn2 variable is reused */
+ Yn2 = acc << shift;
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store the output in the destination buffer in 1.31 format. */
+ *(pOut + 2U) = acc_h;
+
+ /* Read the fourth input into Xn, to reuse the value */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ acc = (q63_t) Xn * b0;
+
+ /* acc += b1 * x[n-1] */
+ acc += (q63_t) Xn1 * b1;
+
+ /* acc += b[2] * x[n-2] */
+ acc += (q63_t) Xn2 * b2;
+
+ /* acc += a1 * y[n-1] */
+ acc += mult32x64(Yn2, a1);
+
+ /* acc += a2 * y[n-2] */
+ acc += mult32x64(Yn1, a2);
+
+ /* The result is converted to 1.63, Yn1 variable is reused */
+ Yn1 = acc << shift;
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store the output in the destination buffer in 1.31 format. */
+ *(pOut + 3U) = acc_h;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+
+ /* update output pointer */
+ pOut += 4U;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = blockSize & 0x3U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+
+ /* acc = b0 * x[n] */
+ acc = (q63_t) Xn * b0;
+ /* acc += b1 * x[n-1] */
+ acc += (q63_t) Xn1 * b1;
+ /* acc += b[2] * x[n-2] */
+ acc += (q63_t) Xn2 * b2;
+ /* acc += a1 * y[n-1] */
+ acc += mult32x64(Yn1, a1);
+ /* acc += a2 * y[n-2] */
+ acc += mult32x64(Yn2, a2);
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+ Yn2 = Yn1;
+
+ /* The result is converted to 1.63, Yn1 variable is reused */
+ Yn1 = acc << shift;
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store the output in the destination buffer in 1.31 format. */
+ *pOut++ = acc_h;
+ /* Yn1 = acc << shift; */
+
+ /* Store the output in the destination buffer in 1.31 format. */
+/* *pOut++ = (q31_t) (acc >> (32 - shift)); */
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* The first stage output is given as input to the second stage. */
+ pIn = pDst;
+
+ /* Reset to destination buffer working pointer */
+ pOut = pDst;
+
+ /* Store the updated state variables back into the pState array */
+ *pState++ = (q63_t) Xn1;
+ *pState++ = (q63_t) Xn2;
+ *pState++ = Yn1;
+ *pState++ = Yn2;
+
+ } while (--stage);
+
+}
+
+/**
+ @} end of BiquadCascadeDF1_32x64 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c new file mode 100644 index 0000000..ea75d3b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_f32.c @@ -0,0 +1,495 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_f32.c
+ * Description: Processing function for the floating-point Biquad cascade DirectFormI(DF1) filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup BiquadCascadeDF1 Biquad Cascade IIR Filters Using Direct Form I Structure
+
+ This set of functions implements arbitrary order recursive (IIR) filters.
+ The filters are implemented as a cascade of second order Biquad sections.
+ The functions support Q15, Q31 and floating-point data types.
+ Fast version of Q15 and Q31 also available.
+
+ The functions operate on blocks of input and output data and each call to the function
+ processes <code>blockSize</code> samples through the filter.
+ <code>pSrc</code> points to the array of input data and
+ <code>pDst</code> points to the array of output data.
+ Both arrays contain <code>blockSize</code> values.
+
+ @par Algorithm
+ Each Biquad stage implements a second order filter using the difference equation:
+ <pre>
+ y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ </pre>
+ A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.
+ \image html Biquad.gif "Single Biquad filter stage"
+ Coefficients <code>b0, b1 and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
+ Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
+ Pay careful attention to the sign of the feedback coefficients.
+ Some design tools use the difference equation
+ <pre>
+ y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
+ </pre>
+ In this case the feedback coefficients <code>a1</code> and <code>a2</code>
+ must be negated when used with the CMSIS DSP Library.
+
+ @par
+ Higher order filters are realized as a cascade of second order sections.
+ <code>numStages</code> refers to the number of second order stages used.
+ For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
+ \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"
+ A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
+
+ @par
+ The <code>pState</code> points to state variables array.
+ Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
+ The state variables are arranged in the <code>pState</code> array as:
+ <pre>
+ {x[n-1], x[n-2], y[n-1], y[n-2]}
+ </pre>
+
+ @par
+ The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
+ The state array has a total length of <code>4*numStages</code> values.
+ The state variables are updated after each block of data is processed, the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Init Function
+ There is also an associated initialization function for each data type.
+ The initialization function performs following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numStages, pCoeffs, pState. Also set all of the values in pState to zero.
+
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros before static initialization.
+ The code below statically initializes each of the 3 different data type filter instance structures
+ <pre>
+ arm_biquad_casd_df1_inst_f32 S1 = {numStages, pState, pCoeffs};
+ arm_biquad_casd_df1_inst_q15 S2 = {numStages, pState, pCoeffs, postShift};
+ arm_biquad_casd_df1_inst_q31 S3 = {numStages, pState, pCoeffs, postShift};
+ </pre>
+ where <code>numStages</code> is the number of Biquad stages in the filter;
+ <code>pState</code> is the address of the state buffer;
+ <code>pCoeffs</code> is the address of the coefficient buffer;
+ <code>postShift</code> shift to be applied.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the Q15 and Q31 versions of the Biquad Cascade filter functions.
+ Following issues must be considered:
+ - Scaling of coefficients
+ - Filter gain
+ - Overflow and saturation
+
+ @par Scaling of coefficients
+ Filter coefficients are represented as fractional values and
+ coefficients are restricted to lie in the range <code>[-1 +1)</code>.
+ The fixed-point functions have an additional scaling parameter <code>postShift</code>
+ which allow the filter coefficients to exceed the range <code>[+1 -1)</code>.
+ At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
+ \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"
+ This essentially scales the filter coefficients by <code>2^postShift</code>.
+ For example, to realize the coefficients
+ <pre>
+ {1.5, -0.8, 1.2, 1.6, -0.9}
+ </pre>
+ set the pCoeffs array to:
+ <pre>
+ {0.75, -0.4, 0.6, 0.8, -0.45}
+ </pre>
+ and set <code>postShift=1</code>
+
+ @par Filter gain
+ The frequency response of a Biquad filter is a function of its coefficients.
+ It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies.
+ This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter.
+ To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed.
+
+ @par Overflow and saturation
+ For Q15 and Q31 versions, it is described separately as part of the function specific documentation below.
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point Biquad cascade filter.
+ @param[in] S points to an instance of the floating-point Biquad cascade structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+#if defined(ARM_MATH_NEON)
+void arm_biquad_cascade_df1_f32(
+ const arm_biquad_casd_df1_inst_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+
+ const float32_t *pIn = pSrc; /* source pointer */
+ float32_t *pOut = pDst; /* destination pointer */
+ float32_t *pState = S->pState; /* pState pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
+ float32_t acc; /* Simulates the accumulator */
+
+ uint32_t sample, stage = S->numStages; /* loop counters */
+
+ float32x4_t Xn;
+ float32x2_t Yn;
+ float32x2_t a;
+ float32x4_t b;
+
+ float32x4_t x,tmp;
+ float32x2_t t;
+ float32x2x2_t y;
+
+ float32_t Xns;
+
+ while (stage > 0U)
+ {
+ /* Reading the coefficients */
+ Xn = vld1q_f32(pState);
+ Yn = vld1_f32(pState + 2);
+
+ b = vld1q_f32(pCoeffs);
+ b = vrev64q_f32(b);
+ b = vcombine_f32(vget_high_f32(b), vget_low_f32(b));
+
+ a = vld1_f32(pCoeffs + 3);
+ a = vrev64_f32(a);
+ b[0] = 0.0;
+ pCoeffs += 5;
+
+ /* Reading the pState values */
+
+ /* Apply loop unrolling and compute 4 output values simultaneously. */
+ /* The variable acc hold output values that are being computed:
+ *
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ /* 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. */
+ sample = blockSize >> 2U;
+
+ while (sample > 0U)
+ {
+ /* Read the first 4 inputs */
+ x = vld1q_f32(pIn);
+
+ pIn += 4;
+
+ tmp = vextq_f32(Xn, x, 1);
+ t = vmul_f32(vget_high_f32(b), vget_high_f32(tmp));
+ t = vmla_f32(t, vget_low_f32(b), vget_low_f32(tmp));
+ t = vmla_f32(t, a, Yn);
+ t = vpadd_f32(t, t);
+ Yn = vext_f32(Yn, t, 1);
+
+ tmp = vextq_f32(Xn, x, 2);
+ t = vmul_f32(vget_high_f32(b), vget_high_f32(tmp));
+ t = vmla_f32(t, vget_low_f32(b), vget_low_f32(tmp));
+ t = vmla_f32(t, a, Yn);
+ t = vpadd_f32(t, t);
+ Yn = vext_f32(Yn, t, 1);
+
+ y.val[0] = Yn;
+
+ tmp = vextq_f32(Xn, x, 3);
+ t = vmul_f32(vget_high_f32(b), vget_high_f32(tmp));
+ t = vmla_f32(t, vget_low_f32(b), vget_low_f32(tmp));
+ t = vmla_f32(t, a, Yn);
+ t = vpadd_f32(t, t);
+ Yn = vext_f32(Yn, t, 1);
+
+ Xn = x;
+ t = vmul_f32(vget_high_f32(b), vget_high_f32(Xn));
+ t = vmla_f32(t, vget_low_f32(b), vget_low_f32(Xn));
+ t = vmla_f32(t, a, Yn);
+ t = vpadd_f32(t, t);
+ Yn = vext_f32(Yn, t, 1);
+
+ y.val[1] = Yn;
+
+ tmp = vcombine_f32(y.val[0], y.val[1]);
+
+ /* Store the 4 outputs and increment the pointer */
+ vst1q_f32(pOut, tmp);
+ pOut += 4;
+
+ /* Decrement the loop counter */
+ sample--;
+ }
+
+ /* If the block size is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ sample = blockSize & 0x3U;
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xns = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = (b[1] * Xn[2]) + (b[2] * Xn[3]) + (b[3] * Xns) + (a[0] * Yn[0]) + (a[1] * Yn[1]);
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = acc;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn[2] = Xn[3];
+ Xn[3] = Xns;
+ Yn[0] = Yn[1];
+ Yn[1] = acc;
+
+ /* Decrement the loop counter */
+ sample--;
+
+ }
+
+ vst1q_f32(pState,vcombine_f32(vrev64_f32(vget_high_f32(Xn)),vrev64_f32(Yn)));
+ pState += 4;
+ /* Store the updated state variables back into the pState array */
+
+ /* The first stage goes from the input buffer to the output buffer. */
+ /* Subsequent numStages occur in-place in the output buffer */
+ pIn = pDst;
+
+ /* Reset the output pointer */
+ pOut = pDst;
+
+ /* Decrement the loop counter */
+ stage--;
+ }
+}
+
+#else
+void arm_biquad_cascade_df1_f32(
+ const arm_biquad_casd_df1_inst_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ const float32_t *pIn = pSrc; /* Source pointer */
+ float32_t *pOut = pDst; /* Destination pointer */
+ float32_t *pState = S->pState; /* pState pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t acc; /* Accumulator */
+ float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ float32_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */
+ float32_t Xn; /* Temporary input */
+ uint32_t sample, stage = S->numStages; /* Loop counters */
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = *pCoeffs++;
+ b1 = *pCoeffs++;
+ b2 = *pCoeffs++;
+ a1 = *pCoeffs++;
+ a2 = *pCoeffs++;
+
+ /* Reading the pState values */
+ Xn1 = pState[0];
+ Xn2 = pState[1];
+ Yn1 = pState[2];
+ Yn2 = pState[3];
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Apply loop unrolling and compute 4 output values simultaneously. */
+ /* Variable acc hold output values that are being computed:
+ *
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ sample = blockSize >> 2U;
+
+ while (sample > 0U)
+ {
+ /* Read the first input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ Yn2 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn2;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+
+ /* Read the second input */
+ Xn2 = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ Yn1 = (b0 * Xn2) + (b1 * Xn) + (b2 * Xn1) + (a1 * Yn2) + (a2 * Yn1);
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn1;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+
+ /* Read the third input */
+ Xn1 = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ Yn2 = (b0 * Xn1) + (b1 * Xn2) + (b2 * Xn) + (a1 * Yn1) + (a2 * Yn2);
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn2;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+
+ /* Read the forth input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ Yn1 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn2) + (a2 * Yn1);
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn1;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = blockSize & 0x3U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2);
+
+ /* Store output in destination buffer. */
+ *pOut++ = acc;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+ Yn2 = Yn1;
+ Yn1 = acc;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Store the updated state variables back into the pState array */
+ *pState++ = Xn1;
+ *pState++ = Xn2;
+ *pState++ = Yn1;
+ *pState++ = Yn2;
+
+ /* The first stage goes from the input buffer to the output buffer. */
+ /* Subsequent numStages occur in-place in the output buffer */
+ pIn = pDst;
+
+ /* Reset output pointer */
+ pOut = pDst;
+
+ /* decrement loop counter */
+ stage--;
+
+ } while (stage > 0U);
+
+}
+
+#endif /* #if defined(ARM_MATH_NEON) */
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c new file mode 100644 index 0000000..917e028 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c @@ -0,0 +1,250 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_fast_q15.c
+ * Description: Fast processing function for the Q15 Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 Biquad cascade filter (fast variant).
+ @param[in] S points to an instance of the Q15 Biquad cascade structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process per call
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around and distorts the result.
+ In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25).
+ The 2.30 accumulator is then shifted by <code>postShift</code> bits and the result truncated to 1.15 format by discarding the low 16 bits.
+ @remark
+ Refer to \ref arm_biquad_cascade_df1_q15() for a slower implementation of this function
+ which uses 64-bit accumulation to avoid wrap around distortion. Both the slow and the fast versions use the same instance structure.
+ Use the function \ref arm_biquad_cascade_df1_init_q15() to initialize the filter structure.
+ */
+
+void arm_biquad_cascade_df1_fast_q15(
+ const arm_biquad_casd_df1_inst_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ const q15_t *pIn = pSrc; /* Source pointer */
+ q15_t *pOut = pDst; /* Destination pointer */
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t acc; /* Accumulator */
+ q31_t in; /* Temporary variable to hold input value */
+ q31_t out; /* Temporary variable to hold output value */
+ q31_t b0; /* Temporary variable to hold bo value */
+ q31_t b1, a1; /* Filter coefficients */
+ q31_t state_in, state_out; /* Filter state variables */
+ int32_t shift = (int32_t) (15 - S->postShift); /* Post shift */
+ uint32_t sample, stage = S->numStages; /* Loop counters */
+
+ do
+ {
+ /* Read the b0 and 0 coefficients using SIMD */
+ b0 = read_q15x2_ia ((q15_t **) &pCoeffs);
+
+ /* Read the b1 and b2 coefficients using SIMD */
+ b1 = read_q15x2_ia ((q15_t **) &pCoeffs);
+
+ /* Read the a1 and a2 coefficients using SIMD */
+ a1 = read_q15x2_ia ((q15_t **) &pCoeffs);
+
+ /* Read the input state values from the state buffer: x[n-1], x[n-2] */
+ state_in = read_q15x2_ia (&pState);
+
+ /* Read the output state values from the state buffer: y[n-1], y[n-2] */
+ state_out = read_q15x2_da (&pState);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Apply loop unrolling and compute 2 output values simultaneously. */
+ /* Variable acc hold output values that are being computed:
+ *
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ /* Loop unrolling: Compute 2 outputs at a time */
+ sample = blockSize >> 1U;
+
+ while (sample > 0U)
+ {
+
+ /* Read the input */
+ in = read_q15x2_ia ((q15_t **) &pIn);
+
+ /* out = b0 * x[n] + 0 * 0 */
+ out = __SMUAD(b0, in);
+ /* acc = b1 * x[n-1] + acc += b2 * x[n-2] + out */
+ acc = __SMLAD(b1, state_in, out);
+ /* acc += a1 * y[n-1] + acc += a2 * y[n-2] */
+ acc = __SMLAD(a1, state_out, acc);
+
+ /* The result is converted from 3.29 to 1.31 and then saturation is applied */
+ out = __SSAT((acc >> shift), 16);
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+ /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ state_in = __PKHBT(in, state_in, 16);
+ state_out = __PKHBT(out, state_out, 16);
+#else
+ state_in = __PKHBT(state_in >> 16, (in >> 16), 16);
+ state_out = __PKHBT(state_out >> 16, (out), 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* out = b0 * x[n] + 0 * 0 */
+ out = __SMUADX(b0, in);
+ /* acc0 = b1 * x[n-1] , acc0 += b2 * x[n-2] + out */
+ acc = __SMLAD(b1, state_in, out);
+ /* acc += a1 * y[n-1] + acc += a2 * y[n-2] */
+ acc = __SMLAD(a1, state_out, acc);
+
+ /* The result is converted from 3.29 to 1.31 and then saturation is applied */
+ out = __SSAT((acc >> shift), 16);
+
+ /* Store the output in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(state_out, out, 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(out, state_out >> 16, 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+ /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+#ifndef ARM_MATH_BIG_ENDIAN
+ state_in = __PKHBT(in >> 16, state_in, 16);
+ state_out = __PKHBT(out, state_out, 16);
+#else
+ state_in = __PKHBT(state_in >> 16, in, 16);
+ state_out = __PKHBT(state_out >> 16, out, 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = (blockSize & 0x1U);
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ in = *pIn++;
+
+ /* out = b0 * x[n] + 0 * 0 */
+#ifndef ARM_MATH_BIG_ENDIAN
+ out = __SMUAD(b0, in);
+#else
+ out = __SMUADX(b0, in);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* acc = b1 * x[n-1], acc += b2 * x[n-2] + out */
+ acc = __SMLAD(b1, state_in, out);
+ /* acc += a1 * y[n-1] + acc += a2 * y[n-2] */
+ acc = __SMLAD(a1, state_out, acc);
+
+ /* The result is converted from 3.29 to 1.31 and then saturation is applied */
+ out = __SSAT((acc >> shift), 16);
+
+ /* Store the output in the destination buffer. */
+ *pOut++ = (q15_t) out;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+ /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+#ifndef ARM_MATH_BIG_ENDIAN
+ state_in = __PKHBT(in, state_in, 16);
+ state_out = __PKHBT(out, state_out, 16);
+#else
+ state_in = __PKHBT(state_in >> 16, in, 16);
+ state_out = __PKHBT(state_out >> 16, out, 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* The first stage goes from the input buffer to the output buffer. */
+ /* Subsequent (numStages - 1) occur in-place in the output buffer */
+ pIn = pDst;
+
+ /* Reset the output pointer */
+ pOut = pDst;
+
+ /* Store the updated state variables back into the state array */
+ write_q15x2_ia(&pState, state_in);
+ write_q15x2_ia(&pState, state_out);
+
+ /* Decrement loop counter */
+ stage--;
+
+ } while (stage > 0U);
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c new file mode 100644 index 0000000..4c30cda --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c @@ -0,0 +1,296 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_fast_q31.c
+ * Description: Processing function for the Q31 Fast Biquad cascade DirectFormI(DF1) filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 Biquad cascade filter (fast variant).
+ @param[in] S points to an instance of the Q31 Biquad cascade structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process per call
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This function is optimized for speed at the expense of fixed-point precision and overflow protection.
+ The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
+ These intermediate results are added to a 2.30 accumulator.
+ Finally, the accumulator is saturated and converted to a 1.31 result.
+ The fast version has the same overflow behavior as the standard version and provides less precision since it discards the low 32 bits of each multiplication result.
+ In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25). Use the intialization function
+ arm_biquad_cascade_df1_init_q31() to initialize filter structure.
+ @remark
+ Refer to \ref arm_biquad_cascade_df1_q31() for a slower implementation of this function
+ which uses 64-bit accumulation to provide higher precision. Both the slow and the fast versions use the same instance structure.
+ Use the function \ref arm_biquad_cascade_df1_init_q31() to initialize the filter structure.
+ */
+
+void arm_biquad_cascade_df1_fast_q31(
+ const arm_biquad_casd_df1_inst_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ const q31_t *pIn = pSrc; /* Source pointer */
+ q31_t *pOut = pDst; /* Destination pointer */
+ q31_t *pState = S->pState; /* pState pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t acc = 0; /* Accumulator */
+ q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ q31_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */
+ q31_t Xn; /* Temporary input */
+ int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
+ uint32_t sample, stage = S->numStages; /* Loop counters */
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = *pCoeffs++;
+ b1 = *pCoeffs++;
+ b2 = *pCoeffs++;
+ a1 = *pCoeffs++;
+ a2 = *pCoeffs++;
+
+ /* Reading the pState values */
+ Xn1 = pState[0];
+ Xn2 = pState[1];
+ Yn1 = pState[2];
+ Yn2 = pState[3];
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Apply loop unrolling and compute 4 output values simultaneously. */
+ /* Variables acc ... acc3 hold output values that are being computed:
+ *
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ sample = blockSize >> 2U;
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ /* acc = (q31_t) (((q63_t) b1 * Xn1) >> 32);*/
+ mult_32x32_keep32_R(acc, b1, Xn1);
+ /* acc += b1 * x[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b0 * (Xn))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b0, Xn);
+ /* acc += b[2] * x[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b2, Xn2);
+ /* acc += a1 * y[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a1, Yn1);
+ /* acc += a2 * y[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a2, Yn2);
+
+ /* The result is converted to 1.31 , Yn2 variable is reused */
+ Yn2 = acc << shift;
+
+ /* Read the second input */
+ Xn2 = *(pIn + 1U);
+
+ /* Store the output in the destination buffer. */
+ *pOut = Yn2;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ /* acc = (q31_t) (((q63_t) b0 * (Xn2)) >> 32);*/
+ mult_32x32_keep32_R(acc, b0, Xn2);
+ /* acc += b1 * x[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b1, Xn);
+ /* acc += b[2] * x[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b2, Xn1);
+ /* acc += a1 * y[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a1, Yn2);
+ /* acc += a2 * y[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a2, Yn1);
+
+ /* The result is converted to 1.31, Yn1 variable is reused */
+ Yn1 = acc << shift;
+
+ /* Read the third input */
+ Xn1 = *(pIn + 2U);
+
+ /* Store the output in the destination buffer. */
+ *(pOut + 1U) = Yn1;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ /* acc = (q31_t) (((q63_t) b0 * (Xn1)) >> 32);*/
+ mult_32x32_keep32_R(acc, b0, Xn1);
+ /* acc += b1 * x[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b1, Xn2);
+ /* acc += b[2] * x[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b2, Xn);
+ /* acc += a1 * y[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a1, Yn1);
+ /* acc += a2 * y[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a2, Yn2);
+
+ /* The result is converted to 1.31, Yn2 variable is reused */
+ Yn2 = acc << shift;
+
+ /* Read the forth input */
+ Xn = *(pIn + 3U);
+
+ /* Store the output in the destination buffer. */
+ *(pOut + 2U) = Yn2;
+ pIn += 4U;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ /* acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);*/
+ mult_32x32_keep32_R(acc, b0, Xn);
+ /* acc += b1 * x[n-1] */
+ /*acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b1, Xn1);
+ /* acc += b[2] * x[n-2] */
+ /*acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b2, Xn2);
+ /* acc += a1 * y[n-1] */
+ /*acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a1, Yn2);
+ /* acc += a2 * y[n-2] */
+ /*acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a2, Yn1);
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ Xn2 = Xn1;
+
+ /* The result is converted to 1.31, Yn1 variable is reused */
+ Yn1 = acc << shift;
+
+ /* Xn1 = Xn */
+ Xn1 = Xn;
+
+ /* Store the output in the destination buffer. */
+ *(pOut + 3U) = Yn1;
+ pOut += 4U;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = (blockSize & 0x3U);
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ /* acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);*/
+ mult_32x32_keep32_R(acc, b0, Xn);
+ /* acc += b1 * x[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b1, Xn1);
+ /* acc += b[2] * x[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, b2, Xn2);
+ /* acc += a1 * y[n-1] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a1, Yn1);
+ /* acc += a2 * y[n-2] */
+ /* acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);*/
+ multAcc_32x32_keep32_R(acc, a2, Yn2);
+
+ /* The result is converted to 1.31 */
+ acc = acc << shift;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+ Yn2 = Yn1;
+ Yn1 = acc;
+
+ /* Store the output in the destination buffer. */
+ *pOut++ = acc;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* The first stage goes from the input buffer to the output buffer. */
+ /* Subsequent stages occur in-place in the output buffer */
+ pIn = pDst;
+
+ /* Reset to destination pointer */
+ pOut = pDst;
+
+ /* Store the updated state variables back into the pState array */
+ *pState++ = Xn1;
+ *pState++ = Xn2;
+ *pState++ = Yn1;
+ *pState++ = Yn2;
+
+ } while (--stage);
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c new file mode 100644 index 0000000..b183642 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f32.c @@ -0,0 +1,91 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_init_f32.c
+ * Description: Floating-point Biquad cascade DirectFormI(DF1) filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point Biquad cascade filter.
+ @param[in,out] S points to an instance of the floating-point Biquad cascade structure.
+ @param[in] numStages number of 2nd order stages in the filter.
+ @param[in] pCoeffs points to the filter coefficients.
+ @param[in] pState points to the state buffer.
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order:
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+
+ @par
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
+ @par
+ The <code>pState</code> is a pointer to state array.
+ Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
+ The state variables are arranged in the <code>pState</code> array as:
+ <pre>
+ {x[n-1], x[n-2], y[n-1], y[n-2]}
+ </pre>
+ The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
+ The state array has a total length of <code>4*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+void arm_biquad_cascade_df1_init_f32(
+ arm_biquad_casd_df1_inst_f32 * S,
+ uint8_t numStages,
+ const float32_t * pCoeffs,
+ float32_t * pState)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 4 * numStages */
+ memset(pState, 0, (4U * (uint32_t) numStages) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c new file mode 100644 index 0000000..a57c5d5 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q15.c @@ -0,0 +1,96 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_init_q15.c
+ * Description: Q15 Biquad cascade DirectFormI(DF1) filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 Biquad cascade filter.
+ @param[in,out] S points to an instance of the Q15 Biquad cascade structure.
+ @param[in] numStages number of 2nd order stages in the filter.
+ @param[in] pCoeffs points to the filter coefficients.
+ @param[in] pState points to the state buffer.
+ @param[in] postShift Shift to be applied to the accumulator result. Varies according to the coefficients format
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order:
+ <pre>
+ {b10, 0, b11, b12, a11, a12, b20, 0, b21, b22, a21, a22, ...}
+ </pre>
+ @par
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>6*numStages</code> values.
+ The zero coefficient between <code>b1</code> and <code>b2</code> facilities use of 16-bit SIMD instructions on the Cortex-M4.
+ @par
+ The state variables are stored in the array <code>pState</code>.
+ Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
+ The state variables are arranged in the <code>pState</code> array as:
+ <pre>
+ {x[n-1], x[n-2], y[n-1], y[n-2]}
+ </pre>
+ The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
+ The state array has a total length of <code>4*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+void arm_biquad_cascade_df1_init_q15(
+ arm_biquad_casd_df1_inst_q15 * S,
+ uint8_t numStages,
+ const q15_t * pCoeffs,
+ q15_t * pState,
+ int8_t postShift)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign postShift to be applied to the output */
+ S->postShift = postShift;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 4 * numStages */
+ memset(pState, 0, (4U * (uint32_t) numStages) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c new file mode 100644 index 0000000..301181c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_init_q31.c @@ -0,0 +1,95 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_init_q31.c
+ * Description: Q31 Biquad cascade DirectFormI(DF1) filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 Biquad cascade filter.
+ @param[in,out] S points to an instance of the Q31 Biquad cascade structure.
+ @param[in] numStages number of 2nd order stages in the filter.
+ @param[in] pCoeffs points to the filter coefficients.
+ @param[in] pState points to the state buffer.
+ @param[in] postShift Shift to be applied after the accumulator. Varies according to the coefficients format
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order:
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+ @par
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
+ @par
+ The <code>pState</code> points to state variables array.
+ Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>.
+ The state variables are arranged in the <code>pState</code> array as:
+ <pre>
+ {x[n-1], x[n-2], y[n-1], y[n-2]}
+ </pre>
+ The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
+ The state array has a total length of <code>4*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+void arm_biquad_cascade_df1_init_q31(
+ arm_biquad_casd_df1_inst_q31 * S,
+ uint8_t numStages,
+ const q31_t * pCoeffs,
+ q31_t * pState,
+ int8_t postShift)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign postShift to be applied to the output */
+ S->postShift = postShift;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 4 * numStages */
+ memset(pState, 0, (4U * (uint32_t) numStages) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c new file mode 100644 index 0000000..0d3a2f3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q15.c @@ -0,0 +1,363 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_q15.c
+ * Description: Processing function for the Q15 Biquad cascade DirectFormI(DF1) filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 Biquad cascade filter.
+ @param[in] S points to an instance of the Q15 Biquad cascade structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the location where the output result is written
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ The accumulator is then shifted by <code>postShift</code> bits to truncate the result to 1.15 format by discarding the low 16 bits.
+ Finally, the result is saturated to 1.15 format.
+ @remark
+ Refer to \ref arm_biquad_cascade_df1_fast_q15() for a faster but less precise implementation of this filter.
+ */
+
+void arm_biquad_cascade_df1_q15(
+ const arm_biquad_casd_df1_inst_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+
+
+#if defined (ARM_MATH_DSP)
+
+ const q15_t *pIn = pSrc; /* Source pointer */
+ q15_t *pOut = pDst; /* Destination pointer */
+ q31_t in; /* Temporary variable to hold input value */
+ q31_t out; /* Temporary variable to hold output value */
+ q31_t b0; /* Temporary variable to hold bo value */
+ q31_t b1, a1; /* Filter coefficients */
+ q31_t state_in, state_out; /* Filter state variables */
+ q31_t acc_l, acc_h;
+ q63_t acc; /* Accumulator */
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ int32_t lShift = (15 - (int32_t) S->postShift); /* Post shift */
+ uint32_t sample, stage = (uint32_t) S->numStages; /* Stage loop counter */
+ int32_t uShift = (32 - lShift);
+
+ do
+ {
+ /* Read the b0 and 0 coefficients using SIMD */
+ b0 = read_q15x2_ia ((q15_t **) &pCoeffs);
+
+ /* Read the b1 and b2 coefficients using SIMD */
+ b1 = read_q15x2_ia ((q15_t **) &pCoeffs);
+
+ /* Read the a1 and a2 coefficients using SIMD */
+ a1 = read_q15x2_ia ((q15_t **) &pCoeffs);
+
+ /* Read the input state values from the state buffer: x[n-1], x[n-2] */
+ state_in = read_q15x2_ia (&pState);
+
+ /* Read the output state values from the state buffer: y[n-1], y[n-2] */
+ state_out = read_q15x2_da (&pState);
+
+ /* Apply loop unrolling and compute 2 output values simultaneously. */
+ /* The variable acc hold output values that are being computed:
+ *
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+ sample = blockSize >> 1U;
+
+ /* First part of the processing with loop unrolling. Compute 2 outputs at a time.
+ ** a second loop below computes the remaining 1 sample. */
+ while (sample > 0U)
+ {
+
+ /* Read the input */
+ in = read_q15x2_ia ((q15_t **) &pIn);
+
+ /* out = b0 * x[n] + 0 * 0 */
+ out = __SMUAD(b0, in);
+
+ /* acc += b1 * x[n-1] + b2 * x[n-2] + out */
+ acc = __SMLALD(b1, state_in, out);
+ /* acc += a1 * y[n-1] + a2 * y[n-2] */
+ acc = __SMLALD(a1, state_out, acc);
+
+ /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ out = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ out = __SSAT(out, 16);
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+ /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ state_in = __PKHBT(in, state_in, 16);
+ state_out = __PKHBT(out, state_out, 16);
+#else
+ state_in = __PKHBT(state_in >> 16, (in >> 16), 16);
+ state_out = __PKHBT(state_out >> 16, (out), 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* out = b0 * x[n] + 0 * 0 */
+ out = __SMUADX(b0, in);
+ /* acc += b1 * x[n-1] + b2 * x[n-2] + out */
+ acc = __SMLALD(b1, state_in, out);
+ /* acc += a1 * y[n-1] + a2 * y[n-2] */
+ acc = __SMLALD(a1, state_out, acc);
+
+ /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ out = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ out = __SSAT(out, 16);
+
+ /* Store the output in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(state_out, out, 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(out, state_out >> 16, 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+ /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+#ifndef ARM_MATH_BIG_ENDIAN
+ state_in = __PKHBT(in >> 16, state_in, 16);
+ state_out = __PKHBT(out, state_out, 16);
+#else
+ state_in = __PKHBT(state_in >> 16, in, 16);
+ state_out = __PKHBT(state_out >> 16, out, 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Decrement loop counter */
+ sample--;
+ }
+
+ /* If the blockSize is not a multiple of 2, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+
+ if ((blockSize & 0x1U) != 0U)
+ {
+ /* Read the input */
+ in = *pIn++;
+
+ /* out = b0 * x[n] + 0 * 0 */
+#ifndef ARM_MATH_BIG_ENDIAN
+ out = __SMUAD(b0, in);
+#else
+ out = __SMUADX(b0, in);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* acc = b1 * x[n-1] + b2 * x[n-2] + out */
+ acc = __SMLALD(b1, state_in, out);
+ /* acc += a1 * y[n-1] + a2 * y[n-2] */
+ acc = __SMLALD(a1, state_out, acc);
+
+ /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ out = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ out = __SSAT(out, 16);
+
+ /* Store the output in the destination buffer. */
+ *pOut++ = (q15_t) out;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
+ /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
+#ifndef ARM_MATH_BIG_ENDIAN
+ state_in = __PKHBT(in, state_in, 16);
+ state_out = __PKHBT(out, state_out, 16);
+#else
+ state_in = __PKHBT(state_in >> 16, in, 16);
+ state_out = __PKHBT(state_out >> 16, out, 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+ }
+
+ /* The first stage goes from the input wire to the output wire. */
+ /* Subsequent numStages occur in-place in the output wire */
+ pIn = pDst;
+
+ /* Reset the output pointer */
+ pOut = pDst;
+
+ /* Store the updated state variables back into the state array */
+ write_q15x2_ia (&pState, state_in);
+ write_q15x2_ia (&pState, state_out);
+
+ /* Decrement loop counter */
+ stage--;
+
+ } while (stage > 0U);
+
+#else
+
+ const q15_t *pIn = pSrc; /* Source pointer */
+ q15_t *pOut = pDst; /* Destination pointer */
+ q15_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ q15_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */
+ q15_t Xn; /* temporary input */
+ q63_t acc; /* Accumulator */
+ int32_t shift = (15 - (int32_t) S->postShift); /* Post shift */
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ uint32_t sample, stage = (uint32_t) S->numStages; /* Stage loop counter */
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = *pCoeffs++;
+ pCoeffs++; // skip the 0 coefficient
+ b1 = *pCoeffs++;
+ b2 = *pCoeffs++;
+ a1 = *pCoeffs++;
+ a2 = *pCoeffs++;
+
+ /* Reading the state values */
+ Xn1 = pState[0];
+ Xn2 = pState[1];
+ Yn1 = pState[2];
+ Yn2 = pState[3];
+
+ /* The variables acc holds the output value that is computed:
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ sample = blockSize;
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ /* acc = b0 * x[n] */
+ acc = (q31_t) b0 *Xn;
+
+ /* acc += b1 * x[n-1] */
+ acc += (q31_t) b1 *Xn1;
+ /* acc += b[2] * x[n-2] */
+ acc += (q31_t) b2 *Xn2;
+ /* acc += a1 * y[n-1] */
+ acc += (q31_t) a1 *Yn1;
+ /* acc += a2 * y[n-2] */
+ acc += (q31_t) a2 *Yn2;
+
+ /* The result is converted to 1.31 */
+ acc = __SSAT((acc >> shift), 16);
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+ Yn2 = Yn1;
+ Yn1 = (q15_t) acc;
+
+ /* Store the output in the destination buffer. */
+ *pOut++ = (q15_t) acc;
+
+ /* decrement the loop counter */
+ sample--;
+ }
+
+ /* The first stage goes from the input buffer to the output buffer. */
+ /* Subsequent stages occur in-place in the output buffer */
+ pIn = pDst;
+
+ /* Reset to destination pointer */
+ pOut = pDst;
+
+ /* Store the updated state variables back into the pState array */
+ *pState++ = Xn1;
+ *pState++ = Xn2;
+ *pState++ = Yn1;
+ *pState++ = Yn2;
+
+ } while (--stage);
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c new file mode 100644 index 0000000..5ec3231 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df1_q31.c @@ -0,0 +1,247 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df1_q31.c
+ * Description: Processing function for the Q31 Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF1
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 Biquad cascade filter.
+ @param[in] S points to an instance of the Q31 Biquad cascade structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by 2 bits and lie in the range [-0.25 +0.25).
+ After all 5 multiply-accumulates are performed, the 2.62 accumulator is shifted by <code>postShift</code> bits and the result truncated to
+ 1.31 format by discarding the low 32 bits.
+ @remark
+ Refer to \ref arm_biquad_cascade_df1_fast_q31() for a faster but less precise implementation of this filter.
+ */
+
+void arm_biquad_cascade_df1_q31(
+ const arm_biquad_casd_df1_inst_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ const q31_t *pIn = pSrc; /* Source pointer */
+ q31_t *pOut = pDst; /* Destination pointer */
+ q31_t *pState = S->pState; /* pState pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q63_t acc; /* Accumulator */
+ q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ q31_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */
+ q31_t Xn; /* Temporary input */
+ uint32_t uShift = ((uint32_t) S->postShift + 1U);
+ uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
+ uint32_t sample, stage = S->numStages; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc_l, acc_h; /* temporary output variables */
+#endif
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = *pCoeffs++;
+ b1 = *pCoeffs++;
+ b2 = *pCoeffs++;
+ a1 = *pCoeffs++;
+ a2 = *pCoeffs++;
+
+ /* Reading the pState values */
+ Xn1 = pState[0];
+ Xn2 = pState[1];
+ Yn1 = pState[2];
+ Yn2 = pState[3];
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Apply loop unrolling and compute 4 output values simultaneously. */
+ /* Variable acc hold output values that are being computed:
+ *
+ * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
+ */
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ sample = blockSize >> 2U;
+
+ while (sample > 0U)
+ {
+ /* Read the first input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = ((q63_t) b0 * Xn) + ((q63_t) b1 * Xn1) + ((q63_t) b2 * Xn2) + ((q63_t) a1 * Yn1) + ((q63_t) a2 * Yn2);
+
+ /* The result is converted to 1.31 , Yn2 variable is reused */
+ acc_l = (acc ) & 0xffffffff; /* Calc lower part of acc */
+ acc_h = (acc >> 32) & 0xffffffff; /* Calc upper part of acc */
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ Yn2 = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn2;
+
+ /* Read the second input */
+ Xn2 = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = ((q63_t) b0 * Xn2) + ((q63_t) b1 * Xn) + ((q63_t) b2 * Xn1) + ((q63_t) a1 * Yn2) + ((q63_t) a2 * Yn1);
+
+ /* The result is converted to 1.31, Yn1 variable is reused */
+ acc_l = (acc ) & 0xffffffff; /* Calc lower part of acc */
+ acc_h = (acc >> 32) & 0xffffffff; /* Calc upper part of acc */
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ Yn1 = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn1;
+
+ /* Read the third input */
+ Xn1 = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = ((q63_t) b0 * Xn1) + ((q63_t) b1 * Xn2) + ((q63_t) b2 * Xn) + ((q63_t) a1 * Yn1) + ((q63_t) a2 * Yn2);
+
+ /* The result is converted to 1.31, Yn2 variable is reused */
+ acc_l = (acc ) & 0xffffffff; /* Calc lower part of acc */
+ acc_h = (acc >> 32) & 0xffffffff; /* Calc upper part of acc */
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ Yn2 = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn2;
+
+ /* Read the forth input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = ((q63_t) b0 * Xn) + ((q63_t) b1 * Xn1) + ((q63_t) b2 * Xn2) + ((q63_t) a1 * Yn2) + ((q63_t) a2 * Yn1);
+
+ /* The result is converted to 1.31, Yn1 variable is reused */
+ acc_l = (acc ) & 0xffffffff; /* Calc lower part of acc */
+ acc_h = (acc >> 32) & 0xffffffff; /* Calc upper part of acc */
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ Yn1 = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store output in destination buffer. */
+ *pOut++ = Yn1;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = blockSize & 0x3U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn = *pIn++;
+
+ /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
+ acc = ((q63_t) b0 * Xn) + ((q63_t) b1 * Xn1) + ((q63_t) b2 * Xn2) + ((q63_t) a1 * Yn1) + ((q63_t) a2 * Yn2);
+
+ /* The result is converted to 1.31 */
+ acc = acc >> lShift;
+
+ /* Store output in destination buffer. */
+ *pOut++ = (q31_t) acc;
+
+ /* Every time after the output is computed state should be updated. */
+ /* The states should be updated as: */
+ /* Xn2 = Xn1 */
+ /* Xn1 = Xn */
+ /* Yn2 = Yn1 */
+ /* Yn1 = acc */
+ Xn2 = Xn1;
+ Xn1 = Xn;
+ Yn2 = Yn1;
+ Yn1 = (q31_t) acc;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Store the updated state variables back into the pState array */
+ *pState++ = Xn1;
+ *pState++ = Xn2;
+ *pState++ = Yn1;
+ *pState++ = Yn2;
+
+ /* The first stage goes from the input buffer to the output buffer. */
+ /* Subsequent numStages occur in-place in the output buffer */
+ pIn = pDst;
+
+ /* Reset output pointer */
+ pOut = pDst;
+
+ /* decrement loop counter */
+ stage--;
+
+ } while (stage > 0U);
+
+}
+
+/**
+ @} end of BiquadCascadeDF1 group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c new file mode 100644 index 0000000..205fcb6 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c @@ -0,0 +1,523 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df2T_f32.c
+ * Description: Processing function for floating-point transposed direct form II Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+*/
+
+/**
+ @addtogroup BiquadCascadeDF2T
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
+ @param[in] S points to an instance of the filter data structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+#if defined(ARM_MATH_NEON)
+
+void arm_biquad_cascade_df2T_f32(
+ const arm_biquad_cascade_df2T_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ const float32_t *pIn = pSrc; /* source pointer */
+ float32_t *pOut = pDst; /* destination pointer */
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
+ float32_t acc1; /* accumulator */
+ float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ float32_t Xn1; /* temporary input */
+ float32_t d1, d2; /* state variables */
+ uint32_t sample, stageCnt,stage = S->numStages; /* loop counters */
+
+
+ float32_t Xn2, Xn3, Xn4; /* Input State variables */
+ float32_t acc2, acc3, acc4; /* accumulator */
+
+
+ float32_t p0, p1, p2, p3, p4, A1;
+
+ float32x4_t XnV, YnV;
+ float32x4x2_t dV;
+ float32x4_t zeroV = vdupq_n_f32(0.0);
+ float32x4_t t1,t2,t3,t4,b1V,b2V,a1V,a2V,s;
+
+ /* Loop unrolling. Compute 4 outputs at a time */
+ stageCnt = stage >> 2;
+
+ while (stageCnt > 0U)
+ {
+ /* Reading the coefficients */
+ t1 = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ t2 = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ t3 = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ t4 = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ b1V = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ b2V = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ a1V = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ a2V = vld1q_f32(pCoeffs);
+ pCoeffs += 4;
+
+ /* Reading the state values */
+ dV = vld2q_f32(pState);
+
+ sample = blockSize;
+
+ while (sample > 0U) {
+ /* y[n] = b0 * x[n] + d1 */
+ /* d1 = b1 * x[n] + a1 * y[n] + d2 */
+ /* d2 = b2 * x[n] + a2 * y[n] */
+
+ XnV = vdupq_n_f32(*pIn++);
+
+ s = dV.val[0];
+ YnV = s;
+
+ s = vextq_f32(zeroV,dV.val[0],3);
+ YnV = vmlaq_f32(YnV, t1, s);
+
+ s = vextq_f32(zeroV,dV.val[0],2);
+ YnV = vmlaq_f32(YnV, t2, s);
+
+ s = vextq_f32(zeroV,dV.val[0],1);
+ YnV = vmlaq_f32(YnV, t3, s);
+
+ YnV = vmlaq_f32(YnV, t4, XnV);
+
+ s = vextq_f32(XnV,YnV,3);
+
+ dV.val[0] = vmlaq_f32(dV.val[1], s, b1V);
+ dV.val[0] = vmlaq_f32(dV.val[0], YnV, a1V);
+
+ dV.val[1] = vmulq_f32(s, b2V);
+ dV.val[1] = vmlaq_f32(dV.val[1], YnV, a2V);
+
+ *pOut++ = YnV[3];
+
+ sample--;
+ }
+
+ /* Store the updated state variables back into the state array */
+ vst2q_f32(pState,dV);
+ pState += 8;
+
+ /* 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 */
+ stageCnt--;
+
+ }
+
+ /* Tail */
+ stageCnt = stage & 3;
+
+ while (stageCnt > 0U)
+ {
+ /* Reading the coefficients */
+ b0 = *pCoeffs++;
+ b1 = *pCoeffs++;
+ b2 = *pCoeffs++;
+ a1 = *pCoeffs++;
+ a2 = *pCoeffs++;
+
+ /*Reading the state values */
+ d1 = pState[0];
+ d2 = pState[1];
+
+ sample = blockSize;
+
+ while (sample > 0U)
+ {
+ /* Read the input */
+ Xn1 = *pIn++;
+
+ /* y[n] = b0 * x[n] + d1 */
+ acc1 = (b0 * Xn1) + d1;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = acc1;
+
+ /* Every time after the output is computed state should be updated. */
+ /* d1 = b1 * x[n] + a1 * y[n] + d2 */
+ d1 = ((b1 * Xn1) + (a1 * acc1)) + d2;
+
+ /* d2 = b2 * x[n] + a2 * y[n] */
+ d2 = (b2 * Xn1) + (a2 * acc1);
+
+ /* decrement the loop counter */
+ sample--;
+ }
+
+ /* Store the updated state variables back into the state array */
+ *pState++ = d1;
+ *pState++ = d2;
+
+ /* 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 */
+ stageCnt--;
+ }
+}
+#else
+LOW_OPTIMIZATION_ENTER
+void arm_biquad_cascade_df2T_f32(
+ const arm_biquad_cascade_df2T_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ const float32_t *pIn = pSrc; /* Source pointer */
+ float32_t *pOut = pDst; /* Destination pointer */
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t acc1; /* Accumulator */
+ float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ float32_t Xn1; /* Temporary input */
+ float32_t d1, d2; /* State variables */
+ uint32_t sample, stage = S->numStages; /* Loop counters */
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = pCoeffs[0];
+ b1 = pCoeffs[1];
+ b2 = pCoeffs[2];
+ a1 = pCoeffs[3];
+ a2 = pCoeffs[4];
+
+ /* Reading the state values */
+ d1 = pState[0];
+ d2 = pState[1];
+
+ pCoeffs += 5U;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 16 outputs at a time */
+ sample = blockSize >> 4U;
+
+ while (sample > 0U) {
+
+ /* y[n] = b0 * x[n] + d1 */
+ /* d1 = b1 * x[n] + a1 * y[n] + d2 */
+ /* d2 = b2 * x[n] + a2 * y[n] */
+
+/* 1 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 2 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 3 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 4 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 5 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 6 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 7 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 8 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 9 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 10 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 11 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 12 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 13 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 14 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 15 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 16 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = blockSize & 0xFU;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U) {
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Store the updated state variables back into the state array */
+ pState[0] = d1;
+ pState[1] = d2;
+
+ pState += 2U;
+
+ /* The current stage input is given as the output to the next stage */
+ pIn = pDst;
+
+ /* Reset the output working pointer */
+ pOut = pDst;
+
+ /* decrement loop counter */
+ stage--;
+
+ } while (stage > 0U);
+
+}
+LOW_OPTIMIZATION_EXIT
+#endif /* #if defined(ARM_MATH_NEON) */
+
+/**
+ @} end of BiquadCascadeDF2T group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c new file mode 100644 index 0000000..3d527a0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_f64.c @@ -0,0 +1,443 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df2T_f64.c
+ * Description: Processing function for floating-point transposed direct form II Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+*/
+
+/**
+ @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure
+
+ This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure.
+ The filters are implemented as a cascade of second order Biquad sections.
+ These functions provide a slight memory savings as compared to the direct form I Biquad filter functions.
+ Only floating-point data is supported.
+
+ This function operate on blocks of input and output data and each call to the function
+ processes <code>blockSize</code> samples through the filter.
+ <code>pSrc</code> points to the array of input data and
+ <code>pDst</code> points to the array of output data.
+ Both arrays contain <code>blockSize</code> values.
+
+ @par Algorithm
+ Each Biquad stage implements a second order filter using the difference equation:
+ <pre>
+ y[n] = b0 * x[n] + d1
+ d1 = b1 * x[n] + a1 * y[n] + d2
+ d2 = b2 * x[n] + a2 * y[n]
+ </pre>
+ where d1 and d2 represent the two state values.
+ @par
+ A Biquad filter using a transposed Direct Form II structure is shown below.
+ \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad"
+ Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
+ Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
+ Pay careful attention to the sign of the feedback coefficients.
+ Some design tools flip the sign of the feedback coefficients:
+ <pre>
+ y[n] = b0 * x[n] + d1;
+ d1 = b1 * x[n] - a1 * y[n] + d2;
+ d2 = b2 * x[n] - a2 * y[n];
+ </pre>
+ In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
+ @par
+ Higher order filters are realized as a cascade of second order sections.
+ <code>numStages</code> refers to the number of second order stages used.
+ For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
+ A 9th order filter would be realized with <code>numStages=5</code> second order stages with the
+ coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
+ @par
+ <code>pState</code> points to the state variable array.
+ Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>.
+ The state variables are arranged in the <code>pState</code> array as:
+ <pre>
+ {d11, d12, d21, d22, ...}
+ </pre>
+ where <code>d1x</code> refers to the state variables for the first Biquad and
+ <code>d2x</code> refers to the state variables for the second Biquad.
+ The state array has a total length of <code>2*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ @par
+ The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II.
+ The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types.
+ That is why the Direct Form I structure supports Q15 and Q31 data types.
+ The transposed Direct Form II structure, on the other hand, requires a wide dynamic range for the state variables <code>d1</code> and <code>d2</code>.
+ Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad.
+ The advantage of the Direct Form II Biquad is that it requires half the number of state variables, 2 rather than 4, per Biquad stage.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
+
+ @par Init Functions
+ There is also an associated initialization function.
+ The initialization function performs following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numStages, pCoeffs, pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros before static initialization.
+ For example, to statically initialize the instance structure use
+ <pre>
+ arm_biquad_cascade_df2T_instance_f64 S1 = {numStages, pState, pCoeffs};
+ arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs};
+ </pre>
+ where <code>numStages</code> is the number of Biquad stages in the filter;
+ <code>pState</code> is the address of the state buffer.
+ <code>pCoeffs</code> is the address of the coefficient buffer;
+*/
+
+/**
+ @addtogroup BiquadCascadeDF2T
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
+ @param[in] S points to an instance of the filter data structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+LOW_OPTIMIZATION_ENTER
+void arm_biquad_cascade_df2T_f64(
+ const arm_biquad_cascade_df2T_instance_f64 * S,
+ float64_t * pSrc,
+ float64_t * pDst,
+ uint32_t blockSize)
+{
+
+ float64_t *pIn = pSrc; /* Source pointer */
+ float64_t *pOut = pDst; /* Destination pointer */
+ float64_t *pState = S->pState; /* State pointer */
+ float64_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float64_t acc1; /* Accumulator */
+ float64_t b0, b1, b2, a1, a2; /* Filter coefficients */
+ float64_t Xn1; /* Temporary input */
+ float64_t d1, d2; /* State variables */
+ uint32_t sample, stage = S->numStages; /* Loop counters */
+
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = pCoeffs[0];
+ b1 = pCoeffs[1];
+ b2 = pCoeffs[2];
+ a1 = pCoeffs[3];
+ a2 = pCoeffs[4];
+
+ /* Reading the state values */
+ d1 = pState[0];
+ d2 = pState[1];
+
+ pCoeffs += 5U;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 16 outputs at a time */
+ sample = blockSize >> 4U;
+
+ while (sample > 0U) {
+
+ /* y[n] = b0 * x[n] + d1 */
+ /* d1 = b1 * x[n] + a1 * y[n] + d2 */
+ /* d2 = b2 * x[n] + a2 * y[n] */
+
+/* 1 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+
+/* 2 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 3 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 4 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 5 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 6 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 7 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 8 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 9 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 10 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 11 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 12 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 13 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 14 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 15 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+/* 16 */
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = blockSize & 0xFU;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (sample > 0U) {
+ Xn1 = *pIn++;
+
+ acc1 = b0 * Xn1 + d1;
+
+ d1 = b1 * Xn1 + d2;
+ d1 += a1 * acc1;
+
+ d2 = b2 * Xn1;
+ d2 += a2 * acc1;
+
+ *pOut++ = acc1;
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Store the updated state variables back into the state array */
+ pState[0] = d1;
+ pState[1] = d2;
+
+ pState += 2U;
+
+ /* The current stage input is given as the output to the next stage */
+ pIn = pDst;
+
+ /* Reset the output working pointer */
+ pOut = pDst;
+
+ /* decrement loop counter */
+ stage--;
+
+ } while (stage > 0U);
+
+}
+LOW_OPTIMIZATION_EXIT
+
+/**
+ @} end of BiquadCascadeDF2T group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c new file mode 100644 index 0000000..1da7f3e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f32.c @@ -0,0 +1,211 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df2T_init_f32.c
+ * Description: Initialization function for floating-point transposed direct form II Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF2T
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+ @param[in,out] S points to an instance of the filter data structure.
+ @param[in] numStages number of 2nd order stages in the filter.
+ @param[in] pCoeffs points to the filter coefficients.
+ @param[in] pState points to the state buffer.
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order
+ in the not Neon version.
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+
+ @par
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
+
+ For Neon version, this array is bigger. If numstages = 4x + y, then the array has size:
+ 32*x + 5*y
+ and it must be initialized using the function
+ arm_biquad_cascade_df2T_compute_coefs_f32 which is taking the
+ standard array coefficient as parameters.
+
+ But, an array of 8*numstages is a good approximation.
+
+ Then, the initialization can be done with:
+ <pre>
+ arm_biquad_cascade_df2T_init_f32(&SNeon, nbCascade, neonCoefs, stateNeon);
+ arm_biquad_cascade_df2T_compute_coefs_f32(&SNeon,nbCascade,coefs);
+ </pre>
+
+ @par In this example, neonCoefs is a bigger array of size 8 * numStages.
+ coefs is the standard array:
+
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+
+
+ @par
+ The <code>pState</code> is a pointer to state array.
+ Each Biquad stage has 2 state variables <code>d1,</code> and <code>d2</code>.
+ The 2 state variables for stage 1 are first, then the 2 state variables for stage 2, and so on.
+ The state array has a total length of <code>2*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+#if defined(ARM_MATH_NEON)
+/*
+
+Must be called after initializing the biquad instance.
+pCoeffs has size 5 * nbCascade
+Whereas the pCoeffs for the init has size (4*4 + 4*4)* nbCascade
+
+So this pCoeffs is the one which would be used for the not Neon version.
+The pCoeffs passed in init is bigger than the one for the not Neon version.
+
+*/
+void arm_biquad_cascade_df2T_compute_coefs_f32(
+ arm_biquad_cascade_df2T_instance_f32 * S,
+ uint8_t numStages,
+ float32_t * pCoeffs)
+{
+ uint8_t cnt;
+ float32_t *pDstCoeffs;
+ float32_t b0[4],b1[4],b2[4],a1[4],a2[4];
+
+ pDstCoeffs = S->pCoeffs;
+
+ cnt = numStages >> 2;
+ while(cnt > 0)
+ {
+ for(int i=0;i<4;i++)
+ {
+ b0[i] = pCoeffs[0];
+ b1[i] = pCoeffs[1];
+ b2[i] = pCoeffs[2];
+ a1[i] = pCoeffs[3];
+ a2[i] = pCoeffs[4];
+ pCoeffs += 5;
+ }
+
+ /* Vec 1 */
+ *pDstCoeffs++ = 0;
+ *pDstCoeffs++ = b0[1];
+ *pDstCoeffs++ = b0[2];
+ *pDstCoeffs++ = b0[3];
+
+ /* Vec 2 */
+ *pDstCoeffs++ = 0;
+ *pDstCoeffs++ = 0;
+ *pDstCoeffs++ = b0[1] * b0[2];
+ *pDstCoeffs++ = b0[2] * b0[3];
+
+ /* Vec 3 */
+ *pDstCoeffs++ = 0;
+ *pDstCoeffs++ = 0;
+ *pDstCoeffs++ = 0;
+ *pDstCoeffs++ = b0[1] * b0[2] * b0[3];
+
+ /* Vec 4 */
+ *pDstCoeffs++ = b0[0];
+ *pDstCoeffs++ = b0[0] * b0[1];
+ *pDstCoeffs++ = b0[0] * b0[1] * b0[2];
+ *pDstCoeffs++ = b0[0] * b0[1] * b0[2] * b0[3];
+
+ /* Vec 5 */
+ *pDstCoeffs++ = b1[0];
+ *pDstCoeffs++ = b1[1];
+ *pDstCoeffs++ = b1[2];
+ *pDstCoeffs++ = b1[3];
+
+ /* Vec 6 */
+ *pDstCoeffs++ = b2[0];
+ *pDstCoeffs++ = b2[1];
+ *pDstCoeffs++ = b2[2];
+ *pDstCoeffs++ = b2[3];
+
+ /* Vec 7 */
+ *pDstCoeffs++ = a1[0];
+ *pDstCoeffs++ = a1[1];
+ *pDstCoeffs++ = a1[2];
+ *pDstCoeffs++ = a1[3];
+
+ /* Vec 8 */
+ *pDstCoeffs++ = a2[0];
+ *pDstCoeffs++ = a2[1];
+ *pDstCoeffs++ = a2[2];
+ *pDstCoeffs++ = a2[3];
+
+ cnt--;
+ }
+
+ cnt = numStages & 0x3;
+ while(cnt > 0)
+ {
+ *pDstCoeffs++ = *pCoeffs++;
+ *pDstCoeffs++ = *pCoeffs++;
+ *pDstCoeffs++ = *pCoeffs++;
+ *pDstCoeffs++ = *pCoeffs++;
+ *pDstCoeffs++ = *pCoeffs++;
+ cnt--;
+ }
+
+}
+#endif
+
+void arm_biquad_cascade_df2T_init_f32(
+ arm_biquad_cascade_df2T_instance_f32 * S,
+ uint8_t numStages,
+ const float32_t * pCoeffs,
+ float32_t * pState)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 2 * numStages */
+ memset(pState, 0, (2U * (uint32_t) numStages) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF2T group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f64.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f64.c new file mode 100644 index 0000000..6d17878 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f64.c @@ -0,0 +1,86 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_df2T_init_f64.c
+ * Description: Initialization function for floating-point transposed direct form II Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF2T
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+ @param[in,out] S points to an instance of the filter data structure
+ @param[in] numStages number of 2nd order stages in the filter
+ @param[in] pCoeffs points to the filter coefficients
+ @param[in] pState points to the state buffer
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order:
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+ @par
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
+ @par
+ The <code>pState</code> is a pointer to state array.
+ Each Biquad stage has 2 state variables <code>d1,</code> and <code>d2</code>.
+ The 2 state variables for stage 1 are first, then the 2 state variables for stage 2, and so on.
+ The state array has a total length of <code>2*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+void arm_biquad_cascade_df2T_init_f64(
+ arm_biquad_cascade_df2T_instance_f64 * S,
+ uint8_t numStages,
+ float64_t * pCoeffs,
+ float64_t * pState)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 2 * numStages */
+ memset(pState, 0, (2U * (uint32_t) numStages) * sizeof(float64_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF2T group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f32.c new file mode 100644 index 0000000..1b97262 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f32.c @@ -0,0 +1,285 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_stereo_df2T_f32.c
+ * Description: Processing function for floating-point transposed direct form II Biquad cascade filter. 2 channels
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+*/
+
+/**
+ @addtogroup BiquadCascadeDF2T
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
+ @param[in] S points to an instance of the filter data structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+LOW_OPTIMIZATION_ENTER
+void arm_biquad_cascade_stereo_df2T_f32(
+ const arm_biquad_cascade_stereo_df2T_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ const float32_t *pIn = pSrc; /* Source pointer */
+ float32_t *pOut = pDst; /* Destination pointer */
+ float32_t *pState = S->pState; /* State pointer */
+ const 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 */
+
+ do
+ {
+ /* Reading the coefficients */
+ b0 = pCoeffs[0];
+ b1 = pCoeffs[1];
+ b2 = pCoeffs[2];
+ a1 = pCoeffs[3];
+ a2 = pCoeffs[4];
+
+ /* Reading the state values */
+ d1a = pState[0];
+ d2a = pState[1];
+ d1b = pState[2];
+ d2b = pState[3];
+
+ pCoeffs += 5U;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 8 outputs at a time */
+ sample = blockSize >> 3U;
+
+ while (sample > 0U) {
+ /* y[n] = b0 * x[n] + d1 */
+ /* d1 = b1 * x[n] + a1 * y[n] + d2 */
+ /* d2 = b2 * x[n] + a2 * y[n] */
+
+/* 1 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 2 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 3 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 4 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 5 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 6 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 7 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+/* 8 */
+ Xn1a = *pIn++; /* Channel a */
+ Xn1b = *pIn++; /* Channel b */
+
+ acc1a = (b0 * Xn1a) + d1a;
+ acc1b = (b0 * Xn1b) + d1b;
+
+ *pOut++ = acc1a;
+ *pOut++ = acc1b;
+
+ d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
+ d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
+
+ d2a = (b2 * Xn1a) + (a2 * acc1a);
+ d2b = (b2 * Xn1b) + (a2 * acc1b);
+
+ /* decrement loop counter */
+ sample--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ sample = blockSize & 0x7U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ sample = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ 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 loop counter */
+ sample--;
+ }
+
+ /* Store the updated state variables back into the state array */
+ pState[0] = d1a;
+ pState[1] = d2a;
+
+ pState[2] = d1b;
+ pState[3] = d2b;
+
+ pState += 4U;
+
+ /* 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);
+
+}
+LOW_OPTIMIZATION_EXIT
+/**
+ @} end of BiquadCascadeDF2T group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f32.c new file mode 100644 index 0000000..f5f297c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f32.c @@ -0,0 +1,86 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_biquad_cascade_stereo_df2T_init_f32.c
+ * Description: Initialization function for floating-point transposed direct form II Biquad cascade filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup BiquadCascadeDF2T
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter.
+ @param[in,out] S points to an instance of the filter data structure.
+ @param[in] numStages number of 2nd order stages in the filter.
+ @param[in] pCoeffs points to the filter coefficients.
+ @param[in] pState points to the state buffer.
+ @return none
+
+ @par Coefficient and State Ordering
+ The coefficients are stored in the array <code>pCoeffs</code> in the following order:
+ <pre>
+ {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+ </pre>
+ @par
+ where <code>b1x</code> and <code>a1x</code> are the coefficients for the first stage,
+ <code>b2x</code> and <code>a2x</code> are the coefficients for the second stage,
+ and so on. The <code>pCoeffs</code> array contains a total of <code>5*numStages</code> values.
+ @par
+ The <code>pState</code> is a pointer to state array.
+ Each Biquad stage has 2 state variables <code>d1,</code> and <code>d2</code> for each channel.
+ The 2 state variables for stage 1 are first, then the 2 state variables for stage 2, and so on.
+ The state array has a total length of <code>2*numStages</code> values.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+ */
+
+void arm_biquad_cascade_stereo_df2T_init_f32(
+ arm_biquad_cascade_stereo_df2T_instance_f32 * S,
+ uint8_t numStages,
+ const float32_t * pCoeffs,
+ float32_t * pState)
+{
+ /* Assign filter stages */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always 4 * numStages */
+ memset(pState, 0, (4U * (uint32_t) numStages) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of BiquadCascadeDF2T group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_f32.c new file mode 100644 index 0000000..84424ca --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_f32.c @@ -0,0 +1,816 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_f32.c
+ * Description: Convolution of floating-point sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup Conv Convolution
+
+ Convolution is a mathematical operation that operates on two finite length vectors to generate a finite length output vector.
+ Convolution is similar to correlation and is frequently used in filtering and data analysis.
+ The CMSIS DSP library contains functions for convolving Q7, Q15, Q31, and floating-point data types.
+ The library also provides fast versions of the Q15 and Q31 functions.
+
+ @par Algorithm
+ Let <code>a[n]</code> and <code>b[n]</code> be sequences of length <code>srcALen</code> and
+ <code>srcBLen</code> samples respectively. Then the convolution
+ <pre>
+ c[n] = a[n] * b[n]
+ </pre>
+ @par
+ is defined as
+ \image html ConvolutionEquation.gif
+ @par
+ Note that <code>c[n]</code> is of length <code>srcALen + srcBLen - 1</code> and is defined over the interval <code>n=0, 1, 2, ..., srcALen + srcBLen - 2</code>.
+ <code>pSrcA</code> points to the first input vector of length <code>srcALen</code> and
+ <code>pSrcB</code> points to the second input vector of length <code>srcBLen</code>.
+ The output result is written to <code>pDst</code> and the calling function must allocate <code>srcALen+srcBLen-1</code> words for the result.
+ @par
+ Conceptually, when two signals <code>a[n]</code> and <code>b[n]</code> are convolved,
+ the signal <code>b[n]</code> slides over <code>a[n]</code>.
+ For each offset \c n, the overlapping portions of a[n] and b[n] are multiplied and summed together.
+ @par
+ Note that convolution is a commutative operation:
+ <pre>
+ a[n] * b[n] = b[n] * a[n].
+ </pre>
+ @par
+ This means that switching the A and B arguments to the convolution functions has no effect.
+
+ @par Fixed-Point Behavior
+ Convolution requires summing up a large number of intermediate products.
+ As such, the Q7, Q15, and Q31 functions run a risk of overflow and saturation.
+ Refer to the function specific documentation below for further details of the particular algorithm used.
+
+ @par Fast Versions
+ Fast versions are supported for Q31 and Q15. Cycles for Fast versions are less compared to Q31 and Q15 of conv and the design requires
+ the input signals should be scaled down to avoid intermediate overflows.
+
+ @par Opt Versions
+ Opt versions are supported for Q15 and Q7. Design uses internal scratch buffer for getting good optimisation.
+ These versions are optimised in cycles and consumes more memory (Scratch memory) compared to Q15 and Q7 versions
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of floating-point sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @return none
+ */
+
+void arm_conv_f32(
+ const float32_t * pSrcA,
+ uint32_t srcALen,
+ const float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const float32_t *pIn1; /* InputA pointer */
+ const float32_t *pIn2; /* InputB pointer */
+ float32_t *pOut = pDst; /* Output pointer */
+ const float32_t *px; /* Intermediate inputA pointer */
+ const float32_t *py; /* Intermediate inputB pointer */
+ const float32_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ float32_t sum; /* Accumulators */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+ float32_t acc0, acc1, acc2, acc3; /* Accumulators */
+ float32_t x0, x1, x2, x3, c0; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+#if defined(ARM_MATH_NEON)
+ float32x4_t vec1;
+ float32x4_t vec2;
+ float32x4_t res = vdupq_n_f32(0) ;
+ float32x2_t accum = vdup_n_f32(0);
+#endif /* #if defined(ARM_MATH_NEON) */
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ res = vdupq_n_f32(0) ;
+ accum = vdup_n_f32(0);
+
+ /* Compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+
+ while (k > 0U)
+ {
+ vec1 = vld1q_f32(px);
+ vec2 = vld1q_f32(py-3);
+ vec2 = vrev64q_f32(vec2);
+ vec2 = vcombine_f32(vget_high_f32(vec2), vget_low_f32(vec2));
+
+ res = vmlaq_f32(res,vec1, vec2);
+
+ /* Increment pointers */
+ px += 4;
+ py -= 4;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ accum = vpadd_f32(vget_low_f32(res), vget_high_f32(res));
+ sum += accum[0] + accum[1];
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count & 3;
+#else
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 1] */
+ sum += *px++ * *py--;
+
+ /* x[1] * y[srcBLen - 2] */
+ sum += *px++ * *py--;
+
+ /* x[2] * y[srcBLen - 3] */
+ sum += *px++ * *py--;
+
+ /* x[3] * y[srcBLen - 4] */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#endif /* #if defined(ARM_MATH_NEON) */
+
+#else
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+
+#if defined(ARM_MATH_NEON)
+ float32x4_t c;
+ float32x4_t x1v;
+ float32x4_t x2v;
+ uint32x4_t x1v_u;
+ uint32x4_t x2v_u;
+ uint32x4_t x_u;
+ float32x4_t x;
+ float32x4_t res = vdupq_n_f32(0) ;
+#endif /* #if defined(ARM_MATH_NEON) */
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0.0f;
+ acc1 = 0.0f;
+ acc2 = 0.0f;
+ acc3 = 0.0f;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ res = vdupq_n_f32(0) ;
+
+ x1v = vld1q_f32(px);
+ x2v = vld1q_f32(px+4);
+
+ do
+ {
+ c = vld1q_f32(py-3);
+
+ px += 4;
+ x = x1v;
+ res = vmlaq_n_f32(res,x,c[3]);
+
+ x = vextq_f32(x1v,x2v,1);
+
+ res = vmlaq_n_f32(res,x,c[2]);
+
+ x = vextq_f32(x1v,x2v,2);
+
+ res = vmlaq_n_f32(res,x,c[1]);
+
+ x = vextq_f32(x1v,x2v,3);
+
+ res = vmlaq_n_f32(res,x,c[0]);
+
+ py -= 4;
+
+ x1v = x2v ;
+ x2v = vld1q_f32(px+4);
+
+ } while (--k);
+
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen & 0x3;
+
+ x1v = vld1q_f32(px);
+ px += 4;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *(py--);
+
+ res = vmlaq_n_f32(res,x1v,c0);
+
+ /* Reuse the present samples for the next MAC */
+ x1v[0] = x1v[1];
+ x1v[1] = x1v[2];
+ x1v[2] = x1v[3];
+
+ x1v[3] = *(px++);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ acc0 = res[0];
+ acc1 = res[1];
+ acc2 = res[2];
+ acc3 = res[3];
+
+#else
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *py--;
+ /* Read x[3] sample */
+ x3 = *(px);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[srcBLen - 1] */
+ acc0 += x0 * c0;
+ /* acc1 += x[1] * y[srcBLen - 1] */
+ acc1 += x1 * c0;
+ /* acc2 += x[2] * y[srcBLen - 1] */
+ acc2 += x2 * c0;
+ /* acc3 += x[3] * y[srcBLen - 1] */
+ acc3 += x3 * c0;
+
+ /* Read y[srcBLen - 2] sample */
+ c0 = *py--;
+ /* Read x[4] sample */
+ x0 = *(px + 1U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[srcBLen - 2] */
+ acc0 += x1 * c0;
+ /* acc1 += x[2] * y[srcBLen - 2] */
+ acc1 += x2 * c0;
+ /* acc2 += x[3] * y[srcBLen - 2] */
+ acc2 += x3 * c0;
+ /* acc3 += x[4] * y[srcBLen - 2] */
+ acc3 += x0 * c0;
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *py--;
+ /* Read x[5] sample */
+ x1 = *(px + 2U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[2] * y[srcBLen - 3] */
+ acc0 += x2 * c0;
+ /* acc1 += x[3] * y[srcBLen - 2] */
+ acc1 += x3 * c0;
+ /* acc2 += x[4] * y[srcBLen - 2] */
+ acc2 += x0 * c0;
+ /* acc3 += x[5] * y[srcBLen - 2] */
+ acc3 += x1 * c0;
+
+ /* Read y[srcBLen - 4] sample */
+ c0 = *py--;
+ /* Read x[6] sample */
+ x2 = *(px + 3U);
+ px += 4U;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[3] * y[srcBLen - 4] */
+ acc0 += x3 * c0;
+ /* acc1 += x[4] * y[srcBLen - 4] */
+ acc1 += x0 * c0;
+ /* acc2 += x[5] * y[srcBLen - 4] */
+ acc2 += x1 * c0;
+ /* acc3 += x[6] * y[srcBLen - 4] */
+ acc3 += x2 * c0;
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 += x0 * c0;
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 += x1 * c0;
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 += x2 * c0;
+ /* acc3 += x[7] * y[srcBLen - 5] */
+ acc3 += x3 * c0;
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+#endif /* #if defined(ARM_MATH_NEON) */
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = acc0;
+ *pOut++ = acc1;
+ *pOut++ = acc2;
+ *pOut++ = acc3;
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) || defined (ARM_MATH_NEON)*/
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined(ARM_MATH_NEON) || defined (ARM_MATH_LOOPUNROLL)
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+#if defined (ARM_MATH_NEON)
+ float32x4_t res = vdupq_n_f32(0) ;
+ float32x4_t x = vdupq_n_f32(0) ;
+ float32x4_t y = vdupq_n_f32(0) ;
+ float32x2_t accum = vdup_n_f32(0) ;
+
+ /* First part of the processing. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ x = vld1q_f32(px);
+ y = vld1q_f32(py-3);
+
+ y = vrev64q_f32(y);
+ y = vcombine_f32(vget_high_f32(y), vget_low_f32(y));
+
+ res = vmlaq_f32(res,x,y);
+
+ px += 4 ;
+ py -= 4 ;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ accum = vpadd_f32(vget_low_f32(res), vget_high_f32(res));
+ sum += accum[0] + accum[1];
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen & 0x3U;
+
+#else
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+ sum += *px++ * *py--;
+ sum += *px++ * *py--;
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#endif /* if defined (ARM_MATH_NEON) */
+#else
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined(ARM_MATH_NEON) || defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+
+ /* Working pointer of inputA */
+ pSrc1 = pIn1 + (srcALen - (srcBLen - 1U));
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = blockSize3 >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ float32x4_t res = vdupq_n_f32(0) ;
+ float32x4_t x = vdupq_n_f32(0) ;
+ float32x4_t y = vdupq_n_f32(0) ;
+ float32x2_t accum = vdup_n_f32(0) ;
+
+ while (k > 0U)
+ {
+ x = vld1q_f32(px);
+ y = vld1q_f32(py-3);
+
+ y = vrev64q_f32(y);
+ y = vcombine_f32(vget_high_f32(y), vget_low_f32(y));
+
+ res = vmlaq_f32(res,x,y);
+
+ px += 4 ;
+ py -= 4 ;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ accum = vpadd_f32(vget_low_f32(res), vget_high_f32(res));
+ sum += accum[0] + accum[1];
+
+#else
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ sum += *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum += *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ sum += *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+#endif /* #if defined (ARM_MATH_NEON) */
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = blockSize3 % 0x4U;
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = blockSize3;
+
+#endif /* #if defined (ARM_MATH_NEON) || defined (ARM_MATH_LOOPUNROLL)*/
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const float32_t *pIn1 = pSrcA; /* InputA pointer */
+ const float32_t *pIn2 = pSrcB; /* InputB pointer */
+ float32_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i < (srcALen + srcBLen - 1U); i++)
+ {
+ /* Initialize sum with zero to carry out MAC operations */
+ sum = 0.0f;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ( pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = sum;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_opt_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_opt_q15.c new file mode 100644 index 0000000..b2144ad --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_opt_q15.c @@ -0,0 +1,366 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_fast_opt_q15.c
+ * Description: Fast Q15 Convolution
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q15 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1
+ @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2
+ @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results
+ but provides only a single guard bit. There is no saturation on intermediate additions.
+ Thus, if the accumulator overflows it wraps around and distorts the result.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down the inputs by log2(min(srcALen, srcBLen)) (log2 is read as log to the base 2) times to avoid overflows,
+ as maximum of min(srcALen, srcBLen) number of additions are carried internally.
+ The 2.30 accumulator is right shifted by 15 bits and then saturated to 1.15 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_conv_q15() for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.
+ */
+
+void arm_conv_fast_opt_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+ q31_t acc0; /* Accumulators */
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch1 */
+ q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch1 */
+ const q15_t *px; /* Intermediate inputA pointer */
+ q15_t *py; /* Intermediate inputB pointer */
+ uint32_t j, k, blkCnt; /* Loop counter */
+ uint32_t tapCnt; /* Loop count */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x1, x2, x3; /* Temporary variables to hold state and coefficient values */
+ q31_t y1, y2; /* State variables */
+#endif
+
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Pointer to take end of scratch2 buffer */
+ pScr2 = pScratch2 + srcBLen - 1;
+
+ /* points to smaller length sequence */
+ px = pIn2;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ /* Copy smaller length input sequence in reverse order into second scratch buffer */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Initialze temporary scratch pointer */
+ pScr1 = pScratch1;
+
+ /* Assuming scratch1 buffer is aligned by 32-bit */
+ /* Fill (srcBLen - 1U) zeros in scratch1 buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy bigger length sequence(srcALen) samples in scratch1 buffer */
+
+ /* Copy (srcALen) samples in scratch buffer */
+ arm_copy_q15(pIn1, pScr1, srcALen);
+
+ /* Update pointers */
+ pScr1 += srcALen;
+
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+
+ /* Initialization of pIn2 pointer */
+ pIn2 = py;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = (srcALen + srcBLen - 1U) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+ y2 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+ acc2 = __SMLAD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x2, y2, acc0);
+ acc2 = __SMLAD(x1, y2, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+ acc1 = __SMLADX(x3, y2, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y2, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2);
+ acc1 += (*pScr1++ * *pIn2);
+ acc2 += (*pScr1++ * *pIn2);
+ acc3 += (*pScr1++ * *pIn2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the results in the accumulators in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (srcALen + srcBLen - 1U) & 0x3;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = (srcALen + srcBLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Calculate convolution for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read next two samples from scratch1 buffer */
+ acc0 += (*pScr1++ * *pIn2++);
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ Then store the output in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 1U;
+ }
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q15.c new file mode 100644 index 0000000..fc3ca73 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q15.c @@ -0,0 +1,663 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_fast_q15.c
+ * Description: Fast Q15 Convolution
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q15 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results
+ but provides only a single guard bit. There is no saturation on intermediate additions.
+ Thus, if the accumulator overflows it wraps around and distorts the result.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down the inputs by log2(min(srcALen, srcBLen)) (log2 is read as log to the base 2) times to avoid overflows,
+ as maximum of min(srcALen, srcBLen) number of additions are carried internally.
+ The 2.30 accumulator is right shifted by 15 bits and then saturated to 1.15 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_conv_q15() for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.
+ */
+
+void arm_conv_fast_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst)
+{
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
+ const q15_t *px; /* Intermediate inputA pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ const q15_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t x0, x1, x2, x3, c0; /* Temporary variables to hold state and coefficient values */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations less than 4 */
+ /* Second part of this stage computes the MAC operations greater than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ while ((count < 4U) && (blockSize1 > 0U))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Loop over number of MAC operations between
+ * inputA samples and inputB samples */
+ k = count;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* The second part of the stage starts here */
+ /* The internal loop, over count, is unrolled by 4 */
+ /* To, read the last two inputB samples using SIMD:
+ * y[srcBLen] and y[srcBLen-1] coefficients, py is decremented by 1 */
+ py = py - 1;
+
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* x[0], x[1] are multiplied with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[2], x[3] are multiplied with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ * So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + (count - 1U);
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* --------------------
+ * Stage2 process
+ * -------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unroll over blockSize2, by 4 */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ py = py - 1U;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1] samples */
+ x0 = read_q15x2 ((q15_t *) px);
+ /* read x[1], x[2] samples */
+ x1 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read the last two inputB samples using SIMD:
+ * y[srcBLen - 1] and y[srcBLen - 2] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
+ acc0 = __SMLADX(x0, c0, acc0);
+
+ /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
+ acc1 = __SMLADX(x1, c0, acc1);
+
+ /* Read x[2], x[3] */
+ x2 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[3], x[4] */
+ x3 = read_q15x2 ((q15_t *) px + 1);
+
+ /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
+ acc2 = __SMLADX(x2, c0, acc2);
+
+ /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
+ acc3 = __SMLADX(x3, c0, acc3);
+
+ /* Read y[srcBLen - 3] and y[srcBLen - 4] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
+ acc0 = __SMLADX(x2, c0, acc0);
+
+ /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
+ acc1 = __SMLADX(x3, c0, acc1);
+
+ /* Read x[4], x[5] */
+ x0 = read_q15x2 ((q15_t *) px + 2);
+
+ /* Read x[5], x[6] */
+ x1 = read_q15x2 ((q15_t *) px + 3);
+ px += 4U;
+
+ /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
+ acc2 = __SMLADX(x0, c0, acc2);
+
+ /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
+ acc3 = __SMLADX(x1, c0, acc3);
+
+ } while (--k);
+
+ /* For the next MAC operations, SIMD is not used
+ * So, the 16 bit pointer if inputB, py is updated */
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ if (k == 1U)
+ {
+ /* Read y[srcBLen - 5] */
+ c0 = *(py+1);
+
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[7] */
+ x3 = read_q15x2 ((q15_t *) px);
+ px++;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc1 = __SMLAD(x1, c0, acc1);
+ acc2 = __SMLADX(x1, c0, acc2);
+ acc3 = __SMLADX(x3, c0, acc3);
+ }
+
+ if (k == 2U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLADX(x0, c0, acc0);
+ acc1 = __SMLADX(x1, c0, acc1);
+ acc2 = __SMLADX(x3, c0, acc2);
+ acc3 = __SMLADX(x2, c0, acc3);
+ }
+
+ if (k == 3U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLADX(x0, c0, acc0);
+ acc1 = __SMLADX(x1, c0, acc1);
+ acc2 = __SMLADX(x3, c0, acc2);
+ acc3 = __SMLADX(x2, c0, acc3);
+
+ /* Read y[srcBLen - 7] */
+ c0 = *(py-1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[10] */
+ x3 = read_q15x2 ((q15_t *) px + 2);
+ px += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLADX(x1, c0, acc0);
+ acc1 = __SMLAD(x2, c0, acc1);
+ acc2 = __SMLADX(x2, c0, acc2);
+ acc3 = __SMLADX(x3, c0, acc3);
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT((acc0 >> 15), (acc1 >> 15), 16));
+ write_q15x2_ia (&pOut, __PKHBT((acc2 >> 15), (acc3 >> 15), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT((acc1 >> 15), (acc0 >> 15), 16));
+ write_q15x2_ia (&pOut, __PKHBT((acc3 >> 15), (acc2 >> 15), 16));
+#endif /*#ifndef ARM_MATH_BIG_ENDIAN*/
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q31_t) *px++ * *py--);
+ sum += ((q31_t) *px++ * *py--);
+ sum += ((q31_t) *px++ * *py--);
+ sum += ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ pIn2 = pSrc2 - 1U;
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations greater than 4 */
+ /* Second part of this stage computes the MAC operations less than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ j = blockSize3 >> 2U;
+
+ while ((j > 0U) && (blockSize3 > 0U))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = blockSize3 >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[srcALen - srcBLen + 1], x[srcALen - srcBLen + 2] are multiplied
+ * with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[srcALen - srcBLen + 3], x[srcALen - srcBLen + 4] are multiplied
+ * with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ * So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the blockSize3 is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = blockSize3 % 0x4U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 5] * y[srcBLen - 5] */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blockSize3--;
+
+ j--;
+ }
+
+ /* The second part of the stage starts here */
+ /* SIMD is not used for the next MAC operations,
+ * so pointer py is updated to read only one sample at a time */
+ py = py + 1U;
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = blockSize3;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q31.c new file mode 100644 index 0000000..c3943f0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_fast_q31.c @@ -0,0 +1,558 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_fast_q31.c
+ * Description: Fast Q31 Convolution
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q31 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence.
+ @param[in] srcALen length of the first input sequence.
+ @param[in] pSrcB points to the second input sequence.
+ @param[in] srcBLen length of the second input sequence.
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This function is optimized for speed at the expense of fixed-point precision and overflow protection.
+ The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
+ These intermediate results are accumulated in a 32-bit register in 2.30 format.
+ Finally, the accumulator is saturated and converted to a 1.31 result.
+ @par
+ The fast version has the same overflow behavior as the standard version but provides less precision since it discards the low 32 bits of each multiplication result.
+ In order to avoid overflows completely the input signals must be scaled down.
+ Scale down the inputs by log2(min(srcALen, srcBLen)) (log2 is read as log to the base 2) times to avoid overflows,
+ as maximum of min(srcALen, srcBLen) number of additions are carried internally.
+ @remark
+ Refer to \ref arm_conv_q31() for a slower implementation of this function which uses 64-bit accumulation to provide higher precision.
+ */
+
+void arm_conv_fast_q31(
+ const q31_t * pSrcA,
+ uint32_t srcALen,
+ const q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst)
+{
+ const q31_t *pIn1; /* InputA pointer */
+ const q31_t *pIn2; /* InputB pointer */
+ q31_t *pOut = pDst; /* Output pointer */
+ const q31_t *px; /* Intermediate inputA pointer */
+ const q31_t *py; /* Intermediate inputB pointer */
+ const q31_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
+ q31_t x0, x1, x2, x3, c0; /* Temporary variables to hold state and coefficient values */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* x[1] * y[srcBLen - 2] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* x[2] * y[srcBLen - 3] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* x[3] * y[srcBLen - 4] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unroll over blockSize2, by 4 */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *py--;
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[srcBLen - 1] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc1 += x[1] * y[srcBLen - 1] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc2 += x[2] * y[srcBLen - 1] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc3 += x[3] * y[srcBLen - 1] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+
+ /* Read y[srcBLen - 2] sample */
+ c0 = *py--;
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[srcBLen - 2] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc1 += x[2] * y[srcBLen - 2] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc2 += x[3] * y[srcBLen - 2] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc3 += x[4] * y[srcBLen - 2] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *py--;
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[2] * y[srcBLen - 3] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc1 += x[3] * y[srcBLen - 3] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc2 += x[4] * y[srcBLen - 3] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc3 += x[5] * y[srcBLen - 3] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x1 * c0)) >> 32);
+
+
+ /* Read y[srcBLen - 4] sample */
+ c0 = *py--;
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[3] * y[srcBLen - 4] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc1 += x[4] * y[srcBLen - 4] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc2 += x[5] * y[srcBLen - 4] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc3 += x[6] * y[srcBLen - 4] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x2 * c0)) >> 32);
+
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc3 += x[7] * y[srcBLen - 5] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (acc0 << 1);
+ *pOut++ = (q31_t) (acc1 << 1);
+ *pOut++ = (q31_t) (acc2 << 1);
+ *pOut++ = (q31_t) (acc3 << 1);
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = blockSize3 >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the blockSize3 is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = blockSize3 % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q15.c new file mode 100644 index 0000000..6839103 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q15.c @@ -0,0 +1,362 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_opt_q15.c
+ * Description: Convolution of Q15 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q15 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both inputs are in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ This approach provides 33 guard bits and there is no risk of overflow.
+ The 34.30 result is then truncated to 34.15 format by discarding the low 15 bits and then saturated to 1.15 format.
+ @remark
+ Refer to \ref arm_conv_fast_q15() for a faster but less precise version of this function.
+ */
+
+void arm_conv_opt_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+ q63_t acc0; /* Accumulators */
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch1 */
+ q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch1 */
+ const q15_t *px; /* Intermediate inputA pointer */
+ q15_t *py; /* Intermediate inputB pointer */
+ uint32_t j, k, blkCnt; /* Loop counter */
+ uint32_t tapCnt; /* Loop count */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x1, x2, x3; /* Temporary variables to hold state and coefficient values */
+ q31_t y1, y2; /* State variables */
+#endif
+
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Pointer to take end of scratch2 buffer */
+ pScr2 = pScratch2 + srcBLen - 1;
+
+ /* points to smaller length sequence */
+ px = pIn2;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ /* Copy smaller length input sequence in reverse order into second scratch buffer */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Initialze temporary scratch pointer */
+ pScr1 = pScratch1;
+
+ /* Assuming scratch1 buffer is aligned by 32-bit */
+ /* Fill (srcBLen - 1U) zeros in scratch1 buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy bigger length sequence(srcALen) samples in scratch1 buffer */
+
+ /* Copy (srcALen) samples in scratch buffer */
+ arm_copy_q15(pIn1, pScr1, srcALen);
+
+ /* Update pointers */
+ pScr1 += srcALen;
+
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+
+ /* Initialization of pIn2 pointer */
+ pIn2 = py;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = (srcALen + srcBLen - 1U) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+ y2 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLALD(x1, y1, acc0);
+ acc2 = __SMLALD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLALDX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* multiply and accumlate */
+ acc0 = __SMLALD(x2, y2, acc0);
+ acc2 = __SMLALD(x1, y2, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLALDX(x3, y1, acc3);
+ acc1 = __SMLALDX(x3, y2, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLALDX(x3, y2, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2);
+ acc1 += (*pScr1++ * *pIn2);
+ acc2 += (*pScr1++ * *pIn2);
+ acc3 += (*pScr1++ * *pIn2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the results in the accumulators in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (srcALen + srcBLen - 1U) & 0x3;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = (srcALen + srcBLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Calculate convolution for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read next two samples from scratch1 buffer */
+ acc0 += (*pScr1++ * *pIn2++);
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ Then store the output in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 1U;
+ }
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q7.c new file mode 100644 index 0000000..3ca938e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_opt_q7.c @@ -0,0 +1,360 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_opt_q7.c
+ * Description: Convolution of Q7 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q7 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @param[in] pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @param[in] pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 32-bit internal accumulator.
+ Both the inputs are represented in 1.7 format and multiplications yield a 2.14 result.
+ The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
+ This approach provides 17 guard bits and there is no risk of overflow as long as <code>max(srcALen, srcBLen)<131072</code>.
+ The 18.14 result is then truncated to 18.7 format by discarding the low 7 bits and then saturated to 1.7 format.
+ */
+
+void arm_conv_opt_q7(
+ const q7_t * pSrcA,
+ uint32_t srcALen,
+ const q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+ q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch */
+ q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch */
+ q15_t x4; /* Temporary input variable */
+ q15_t *py; /* Temporary input2 pointer */
+ q31_t acc0, acc1, acc2, acc3; /* Accumulators */
+ const q7_t *pIn1, *pIn2; /* InputA and inputB pointer */
+ uint32_t j, k, blkCnt, tapCnt; /* Loop counter */
+ q31_t x1, x2, x3, y1; /* Temporary input variables */
+ const q7_t *px; /* Temporary input1 pointer */
+ q7_t *pOut = pDst; /* Output pointer */
+ q7_t out0, out1, out2, out3; /* Temporary variables */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* points to smaller length sequence */
+ px = pIn2 + srcBLen - 1;
+
+ /* Apply loop unrolling and do 4 Copies simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling copies 4 data points at a time.
+ ** a second loop below copies for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, copy remaining samples here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy (srcALen) samples in scratch buffer */
+ /* Apply loop unrolling and do 4 Copies simultaneously. */
+ k = srcALen >> 2U;
+
+ /* First part of the processing with loop unrolling copies 4 data points at a time.
+ ** a second loop below copies for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, copy remaining samples here.
+ ** No loop unrolling is used. */
+ k = srcALen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ x4 = (q15_t) * pIn1++;
+ *pScr1++ = x4;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+ /* Initialization of pIn2 pointer */
+ pIn2 = (q7_t *) py;
+
+ pScr2 = py;
+
+ /* Actual convolution process starts here */
+ blkCnt = (srcALen + srcBLen - 1U) >> 2U;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+ acc2 = __SMLAD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ acc0 = __SMLAD(x2, y1, acc0);
+
+ acc2 = __SMLAD(x1, y1, acc2);
+
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pScr2);
+ acc1 += (*pScr1++ * *pScr2);
+ acc2 += (*pScr1++ * *pScr2);
+ acc3 += (*pScr1++ * *pScr2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ out0 = (q7_t) (__SSAT(acc0 >> 7U, 8));
+ out1 = (q7_t) (__SSAT(acc1 >> 7U, 8));
+ out2 = (q7_t) (__SSAT(acc2 >> 7U, 8));
+ out3 = (q7_t) (__SSAT(acc3 >> 7U, 8));
+
+ write_q7x4_ia (&pOut, __PACKq7(out0, out1, out2, out3));
+
+ /* Initialization of inputB pointer */
+ pScr2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ blkCnt = (srcALen + srcBLen - 1U) & 0x3;
+
+ /* Calculate convolution for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+ acc0 += (*pScr1++ * *pScr2++);
+ acc0 += (*pScr1++ * *pScr2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pScr2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(acc0 >> 7U, 8));
+
+ /* Initialization of inputB pointer */
+ pScr2 = py;
+
+ pScratch1 += 1U;
+ }
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_f32.c new file mode 100644 index 0000000..53bc9a3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_f32.c @@ -0,0 +1,676 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_f32.c
+ * Description: Partial convolution of floating-point sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup PartialConv Partial Convolution
+
+ Partial Convolution is equivalent to Convolution except that a subset of the output samples is generated.
+ Each function has two additional arguments.
+ <code>firstIndex</code> specifies the starting index of the subset of output samples.
+ <code>numPoints</code> is the number of output samples to compute.
+ The function computes the output in the range
+ <code>[firstIndex, ..., firstIndex+numPoints-1]</code>.
+ The output array <code>pDst</code> contains <code>numPoints</code> values.
+
+ The allowable range of output indices is [0 srcALen+srcBLen-2].
+ If the requested subset does not fall in this range then the functions return ARM_MATH_ARGUMENT_ERROR.
+ Otherwise the functions return ARM_MATH_SUCCESS.
+ \note Refer to \ref arm_conv_f32() for details on fixed point behavior.
+
+ @par Fast Versions
+ Fast versions are supported for Q31 and Q15 of partial convolution.
+ Cycles for Fast versions are less compared to Q31 and Q15 of partial conv and the design requires
+ the input signals should be scaled down to avoid intermediate overflows.
+
+ @par Opt Versions
+ Opt versions are supported for Q15 and Q7. Design uses internal scratch buffer for getting good optimisation.
+ These versions are optimised in cycles and consumes more memory (Scratch memory) compared to Q15 and Q7 versions of partial convolution
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of floating-point sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+ */
+
+arm_status arm_conv_partial_f32(
+ const float32_t * pSrcA,
+ uint32_t srcALen,
+ const float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const float32_t *pIn1 = pSrcA; /* InputA pointer */
+ const float32_t *pIn2 = pSrcB; /* InputB pointer */
+ float32_t *pOut = pDst; /* Output pointer */
+ const float32_t *px; /* Intermediate inputA pointer */
+ const float32_t *py; /* Intermediate inputB pointer */
+ const float32_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ float32_t sum; /* Accumulator */
+ uint32_t j, k, count, blkCnt, check;
+ int32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ float32_t acc0, acc1, acc2, acc3; /* Accumulator */
+ float32_t x0, x1, x2, x3, c0; /* Temporary variables */
+#endif
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Conditions to check which loopCounter holds
+ * the first and last indices of the output samples to be calculated. */
+ check = firstIndex + numPoints;
+ blockSize3 = ((int32_t)check > (int32_t)srcALen) ? (int32_t)check - (int32_t)srcALen : 0;
+ blockSize3 = ((int32_t)firstIndex > (int32_t)srcALen - 1) ? blockSize3 - (int32_t)firstIndex + (int32_t)srcALen : blockSize3;
+ blockSize1 = ((int32_t) srcBLen - 1) - (int32_t) firstIndex;
+ blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1U)) ? blockSize1 : (int32_t) numPoints) : 0;
+ blockSize2 = ((int32_t) check - blockSize3) - (blockSize1 + (int32_t) firstIndex);
+ blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* Set the output pointer to point to the firstIndex
+ * of the output sample to be calculated. */
+ pOut = pDst + firstIndex;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed.
+ Since the partial convolution starts from firstIndex
+ Number of Macs to be performed is firstIndex + 1 */
+ count = 1U + firstIndex;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + firstIndex;
+ py = pSrc1;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 1] */
+ sum += *px++ * *py--;
+
+ /* x[1] * y[srcBLen - 2] */
+ sum += *px++ * *py--;
+
+ /* x[2] * y[srcBLen - 3] */
+ sum += *px++ * *py--;
+
+ /* x[3] * y[srcBLen - 4] */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc1;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ if ((int32_t)firstIndex - (int32_t)srcBLen + 1 > 0)
+ {
+ pSrc1 = pIn1 + firstIndex - srcBLen + 1;
+ }
+ else
+ {
+ pSrc1 = pIn1;
+ }
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = ((uint32_t) blockSize2 >> 2U);
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0.0f;
+ acc1 = 0.0f;
+ acc2 = 0.0f;
+ acc3 = 0.0f;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *py--;
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[srcBLen - 1] */
+ acc0 += x0 * c0;
+ /* acc1 += x[1] * y[srcBLen - 1] */
+ acc1 += x1 * c0;
+ /* acc2 += x[2] * y[srcBLen - 1] */
+ acc2 += x2 * c0;
+ /* acc3 += x[3] * y[srcBLen - 1] */
+ acc3 += x3 * c0;
+
+ /* Read y[srcBLen - 2] sample */
+ c0 = *py--;
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[srcBLen - 2] */
+ acc0 += x1 * c0;
+ /* acc1 += x[2] * y[srcBLen - 2] */
+ acc1 += x2 * c0;
+ /* acc2 += x[3] * y[srcBLen - 2] */
+ acc2 += x3 * c0;
+ /* acc3 += x[4] * y[srcBLen - 2] */
+ acc3 += x0 * c0;
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *py--;
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[2] * y[srcBLen - 3] */
+ acc0 += x2 * c0;
+ /* acc1 += x[3] * y[srcBLen - 2] */
+ acc1 += x3 * c0;
+ /* acc2 += x[4] * y[srcBLen - 2] */
+ acc2 += x0 * c0;
+ /* acc3 += x[5] * y[srcBLen - 2] */
+ acc3 += x1 * c0;
+
+ /* Read y[srcBLen - 4] sample */
+ c0 = *py--;
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[3] * y[srcBLen - 4] */
+ acc0 += x3 * c0;
+ /* acc1 += x[4] * y[srcBLen - 4] */
+ acc1 += x0 * c0;
+ /* acc2 += x[5] * y[srcBLen - 4] */
+ acc2 += x1 * c0;
+ /* acc3 += x[6] * y[srcBLen - 4] */
+ acc3 += x2 * c0;
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 += x0 * c0;
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 += x1 * c0;
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 += x2 * c0;
+ /* acc3 += x[7] * y[srcBLen - 5] */
+ acc3 += x3 * c0;
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = acc0;
+ *pOut++ = acc1;
+ *pOut++ = acc2;
+ *pOut++ = acc3;
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (uint32_t) blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += *px++ * *py--;
+ sum += *px++ * *py--;
+ sum += *px++ * *py--;
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = (uint32_t) blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ sum += *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum += *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ sum += *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum += *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const float32_t *pIn1 = pSrcA; /* InputA pointer */
+ const float32_t *pIn2 = pSrcB; /* InputB pointer */
+ float32_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* Loop to calculate convolution for output length number of values */
+ for (i = firstIndex; i <= (firstIndex + numPoints - 1); i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0.0f;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ( pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = sum;
+ }
+
+ /* Set status as ARM_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c new file mode 100644 index 0000000..ae640f2 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_opt_q15.c @@ -0,0 +1,387 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_fast_opt_q15.c
+ * Description: Fast Q15 Partial convolution
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q15 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2
+ @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen)
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+
+ @remark
+ Refer to \ref arm_conv_partial_q15() for a slower implementation of this function which uses a 64-bit accumulator to avoid wrap around distortion.
+ */
+
+arm_status arm_conv_partial_fast_opt_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+ q15_t *pOut = pDst; /* Output pointer */
+ q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch1 */
+ q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch1 */
+ q31_t acc0; /* Accumulator */
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ const q15_t *px; /* Intermediate inputA pointer */
+ q15_t *py; /* Intermediate inputB pointer */
+ uint32_t j, k, blkCnt; /* Loop counter */
+ uint32_t tapCnt; /* Loop count */
+ arm_status status; /* Status variable */
+ q31_t x1; /* Temporary variables to hold state and coefficient values */
+ q31_t y1; /* State variables */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc1, acc2, acc3; /* Accumulator */
+ q31_t x2, x3; /* Temporary variables to hold state and coefficient values */
+ q31_t y2; /* State variables */
+#endif
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+ /* pointer to take end of scratch2 buffer */
+ pScr2 = pScratch2 + srcBLen - 1;
+
+ /* points to smaller length sequence */
+ px = pIn2;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ /* Copy smaller length input sequence in reverse order into second scratch buffer */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Initialze temporary scratch pointer */
+ pScr1 = pScratch1;
+
+ /* Assuming scratch1 buffer is aligned by 32-bit */
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy bigger length sequence(srcALen) samples in scratch1 buffer */
+
+ /* Copy (srcALen) samples in scratch buffer */
+ arm_copy_q15(pIn1, pScr1, srcALen);
+
+ /* Update pointers */
+ pScr1 += srcALen;
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Initialization of pIn2 pointer */
+ pIn2 = py;
+
+ pScratch1 += firstIndex;
+
+ pOut = pDst + firstIndex;
+
+ /* Actual convolution process starts here */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = (numPoints) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+ y2 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+ acc2 = __SMLAD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x2, y2, acc0);
+ acc2 = __SMLAD(x1, y2, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+ acc1 = __SMLADX(x3, y2, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc3 = __SMLADX(x3, y2, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2);
+ acc1 += (*pScr1++ * *pIn2);
+ acc2 += (*pScr1++ * *pIn2);
+ acc3 += (*pScr1++ * *pIn2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the results in the accumulators in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = numPoints & 0x3;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = numPoints;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Calculate convolution for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read two samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ ** Then store the output in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 1U;
+
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q15.c new file mode 100644 index 0000000..92e7487 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q15.c @@ -0,0 +1,700 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_fast_q15.c
+ * Description: Fast Q15 Partial convolution
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q15 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+ @remark
+ Refer to \ref arm_conv_partial_q15() for a slower implementation of this function which uses a 64-bit accumulator to avoid wrap around distortion.
+ */
+
+arm_status arm_conv_partial_fast_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints)
+{
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q31_t sum, acc0, acc1, acc2, acc3; /* Accumulator */
+ const q15_t *px; /* Intermediate inputA pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ const q15_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t x0, x1, x2, x3, c0; /* Temporary input variables */
+ uint32_t j, k, count, blkCnt, check;
+ int32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Conditions to check which loopCounter holds
+ * the first and last indices of the output samples to be calculated. */
+ check = firstIndex + numPoints;
+ blockSize3 = ((int32_t)check > (int32_t)srcALen) ? (int32_t)check - (int32_t)srcALen : 0;
+ blockSize3 = ((int32_t)firstIndex > (int32_t)srcALen - 1) ? blockSize3 - (int32_t)firstIndex + (int32_t)srcALen : blockSize3;
+ blockSize1 = ((int32_t) srcBLen - 1) - (int32_t) firstIndex;
+ blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1U)) ? blockSize1 : (int32_t) numPoints) : 0;
+ blockSize2 = (int32_t) check - ((blockSize3 + blockSize1) + (int32_t) firstIndex);
+ blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* Set the output pointer to point to the firstIndex
+ * of the output sample to be calculated. */
+ pOut = pDst + firstIndex;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed.
+ Since the partial convolution starts from firstIndex
+ Number of Macs to be performed is firstIndex + 1 */
+ count = 1U + firstIndex;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + firstIndex;
+ py = pSrc2;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations less than 4 */
+ /* Second part of this stage computes the MAC operations greater than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ while ((count < 4U) && (blockSize1 > 0))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Loop over number of MAC operations between
+ * inputA samples and inputB samples */
+ k = count;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* The second part of the stage starts here */
+ /* The internal loop, over count, is unrolled by 4 */
+ /* To, read the last two inputB samples using SIMD:
+ * y[srcBLen] and y[srcBLen-1] coefficients, py is decremented by 1 */
+ py = py - 1;
+
+ while (blockSize1 > 0)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0], x[1] are multiplied with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[2], x[3] are multiplied with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2 - 1U;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ if ((int32_t)firstIndex - (int32_t)srcBLen + 1 > 0)
+ {
+ pSrc1 = pIn1 + firstIndex - srcBLen + 1;
+ }
+ else
+ {
+ pSrc1 = pIn1;
+ }
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = ((uint32_t) blockSize2 >> 2U);
+
+ while (blkCnt > 0U)
+ {
+ py = py - 1U;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+
+ /* read x[0], x[1] samples */
+ x0 = read_q15x2 ((q15_t *) px);
+ /* read x[1], x[2] samples */
+ x1 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read the last two inputB samples using SIMD:
+ * y[srcBLen - 1] and y[srcBLen - 2] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
+ acc0 = __SMLADX(x0, c0, acc0);
+
+ /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
+ acc1 = __SMLADX(x1, c0, acc1);
+
+ /* Read x[2], x[3] */
+ x2 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[3], x[4] */
+ x3 = read_q15x2 ((q15_t *) px + 1);
+
+ /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
+ acc2 = __SMLADX(x2, c0, acc2);
+
+ /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
+ acc3 = __SMLADX(x3, c0, acc3);
+
+ /* Read y[srcBLen - 3] and y[srcBLen - 4] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
+ acc0 = __SMLADX(x2, c0, acc0);
+
+ /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
+ acc1 = __SMLADX(x3, c0, acc1);
+
+ /* Read x[4], x[5] */
+ x0 = read_q15x2 ((q15_t *) px + 2);
+
+ /* Read x[5], x[6] */
+ x1 = read_q15x2 ((q15_t *) px + 3);
+ px += 4U;
+
+ /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
+ acc2 = __SMLADX(x0, c0, acc2);
+
+ /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
+ acc3 = __SMLADX(x1, c0, acc3);
+
+ } while (--k);
+
+ /* For the next MAC operations, SIMD is not used
+ So, the 16 bit pointer if inputB, py is updated */
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ if (k == 1U)
+ {
+ /* Read y[srcBLen - 5] */
+ c0 = *(py + 1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[7] */
+ x3 = read_q15x2 ((q15_t *) px);
+ px++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLAD (x0, c0, acc0);
+ acc1 = __SMLAD (x1, c0, acc1);
+ acc2 = __SMLADX(x1, c0, acc2);
+ acc3 = __SMLADX(x3, c0, acc3);
+ }
+
+ if (k == 2U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLADX(x0, c0, acc0);
+ acc1 = __SMLADX(x1, c0, acc1);
+ acc2 = __SMLADX(x3, c0, acc2);
+ acc3 = __SMLADX(x2, c0, acc3);
+ }
+
+ if (k == 3U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLADX(x0, c0, acc0);
+ acc1 = __SMLADX(x1, c0, acc1);
+ acc2 = __SMLADX(x3, c0, acc2);
+ acc3 = __SMLADX(x2, c0, acc3);
+
+ c0 = *(py-1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[10] */
+ x3 = read_q15x2 ((q15_t *) px + 2);
+ px += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLADX(x1, c0, acc0);
+ acc1 = __SMLAD (x2, c0, acc1);
+ acc2 = __SMLADX(x2, c0, acc2);
+ acc3 = __SMLADX(x3, c0, acc3);
+ }
+
+ /* Store the results in the accumulators in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(acc0 >> 15, acc1 >> 15, 16));
+ write_q15x2_ia (&pOut, __PKHBT(acc2 >> 15, acc3 >> 15, 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(acc1 >> 15, acc0 >> 15, 16));
+ write_q15x2_ia (&pOut, __PKHBT(acc3 >> 15, acc2 >> 15, 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ No loop unrolling is used. */
+ blkCnt = (uint32_t) blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q31_t) *px++ * *py--);
+ sum += ((q31_t) *px++ * *py--);
+ sum += ((q31_t) *px++ * *py--);
+ sum += ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q31_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = (uint32_t) blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q31_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ pIn2 = pSrc2 - 1U;
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations greater than 4 */
+ /* Second part of this stage computes the MAC operations less than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ j = count >> 2U;
+
+ while ((j > 0U) && (blockSize3 > 0))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[srcALen - srcBLen + 1], x[srcALen - srcBLen + 2] are multiplied
+ * with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[srcALen - srcBLen + 3], x[srcALen - srcBLen + 4] are multiplied
+ * with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLADX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 5] * y[srcBLen - 5] */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement the MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+
+ j--;
+ }
+
+ /* The second part of the stage starts here */
+ /* SIMD is not used for the next MAC operations,
+ * so pointer py is updated to read only one sample at a time */
+ py = py + 1U;
+
+ while (blockSize3 > 0)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum = __SMLAD(*px++, *py--, sum);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (sum >> 15);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement the MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q31.c new file mode 100644 index 0000000..f906b5c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_fast_q31.c @@ -0,0 +1,618 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_fast_q31.c
+ * Description: Fast Q31 Partial convolution
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q31 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+
+ @remark
+ Refer to \ref arm_conv_partial_q31() for a slower implementation of this function which uses a 64-bit accumulator to provide higher precision.
+ */
+
+arm_status arm_conv_partial_fast_q31(
+ const q31_t * pSrcA,
+ uint32_t srcALen,
+ const q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints)
+{
+ const q31_t *pIn1; /* InputA pointer */
+ const q31_t *pIn2; /* InputB pointer */
+ q31_t *pOut = pDst; /* Output pointer */
+ const q31_t *px; /* Intermediate inputA pointer */
+ const q31_t *py; /* Intermediate inputB pointer */
+ const q31_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t sum; /* Accumulators */
+ uint32_t j, k, count, check, blkCnt;
+ int32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc0, acc1, acc2, acc3; /* Accumulators */
+ q31_t x0, x1, x2, x3, c0;
+#endif
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Conditions to check which loopCounter holds
+ * the first and last indices of the output samples to be calculated. */
+ check = firstIndex + numPoints;
+ blockSize3 = ((int32_t)check > (int32_t)srcALen) ? (int32_t)check - (int32_t)srcALen : 0;
+ blockSize3 = ((int32_t)firstIndex > (int32_t)srcALen - 1) ? blockSize3 - (int32_t)firstIndex + (int32_t)srcALen : blockSize3;
+ blockSize1 = ((int32_t) srcBLen - 1) - (int32_t) firstIndex;
+ blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1U)) ? blockSize1 : (int32_t) numPoints) : 0;
+ blockSize2 = (int32_t) check - ((blockSize3 + blockSize1) + (int32_t) firstIndex);
+ blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* Set the output pointer to point to the firstIndex
+ * of the output sample to be calculated. */
+ pOut = pDst + firstIndex;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed.
+ Since the partial convolution starts from firstIndex
+ Number of Macs to be performed is firstIndex + 1 */
+ count = 1U + firstIndex;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + firstIndex;
+ py = pSrc2;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* x[1] * y[srcBLen - 2] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* x[2] * y[srcBLen - 3] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* x[3] * y[srcBLen - 4] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ if ((int32_t)firstIndex - (int32_t)srcBLen + 1 > 0)
+ {
+ pSrc1 = pIn1 + firstIndex - srcBLen + 1;
+ }
+ else
+ {
+ pSrc1 = pIn1;
+ }
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = ((uint32_t) blockSize2 >> 2U);
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *py--;
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[srcBLen - 1] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc1 += x[1] * y[srcBLen - 1] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc2 += x[2] * y[srcBLen - 1] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc3 += x[3] * y[srcBLen - 1] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Read y[srcBLen - 2] sample */
+ c0 = *py--;
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[srcBLen - 2] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc1 += x[2] * y[srcBLen - 2] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc2 += x[3] * y[srcBLen - 2] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc3 += x[4] * y[srcBLen - 2] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *py--;
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[2] * y[srcBLen - 3] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc1 += x[3] * y[srcBLen - 2] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc2 += x[4] * y[srcBLen - 2] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc3 += x[5] * y[srcBLen - 2] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x1 * c0)) >> 32);
+
+ /* Read y[srcBLen - 4] sample */
+ c0 = *py--;
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[3] * y[srcBLen - 4] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc1 += x[4] * y[srcBLen - 4] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc2 += x[5] * y[srcBLen - 4] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc3 += x[6] * y[srcBLen - 4] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x2 * c0)) >> 32);
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc3 += x[7] * y[srcBLen - 5] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (acc0 << 1);
+ *pOut++ = (q31_t) (acc1 << 1);
+ *pOut++ = (q31_t) (acc2 << 1);
+ *pOut++ = (q31_t) (acc3 << 1);
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (uint32_t) blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) * px++ * (*py--))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) * px++ * (*py--))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) * px++ * (*py--))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) * px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = (uint32_t) blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py--))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = sum << 1;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q15.c new file mode 100644 index 0000000..05c3284 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q15.c @@ -0,0 +1,386 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_opt_q15.c
+ * Description: Partial convolution of Q15 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q15 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @param[in] pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @param[in] pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+
+ @remark
+ Refer to \ref arm_conv_partial_fast_q15() for a faster but less precise version of this function.
+ */
+
+arm_status arm_conv_partial_opt_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+
+ q15_t *pOut = pDst; /* Output pointer */
+ q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch1 */
+ q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch1 */
+ q63_t acc0; /* Accumulator */
+ q31_t x1; /* Temporary variables to hold state and coefficient values */
+ q31_t y1; /* State variables */
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ const q15_t *px; /* Intermediate inputA pointer */
+ q15_t *py; /* Intermediate inputB pointer */
+ uint32_t j, k, blkCnt; /* Loop counter */
+ uint32_t tapCnt; /* Loop count */
+ arm_status status; /* Status variable */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc1, acc2, acc3; /* Accumulator */
+ q31_t x2, x3; /* Temporary variables to hold state and coefficient values */
+ q31_t y2; /* State variables */
+#endif
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+ /* pointer to take end of scratch2 buffer */
+ pScr2 = pScratch2 + srcBLen - 1;
+
+ /* points to smaller length sequence */
+ px = pIn2;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ /* Copy smaller length input sequence in reverse order into second scratch buffer */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ *pScr2-- = *px++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Initialze temporary scratch pointer */
+ pScr1 = pScratch1;
+
+ /* Assuming scratch1 buffer is aligned by 32-bit */
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy bigger length sequence(srcALen) samples in scratch1 buffer */
+
+ /* Copy (srcALen) samples in scratch buffer */
+ arm_copy_q15(pIn1, pScr1, srcALen);
+
+ /* Update pointers */
+ pScr1 += srcALen;
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Initialization of pIn2 pointer */
+ pIn2 = py;
+
+ pScratch1 += firstIndex;
+
+ pOut = pDst + firstIndex;
+
+ /* Actual convolution process starts here */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = (numPoints) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+ y2 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLALD(x1, y1, acc0);
+ acc2 = __SMLALD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLALDX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* multiply and accumlate */
+ acc0 = __SMLALD(x2, y2, acc0);
+ acc2 = __SMLALD(x1, y2, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLALDX(x3, y1, acc3);
+ acc1 = __SMLALDX(x3, y2, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLALDX(x3, y2, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2);
+ acc1 += (*pScr1++ * *pIn2);
+ acc2 += (*pScr1++ * *pIn2);
+ acc3 += (*pScr1++ * *pIn2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the results in the accumulators in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = numPoints & 0x3;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = numPoints;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Calculate convolution for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read two samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ acc0 = __SMLALD(x1, y1, acc0);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ ** Then store the output in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch1 += 1U;
+
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q7.c new file mode 100644 index 0000000..5ac2af4 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_opt_q7.c @@ -0,0 +1,390 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_opt_q7.c
+ * Description: Partial convolution of Q7 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q7 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @param[in] pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @param[in] pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+ */
+
+arm_status arm_conv_partial_opt_q7(
+ const q7_t * pSrcA,
+ uint32_t srcALen,
+ const q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+ q15_t *pScr2, *pScr1; /* Intermediate pointers for scratch pointers */
+ q15_t x4; /* Temporary input variable */
+ const q7_t *pIn1, *pIn2; /* InputA and inputB pointer */
+ uint32_t j, k, blkCnt, tapCnt; /* Loop counter */
+ const q7_t *px; /* Temporary input1 pointer */
+ q15_t *py; /* Temporary input2 pointer */
+ q31_t acc0, acc1, acc2, acc3; /* Accumulator */
+ q31_t x1, x2, x3, y1; /* Temporary input variables */
+ arm_status status;
+ q7_t *pOut = pDst; /* Output pointer */
+ q7_t out0, out1, out2, out3; /* Temporary variables */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* pointer to take end of scratch2 buffer */
+ pScr2 = pScratch2;
+
+ /* points to smaller length sequence */
+ px = pIn2 + srcBLen - 1;
+
+ /* Apply loop unrolling and do 4 Copies simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling copies 4 data points at a time.
+ ** a second loop below copies for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, copy remaining samples here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ x4 = (q15_t) *px--;
+ *pScr2++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Initialze temporary scratch pointer */
+ pScr1 = pScratch1;
+
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy (srcALen) samples in scratch buffer */
+ /* Apply loop unrolling and do 4 Copies simultaneously. */
+ k = srcALen >> 2U;
+
+ /* First part of the processing with loop unrolling copies 4 data points at a time.
+ ** a second loop below copies for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, copy remaining samples here.
+ ** No loop unrolling is used. */
+ k = srcALen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+ /* Initialization of pIn2 pointer */
+ pIn2 = (q7_t *) py;
+
+ pScr2 = py;
+
+ pOut = pDst + firstIndex;
+
+ pScratch1 += firstIndex;
+
+ /* Actual convolution process starts here */
+ blkCnt = (numPoints) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialize temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumulators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+ acc2 = __SMLAD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ acc0 = __SMLAD(x2, y1, acc0);
+
+ acc2 = __SMLAD(x1, y1, acc2);
+
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pScr2);
+ acc1 += (*pScr1++ * *pScr2);
+ acc2 += (*pScr1++ * *pScr2);
+ acc3 += (*pScr1++ * *pScr2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ out0 = (q7_t) (__SSAT(acc0 >> 7U, 8));
+ out1 = (q7_t) (__SSAT(acc1 >> 7U, 8));
+ out2 = (q7_t) (__SSAT(acc2 >> 7U, 8));
+ out3 = (q7_t) (__SSAT(acc3 >> 7U, 8));
+
+ write_q7x4_ia (&pOut, __PACKq7(out0, out1, out2, out3));
+
+ /* Initialization of inputB pointer */
+ pScr2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ blkCnt = (numPoints) & 0x3;
+
+ /* Calculate convolution for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read two samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ acc0 = __SMLAD(x1, y1, acc0);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pScr2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(acc0 >> 7U, 8));
+
+ /* Initialization of inputB pointer */
+ pScr2 = py;
+
+ pScratch1 += 1U;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q15.c new file mode 100644 index 0000000..15ea796 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q15.c @@ -0,0 +1,752 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_q15.c
+ * Description: Partial convolution of Q15 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q15 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+
+ @remark
+ Refer to \ref arm_conv_partial_fast_q15() for a faster but less precise version of this function.
+ @remark
+ Refer to \ref arm_conv_partial_opt_q15() for a faster implementation of this function using scratch buffers.
+ */
+
+arm_status arm_conv_partial_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints)
+{
+
+#if defined (ARM_MATH_DSP)
+
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q63_t sum, acc0, acc1, acc2, acc3; /* Accumulator */
+ const q15_t *px; /* Intermediate inputA pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ const q15_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t x0, x1, x2, x3, c0; /* Temporary input variables to hold state and coefficient values */
+ int32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt, check;
+ arm_status status; /* Status of Partial convolution */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Conditions to check which loopCounter holds
+ * the first and last indices of the output samples to be calculated. */
+ check = firstIndex + numPoints;
+ blockSize3 = ((int32_t)check > (int32_t)srcALen) ? (int32_t)check - (int32_t)srcALen : 0;
+ blockSize3 = ((int32_t)firstIndex > (int32_t)srcALen - 1) ? blockSize3 - (int32_t)firstIndex + (int32_t)srcALen : blockSize3;
+ blockSize1 = ((int32_t) srcBLen - 1) - (int32_t) firstIndex;
+ blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1U)) ? blockSize1 : (int32_t) numPoints) : 0;
+ blockSize2 = (int32_t) check - ((blockSize3 + blockSize1) + (int32_t) firstIndex);
+ blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* Set the output pointer to point to the firstIndex
+ * of the output sample to be calculated. */
+ pOut = pDst + firstIndex;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed.
+ Since the partial convolution starts from firstIndex
+ Number of Macs to be performed is firstIndex + 1 */
+ count = 1U + firstIndex;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + firstIndex;
+ py = pSrc2;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations less than 4 */
+ /* Second part of this stage computes the MAC operations greater than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ while ((count < 4U) && (blockSize1 > 0U))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Loop over number of MAC operations between
+ * inputA samples and inputB samples */
+ k = count;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* The second part of the stage starts here */
+ /* The internal loop, over count, is unrolled by 4 */
+ /* To, read the last two inputB samples using SIMD:
+ * y[srcBLen] and y[srcBLen-1] coefficients, py is decremented by 1 */
+ py = py - 1;
+
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0], x[1] are multiplied with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[2], x[3] are multiplied with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ * So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2 - 1U;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ if ((int32_t)firstIndex - (int32_t)srcBLen + 1 > 0)
+ {
+ pSrc1 = pIn1 + firstIndex - srcBLen + 1;
+ }
+ else
+ {
+ pSrc1 = pIn1;
+ }
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = ((uint32_t) blockSize2 >> 2U);
+
+ while (blkCnt > 0U)
+ {
+ py = py - 1U;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+
+ /* read x[0], x[1] samples */
+ x0 = read_q15x2 ((q15_t *) px);
+ /* read x[1], x[2] samples */
+ x1 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read the last two inputB samples using SIMD:
+ * y[srcBLen - 1] and y[srcBLen - 2] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
+ acc0 = __SMLALDX(x0, c0, acc0);
+
+ /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
+ acc1 = __SMLALDX(x1, c0, acc1);
+
+ /* Read x[2], x[3] */
+ x2 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[3], x[4] */
+ x3 = read_q15x2 ((q15_t *) px + 1);
+
+ /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
+ acc2 = __SMLALDX(x2, c0, acc2);
+
+ /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
+ acc3 = __SMLALDX(x3, c0, acc3);
+
+ /* Read y[srcBLen - 3] and y[srcBLen - 4] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
+ acc0 = __SMLALDX(x2, c0, acc0);
+
+ /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
+ acc1 = __SMLALDX(x3, c0, acc1);
+
+ /* Read x[4], x[5] */
+ x0 = read_q15x2 ((q15_t *) px + 2);
+
+ /* Read x[5], x[6] */
+ x1 = read_q15x2 ((q15_t *) px + 3);
+ px += 4U;
+
+ /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
+ acc2 = __SMLALDX(x0, c0, acc2);
+
+ /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
+ acc3 = __SMLALDX(x1, c0, acc3);
+
+ } while (--k);
+
+ /* For the next MAC operations, SIMD is not used
+ * So, the 16 bit pointer if inputB, py is updated */
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ if (k == 1U)
+ {
+ /* Read y[srcBLen - 5] */
+ c0 = *(py+1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[7] */
+ x3 = read_q15x2 ((q15_t *) px);
+ px++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD (x0, c0, acc0);
+ acc1 = __SMLALD (x1, c0, acc1);
+ acc2 = __SMLALDX(x1, c0, acc2);
+ acc3 = __SMLALDX(x3, c0, acc3);
+ }
+
+ if (k == 2U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALDX(x0, c0, acc0);
+ acc1 = __SMLALDX(x1, c0, acc1);
+ acc2 = __SMLALDX(x3, c0, acc2);
+ acc3 = __SMLALDX(x2, c0, acc3);
+ }
+
+ if (k == 3U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALDX(x0, c0, acc0);
+ acc1 = __SMLALDX(x1, c0, acc1);
+ acc2 = __SMLALDX(x3, c0, acc2);
+ acc3 = __SMLALDX(x2, c0, acc3);
+
+ c0 = *(py-1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[10] */
+ x3 = read_q15x2 ((q15_t *) px + 2);
+ px += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLALDX(x1, c0, acc0);
+ acc1 = __SMLALD (x2, c0, acc1);
+ acc2 = __SMLALDX(x2, c0, acc2);
+ acc3 = __SMLALDX(x3, c0, acc3);
+ }
+
+ /* Store the results in the accumulators in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ No loop unrolling is used. */
+ blkCnt = (uint32_t) blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT(sum >> 15, 16));
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = (uint32_t) blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT(sum >> 15, 16));
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ pIn2 = pSrc2 - 1U;
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations greater than 4 */
+ /* Second part of this stage computes the MAC operations less than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ j = count >> 2U;
+
+ while ((j > 0U) && (blockSize3 > 0U))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[srcALen - srcBLen + 1], x[srcALen - srcBLen + 2] are multiplied
+ * with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[srcALen - srcBLen + 3], x[srcALen - srcBLen + 4] are multiplied
+ * with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ * So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 5] * y[srcBLen - 5] */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement loop counter */
+ blockSize3--;
+
+ j--;
+ }
+
+ /* The second part of the stage starts here */
+ /* SIMD is not used for the next MAC operations,
+ * so pointer py is updated to read only one sample at a time */
+ py = py + 1U;
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+ const q15_t *pIn1 = pSrcA; /* InputA pointer */
+ const q15_t *pIn2 = pSrcB; /* InputB pointer */
+ q63_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* Loop to calculate convolution for output length number of values */
+ for (i = firstIndex; i <= (firstIndex + numPoints - 1); i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q31_t) pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = (q15_t) __SSAT((sum >> 15U), 16U);
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q31.c new file mode 100644 index 0000000..b8042ef --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q31.c @@ -0,0 +1,634 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_q31.c
+ * Description: Partial convolution of Q31 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q31 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+
+ @remark
+ Refer to \ref arm_conv_partial_fast_q31() for a faster but less precise implementation of this function.
+ */
+
+arm_status arm_conv_partial_q31(
+ const q31_t * pSrcA,
+ uint32_t srcALen,
+ const q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const q31_t *pIn1; /* InputA pointer */
+ const q31_t *pIn2; /* InputB pointer */
+ q31_t *pOut = pDst; /* Output pointer */
+ const q31_t *px; /* Intermediate inputA pointer */
+ const q31_t *py; /* Intermediate inputB pointer */
+ const q31_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q63_t sum; /* Accumulator */
+ uint32_t j, k, count, blkCnt, check;
+ int32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc0, acc1, acc2; /* Accumulator */
+ q31_t x0, x1, x2, c0; /* Temporary variables */
+#endif
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Conditions to check which loopCounter holds
+ * the first and last indices of the output samples to be calculated. */
+ check = firstIndex + numPoints;
+ blockSize3 = ((int32_t)check > (int32_t)srcALen) ? (int32_t)check - (int32_t)srcALen : 0;
+ blockSize3 = ((int32_t)firstIndex > (int32_t)srcALen - 1) ? blockSize3 - (int32_t)firstIndex + (int32_t)srcALen : blockSize3;
+ blockSize1 = ((int32_t) srcBLen - 1) - (int32_t) firstIndex;
+ blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1U)) ? blockSize1 : (int32_t) numPoints) : 0;
+ blockSize2 = (int32_t) check - ((blockSize3 + blockSize1) + (int32_t) firstIndex);
+ blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* Set the output pointer to point to the firstIndex
+ * of the output sample to be calculated. */
+ pOut = pDst + firstIndex;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed.
+ Since the partial convolution starts from firstIndex
+ Number of Macs to be performed is firstIndex + 1 */
+ count = 1U + firstIndex;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + firstIndex;
+ py = pSrc2;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 1] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* x[1] * y[srcBLen - 2] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* x[2] * y[srcBLen - 3] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* x[3] * y[srcBLen - 4] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ if ((int32_t)firstIndex - (int32_t)srcBLen + 1 > 0)
+ {
+ pSrc1 = pIn1 + firstIndex - srcBLen + 1;
+ }
+ else
+ {
+ pSrc1 = pIn1;
+ }
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unroll over blkCnt */
+ blkCnt = blockSize2 / 3;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+
+ /* read x[0], x[1] samples */
+ x0 = *px++;
+ x1 = *px++;
+
+ /* Apply loop unrolling and compute 3 MACs simultaneously. */
+ k = srcBLen / 3;
+
+ /* First part of the processing with loop unrolling. Compute 3 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 2 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *(py);
+
+ /* Read x[2] sample */
+ x2 = *(px);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[srcBLen - 1] */
+ acc0 += (q63_t) x0 * c0;
+ /* acc1 += x[1] * y[srcBLen - 1] */
+ acc1 += (q63_t) x1 * c0;
+ /* acc2 += x[2] * y[srcBLen - 1] */
+ acc2 += (q63_t) x2 * c0;
+
+ /* Read y[srcBLen - 2] sample */
+ c0 = *(py - 1U);
+
+ /* Read x[3] sample */
+ x0 = *(px + 1U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[srcBLen - 2] */
+ acc0 += (q63_t) x1 * c0;
+ /* acc1 += x[2] * y[srcBLen - 2] */
+ acc1 += (q63_t) x2 * c0;
+ /* acc2 += x[3] * y[srcBLen - 2] */
+ acc2 += (q63_t) x0 * c0;
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *(py - 2U);
+
+ /* Read x[4] sample */
+ x1 = *(px + 2U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[2] * y[srcBLen - 3] */
+ acc0 += (q63_t) x2 * c0;
+ /* acc1 += x[3] * y[srcBLen - 2] */
+ acc1 += (q63_t) x0 * c0;
+ /* acc2 += x[4] * y[srcBLen - 2] */
+ acc2 += (q63_t) x1 * c0;
+
+
+ px += 3U;
+
+ py -= 3U;
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 3, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen - (3 * (srcBLen / 3));
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x2 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 += (q63_t) x0 * c0;
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 += (q63_t) x1 * c0;
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 += (q63_t) x2 * c0;
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (acc0 >> 31);
+ *pOut++ = (q31_t) (acc1 >> 31);
+ *pOut++ = (q31_t) (acc2 >> 31);
+
+ /* Increment the pointer pIn1 index, count by 3 */
+ count += 3U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize2 - 3 * (blockSize2 / 3);
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += (q63_t) *px++ * (*py--);
+ sum += (q63_t) *px++ * (*py--);
+ sum += (q63_t) *px++ * (*py--);
+ sum += (q63_t) *px++ * (*py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = (uint32_t) blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const q31_t *pIn1 = pSrcA; /* InputA pointer */
+ const q31_t *pIn2 = pSrcB; /* InputB pointer */
+ q63_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* Loop to calculate convolution for output length number of values */
+ for (i = firstIndex; i <= (firstIndex + numPoints - 1); i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q63_t) pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = (q31_t) (sum >> 31U);
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q7.c new file mode 100644 index 0000000..882e6c5 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_partial_q7.c @@ -0,0 +1,753 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_partial_q7.c
+ * Description: Partial convolution of Q7 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup PartialConv
+ @{
+ */
+
+/**
+ @brief Partial convolution of Q7 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written
+ @param[in] firstIndex is the first output sample to start with
+ @param[in] numPoints is the number of output points to be computed
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : requested subset is not in the range [0 srcALen+srcBLen-2]
+
+ @remark
+ Refer to \ref arm_conv_partial_opt_q7() for a faster implementation of this function.
+ */
+
+arm_status arm_conv_partial_q7(
+ const q7_t * pSrcA,
+ uint32_t srcALen,
+ const q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ uint32_t firstIndex,
+ uint32_t numPoints)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const q7_t *pIn1; /* InputA pointer */
+ const q7_t *pIn2; /* InputB pointer */
+ q7_t *pOut = pDst; /* Output pointer */
+ const q7_t *px; /* Intermediate inputA pointer */
+ const q7_t *py; /* Intermediate inputB pointer */
+ const q7_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t sum; /* Accumulator */
+ uint32_t j, k, count, blkCnt, check; /* Loop counters */
+ int32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc0, acc1, acc2, acc3; /* Accumulator */
+ q31_t input1, input2; /* Temporary input variables */
+ q15_t in1, in2; /* Temporary input variables */
+ q7_t x0, x1, x2, x3, c0, c1; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* Conditions to check which loopCounter holds
+ * the first and last indices of the output samples to be calculated. */
+ check = firstIndex + numPoints;
+ blockSize3 = ((int32_t)check > (int32_t)srcALen) ? (int32_t)check - (int32_t)srcALen : 0;
+ blockSize3 = ((int32_t)firstIndex > (int32_t)srcALen - 1) ? blockSize3 - (int32_t)firstIndex + (int32_t)srcALen : blockSize3;
+ blockSize1 = ((int32_t) srcBLen - 1) - (int32_t) firstIndex;
+ blockSize1 = (blockSize1 > 0) ? ((check > (srcBLen - 1U)) ? blockSize1 : (int32_t) numPoints) : 0;
+ blockSize2 = (int32_t) check - ((blockSize3 + blockSize1) + (int32_t) firstIndex);
+ blockSize2 = (blockSize2 > 0) ? blockSize2 : 0;
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* Set the output pointer to point to the firstIndex
+ * of the output sample to be calculated. */
+ pOut = pDst + firstIndex;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed.
+ Since the partial convolution starts from firstIndex
+ Number of Macs to be performed is firstIndex + 1 */
+ count = 1U + firstIndex;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + firstIndex;
+ py = pSrc2;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] , x[1] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* y[srcBLen - 1] , y[srcBLen - 2] */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* x[0] * y[srcBLen - 1] */
+ /* x[1] * y[srcBLen - 2] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* x[2] , x[3] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* y[srcBLen - 3] , y[srcBLen - 4] */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* x[2] * y[srcBLen - 3] */
+ /* x[3] * y[srcBLen - 4] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q31_t) * px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7, 8));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = ++pSrc2;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ if ((int32_t)firstIndex - (int32_t)srcBLen + 1 > 0)
+ {
+ pSrc1 = pIn1 + firstIndex - srcBLen + 1;
+ }
+ else
+ {
+ pSrc1 = pIn1;
+ }
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = ((uint32_t) blockSize2 >> 2U);
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *py--;
+ /* Read y[srcBLen - 2] sample */
+ c1 = *py--;
+
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* x[0] and x[1] are packed */
+ in1 = (q15_t) x0;
+ in2 = (q15_t) x1;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* y[srcBLen - 1] and y[srcBLen - 2] are packed */
+ in1 = (q15_t) c0;
+ in2 = (q15_t) c1;
+
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
+ acc0 = __SMLAD(input1, input2, acc0);
+
+ /* x[1] and x[2] are packed */
+ in1 = (q15_t) x1;
+ in2 = (q15_t) x2;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
+ acc1 = __SMLAD(input1, input2, acc1);
+
+ /* x[2] and x[3] are packed */
+ in1 = (q15_t) x2;
+ in2 = (q15_t) x3;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
+ acc2 = __SMLAD(input1, input2, acc2);
+
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* x[3] and x[4] are packed */
+ in1 = (q15_t) x3;
+ in2 = (q15_t) x0;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
+ acc3 = __SMLAD(input1, input2, acc3);
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *py--;
+ /* Read y[srcBLen - 4] sample */
+ c1 = *py--;
+
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* x[2] and x[3] are packed */
+ in1 = (q15_t) x2;
+ in2 = (q15_t) x3;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* y[srcBLen - 3] and y[srcBLen - 4] are packed */
+ in1 = (q15_t) c0;
+ in2 = (q15_t) c1;
+
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
+ acc0 = __SMLAD(input1, input2, acc0);
+
+ /* x[3] and x[4] are packed */
+ in1 = (q15_t) x3;
+ in2 = (q15_t) x0;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
+ acc1 = __SMLAD(input1, input2, acc1);
+
+ /* x[4] and x[5] are packed */
+ in1 = (q15_t) x0;
+ in2 = (q15_t) x1;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
+ acc2 = __SMLAD(input1, input2, acc2);
+
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* x[5] and x[6] are packed */
+ in1 = (q15_t) x1;
+ in2 = (q15_t) x2;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
+ acc3 = __SMLAD(input1, input2, acc3);
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 += ((q31_t) x0 * c0);
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 += ((q31_t) x1 * c0);
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 += ((q31_t) x2 * c0);
+ /* acc3 += x[7] * y[srcBLen - 5] */
+ acc3 += ((q31_t) x3 * c0);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(acc0 >> 7, 8));
+ *pOut++ = (q7_t) (__SSAT(acc1 >> 7, 8));
+ *pOut++ = (q7_t) (__SSAT(acc2 >> 7, 8));
+ *pOut++ = (q7_t) (__SSAT(acc3 >> 7, 8));
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (uint32_t) blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+ /* Reading two inputs of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* Reading two inputs of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* Perform the multiply-accumulate */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Reading two inputs of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* Reading two inputs of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* Perform the multiply-accumulate */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q31_t) * px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7, 8));
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = (uint32_t) blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q31_t) * px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7, 8));
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pSrc1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* Reading two inputs, x[srcALen - srcBLen + 1] and x[srcALen - srcBLen + 2] of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* Reading two inputs, y[srcBLen - 1] and y[srcBLen - 2] of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Reading two inputs, x[srcALen - srcBLen + 3] and x[srcALen - srcBLen + 4] of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* Reading two inputs, y[srcBLen - 3] and y[srcBLen - 4] of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum += ((q31_t) * px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7, 8));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const q7_t *pIn1 = pSrcA; /* InputA pointer */
+ const q7_t *pIn2 = pSrcB; /* InputB pointer */
+ q31_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+ arm_status status; /* Status of Partial convolution */
+
+ /* Check for range of output samples to be calculated */
+ if ((firstIndex + numPoints) > ((srcALen + (srcBLen - 1U))))
+ {
+ /* Set status as ARM_MATH_ARGUMENT_ERROR */
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* Loop to calculate convolution for output length number of values */
+ for (i = firstIndex; i <= (firstIndex + numPoints - 1); i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q15_t) pIn1[j] * (pIn2[i - j]));
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = (q7_t) __SSAT((sum >> 7U), 8U);
+ }
+
+ /* Set status as ARM_MATH_SUCCESS */
+ status = ARM_MATH_SUCCESS;
+ }
+
+ /* Return to application */
+ return (status);
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of PartialConv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q15.c new file mode 100644 index 0000000..8e37ac7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q15.c @@ -0,0 +1,696 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_q15.c
+ * Description: Convolution of Q15 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q15 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both inputs are in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ This approach provides 33 guard bits and there is no risk of overflow.
+ The 34.30 result is then truncated to 34.15 format by discarding the low 15 bits and then saturated to 1.15 format.
+
+ @remark
+ Refer to \ref arm_conv_fast_q15() for a faster but less precise version of this function.
+ @remark
+ Refer to \ref arm_conv_opt_q15() for a faster implementation of this function using scratch buffers.
+ */
+
+void arm_conv_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst)
+{
+
+#if defined (ARM_MATH_DSP)
+
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q63_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
+ const q15_t *px; /* Intermediate inputA pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ const q15_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t x0, x1, x2, x3, c0; /* Temporary input variables to hold state and coefficient values */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations less than 4 */
+ /* Second part of this stage computes the MAC operations greater than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ while ((count < 4U) && (blockSize1 > 0U))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Loop over number of MAC operations between
+ * inputA samples and inputB samples */
+ k = count;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* The second part of the stage starts here */
+ /* The internal loop, over count, is unrolled by 4 */
+ /* To, read the last two inputB samples using SIMD:
+ * y[srcBLen] and y[srcBLen-1] coefficients, py is decremented by 1 */
+ py = py - 1;
+
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0], x[1] are multiplied with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[2], x[3] are multiplied with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ * So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + (count - 1U);
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ py = py - 1U;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1] samples */
+ x0 = read_q15x2 ((q15_t *) px);
+
+ /* read x[1], x[2] samples */
+ x1 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read the last two inputB samples using SIMD:
+ * y[srcBLen - 1] and y[srcBLen - 2] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
+ acc0 = __SMLALDX(x0, c0, acc0);
+
+ /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
+ acc1 = __SMLALDX(x1, c0, acc1);
+
+ /* Read x[2], x[3] */
+ x2 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[3], x[4] */
+ x3 = read_q15x2 ((q15_t *) px + 1);
+
+ /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
+ acc2 = __SMLALDX(x2, c0, acc2);
+
+ /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
+ acc3 = __SMLALDX(x3, c0, acc3);
+
+ /* Read y[srcBLen - 3] and y[srcBLen - 4] */
+ c0 = read_q15x2_da ((q15_t **) &py);
+
+ /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
+ acc0 = __SMLALDX(x2, c0, acc0);
+
+ /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
+ acc1 = __SMLALDX(x3, c0, acc1);
+
+ /* Read x[4], x[5] */
+ x0 = read_q15x2 ((q15_t *) px + 2);
+
+ /* Read x[5], x[6] */
+ x1 = read_q15x2 ((q15_t *) px + 3);
+
+ px += 4U;
+
+ /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
+ acc2 = __SMLALDX(x0, c0, acc2);
+
+ /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
+ acc3 = __SMLALDX(x1, c0, acc3);
+
+ } while (--k);
+
+ /* For the next MAC operations, SIMD is not used
+ * So, the 16 bit pointer if inputB, py is updated */
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ if (k == 1U)
+ {
+ /* Read y[srcBLen - 5] */
+ c0 = *(py + 1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[7] */
+ x3 = read_q15x2 ((q15_t *) px);
+ px++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc1 = __SMLALD(x1, c0, acc1);
+ acc2 = __SMLALDX(x1, c0, acc2);
+ acc3 = __SMLALDX(x3, c0, acc3);
+ }
+
+ if (k == 2U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALDX(x0, c0, acc0);
+ acc1 = __SMLALDX(x1, c0, acc1);
+ acc2 = __SMLALDX(x3, c0, acc2);
+ acc3 = __SMLALDX(x2, c0, acc3);
+ }
+
+ if (k == 3U)
+ {
+ /* Read y[srcBLen - 5], y[srcBLen - 6] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALDX(x0, c0, acc0);
+ acc1 = __SMLALDX(x1, c0, acc1);
+ acc2 = __SMLALDX(x3, c0, acc2);
+ acc3 = __SMLALDX(x2, c0, acc3);
+
+ c0 = *(py-1);
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[10] */
+ x3 = read_q15x2 ((q15_t *) px + 2);
+ px += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLALDX(x1, c0, acc0);
+ acc1 = __SMLALD(x2, c0, acc1);
+ acc2 = __SMLALDX(x2, c0, acc2);
+ acc3 = __SMLALDX(x3, c0, acc3);
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pOut, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT(sum >> 15, 16));
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) ((q31_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT(sum >> 15, 16));
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+ blockSize3 = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ pIn2 = pSrc2 - 1U;
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ /* For loop unrolling by 4, this stage is divided into two. */
+ /* First part of this stage computes the MAC operations greater than 4 */
+ /* Second part of this stage computes the MAC operations less than or equal to 4 */
+
+ /* The first part of the stage starts here */
+ j = blockSize3 >> 2U;
+
+ while ((j > 0U) && (blockSize3 > 0U))
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = blockSize3 >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[srcALen - srcBLen + 1], x[srcALen - srcBLen + 2] are multiplied
+ * with y[srcBLen - 1], y[srcBLen - 2] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+ /* x[srcALen - srcBLen + 3], x[srcALen - srcBLen + 4] are multiplied
+ * with y[srcBLen - 3], y[srcBLen - 4] respectively */
+ sum = __SMLALDX(read_q15x2_ia ((q15_t **) &px), read_q15x2_da ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* For the next MAC operations, the pointer py is used without SIMD
+ * So, py is incremented by 1 */
+ py = py + 1U;
+
+ /* If the blockSize3 is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = blockSize3 % 0x4U;
+
+ while (k > 0U)
+ {
+ /* sum += x[srcALen - srcBLen + 5] * y[srcBLen - 5] */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blockSize3--;
+
+ j--;
+ }
+
+ /* The second part of the stage starts here */
+ /* SIMD is not used for the next MAC operations,
+ * so pointer py is updated to read only one sample at a time */
+ py = py + 1U;
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = blockSize3;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum = __SMLALD(*px++, *py--, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q15_t) (__SSAT((sum >> 15), 16));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+ const q15_t *pIn1 = pSrcA; /* InputA pointer */
+ const q15_t *pIn2 = pSrcB; /* InputB pointer */
+ q63_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+
+ /* Loop to calculate convolution for output length number of values */
+ for (i = 0; i < (srcALen + srcBLen - 1); i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q31_t) pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = (q15_t) __SSAT((sum >> 15U), 16U);
+ }
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q31.c new file mode 100644 index 0000000..590819b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q31.c @@ -0,0 +1,581 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_q31.c
+ * Description: Convolution of Q31 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q31 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ There is no saturation on intermediate additions.
+ Thus, if the accumulator overflows it wraps around and distorts the result.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down the inputs by log2(min(srcALen, srcBLen)) (log2 is read as log to the base 2) times to avoid overflows,
+ as maximum of min(srcALen, srcBLen) number of additions are carried internally.
+ The 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_conv_fast_q31() for a faster but less precise implementation of this function.
+ */
+
+void arm_conv_q31(
+ const q31_t * pSrcA,
+ uint32_t srcALen,
+ const q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const q31_t *pIn1; /* InputA pointer */
+ const q31_t *pIn2; /* InputB pointer */
+ q31_t *pOut = pDst; /* Output pointer */
+ const q31_t *px; /* Intermediate inputA pointer */
+ const q31_t *py; /* Intermediate inputB pointer */
+ const q31_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q63_t sum; /* Accumulators */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc0, acc1, acc2; /* Accumulators */
+ q31_t x0, x1, x2, c0; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 1] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* x[1] * y[srcBLen - 2] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* x[2] * y[srcBLen - 3] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* x[3] * y[srcBLen - 4] */
+ sum += (q63_t) *px++ * (*py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unroll by 3 */
+ blkCnt = blockSize2 / 3;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+
+ /* Apply loop unrolling and compute 3 MACs simultaneously. */
+ k = srcBLen / 3;
+
+ /* First part of the processing with loop unrolling. Compute 3 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 2 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *(py);
+ /* Read x[3] sample */
+ x2 = *(px);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[srcBLen - 1] */
+ acc0 += ((q63_t) x0 * c0);
+ /* acc1 += x[1] * y[srcBLen - 1] */
+ acc1 += ((q63_t) x1 * c0);
+ /* acc2 += x[2] * y[srcBLen - 1] */
+ acc2 += ((q63_t) x2 * c0);
+
+ /* Read y[srcBLen - 2] sample */
+ c0 = *(py - 1U);
+ /* Read x[4] sample */
+ x0 = *(px + 1U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[srcBLen - 2] */
+ acc0 += ((q63_t) x1 * c0);
+ /* acc1 += x[2] * y[srcBLen - 2] */
+ acc1 += ((q63_t) x2 * c0);
+ /* acc2 += x[3] * y[srcBLen - 2] */
+ acc2 += ((q63_t) x0 * c0);
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *(py - 2U);
+ /* Read x[5] sample */
+ x1 = *(px + 2U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[2] * y[srcBLen - 3] */
+ acc0 += ((q63_t) x2 * c0);
+ /* acc1 += x[3] * y[srcBLen - 2] */
+ acc1 += ((q63_t) x0 * c0);
+ /* acc2 += x[4] * y[srcBLen - 2] */
+ acc2 += ((q63_t) x1 * c0);
+
+ /* update scratch pointers */
+ px += 3U;
+ py -= 3U;
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 3, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen - (3 * (srcBLen / 3));
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x2 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 += ((q63_t) x0 * c0);
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 += ((q63_t) x1 * c0);
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 += ((q63_t) x2 * c0);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (acc0 >> 31);
+ *pOut++ = (q31_t) (acc1 >> 31);
+ *pOut++ = (q31_t) (acc2 >> 31);
+
+ /* Increment the pointer pIn1 index, count by 3 */
+ count += 3U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize2 - 3 * (blockSize2 / 3);
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += (q63_t) *px++ * *py--;
+ sum += (q63_t) *px++ * *py--;
+ sum += (q63_t) *px++ * *py--;
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = blockSize3 >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = blockSize3 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = blockSize3;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum += (q63_t) *px++ * *py--;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q31_t) (sum >> 31);
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const q31_t *pIn1 = pSrcA; /* InputA pointer */
+ const q31_t *pIn2 = pSrcB; /* InputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i, j; /* Loop counters */
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i < (srcALen + srcBLen - 1U); i++)
+ {
+ /* Initialize sum with zero to carry out MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q63_t) pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = (q31_t) (sum >> 31U);
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q7.c new file mode 100644 index 0000000..f7491cf --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_conv_q7.c @@ -0,0 +1,700 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_conv_q7.c
+ * Description: Convolution of Q7 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Conv
+ @{
+ */
+
+/**
+ @brief Convolution of Q7 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 32-bit internal accumulator.
+ Both the inputs are represented in 1.7 format and multiplications yield a 2.14 result.
+ The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
+ This approach provides 17 guard bits and there is no risk of overflow as long as <code>max(srcALen, srcBLen)<131072</code>.
+ The 18.14 result is then truncated to 18.7 format by discarding the low 7 bits and then saturated to 1.7 format.
+ @remark
+ Refer to \ref arm_conv_opt_q7() for a faster implementation of this function.
+ */
+
+void arm_conv_q7(
+ const q7_t * pSrcA,
+ uint32_t srcALen,
+ const q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const q7_t *pIn1; /* InputA pointer */
+ const q7_t *pIn2; /* InputB pointer */
+ q7_t *pOut = pDst; /* Output pointer */
+ const q7_t *px; /* Intermediate inputA pointer */
+ const q7_t *py; /* Intermediate inputB pointer */
+ const q7_t *pSrc1, *pSrc2; /* Intermediate pointers */
+ q31_t sum; /* Accumulators */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc0, acc1, acc2, acc3; /* Accumulators */
+ q31_t input1, input2; /* Temporary input variables */
+ q15_t in1, in2; /* Temporary input variables */
+ q7_t x0, x1, x2, x3, c0, c1; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+ }
+
+ /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[0]
+ * sum = x[0] * y[1] + x[1] * y[0]
+ * ....
+ * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] , x[1] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[srcBLen - 1] , y[srcBLen - 2] */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* x[0] * y[srcBLen - 1] */
+ /* x[1] * y[srcBLen - 2] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* x[2] , x[3] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[srcBLen - 3] , y[srcBLen - 4] */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* x[2] * y[srcBLen - 3] */
+ /* x[3] * y[srcBLen - 4] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q15_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7U, 8));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pIn2 + count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
+ * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[srcBLen - 1] sample */
+ c0 = *py--;
+ /* Read y[srcBLen - 2] sample */
+ c1 = *py--;
+
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* x[0] and x[1] are packed */
+ in1 = (q15_t) x0;
+ in2 = (q15_t) x1;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[srcBLen - 1] and y[srcBLen - 2] are packed */
+ in1 = (q15_t) c0;
+ in2 = (q15_t) c1;
+
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc0 += x[0] * y[srcBLen - 1] + x[1] * y[srcBLen - 2] */
+ acc0 = __SMLAD(input1, input2, acc0);
+
+ /* x[1] and x[2] are packed */
+ in1 = (q15_t) x1;
+ in2 = (q15_t) x2;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc1 += x[1] * y[srcBLen - 1] + x[2] * y[srcBLen - 2] */
+ acc1 = __SMLAD(input1, input2, acc1);
+
+ /* x[2] and x[3] are packed */
+ in1 = (q15_t) x2;
+ in2 = (q15_t) x3;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc2 += x[2] * y[srcBLen - 1] + x[3] * y[srcBLen - 2] */
+ acc2 = __SMLAD(input1, input2, acc2);
+
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* x[3] and x[4] are packed */
+ in1 = (q15_t) x3;
+ in2 = (q15_t) x0;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc3 += x[3] * y[srcBLen - 1] + x[4] * y[srcBLen - 2] */
+ acc3 = __SMLAD(input1, input2, acc3);
+
+ /* Read y[srcBLen - 3] sample */
+ c0 = *py--;
+ /* Read y[srcBLen - 4] sample */
+ c1 = *py--;
+
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* x[2] and x[3] are packed */
+ in1 = (q15_t) x2;
+ in2 = (q15_t) x3;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[srcBLen - 3] and y[srcBLen - 4] are packed */
+ in1 = (q15_t) c0;
+ in2 = (q15_t) c1;
+
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc0 += x[2] * y[srcBLen - 3] + x[3] * y[srcBLen - 4] */
+ acc0 = __SMLAD(input1, input2, acc0);
+
+ /* x[3] and x[4] are packed */
+ in1 = (q15_t) x3;
+ in2 = (q15_t) x0;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc1 += x[3] * y[srcBLen - 3] + x[4] * y[srcBLen - 4] */
+ acc1 = __SMLAD(input1, input2, acc1);
+
+ /* x[4] and x[5] are packed */
+ in1 = (q15_t) x0;
+ in2 = (q15_t) x1;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc2 += x[4] * y[srcBLen - 3] + x[5] * y[srcBLen - 4] */
+ acc2 = __SMLAD(input1, input2, acc2);
+
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* x[5] and x[6] are packed */
+ in1 = (q15_t) x1;
+ in2 = (q15_t) x2;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc3 += x[5] * y[srcBLen - 3] + x[6] * y[srcBLen - 4] */
+ acc3 = __SMLAD(input1, input2, acc3);
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *py--;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[srcBLen - 5] */
+ acc0 += ((q15_t) x0 * c0);
+ /* acc1 += x[5] * y[srcBLen - 5] */
+ acc1 += ((q15_t) x1 * c0);
+ /* acc2 += x[6] * y[srcBLen - 5] */
+ acc2 += ((q15_t) x2 * c0);
+ /* acc3 += x[7] * y[srcBLen - 5] */
+ acc3 += ((q15_t) x3 * c0);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(acc0 >> 7U, 8));
+ *pOut++ = (q7_t) (__SSAT(acc1 >> 7U, 8));
+ *pOut++ = (q7_t) (__SSAT(acc2 >> 7U, 8));
+ *pOut++ = (q7_t) (__SSAT(acc3 >> 7U, 8));
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+
+ /* Reading two inputs of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Reading two inputs of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Perform the multiply-accumulate */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Reading two inputs of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Reading two inputs of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Perform the multiply-accumulate */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q15_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7U, 8));
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q15_t) *px++ * *py--);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7U, 8));
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
+ * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
+ * ....
+ * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
+ * sum += x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The blockSize3 variable holds the number of MAC operations performed */
+
+ /* Working pointer of inputA */
+ pSrc1 = pIn1 + (srcALen - (srcBLen - 1U));
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ pSrc2 = pIn2 + (srcBLen - 1U);
+ py = pSrc2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = blockSize3 >> 2U;
+
+ while (k > 0U)
+ {
+ /* Reading two inputs, x[srcALen - srcBLen + 1] and x[srcALen - srcBLen + 2] of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Reading two inputs, y[srcBLen - 1] and y[srcBLen - 2] of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
+ /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Reading two inputs, x[srcALen - srcBLen + 3] and x[srcALen - srcBLen + 4] of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Reading two inputs, y[srcBLen - 3] and y[srcBLen - 4] of SrcB buffer and packing */
+ in1 = (q15_t) *py--;
+ in2 = (q15_t) *py--;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
+ /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = blockSize3 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = blockSize3;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen-1] * y[srcBLen-1] */
+ sum += ((q15_t) *px++ * *py--);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut++ = (q7_t) (__SSAT(sum >> 7U, 8));
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pSrc2;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const q7_t *pIn1 = pSrcA; /* InputA pointer */
+ const q7_t *pIn2 = pSrcB; /* InputB pointer */
+ q31_t sum; /* Accumulator */
+ uint32_t i, j; /* Loop counters */
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i < (srcALen + srcBLen - 1U); i++)
+ {
+ /* Initialize sum with zero to carry out MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q15_t) pIn1[j] * pIn2[i - j]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ pDst[i] = (q7_t) __SSAT((sum >> 7U), 8U);
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of Conv group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_f32.c new file mode 100644 index 0000000..9aa648b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_f32.c @@ -0,0 +1,893 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_f32.c
+ * Description: Correlation of floating-point sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup Corr Correlation
+
+ Correlation is a mathematical operation that is similar to convolution.
+ As with convolution, correlation uses two signals to produce a third signal.
+ The underlying algorithms in correlation and convolution are identical except that one of the inputs is flipped in convolution.
+ Correlation is commonly used to measure the similarity between two signals.
+ It has applications in pattern recognition, cryptanalysis, and searching.
+ The CMSIS library provides correlation functions for Q7, Q15, Q31 and floating-point data types.
+ Fast versions of the Q15 and Q31 functions are also provided.
+
+ @par Algorithm
+ Let <code>a[n]</code> and <code>b[n]</code> be sequences of length <code>srcALen</code> and <code>srcBLen</code> samples respectively.
+ The convolution of the two signals is denoted by
+ <pre>
+ c[n] = a[n] * b[n]
+ </pre>
+ In correlation, one of the signals is flipped in time
+ <pre>
+ c[n] = a[n] * b[-n]
+ </pre>
+ @par
+ and this is mathematically defined as
+ \image html CorrelateEquation.gif
+ @par
+ The <code>pSrcA</code> points to the first input vector of length <code>srcALen</code> and <code>pSrcB</code> points to the second input vector of length <code>srcBLen</code>.
+ The result <code>c[n]</code> is of length <code>2 * max(srcALen, srcBLen) - 1</code> and is defined over the interval <code>n=0, 1, 2, ..., (2 * max(srcALen, srcBLen) - 2)</code>.
+ The output result is written to <code>pDst</code> and the calling function must allocate <code>2 * max(srcALen, srcBLen) - 1</code> words for the result.
+
+ @note
+ The <code>pDst</code> should be initialized to all zeros before being used.
+
+ @par Fixed-Point Behavior
+ Correlation requires summing up a large number of intermediate products.
+ As such, the Q7, Q15, and Q31 functions run a risk of overflow and saturation.
+ Refer to the function specific documentation below for further details of the particular algorithm used.
+
+ @par Fast Versions
+ Fast versions are supported for Q31 and Q15. Cycles for Fast versions are less compared to Q31 and Q15 of correlate and the design requires
+ the input signals should be scaled down to avoid intermediate overflows.
+
+ @par Opt Versions
+ Opt versions are supported for Q15 and Q7. Design uses internal scratch buffer for getting good optimisation.
+ These versions are optimised in cycles and consumes more memory (Scratch memory) compared to Q15 and Q7 versions of correlate
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of floating-point sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @return none
+ */
+
+void arm_correlate_f32(
+ const float32_t * pSrcA,
+ uint32_t srcALen,
+ const float32_t * pSrcB,
+ uint32_t srcBLen,
+ float32_t * pDst)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const float32_t *pIn1; /* InputA pointer */
+ const float32_t *pIn2; /* InputB pointer */
+ float32_t *pOut = pDst; /* Output pointer */
+ const float32_t *px; /* Intermediate inputA pointer */
+ const float32_t *py; /* Intermediate inputB pointer */
+ const float32_t *pSrc1;
+ float32_t sum;
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+ uint32_t outBlockSize; /* Loop counter */
+ int32_t inc = 1; /* Destination address modifier */
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined (ARM_MATH_NEON)
+ float32_t acc0, acc1, acc2, acc3; /* Accumulators */
+ float32_t x0, x1, x2, x3, c0; /* temporary variables for holding input and coefficient values */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we assume zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (2U * srcALen) - 1U;
+
+ /* When srcALen > srcBLen, zero padding has to be done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[srcBlen - 1]
+ * sum = x[0] * y[srcBlen-2] + x[1] * y[srcBlen - 1]
+ * ....
+ * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + (srcBLen - 1U);
+ py = pSrc1;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ float32x4_t x,y;
+ float32x4_t res = vdupq_n_f32(0) ;
+ float32x2_t accum = vdup_n_f32(0);
+
+ while (k > 0U)
+ {
+ x = vld1q_f32(px);
+ y = vld1q_f32(py);
+
+ res = vmlaq_f32(res,x, y);
+
+ px += 4;
+ py += 4;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ accum = vpadd_f32(vget_low_f32(res), vget_high_f32(res));
+ sum += accum[0] + accum[1];
+
+ k = count & 0x3;
+#else
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 4] */
+ sum += *px++ * *py++;
+
+ /* x[1] * y[srcBLen - 3] */
+ sum += *px++ * *py++;
+
+ /* x[2] * y[srcBLen - 2] */
+ sum += *px++ * *py++;
+
+ /* x[3] * y[srcBLen - 1] */
+ sum += *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#endif /* #if defined(ARM_MATH_NEON) */
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0] * y[srcBLen - 1] */
+ sum += *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pSrc1 - count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
+ * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize2 >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ float32x4_t c;
+ float32x4_t x1v;
+ float32x4_t x2v;
+ uint32x4_t x1v_u;
+ uint32x4_t x2v_u;
+ float32x4_t x;
+ uint32x4_t x_u;
+ float32x4_t res = vdupq_n_f32(0) ;
+#endif /* #if defined(ARM_MATH_NEON) */
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0.0f;
+ acc1 = 0.0f;
+ acc2 = 0.0f;
+ acc3 = 0.0f;
+
+#if defined(ARM_MATH_NEON)
+ /* Compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ res = vdupq_n_f32(0) ;
+
+ x1v = vld1q_f32(px);
+ px += 4;
+ do
+ {
+ x2v = vld1q_f32(px);
+ c = vld1q_f32(py);
+
+ py += 4;
+
+ x = x1v;
+ res = vmlaq_n_f32(res,x,c[0]);
+
+ x = vextq_f32(x1v,x2v,1);
+
+ res = vmlaq_n_f32(res,x,c[1]);
+
+ x = vextq_f32(x1v,x2v,2);
+
+ res = vmlaq_n_f32(res,x,c[2]);
+
+ x = vextq_f32(x1v,x2v,3);
+
+ res = vmlaq_n_f32(res,x,c[3]);
+
+ x1v = x2v;
+ px+=4;
+ x2v = vld1q_f32(px);
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen & 0x3;
+
+ while (k > 0U)
+ {
+ /* Read y[srcBLen - 5] sample */
+ c0 = *(py++);
+
+ res = vmlaq_n_f32(res,x1v,c0);
+
+ /* Reuse the present samples for the next MAC */
+ x1v[0] = x1v[1];
+ x1v[1] = x1v[2];
+ x1v[2] = x1v[3];
+
+ x1v[3] = *(px++);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ px-=1;
+
+ acc0 = res[0];
+ acc1 = res[1];
+ acc2 = res[2];
+ acc3 = res[3];
+#else
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[0] sample */
+ c0 = *(py++);
+ /* Read x[3] sample */
+ x3 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[0] */
+ acc0 += x0 * c0;
+ /* acc1 += x[1] * y[0] */
+ acc1 += x1 * c0;
+ /* acc2 += x[2] * y[0] */
+ acc2 += x2 * c0;
+ /* acc3 += x[3] * y[0] */
+ acc3 += x3 * c0;
+
+ /* Read y[1] sample */
+ c0 = *(py++);
+ /* Read x[4] sample */
+ x0 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[1] */
+ acc0 += x1 * c0;
+ /* acc1 += x[2] * y[1] */
+ acc1 += x2 * c0;
+ /* acc2 += x[3] * y[1] */
+ acc2 += x3 * c0;
+ /* acc3 += x[4] * y[1] */
+ acc3 += x0 * c0;
+
+ /* Read y[2] sample */
+ c0 = *(py++);
+ /* Read x[5] sample */
+ x1 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[2] * y[2] */
+ acc0 += x2 * c0;
+ /* acc1 += x[3] * y[2] */
+ acc1 += x3 * c0;
+ /* acc2 += x[4] * y[2] */
+ acc2 += x0 * c0;
+ /* acc3 += x[5] * y[2] */
+ acc3 += x1 * c0;
+
+ /* Read y[3] sample */
+ c0 = *(py++);
+ /* Read x[6] sample */
+ x2 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[3] * y[3] */
+ acc0 += x3 * c0;
+ /* acc1 += x[4] * y[3] */
+ acc1 += x0 * c0;
+ /* acc2 += x[5] * y[3] */
+ acc2 += x1 * c0;
+ /* acc3 += x[6] * y[3] */
+ acc3 += x2 * c0;
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[4] sample */
+ c0 = *(py++);
+ /* Read x[7] sample */
+ x3 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[4] * y[4] */
+ acc0 += x0 * c0;
+ /* acc1 += x[5] * y[4] */
+ acc1 += x1 * c0;
+ /* acc2 += x[6] * y[4] */
+ acc2 += x2 * c0;
+ /* acc3 += x[7] * y[4] */
+ acc3 += x3 * c0;
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+#endif /* #if defined(ARM_MATH_NEON) */
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = acc0;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ *pOut = acc1;
+ pOut += inc;
+
+ *pOut = acc2;
+ pOut += inc;
+
+ *pOut = acc3;
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ float32x4_t x,y;
+ float32x4_t res = vdupq_n_f32(0) ;
+ float32x2_t accum = vdup_n_f32(0);
+
+ while (k > 0U)
+ {
+ x = vld1q_f32(px);
+ y = vld1q_f32(py);
+
+ res = vmlaq_f32(res,x, y);
+
+ px += 4;
+ py += 4;
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ accum = vpadd_f32(vget_low_f32(res), vget_high_f32(res));
+ sum += accum[0] + accum[1];
+#else
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py++;
+ sum += *px++ * *py++;
+ sum += *px++ * *py++;
+ sum += *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+#endif /* #if defined(ARM_MATH_NEON) */
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py++;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum;
+
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+ /* Loop over srcBLen */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py++;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * ....
+ * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
+ * sum += x[srcALen-1] * y[0]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = pIn1 + (srcALen - (srcBLen - 1U));
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+#if defined(ARM_MATH_NEON)
+ float32x4_t x,y;
+ float32x4_t res = vdupq_n_f32(0) ;
+ float32x2_t accum = vdup_n_f32(0);
+
+ while (k > 0U)
+ {
+ x = vld1q_f32(px);
+ y = vld1q_f32(py);
+
+ res = vmlaq_f32(res,x, y);
+
+ px += 4;
+ py += 4;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ accum = vpadd_f32(vget_low_f32(res), vget_high_f32(res));
+ sum += accum[0] + accum[1];
+#else
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 4] * y[3] */
+ sum += *px++ * *py++;
+
+ /* sum += x[srcALen - srcBLen + 3] * y[2] */
+ sum += *px++ * *py++;
+
+ /* sum += x[srcALen - srcBLen + 2] * y[1] */
+ sum += *px++ * *py++;
+
+ /* sum += x[srcALen - srcBLen + 1] * y[0] */
+ sum += *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+#endif /* #if defined (ARM_MATH_NEON) */
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) || defined(ARM_MATH_NEON) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const float32_t *pIn1 = pSrcA; /* inputA pointer */
+ const float32_t *pIn2 = pSrcB + (srcBLen - 1U); /* inputB pointer */
+ float32_t sum; /* Accumulator */
+ uint32_t i = 0U, j; /* Loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and a varaible, inv is set to 1 */
+ /* If lengths are not equal then zero pad has to be done to make the two
+ * inputs of same length. But to improve the performance, we assume zeroes
+ * in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * starting of the output buffer */
+ /* If srcALen < srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * ending of the output buffer */
+ /* Once the zero padding is done the remaining of the output is calcualted
+ * using convolution but with the shorter signal time shifted. */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry out MAC operations */
+ sum = 0.0f;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if ((((i - j) < srcBLen) && (j < srcALen)))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += pIn1[j] * pIn2[-((int32_t) i - j)];
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = sum;
+ else
+ *pDst++ = sum;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c new file mode 100644 index 0000000..31fc680 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_opt_q15.c @@ -0,0 +1,345 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_fast_opt_q15.c
+ * Description: Fast Q15 Correlation
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q15 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence.
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @param[in] pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ There is no saturation on intermediate additions.
+ Thus, if the accumulator overflows it wraps around and distorts the result.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down one of the inputs by 1/min(srcALen, srcBLen) to avoid overflow since a
+ maximum of min(srcALen, srcBLen) number of additions is carried internally.
+ The 2.30 accumulator is right shifted by 15 bits and then saturated to 1.15 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_correlate_q15() for a slower implementation of this function which uses a 64-bit accumulator to avoid wrap around distortion.
+ */
+
+void arm_correlate_fast_opt_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch)
+{
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q31_t acc0; /* Accumulators */
+ q15_t *pOut = pDst; /* Output pointer */
+ q15_t *pScr1 = pScratch; /* Temporary pointer for scratch */
+ const q15_t *py; /* Intermediate inputB pointer */
+ uint32_t j, blkCnt, outBlockSize; /* Loop counter */
+ int32_t inc = 1; /* Destination address modifier */
+ uint32_t tapCnt; /* Loop count */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x1, x2, x3; /* Temporary variables for holding input and coefficient values */
+ q31_t y1, y2; /* State variables */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (2U * srcALen) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+ pScr1 = pScratch;
+
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+
+ /* Copy (srcALen) samples in scratch buffer */
+ arm_copy_q15(pIn1, pScr1, srcALen);
+
+ /* Update pointers */
+ pScr1 += srcALen;
+
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Temporary pointer for scratch2 */
+ py = pIn2;
+
+
+ /* Actual correlation process starts here */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = (srcALen + srcBLen - 1U) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+ y2 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+ acc2 = __SMLAD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ /* Read next two samples from scratch buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x2, y2, acc0);
+ acc2 = __SMLAD(x1, y2, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+ acc1 = __SMLADX(x3, y2, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y2, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2);
+ acc1 += (*pScr1++ * *pIn2);
+ acc2 += (*pScr1++ * *pIn2);
+ acc3 += (*pScr1++ * *pIn2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the results in the accumulators in the destination buffer. */
+ *pOut = (__SSAT(acc0 >> 15U, 16));
+ pOut += inc;
+ *pOut = (__SSAT(acc1 >> 15U, 16));
+ pOut += inc;
+ *pOut = (__SSAT(acc2 >> 15U, 16));
+ pOut += inc;
+ *pOut = (__SSAT(acc3 >> 15U, 16));
+ pOut += inc;
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch += 4U;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (srcALen + srcBLen - 1U) & 0x3;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = (srcALen + srcBLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Calculate correlation for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read next two samples from scratch buffer */
+ acc0 += (*pScr1++ * *pIn2++);
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ ** Then store the output in the destination buffer. */
+ *pOut = (q15_t) (__SSAT((acc0 >> 15), 16));
+ pOut += inc;
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch += 1U;
+ }
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q15.c new file mode 100644 index 0000000..0c0f12a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q15.c @@ -0,0 +1,614 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_fast_q15.c
+ * Description: Fast Q15 Correlation
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q15 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ There is no saturation on intermediate additions.
+ Thus, if the accumulator overflows it wraps around and distorts the result.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down one of the inputs by 1/min(srcALen, srcBLen) to avoid overflow since a
+ maximum of min(srcALen, srcBLen) number of additions is carried internally.
+ The 2.30 accumulator is right shifted by 15 bits and then saturated to 1.15 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_correlate_q15() for a slower implementation of this function which uses a 64-bit accumulator to avoid wrap around distortion.
+ */
+
+void arm_correlate_fast_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst)
+{
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
+ const q15_t *px; /* Intermediate inputA pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ const q15_t *pSrc1; /* Intermediate pointers */
+ q31_t x0, x1, x2, x3, c0; /* Temporary variables for holding input and coefficient values */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+ uint32_t outBlockSize;
+ int32_t inc = 1; /* Destination address modifier */
+
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (2U * srcALen) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+
+ }
+
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[srcBlen - 1]
+ * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
+ * ....
+ * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + (srcBLen - 1U);
+ py = pSrc1;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first loop starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 4] , x[1] * y[srcBLen - 3] */
+ sum = __SMLAD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+ /* x[3] * y[srcBLen - 1] , x[2] * y[srcBLen - 2] */
+ sum = __SMLAD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* x[0] * y[srcBLen - 1] */
+ sum = __SMLAD(*px++, *py++, sum);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (sum >> 15);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pSrc1 - count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
+ * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* --------------------
+ * Stage2 process
+ * -------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unroll over blockSize2, by 4 */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1] samples */
+ x0 = read_q15x2 ((q15_t *) px);
+ /* read x[1], x[2] samples */
+ x1 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read the first two inputB samples using SIMD:
+ * y[0] and y[1] */
+ c0 = read_q15x2_ia ((q15_t **) &py);
+
+ /* acc0 += x[0] * y[0] + x[1] * y[1] */
+ acc0 = __SMLAD(x0, c0, acc0);
+
+ /* acc1 += x[1] * y[0] + x[2] * y[1] */
+ acc1 = __SMLAD(x1, c0, acc1);
+
+ /* Read x[2], x[3] */
+ x2 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[3], x[4] */
+ x3 = read_q15x2 ((q15_t *) px + 1);
+
+ /* acc2 += x[2] * y[0] + x[3] * y[1] */
+ acc2 = __SMLAD(x2, c0, acc2);
+
+ /* acc3 += x[3] * y[0] + x[4] * y[1] */
+ acc3 = __SMLAD(x3, c0, acc3);
+
+ /* Read y[2] and y[3] */
+ c0 = read_q15x2_ia ((q15_t **) &py);
+
+ /* acc0 += x[2] * y[2] + x[3] * y[3] */
+ acc0 = __SMLAD(x2, c0, acc0);
+
+ /* acc1 += x[3] * y[2] + x[4] * y[3] */
+ acc1 = __SMLAD(x3, c0, acc1);
+
+ /* Read x[4], x[5] */
+ x0 = read_q15x2 ((q15_t *) px + 2);
+
+ /* Read x[5], x[6] */
+ x1 = read_q15x2 ((q15_t *) px + 3);
+ px += 4U;
+
+ /* acc2 += x[4] * y[2] + x[5] * y[3] */
+ acc2 = __SMLAD(x0, c0, acc2);
+
+ /* acc3 += x[5] * y[2] + x[6] * y[3] */
+ acc3 = __SMLAD(x1, c0, acc3);
+
+ } while (--k);
+
+ /* For the next MAC operations, SIMD is not used
+ * So, the 16 bit pointer if inputB, py is updated */
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ if (k == 1U)
+ {
+ /* Read y[4] */
+ c0 = *py;
+
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[7] */
+ x3 = read_q15x2 ((q15_t *) px);
+ px++;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLAD (x0, c0, acc0);
+ acc1 = __SMLAD (x1, c0, acc1);
+ acc2 = __SMLADX(x1, c0, acc2);
+ acc3 = __SMLADX(x3, c0, acc3);
+ }
+
+ if (k == 2U)
+ {
+ /* Read y[4], y[5] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc1 = __SMLAD(x1, c0, acc1);
+ acc2 = __SMLAD(x3, c0, acc2);
+ acc3 = __SMLAD(x2, c0, acc3);
+ }
+
+ if (k == 3U)
+ {
+ /* Read y[4], y[5] */
+ c0 = read_q15x2_ia ((q15_t **) &py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc1 = __SMLAD(x1, c0, acc1);
+ acc2 = __SMLAD(x3, c0, acc2);
+ acc3 = __SMLAD(x2, c0, acc3);
+
+ c0 = (*py);
+ /* Read y[6] */
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[10] */
+ x3 = read_q15x2 ((q15_t *) px + 2);
+ px += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLADX(x1, c0, acc0);
+ acc1 = __SMLAD (x2, c0, acc1);
+ acc2 = __SMLADX(x2, c0, acc2);
+ acc3 = __SMLADX(x3, c0, acc3);
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (acc0 >> 15);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ *pOut = (q15_t) (acc1 >> 15);
+ pOut += inc;
+
+ *pOut = (q15_t) (acc2 >> 15);
+ pOut += inc;
+
+ *pOut = (q15_t) (acc3 >> 15);
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q31_t) *px++ * *py++);
+ sum += ((q31_t) *px++ * *py++);
+ sum += ((q31_t) *px++ * *py++);
+ sum += ((q31_t) *px++ * *py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q31_t) * px++ * *py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (sum >> 15);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q31_t) *px++ * *py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (sum >> 15);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * ....
+ * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
+ * sum += x[srcALen-1] * y[0]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ /* sum += x[srcALen - srcBLen + 4] * y[3] , sum += x[srcALen - srcBLen + 3] * y[2] */
+ sum = __SMLAD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+ /* sum += x[srcALen - srcBLen + 2] * y[1] , sum += x[srcALen - srcBLen + 1] * y[0] */
+ sum = __SMLAD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = __SMLAD(*px++, *py++, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (sum >> 15);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement the MAC count */
+ count--;
+
+ /* Decrement the loop counter */
+ blockSize3--;
+ }
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q31.c new file mode 100644 index 0000000..8686c52 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_fast_q31.c @@ -0,0 +1,601 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_fast_q31.c
+ * Description: Fast Q31 Correlation
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q31 sequences (fast version).
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This function is optimized for speed at the expense of fixed-point precision and overflow protection.
+ The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
+ These intermediate results are accumulated in a 32-bit register in 2.30 format.
+ Finally, the accumulator is saturated and converted to a 1.31 result.
+ @par
+ The fast version has the same overflow behavior as the standard version but provides less precision since it discards the low 32 bits of each multiplication result.
+ In order to avoid overflows completely the input signals must be scaled down.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down one of the inputs by 1/min(srcALen, srcBLen)to avoid overflows since a
+ maximum of min(srcALen, srcBLen) number of additions is carried internally.
+
+ @remark
+ Refer to \ref arm_correlate_q31() for a slower implementation of this function which uses 64-bit accumulation to provide higher precision.
+ */
+
+void arm_correlate_fast_q31(
+ const q31_t * pSrcA,
+ uint32_t srcALen,
+ const q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst)
+{
+ const q31_t *pIn1; /* InputA pointer */
+ const q31_t *pIn2; /* InputB pointer */
+ q31_t *pOut = pDst; /* Output pointer */
+ const q31_t *px; /* Intermediate inputA pointer */
+ const q31_t *py; /* Intermediate inputB pointer */
+ const q31_t *pSrc1; /* Intermediate pointers */
+ q31_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
+ q31_t x0, x1, x2, x3, c0; /* Temporary variables for holding input and coefficient values */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+ uint32_t outBlockSize;
+ int32_t inc = 1; /* Destination address modifier */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (2U * srcALen) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+
+ }
+
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[srcBlen - 1]
+ * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
+ * ....
+ * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + (srcBLen - 1U);
+ py = pSrc1;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 4] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* x[1] * y[srcBLen - 3] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* x[2] * y[srcBLen - 2] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* x[3] * y[srcBLen - 1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0] * y[srcBLen - 1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum << 1;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pSrc1 - count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
+ * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unroll over blockSize2, by 4 */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[0] sample */
+ c0 = *py++;
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[0] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc1 += x[1] * y[0] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc2 += x[2] * y[0] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc3 += x[3] * y[0] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+
+ /* Read y[1] sample */
+ c0 = *py++;
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[1] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc1 += x[2] * y[1] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc2 += x[3] * y[1] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc3 += x[4] * y[1] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+
+ /* Read y[2] sample */
+ c0 = *py++;
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[2] * y[2] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc1 += x[3] * y[2] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc2 += x[4] * y[2] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc3 += x[5] * y[2] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x1 * c0)) >> 32);
+
+
+ /* Read y[3] sample */
+ c0 = *py++;
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[3] * y[3] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x3 * c0)) >> 32);
+ /* acc1 += x[4] * y[3] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc2 += x[5] * y[3] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc3 += x[6] * y[3] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x2 * c0)) >> 32);
+
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[4] sample */
+ c0 = *py++;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[4] */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ /* acc1 += x[5] * y[4] */
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ /* acc2 += x[6] * y[4] */
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ /* acc3 += x[7] * y[4] */
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q31_t) (acc0 << 1);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ *pOut = (q31_t) (acc1 << 1);
+ pOut += inc;
+
+ *pOut = (q31_t) (acc2 << 1);
+ pOut += inc;
+
+ *pOut = (q31_t) (acc3 << 1);
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 4 */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum << 1;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum << 1;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * ....
+ * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
+ * sum += x[srcALen-1] * y[0]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = ((pIn1 + srcALen) - srcBLen) + 1U;
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 4] * y[3] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 3] * y[2] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 2] * y[1] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* sum += x[srcALen - srcBLen + 1] * y[0] */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = (q31_t) ((((q63_t) sum << 32) +
+ ((q63_t) *px++ * (*py++))) >> 32);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = sum << 1;
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q15.c new file mode 100644 index 0000000..ae0ab45 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q15.c @@ -0,0 +1,341 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_opt_q15.c
+ * Description: Correlation of Q15 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q15 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @param[in] pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both inputs are in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ This approach provides 33 guard bits and there is no risk of overflow.
+ The 34.30 result is then truncated to 34.15 format by discarding the low 15 bits and then saturated to 1.15 format.
+
+ @remark
+ Refer to \ref arm_correlate_fast_q15() for a faster but less precise version of this function.
+ */
+
+void arm_correlate_opt_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst,
+ q15_t * pScratch)
+{
+ q63_t acc0; /* Accumulators */
+ q15_t *pOut = pDst; /* Output pointer */
+ q15_t *pScr1; /* Temporary pointer for scratch1 */
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ uint32_t j, blkCnt, outBlockSize; /* Loop counter */
+ int32_t inc = 1; /* Output pointer increment */
+ uint32_t tapCnt;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x1, x2, x3; /* Temporary variables for holding input1 and input2 values */
+ q31_t y1, y2; /* State variables */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (srcALen * 2U) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+ pScr1 = pScratch;
+
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy (srcALen) samples in scratch buffer */
+ arm_copy_q15(pIn1, pScr1, srcALen);
+
+ /* Update pointers */
+ pScr1 += srcALen;
+
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Temporary pointer for scratch2 */
+ py = pIn2;
+
+
+ /* Actual correlation process starts here */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = (srcALen + srcBLen - 1U) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia ((q15_t **) &pIn2);
+ y2 = read_q15x2_ia ((q15_t **) &pIn2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLALD(x1, y1, acc0);
+ acc2 = __SMLALD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLALDX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* multiply and accumlate */
+ acc0 = __SMLALD(x2, y2, acc0);
+ acc2 = __SMLALD(x1, y2, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLALDX(x3, y1, acc3);
+ acc1 = __SMLALDX(x3, y2, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLALDX(x3, y2, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2);
+ acc1 += (*pScr1++ * *pIn2);
+ acc2 += (*pScr1++ * *pIn2);
+ acc3 += (*pScr1++ * *pIn2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+
+ /* Store the results in the accumulators in the destination buffer. */
+ *pOut = (__SSAT(acc0 >> 15U, 16));
+ pOut += inc;
+ *pOut = (__SSAT(acc1 >> 15U, 16));
+ pOut += inc;
+ *pOut = (__SSAT(acc2 >> 15U, 16));
+ pOut += inc;
+ *pOut = (__SSAT(acc3 >> 15U, 16));
+ pOut += inc;
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch += 4U;
+ }
+
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = (srcALen + srcBLen - 1U) & 0x3;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = (srcALen + srcBLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Calculate correlation for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+
+ /* Read next two samples from scratch1 buffer */
+ acc0 += (*pScr1++ * *pIn2++);
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pIn2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ Then store the output in the destination buffer. */
+ *pOut = (q15_t) (__SSAT((acc0 >> 15), 16));
+ pOut += inc;
+
+ /* Initialization of inputB pointer */
+ pIn2 = py;
+
+ pScratch += 1U;
+ }
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q7.c new file mode 100644 index 0000000..10c6e70 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_opt_q7.c @@ -0,0 +1,388 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_opt_q7.c
+ * Description: Correlation of Q7 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q7 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @param[in] pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
+ @param[in] pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 32-bit internal accumulator.
+ Both the inputs are represented in 1.7 format and multiplications yield a 2.14 result.
+ The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
+ This approach provides 17 guard bits and there is no risk of overflow as long as <code>max(srcALen, srcBLen)<131072</code>.
+ The 18.14 result is then truncated to 18.7 format by discarding the low 7 bits and then saturated to 1.7 format.
+ */
+
+void arm_correlate_opt_q7(
+ const q7_t * pSrcA,
+ uint32_t srcALen,
+ const q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst,
+ q15_t * pScratch1,
+ q15_t * pScratch2)
+{
+ q15_t *pScr1 = pScratch1; /* Temporary pointer for scratch */
+ q15_t *pScr2 = pScratch2; /* Temporary pointer for scratch */
+ q15_t x4; /* Temporary input variable */
+ q15_t *py; /* Temporary input2 pointer */
+ q31_t acc0, acc1, acc2, acc3; /* Accumulators */
+ const q7_t *pIn1, *pIn2; /* InputA and inputB pointer */
+ uint32_t j, k, blkCnt, tapCnt; /* Loop counter */
+ int32_t inc = 1; /* Output pointer increment */
+ uint32_t outBlockSize; /* Loop counter */
+ q31_t x1, x2, x3, y1; /* Temporary input variables */
+ q7_t *pOut = pDst; /* Output pointer */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (srcALen * 2U) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+
+ /* Copy (srcBLen) samples in scratch buffer */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling copies 4 data points at a time.
+ a second loop below copies for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ x4 = (q15_t) *pIn2++;
+ *pScr2++ = x4;
+ x4 = (q15_t) *pIn2++;
+ *pScr2++ = x4;
+ x4 = (q15_t) *pIn2++;
+ *pScr2++ = x4;
+ x4 = (q15_t) *pIn2++;
+ *pScr2++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, copy remaining samples here.
+ No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ x4 = (q15_t) *pIn2++;
+ *pScr2++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Fill (srcBLen - 1U) zeros in scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update temporary scratch pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Copy (srcALen) samples in scratch buffer */
+ /* Apply loop unrolling and do 4 Copies simultaneously. */
+ k = srcALen >> 2U;
+
+ /* First part of the processing with loop unrolling copies 4 data points at a time.
+ a second loop below copies for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner */
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+ x4 = (q15_t) *pIn1++;
+ *pScr1++ = x4;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, copy remaining samples here.
+ No loop unrolling is used. */
+ k = srcALen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* copy second buffer in reversal manner for remaining samples */
+ x4 = (q15_t) * pIn1++;
+ *pScr1++ = x4;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Fill (srcBLen - 1U) zeros at end of scratch buffer */
+ arm_fill_q15(0, pScr1, (srcBLen - 1U));
+
+ /* Update pointer */
+ pScr1 += (srcBLen - 1U);
+
+ /* Temporary pointer for scratch2 */
+ py = pScratch2;
+
+ /* Initialization of pScr2 pointer */
+ pScr2 = pScratch2;
+
+ /* Actual correlation process starts here */
+ blkCnt = (srcALen + srcBLen - 1U) >> 2;
+
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Read two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* Read next two samples from scratch1 buffer */
+ x2 = read_q15x2_ia (&pScr1);
+
+ tapCnt = (srcBLen) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ /* multiply and accumlate */
+ acc0 = __SMLAD(x1, y1, acc0);
+ acc2 = __SMLAD(x2, y1, acc2);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ /* multiply and accumlate */
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ /* Read next two samples from scratch1 buffer */
+ x1 = read_q15x2_ia (&pScr1);
+
+ /* pack input data */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x1, x2, 0);
+#else
+ x3 = __PKHBT(x2, x1, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+
+ /* Read four samples from smaller buffer */
+ y1 = read_q15x2_ia (&pScr2);
+
+ acc0 = __SMLAD(x2, y1, acc0);
+
+ acc2 = __SMLAD(x1, y1, acc2);
+
+ acc1 = __SMLADX(x3, y1, acc1);
+
+ x2 = read_q15x2_ia (&pScr1);
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ x3 = __PKHBT(x2, x1, 0);
+#else
+ x3 = __PKHBT(x1, x2, 0);
+#endif
+
+ acc3 = __SMLADX(x3, y1, acc3);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Update scratch pointer for remaining samples of smaller length sequence */
+ pScr1 -= 4U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ tapCnt = (srcBLen) & 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pScr2);
+ acc1 += (*pScr1++ * *pScr2);
+ acc2 += (*pScr1++ * *pScr2);
+ acc3 += (*pScr1++ * *pScr2++);
+
+ pScr1 -= 3U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(acc0 >> 7U, 8));
+ pOut += inc;
+ *pOut = (q7_t) (__SSAT(acc1 >> 7U, 8));
+ pOut += inc;
+ *pOut = (q7_t) (__SSAT(acc2 >> 7U, 8));
+ pOut += inc;
+ *pOut = (q7_t) (__SSAT(acc3 >> 7U, 8));
+ pOut += inc;
+
+ /* Initialization of inputB pointer */
+ pScr2 = py;
+
+ pScratch1 += 4U;
+ }
+
+ blkCnt = (srcALen + srcBLen - 1U) & 0x3;
+
+ /* Calculate correlation for remaining samples of Bigger length sequence */
+ while (blkCnt > 0)
+ {
+ /* Initialze temporary scratch pointer as scratch1 */
+ pScr1 = pScratch1;
+
+ /* Clear Accumlators */
+ acc0 = 0;
+
+ tapCnt = (srcBLen) >> 1U;
+
+ while (tapCnt > 0U)
+ {
+ acc0 += (*pScr1++ * *pScr2++);
+ acc0 += (*pScr1++ * *pScr2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (srcBLen) & 1U;
+
+ /* apply same above for remaining samples of smaller length sequence */
+ while (tapCnt > 0U)
+ {
+ /* accumlate the results */
+ acc0 += (*pScr1++ * *pScr2++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ blkCnt--;
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(acc0 >> 7U, 8));
+ pOut += inc;
+
+ /* Initialization of inputB pointer */
+ pScr2 = py;
+
+ pScratch1 += 1U;
+ }
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q15.c new file mode 100644 index 0000000..c847495 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q15.c @@ -0,0 +1,696 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_q15.c
+ * Description: Correlation of Q15 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q15 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both inputs are in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ This approach provides 33 guard bits and there is no risk of overflow.
+ The 34.30 result is then truncated to 34.15 format by discarding the low 15 bits and then saturated to 1.15 format.
+
+ @remark
+ Refer to \ref arm_correlate_fast_q15() for a faster but less precise version of this function.
+ @remark
+ Refer to \ref arm_correlate_opt_q15() for a faster implementation of this function using scratch buffers.
+ */
+
+void arm_correlate_q15(
+ const q15_t * pSrcA,
+ uint32_t srcALen,
+ const q15_t * pSrcB,
+ uint32_t srcBLen,
+ q15_t * pDst)
+{
+
+#if defined (ARM_MATH_DSP)
+
+ const q15_t *pIn1; /* InputA pointer */
+ const q15_t *pIn2; /* InputB pointer */
+ q15_t *pOut = pDst; /* Output pointer */
+ q63_t sum, acc0, acc1, acc2, acc3; /* Accumulators */
+ const q15_t *px; /* Intermediate inputA pointer */
+ const q15_t *py; /* Intermediate inputB pointer */
+ const q15_t *pSrc1; /* Intermediate pointers */
+ q31_t x0, x1, x2, x3, c0; /* Temporary input variables for holding input and coefficient values */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+ uint32_t outBlockSize;
+ int32_t inc = 1; /* Destination address modifier */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (srcALen * 2U) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[srcBlen - 1]
+ * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
+ * ....
+ * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + (srcBLen - 1U);
+ py = pSrc1;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first loop starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0] * y[srcBLen - 4] , x[1] * y[srcBLen - 3] */
+ sum = __SMLALD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+ /* x[3] * y[srcBLen - 1] , x[2] * y[srcBLen - 2] */
+ sum = __SMLALD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0] * y[srcBLen - 1] */
+ sum = __SMLALD(*px++, *py++, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (__SSAT((sum >> 15), 16));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pSrc1 - count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
+ * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* count is the index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1] samples */
+ x0 = read_q15x2 ((q15_t *) px);
+
+ /* read x[1], x[2] samples */
+ x1 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read the first two inputB samples using SIMD:
+ * y[0] and y[1] */
+ c0 = read_q15x2_ia ((q15_t **) &py);
+
+ /* acc0 += x[0] * y[0] + x[1] * y[1] */
+ acc0 = __SMLALD(x0, c0, acc0);
+
+ /* acc1 += x[1] * y[0] + x[2] * y[1] */
+ acc1 = __SMLALD(x1, c0, acc1);
+
+ /* Read x[2], x[3] */
+ x2 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[3], x[4] */
+ x3 = read_q15x2 ((q15_t *) px + 1);
+
+ /* acc2 += x[2] * y[0] + x[3] * y[1] */
+ acc2 = __SMLALD(x2, c0, acc2);
+
+ /* acc3 += x[3] * y[0] + x[4] * y[1] */
+ acc3 = __SMLALD(x3, c0, acc3);
+
+ /* Read y[2] and y[3] */
+ c0 = read_q15x2_ia ((q15_t **) &py);
+
+ /* acc0 += x[2] * y[2] + x[3] * y[3] */
+ acc0 = __SMLALD(x2, c0, acc0);
+
+ /* acc1 += x[3] * y[2] + x[4] * y[3] */
+ acc1 = __SMLALD(x3, c0, acc1);
+
+ /* Read x[4], x[5] */
+ x0 = read_q15x2 ((q15_t *) px + 2);
+
+ /* Read x[5], x[6] */
+ x1 = read_q15x2 ((q15_t *) px + 3);
+ px += 4U;
+
+ /* acc2 += x[4] * y[2] + x[5] * y[3] */
+ acc2 = __SMLALD(x0, c0, acc2);
+
+ /* acc3 += x[5] * y[2] + x[6] * y[3] */
+ acc3 = __SMLALD(x1, c0, acc3);
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ if (k == 1U)
+ {
+ /* Read y[4] */
+ c0 = *py;
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[7] */
+ x3 = read_q15x2 ((q15_t *) px);
+ px++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD (x0, c0, acc0);
+ acc1 = __SMLALD (x1, c0, acc1);
+ acc2 = __SMLALDX(x1, c0, acc2);
+ acc3 = __SMLALDX(x3, c0, acc3);
+ }
+
+ if (k == 2U)
+ {
+ /* Read y[4], y[5] */
+ c0 = read_q15x2 ((q15_t *) py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+ px += 2U;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc1 = __SMLALD(x1, c0, acc1);
+ acc2 = __SMLALD(x3, c0, acc2);
+ acc3 = __SMLALD(x2, c0, acc3);
+ }
+
+ if (k == 3U)
+ {
+ /* Read y[4], y[5] */
+ c0 = read_q15x2_ia ((q15_t **) &py);
+
+ /* Read x[7], x[8] */
+ x3 = read_q15x2 ((q15_t *) px);
+
+ /* Read x[9] */
+ x2 = read_q15x2 ((q15_t *) px + 1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc1 = __SMLALD(x1, c0, acc1);
+ acc2 = __SMLALD(x3, c0, acc2);
+ acc3 = __SMLALD(x2, c0, acc3);
+
+ c0 = (*py);
+
+ /* Read y[6] */
+#ifdef ARM_MATH_BIG_ENDIAN
+ c0 = c0 << 16U;
+#else
+ c0 = c0 & 0x0000FFFF;
+#endif /* #ifdef ARM_MATH_BIG_ENDIAN */
+
+ /* Read x[10] */
+ x3 = read_q15x2 ((q15_t *) px + 2);
+ px += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLALDX(x1, c0, acc0);
+ acc1 = __SMLALD (x2, c0, acc1);
+ acc2 = __SMLALDX(x2, c0, acc2);
+ acc3 = __SMLALDX(x3, c0, acc3);
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (__SSAT(acc0 >> 15, 16));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ *pOut = (q15_t) (__SSAT(acc1 >> 15, 16));
+ pOut += inc;
+
+ *pOut = (q15_t) (__SSAT(acc2 >> 15, 16));
+ pOut += inc;
+
+ *pOut = (q15_t) (__SSAT(acc3 >> 15, 16));
+ pOut += inc;
+
+ /* Increment the count by 4 as 4 output values are computed */
+ count += 4U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize2 is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+ blkCnt = blockSize2 % 0x4U;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q63_t) *px++ * *py++);
+ sum += ((q63_t) *px++ * *py++);
+ sum += ((q63_t) *px++ * *py++);
+ sum += ((q63_t) *px++ * *py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += ((q63_t) *px++ * *py++);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (__SSAT(sum >> 15, 16));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment count by 1, as one output value is computed */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q63_t) *px++ * *py++);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (__SSAT(sum >> 15, 16));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * ....
+ * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
+ * sum += x[srcALen-1] * y[0]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = (pIn1 + srcALen) - (srcBLen - 1U);
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = count >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 4] * y[3] , sum += x[srcALen - srcBLen + 3] * y[2] */
+ sum = __SMLALD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+ /* sum += x[srcALen - srcBLen + 2] * y[1] , sum += x[srcALen - srcBLen + 1] * y[0] */
+ sum = __SMLALD(read_q15x2_ia ((q15_t **) &px), read_q15x2_ia ((q15_t **) &py), sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* If the count is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = count % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum = __SMLALD(*px++, *py++, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q15_t) (__SSAT((sum >> 15), 16));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+ const q15_t *pIn1 = pSrcA; /* InputA pointer */
+ const q15_t *pIn2 = pSrcB + (srcBLen - 1U); /* InputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* Loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and a varaible, inv is set to 1 */
+ /* If lengths are not equal then zero pad has to be done to make the two
+ * inputs of same length. But to improve the performance, we include zeroes
+ * in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * starting of the output buffer */
+ /* If srcALen < srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * ending of the output buffer */
+ /* Once the zero padding is done the remaining of the output is calcualted
+ * using convolution but with the shorter signal time shifted. */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+ }
+
+ /* Loop to calculate convolution for output length number of values */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry on MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q31_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q15_t) __SSAT((sum >> 15U), 16U);
+ else
+ *pDst++ = (q15_t) __SSAT((sum >> 15U), 16U);
+ }
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q31.c new file mode 100644 index 0000000..60c1b70 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q31.c @@ -0,0 +1,682 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_q31.c
+ * Description: Correlation of Q31 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q31 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ There is no saturation on intermediate additions.
+ Thus, if the accumulator overflows it wraps around and distorts the result.
+ The input signals should be scaled down to avoid intermediate overflows.
+ Scale down one of the inputs by 1/min(srcALen, srcBLen)to avoid overflows since a
+ maximum of min(srcALen, srcBLen) number of additions is carried internally.
+ The 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_correlate_fast_q31() for a faster but less precise implementation of this function.
+ */
+
+void arm_correlate_q31(
+ const q31_t * pSrcA,
+ uint32_t srcALen,
+ const q31_t * pSrcB,
+ uint32_t srcBLen,
+ q31_t * pDst)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const q31_t *pIn1; /* InputA pointer */
+ const q31_t *pIn2; /* InputB pointer */
+ q31_t *pOut = pDst; /* Output pointer */
+ const q31_t *px; /* Intermediate inputA pointer */
+ const q31_t *py; /* Intermediate inputB pointer */
+ const q31_t *pSrc1; /* Intermediate pointers */
+ q63_t sum; /* Accumulators */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+ uint32_t outBlockSize;
+ int32_t inc = 1; /* Destination address modifier */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc0, acc1, acc2; /* Accumulators */
+ q31_t x0, x1, x2, c0; /* Temporary variables for holding input and coefficient values */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (2U * srcALen) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[srcBlen - 1]
+ * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
+ * ....
+ * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + (srcBLen - 1U);
+ py = pSrc1;
+
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] * y[srcBLen - 4] */
+ sum += (q63_t) *px++ * (*py++);
+
+ /* x[1] * y[srcBLen - 3] */
+ sum += (q63_t) *px++ * (*py++);
+
+ /* x[2] * y[srcBLen - 2] */
+ sum += (q63_t) *px++ * (*py++);
+
+ /* x[3] * y[srcBLen - 1] */
+ sum += (q63_t) *px++ * (*py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0] * y[srcBLen - 1] */
+ sum += (q63_t) *px++ * (*py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q31_t) (sum >> 31);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pSrc1 - count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
+ * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unroll by 3 */
+ blkCnt = blockSize2 / 3;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+
+ /* read x[0], x[1] samples */
+ x0 = *px++;
+ x1 = *px++;
+
+ /* Apply loop unrolling and compute 3 MACs simultaneously. */
+ k = srcBLen / 3;
+
+ /* First part of the processing with loop unrolling. Compute 3 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 2 samples. */
+ do
+ {
+ /* Read y[0] sample */
+ c0 = *(py);
+ /* Read x[2] sample */
+ x2 = *(px);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[0] * y[0] */
+ acc0 += ((q63_t) x0 * c0);
+ /* acc1 += x[1] * y[0] */
+ acc1 += ((q63_t) x1 * c0);
+ /* acc2 += x[2] * y[0] */
+ acc2 += ((q63_t) x2 * c0);
+
+ /* Read y[1] sample */
+ c0 = *(py + 1U);
+ /* Read x[3] sample */
+ x0 = *(px + 1U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[1] * y[1] */
+ acc0 += ((q63_t) x1 * c0);
+ /* acc1 += x[2] * y[1] */
+ acc1 += ((q63_t) x2 * c0);
+ /* acc2 += x[3] * y[1] */
+ acc2 += ((q63_t) x0 * c0);
+
+ /* Read y[2] sample */
+ c0 = *(py + 2U);
+ /* Read x[4] sample */
+ x1 = *(px + 2U);
+
+ /* Perform the multiply-accumulate */
+ /* acc0 += x[2] * y[2] */
+ acc0 += ((q63_t) x2 * c0);
+ /* acc1 += x[3] * y[2] */
+ acc1 += ((q63_t) x0 * c0);
+ /* acc2 += x[4] * y[2] */
+ acc2 += ((q63_t) x1 * c0);
+
+ /* update scratch pointers */
+ px += 3U;
+ py += 3U;
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 3, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen - (3 * (srcBLen / 3));
+
+ while (k > 0U)
+ {
+ /* Read y[4] sample */
+ c0 = *(py++);
+
+ /* Read x[7] sample */
+ x2 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[4] */
+ acc0 += ((q63_t) x0 * c0);
+ /* acc1 += x[5] * y[4] */
+ acc1 += ((q63_t) x1 * c0);
+ /* acc2 += x[6] * y[4] */
+ acc2 += ((q63_t) x2 * c0);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q31_t) (acc0 >> 31);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ *pOut = (q31_t) (acc1 >> 31);
+ pOut += inc;
+
+ *pOut = (q31_t) (acc2 >> 31);
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 3 */
+ count += 3U;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize2 - 3 * (blockSize2 / 3);
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulates */
+ sum += (q63_t) *px++ * *py++;
+ sum += (q63_t) *px++ * *py++;
+ sum += (q63_t) *px++ * *py++;
+ sum += (q63_t) *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py++;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q31_t) (sum >> 31);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py++;
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q31_t) (sum >> 31);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * ....
+ * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
+ * sum += x[srcALen-1] * y[0]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = pIn1 + (srcALen - (srcBLen - 1U));
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* sum += x[srcALen - srcBLen + 4] * y[3] */
+ sum += (q63_t) *px++ * *py++;
+
+ /* sum += x[srcALen - srcBLen + 3] * y[2] */
+ sum += (q63_t) *px++ * *py++;
+
+ /* sum += x[srcALen - srcBLen + 2] * y[1] */
+ sum += (q63_t) *px++ * *py++;
+
+ /* sum += x[srcALen - srcBLen + 1] * y[0] */
+ sum += (q63_t) *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (q63_t) *px++ * *py++;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q31_t) (sum >> 31);
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const q31_t *pIn1 = pSrcA; /* InputA pointer */
+ const q31_t *pIn2 = pSrcB + (srcBLen - 1U); /* InputB pointer */
+ q63_t sum; /* Accumulators */
+ uint32_t i = 0U, j; /* Loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and a varaible, inv is set to 1 */
+ /* If lengths are not equal then zero pad has to be done to make the two
+ * inputs of same length. But to improve the performance, we include zeroes
+ * in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * starting of the output buffer */
+ /* If srcALen < srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * ending of the output buffer */
+ /* Once the zero padding is done the remaining of the output is calcualted
+ * using correlation but with the shorter signal time shifted. */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+ }
+
+ /* Loop to calculate correlation for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry out MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to correlation equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q63_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q31_t) (sum >> 31U);
+ else
+ *pDst++ = (q31_t) (sum >> 31U);
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q7.c new file mode 100644 index 0000000..0fa49ec --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_correlate_q7.c @@ -0,0 +1,800 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_correlate_q7.c
+ * Description: Correlation of Q7 sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup Corr
+ @{
+ */
+
+/**
+ @brief Correlation of Q7 sequences.
+ @param[in] pSrcA points to the first input sequence
+ @param[in] srcALen length of the first input sequence
+ @param[in] pSrcB points to the second input sequence
+ @param[in] srcBLen length of the second input sequence
+ @param[out] pDst points to the location where the output result is written. Length 2 * max(srcALen, srcBLen) - 1.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 32-bit internal accumulator.
+ Both the inputs are represented in 1.7 format and multiplications yield a 2.14 result.
+ The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
+ This approach provides 17 guard bits and there is no risk of overflow as long as <code>max(srcALen, srcBLen)<131072</code>.
+ The 18.14 result is then truncated to 18.7 format by discarding the low 7 bits and saturated to 1.7 format.
+
+ @remark
+ Refer to \ref arm_correlate_opt_q7() for a faster implementation of this function.
+ */
+
+void arm_correlate_q7(
+ const q7_t * pSrcA,
+ uint32_t srcALen,
+ const q7_t * pSrcB,
+ uint32_t srcBLen,
+ q7_t * pDst)
+{
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ const q7_t *pIn1; /* InputA pointer */
+ const q7_t *pIn2; /* InputB pointer */
+ q7_t *pOut = pDst; /* Output pointer */
+ const q7_t *px; /* Intermediate inputA pointer */
+ const q7_t *py; /* Intermediate inputB pointer */
+ const q7_t *pSrc1; /* Intermediate pointers */
+ q31_t sum; /* Accumulators */
+ uint32_t blockSize1, blockSize2, blockSize3; /* Loop counters */
+ uint32_t j, k, count, blkCnt; /* Loop counters */
+ uint32_t outBlockSize;
+ int32_t inc = 1;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc0, acc1, acc2, acc3; /* Accumulators */
+ q31_t input1, input2; /* Temporary input variables */
+ q15_t in1, in2; /* Temporary input variables */
+ q7_t x0, x1, x2, x3, c0, c1; /* Temporary variables for holding input and coefficient values */
+#endif
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and the destination pointer modifier, inc is set to -1 */
+ /* If srcALen > srcBLen, zero pad has to be done to srcB to make the two inputs of same length */
+ /* But to improve the performance,
+ * we include zeroes in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the starting of the output buffer */
+ /* If srcALen < srcBLen,
+ * (srcALen - srcBLen) zeroes has to included in the ending of the output buffer */
+ if (srcALen >= srcBLen)
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcA;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcB;
+
+ /* Number of output samples is calculated */
+ outBlockSize = (2U * srcALen) - 1U;
+
+ /* When srcALen > srcBLen, zero padding is done to srcB
+ * to make their lengths equal.
+ * Instead, (outBlockSize - (srcALen + srcBLen - 1))
+ * number of output samples are made zero */
+ j = outBlockSize - (srcALen + (srcBLen - 1U));
+
+ /* Updating the pointer position to non zero value */
+ pOut += j;
+ }
+ else
+ {
+ /* Initialization of inputA pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization of inputB pointer */
+ pIn2 = pSrcA;
+
+ /* srcBLen is always considered as shorter or equal to srcALen */
+ j = srcBLen;
+ srcBLen = srcALen;
+ srcALen = j;
+
+ /* CORR(x, y) = Reverse order(CORR(y, x)) */
+ /* Hence set the destination pointer to point to the last output sample */
+ pOut = pDst + ((srcALen + srcBLen) - 2U);
+
+ /* Destination address modifier is set to -1 */
+ inc = -1;
+ }
+
+ /* The function is internally
+ * divided into three stages according to the number of multiplications that has to be
+ * taken place between inputA samples and inputB samples. In the first stage of the
+ * algorithm, the multiplications increase by one for every iteration.
+ * In the second stage of the algorithm, srcBLen number of multiplications are done.
+ * In the third stage of the algorithm, the multiplications decrease by one
+ * for every iteration. */
+
+ /* The algorithm is implemented in three stages.
+ The loop counters of each stage is initiated here. */
+ blockSize1 = srcBLen - 1U;
+ blockSize2 = srcALen - (srcBLen - 1U);
+ blockSize3 = blockSize1;
+
+ /* --------------------------
+ * Initializations of stage1
+ * -------------------------*/
+
+ /* sum = x[0] * y[srcBlen - 1]
+ * sum = x[0] * y[srcBlen - 2] + x[1] * y[srcBlen - 1]
+ * ....
+ * sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen - 1] * y[srcBLen - 1]
+ */
+
+ /* In this stage the MAC operations are increased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = 1U;
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ pSrc1 = pIn2 + (srcBLen - 1U);
+ py = pSrc1;
+
+ /* ------------------------
+ * Stage1 process
+ * ----------------------*/
+
+ /* The first stage starts here */
+ while (blockSize1 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[0] , x[1] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* y[srcBLen - 4] , y[srcBLen - 3] */
+ in1 = (q15_t) *py++;
+ in2 = (q15_t) *py++;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* x[0] * y[srcBLen - 4] */
+ /* x[1] * y[srcBLen - 3] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* x[2] , x[3] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* y[srcBLen - 2] , y[srcBLen - 1] */
+ in1 = (q15_t) *py++;
+ in2 = (q15_t) *py++;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16);
+
+ /* x[2] * y[srcBLen - 2] */
+ /* x[3] * y[srcBLen - 1] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize k with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* x[0] * y[srcBLen - 1] */
+ sum += (q31_t) ((q15_t) *px++ * *py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(sum >> 7U, 8));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ py = pSrc1 - count;
+ px = pIn1;
+
+ /* Increment MAC count */
+ count++;
+
+ /* Decrement loop counter */
+ blockSize1--;
+ }
+
+ /* --------------------------
+ * Initializations of stage2
+ * ------------------------*/
+
+ /* sum = x[0] * y[0] + x[1] * y[1] +...+ x[srcBLen-1] * y[srcBLen-1]
+ * sum = x[1] * y[0] + x[2] * y[1] +...+ x[srcBLen] * y[srcBLen-1]
+ * ....
+ * sum = x[srcALen-srcBLen-2] * y[0] + x[srcALen-srcBLen-1] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ */
+
+ /* Working pointer of inputA */
+ px = pIn1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* count is index by which the pointer pIn1 to be incremented */
+ count = 0U;
+
+ /* -------------------
+ * Stage2 process
+ * ------------------*/
+
+ /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
+ * So, to loop unroll over blockSize2,
+ * srcBLen should be greater than or equal to 4 */
+ if (srcBLen >= 4U)
+ {
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize2 >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* read x[0], x[1], x[2] samples */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Apply loop unrolling and compute 4 MACs simultaneously. */
+ k = srcBLen >> 2U;
+
+ /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
+ ** a second loop below computes MACs for the remaining 1 to 3 samples. */
+ do
+ {
+ /* Read y[0] sample */
+ c0 = *py++;
+ /* Read y[1] sample */
+ c1 = *py++;
+
+ /* Read x[3] sample */
+ x3 = *px++;
+
+ /* x[0] and x[1] are packed */
+ in1 = (q15_t) x0;
+ in2 = (q15_t) x1;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[0] and y[1] are packed */
+ in1 = (q15_t) c0;
+ in2 = (q15_t) c1;
+
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc0 += x[0] * y[0] + x[1] * y[1] */
+ acc0 = __SMLAD(input1, input2, acc0);
+
+ /* x[1] and x[2] are packed */
+ in1 = (q15_t) x1;
+ in2 = (q15_t) x2;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc1 += x[1] * y[0] + x[2] * y[1] */
+ acc1 = __SMLAD(input1, input2, acc1);
+
+ /* x[2] and x[3] are packed */
+ in1 = (q15_t) x2;
+ in2 = (q15_t) x3;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc2 += x[2] * y[0] + x[3] * y[1] */
+ acc2 = __SMLAD(input1, input2, acc2);
+
+ /* Read x[4] sample */
+ x0 = *px++;
+
+ /* x[3] and x[4] are packed */
+ in1 = (q15_t) x3;
+ in2 = (q15_t) x0;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc3 += x[3] * y[0] + x[4] * y[1] */
+ acc3 = __SMLAD(input1, input2, acc3);
+
+ /* Read y[2] sample */
+ c0 = *py++;
+ /* Read y[3] sample */
+ c1 = *py++;
+
+ /* Read x[5] sample */
+ x1 = *px++;
+
+ /* x[2] and x[3] are packed */
+ in1 = (q15_t) x2;
+ in2 = (q15_t) x3;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[2] and y[3] are packed */
+ in1 = (q15_t) c0;
+ in2 = (q15_t) c1;
+
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc0 += x[2] * y[2] + x[3] * y[3] */
+ acc0 = __SMLAD(input1, input2, acc0);
+
+ /* x[3] and x[4] are packed */
+ in1 = (q15_t) x3;
+ in2 = (q15_t) x0;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc1 += x[3] * y[2] + x[4] * y[3] */
+ acc1 = __SMLAD(input1, input2, acc1);
+
+ /* x[4] and x[5] are packed */
+ in1 = (q15_t) x0;
+ in2 = (q15_t) x1;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc2 += x[4] * y[2] + x[5] * y[3] */
+ acc2 = __SMLAD(input1, input2, acc2);
+
+ /* Read x[6] sample */
+ x2 = *px++;
+
+ /* x[5] and x[6] are packed */
+ in1 = (q15_t) x1;
+ in2 = (q15_t) x2;
+
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* acc3 += x[5] * y[2] + x[6] * y[3] */
+ acc3 = __SMLAD(input1, input2, acc3);
+
+ } while (--k);
+
+ /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
+ ** No loop unrolling is used. */
+ k = srcBLen % 0x4U;
+
+ while (k > 0U)
+ {
+ /* Read y[4] sample */
+ c0 = *py++;
+ /* Read x[7] sample */
+ x3 = *px++;
+
+ /* Perform the multiply-accumulates */
+ /* acc0 += x[4] * y[4] */
+ acc0 += ((q15_t) x0 * c0);
+ /* acc1 += x[5] * y[4] */
+ acc1 += ((q15_t) x1 * c0);
+ /* acc2 += x[6] * y[4] */
+ acc2 += ((q15_t) x2 * c0);
+ /* acc3 += x[7] * y[4] */
+ acc3 += ((q15_t) x3 * c0);
+
+ /* Reuse the present samples for the next MAC */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(acc0 >> 7, 8));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ *pOut = (q7_t) (__SSAT(acc1 >> 7, 8));
+ pOut += inc;
+
+ *pOut = (q7_t) (__SSAT(acc2 >> 7, 8));
+ pOut += inc;
+
+ *pOut = (q7_t) (__SSAT(acc3 >> 7, 8));
+ pOut += inc;
+
+ count += 4U;
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize2 % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize2;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = srcBLen >> 2U;
+
+ while (k > 0U)
+ {
+
+ /* Reading two inputs of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Reading two inputs of SrcB buffer and packing */
+ in1 = (q15_t) *py++;
+ in2 = (q15_t) *py++;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Perform the multiply-accumulate */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Reading two inputs of SrcA buffer and packing */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Reading two inputs of SrcB buffer and packing */
+ in1 = (q15_t) *py++;
+ in2 = (q15_t) *py++;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* Perform the multiply-accumulate */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = srcBLen % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = srcBLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q15_t) *px++ * *py++);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(sum >> 7U, 8));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment the pointer pIn1 index, count by 1 */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+ }
+ else
+ {
+ /* If the srcBLen is not a multiple of 4,
+ * the blockSize2 loop cannot be unrolled by 4 */
+ blkCnt = blockSize2;
+
+ while (blkCnt > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+ /* srcBLen number of MACS should be performed */
+ k = srcBLen;
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q15_t) *px++ * *py++);
+
+ /* Decrement the loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(sum >> 7U, 8));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Increment the MAC count */
+ count++;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = pIn1 + count;
+ py = pIn2;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+ }
+
+
+ /* --------------------------
+ * Initializations of stage3
+ * -------------------------*/
+
+ /* sum += x[srcALen-srcBLen+1] * y[0] + x[srcALen-srcBLen+2] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * sum += x[srcALen-srcBLen+2] * y[0] + x[srcALen-srcBLen+3] * y[1] +...+ x[srcALen-1] * y[srcBLen-1]
+ * ....
+ * sum += x[srcALen-2] * y[0] + x[srcALen-1] * y[1]
+ * sum += x[srcALen-1] * y[0]
+ */
+
+ /* In this stage the MAC operations are decreased by 1 for every iteration.
+ The count variable holds the number of MAC operations performed */
+ count = srcBLen - 1U;
+
+ /* Working pointer of inputA */
+ pSrc1 = pIn1 + (srcALen - (srcBLen - 1U));
+ px = pSrc1;
+
+ /* Working pointer of inputB */
+ py = pIn2;
+
+ /* -------------------
+ * Stage3 process
+ * ------------------*/
+
+ while (blockSize3 > 0U)
+ {
+ /* Accumulator is made zero for every iteration */
+ sum = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ k = count >> 2U;
+
+ while (k > 0U)
+ {
+ /* x[srcALen - srcBLen + 1] , x[srcALen - srcBLen + 2] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[0] , y[1] */
+ in1 = (q15_t) *py++;
+ in2 = (q15_t) *py++;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* sum += x[srcALen - srcBLen + 1] * y[0] */
+ /* sum += x[srcALen - srcBLen + 2] * y[1] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* x[srcALen - srcBLen + 3] , x[srcALen - srcBLen + 4] */
+ in1 = (q15_t) *px++;
+ in2 = (q15_t) *px++;
+ input1 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* y[2] , y[3] */
+ in1 = (q15_t) *py++;
+ in2 = (q15_t) *py++;
+ input2 = ((q31_t) in1 & 0x0000FFFF) | ((q31_t) in2 << 16U);
+
+ /* sum += x[srcALen - srcBLen + 3] * y[2] */
+ /* sum += x[srcALen - srcBLen + 4] * y[3] */
+ sum = __SMLAD(input1, input2, sum);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ k = count % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ k = count;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (k > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += ((q15_t) *px++ * *py++);
+
+ /* Decrement loop counter */
+ k--;
+ }
+
+ /* Store the result in the accumulator in the destination buffer. */
+ *pOut = (q7_t) (__SSAT(sum >> 7U, 8));
+ /* Destination pointer is updated according to the address modifier, inc */
+ pOut += inc;
+
+ /* Update the inputA and inputB pointers for next MAC calculation */
+ px = ++pSrc1;
+ py = pIn2;
+
+ /* Decrement MAC count */
+ count--;
+
+ /* Decrement loop counter */
+ blockSize3--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ const q7_t *pIn1 = pSrcA; /* InputA pointer */
+ const q7_t *pIn2 = pSrcB + (srcBLen - 1U); /* InputB pointer */
+ q31_t sum; /* Accumulator */
+ uint32_t i = 0U, j; /* Loop counters */
+ uint32_t inv = 0U; /* Reverse order flag */
+ uint32_t tot = 0U; /* Length */
+
+ /* The algorithm implementation is based on the lengths of the inputs. */
+ /* srcB is always made to slide across srcA. */
+ /* So srcBLen is always considered as shorter or equal to srcALen */
+ /* But CORR(x, y) is reverse of CORR(y, x) */
+ /* So, when srcBLen > srcALen, output pointer is made to point to the end of the output buffer */
+ /* and a varaible, inv is set to 1 */
+ /* If lengths are not equal then zero pad has to be done to make the two
+ * inputs of same length. But to improve the performance, we include zeroes
+ * in the output instead of zero padding either of the the inputs*/
+ /* If srcALen > srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * starting of the output buffer */
+ /* If srcALen < srcBLen, (srcALen - srcBLen) zeroes has to included in the
+ * ending of the output buffer */
+ /* Once the zero padding is done the remaining of the output is calcualted
+ * using convolution but with the shorter signal time shifted. */
+
+ /* Calculate the length of the remaining sequence */
+ tot = ((srcALen + srcBLen) - 2U);
+
+ if (srcALen > srcBLen)
+ {
+ /* Calculating the number of zeros to be padded to the output */
+ j = srcALen - srcBLen;
+
+ /* Initialise the pointer after zero padding */
+ pDst += j;
+ }
+
+ else if (srcALen < srcBLen)
+ {
+ /* Initialization to inputB pointer */
+ pIn1 = pSrcB;
+
+ /* Initialization to the end of inputA pointer */
+ pIn2 = pSrcA + (srcALen - 1U);
+
+ /* Initialisation of the pointer after zero padding */
+ pDst = pDst + tot;
+
+ /* Swapping the lengths */
+ j = srcALen;
+ srcALen = srcBLen;
+ srcBLen = j;
+
+ /* Setting the reverse flag */
+ inv = 1;
+ }
+
+ /* Loop to calculate convolution for output length number of times */
+ for (i = 0U; i <= tot; i++)
+ {
+ /* Initialize sum with zero to carry out MAC operations */
+ sum = 0;
+
+ /* Loop to perform MAC operations according to convolution equation */
+ for (j = 0U; j <= i; j++)
+ {
+ /* Check the array limitations */
+ if (((i - j) < srcBLen) && (j < srcALen))
+ {
+ /* z[i] += x[i-j] * y[j] */
+ sum += ((q15_t) pIn1[j] * pIn2[-((int32_t) i - j)]);
+ }
+ }
+
+ /* Store the output in the destination buffer */
+ if (inv == 1)
+ *pDst-- = (q7_t) __SSAT((sum >> 7U), 8U);
+ else
+ *pDst++ = (q7_t) __SSAT((sum >> 7U), 8U);
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of Corr group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_f32.c new file mode 100644 index 0000000..d91543c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_f32.c @@ -0,0 +1,703 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_f32.c
+ * Description: FIR decimation for floating-point sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup FIR_decimate Finite Impulse Response (FIR) Decimator
+
+ These functions combine an FIR filter together with a decimator.
+ They are used in multirate systems for reducing the sample rate of a signal without introducing aliasing distortion.
+ Conceptually, the functions are equivalent to the block diagram below:
+ \image html FIRDecimator.gif "Components included in the FIR Decimator functions"
+ When decimating by a factor of <code>M</code>, the signal should be prefiltered by a lowpass filter with a normalized
+ cutoff frequency of <code>1/M</code> in order to prevent aliasing distortion.
+ The user of the function is responsible for providing the filter coefficients.
+
+ The FIR decimator functions provided in the CMSIS DSP Library combine the FIR filter and the decimator in an efficient manner.
+ Instead of calculating all of the FIR filter outputs and discarding <code>M-1</code> out of every <code>M</code>, only the
+ samples output by the decimator are computed.
+ The functions operate on blocks of input and output data.
+ <code>pSrc</code> points to an array of <code>blockSize</code> input values and
+ <code>pDst</code> points to an array of <code>blockSize/M</code> output values.
+ In order to have an integer number of output samples <code>blockSize</code>
+ must always be a multiple of the decimation factor <code>M</code>.
+
+ The library provides separate functions for Q15, Q31 and floating-point data types.
+
+ @par Algorithm:
+ The FIR portion of the algorithm uses the standard form filter:
+ <pre>
+ y[n] = b[0] * x[n] + b[1] * x[n-1] + b[2] * x[n-2] + ...+ b[numTaps-1] * x[n-numTaps+1]
+ </pre>
+ where, <code>b[n]</code> are the filter coefficients.
+ @par
+ The <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
+ Coefficients are stored in time reversed order.
+ @par
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to a state array of size <code>numTaps + blockSize - 1</code>.
+ Samples in the state buffer are stored in the order:
+ @par
+ <pre>
+ {x[n-numTaps+1], x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2]....x[0], x[1], ..., x[blockSize-1]}
+ </pre>
+ The state variables are updated after each block of data is processed, the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable array should be allocated separately.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ - Checks to make sure that the size of the input is a multiple of the decimation factor.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numTaps, pCoeffs, M (decimation factor), pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ The code below statically initializes each of the 3 different data type filter instance structures
+ <pre>
+ arm_fir_decimate_instance_f32 S = {M, numTaps, pCoeffs, pState};
+ arm_fir_decimate_instance_q31 S = {M, numTaps, pCoeffs, pState};
+ arm_fir_decimate_instance_q15 S = {M, numTaps, pCoeffs, pState};
+ </pre>
+ where <code>M</code> is the decimation factor; <code>numTaps</code> is the number of filter coefficients in the filter;
+ <code>pCoeffs</code> is the address of the coefficient buffer;
+ <code>pState</code> is the address of the state buffer.
+ Be sure to set the values in the state buffer to zeros when doing static initialization.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the fixed-point versions of the FIR decimate filter functions.
+ In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ Refer to the function specific documentation below for usage guidelines.
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Processing function for floating-point FIR decimator.
+ @param[in] S points to an instance of the floating-point FIR decimator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+#if defined(ARM_MATH_NEON)
+void arm_fir_decimate_f32(
+ const arm_fir_decimate_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCurnt; /* Points to the current sample of the state */
+ float32_t *px; /* Temporary pointer for state buffer */
+ const float32_t *pb; /* Temporary pointer for coefficient buffer */
+ 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, tapCnt, blkCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+ uint32_t blkCntN4;
+ float32_t *px0, *px1, *px2, *px3;
+ float32_t acc0, acc1, acc2, acc3;
+ float32_t x1, x2, x3;
+
+ float32x4_t accv,acc0v,acc1v,acc2v,acc3v;
+ float32x4_t x0v, x1v, x2v, x3v;
+ float32x4_t c0v;
+ float32x2_t temp;
+ float32x4_t sum0v;
+
+ /* 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 = outBlockSize / 4;
+ blkCntN4 = outBlockSize - (4 * blkCnt);
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 * decimation factor number of new input samples into the state buffer */
+ i = 4 * S->M;
+
+ do
+ {
+ *pStateCurnt++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulators to zero */
+ acc0v = vdupq_n_f32(0.0);
+ acc1v = vdupq_n_f32(0.0);
+ acc2v = vdupq_n_f32(0.0);
+ acc3v = vdupq_n_f32(0.0);
+
+ /* Initialize state pointer for all the samples */
+ px0 = pState;
+ px1 = pState + S->M;
+ px2 = pState + 2 * S->M;
+ px3 = pState + 3 * S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+ /* Process 4 taps at a time. */
+ tapCnt = numTaps >> 2;
+
+ /* Loop over the number of taps.
+ ** Repeat until we've computed numTaps-4 coefficients. */
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0v = vld1q_f32(pb);
+ pb += 4;
+
+ /* Read x[n-numTaps-1] sample for acc0 */
+ x0v = vld1q_f32(px0);
+ x1v = vld1q_f32(px1);
+ x2v = vld1q_f32(px2);
+ x3v = vld1q_f32(px3);
+
+ px0 += 4;
+ px1 += 4;
+ px2 += 4;
+ px3 += 4;
+
+ acc0v = vmlaq_f32(acc0v, x0v, c0v);
+ acc1v = vmlaq_f32(acc1v, x1v, c0v);
+ acc2v = vmlaq_f32(acc2v, x2v, c0v);
+ acc3v = vmlaq_f32(acc3v, x3v, c0v);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ temp = vpadd_f32(vget_low_f32(acc0v),vget_high_f32(acc0v));
+ accv[0] = temp[0] + temp[1];
+
+ temp = vpadd_f32(vget_low_f32(acc1v),vget_high_f32(acc1v));
+ accv[1] = temp[0] + temp[1];
+
+ temp = vpadd_f32(vget_low_f32(acc2v),vget_high_f32(acc2v));
+ accv[2] = temp[0] + temp[1];
+
+ temp = vpadd_f32(vget_low_f32(acc3v),vget_high_f32(acc3v));
+ accv[3] = temp[0] + temp[1];
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch state variables for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ accv[0] += x0 * c0;
+ accv[1] += x1 * c0;
+ accv[2] += x2 * c0;
+ accv[3] += x3 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + 4 * S->M;
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ vst1q_f32(pDst,accv);
+ pDst += 4;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+
+ while (blkCntN4 > 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 */
+ sum0v = vdupq_n_f32(0.0);
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+ /* Process 4 taps at a time. */
+ tapCnt = numTaps >> 2;
+
+ /* Loop over the number of taps.
+ ** Repeat until we've computed numTaps-4 coefficients. */
+ while (tapCnt > 0U)
+ {
+ c0v = vld1q_f32(pb);
+ pb += 4;
+
+ x0v = vld1q_f32(px);
+ px += 4;
+
+ sum0v = vmlaq_f32(sum0v, x0v, c0v);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ temp = vpadd_f32(vget_low_f32(sum0v),vget_high_f32(sum0v));
+ sum0 = temp[0] + temp[1];
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch 1 state variable */
+ x0 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* 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++ = sum0;
+
+ /* Decrement the loop counter */
+ blkCntN4--;
+ }
+
+ /* Processing is complete.
+ ** Now copy the last numTaps - 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;
+
+ i = (numTaps - 1U) >> 2;
+
+ /* Copy data */
+ while (i > 0U)
+ {
+ sum0v = vld1q_f32(pState);
+ vst1q_f32(pStateCurnt,sum0v);
+ pState += 4;
+ pStateCurnt += 4;
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ i = (numTaps - 1U) % 0x04U;
+
+ /* Copy data */
+ while (i > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ i--;
+ }
+}
+#else
+void arm_fir_decimate_f32(
+ const arm_fir_decimate_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCur; /* Points to the current sample of the state */
+ float32_t *px0; /* Temporary pointer for state buffer */
+ const float32_t *pb; /* Temporary pointer for coefficient buffer */
+ float32_t x0, c0; /* Temporary variables to hold state and coefficient values */
+ float32_t acc0; /* Accumulator */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ float32_t *px1, *px2, *px3;
+ float32_t x1, x2, x3;
+ float32_t acc1, acc2, acc3;
+#endif
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 samples at a time */
+ blkCnt = outBlockSize >> 2U;
+
+ /* Samples loop unrolled by 4 */
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 4;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulators to zero */
+ acc0 = 0.0f;
+ acc1 = 0.0f;
+ acc2 = 0.0f;
+ acc3 = 0.0f;
+
+ /* Initialize state pointer for all the samples */
+ px0 = pState;
+ px1 = pState + S->M;
+ px2 = pState + 2 * S->M;
+ px3 = pState + 3 * S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-1] sample for acc0 */
+ x0 = *(px0++);
+ /* Read x[n-numTaps-1] sample for acc1 */
+ x1 = *(px1++);
+ /* Read x[n-numTaps-1] sample for acc2 */
+ x2 = *(px2++);
+ /* Read x[n-numTaps-1] sample for acc3 */
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-2] sample for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-3] sample acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-4] sample acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch state variables for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 4;
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst++ = acc0;
+ *pDst++ = acc1;
+ *pDst++ = acc2;
+ *pDst++ = acc3;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining samples */
+ blkCnt = outBlockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = outBlockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0.0f;
+
+ /* Initialize state pointer */
+ px0 = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* 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++ = acc0;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete.
+ Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x04U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+#endif /* #if defined(ARM_MATH_NEON) */
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c new file mode 100644 index 0000000..c782392 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q15.c @@ -0,0 +1,595 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_fast_q15.c
+ * Description: Fast Q15 FIR Decimator
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 FIR decimator (fast variant).
+ @param[in] S points to an instance of the Q15 FIR decimator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of input samples to process per call
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around and distorts the result.
+ In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits (log2 is read as log to the base 2).
+ The 2.30 accumulator is then truncated to 2.15 format and saturated to yield the 1.15 result.
+ @remark
+ Refer to \ref arm_fir_decimate_q15() for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.
+ Both the slow and the fast versions use the same instance structure.
+ Use function \ref arm_fir_decimate_init_q15() to initialize the filter structure.
+ */
+
+#if defined (ARM_MATH_DSP)
+
+void arm_fir_decimate_fast_q15(
+ const arm_fir_decimate_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCur; /* Points to the current sample of the state */
+ q15_t *px; /* Temporary pointer for state buffer */
+ const q15_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t x0, x1, c0; /* Temporary variables to hold state and coefficient values */
+ q31_t sum0; /* Accumulators */
+ q31_t acc0, acc1;
+ q15_t *px0, *px1;
+ uint32_t blkCntN3;
+ uint32_t numTaps = S->numTaps; /* Number of taps */
+ uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t c1; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+ /* Total number of output samples to be computed */
+ blkCnt = outBlockSize / 2;
+ blkCntN3 = outBlockSize - (2 * blkCnt);
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 2 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 2;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0;
+ acc1 = 0;
+
+ /* Initialize state pointer for all the samples */
+ px0 = pState;
+ px1 = pState + S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] and b[numTaps-2] coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */
+ x0 = read_q15x2_ia (&px0);
+ x1 = read_q15x2_ia (&px1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc1 = __SMLAD(x1, c0, acc1);
+
+ /* Read the b[numTaps-3] and b[numTaps-4] coefficient */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
+ x0 = read_q15x2_ia (&px0);
+ x1 = read_q15x2_ia (&px1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc1 = __SMLAD(x1, c0, acc1);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch state variables for acc0, acc1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc1 = __SMLAD(x1, c0, acc1);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 2;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+ *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+ *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ while (blkCntN3 > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] and b[numTaps-2] coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Read x[n-numTaps-1] and x[n-numTaps-2] sample */
+ x0 = read_q15x2_ia (&px);
+
+ /* Read the b[numTaps-3] and b[numTaps-4] coefficients */
+ c1 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Perform the multiply-accumulate */
+ sum0 = __SMLAD(x0, c0, sum0);
+
+ /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
+ x0 = read_q15x2_ia (&px);
+
+ /* Perform the multiply-accumulate */
+ sum0 = __SMLAD(x0, c1, sum0);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 = __SMLAD(x0, c0, sum0);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+ *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCntN3--;
+ }
+
+ /* Processing is complete.
+ Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+ i = (numTaps - 1U) >> 2U;
+
+ /* copy data */
+ while (i > 0U)
+ {
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ i = (numTaps - 1U) % 0x04U;
+
+ /* Copy data */
+ while (i > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+}
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+void arm_fir_decimate_fast_q15(
+ const arm_fir_decimate_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCur; /* Points to the current sample of the state */
+ q15_t *px; /* Temporary pointer for state buffer */
+ const q15_t *pb; /* Temporary pointer for coefficient buffer */
+ q15_t x0, x1, c0; /* Temporary variables to hold state and coefficient values */
+ q31_t sum0; /* Accumulators */
+ q31_t acc0, acc1;
+ q15_t *px0, *px1;
+ uint32_t blkCntN3;
+ uint32_t numTaps = S->numTaps; /* Number of taps */
+ uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+ /* Total number of output samples to be computed */
+ blkCnt = outBlockSize / 2;
+ blkCntN3 = outBlockSize - (2 * blkCnt);
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 2 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 2;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0;
+ acc1 = 0;
+
+ /* Initialize state pointer */
+ px0 = pState;
+ px1 = pState + S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the Read b[numTaps-1] coefficients */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] for sample 0 and for sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] for sample 0 and sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Read the b[numTaps-3] coefficients */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] for sample 0 and sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] for sample 0 and sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 2;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+
+ *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+ *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ while (blkCntN3 > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+ *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCntN3--;
+ }
+
+ /* Processing is complete.
+ ** Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+ i = (numTaps - 1U) >> 2U;
+
+ /* copy data */
+ while (i > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ i = (numTaps - 1U) % 0x04U;
+
+ /* copy data */
+ while (i > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+}
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c new file mode 100644 index 0000000..f9a686d --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_fast_q31.c @@ -0,0 +1,390 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_fast_q31.c
+ * Description: Fast Q31 FIR Decimator
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 FIR decimator (fast variant).
+ @param[in] S points to an instance of the Q31 FIR decimator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This function is optimized for speed at the expense of fixed-point precision and overflow protection.
+ The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
+ These intermediate results are added to a 2.30 accumulator.
+ Finally, the accumulator is saturated and converted to a 1.31 result.
+ The fast version has the same overflow behavior as the standard version and provides less precision since it discards the low 32 bits of each multiplication result.
+ In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits (where log2 is read as log to the base 2).
+
+ @remark
+ Refer to \ref arm_fir_decimate_q31() for a slower implementation of this function which uses a 64-bit accumulator to provide higher precision.
+ Both the slow and the fast versions use the same instance structure.
+ Use function \ref arm_fir_decimate_init_q31() to initialize the filter structure.
+ */
+
+void arm_fir_decimate_fast_q31(
+ const arm_fir_decimate_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *pStateCur; /* Points to the current sample of the state */
+ q31_t *px0; /* Temporary pointer for state buffer */
+ const q31_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
+ q63_t acc0; /* Accumulator */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t *px1, *px2, *px3;
+ q31_t x1, x2, x3;
+ q63_t acc1, acc2, acc3;
+#endif
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 samples at a time */
+ blkCnt = outBlockSize >> 2U;
+
+ /* Samples loop unrolled by 4 */
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 4;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Initialize state pointer for all the samples */
+ px0 = pState;
+ px1 = pState + S->M;
+ px2 = pState + 2 * S->M;
+ px3 = pState + 3 * S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-1] sample for acc0 */
+ x0 = *(px0++);
+ /* Read x[n-numTaps-1] sample for acc1 */
+ x1 = *(px1++);
+ /* Read x[n-numTaps-1] sample for acc2 */
+ x2 = *(px2++);
+ /* Read x[n-numTaps-1] sample for acc3 */
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-2] sample for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-3] sample acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-4] sample acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch state variables for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+ acc1 = (q31_t) ((((q63_t) acc1 << 32) + ((q63_t) x1 * c0)) >> 32);
+ acc2 = (q31_t) ((((q63_t) acc2 << 32) + ((q63_t) x2 * c0)) >> 32);
+ acc3 = (q31_t) ((((q63_t) acc3 << 32) + ((q63_t) x3 * c0)) >> 32);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 4;
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst++ = (q31_t) (acc0 << 1);
+ *pDst++ = (q31_t) (acc1 << 1);
+ *pDst++ = (q31_t) (acc2 << 1);
+ *pDst++ = (q31_t) (acc3 << 1);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining samples */
+ blkCnt = outBlockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = outBlockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0;
+
+ /* Initialize state pointer */
+ px0 = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = (q31_t) ((((q63_t) acc0 << 32) + ((q63_t) x0 * c0)) >> 32);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* 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) (acc0 << 1);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete.
+ Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x04U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_f32.c new file mode 100644 index 0000000..851fba2 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_f32.c @@ -0,0 +1,105 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_init_f32.c
+ * Description: Floating-point FIR Decimator initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point FIR decimator.
+ @param[in,out] S points to an instance of the floating-point FIR decimator structure
+ @param[in] numTaps number of coefficients in the filter
+ @param[in] M decimation factor
+ @param[in] pCoeffs points to the filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of input samples to process per call
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_LENGTH_ERROR : <code>blockSize</code> is not a multiple of <code>M</code>
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize-1</code> words where <code>blockSize</code> is the number of input samples passed to <code>arm_fir_decimate_f32()</code>.
+ <code>M</code> is the decimation factor.
+ */
+
+arm_status arm_fir_decimate_init_f32(
+ arm_fir_decimate_instance_f32 * S,
+ uint16_t numTaps,
+ uint8_t M,
+ const float32_t * pCoeffs,
+ float32_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+ /* The size of the input block must be a multiple of the decimation factor */
+ if ((blockSize % M) != 0U)
+ {
+ /* Set status as ARM_MATH_LENGTH_ERROR */
+ status = ARM_MATH_LENGTH_ERROR;
+ }
+ else
+ {
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear the state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Decimation Factor */
+ S->M = M;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+
+}
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q15.c new file mode 100644 index 0000000..1c9894f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q15.c @@ -0,0 +1,106 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_init_q15.c
+ * Description: Initialization function for the Q15 FIR Decimator
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 FIR decimator.
+ @param[in,out] S points to an instance of the Q15 FIR decimator structure
+ @param[in] numTaps number of coefficients in the filter
+ @param[in] M decimation factor
+ @param[in] pCoeffs points to the filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of input samples to process
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_LENGTH_ERROR : <code>blockSize</code> is not a multiple of <code>M</code>
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize-1</code> words where <code>blockSize</code> is the number of input samples
+ to the call <code>arm_fir_decimate_q15()</code>.
+ <code>M</code> is the decimation factor.
+ */
+
+arm_status arm_fir_decimate_init_q15(
+ arm_fir_decimate_instance_q15 * S,
+ uint16_t numTaps,
+ uint8_t M,
+ const q15_t * pCoeffs,
+ q15_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+ /* The size of the input block must be a multiple of the decimation factor */
+ if ((blockSize % M) != 0U)
+ {
+ /* Set status as ARM_MATH_LENGTH_ERROR */
+ status = ARM_MATH_LENGTH_ERROR;
+ }
+ else
+ {
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear the state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Decimation Factor */
+ S->M = M;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+
+}
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q31.c new file mode 100644 index 0000000..6317ec8 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_init_q31.c @@ -0,0 +1,105 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_init_q31.c
+ * Description: Initialization function for Q31 FIR Decimation filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 FIR decimator.
+ @param[in,out] S points to an instance of the Q31 FIR decimator structure
+ @param[in] numTaps number of coefficients in the filter
+ @param[in] M decimation factor
+ @param[in] pCoeffs points to the filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of input samples to process
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_LENGTH_ERROR : <code>blockSize</code> is not a multiple of <code>M</code>
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize-1</code> words where <code>blockSize</code> is the number of input samples passed to <code>arm_fir_decimate_q31()</code>.
+ <code>M</code> is the decimation factor.
+ */
+
+arm_status arm_fir_decimate_init_q31(
+ arm_fir_decimate_instance_q31 * S,
+ uint16_t numTaps,
+ uint8_t M,
+ const q31_t * pCoeffs,
+ q31_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+ /* The size of the input block must be a multiple of the decimation factor */
+ if ((blockSize % M) != 0U)
+ {
+ /* Set status as ARM_MATH_LENGTH_ERROR */
+ status = ARM_MATH_LENGTH_ERROR;
+ }
+ else
+ {
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear the state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Decimation Factor */
+ S->M = M;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+
+}
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q15.c new file mode 100644 index 0000000..4179194 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q15.c @@ -0,0 +1,595 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_q15.c
+ * Description: Q15 FIR Decimator
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 FIR decimator.
+ @param[in] S points to an instance of the Q15 FIR decimator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of input samples to process per call
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+ Lastly, the accumulator is saturated to yield a result in 1.15 format.
+
+ @remark
+ Refer to \ref arm_fir_decimate_fast_q15() for a faster but less precise implementation of this function.
+ */
+
+#if defined (ARM_MATH_DSP)
+
+void arm_fir_decimate_q15(
+ const arm_fir_decimate_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCur; /* Points to the current sample of the state */
+ q15_t *px; /* Temporary pointer for state buffer */
+ const q15_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t x0, x1, c0; /* Temporary variables to hold state and coefficient values */
+ q63_t sum0; /* Accumulators */
+ q63_t acc0, acc1;
+ q15_t *px0, *px1;
+ uint32_t blkCntN3;
+ uint32_t numTaps = S->numTaps; /* Number of taps */
+ uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t c1; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+ /* Total number of output samples to be computed */
+ blkCnt = outBlockSize / 2;
+ blkCntN3 = outBlockSize - (2 * blkCnt);
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 2 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 2;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0;
+ acc1 = 0;
+
+ /* Initialize state pointer for all the samples */
+ px0 = pState;
+ px1 = pState + S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] and b[numTaps-2] coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */
+ x0 = read_q15x2_ia (&px0);
+ x1 = read_q15x2_ia (&px1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc1 = __SMLALD(x1, c0, acc1);
+
+ /* Read the b[numTaps-3] and b[numTaps-4] coefficient */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
+ x0 = read_q15x2_ia (&px0);
+ x1 = read_q15x2_ia (&px1);
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc1 = __SMLALD(x1, c0, acc1);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch state variables for acc0, acc1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc1 = __SMLALD(x1, c0, acc1);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 2;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+ *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+ *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ while (blkCntN3 > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] and b[numTaps-2] coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Read x[n-numTaps-1] and x[n-numTaps-2] sample */
+ x0 = read_q15x2_ia (&px);
+
+ /* Read the b[numTaps-3] and b[numTaps-4] coefficients */
+ c1 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Perform the multiply-accumulate */
+ sum0 = __SMLALD(x0, c0, sum0);
+
+ /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
+ x0 = read_q15x2_ia (&px);
+
+ /* Perform the multiply-accumulate */
+ sum0 = __SMLALD(x0, c1, sum0);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 = __SMLALD(x0, c0, sum0);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+ *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCntN3--;
+ }
+
+ /* Processing is complete.
+ Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+ i = (numTaps - 1U) >> 2U;
+
+ /* copy data */
+ while (i > 0U)
+ {
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ i = (numTaps - 1U) % 0x04U;
+
+ /* Copy data */
+ while (i > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+}
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+void arm_fir_decimate_q15(
+ const arm_fir_decimate_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCur; /* Points to the current sample of the state */
+ q15_t *px; /* Temporary pointer for state buffer */
+ const q15_t *pb; /* Temporary pointer for coefficient buffer */
+ q15_t x0, x1, c0; /* Temporary variables to hold state and coefficient values */
+ q63_t sum0; /* Accumulators */
+ q63_t acc0, acc1;
+ q15_t *px0, *px1;
+ uint32_t blkCntN3;
+ uint32_t numTaps = S->numTaps; /* Number of taps */
+ uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+ /* Total number of output samples to be computed */
+ blkCnt = outBlockSize / 2;
+ blkCntN3 = outBlockSize - (2 * blkCnt);
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 2 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 2;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0;
+ acc1 = 0;
+
+ /* Initialize state pointer */
+ px0 = pState;
+ px1 = pState + S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the Read b[numTaps-1] coefficients */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] for sample 0 and for sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] for sample 0 and sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Read the b[numTaps-3] coefficients */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] for sample 0 and sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] for sample 0 and sample 1 */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px0++;
+ x1 = *px1++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 2;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+
+ *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+ *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ while (blkCntN3 > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px++;
+
+ /* Perform the multiply-accumulate */
+ sum0 += x0 * c0;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M;
+
+ /* Store filter output, smlad returns the values in 2.14 format */
+ /* so downsacle by 15 to get output in 1.15 */
+ *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
+
+ /* Decrement loop counter */
+ blkCntN3--;
+ }
+
+ /* Processing is complete.
+ ** Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+ i = (numTaps - 1U) >> 2U;
+
+ /* copy data */
+ while (i > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ i = (numTaps - 1U) % 0x04U;
+
+ /* copy data */
+ while (i > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+}
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q31.c new file mode 100644 index 0000000..41cea04 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_decimate_q31.c @@ -0,0 +1,387 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_decimate_q31.c
+ * Description: Q31 FIR Decimator
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_decimate
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 FIR decimator.
+ @param[in] S points to an instance of the Q31 FIR decimator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits (where log2 is read as log to the base 2).
+ After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
+
+ @remark
+ Refer to \ref arm_fir_decimate_fast_q31() for a faster but less precise implementation of this function.
+ */
+
+void arm_fir_decimate_q31(
+ const arm_fir_decimate_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *pStateCur; /* Points to the current sample of the state */
+ q31_t *px0; /* Temporary pointer for state buffer */
+ const q31_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
+ q63_t acc0; /* Accumulator */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt, outBlockSize = blockSize / S->M; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t *px1, *px2, *px3;
+ q31_t x1, x2, x3;
+ q63_t acc1, acc2, acc3;
+#endif
+
+ /* S->pState buffer contains previous frame (numTaps - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (numTaps - 1U);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 samples at a time */
+ blkCnt = outBlockSize >> 2U;
+
+ /* Samples loop unrolled by 4 */
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 * decimation factor number of new input samples into the state buffer */
+ i = S->M * 4;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Initialize state pointer for all the samples */
+ px0 = pState;
+ px1 = pState + S->M;
+ px2 = pState + 2 * S->M;
+ px3 = pState + 3 * S->M;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-1] sample for acc0 */
+ x0 = *(px0++);
+ /* Read x[n-numTaps-1] sample for acc1 */
+ x1 = *(px1++);
+ /* Read x[n-numTaps-1] sample for acc2 */
+ x2 = *(px2++);
+ /* Read x[n-numTaps-1] sample for acc3 */
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-2] sample for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-3] sample acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-4] sample acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch state variables for acc0, acc1, acc2, acc3 */
+ x0 = *(px0++);
+ x1 = *(px1++);
+ x2 = *(px2++);
+ x3 = *(px3++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by the decimation factor
+ * to process the next group of decimation factor number samples */
+ pState = pState + S->M * 4;
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst++ = (q31_t) (acc0 >> 31);
+ *pDst++ = (q31_t) (acc1 >> 31);
+ *pDst++ = (q31_t) (acc2 >> 31);
+ *pDst++ = (q31_t) (acc3 >> 31);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining samples */
+ blkCnt = outBlockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = outBlockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy decimation factor number of new input samples into the state buffer */
+ i = S->M;
+
+ do
+ {
+ *pStateCur++ = *pSrc++;
+
+ } while (--i);
+
+ /* Set accumulator to zero */
+ acc0 = 0;
+
+ /* Initialize state pointer */
+ px0 = pState;
+
+ /* Initialize coeff pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-1] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-2] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-3] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *pb++;
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *pb++;
+
+ /* Fetch 1 state variable */
+ x0 = *px0++;
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* 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) (acc0 >> 31);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete.
+ Now copy the last numTaps - 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 */
+ pStateCur = S->pState;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x04U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_decimate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_f32.c new file mode 100644 index 0000000..8e5c777 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_f32.c @@ -0,0 +1,715 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_f32.c
+ * Description: Floating-point FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup FIR Finite Impulse Response (FIR) Filters
+
+ This set of functions implements Finite Impulse Response (FIR) filters
+ for Q7, Q15, Q31, and floating-point data types. Fast versions of Q15 and Q31 are also provided.
+ The functions operate on blocks of input and output data and each call to the function processes
+ <code>blockSize</code> samples through the filter. <code>pSrc</code> and
+ <code>pDst</code> points to input and output arrays containing <code>blockSize</code> values.
+
+ @par Algorithm
+ The FIR filter algorithm is based upon a sequence of multiply-accumulate (MAC) operations.
+ Each filter coefficient <code>b[n]</code> is multiplied by a state variable which equals a previous input sample <code>x[n]</code>.
+ <pre>
+ y[n] = b[0] * x[n] + b[1] * x[n-1] + b[2] * x[n-2] + ...+ b[numTaps-1] * x[n-numTaps+1]
+ </pre>
+ @par
+ \image html FIR.GIF "Finite Impulse Response filter"
+ @par
+ <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
+ Coefficients are stored in time reversed order.
+ @par
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to a state array of size <code>numTaps + blockSize - 1</code>.
+ Samples in the state buffer are stored in the following order.
+ @par
+ <pre>
+ {x[n-numTaps+1], x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2]....x[0], x[1], ..., x[blockSize-1]}
+ </pre>
+ @par
+ Note that the length of the state buffer exceeds the length of the coefficient array by <code>blockSize-1</code>.
+ The increased state buffer length allows circular addressing, which is traditionally used in the FIR filters,
+ to be avoided and yields a significant speed improvement.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
+ There are separate instance structure declarations for each of the 4 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numTaps, pCoeffs, pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros before static initialization.
+ The code below statically initializes each of the 4 different data type filter instance structures
+ <pre>
+ arm_fir_instance_f32 S = {numTaps, pState, pCoeffs};
+ arm_fir_instance_q31 S = {numTaps, pState, pCoeffs};
+ arm_fir_instance_q15 S = {numTaps, pState, pCoeffs};
+ arm_fir_instance_q7 S = {numTaps, pState, pCoeffs};
+ </pre>
+ where <code>numTaps</code> is the number of filter coefficients in the filter; <code>pState</code> is the address of the state buffer;
+ <code>pCoeffs</code> is the address of the coefficient buffer.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the fixed-point versions of the FIR filter functions.
+ In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ Refer to the function specific documentation below for usage guidelines.
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Processing function for floating-point FIR filter.
+ @param[in] S points to an instance of the floating-point FIR filter structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+#if defined(ARM_MATH_NEON)
+
+void arm_fir_f32(
+const arm_fir_instance_f32 * S,
+const float32_t * pSrc,
+float32_t * pDst,
+uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCurnt; /* Points to the current sample of the state */
+ float32_t *px; /* Temporary pointers for state buffer */
+ const float32_t *pb; /* Temporary pointers for coefficient buffer */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt; /* Loop counters */
+
+ float32x4_t accv0,accv1,samples0,samples1,x0,x1,x2,xa,xb,x,b,accv;
+ uint32x4_t x0_u,x1_u,x2_u,xa_u,xb_u;
+ float32_t acc;
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* Loop unrolling */
+ blkCnt = blockSize >> 3;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 8 samples at a time into state buffers */
+ samples0 = vld1q_f32(pSrc);
+ vst1q_f32(pStateCurnt,samples0);
+
+ pStateCurnt += 4;
+ pSrc += 4 ;
+
+ samples1 = vld1q_f32(pSrc);
+ vst1q_f32(pStateCurnt,samples1);
+
+ pStateCurnt += 4;
+ pSrc += 4 ;
+
+ /* Set the accumulators to zero */
+ accv0 = vdupq_n_f32(0);
+ accv1 = vdupq_n_f32(0);
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Loop unroling */
+ i = numTaps >> 2;
+
+ /* Perform the multiply-accumulates */
+ x0 = vld1q_f32(px);
+ x1 = vld1q_f32(px + 4);
+
+ while(i > 0)
+ {
+ /* acc = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
+ x2 = vld1q_f32(px + 8);
+ b = vld1q_f32(pb);
+ xa = x0;
+ xb = x1;
+ accv0 = vmlaq_n_f32(accv0,xa,b[0]);
+ accv1 = vmlaq_n_f32(accv1,xb,b[0]);
+
+ xa = vextq_f32(x0,x1,1);
+ xb = vextq_f32(x1,x2,1);
+
+ accv0 = vmlaq_n_f32(accv0,xa,b[1]);
+ accv1 = vmlaq_n_f32(accv1,xb,b[1]);
+
+ xa = vextq_f32(x0,x1,2);
+ xb = vextq_f32(x1,x2,2);
+
+ accv0 = vmlaq_n_f32(accv0,xa,b[2]);
+ accv1 = vmlaq_n_f32(accv1,xb,b[2]);
+
+ xa = vextq_f32(x0,x1,3);
+ xb = vextq_f32(x1,x2,3);
+
+ accv0 = vmlaq_n_f32(accv0,xa,b[3]);
+ accv1 = vmlaq_n_f32(accv1,xb,b[3]);
+
+ pb += 4;
+ x0 = x1;
+ x1 = x2;
+ px += 4;
+ i--;
+
+ }
+
+ /* Tail */
+ i = numTaps & 3;
+ x2 = vld1q_f32(px + 8);
+
+ /* Perform the multiply-accumulates */
+ switch(i)
+ {
+ case 3:
+ {
+ accv0 = vmlaq_n_f32(accv0,x0,*pb);
+ accv1 = vmlaq_n_f32(accv1,x1,*pb);
+
+ pb++;
+
+ xa = vextq_f32(x0,x1,1);
+ xb = vextq_f32(x1,x2,1);
+
+ accv0 = vmlaq_n_f32(accv0,xa,*pb);
+ accv1 = vmlaq_n_f32(accv1,xb,*pb);
+
+ pb++;
+
+ xa = vextq_f32(x0,x1,2);
+ xb = vextq_f32(x1,x2,2);
+
+ accv0 = vmlaq_n_f32(accv0,xa,*pb);
+ accv1 = vmlaq_n_f32(accv1,xb,*pb);
+
+ }
+ break;
+ case 2:
+ {
+ accv0 = vmlaq_n_f32(accv0,x0,*pb);
+ accv1 = vmlaq_n_f32(accv1,x1,*pb);
+
+ pb++;
+
+ xa = vextq_f32(x0,x1,1);
+ xb = vextq_f32(x1,x2,1);
+
+ accv0 = vmlaq_n_f32(accv0,xa,*pb);
+ accv1 = vmlaq_n_f32(accv1,xb,*pb);
+
+ }
+ break;
+ case 1:
+ {
+
+ accv0 = vmlaq_n_f32(accv0,x0,*pb);
+ accv1 = vmlaq_n_f32(accv1,x1,*pb);
+
+ }
+ break;
+ default:
+ break;
+ }
+
+ /* The result is stored in the destination buffer. */
+ vst1q_f32(pDst,accv0);
+ pDst += 4;
+ vst1q_f32(pDst,accv1);
+ pDst += 4;
+
+ /* Advance state pointer by 8 for the next 8 samples */
+ pState = pState + 8;
+
+ blkCnt--;
+ }
+
+ /* Tail */
+ blkCnt = blockSize & 0x7;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy one sample at a time into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc = 0.0f;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize Coefficient pointer */
+ pb = pCoeffs;
+
+ i = numTaps;
+
+ /* Perform the multiply-accumulates */
+ do
+ {
+ /* acc = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
+ acc += *px++ * *pb++;
+ i--;
+
+ } while (i > 0U);
+
+ /* The result is stored in the destination buffer. */
+ *pDst++ = acc;
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1;
+
+ blkCnt--;
+ }
+
+ /* Processing is complete.
+ ** Now copy the last numTaps - 1 samples to the starting 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 */
+ tapCnt = numTaps - 1U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+}
+#else
+void arm_fir_f32(
+ const arm_fir_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCurnt; /* Points to the current sample of the state */
+ float32_t *px; /* Temporary pointer for state buffer */
+ const float32_t *pb; /* Temporary pointer for coefficient buffer */
+ float32_t acc0; /* Accumulator */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ float32_t acc1, acc2, acc3, acc4, acc5, acc6, acc7; /* Accumulators */
+ float32_t x0, x1, x2, x3, x4, x5, x6, x7; /* Temporary variables to hold state values */
+ float32_t c0; /* Temporary variable to hold coefficient value */
+#endif
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 8 output values simultaneously.
+ * The variables acc0 ... acc7 hold output values that are being computed:
+ *
+ * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
+ * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
+ * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
+ * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
+ */
+
+ blkCnt = blockSize >> 3U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 new input samples into the state buffer. */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set all accumulators to zero */
+ acc0 = 0.0f;
+ acc1 = 0.0f;
+ acc2 = 0.0f;
+ acc3 = 0.0f;
+ acc4 = 0.0f;
+ acc5 = 0.0f;
+ acc6 = 0.0f;
+ acc7 = 0.0f;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* This is separated from the others to avoid
+ * a call to __aeabi_memmove which would be slower
+ */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Read the first 7 samples from the state buffer: x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2] */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+ x3 = *px++;
+ x4 = *px++;
+ x5 = *px++;
+ x6 = *px++;
+
+ /* Loop unrolling: process 8 taps at a time. */
+ tapCnt = numTaps >> 3U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-3] sample */
+ x7 = *(px++);
+
+ /* acc0 += b[numTaps-1] * x[n-numTaps] */
+ acc0 += x0 * c0;
+
+ /* acc1 += b[numTaps-1] * x[n-numTaps-1] */
+ acc1 += x1 * c0;
+
+ /* acc2 += b[numTaps-1] * x[n-numTaps-2] */
+ acc2 += x2 * c0;
+
+ /* acc3 += b[numTaps-1] * x[n-numTaps-3] */
+ acc3 += x3 * c0;
+
+ /* acc4 += b[numTaps-1] * x[n-numTaps-4] */
+ acc4 += x4 * c0;
+
+ /* acc1 += b[numTaps-1] * x[n-numTaps-5] */
+ acc5 += x5 * c0;
+
+ /* acc2 += b[numTaps-1] * x[n-numTaps-6] */
+ acc6 += x6 * c0;
+
+ /* acc3 += b[numTaps-1] * x[n-numTaps-7] */
+ acc7 += x7 * c0;
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *(px++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x1 * c0;
+ acc1 += x2 * c0;
+ acc2 += x3 * c0;
+ acc3 += x4 * c0;
+ acc4 += x5 * c0;
+ acc5 += x6 * c0;
+ acc6 += x7 * c0;
+ acc7 += x0 * c0;
+
+ /* Read the b[numTaps-3] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-5] sample */
+ x1 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += x2 * c0;
+ acc1 += x3 * c0;
+ acc2 += x4 * c0;
+ acc3 += x5 * c0;
+ acc4 += x6 * c0;
+ acc5 += x7 * c0;
+ acc6 += x0 * c0;
+ acc7 += x1 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-6] sample */
+ x2 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += x3 * c0;
+ acc1 += x4 * c0;
+ acc2 += x5 * c0;
+ acc3 += x6 * c0;
+ acc4 += x7 * c0;
+ acc5 += x0 * c0;
+ acc6 += x1 * c0;
+ acc7 += x2 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-6] sample */
+ x3 = *(px++);
+ /* Perform the multiply-accumulates */
+ acc0 += x4 * c0;
+ acc1 += x5 * c0;
+ acc2 += x6 * c0;
+ acc3 += x7 * c0;
+ acc4 += x0 * c0;
+ acc5 += x1 * c0;
+ acc6 += x2 * c0;
+ acc7 += x3 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-6] sample */
+ x4 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += x5 * c0;
+ acc1 += x6 * c0;
+ acc2 += x7 * c0;
+ acc3 += x0 * c0;
+ acc4 += x1 * c0;
+ acc5 += x2 * c0;
+ acc6 += x3 * c0;
+ acc7 += x4 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-6] sample */
+ x5 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += x6 * c0;
+ acc1 += x7 * c0;
+ acc2 += x0 * c0;
+ acc3 += x1 * c0;
+ acc4 += x2 * c0;
+ acc5 += x3 * c0;
+ acc6 += x4 * c0;
+ acc7 += x5 * c0;
+
+ /* Read the b[numTaps-4] coefficient */
+ c0 = *(pb++);
+
+ /* Read x[n-numTaps-6] sample */
+ x6 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += x7 * c0;
+ acc1 += x0 * c0;
+ acc2 += x1 * c0;
+ acc3 += x2 * c0;
+ acc4 += x3 * c0;
+ acc5 += x4 * c0;
+ acc6 += x5 * c0;
+ acc7 += x6 * c0;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = numTaps % 0x8U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch 1 state variable */
+ x7 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+ acc4 += x4 * c0;
+ acc5 += x5 * c0;
+ acc6 += x6 * c0;
+ acc7 += x7 * c0;
+
+ /* Reuse the present sample states for next sample */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+ x3 = x4;
+ x4 = x5;
+ x5 = x6;
+ x6 = x7;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by 8 to process the next group of 8 samples */
+ pState = pState + 8;
+
+ /* The results in the 8 accumulators, store in the destination buffer. */
+ *pDst++ = acc0;
+ *pDst++ = acc1;
+ *pDst++ = acc2;
+ *pDst++ = acc3;
+ *pDst++ = acc4;
+ *pDst++ = acc5;
+ *pDst++ = acc6;
+ *pDst++ = acc7;
+
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining output samples */
+ blkCnt = blockSize % 0x8U;
+
+#else
+
+ /* Initialize blkCnt with number of taps */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy one sample at a time into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc0 = 0.0f;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize Coefficient pointer */
+ pb = pCoeffs;
+
+ i = numTaps;
+
+ /* Perform the multiply-accumulates */
+ do
+ {
+ /* acc = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
+ acc0 += *px++ * *pb++;
+
+ i--;
+ } while (i > 0U);
+
+ /* Store result in destination buffer. */
+ *pDst++ = acc0;
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1U;
+
+ /* Decrement 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;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy remaining data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+#endif /* #if defined(ARM_MATH_NEON) */
+/**
+* @} end of FIR group
+*/
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q15.c new file mode 100644 index 0000000..c81c809 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q15.c @@ -0,0 +1,332 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_fast_q15.c
+ * Description: Q15 Fast FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 FIR filter (fast version).
+ @param[in] S points to an instance of the Q15 FIR filter structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This fast version uses a 32-bit accumulator with 2.30 format.
+ The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around and distorts the result.
+ In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits.
+ The 2.30 accumulator is then truncated to 2.15 format and saturated to yield the 1.15 result.
+
+ @remark
+ Refer to \ref arm_fir_q15() for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion. Both the slow and the fast versions use the same instance structure.
+ Use function \ref arm_fir_init_q15() to initialize the filter structure.
+ */
+
+void arm_fir_fast_q15(
+ const arm_fir_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCurnt; /* Points to the current sample of the state */
+ q15_t *px; /* Temporary pointer for state buffer */
+ const q15_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t acc0; /* Accumulators */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x0, x1, x2, c0; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 output values simultaneously.
+ * The variables acc0 ... acc3 hold output values that are being computed:
+ *
+ * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
+ * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
+ * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
+ * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
+ */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 new input samples into the state buffer. */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Typecast q15_t pointer to q31_t pointer for state reading in q31_t */
+ px = pState;
+
+ /* Typecast q15_t pointer to q31_t pointer for coefficient reading in q31_t */
+ pb = pCoeffs;
+
+ /* Read the first two samples from the state buffer: x[n-N], x[n-N-1] */
+ x0 = read_q15x2_ia (&px);
+
+ /* Read the third and forth samples from the state buffer: x[n-N-2], x[n-N-3] */
+ x2 = read_q15x2_ia (&px);
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-(numTaps%4) coefficients. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the first two coefficients using SIMD: b[N] and b[N-1] coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* acc0 += b[N] * x[n-N] + b[N-1] * x[n-N-1] */
+ acc0 = __SMLAD(x0, c0, acc0);
+
+ /* acc2 += b[N] * x[n-N-2] + b[N-1] * x[n-N-3] */
+ acc2 = __SMLAD(x2, c0, acc2);
+
+ /* pack x[n-N-1] and x[n-N-2] */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x2, x0, 0);
+#else
+ x1 = __PKHBT(x0, x2, 0);
+#endif
+
+ /* Read state x[n-N-4], x[n-N-5] */
+ x0 = read_q15x2_ia (&px);
+
+ /* acc1 += b[N] * x[n-N-1] + b[N-1] * x[n-N-2] */
+ acc1 = __SMLADX(x1, c0, acc1);
+
+ /* pack x[n-N-3] and x[n-N-4] */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x0, x2, 0);
+#else
+ x1 = __PKHBT(x2, x0, 0);
+#endif
+
+ /* acc3 += b[N] * x[n-N-3] + b[N-1] * x[n-N-4] */
+ acc3 = __SMLADX(x1, c0, acc3);
+
+ /* Read coefficients b[N-2], b[N-3] */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* acc0 += b[N-2] * x[n-N-2] + b[N-3] * x[n-N-3] */
+ acc0 = __SMLAD(x2, c0, acc0);
+
+ /* Read state x[n-N-6], x[n-N-7] with offset */
+ x2 = read_q15x2_ia (&px);
+
+ /* acc2 += b[N-2] * x[n-N-4] + b[N-3] * x[n-N-5] */
+ acc2 = __SMLAD(x0, c0, acc2);
+
+ /* acc1 += b[N-2] * x[n-N-3] + b[N-3] * x[n-N-4] */
+ acc1 = __SMLADX(x1, c0, acc1);
+
+ /* pack x[n-N-5] and x[n-N-6] */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x2, x0, 0);
+#else
+ x1 = __PKHBT(x0, x2, 0);
+#endif
+
+ /* acc3 += b[N-2] * x[n-N-5] + b[N-3] * x[n-N-6] */
+ acc3 = __SMLADX(x1, c0, acc3);
+
+ /* Decrement tap count */
+ tapCnt--;
+ }
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps.
+ This is always be 2 taps since the filter length is even. */
+ if ((numTaps & 0x3U) != 0U)
+ {
+ /* Read last two coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLAD(x0, c0, acc0);
+ acc2 = __SMLAD(x2, c0, acc2);
+
+ /* pack state variables */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x2, x0, 0);
+#else
+ x1 = __PKHBT(x0, x2, 0);
+#endif
+
+ /* Read last state variables */
+ x0 = read_q15x2 (px);
+
+ /* Perform the multiply-accumulates */
+ acc1 = __SMLADX(x1, c0, acc1);
+
+ /* pack state variables */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x0, x2, 0);
+#else
+ x1 = __PKHBT(x2, x0, 0);
+#endif
+
+ /* Perform the multiply-accumulates */
+ acc3 = __SMLADX(x1, c0, acc3);
+ }
+
+ /* The results in the 4 accumulators are in 2.30 format. Convert to 1.15 with saturation.
+ Then store the 4 outputs in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 4U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining output samples */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of taps */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy two samples into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc0 = 0;
+
+ /* Use SIMD to hold states and coefficients */
+ px = pState;
+ pb = pCoeffs;
+
+ tapCnt = numTaps >> 1U;
+
+ do
+ {
+ acc0 += (q31_t) *px++ * *pb++;
+ acc0 += (q31_t) *px++ * *pb++;
+
+ tapCnt--;
+ }
+ while (tapCnt > 0U);
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ Then store the output in the destination buffer. */
+ *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1U;
+
+ /* Decrement 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;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy remaining data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q31.c new file mode 100644 index 0000000..009e4e9 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_fast_q31.c @@ -0,0 +1,324 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_fast_q31.c
+ * Description: Processing function for the Q31 Fast FIR filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 FIR filter (fast version).
+ @param[in] S points to an instance of the Q31 structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ This function is optimized for speed at the expense of fixed-point precision and overflow protection.
+ The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
+ These intermediate results are added to a 2.30 accumulator.
+ Finally, the accumulator is saturated and converted to a 1.31 result.
+ The fast version has the same overflow behavior as the standard version and provides less precision since it discards the low 32 bits of each multiplication result.
+ In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits.
+
+ @remark
+ Refer to \ref arm_fir_q31() for a slower implementation of this function which uses a 64-bit accumulator to provide higher precision. Both the slow and the fast versions use the same instance structure.
+ Use function \ref arm_fir_init_q31() to initialize the filter structure.
+ */
+
+IAR_ONLY_LOW_OPTIMIZATION_ENTER
+void arm_fir_fast_q31(
+ const arm_fir_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *pStateCurnt; /* Points to the current sample of the state */
+ q31_t *px; /* Temporary pointer for state buffer */
+ const q31_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t acc0; /* Accumulators */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x0, x1, x2, x3, c0; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 output values simultaneously.
+ * The variables acc0 ... acc3 hold output values that are being computed:
+ *
+ * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
+ * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
+ * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
+ * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
+ */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 new input samples into the state buffer. */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Read the first 3 samples from the state buffer:
+ * x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2] */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Loop unrolling. Process 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-4 coefficients. */
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps] coefficient */
+ c0 = *pb;
+
+ /* Read x[n-numTaps-3] sample */
+ x3 = *px;
+
+ /* acc0 += b[numTaps] * x[n-numTaps] */
+ multAcc_32x32_keep32_R(acc0, x0, c0);
+
+ /* acc1 += b[numTaps] * x[n-numTaps-1] */
+ multAcc_32x32_keep32_R(acc1, x1, c0);
+
+ /* acc2 += b[numTaps] * x[n-numTaps-2] */
+ multAcc_32x32_keep32_R(acc2, x2, c0);
+
+ /* acc3 += b[numTaps] * x[n-numTaps-3] */
+ multAcc_32x32_keep32_R(acc3, x3, c0);
+
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *(pb + 1U);
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *(px + 1U);
+
+ /* Perform the multiply-accumulates */
+ multAcc_32x32_keep32_R(acc0, x1, c0);
+ multAcc_32x32_keep32_R(acc1, x2, c0);
+ multAcc_32x32_keep32_R(acc2, x3, c0);
+ multAcc_32x32_keep32_R(acc3, x0, c0);
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *(pb + 2U);
+
+ /* Read x[n-numTaps-5] sample */
+ x1 = *(px + 2U);
+
+ /* Perform the multiply-accumulates */
+ multAcc_32x32_keep32_R(acc0, x2, c0);
+ multAcc_32x32_keep32_R(acc1, x3, c0);
+ multAcc_32x32_keep32_R(acc2, x0, c0);
+ multAcc_32x32_keep32_R(acc3, x1, c0);
+
+ /* Read the b[numTaps-3] coefficients */
+ c0 = *(pb + 3U);
+
+ /* Read x[n-numTaps-6] sample */
+ x2 = *(px + 3U);
+
+ /* Perform the multiply-accumulates */
+ multAcc_32x32_keep32_R(acc0, x3, c0);
+ multAcc_32x32_keep32_R(acc1, x0, c0);
+ multAcc_32x32_keep32_R(acc2, x1, c0);
+ multAcc_32x32_keep32_R(acc3, x2, c0);
+
+ /* update coefficient pointer */
+ pb += 4U;
+ px += 4U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch 1 state variable */
+ x3 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ multAcc_32x32_keep32_R(acc0, x0, c0);
+ multAcc_32x32_keep32_R(acc1, x1, c0);
+ multAcc_32x32_keep32_R(acc2, x2, c0);
+ multAcc_32x32_keep32_R(acc3, x3, c0);
+
+ /* Reuse the present sample states for next sample */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The results in the 4 accumulators are in 2.30 format. Convert to 1.31
+ Then store the 4 outputs in the destination buffer. */
+ *pDst++ = (q31_t) (acc0 << 1);
+ *pDst++ = (q31_t) (acc1 << 1);
+ *pDst++ = (q31_t) (acc2 << 1);
+ *pDst++ = (q31_t) (acc3 << 1);
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 4U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining output samples */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of taps */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy one sample at a time into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize Coefficient pointer */
+ pb = pCoeffs;
+
+ i = numTaps;
+
+ /* Perform the multiply-accumulates */
+ do
+ {
+ multAcc_32x32_keep32_R(acc0, (*px++), (*pb++));
+ i--;
+ } while (i > 0U);
+
+ /* The result is in 2.30 format. Convert to 1.31
+ Then store the output in the destination buffer. */
+ *pDst++ = (q31_t) (acc0 << 1);
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1U;
+
+ /* Decrement 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;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy remaining data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+}
+IAR_ONLY_LOW_OPTIMIZATION_EXIT
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_f32.c new file mode 100644 index 0000000..8002400 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_f32.c @@ -0,0 +1,81 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_init_f32.c
+ * Description: Floating-point FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point FIR filter.
+ @param[in,out] S points to an instance of the floating-point FIR filter structure
+ @param[in] numTaps number of filter coefficients in the filter
+ @param[in] pCoeffs points to the filter coefficients buffer
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of samples processed per call
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize-1</code> samples, where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_f32()</code>.
+ */
+
+void arm_fir_init_f32(
+ arm_fir_instance_f32 * S,
+ uint16_t numTaps,
+ const float32_t * pCoeffs,
+ float32_t * pState,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q15.c new file mode 100644 index 0000000..66c69c0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q15.c @@ -0,0 +1,137 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_init_q15.c
+ * Description: Q15 FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 FIR filter.
+ @param[in,out] S points to an instance of the Q15 FIR filter structure.
+ @param[in] numTaps number of filter coefficients in the filter. Must be even and greater than or equal to 4.
+ @param[in] pCoeffs points to the filter coefficients buffer.
+ @param[in] pState points to the state buffer.
+ @param[in] blockSize number of samples processed per call.
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : <code>numTaps</code> is not greater than or equal to 4 and even
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ Note that <code>numTaps</code> must be even and greater than or equal to 4.
+ To implement an odd length filter simply increase <code>numTaps</code> by 1 and set the last coefficient to zero.
+ For example, to implement a filter with <code>numTaps=3</code> and coefficients
+ <pre>
+ {0.3, -0.8, 0.3}
+ </pre>
+ set <code>numTaps=4</code> and use the coefficients:
+ <pre>
+ {0.3, -0.8, 0.3, 0}.
+ </pre>
+ Similarly, to implement a two point filter
+ <pre>
+ {0.3, -0.3}
+ </pre>
+ set <code>numTaps=4</code> and use the coefficients:
+ <pre>
+ {0.3, -0.3, 0, 0}.
+ </pre>
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize</code>, when running on Cortex-M4 and Cortex-M3 and is of length <code>numTaps+blockSize-1</code>, when running on Cortex-M0 where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_q15()</code>.
+ */
+
+arm_status arm_fir_init_q15(
+ arm_fir_instance_q15 * S,
+ uint16_t numTaps,
+ const q15_t * pCoeffs,
+ q15_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+#if defined (ARM_MATH_DSP)
+
+ /* The Number of filter coefficients in the filter must be even and at least 4 */
+ if (numTaps & 0x1U)
+ {
+ status = ARM_MATH_ARGUMENT_ERROR;
+ }
+ else
+ {
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear the state buffer. The size is always (blockSize + numTaps ) */
+ memset(pState, 0, (numTaps + (blockSize)) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+
+#else
+
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ status = ARM_MATH_SUCCESS;
+
+ return (status);
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q31.c new file mode 100644 index 0000000..00de3e1 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q31.c @@ -0,0 +1,80 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_init_q31.c
+ * Description: Q31 FIR filter initialization function.
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 FIR filter.
+ @param[in,out] S points to an instance of the Q31 FIR filter structure
+ @param[in] numTaps number of filter coefficients in the filter
+ @param[in] pCoeffs points to the filter coefficients buffer
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of samples processed
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize-1</code> samples, where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_q31()</code>.
+ */
+
+void arm_fir_init_q31(
+ arm_fir_instance_q31 * S,
+ uint16_t numTaps,
+ const q31_t * pCoeffs,
+ q31_t * pState,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q7.c new file mode 100644 index 0000000..1adb2e2 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_init_q7.c @@ -0,0 +1,81 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_init_q7.c
+ * Description: Q7 FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q7 FIR filter.
+ @param[in,out] S points to an instance of the Q7 FIR filter structure
+ @param[in] numTaps number of filter coefficients in the filter
+ @param[in] pCoeffs points to the filter coefficients buffer
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of samples processed
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>numTaps+blockSize-1</code> samples, where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_q7()</code>.
+ */
+
+void arm_fir_init_q7(
+ arm_fir_instance_q7 * S,
+ uint16_t numTaps,
+ const q7_t * pCoeffs,
+ q7_t * pState,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer. The size is always (blockSize + numTaps - 1) */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q7_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_f32.c new file mode 100644 index 0000000..471f25f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_f32.c @@ -0,0 +1,914 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_interpolate_f32.c
+ * Description: Floating-point FIR interpolation sequences
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @defgroup FIR_Interpolate Finite Impulse Response (FIR) Interpolator
+
+ These functions combine an upsampler (zero stuffer) and an FIR filter.
+ They are used in multirate systems for increasing the sample rate of a signal without introducing high frequency images.
+ Conceptually, the functions are equivalent to the block diagram below:
+ \image html FIRInterpolator.gif "Components included in the FIR Interpolator functions"
+ After upsampling by a factor of <code>L</code>, the signal should be filtered by a lowpass filter with a normalized
+ cutoff frequency of <code>1/L</code> in order to eliminate high frequency copies of the spectrum.
+ The user of the function is responsible for providing the filter coefficients.
+
+ The FIR interpolator functions provided in the CMSIS DSP Library combine the upsampler and FIR filter in an efficient manner.
+ The upsampler inserts <code>L-1</code> zeros between each sample.
+ Instead of multiplying by these zero values, the FIR filter is designed to skip them.
+ This leads to an efficient implementation without any wasted effort.
+ The functions operate on blocks of input and output data.
+ <code>pSrc</code> points to an array of <code>blockSize</code> input values and
+ <code>pDst</code> points to an array of <code>blockSize*L</code> output values.
+
+ The library provides separate functions for Q15, Q31, and floating-point data types.
+
+ @par Algorithm
+ The functions use a polyphase filter structure:
+ <pre>
+ y[n] = b[0] * x[n] + b[L] * x[n-1] + ... + b[L*(phaseLength-1)] * x[n-phaseLength+1]
+ y[n+1] = b[1] * x[n] + b[L+1] * x[n-1] + ... + b[L*(phaseLength-1)+1] * x[n-phaseLength+1]
+ ...
+ y[n+(L-1)] = b[L-1] * x[n] + b[2*L-1] * x[n-1] + ....+ b[L*(phaseLength-1)+(L-1)] * x[n-phaseLength+1]
+ </pre>
+ This approach is more efficient than straightforward upsample-then-filter algorithms.
+ With this method the computation is reduced by a factor of <code>1/L</code> when compared to using a standard FIR filter.
+ @par
+ <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
+ <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code> and this is checked by the
+ initialization functions.
+ Internally, the function divides the FIR filter's impulse response into shorter filters of length
+ <code>phaseLength=numTaps/L</code>.
+ Coefficients are stored in time reversed order.
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to a state array of size <code>blockSize + phaseLength - 1</code>.
+ Samples in the state buffer are stored in the order:
+ <pre>
+ {x[n-phaseLength+1], x[n-phaseLength], x[n-phaseLength-1], x[n-phaseLength-2]....x[0], x[1], ..., x[blockSize-1]}
+ </pre>
+ @par
+ The state variables are updated after each block of data is processed, the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable array should be allocated separately.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ - Checks to make sure that the length of the filter is a multiple of the interpolation factor.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ L (interpolation factor), pCoeffs, phaseLength (numTaps / L), pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ The code below statically initializes each of the 3 different data type filter instance structures
+ <pre>
+ arm_fir_interpolate_instance_f32 S = {L, phaseLength, pCoeffs, pState};
+ arm_fir_interpolate_instance_q31 S = {L, phaseLength, pCoeffs, pState};
+ arm_fir_interpolate_instance_q15 S = {L, phaseLength, pCoeffs, pState};
+ </pre>
+ @par
+ where <code>L</code> is the interpolation factor; <code>phaseLength=numTaps/L</code> is the
+ length of each of the shorter FIR filters used internally,
+ <code>pCoeffs</code> is the address of the coefficient buffer;
+ <code>pState</code> is the address of the state buffer.
+ Be sure to set the values in the state buffer to zeros when doing static initialization.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the fixed-point versions of the FIR interpolate filter functions.
+ In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ Refer to the function specific documentation below for usage guidelines.
+ */
+
+/**
+ @addtogroup FIR_Interpolate
+ @{
+ */
+
+/**
+ @brief Processing function for floating-point FIR interpolator.
+ @param[in] S points to an instance of the floating-point FIR interpolator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+#if defined(ARM_MATH_NEON)
+void arm_fir_interpolate_f32(
+ const arm_fir_interpolate_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCurnt; /* Points to the current sample of the state */
+ float32_t *ptr1; /* Temporary pointers for state buffer */
+ const float32_t *ptr2; /* Temporary pointers for coefficient buffer */
+ float32_t sum0; /* Accumulators */
+ float32_t x0, c0; /* Temporary variables to hold state and coefficient values */
+ uint32_t i, blkCnt, j; /* Loop counters */
+ uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */
+ float32_t acc0, acc1, acc2, acc3;
+ float32_t x1, x2, x3;
+ uint32_t blkCntN4;
+ float32_t c1, c2, c3;
+
+ float32x4_t sum0v;
+ float32x4_t accV,accV0,accV1;
+ float32x4_t x0v,x1v,x2v,xa,xb;
+ uint32x4_t x0v_u,x1v_u,x2v_u,xa_u,xb_u;
+ float32x2_t tempV;
+
+ /* 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 - 1U);
+
+ /* Initialise blkCnt */
+ blkCnt = blockSize >> 3;
+ blkCntN4 = blockSize & 7;
+
+ /* Loop unrolling */
+ while (blkCnt > 0U)
+ {
+ /* Copy new input samples into the state buffer */
+ sum0v = vld1q_f32(pSrc);
+ vst1q_f32(pStateCurnt,sum0v);
+ pSrc += 4;
+ pStateCurnt += 4;
+
+ sum0v = vld1q_f32(pSrc);
+ vst1q_f32(pStateCurnt,sum0v);
+ pSrc += 4;
+ pStateCurnt += 4;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = (S->L);
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ accV0 = vdupq_n_f32(0.0);
+ accV1 = vdupq_n_f32(0.0);
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length. Unroll by a factor of 4.
+ ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
+ tapCnt = phaseLen >> 2U;
+
+ x0v = vld1q_f32(ptr1);
+ x1v = vld1q_f32(ptr1 + 4);
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input samples */
+ x2v = vld1q_f32(ptr1 + 8);
+
+ /* Read the coefficients */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ accV0 = vmlaq_n_f32(accV0,x0v,c0);
+ accV1 = vmlaq_n_f32(accV1,x1v,c0);
+
+ /* Read the coefficients, inputs and perform multiply-accumulate */
+ c1 = *(ptr2 + S->L);
+
+ xa = vextq_f32(x0v,x1v,1);
+ xb = vextq_f32(x1v,x2v,1);
+
+ accV0 = vmlaq_n_f32(accV0,xa,c1);
+ accV1 = vmlaq_n_f32(accV1,xb,c1);
+
+ /* Read the coefficients, inputs and perform multiply-accumulate */
+ c2 = *(ptr2 + S->L * 2);
+
+ xa = vextq_f32(x0v,x1v,2);
+ xb = vextq_f32(x1v,x2v,2);
+
+ accV0 = vmlaq_n_f32(accV0,xa,c2);
+ accV1 = vmlaq_n_f32(accV1,xb,c2);
+
+ /* Read the coefficients, inputs and perform multiply-accumulate */
+ c3 = *(ptr2 + S->L * 3);
+
+ xa = vextq_f32(x0v,x1v,3);
+ xb = vextq_f32(x1v,x2v,3);
+
+ accV0 = vmlaq_n_f32(accV0,xa,c3);
+ accV1 = vmlaq_n_f32(accV1,xb,c3);
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += 4 * S->L;
+ ptr1 += 4;
+ x0v = x1v;
+ x1v = x2v;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = phaseLen % 0x4U;
+
+ x2v = vld1q_f32(ptr1 + 8);
+
+ switch (tapCnt)
+ {
+ case 3:
+ c0 = *(ptr2);
+ accV0 = vmlaq_n_f32(accV0,x0v,c0);
+ accV1 = vmlaq_n_f32(accV1,x1v,c0);
+ ptr2 += S->L;
+
+ c0 = *(ptr2);
+
+ xa = vextq_f32(x0v,x1v,1);
+ xb = vextq_f32(x1v,x2v,1);
+
+ accV0 = vmlaq_n_f32(accV0,xa,c0);
+ accV1 = vmlaq_n_f32(accV1,xb,c0);
+ ptr2 += S->L;
+
+ c0 = *(ptr2);
+
+ xa = vextq_f32(x0v,x1v,2);
+ xb = vextq_f32(x1v,x2v,2);
+
+ accV0 = vmlaq_n_f32(accV0,xa,c0);
+ accV1 = vmlaq_n_f32(accV1,xb,c0);
+ ptr2 += S->L;
+
+ break;
+
+ case 2:
+ c0 = *(ptr2);
+ accV0 = vmlaq_n_f32(accV0,x0v,c0);
+ accV1 = vmlaq_n_f32(accV1,x1v,c0);
+ ptr2 += S->L;
+
+ c0 = *(ptr2);
+
+ xa = vextq_f32(x0v,x1v,1);
+ xb = vextq_f32(x1v,x2v,1);
+
+ accV0 = vmlaq_n_f32(accV0,xa,c0);
+ accV1 = vmlaq_n_f32(accV1,xb,c0);
+ ptr2 += S->L;
+
+ break;
+
+ case 1:
+ c0 = *(ptr2);
+ accV0 = vmlaq_n_f32(accV0,x0v,c0);
+ accV1 = vmlaq_n_f32(accV1,x1v,c0);
+ ptr2 += S->L;
+
+ break;
+
+ default:
+ break;
+
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst = accV0[0];
+ *(pDst + S->L) = accV0[1];
+ *(pDst + 2 * S->L) = accV0[2];
+ *(pDst + 3 * S->L) = accV0[3];
+
+ *(pDst + 4 * S->L) = accV1[0];
+ *(pDst + 5 * S->L) = accV1[1];
+ *(pDst + 6 * S->L) = accV1[2];
+ *(pDst + 7 * S->L) = accV1[3];
+
+ pDst++;
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* Decrement the loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 8;
+
+ pDst += S->L * 7;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+
+ /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
+ ** No loop unrolling is used. */
+
+ while (blkCntN4 > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0v = vdupq_n_f32(0.0);
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length. Unroll by a factor of 4.
+ ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
+ tapCnt = phaseLen >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the coefficient */
+ x1v[0] = *(ptr2);
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Read the input sample */
+ x0v = vld1q_f32(ptr1);
+ ptr1 += 4;
+
+ /* Read the coefficient */
+ x1v[1] = *(ptr2);
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Read the coefficient */
+ x1v[2] = *(ptr2);
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Read the coefficient */
+ x1v[3] = *(ptr2);
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ sum0v = vmlaq_f32(sum0v,x0v,x1v);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ tempV = vpadd_f32(vget_low_f32(sum0v),vget_high_f32(sum0v));
+ sum0 = tempV[0] + tempV[1];
+
+ /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = phaseLen % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += *(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++ = sum0;
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* 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 */
+ blkCntN4--;
+ }
+
+ /* 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) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ sum0v = vld1q_f32(pState);
+ vst1q_f32(pStateCurnt,sum0v);
+ pState += 4;
+ pStateCurnt += 4;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ tapCnt = (phaseLen - 1U) % 0x04U;
+
+ /* copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+}
+#else
+
+void arm_fir_interpolate_f32(
+ const arm_fir_interpolate_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCur; /* Points to the current sample of the state */
+ float32_t *ptr1; /* Temporary pointer for state buffer */
+ const float32_t *ptr2; /* Temporary pointer for coefficient buffer */
+ float32_t sum0; /* Accumulators */
+ uint32_t i, blkCnt, tapCnt; /* Loop counters */
+ uint32_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
+ uint32_t j;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ float32_t acc0, acc1, acc2, acc3;
+ float32_t x0, x1, x2, x3;
+ float32_t c0, c1, c2, c3;
+#endif
+
+ /* S->pState buffer contains previous frame (phaseLen - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (phaseLen - 1U);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = (S->L);
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ acc0 = 0.0f;
+ acc1 = 0.0f;
+ acc2 = 0.0f;
+ acc3 = 0.0f;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-(4*S->L) coefficients. */
+ tapCnt = phaseLen >> 2U;
+
+ x0 = *(ptr1++);
+ x1 = *(ptr1++);
+ x2 = *(ptr1++);
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input sample */
+ x3 = *(ptr1++);
+
+ /* Read the coefficient */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Read the coefficient */
+ c1 = *(ptr2 + S->L);
+
+ /* Read the input sample */
+ x0 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x1 * c1;
+ acc1 += x2 * c1;
+ acc2 += x3 * c1;
+ acc3 += x0 * c1;
+
+ /* Read the coefficient */
+ c2 = *(ptr2 + S->L * 2);
+
+ /* Read the input sample */
+ x1 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x2 * c2;
+ acc1 += x3 * c2;
+ acc2 += x0 * c2;
+ acc3 += x1 * c2;
+
+ /* Read the coefficient */
+ c3 = *(ptr2 + S->L * 3);
+
+ /* Read the input sample */
+ x2 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x3 * c3;
+ acc1 += x0 * c3;
+ acc2 += x1 * c3;
+ acc3 += x2 * c3;
+
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += 4 * S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = phaseLen % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input sample */
+ x3 = *(ptr1++);
+
+ /* Read the coefficient */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ acc0 += x0 * c0;
+ acc1 += x1 * c0;
+ acc2 += x2 * c0;
+ acc3 += x3 * c0;
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* update states for next sample processing */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *(pDst ) = acc0;
+ *(pDst + S->L) = acc1;
+ *(pDst + 2 * S->L) = acc2;
+ *(pDst + 3 * S->L) = acc3;
+
+ pDst++;
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 4;
+
+ pDst += S->L * 3;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0 = 0.0f;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length.
+ Repeat until we've computed numTaps-(4*S->L) coefficients. */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ tapCnt = phaseLen >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += *ptr1++ * *ptr2;
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ sum0 += *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ sum0 += *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ sum0 += *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = phaseLen % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = phaseLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += *ptr1++ * *ptr2;
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst++ = sum0;
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* 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 */
+ pStateCur = S->pState;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ tapCnt = (phaseLen - 1U) >> 2U;
+
+ /* copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = (phaseLen - 1U) % 0x04U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (phaseLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *pStateCur; /* Points to the current sample of the state */
+ float32_t *ptr1; /* Temporary pointer for state buffer */
+ const float32_t *ptr2; /* Temporary pointer for coefficient buffer */
+ float32_t sum0; /* Accumulators */
+ uint32_t i, blkCnt, tapCnt; /* Loop counters */
+ uint32_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
+
+ /* S->pState buffer contains previous frame (phaseLen - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (phaseLen - 1U);
+
+ /* Total number of intput samples */
+ blkCnt = blockSize;
+
+ /* Loop over the blockSize. */
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0 = 0.0f;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (i - 1U);
+
+ /* Loop over the polyPhase length */
+ tapCnt = phaseLen;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += *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++ = sum0;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 1;
+
+ /* Decrement 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 */
+ pStateCur = S->pState;
+
+ tapCnt = phaseLen - 1U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+#endif /* #if defined(ARM_MATH_NEON) */
+
+/**
+ @} end of FIR_Interpolate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c new file mode 100644 index 0000000..3309463 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_f32.c @@ -0,0 +1,106 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_interpolate_init_f32.c
+ * Description: Floating-point FIR interpolator initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Interpolate
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point FIR interpolator.
+ @param[in,out] S points to an instance of the floating-point FIR interpolator structure
+ @param[in] L upsample factor
+ @param[in] numTaps number of filter coefficients in the filter
+ @param[in] pCoeffs points to the filter coefficient buffer
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of input samples to process per call
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[numTaps-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ The length of the filter <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code>.
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>(numTaps/L)+blockSize-1</code> words
+ where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_interpolate_f32()</code>.
+ */
+
+arm_status arm_fir_interpolate_init_f32(
+ arm_fir_interpolate_instance_f32 * S,
+ uint8_t L,
+ uint16_t numTaps,
+ const float32_t * pCoeffs,
+ float32_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+ /* The filter length must be a multiple of the interpolation factor */
+ if ((numTaps % L) != 0U)
+ {
+ /* Set status as ARM_MATH_LENGTH_ERROR */
+ status = ARM_MATH_LENGTH_ERROR;
+ }
+ else
+ {
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign Interpolation factor */
+ S->L = L;
+
+ /* Assign polyPhaseLength */
+ S->phaseLength = numTaps / L;
+
+ /* Clear state buffer and size of buffer is always phaseLength + blockSize - 1 */
+ memset(pState, 0, (blockSize + ((uint32_t) S->phaseLength - 1U)) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+}
+
+/**
+ @} end of FIR_Interpolate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c new file mode 100644 index 0000000..bae10c7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q15.c @@ -0,0 +1,106 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_interpolate_init_q15.c
+ * Description: Q15 FIR interpolator initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Interpolate
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 FIR interpolator.
+ @param[in,out] S points to an instance of the Q15 FIR interpolator structure
+ @param[in] L upsample factor
+ @param[in] numTaps number of filter coefficients in the filter
+ @param[in] pCoeffs points to the filter coefficient buffer
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of input samples to process per call
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>
+
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[numTaps-2], ..., b[1], b[0]}
+ </pre>
+ The length of the filter <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code>.
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>(numTaps/L)+blockSize-1</code> words
+ where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_interpolate_q15()</code>.
+ */
+
+arm_status arm_fir_interpolate_init_q15(
+ arm_fir_interpolate_instance_q15 * S,
+ uint8_t L,
+ uint16_t numTaps,
+ const q15_t * pCoeffs,
+ q15_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+ /* The filter length must be a multiple of the interpolation factor */
+ if ((numTaps % L) != 0U)
+ {
+ /* Set status as ARM_MATH_LENGTH_ERROR */
+ status = ARM_MATH_LENGTH_ERROR;
+ }
+ else
+ {
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign Interpolation factor */
+ S->L = L;
+
+ /* Assign polyPhaseLength */
+ S->phaseLength = numTaps / L;
+
+ /* Clear state buffer and size of buffer is always phaseLength + blockSize - 1 */
+ memset(pState, 0, (blockSize + ((uint32_t) S->phaseLength - 1U)) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+}
+
+/**
+ @} end of FIR_Interpolate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c new file mode 100644 index 0000000..40c8600 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_init_q31.c @@ -0,0 +1,105 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_interpolate_init_q31.c
+ * Description: Q31 FIR interpolator initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Interpolate
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 FIR interpolator.
+ @param[in,out] S points to an instance of the Q31 FIR interpolator structure
+ @param[in] L upsample factor
+ @param[in] numTaps number of filter coefficients in the filter
+ @param[in] pCoeffs points to the filter coefficient buffer
+ @param[in] pState points to the state buffer
+ @param[in] blockSize number of input samples to process per call
+ @return execution status
+ - \ref ARM_MATH_SUCCESS : Operation successful
+ - \ref ARM_MATH_ARGUMENT_ERROR : filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[numTaps-2], ..., b[1], b[0]}
+ </pre>
+ The length of the filter <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code>.
+ @par
+ <code>pState</code> points to the array of state variables.
+ <code>pState</code> is of length <code>(numTaps/L)+blockSize-1</code> words
+ where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_fir_interpolate_q31()</code>.
+ */
+
+arm_status arm_fir_interpolate_init_q31(
+ arm_fir_interpolate_instance_q31 * S,
+ uint8_t L,
+ uint16_t numTaps,
+ const q31_t * pCoeffs,
+ q31_t * pState,
+ uint32_t blockSize)
+{
+ arm_status status;
+
+ /* The filter length must be a multiple of the interpolation factor */
+ if ((numTaps % L) != 0U)
+ {
+ /* Set status as ARM_MATH_LENGTH_ERROR */
+ status = ARM_MATH_LENGTH_ERROR;
+ }
+ else
+ {
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign Interpolation factor */
+ S->L = L;
+
+ /* Assign polyPhaseLength */
+ S->phaseLength = numTaps / L;
+
+ /* Clear state buffer and size of buffer is always phaseLength + blockSize - 1 */
+ memset(pState, 0, (blockSize + ((uint32_t) S->phaseLength - 1U)) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ status = ARM_MATH_SUCCESS;
+ }
+
+ return (status);
+}
+
+/**
+ @} end of FIR_Interpolate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q15.c new file mode 100644 index 0000000..ad5194a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q15.c @@ -0,0 +1,479 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_interpolate_q15.c
+ * Description: Q15 FIR interpolation
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Interpolate
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 FIR interpolator.
+ @param[in] S points to an instance of the Q15 FIR interpolator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+ Lastly, the accumulator is saturated to yield a result in 1.15 format.
+ */
+
+void arm_fir_interpolate_q15(
+ const arm_fir_interpolate_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCur; /* Points to the current sample of the state */
+ q15_t *ptr1; /* Temporary pointer for state buffer */
+ const q15_t *ptr2; /* Temporary pointer for coefficient buffer */
+ q63_t sum0; /* Accumulators */
+ uint32_t i, blkCnt, tapCnt; /* Loop counters */
+ uint32_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
+ uint32_t j;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc0, acc1, acc2, acc3;
+ q15_t x0, x1, x2, x3;
+ q15_t c0, c1, c2, c3;
+#endif
+
+ /* S->pState buffer contains previous frame (phaseLen - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (phaseLen - 1U);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = (S->L);
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-(4*S->L) coefficients. */
+ tapCnt = phaseLen >> 2U;
+
+ x0 = *(ptr1++);
+ x1 = *(ptr1++);
+ x2 = *(ptr1++);
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input sample */
+ x3 = *(ptr1++);
+
+ /* Read the coefficient */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Read the coefficient */
+ c1 = *(ptr2 + S->L);
+
+ /* Read the input sample */
+ x0 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x1 * c1;
+ acc1 += (q63_t) x2 * c1;
+ acc2 += (q63_t) x3 * c1;
+ acc3 += (q63_t) x0 * c1;
+
+ /* Read the coefficient */
+ c2 = *(ptr2 + S->L * 2);
+
+ /* Read the input sample */
+ x1 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x2 * c2;
+ acc1 += (q63_t) x3 * c2;
+ acc2 += (q63_t) x0 * c2;
+ acc3 += (q63_t) x1 * c2;
+
+ /* Read the coefficient */
+ c3 = *(ptr2 + S->L * 3);
+
+ /* Read the input sample */
+ x2 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x3 * c3;
+ acc1 += (q63_t) x0 * c3;
+ acc2 += (q63_t) x1 * c3;
+ acc3 += (q63_t) x2 * c3;
+
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += 4 * S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = phaseLen % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input sample */
+ x3 = *(ptr1++);
+
+ /* Read the coefficient */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* update states for next sample processing */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *(pDst ) = (q15_t) (__SSAT((acc0 >> 15), 16));
+ *(pDst + S->L) = (q15_t) (__SSAT((acc1 >> 15), 16));
+ *(pDst + 2 * S->L) = (q15_t) (__SSAT((acc2 >> 15), 16));
+ *(pDst + 3 * S->L) = (q15_t) (__SSAT((acc3 >> 15), 16));
+
+ pDst++;
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 4;
+
+ pDst += S->L * 3;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length.
+ Repeat until we've computed numTaps-(4*S->L) coefficients. */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ tapCnt = phaseLen >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = phaseLen % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = phaseLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* 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 */
+ pStateCur = S->pState;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ tapCnt = (phaseLen - 1U) >> 2U;
+
+ /* copy data */
+ while (tapCnt > 0U)
+ {
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = (phaseLen - 1U) % 0x04U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (phaseLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCur; /* Points to the current sample of the state */
+ q15_t *ptr1; /* Temporary pointer for state buffer */
+ const q15_t *ptr2; /* Temporary pointer for coefficient buffer */
+ q63_t sum0; /* Accumulators */
+ uint32_t i, blkCnt, tapCnt; /* Loop counters */
+ uint32_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
+
+ /* S->pState buffer contains previous frame (phaseLen - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (phaseLen - 1U);
+
+ /* Total number of intput samples */
+ blkCnt = blockSize;
+
+ /* Loop over the blockSize. */
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (i - 1U);
+
+ /* Loop over the polyPhase length */
+ tapCnt = phaseLen;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += ((q63_t) *ptr1++ * *ptr2);
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Store the result after converting to 1.15 format in the destination buffer. */
+ *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 1;
+
+ /* Decrement 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 */
+ pStateCur = S->pState;
+
+ tapCnt = phaseLen - 1U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of FIR_Interpolate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q31.c new file mode 100644 index 0000000..ee79cdb --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_interpolate_q31.c @@ -0,0 +1,481 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_interpolate_q31.c
+ * Description: Q31 FIR interpolation
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Interpolate
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 FIR interpolator.
+ @param[in] S points to an instance of the Q31 FIR interpolator structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by <code>1/(numTaps/L)</code>.
+ since <code>numTaps/L</code> additions occur per output sample.
+ After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
+ */
+
+void arm_fir_interpolate_q31(
+ const arm_fir_interpolate_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *pStateCur; /* Points to the current sample of the state */
+ q31_t *ptr1; /* Temporary pointer for state buffer */
+ const q31_t *ptr2; /* Temporary pointer for coefficient buffer */
+ q63_t sum0; /* Accumulators */
+ uint32_t i, blkCnt, tapCnt; /* Loop counters */
+ uint32_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
+ uint32_t j;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc0, acc1, acc2, acc3;
+ q31_t x0, x1, x2, x3;
+ q31_t c0, c1, c2, c3;
+#endif
+
+ /* S->pState buffer contains previous frame (phaseLen - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (phaseLen - 1U);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+ *pStateCur++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = (S->L);
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-(4*S->L) coefficients. */
+ tapCnt = phaseLen >> 2U;
+
+ x0 = *(ptr1++);
+ x1 = *(ptr1++);
+ x2 = *(ptr1++);
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input sample */
+ x3 = *(ptr1++);
+
+ /* Read the coefficient */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Read the coefficient */
+ c1 = *(ptr2 + S->L);
+
+ /* Read the input sample */
+ x0 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x1 * c1;
+ acc1 += (q63_t) x2 * c1;
+ acc2 += (q63_t) x3 * c1;
+ acc3 += (q63_t) x0 * c1;
+
+ /* Read the coefficient */
+ c2 = *(ptr2 + S->L * 2);
+
+ /* Read the input sample */
+ x1 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x2 * c2;
+ acc1 += (q63_t) x3 * c2;
+ acc2 += (q63_t) x0 * c2;
+ acc3 += (q63_t) x1 * c2;
+
+ /* Read the coefficient */
+ c3 = *(ptr2 + S->L * 3);
+
+ /* Read the input sample */
+ x2 = *(ptr1++);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x3 * c3;
+ acc1 += (q63_t) x0 * c3;
+ acc2 += (q63_t) x1 * c3;
+ acc3 += (q63_t) x2 * c3;
+
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += 4 * S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = phaseLen % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the input sample */
+ x3 = *(ptr1++);
+
+ /* Read the coefficient */
+ c0 = *(ptr2);
+
+ /* Perform the multiply-accumulate */
+ acc0 += (q63_t) x0 * c0;
+ acc1 += (q63_t) x1 * c0;
+ acc2 += (q63_t) x2 * c0;
+ acc3 += (q63_t) x3 * c0;
+
+ /* Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* update states for next sample processing */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *(pDst ) = (q31_t) (acc0 >> 31);
+ *(pDst + S->L) = (q31_t) (acc1 >> 31);
+ *(pDst + 2 * S->L) = (q31_t) (acc2 >> 31);
+ *(pDst + 3 * S->L) = (q31_t) (acc3 >> 31);
+
+ pDst++;
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 4;
+
+ pDst += S->L * 3;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+
+ /* Address modifier index of coefficient buffer */
+ j = 1U;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (S->L - j);
+
+ /* Loop over the polyPhase length.
+ Repeat until we've computed numTaps-(4*S->L) coefficients. */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ tapCnt = phaseLen >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+ ptr2 += S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = phaseLen % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = phaseLen;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += (q63_t) *ptr1++ * *ptr2;
+
+ /* Upsampling is done by stuffing L-1 zeros between each sample.
+ * So instead of multiplying zeros with coefficients,
+ * Increment the coefficient pointer by interpolation factor times. */
+ ptr2 += S->L;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The result is in the accumulator, store in the destination buffer. */
+ *pDst++ = (q31_t) (sum0 >> 31);
+
+ /* Increment the address modifier index of coefficient buffer */
+ j++;
+
+ /* 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 */
+ pStateCur = S->pState;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ tapCnt = (phaseLen - 1U) >> 2U;
+
+ /* copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = (phaseLen - 1U) % 0x04U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (phaseLen - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *pStateCur; /* Points to the current sample of the state */
+ q31_t *ptr1; /* Temporary pointer for state buffer */
+ const q31_t *ptr2; /* Temporary pointer for coefficient buffer */
+ q63_t sum0; /* Accumulators */
+ uint32_t i, blkCnt, tapCnt; /* Loop counters */
+ uint32_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
+
+ /* S->pState buffer contains previous frame (phaseLen - 1) samples */
+ /* pStateCur points to the location where the new input data should be written */
+ pStateCur = S->pState + (phaseLen - 1U);
+
+ /* Total number of intput samples */
+ blkCnt = blockSize;
+
+ /* Loop over the blockSize. */
+ while (blkCnt > 0U)
+ {
+ /* Copy new input sample into the state buffer */
+ *pStateCur++ = *pSrc++;
+
+ /* Loop over the Interpolation factor. */
+ i = S->L;
+
+ while (i > 0U)
+ {
+ /* Set accumulator to zero */
+ sum0 = 0;
+
+ /* Initialize state pointer */
+ ptr1 = pState;
+
+ /* Initialize coefficient pointer */
+ ptr2 = pCoeffs + (i - 1U);
+
+ /* Loop over the polyPhase length */
+ tapCnt = phaseLen;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum0 += ((q63_t) *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++ = (q31_t) (sum0 >> 31);
+
+ /* Decrement loop counter */
+ i--;
+ }
+
+ /* Advance the state pointer by 1
+ * to process the next group of interpolation factor number samples */
+ pState = pState + 1;
+
+ /* Decrement 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 */
+ pStateCur = S->pState;
+
+ tapCnt = phaseLen - 1U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of FIR_Interpolate group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_f32.c new file mode 100644 index 0000000..8577fee --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_f32.c @@ -0,0 +1,453 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_lattice_f32.c
+ * Description: Processing function for floating-point FIR Lattice filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup FIR_Lattice Finite Impulse Response (FIR) Lattice Filters
+
+ This set of functions implements Finite Impulse Response (FIR) lattice filters
+ for Q15, Q31 and floating-point data types. Lattice filters are used in a
+ variety of adaptive filter applications. The filter structure is feedforward and
+ the net impulse response is finite length.
+ The functions operate on blocks
+ of input and output data and each call to the function processes
+ <code>blockSize</code> samples through the filter. <code>pSrc</code> and
+ <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
+
+ @par Algorithm
+ \image html FIRLattice.gif "Finite Impulse Response Lattice filter"
+ The following difference equation is implemented:
+ @par
+ <pre>
+ f0[n] = g0[n] = x[n]
+ fm[n] = fm-1[n] + km * gm-1[n-1] for m = 1, 2, ...M
+ gm[n] = km * fm-1[n] + gm-1[n-1] for m = 1, 2, ...M
+ y[n] = fM[n]
+ </pre>
+ @par
+ <code>pCoeffs</code> points to tha array of reflection coefficients of size <code>numStages</code>.
+ Reflection Coefficients are stored in the following order.
+ @par
+ <pre>
+ {k1, k2, ..., kM}
+ </pre>
+ where M is number of stages
+ @par
+ <code>pState</code> points to a state array of size <code>numStages</code>.
+ The state variables (g values) hold previous inputs and are stored in the following order.
+ <pre>
+ {g0[n], g1[n], g2[n] ...gM-1[n]}
+ </pre>
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numStages, pCoeffs, pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
+ <pre>
+ arm_fir_lattice_instance_f32 S = {numStages, pState, pCoeffs};
+ arm_fir_lattice_instance_q31 S = {numStages, pState, pCoeffs};
+ arm_fir_lattice_instance_q15 S = {numStages, pState, pCoeffs};
+ </pre>
+ @par
+ where <code>numStages</code> is the number of stages in the filter;
+ <code>pState</code> is the address of the state buffer;
+ <code>pCoeffs</code> is the address of the coefficient buffer.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the fixed-point versions of the FIR Lattice filter functions.
+ In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ Refer to the function specific documentation below for usage guidelines.
+ */
+
+/**
+ @addtogroup FIR_Lattice
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point FIR lattice filter.
+ @param[in] S points to an instance of the floating-point FIR lattice structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+void arm_fir_lattice_f32(
+ const arm_fir_lattice_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *px; /* Temporary state pointer */
+ const float32_t *pk; /* Temporary coefficient pointer */
+ uint32_t numStages = S->numStages; /* Number of stages in the filter */
+ uint32_t blkCnt, stageCnt; /* Loop counters */
+ float32_t fcurr0, fnext0, gnext0, gcurr0; /* Temporary variables */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ float32_t fcurr1, fnext1, gnext1; /* Temporary variables for second sample in loop unrolling */
+ float32_t fcurr2, fnext2, gnext2; /* Temporary variables for third sample in loop unrolling */
+ float32_t fcurr3, fnext3, gnext3; /* Temporary variables for fourth sample in loop unrolling */
+#endif
+
+ gcurr0 = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Read two samples from input buffer */
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+ fcurr1 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* Read g0(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* Process first sample for first tap */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (gcurr0 * (*pk)) + fcurr0;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (fcurr0 * (*pk)) + gcurr0;
+
+ /* Process second sample for first tap */
+ fnext1 = (fcurr0 * (*pk)) + fcurr1;
+ gnext1 = (fcurr1 * (*pk)) + fcurr0;
+
+ /* Read next two samples from input buffer */
+ /* f0(n+2) = x(n+2) */
+ fcurr2 = *pSrc++;
+ fcurr3 = *pSrc++;
+
+ /* Process third sample for first tap */
+ fnext2 = (fcurr1 * (*pk)) + fcurr2;
+ gnext2 = (fcurr2 * (*pk)) + fcurr1;
+
+ /* Process fourth sample for first tap */
+ fnext3 = (fcurr2 * (*pk )) + fcurr3;
+ gnext3 = (fcurr3 * (*pk++)) + fcurr2;
+
+ /* Copy only last input sample into the state buffer
+ which will be used for next samples processing */
+ *px++ = fcurr3;
+
+ /* Update of f values for next coefficient set processing */
+ fcurr0 = fnext0;
+ fcurr1 = fnext1;
+ fcurr2 = fnext2;
+ fcurr3 = fnext3;
+
+ /* Loop unrolling. Process 4 taps at a time . */
+ stageCnt = (numStages - 1U) >> 2U;
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numStages-3 coefficients. */
+
+ /* Process 2nd, 3rd, 4th and 5th taps ... here */
+ while (stageCnt > 0U)
+ {
+ /* Read g1(n-1), g3(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Process first sample for 2nd, 6th .. tap */
+ /* Sample processing for K2, K6.... */
+ /* f2(n) = f1(n) + K2 * g1(n-1) */
+ fnext0 = (gcurr0 * (*pk)) + fcurr0;
+
+ /* Process second sample for 2nd, 6th .. tap */
+ /* for sample 2 processing */
+ fnext1 = (gnext0 * (*pk)) + fcurr1;
+
+ /* Process third sample for 2nd, 6th .. tap */
+ fnext2 = (gnext1 * (*pk)) + fcurr2;
+
+ /* Process fourth sample for 2nd, 6th .. tap */
+ fnext3 = (gnext2 * (*pk)) + fcurr3;
+
+ /* g2(n) = f1(n) * K2 + g1(n-1) */
+ /* Calculation of state values for next stage */
+ gnext3 = (fcurr3 * (*pk)) + gnext2;
+
+ gnext2 = (fcurr2 * (*pk)) + gnext1;
+
+ gnext1 = (fcurr1 * (*pk)) + gnext0;
+
+ gnext0 = (fcurr0 * (*pk++)) + gcurr0;
+
+
+ /* Read g2(n-1), g4(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g2(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Sample processing for K3, K7.... */
+ /* Process first sample for 3rd, 7th .. tap */
+ /* f3(n) = f2(n) + K3 * g2(n-1) */
+ fcurr0 = (gcurr0 * (*pk)) + fnext0;
+
+ /* Process second sample for 3rd, 7th .. tap */
+ fcurr1 = (gnext0 * (*pk)) + fnext1;
+
+ /* Process third sample for 3rd, 7th .. tap */
+ fcurr2 = (gnext1 * (*pk)) + fnext2;
+
+ /* Process fourth sample for 3rd, 7th .. tap */
+ fcurr3 = (gnext2 * (*pk)) + fnext3;
+
+ /* Calculation of state values for next stage */
+ /* g3(n) = f2(n) * K3 + g2(n-1) */
+ gnext3 = (fnext3 * (*pk)) + gnext2;
+
+ gnext2 = (fnext2 * (*pk)) + gnext1;
+
+ gnext1 = (fnext1 * (*pk)) + gnext0;
+
+ gnext0 = (fnext0 * (*pk++)) + gcurr0;
+
+
+ /* Read g1(n-1), g3(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g3(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Sample processing for K4, K8.... */
+ /* Process first sample for 4th, 8th .. tap */
+ /* f4(n) = f3(n) + K4 * g3(n-1) */
+ fnext0 = (gcurr0 * (*pk)) + fcurr0;
+
+ /* Process second sample for 4th, 8th .. tap */
+ /* for sample 2 processing */
+ fnext1 = (gnext0 * (*pk)) + fcurr1;
+
+ /* Process third sample for 4th, 8th .. tap */
+ fnext2 = (gnext1 * (*pk)) + fcurr2;
+
+ /* Process fourth sample for 4th, 8th .. tap */
+ fnext3 = (gnext2 * (*pk)) + fcurr3;
+
+ /* g4(n) = f3(n) * K4 + g3(n-1) */
+ /* Calculation of state values for next stage */
+ gnext3 = (fcurr3 * (*pk)) + gnext2;
+
+ gnext2 = (fcurr2 * (*pk)) + gnext1;
+
+ gnext1 = (fcurr1 * (*pk)) + gnext0;
+
+ gnext0 = (fcurr0 * (*pk++)) + gcurr0;
+
+
+ /* Read g2(n-1), g4(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g4(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Sample processing for K5, K9.... */
+ /* Process first sample for 5th, 9th .. tap */
+ /* f5(n) = f4(n) + K5 * g4(n-1) */
+ fcurr0 = (gcurr0 * (*pk)) + fnext0;
+
+ /* Process second sample for 5th, 9th .. tap */
+ fcurr1 = (gnext0 * (*pk)) + fnext1;
+
+ /* Process third sample for 5th, 9th .. tap */
+ fcurr2 = (gnext1 * (*pk)) + fnext2;
+
+ /* Process fourth sample for 5th, 9th .. tap */
+ fcurr3 = (gnext2 * (*pk)) + fnext3;
+
+ /* Calculation of state values for next stage */
+ /* g5(n) = f4(n) * K5 + g4(n-1) */
+ gnext3 = (fnext3 * (*pk)) + gnext2;
+
+ gnext2 = (fnext2 * (*pk)) + gnext1;
+
+ gnext1 = (fnext1 * (*pk)) + gnext0;
+
+ gnext0 = (fnext0 * (*pk++)) + gcurr0;
+
+ stageCnt--;
+ }
+
+ /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
+ stageCnt = (numStages - 1U) % 0x4U;
+
+ while (stageCnt > 0U)
+ {
+ gcurr0 = *px;
+
+ /* save g value in state buffer */
+ *px++ = gnext3;
+
+ /* Process four samples for last three taps here */
+ fnext0 = (gcurr0 * (*pk)) + fcurr0;
+
+ fnext1 = (gnext0 * (*pk)) + fcurr1;
+
+ fnext2 = (gnext1 * (*pk)) + fcurr2;
+
+ fnext3 = (gnext2 * (*pk)) + fcurr3;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext3 = (fcurr3 * (*pk)) + gnext2;
+
+ gnext2 = (fcurr2 * (*pk)) + gnext1;
+
+ gnext1 = (fcurr1 * (*pk)) + gnext0;
+
+ gnext0 = (fcurr0 * (*pk++)) + gcurr0;
+
+ /* Update of f values for next coefficient set processing */
+ fcurr0 = fnext0;
+ fcurr1 = fnext1;
+ fcurr2 = fnext2;
+ fcurr3 = fnext3;
+
+ stageCnt--;
+ }
+
+ /* The results in the 4 accumulators, store in the destination buffer. */
+ /* y(n) = fN(n) */
+ *pDst++ = fcurr0;
+ *pDst++ = fcurr1;
+ *pDst++ = fcurr2;
+ *pDst++ = fcurr3;
+
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* read g2(n) from state buffer */
+ gcurr0 = *px;
+
+ /* for sample 1 processing */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (gcurr0 * (*pk)) + fcurr0;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (fcurr0 * (*pk++)) + gcurr0;
+
+ /* save g1(n) in state buffer */
+ *px++ = fcurr0;
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt = (numStages - 1U);
+
+ /* stage loop */
+ while (stageCnt > 0U)
+ {
+ /* read g2(n) from state buffer */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = gnext0;
+
+ /* Sample processing for K2, K3.... */
+ /* f2(n) = f1(n) + K2 * g1(n-1) */
+ fnext0 = (gcurr0 * (*pk)) + fcurr0;
+
+ /* g2(n) = f1(n) * K2 + g1(n-1) */
+ gnext0 = (fcurr0 * (*pk++)) + gcurr0;
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt--;
+ }
+
+ /* y(n) = fN(n) */
+ *pDst++ = fcurr0;
+
+ blkCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_f32.c new file mode 100644 index 0000000..c6aade1 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_f32.c @@ -0,0 +1,70 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_lattice_init_f32.c
+ * Description: Floating-point FIR Lattice filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Lattice
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point FIR lattice filter.
+ @param[in] S points to an instance of the floating-point FIR lattice structure
+ @param[in] numStages number of filter stages
+ @param[in] pCoeffs points to the coefficient buffer. The array is of length numStages
+ @param[in] pState points to the state buffer. The array is of length numStages
+ @return none
+ */
+
+void arm_fir_lattice_init_f32(
+ arm_fir_lattice_instance_f32 * S,
+ uint16_t numStages,
+ const float32_t * pCoeffs,
+ float32_t * pState)
+{
+ /* Assign filter taps */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always numStages */
+ memset(pState, 0, (numStages) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q15.c new file mode 100644 index 0000000..1cdfea3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q15.c @@ -0,0 +1,70 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_lattice_init_q15.c
+ * Description: Q15 FIR Lattice filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Lattice
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 FIR lattice filter.
+ @param[in] S points to an instance of the Q15 FIR lattice structure
+ @param[in] numStages number of filter stages
+ @param[in] pCoeffs points to the coefficient buffer. The array is of length numStages
+ @param[in] pState points to the state buffer. The array is of length numStages
+ @return none
+ */
+
+void arm_fir_lattice_init_q15(
+ arm_fir_lattice_instance_q15 * S,
+ uint16_t numStages,
+ const q15_t * pCoeffs,
+ q15_t * pState)
+{
+ /* Assign filter taps */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always numStages */
+ memset(pState, 0, (numStages) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q31.c new file mode 100644 index 0000000..0aef4ea --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_init_q31.c @@ -0,0 +1,70 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_lattice_init_q31.c
+ * Description: Q31 FIR lattice filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Lattice
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 FIR lattice filter.
+ @param[in] S points to an instance of the Q31 FIR lattice structure
+ @param[in] numStages number of filter stages
+ @param[in] pCoeffs points to the coefficient buffer. The array is of length numStages
+ @param[in] pState points to the state buffer. The array is of length numStages
+ @return none
+ */
+
+void arm_fir_lattice_init_q31(
+ arm_fir_lattice_instance_q31 * S,
+ uint16_t numStages,
+ const q31_t * pCoeffs,
+ q31_t * pState)
+{
+ /* Assign filter taps */
+ S->numStages = numStages;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always numStages */
+ memset(pState, 0, (numStages) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q15.c new file mode 100644 index 0000000..59a000b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q15.c @@ -0,0 +1,506 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_lattice_q15.c
+ * Description: Q15 FIR lattice filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Lattice
+ @{
+ */
+
+/**
+ @brief Processing function for Q15 FIR lattice filter.
+ @param[in] S points to an instance of the Q15 FIR lattice structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+void arm_fir_lattice_q15(
+ const arm_fir_lattice_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *px; /* Temporary state pointer */
+ const q15_t *pk; /* Temporary coefficient pointer */
+ uint32_t numStages = S->numStages; /* Number of stages in the filter */
+ uint32_t blkCnt, stageCnt; /* Loop counters */
+ q31_t fcurr0, fnext0, gnext0, gcurr0; /* Temporary variables */
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t fcurr1, fnext1, gnext1; /* Temporary variables for second sample in loop unrolling */
+ q31_t fcurr2, fnext2, gnext2; /* Temporary variables for third sample in loop unrolling */
+ q31_t fcurr3, fnext3, gnext3; /* Temporary variables for fourth sample in loop unrolling */
+#endif
+
+ gcurr0 = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Read two samples from input buffer */
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+ fcurr1 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* Read g0(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* Process first sample for first tap */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (q31_t) ((gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (q31_t) ((fcurr0 * (*pk)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* Process second sample for first tap */
+ fnext1 = (q31_t) ((fcurr0 * (*pk)) >> 15U) + fcurr1;
+ fnext1 = __SSAT(fnext1, 16);
+ gnext1 = (q31_t) ((fcurr1 * (*pk)) >> 15U) + fcurr0;
+ gnext1 = __SSAT(gnext1, 16);
+
+ /* Read next two samples from input buffer */
+ /* f0(n+2) = x(n+2) */
+ fcurr2 = *pSrc++;
+ fcurr3 = *pSrc++;
+
+ /* Process third sample for first tap */
+ fnext2 = (q31_t) ((fcurr1 * (*pk)) >> 15U) + fcurr2;
+ fnext2 = __SSAT(fnext2, 16);
+ gnext2 = (q31_t) ((fcurr2 * (*pk)) >> 15U) + fcurr1;
+ gnext2 = __SSAT(gnext2, 16);
+
+ /* Process fourth sample for first tap */
+ fnext3 = (q31_t) ((fcurr2 * (*pk )) >> 15U) + fcurr3;
+ fnext3 = __SSAT(fnext3, 16);
+ gnext3 = (q31_t) ((fcurr3 * (*pk++)) >> 15U) + fcurr2;
+ gnext3 = __SSAT(gnext3, 16);
+
+ /* Copy only last input sample into the state buffer
+ which will be used for next samples processing */
+ *px++ = (q15_t) fcurr3;
+
+ /* Update of f values for next coefficient set processing */
+ fcurr0 = fnext0;
+ fcurr1 = fnext1;
+ fcurr2 = fnext2;
+ fcurr3 = fnext3;
+
+ /* Loop unrolling. Process 4 taps at a time . */
+ stageCnt = (numStages - 1U) >> 2U;
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numStages-3 coefficients. */
+
+ /* Process 2nd, 3rd, 4th and 5th taps ... here */
+ while (stageCnt > 0U)
+ {
+ /* Read g1(n-1), g3(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = (q15_t) gnext3;
+
+ /* Process first sample for 2nd, 6th .. tap */
+ /* Sample processing for K2, K6.... */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (q31_t) ((gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ /* Process second sample for 2nd, 6th .. tap */
+ /* for sample 2 processing */
+ fnext1 = (q31_t) ((gnext0 * (*pk)) >> 15U) + fcurr1;
+ fnext1 = __SSAT(fnext1, 16);
+
+ /* Process third sample for 2nd, 6th .. tap */
+ fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fcurr2;
+ fnext2 = __SSAT(fnext2, 16);
+
+ /* Process fourth sample for 2nd, 6th .. tap */
+ fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fcurr3;
+ fnext3 = __SSAT(fnext3, 16);
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ /* Calculation of state values for next stage */
+ gnext3 = (q31_t) ((fcurr3 * (*pk)) >> 15U) + gnext2;
+ gnext3 = __SSAT(gnext3, 16);
+
+ gnext2 = (q31_t) ((fcurr2 * (*pk)) >> 15U) + gnext1;
+ gnext2 = __SSAT(gnext2, 16);
+
+ gnext1 = (q31_t) ((fcurr1 * (*pk)) >> 15U) + gnext0;
+ gnext1 = __SSAT(gnext1, 16);
+
+ gnext0 = (q31_t) ((fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+
+ /* Read g2(n-1), g4(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = (q15_t) gnext3;
+
+ /* Sample processing for K3, K7.... */
+ /* Process first sample for 3rd, 7th .. tap */
+ /* f3(n) = f2(n) + K3 * g2(n-1) */
+ fcurr0 = (q31_t) ((gcurr0 * (*pk)) >> 15U) + fnext0;
+ fcurr0 = __SSAT(fcurr0, 16);
+
+ /* Process second sample for 3rd, 7th .. tap */
+ fcurr1 = (q31_t) ((gnext0 * (*pk)) >> 15U) + fnext1;
+ fcurr1 = __SSAT(fcurr1, 16);
+
+ /* Process third sample for 3rd, 7th .. tap */
+ fcurr2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fnext2;
+ fcurr2 = __SSAT(fcurr2, 16);
+
+ /* Process fourth sample for 3rd, 7th .. tap */
+ fcurr3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fnext3;
+ fcurr3 = __SSAT(fcurr3, 16);
+
+ /* Calculation of state values for next stage */
+ /* g3(n) = f2(n) * K3 + g2(n-1) */
+ gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15U) + gnext2;
+ gnext3 = __SSAT(gnext3, 16);
+
+ gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15U) + gnext1;
+ gnext2 = __SSAT(gnext2, 16);
+
+ gnext1 = (q31_t) ((fnext1 * (*pk)) >> 15U) + gnext0;
+ gnext1 = __SSAT(gnext1, 16);
+
+ gnext0 = (q31_t) ((fnext0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* Read g1(n-1), g3(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = (q15_t) gnext3;
+
+ /* Sample processing for K4, K8.... */
+ /* Process first sample for 4th, 8th .. tap */
+ /* f4(n) = f3(n) + K4 * g3(n-1) */
+ fnext0 = (q31_t) ((gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ /* Process second sample for 4th, 8th .. tap */
+ /* for sample 2 processing */
+ fnext1 = (q31_t) ((gnext0 * (*pk)) >> 15U) + fcurr1;
+ fnext1 = __SSAT(fnext1, 16);
+
+ /* Process third sample for 4th, 8th .. tap */
+ fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fcurr2;
+ fnext2 = __SSAT(fnext2, 16);
+
+ /* Process fourth sample for 4th, 8th .. tap */
+ fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fcurr3;
+ fnext3 = __SSAT(fnext3, 16);
+
+ /* g4(n) = f3(n) * K4 + g3(n-1) */
+ /* Calculation of state values for next stage */
+ gnext3 = (q31_t) ((fcurr3 * (*pk)) >> 15U) + gnext2;
+ gnext3 = __SSAT(gnext3, 16);
+
+ gnext2 = (q31_t) ((fcurr2 * (*pk)) >> 15U) + gnext1;
+ gnext2 = __SSAT(gnext2, 16);
+
+ gnext1 = (q31_t) ((fcurr1 * (*pk)) >> 15U) + gnext0;
+ gnext1 = __SSAT(gnext1, 16);
+
+ gnext0 = (q31_t) ((fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* Read g2(n-1), g4(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g4(n) in state buffer */
+ *px++ = (q15_t) gnext3;
+
+ /* Sample processing for K5, K9.... */
+ /* Process first sample for 5th, 9th .. tap */
+ /* f5(n) = f4(n) + K5 * g4(n-1) */
+ fcurr0 = (q31_t) ((gcurr0 * (*pk)) >> 15U) + fnext0;
+ fcurr0 = __SSAT(fcurr0, 16);
+
+ /* Process second sample for 5th, 9th .. tap */
+ fcurr1 = (q31_t) ((gnext0 * (*pk)) >> 15U) + fnext1;
+ fcurr1 = __SSAT(fcurr1, 16);
+
+ /* Process third sample for 5th, 9th .. tap */
+ fcurr2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fnext2;
+ fcurr2 = __SSAT(fcurr2, 16);
+
+ /* Process fourth sample for 5th, 9th .. tap */
+ fcurr3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fnext3;
+ fcurr3 = __SSAT(fcurr3, 16);
+
+ /* Calculation of state values for next stage */
+ /* g5(n) = f4(n) * K5 + g4(n-1) */
+ gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15U) + gnext2;
+ gnext3 = __SSAT(gnext3, 16);
+
+ gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15U) + gnext1;
+ gnext2 = __SSAT(gnext2, 16);
+
+ gnext1 = (q31_t) ((fnext1 * (*pk)) >> 15U) + gnext0;
+ gnext1 = __SSAT(gnext1, 16);
+
+ gnext0 = (q31_t) ((fnext0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ stageCnt--;
+ }
+
+ /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
+ stageCnt = (numStages - 1U) % 0x4U;
+
+ while (stageCnt > 0U)
+ {
+ gcurr0 = *px;
+
+ /* save g value in state buffer */
+ *px++ = (q15_t) gnext3;
+
+ /* Process four samples for last three taps here */
+ fnext0 = (q31_t) ((gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ fnext1 = (q31_t) ((gnext0 * (*pk)) >> 15U) + fcurr1;
+ fnext1 = __SSAT(fnext1, 16);
+
+ fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fcurr2;
+ fnext2 = __SSAT(fnext2, 16);
+
+ fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fcurr3;
+ fnext3 = __SSAT(fnext3, 16);
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext3 = (q31_t) ((fcurr3 * (*pk)) >> 15U) + gnext2;
+ gnext3 = __SSAT(gnext3, 16);
+
+ gnext2 = (q31_t) ((fcurr2 * (*pk)) >> 15U) + gnext1;
+ gnext2 = __SSAT(gnext2, 16);
+
+ gnext1 = (q31_t) ((fcurr1 * (*pk)) >> 15U) + gnext0;
+ gnext1 = __SSAT(gnext1, 16);
+
+ gnext0 = (q31_t) ((fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* Update of f values for next coefficient set processing */
+ fcurr0 = fnext0;
+ fcurr1 = fnext1;
+ fcurr2 = fnext2;
+ fcurr3 = fnext3;
+
+ stageCnt--;
+ }
+
+ /* The results in the 4 accumulators, store in the destination buffer. */
+ /* y(n) = fN(n) */
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pDst, __PKHBT(fcurr0, fcurr1, 16));
+ write_q15x2_ia (&pDst, __PKHBT(fcurr2, fcurr3, 16));
+#else
+ write_q15x2_ia (&pDst, __PKHBT(fcurr1, fcurr0, 16));
+ write_q15x2_ia (&pDst, __PKHBT(fcurr3, fcurr2, 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* read g2(n) from state buffer */
+ gcurr0 = *px;
+
+ /* for sample 1 processing */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (((q31_t) gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (((q31_t) fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* save g1(n) in state buffer */
+ *px++ = (q15_t) fcurr0;
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt = (numStages - 1U);
+
+ /* stage loop */
+ while (stageCnt > 0U)
+ {
+ /* read g2(n) from state buffer */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = (q15_t) gnext0;
+
+ /* Sample processing for K2, K3.... */
+ /* f2(n) = f1(n) + K2 * g1(n-1) */
+ fnext0 = (((q31_t) gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ /* g2(n) = f1(n) * K2 + g1(n-1) */
+ gnext0 = (((q31_t) fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt--;
+ }
+
+ /* y(n) = fN(n) */
+ *pDst++ = __SSAT(fcurr0, 16);
+
+ blkCnt--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* read g0(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* for sample 1 processing */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = ((gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext, 16);
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = ((fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* save f0(n) in state buffer */
+ *px++ = (q15_t) fcurr0;
+
+ /* f1(n) is saved in fcurr for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt = (numStages - 1U);
+
+ /* stage loop */
+ while (stageCnt > 0U)
+ {
+ /* read g1(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* save g0(n-1) in state buffer */
+ *px++ = (q15_t) gnext0;
+
+ /* Sample processing for K2, K3.... */
+ /* f2(n) = f1(n) + K2 * g1(n-1) */
+ fnext0 = ((gcurr0 * (*pk)) >> 15U) + fcurr0;
+ fnext0 = __SSAT(fnext0, 16);
+
+ /* g2(n) = f1(n) * K2 + g1(n-1) */
+ gnext0 = ((fcurr0 * (*pk++)) >> 15U) + gcurr0;
+ gnext0 = __SSAT(gnext0, 16);
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt--;
+ }
+
+ /* y(n) = fN(n) */
+ *pDst++ = __SSAT(fcurr0, 16);
+
+ blkCnt--;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of FIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q31.c new file mode 100644 index 0000000..9d29295 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_lattice_q31.c @@ -0,0 +1,505 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_lattice_q31.c
+ * Description: Q31 FIR lattice filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Lattice
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 FIR lattice filter.
+ @param[in] S points to an instance of the Q31 FIR lattice structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ In order to avoid overflows the input signal must be scaled down by 2*log2(numStages) bits.
+ */
+
+void arm_fir_lattice_q31(
+ const arm_fir_lattice_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *px; /* Temporary state pointer */
+ const q31_t *pk; /* Temporary coefficient pointer */
+ uint32_t numStages = S->numStages; /* Number of stages in the filter */
+ uint32_t blkCnt, stageCnt; /* Loop counters */
+ q31_t fcurr0, fnext0, gnext0, gcurr0; /* Temporary variables */
+
+#if (1)
+//#if !defined(ARM_MATH_CM0_FAMILY)
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t fcurr1, fnext1, gnext1; /* Temporary variables for second sample in loop unrolling */
+ q31_t fcurr2, fnext2, gnext2; /* Temporary variables for third sample in loop unrolling */
+ q31_t fcurr3, fnext3, gnext3; /* Temporary variables for fourth sample in loop unrolling */
+#endif
+
+ gcurr0 = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Read two samples from input buffer */
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+ fcurr1 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* Read g0(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* Process first sample for first tap */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* Process second sample for first tap */
+ fnext1 = (q31_t) (((q63_t) fcurr0 * (*pk)) >> 32U);
+ fnext1 = (fnext1 << 1U) + fcurr1;
+ gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 32U);
+ gnext1 = (gnext1 << 1U) + fcurr0;
+
+ /* Read next two samples from input buffer */
+ /* f0(n+2) = x(n+2) */
+ fcurr2 = *pSrc++;
+ fcurr3 = *pSrc++;
+
+ /* Process third sample for first tap */
+ fnext2 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 32U);
+ fnext2 = (fnext2 << 1U) + fcurr2;
+ gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 32U);
+ gnext2 = (gnext2 << 1U) + fcurr1;
+
+ /* Process fourth sample for first tap */
+ fnext3 = (q31_t) (((q63_t) fcurr2 * (*pk )) >> 32U);
+ fnext3 = (fnext3 << 1U) + fcurr3;
+ gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk++)) >> 32U);
+ gnext3 = (gnext3 << 1U) + fcurr2;
+
+ /* Copy only last input sample into the state buffer
+ which will be used for next samples processing */
+ *px++ = fcurr3;
+
+ /* Update of f values for next coefficient set processing */
+ fcurr0 = fnext0;
+ fcurr1 = fnext1;
+ fcurr2 = fnext2;
+ fcurr3 = fnext3;
+
+ /* Loop unrolling. Process 4 taps at a time . */
+ stageCnt = (numStages - 1U) >> 2U;
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numStages-3 coefficients. */
+
+ /* Process 2nd, 3rd, 4th and 5th taps ... here */
+ while (stageCnt > 0U)
+ {
+ /* Read g1(n-1), g3(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Process first sample for 2nd, 6th .. tap */
+ /* Sample processing for K2, K6.... */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ /* Process second sample for 2nd, 6th .. tap */
+ /* for sample 2 processing */
+ fnext1 = (q31_t) (((q63_t) gnext0 * (*pk)) >> 32U);
+ fnext1 = (fnext1 << 1U) + fcurr1;
+
+ /* Process third sample for 2nd, 6th .. tap */
+ fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 32U);
+ fnext2 = (fnext2 << 1U) + fcurr2;
+
+ /* Process fourth sample for 2nd, 6th .. tap */
+ fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 32U);
+ fnext3 = (fnext3 << 1U) + fcurr3;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ /* Calculation of state values for next stage */
+ gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 32U);
+ gnext3 = (gnext3 << 1U) + gnext2;
+
+ gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 32U);
+ gnext2 = (gnext2 << 1U) + gnext1;
+
+ gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 32U);
+ gnext1 = (gnext1 << 1U) + gnext0;
+
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+
+ /* Read g2(n-1), g4(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Sample processing for K3, K7.... */
+ /* Process first sample for 3rd, 7th .. tap */
+ /* f3(n) = f2(n) + K3 * g2(n-1) */
+ fcurr0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fcurr0 = (fcurr0 << 1U) + fnext0;
+
+ /* Process second sample for 3rd, 7th .. tap */
+ fcurr1 = (q31_t) (((q63_t) gnext0 * (*pk)) >> 32U);
+ fcurr1 = (fcurr1 << 1U) + fnext1;
+
+ /* Process third sample for 3rd, 7th .. tap */
+ fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 32U);
+ fcurr2 = (fcurr2 << 1U) + fnext2;
+
+ /* Process fourth sample for 3rd, 7th .. tap */
+ fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 32U);
+ fcurr3 = (fcurr3 << 1U) + fnext3;
+
+ /* Calculation of state values for next stage */
+ /* g3(n) = f2(n) * K3 + g2(n-1) */
+ gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 32U);
+ gnext3 = (gnext3 << 1U) + gnext2;
+
+ gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 32U);
+ gnext2 = (gnext2 << 1U) + gnext1;
+
+ gnext1 = (q31_t) (((q63_t) fnext1 * (*pk)) >> 32U);
+ gnext1 = (gnext1 << 1U) + gnext0;
+
+ gnext0 = (q31_t) (((q63_t) fnext0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* Read g1(n-1), g3(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Sample processing for K4, K8.... */
+ /* Process first sample for 4th, 8th .. tap */
+ /* f4(n) = f3(n) + K4 * g3(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ /* Process second sample for 4th, 8th .. tap */
+ /* for sample 2 processing */
+ fnext1 = (q31_t) (((q63_t) gnext0 * (*pk)) >> 32U);
+ fnext1 = (fnext1 << 1U) + fcurr1;
+
+ /* Process third sample for 4th, 8th .. tap */
+ fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 32U);
+ fnext2 = (fnext2 << 1U) + fcurr2;
+
+ /* Process fourth sample for 4th, 8th .. tap */
+ fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 32U);
+ fnext3 = (fnext3 << 1U) + fcurr3;
+
+ /* g4(n) = f3(n) * K4 + g3(n-1) */
+ /* Calculation of state values for next stage */
+ gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 32U);
+ gnext3 = (gnext3 << 1U) + gnext2;
+
+ gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 32U);
+ gnext2 = (gnext2 << 1U) + gnext1;
+
+ gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 32U);
+ gnext1 = (gnext1 << 1U) + gnext0;
+
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* Read g2(n-1), g4(n-1) .... from state */
+ gcurr0 = *px;
+
+ /* save g4(n) in state buffer */
+ *px++ = gnext3;
+
+ /* Sample processing for K5, K9.... */
+ /* Process first sample for 5th, 9th .. tap */
+ /* f5(n) = f4(n) + K5 * g4(n-1) */
+ fcurr0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fcurr0 = (fcurr0 << 1U) + fnext0;
+
+ /* Process second sample for 5th, 9th .. tap */
+ fcurr1 = (q31_t) (((q63_t) gnext0 * (*pk)) >> 32U);
+ fcurr1 = (fcurr1 << 1U) + fnext1;
+
+ /* Process third sample for 5th, 9th .. tap */
+ fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 32U);
+ fcurr2 = (fcurr2 << 1U) + fnext2;
+
+ /* Process fourth sample for 5th, 9th .. tap */
+ fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 32U);
+ fcurr3 = (fcurr3 << 1U) + fnext3;
+
+ /* Calculation of state values for next stage */
+ /* g5(n) = f4(n) * K5 + g4(n-1) */
+ gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 32U);
+ gnext3 = (gnext3 << 1U) + gnext2;
+
+ gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 32U);
+ gnext2 = (gnext2 << 1U) + gnext1;
+
+ gnext1 = (q31_t) (((q63_t) fnext1 * (*pk)) >> 32U);
+ gnext1 = (gnext1 << 1U) + gnext0;
+
+ gnext0 = (q31_t) (((q63_t) fnext0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ stageCnt--;
+ }
+
+ /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
+ stageCnt = (numStages - 1U) % 0x4U;
+
+ while (stageCnt > 0U)
+ {
+ gcurr0 = *px;
+
+ /* save g value in state buffer */
+ *px++ = gnext3;
+
+ /* Process four samples for last three taps here */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ fnext1 = (q31_t) (((q63_t) gnext0 * (*pk)) >> 32U);
+ fnext1 = (fnext1 << 1U) + fcurr1;
+
+ fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 32U);
+ fnext2 = (fnext2 << 1U) + fcurr2;
+
+ fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 32U);
+ fnext3 = (fnext3 << 1U) + fcurr3;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 32U);
+ gnext3 = (gnext3 << 1U) + gnext2;
+
+ gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 32U);
+ gnext2 = (gnext2 << 1U) + gnext1;
+
+ gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 32U);
+ gnext1 = (gnext1 << 1U) + gnext0;
+
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* Update of f values for next coefficient set processing */
+ fcurr0 = fnext0;
+ fcurr1 = fnext1;
+ fcurr2 = fnext2;
+ fcurr3 = fnext3;
+
+ stageCnt--;
+ }
+
+ /* The results in the 4 accumulators, store in the destination buffer. */
+ /* y(n) = fN(n) */
+ *pDst++ = fcurr0;
+ *pDst++ = fcurr1;
+ *pDst++ = fcurr2;
+ *pDst++ = fcurr3;
+
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* read g2(n) from state buffer */
+ gcurr0 = *px;
+
+ /* for sample 1 processing */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* save g1(n) in state buffer */
+ *px++ = fcurr0;
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt = (numStages - 1U);
+
+ /* stage loop */
+ while (stageCnt > 0U)
+ {
+ /* read g2(n) from state buffer */
+ gcurr0 = *px;
+
+ /* save g1(n) in state buffer */
+ *px++ = gnext0;
+
+ /* Sample processing for K2, K3.... */
+ /* f2(n) = f1(n) + K2 * g1(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ /* g2(n) = f1(n) * K2 + g1(n-1) */
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt--;
+ }
+
+ /* y(n) = fN(n) */
+ *pDst++ = fcurr0;
+
+ blkCnt--;
+ }
+
+#else
+/* alternate version for CM0_FAMILY */
+
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* f0(n) = x(n) */
+ fcurr0 = *pSrc++;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pk = pCoeffs;
+
+ /* read g0(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* for sample 1 processing */
+ /* f1(n) = f0(n) + K1 * g0(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext << 1U) + fcurr0;
+
+ /* g1(n) = f0(n) * K1 + g0(n-1) */
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* save f0(n) in state buffer */
+ *px++ = fcurr0;
+
+ /* f1(n) is saved in fcurr for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt = (numStages - 1U);
+
+ /* stage loop */
+ while (stageCnt > 0U)
+ {
+ /* read g1(n-1) from state buffer */
+ gcurr0 = *px;
+
+ /* save g0(n-1) in state buffer */
+ *px++ = gnext0;
+
+ /* Sample processing for K2, K3.... */
+ /* f2(n) = f1(n) + K2 * g1(n-1) */
+ fnext0 = (q31_t) (((q63_t) gcurr0 * (*pk)) >> 32U);
+ fnext0 = (fnext0 << 1U) + fcurr0;
+
+ /* g2(n) = f1(n) * K2 + g1(n-1) */
+ gnext0 = (q31_t) (((q63_t) fcurr0 * (*pk++)) >> 32U);
+ gnext0 = (gnext0 << 1U) + gcurr0;
+
+ /* f1(n) is saved in fcurr0 for next stage processing */
+ fcurr0 = fnext0;
+
+ stageCnt--;
+ }
+
+ /* y(n) = fN(n) */
+ *pDst++ = fcurr0;
+
+ blkCnt--;
+ }
+
+#endif /* #if !defined(ARM_MATH_CM0_FAMILY) */
+
+}
+
+/**
+ @} end of FIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q15.c new file mode 100644 index 0000000..48506a8 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q15.c @@ -0,0 +1,332 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_q15.c
+ * Description: Q15 FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 FIR filter.
+ @param[in] S points to an instance of the Q15 FIR filter structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+ Lastly, the accumulator is saturated to yield a result in 1.15 format.
+
+ @remark
+ Refer to \ref arm_fir_fast_q15() for a faster but less precise implementation of this function.
+ */
+
+void arm_fir_q15(
+ const arm_fir_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *pStateCurnt; /* Points to the current sample of the state */
+ q15_t *px; /* Temporary pointer for state buffer */
+ const q15_t *pb; /* Temporary pointer for coefficient buffer */
+ q63_t acc0; /* Accumulators */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc1, acc2, acc3; /* Accumulators */
+ q31_t x0, x1, x2, c0; /* Temporary variables to hold state and coefficient values */
+#endif
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 output values simultaneously.
+ * The variables acc0 ... acc3 hold output values that are being computed:
+ *
+ * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
+ * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
+ * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
+ * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
+ */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 new input samples into the state buffer. */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Typecast q15_t pointer to q31_t pointer for state reading in q31_t */
+ px = pState;
+
+ /* Typecast q15_t pointer to q31_t pointer for coefficient reading in q31_t */
+ pb = pCoeffs;
+
+ /* Read the first two samples from the state buffer: x[n-N], x[n-N-1] */
+ x0 = read_q15x2_ia (&px);
+
+ /* Read the third and forth samples from the state buffer: x[n-N-2], x[n-N-3] */
+ x2 = read_q15x2_ia (&px);
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-(numTaps%4) coefficients. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the first two coefficients using SIMD: b[N] and b[N-1] coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* acc0 += b[N] * x[n-N] + b[N-1] * x[n-N-1] */
+ acc0 = __SMLALD(x0, c0, acc0);
+
+ /* acc2 += b[N] * x[n-N-2] + b[N-1] * x[n-N-3] */
+ acc2 = __SMLALD(x2, c0, acc2);
+
+ /* pack x[n-N-1] and x[n-N-2] */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x2, x0, 0);
+#else
+ x1 = __PKHBT(x0, x2, 0);
+#endif
+
+ /* Read state x[n-N-4], x[n-N-5] */
+ x0 = read_q15x2_ia (&px);
+
+ /* acc1 += b[N] * x[n-N-1] + b[N-1] * x[n-N-2] */
+ acc1 = __SMLALDX(x1, c0, acc1);
+
+ /* pack x[n-N-3] and x[n-N-4] */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x0, x2, 0);
+#else
+ x1 = __PKHBT(x2, x0, 0);
+#endif
+
+ /* acc3 += b[N] * x[n-N-3] + b[N-1] * x[n-N-4] */
+ acc3 = __SMLALDX(x1, c0, acc3);
+
+ /* Read coefficients b[N-2], b[N-3] */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* acc0 += b[N-2] * x[n-N-2] + b[N-3] * x[n-N-3] */
+ acc0 = __SMLALD(x2, c0, acc0);
+
+ /* Read state x[n-N-6], x[n-N-7] with offset */
+ x2 = read_q15x2_ia (&px);
+
+ /* acc2 += b[N-2] * x[n-N-4] + b[N-3] * x[n-N-5] */
+ acc2 = __SMLALD(x0, c0, acc2);
+
+ /* acc1 += b[N-2] * x[n-N-3] + b[N-3] * x[n-N-4] */
+ acc1 = __SMLALDX(x1, c0, acc1);
+
+ /* pack x[n-N-5] and x[n-N-6] */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x2, x0, 0);
+#else
+ x1 = __PKHBT(x0, x2, 0);
+#endif
+
+ /* acc3 += b[N-2] * x[n-N-5] + b[N-3] * x[n-N-6] */
+ acc3 = __SMLALDX(x1, c0, acc3);
+
+ /* Decrement tap count */
+ tapCnt--;
+ }
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps.
+ This is always be 2 taps since the filter length is even. */
+ if ((numTaps & 0x3U) != 0U)
+ {
+ /* Read last two coefficients */
+ c0 = read_q15x2_ia ((q15_t **) &pb);
+
+ /* Perform the multiply-accumulates */
+ acc0 = __SMLALD(x0, c0, acc0);
+ acc2 = __SMLALD(x2, c0, acc2);
+
+ /* pack state variables */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x2, x0, 0);
+#else
+ x1 = __PKHBT(x0, x2, 0);
+#endif
+
+ /* Read last state variables */
+ x0 = read_q15x2 (px);
+
+ /* Perform the multiply-accumulates */
+ acc1 = __SMLALDX(x1, c0, acc1);
+
+ /* pack state variables */
+#ifndef ARM_MATH_BIG_ENDIAN
+ x1 = __PKHBT(x0, x2, 0);
+#else
+ x1 = __PKHBT(x2, x0, 0);
+#endif
+
+ /* Perform the multiply-accumulates */
+ acc3 = __SMLALDX(x1, c0, acc3);
+ }
+
+ /* The results in the 4 accumulators are in 2.30 format. Convert to 1.15 with saturation.
+ Then store the 4 outputs in the destination buffer. */
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16));
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16));
+#else
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16));
+ write_q15x2_ia (&pDst, __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 4U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining output samples */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of taps */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy two samples into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc0 = 0;
+
+ /* Use SIMD to hold states and coefficients */
+ px = pState;
+ pb = pCoeffs;
+
+ tapCnt = numTaps >> 1U;
+
+ do
+ {
+ acc0 += (q31_t) *px++ * *pb++;
+ acc0 += (q31_t) *px++ * *pb++;
+
+ tapCnt--;
+ }
+ while (tapCnt > 0U);
+
+ /* The result is in 2.30 format. Convert to 1.15 with saturation.
+ Then store the output in the destination buffer. */
+ *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1U;
+
+ /* Decrement 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;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy remaining data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q31.c new file mode 100644 index 0000000..518f87a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q31.c @@ -0,0 +1,288 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_q31.c
+ * Description: Q31 FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Processing function for Q31 FIR filter.
+ @param[in] S points to an instance of the Q31 FIR filter structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits.
+ After all multiply-accumulates are performed, the 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
+
+ @remark
+ Refer to \ref arm_fir_fast_q31() for a faster but less precise implementation of this filter.
+ */
+
+void arm_fir_q31(
+ const arm_fir_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *pStateCurnt; /* Points to the current sample of the state */
+ q31_t *px; /* Temporary pointer for state buffer */
+ const q31_t *pb; /* Temporary pointer for coefficient buffer */
+ q63_t acc0; /* Accumulator */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q63_t acc1, acc2; /* Accumulators */
+ q31_t x0, x1, x2; /* Temporary variables to hold state values */
+ q31_t c0; /* Temporary variable to hold coefficient value */
+#endif
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 output values simultaneously.
+ * The variables acc0 ... acc3 hold output values that are being computed:
+ *
+ * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
+ * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
+ * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
+ * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
+ */
+
+ blkCnt = blockSize / 3;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 3 new input samples into the state buffer. */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Read the first 2 samples from the state buffer: x[n-numTaps], x[n-numTaps-1] */
+ x0 = *px++;
+ x1 = *px++;
+
+ /* Loop unrolling: process 3 taps at a time. */
+ tapCnt = numTaps / 3;
+
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps] coefficient */
+ c0 = *pb;
+
+ /* Read x[n-numTaps-2] sample */
+ x2 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q63_t) x0 * c0);
+ acc1 += ((q63_t) x1 * c0);
+ acc2 += ((q63_t) x2 * c0);
+
+ /* Read the coefficient and state */
+ c0 = *(pb + 1U);
+ x0 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q63_t) x1 * c0);
+ acc1 += ((q63_t) x2 * c0);
+ acc2 += ((q63_t) x0 * c0);
+
+ /* Read the coefficient and state */
+ c0 = *(pb + 2U);
+ x1 = *(px++);
+
+ /* update coefficient pointer */
+ pb += 3U;
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q63_t) x2 * c0);
+ acc1 += ((q63_t) x0 * c0);
+ acc2 += ((q63_t) x1 * c0);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ tapCnt = numTaps % 0x3U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch 1 state variable */
+ x2 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q63_t) x0 * c0);
+ acc1 += ((q63_t) x1 * c0);
+ acc2 += ((q63_t) x2 * c0);
+
+ /* Reuse the present sample states for next sample */
+ x0 = x1;
+ x1 = x2;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Advance the state pointer by 3 to process the next group of 3 samples */
+ pState = pState + 3;
+
+ /* The result is in 2.30 format. Convert to 1.31 and store in destination buffer. */
+ *pDst++ = (q31_t) (acc0 >> 31U);
+ *pDst++ = (q31_t) (acc1 >> 31U);
+ *pDst++ = (q31_t) (acc2 >> 31U);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining output samples */
+ blkCnt = blockSize % 0x3U;
+
+#else
+
+ /* Initialize blkCnt with number of taps */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy one sample at a time into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize Coefficient pointer */
+ pb = pCoeffs;
+
+ i = numTaps;
+
+ /* Perform the multiply-accumulates */
+ do
+ {
+ /* acc = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
+ acc0 += (q63_t) *px++ * *pb++;
+
+ i--;
+ } while (i > 0U);
+
+ /* Result is in 2.62 format. Convert to 1.31 and store in destination buffer. */
+ *pDst++ = (q31_t) (acc0 >> 31U);
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1U;
+
+ /* Decrement 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;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy remaining data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q7.c new file mode 100644 index 0000000..7c9f5bf --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_q7.c @@ -0,0 +1,323 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_q7.c
+ * Description: Q7 FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR
+ @{
+ */
+
+/**
+ @brief Processing function for Q7 FIR filter.
+ @param[in] S points to an instance of the Q7 FIR filter structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 32-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.7 format and multiplications yield a 2.14 result.
+ The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ The accumulator is converted to 18.7 format by discarding the low 7 bits.
+ Finally, the result is truncated to 1.7 format.
+ */
+
+void arm_fir_q7(
+ const arm_fir_instance_q7 * S,
+ const q7_t * pSrc,
+ q7_t * pDst,
+ uint32_t blockSize)
+{
+ q7_t *pState = S->pState; /* State pointer */
+ const q7_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q7_t *pStateCurnt; /* Points to the current sample of the state */
+ q7_t *px; /* Temporary pointer for state buffer */
+ const q7_t *pb; /* Temporary pointer for coefficient buffer */
+ q31_t acc0; /* Accumulators */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t i, tapCnt, blkCnt; /* Loop counters */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t acc1, acc2, acc3; /* Accumulators */
+ q7_t x0, x1, x2, x3, c0; /* Temporary variables to hold state */
+#endif
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 output values simultaneously.
+ * The variables acc0 ... acc3 hold output values that are being computed:
+ *
+ * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
+ * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
+ * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
+ * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
+ */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy 4 new input samples into the state buffer. */
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set all accumulators to zero */
+ acc0 = 0;
+ acc1 = 0;
+ acc2 = 0;
+ acc3 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Read the first 3 samples from the state buffer:
+ * x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2] */
+ x0 = *px++;
+ x1 = *px++;
+ x2 = *px++;
+
+ /* Loop unrolling. Process 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Loop over the number of taps. Unroll by a factor of 4.
+ Repeat until we've computed numTaps-4 coefficients. */
+ while (tapCnt > 0U)
+ {
+ /* Read the b[numTaps] coefficient */
+ c0 = *pb;
+
+ /* Read x[n-numTaps-3] sample */
+ x3 = *px;
+
+ /* acc0 += b[numTaps] * x[n-numTaps] */
+ acc0 += ((q15_t) x0 * c0);
+
+ /* acc1 += b[numTaps] * x[n-numTaps-1] */
+ acc1 += ((q15_t) x1 * c0);
+
+ /* acc2 += b[numTaps] * x[n-numTaps-2] */
+ acc2 += ((q15_t) x2 * c0);
+
+ /* acc3 += b[numTaps] * x[n-numTaps-3] */
+ acc3 += ((q15_t) x3 * c0);
+
+ /* Read the b[numTaps-1] coefficient */
+ c0 = *(pb + 1U);
+
+ /* Read x[n-numTaps-4] sample */
+ x0 = *(px + 1U);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q15_t) x1 * c0);
+ acc1 += ((q15_t) x2 * c0);
+ acc2 += ((q15_t) x3 * c0);
+ acc3 += ((q15_t) x0 * c0);
+
+ /* Read the b[numTaps-2] coefficient */
+ c0 = *(pb + 2U);
+
+ /* Read x[n-numTaps-5] sample */
+ x1 = *(px + 2U);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q15_t) x2 * c0);
+ acc1 += ((q15_t) x3 * c0);
+ acc2 += ((q15_t) x0 * c0);
+ acc3 += ((q15_t) x1 * c0);
+
+ /* Read the b[numTaps-3] coefficients */
+ c0 = *(pb + 3U);
+
+ /* Read x[n-numTaps-6] sample */
+ x2 = *(px + 3U);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q15_t) x3 * c0);
+ acc1 += ((q15_t) x0 * c0);
+ acc2 += ((q15_t) x1 * c0);
+ acc3 += ((q15_t) x2 * c0);
+
+ /* update coefficient pointer */
+ pb += 4U;
+ px += 4U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read coefficients */
+ c0 = *(pb++);
+
+ /* Fetch 1 state variable */
+ x3 = *(px++);
+
+ /* Perform the multiply-accumulates */
+ acc0 += ((q15_t) x0 * c0);
+ acc1 += ((q15_t) x1 * c0);
+ acc2 += ((q15_t) x2 * c0);
+ acc3 += ((q15_t) x3 * c0);
+
+ /* Reuse the present sample states for next sample */
+ x0 = x1;
+ x1 = x2;
+ x2 = x3;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* The results in the 4 accumulators are in 2.62 format. Convert to 1.31
+ Then store the 4 outputs in the destination buffer. */
+ acc0 = __SSAT((acc0 >> 7U), 8);
+ *pDst++ = acc0;
+ acc1 = __SSAT((acc1 >> 7U), 8);
+ *pDst++ = acc1;
+ acc2 = __SSAT((acc2 >> 7U), 8);
+ *pDst++ = acc2;
+ acc3 = __SSAT((acc3 >> 7U), 8);
+ *pDst++ = acc3;
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 4U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining output samples */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of taps */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Copy one sample at a time into state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Set the accumulator to zero */
+ acc0 = 0;
+
+ /* Initialize state pointer */
+ px = pState;
+
+ /* Initialize Coefficient pointer */
+ pb = pCoeffs;
+
+ i = numTaps;
+
+ /* Perform the multiply-accumulates */
+ do
+ {
+ acc0 += (q15_t) * (px++) * (*(pb++));
+ i--;
+ } while (i > 0U);
+
+ /* The result is in 2.14 format. Convert to 1.7
+ Then store the output in the destination buffer. */
+ *pDst++ = __SSAT((acc0 >> 7U), 8);
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1U;
+
+ /* Decrement 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;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of taps */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ /* Copy remaining data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_f32.c new file mode 100644 index 0000000..d31ba4c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_f32.c @@ -0,0 +1,341 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_f32.c
+ * Description: Floating-point sparse FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup FIR_Sparse Finite Impulse Response (FIR) Sparse Filters
+
+ This group of functions implements sparse FIR filters.
+ Sparse FIR filters are equivalent to standard FIR filters except that most of the coefficients are equal to zero.
+ Sparse filters are used for simulating reflections in communications and audio applications.
+
+ There are separate functions for Q7, Q15, Q31, and floating-point data types.
+ The functions operate on blocks of input and output data and each call to the function processes
+ <code>blockSize</code> samples through the filter. <code>pSrc</code> and
+ <code>pDst</code> points to input and output arrays respectively containing <code>blockSize</code> values.
+
+ @par Algorithm
+ The sparse filter instant structure contains an array of tap indices <code>pTapDelay</code> which specifies the locations of the non-zero coefficients.
+ This is in addition to the coefficient array <code>b</code>.
+ The implementation essentially skips the multiplications by zero and leads to an efficient realization.
+ <pre>
+ y[n] = b[0] * x[n-pTapDelay[0]] + b[1] * x[n-pTapDelay[1]] + b[2] * x[n-pTapDelay[2]] + ...+ b[numTaps-1] * x[n-pTapDelay[numTaps-1]]
+ </pre>
+ @par
+ \image html FIRSparse.gif "Sparse FIR filter. b[n] represents the filter coefficients"
+ @par
+ <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>;
+ <code>pTapDelay</code> points to an array of nonzero indices and is also of size <code>numTaps</code>;
+ <code>pState</code> points to a state array of size <code>maxDelay + blockSize</code>, where
+ <code>maxDelay</code> is the largest offset value that is ever used in the <code>pTapDelay</code> array.
+ Some of the processing functions also require temporary working buffers.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient and offset arrays may be shared among several instances while state variable arrays cannot be shared.
+ There are separate instance structure declarations for each of the 4 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numTaps, pCoeffs, pTapDelay, maxDelay, stateIndex, pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros before static initialization.
+ The code below statically initializes each of the 4 different data type filter instance structures
+ <pre>
+ arm_fir_sparse_instance_f32 S = {numTaps, 0, pState, pCoeffs, maxDelay, pTapDelay};
+ arm_fir_sparse_instance_q31 S = {numTaps, 0, pState, pCoeffs, maxDelay, pTapDelay};
+ arm_fir_sparse_instance_q15 S = {numTaps, 0, pState, pCoeffs, maxDelay, pTapDelay};
+ arm_fir_sparse_instance_q7 S = {numTaps, 0, pState, pCoeffs, maxDelay, pTapDelay};
+ </pre>
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the fixed-point versions of the sparse FIR filter functions.
+ In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ Refer to the function specific documentation below for usage guidelines.
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point sparse FIR filter.
+ @param[in] S points to an instance of the floating-point sparse FIR structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] pScratchIn points to a temporary buffer of size blockSize
+ @param[in] blockSize number of input samples to process
+ @return none
+ */
+
+void arm_fir_sparse_f32(
+ arm_fir_sparse_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ float32_t * pScratchIn,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ float32_t *px; /* Scratch buffer pointer */
+ float32_t *py = pState; /* Temporary pointers for state buffer */
+ float32_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
+ float32_t *pOut; /* Destination pointer */
+ int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
+ uint32_t delaySize = S->maxDelay + blockSize; /* state length */
+ uint16_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ int32_t readIndex; /* Read index of the state buffer */
+ uint32_t tapCnt, blkCnt; /* loop counters */
+ float32_t coeff = *pCoeffs++; /* Read the first coefficient value */
+
+
+ /* BlockSize of Input samples are copied into the state buffer */
+ /* StateIndex points to the starting position to write in the state buffer */
+ arm_circularWrite_f32((int32_t *) py, delaySize, &S->stateIndex, 1, (int32_t *) pSrc, 1, blockSize);
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
+ (int32_t *) pb, (int32_t *) pb, blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pOut = pDst;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiplications and store in destination buffer */
+ *pOut++ = *px++ * coeff;
+
+ *pOut++ = *px++ * coeff;
+
+ *pOut++ = *px++ * coeff;
+
+ *pOut++ = *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiplication and store in destination buffer */
+ *pOut++ = *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Loop over the number of taps. */
+ tapCnt = (uint32_t) numTaps - 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
+ (int32_t *) pb, (int32_t *) pb, blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pOut = pDst;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pOut++ += *px++ * coeff;
+
+ *pOut++ += *px++ * coeff;
+
+ *pOut++ += *px++ * coeff;
+
+ *pOut++ += *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pOut++ += *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Decrement tap loop counter */
+ tapCnt--;
+ }
+
+ /* Compute last tap without the final read of pTapDelay */
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
+ (int32_t *) pb, (int32_t *) pb, blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pOut = pDst;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pOut++ += *px++ * coeff;
+ *pOut++ += *px++ * coeff;
+ *pOut++ += *px++ * coeff;
+ *pOut++ += *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pOut++ += *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_f32.c new file mode 100644 index 0000000..978ca46 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_f32.c @@ -0,0 +1,93 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_init_f32.c
+ * Description: Floating-point sparse FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point sparse FIR filter.
+ @param[in,out] S points to an instance of the floating-point sparse FIR structure
+ @param[in] numTaps number of nonzero coefficients in the filter
+ @param[in] pCoeffs points to the array of filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] pTapDelay points to the array of offset times
+ @param[in] maxDelay maximum offset time supported
+ @param[in] blockSize number of samples that will be processed per block
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> holds the filter coefficients and has length <code>numTaps</code>.
+ <code>pState</code> holds the filter's state variables and must be of length
+ <code>maxDelay + blockSize</code>, where <code>maxDelay</code>
+ is the maximum number of delay line values.
+ <code>blockSize</code> is the
+ number of samples processed by the <code>arm_fir_sparse_f32()</code> function.
+ */
+
+void arm_fir_sparse_init_f32(
+ arm_fir_sparse_instance_f32 * S,
+ uint16_t numTaps,
+ const float32_t * pCoeffs,
+ float32_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign TapDelay pointer */
+ S->pTapDelay = pTapDelay;
+
+ /* Assign MaxDelay */
+ S->maxDelay = maxDelay;
+
+ /* reset the stateIndex to 0 */
+ S->stateIndex = 0U;
+
+ /* Clear state buffer and size is always maxDelay + blockSize */
+ memset(pState, 0, (maxDelay + blockSize) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q15.c new file mode 100644 index 0000000..5f6ab54 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q15.c @@ -0,0 +1,93 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_init_q15.c
+ * Description: Q15 sparse FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 sparse FIR filter.
+ @param[in,out] S points to an instance of the Q15 sparse FIR structure
+ @param[in] numTaps number of nonzero coefficients in the filter
+ @param[in] pCoeffs points to the array of filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] pTapDelay points to the array of offset times
+ @param[in] maxDelay maximum offset time supported
+ @param[in] blockSize number of samples that will be processed per block
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> holds the filter coefficients and has length <code>numTaps</code>.
+ <code>pState</code> holds the filter's state variables and must be of length
+ <code>maxDelay + blockSize</code>, where <code>maxDelay</code>
+ is the maximum number of delay line values.
+ <code>blockSize</code> is the
+ number of words processed by <code>arm_fir_sparse_q15()</code> function.
+ */
+
+void arm_fir_sparse_init_q15(
+ arm_fir_sparse_instance_q15 * S,
+ uint16_t numTaps,
+ const q15_t * pCoeffs,
+ q15_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign TapDelay pointer */
+ S->pTapDelay = pTapDelay;
+
+ /* Assign MaxDelay */
+ S->maxDelay = maxDelay;
+
+ /* reset the stateIndex to 0 */
+ S->stateIndex = 0U;
+
+ /* Clear state buffer and size is always maxDelay + blockSize */
+ memset(pState, 0, (maxDelay + blockSize) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q31.c new file mode 100644 index 0000000..ec9232c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q31.c @@ -0,0 +1,92 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_init_q31.c
+ * Description: Q31 sparse FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 sparse FIR filter.
+ @param[in,out] S points to an instance of the Q31 sparse FIR structure
+ @param[in] numTaps number of nonzero coefficients in the filter
+ @param[in] pCoeffs points to the array of filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] pTapDelay points to the array of offset times
+ @param[in] maxDelay maximum offset time supported
+ @param[in] blockSize number of samples that will be processed per block
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> holds the filter coefficients and has length <code>numTaps</code>.
+ <code>pState</code> holds the filter's state variables and must be of length
+ <code>maxDelay + blockSize</code>, where <code>maxDelay</code>
+ is the maximum number of delay line values.
+ <code>blockSize</code> is the number of words processed by <code>arm_fir_sparse_q31()</code> function.
+ */
+
+void arm_fir_sparse_init_q31(
+ arm_fir_sparse_instance_q31 * S,
+ uint16_t numTaps,
+ const q31_t * pCoeffs,
+ q31_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign TapDelay pointer */
+ S->pTapDelay = pTapDelay;
+
+ /* Assign MaxDelay */
+ S->maxDelay = maxDelay;
+
+ /* reset the stateIndex to 0 */
+ S->stateIndex = 0U;
+
+ /* Clear state buffer and size is always maxDelay + blockSize */
+ memset(pState, 0, (maxDelay + blockSize) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q7.c new file mode 100644 index 0000000..f72ea25 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_init_q7.c @@ -0,0 +1,93 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_init_q7.c
+ * Description: Q7 sparse FIR filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q7 sparse FIR filter.
+ @param[in,out] S points to an instance of the Q7 sparse FIR structure
+ @param[in] numTaps number of nonzero coefficients in the filter
+ @param[in] pCoeffs points to the array of filter coefficients
+ @param[in] pState points to the state buffer
+ @param[in] pTapDelay points to the array of offset times
+ @param[in] maxDelay maximum offset time supported
+ @param[in] blockSize number of samples that will be processed per block
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> holds the filter coefficients and has length <code>numTaps</code>.
+ <code>pState</code> holds the filter's state variables and must be of length
+ <code>maxDelay + blockSize</code>, where <code>maxDelay</code>
+ is the maximum number of delay line values.
+ <code>blockSize</code> is the
+ number of samples processed by the <code>arm_fir_sparse_q7()</code> function.
+ */
+
+void arm_fir_sparse_init_q7(
+ arm_fir_sparse_instance_q7 * S,
+ uint16_t numTaps,
+ const q7_t * pCoeffs,
+ q7_t * pState,
+ int32_t * pTapDelay,
+ uint16_t maxDelay,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Assign TapDelay pointer */
+ S->pTapDelay = pTapDelay;
+
+ /* Assign MaxDelay */
+ S->maxDelay = maxDelay;
+
+ /* reset the stateIndex to 0 */
+ S->stateIndex = 0U;
+
+ /* Clear state buffer and size is always maxDelay + blockSize */
+ memset(pState, 0, (maxDelay + blockSize) * sizeof(q7_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q15.c new file mode 100644 index 0000000..99002b8 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q15.c @@ -0,0 +1,341 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_q15.c
+ * Description: Q15 sparse FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 sparse FIR filter.
+ @param[in] S points to an instance of the Q15 sparse FIR structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] pScratchIn points to a temporary buffer of size blockSize
+ @param[in] pScratchOut points to a temporary buffer of size blockSize
+ @param[in] blockSize number of input samples to process per call
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 32-bit accumulator.
+ The 1.15 x 1.15 multiplications yield a 2.30 result and these are added to a 2.30 accumulator.
+ Thus the full precision of the multiplications is maintained but there is only a single guard bit in the accumulator.
+ If the accumulator result overflows it will wrap around rather than saturate.
+ After all multiply-accumulates are performed, the 2.30 accumulator is truncated to 2.15 format and then saturated to 1.15 format.
+ In order to avoid overflows the input signal or coefficients must be scaled down by log2(numTaps) bits.
+ */
+
+void arm_fir_sparse_q15(
+ arm_fir_sparse_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ q15_t * pScratchIn,
+ q31_t * pScratchOut,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q15_t *px; /* Temporary pointers for scratch buffer */
+ q15_t *py = pState; /* Temporary pointers for state buffer */
+ q15_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
+ q15_t *pOut = pDst; /* Working pointer for output */
+ int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
+ uint32_t delaySize = S->maxDelay + blockSize; /* state length */
+ uint16_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ int32_t readIndex; /* Read index of the state buffer */
+ uint32_t tapCnt, blkCnt; /* loop counters */
+ q31_t *pScr2 = pScratchOut; /* Working pointer for scratch buffer of output values */
+ q15_t coeff = *pCoeffs++; /* Read the first coefficient value */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q31_t in1, in2; /* Temporary variables */
+#endif
+
+ /* BlockSize of Input samples are copied into the state buffer */
+ /* StateIndex points to the starting position to write in the state buffer */
+ arm_circularWrite_q15(py, (int32_t) delaySize, &S->stateIndex, 1,pSrc, 1, blockSize);
+
+ /* Loop over the number of taps. */
+ tapCnt = numTaps;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_q15(py, (int32_t) delaySize, &readIndex, 1,
+ pb, pb, (int32_t) blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pScratchOut = pScr2;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform multiplication and store in the scratch buffer */
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiplication and store in the scratch buffer */
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Loop over the number of taps. */
+ tapCnt = (uint32_t) numTaps - 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_q15(py, (int32_t) delaySize, &readIndex, 1,
+ pb, pb, (int32_t) blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pScratchOut = pScr2;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Compute last tap without the final read of pTapDelay */
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_q15(py, (int32_t) delaySize, &readIndex, 1,
+ pb, pb, (int32_t) blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pScratchOut = pScr2;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ *pScratchOut++ += (q31_t) *px++ * coeff;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* All the output values are in pScratchOut buffer.
+ Convert them into 1.15 format, saturate and store in the destination buffer. */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ in1 = *pScr2++;
+ in2 = *pScr2++;
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT((q15_t) __SSAT(in1 >> 15, 16), (q15_t) __SSAT(in2 >> 15, 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT((q15_t) __SSAT(in2 >> 15, 16), (q15_t) __SSAT(in1 >> 15, 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ in1 = *pScr2++;
+ in2 = *pScr2++;
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ write_q15x2_ia (&pOut, __PKHBT((q15_t) __SSAT(in1 >> 15, 16), (q15_t) __SSAT(in2 >> 15, 16), 16));
+#else
+ write_q15x2_ia (&pOut, __PKHBT((q15_t) __SSAT(in2 >> 15, 16), (q15_t) __SSAT(in1 >> 15, 16), 16));
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ *pOut++ = (q15_t) __SSAT(*pScr2++ >> 15, 16);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q31.c new file mode 100644 index 0000000..7c92431 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q31.c @@ -0,0 +1,357 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_q31.c
+ * Description: Q31 sparse FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 sparse FIR filter.
+ @param[in] S points to an instance of the Q31 sparse FIR structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] pScratchIn points to a temporary buffer of size blockSize
+ @param[in] blockSize number of input samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 32-bit accumulator.
+ The 1.31 x 1.31 multiplications are truncated to 2.30 format.
+ This leads to loss of precision on the intermediate multiplications and provides only a single guard bit.
+ If the accumulator result overflows, it wraps around rather than saturate.
+ In order to avoid overflows the input signal or coefficients must be scaled down by log2(numTaps) bits.
+ */
+
+void arm_fir_sparse_q31(
+ arm_fir_sparse_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ q31_t * pScratchIn,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q31_t *px; /* Scratch buffer pointer */
+ q31_t *py = pState; /* Temporary pointers for state buffer */
+ q31_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
+ q31_t *pOut; /* Destination pointer */
+ int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
+ uint32_t delaySize = S->maxDelay + blockSize; /* state length */
+ uint16_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ int32_t readIndex; /* Read index of the state buffer */
+ uint32_t tapCnt, blkCnt; /* loop counters */
+ q31_t coeff = *pCoeffs++; /* Read the first coefficient value */
+ q31_t in;
+ q63_t out; /* Temporary output variable */
+
+
+ /* BlockSize of Input samples are copied into the state buffer */
+ /* StateIndex points to the starting position to write in the state buffer */
+ arm_circularWrite_f32((int32_t *) py, delaySize, &S->stateIndex, 1,
+ (int32_t *) pSrc, 1, blockSize);
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
+ (int32_t *) pb, (int32_t *) pb, blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pOut = pDst;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiplications and store in destination buffer */
+ *pOut++ = (q31_t) (((q63_t) *px++ * coeff) >> 32);
+
+ *pOut++ = (q31_t) (((q63_t) *px++ * coeff) >> 32);
+
+ *pOut++ = (q31_t) (((q63_t) *px++ * coeff) >> 32);
+
+ *pOut++ = (q31_t) (((q63_t) *px++ * coeff) >> 32);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiplication and store in destination buffer */
+ *pOut++ = (q31_t) (((q63_t) *px++ * coeff) >> 32);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Loop over the number of taps. */
+ tapCnt = (uint32_t) numTaps - 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
+ (int32_t *) pb, (int32_t *) pb, blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pOut = pDst;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ out = *pOut;
+ out += ((q63_t) *px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ out = *pOut;
+ out += ((q63_t) *px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ out = *pOut;
+ out += ((q63_t) *px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ out = *pOut;
+ out += ((q63_t) *px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ out = *pOut;
+ out += ((q63_t) *px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Decrement tap loop counter */
+ tapCnt--;
+ }
+
+ /* Compute last tap without the final read of pTapDelay */
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
+ (int32_t *) pb, (int32_t *) pb, blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pOut = pDst;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ out = *pOut;
+ out += ((q63_t) * px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ out = *pOut;
+ out += ((q63_t) * px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ out = *pOut;
+ out += ((q63_t) * px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ out = *pOut;
+ out += ((q63_t) * px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ out = *pOut;
+ out += ((q63_t) *px++ * coeff) >> 32;
+ *pOut++ = (q31_t) (out);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Working output pointer is updated */
+ pOut = pDst;
+
+ /* Output is converted into 1.31 format. */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ in = *pOut << 1;
+ *pOut++ = in;
+ in = *pOut << 1;
+ *pOut++ = in;
+ in = *pOut << 1;
+ *pOut++ = in;
+ in = *pOut << 1;
+ *pOut++ = in;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ in = *pOut << 1;
+ *pOut++ = in;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q7.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q7.c new file mode 100644 index 0000000..9e3de09 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_fir_sparse_q7.c @@ -0,0 +1,341 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_fir_sparse_q7.c
+ * Description: Q7 sparse FIR filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup FIR_Sparse
+ @{
+ */
+
+/**
+ @brief Processing function for the Q7 sparse FIR filter.
+ @param[in] S points to an instance of the Q7 sparse FIR structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] pScratchIn points to a temporary buffer of size blockSize
+ @param[in] pScratchOut points to a temporary buffer of size blockSize
+ @param[in] blockSize number of input samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 32-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.7 format and multiplications yield a 2.14 result.
+ The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ The accumulator is then converted to 18.7 format by discarding the low 7 bits.
+ Finally, the result is truncated to 1.7 format.
+ */
+
+void arm_fir_sparse_q7(
+ arm_fir_sparse_instance_q7 * S,
+ const q7_t * pSrc,
+ q7_t * pDst,
+ q7_t * pScratchIn,
+ q31_t * pScratchOut,
+ uint32_t blockSize)
+{
+ q7_t *pState = S->pState; /* State pointer */
+ const q7_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
+ q7_t *px; /* Scratch buffer pointer */
+ q7_t *py = pState; /* Temporary pointers for state buffer */
+ q7_t *pb = pScratchIn; /* Temporary pointers for scratch buffer */
+ q7_t *pOut = pDst; /* Destination pointer */
+ int32_t *pTapDelay = S->pTapDelay; /* Pointer to the array containing offset of the non-zero tap values. */
+ uint32_t delaySize = S->maxDelay + blockSize; /* state length */
+ uint16_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ int32_t readIndex; /* Read index of the state buffer */
+ uint32_t tapCnt, blkCnt; /* loop counters */
+ q31_t *pScr2 = pScratchOut; /* Working pointer for scratch buffer of output values */
+ q31_t in;
+ q7_t coeff = *pCoeffs++; /* Read the coefficient value */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ q7_t in1, in2, in3, in4;
+#endif
+
+ /* BlockSize of Input samples are copied into the state buffer */
+ /* StateIndex points to the starting position to write in the state buffer */
+ arm_circularWrite_q7(py, (int32_t) delaySize, &S->stateIndex, 1, pSrc, 1, blockSize);
+
+ /* Loop over the number of taps. */
+ tapCnt = numTaps;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_q7(py, (int32_t) delaySize, &readIndex, 1,
+ pb, pb, (int32_t) blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pScratchOut = pScr2;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform multiplication and store in the scratch buffer */
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiplication and store in the scratch buffer */
+ *pScratchOut++ = ((q31_t) *px++ * coeff);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Loop over the number of taps. */
+ tapCnt = (uint32_t) numTaps - 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_q7(py, (int32_t) delaySize, &readIndex, 1,
+ pb, pb, (int32_t) blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pScratchOut = pScr2;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ in = *pScratchOut + ((q31_t) * px++ * coeff);
+ *pScratchOut++ = in;
+ in = *pScratchOut + ((q31_t) * px++ * coeff);
+ *pScratchOut++ = in;
+ in = *pScratchOut + ((q31_t) * px++ * coeff);
+ *pScratchOut++ = in;
+ in = *pScratchOut + ((q31_t) * px++ * coeff);
+ *pScratchOut++ = in;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ in = *pScratchOut + ((q31_t) *px++ * coeff);
+ *pScratchOut++ = in;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Load the coefficient value and
+ * increment the coefficient buffer for the next set of state values */
+ coeff = *pCoeffs++;
+
+ /* Read Index, from where the state buffer should be read, is calculated. */
+ readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
+
+ /* Wraparound of readIndex */
+ if (readIndex < 0)
+ {
+ readIndex += (int32_t) delaySize;
+ }
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Compute last tap without the final read of pTapDelay */
+
+ /* Working pointer for state buffer is updated */
+ py = pState;
+
+ /* blockSize samples are read from the state buffer */
+ arm_circularRead_q7(py, (int32_t) delaySize, &readIndex, 1,
+ pb, pb, (int32_t) blockSize, 1, blockSize);
+
+ /* Working pointer for the scratch buffer of state values */
+ px = pb;
+
+ /* Working pointer for scratch buffer of output values */
+ pScratchOut = pScr2;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ in = *pScratchOut + ((q31_t) *px++ * coeff);
+ *pScratchOut++ = in;
+ in = *pScratchOut + ((q31_t) *px++ * coeff);
+ *pScratchOut++ = in;
+ in = *pScratchOut + ((q31_t) *px++ * coeff);
+ *pScratchOut++ = in;
+ in = *pScratchOut + ((q31_t) *px++ * coeff);
+ *pScratchOut++ = in;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ /* Perform Multiply-Accumulate */
+ in = *pScratchOut + ((q31_t) *px++ * coeff);
+ *pScratchOut++ = in;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* All the output values are in pScratchOut buffer.
+ Convert them into 1.15 format, saturate and store in the destination buffer. */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 outputs at a time. */
+ blkCnt = blockSize >> 2U;
+
+ while (blkCnt > 0U)
+ {
+ in1 = (q7_t) __SSAT(*pScr2++ >> 7, 8);
+ in2 = (q7_t) __SSAT(*pScr2++ >> 7, 8);
+ in3 = (q7_t) __SSAT(*pScr2++ >> 7, 8);
+ in4 = (q7_t) __SSAT(*pScr2++ >> 7, 8);
+
+ write_q7x4_ia (&pOut, __PACKq7(in1, in2, in3, in4));
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining outputs */
+ blkCnt = blockSize % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ blkCnt = blockSize;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (blkCnt > 0U)
+ {
+ *pOut++ = (q7_t) __SSAT(*pScr2++ >> 7, 8);
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+}
+
+/**
+ @} end of FIR_Sparse group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_f32.c new file mode 100644 index 0000000..cc8bff6 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_f32.c @@ -0,0 +1,354 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_iir_lattice_f32.c
+ * Description: Floating-point IIR Lattice filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup IIR_Lattice Infinite Impulse Response (IIR) Lattice Filters
+
+ This set of functions implements lattice filters
+ for Q15, Q31 and floating-point data types. Lattice filters are used in a
+ variety of adaptive filter applications. The filter structure has feedforward and
+ feedback components and the net impulse response is infinite length.
+ The functions operate on blocks
+ of input and output data and each call to the function processes
+ <code>blockSize</code> samples through the filter. <code>pSrc</code> and
+ <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
+
+ @par Algorithm
+ \image html IIRLattice.gif "Infinite Impulse Response Lattice filter"
+ @par
+ <pre>
+ fN(n) = x(n)
+ fm-1(n) = fm(n) - km * gm-1(n-1) for m = N, N-1, ..., 1
+ gm(n) = km * fm-1(n) + gm-1(n-1) for m = N, N-1, ..., 1
+ y(n) = vN * gN(n) + vN-1 * gN-1(n) + ...+ v0 * g0(n)
+ </pre>
+ @par
+ <code>pkCoeffs</code> points to array of reflection coefficients of size <code>numStages</code>.
+ Reflection Coefficients are stored in time-reversed order.
+ @par
+ <pre>
+ {kN, kN-1, ..., k1}
+ </pre>
+ @par
+ <code>pvCoeffs</code> points to the array of ladder coefficients of size <code>(numStages+1)</code>.
+ Ladder coefficients are stored in time-reversed order.
+ <pre>
+ {vN, vN-1, ..., v0}
+ </pre>
+ @par
+ <code>pState</code> points to a state array of size <code>numStages + blockSize</code>.
+ The state variables shown in the figure above (the g values) are stored in the <code>pState</code> array.
+ The state variables are updated after each block of data is processed; the coefficients are untouched.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter.
+ Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numStages, pkCoeffs, pvCoeffs, pState. Also set all of the values in pState to zero.
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
+ <pre>
+ arm_iir_lattice_instance_f32 S = {numStages, pState, pkCoeffs, pvCoeffs};
+ arm_iir_lattice_instance_q31 S = {numStages, pState, pkCoeffs, pvCoeffs};
+ arm_iir_lattice_instance_q15 S = {numStages, pState, pkCoeffs, pvCoeffs};
+ </pre>
+ @par
+ where <code>numStages</code> is the number of stages in the filter; <code>pState</code> points to the state buffer array;
+ <code>pkCoeffs</code> points to array of the reflection coefficients; <code>pvCoeffs</code> points to the array of ladder coefficients.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the fixed-point versions of the IIR lattice filter functions.
+ In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
+ Refer to the function specific documentation below for usage guidelines.
+ */
+
+/**
+ @addtogroup IIR_Lattice
+ @{
+ */
+
+/**
+ @brief Processing function for the floating-point IIR lattice filter.
+ @param[in] S points to an instance of the floating-point IIR lattice structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+void arm_iir_lattice_f32(
+ const arm_iir_lattice_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pDst,
+ uint32_t blockSize)
+{
+ float32_t *pState = S->pState; /* State pointer */
+ float32_t *pStateCur; /* State current pointer */
+ float32_t acc; /* Accumlator */
+ float32_t fnext1, fnext2, gcurr1, gnext; /* Temporary variables for lattice stages */
+ float32_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */
+ uint32_t numStages = S->numStages; /* Number of stages */
+ uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
+
+#if defined (ARM_MATH_LOOPUNROLL)
+ float32_t gcurr2; /* Temporary variables for lattice stages */
+ float32_t k1, k2;
+ float32_t v1, v2, v3, v4;
+#endif
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ /* Sample processing */
+ while (blkCnt > 0U)
+ {
+ /* Read Sample from input buffer */
+ /* fN(n) = x(n) */
+ fnext2 = *pSrc++;
+
+ /* Initialize Ladder coeff pointer */
+ pv = &S->pvCoeffs[0];
+
+ /* Initialize Reflection coeff pointer */
+ pk = &S->pkCoeffs[0];
+
+ /* Initialize state read pointer */
+ px1 = pState;
+
+ /* Initialize state write pointer */
+ px2 = pState;
+
+ /* Set accumulator to zero */
+ acc = 0.0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numStages) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Read gN-1(n-1) from state buffer */
+ gcurr1 = *px1;
+
+ /* read reflection coefficient kN */
+ k1 = *pk;
+
+ /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
+ fnext1 = fnext2 - (k1 * gcurr1);
+
+ /* read ladder coefficient vN */
+ v1 = *pv;
+
+ /* read next reflection coefficient kN-1 */
+ k2 = *(pk + 1U);
+
+ /* Read gN-2(n-1) from state buffer */
+ gcurr2 = *(px1 + 1U);
+
+ /* read next ladder coefficient vN-1 */
+ v2 = *(pv + 1U);
+
+ /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
+ fnext2 = fnext1 - (k2 * gcurr2);
+
+ /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
+ gnext = gcurr1 + (k1 * fnext1);
+
+ /* read reflection coefficient kN-2 */
+ k1 = *(pk + 2U);
+
+ /* write gN(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* Read gN-3(n-1) from state buffer */
+ gcurr1 = *(px1 + 2U);
+
+ /* y(n) += gN(n) * vN */
+ acc += (gnext * v1);
+
+ /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
+ fnext1 = fnext2 - (k1 * gcurr1);
+
+ /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
+ gnext = gcurr2 + (k2 * fnext2);
+
+ /* Read gN-4(n-1) from state buffer */
+ gcurr2 = *(px1 + 3U);
+
+ /* y(n) += gN-1(n) * vN-1 */
+ acc += (gnext * v2);
+
+ /* read reflection coefficient kN-3 */
+ k2 = *(pk + 3U);
+
+ /* write gN-1(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
+ fnext2 = fnext1 - (k2 * gcurr2);
+
+ /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
+ gnext = gcurr1 + (k1 * fnext1);
+
+ /* read ladder coefficient vN-2 */
+ v3 = *(pv + 2U);
+
+ /* y(n) += gN-2(n) * vN-2 */
+ acc += (gnext * v3);
+
+ /* write gN-2(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* update pointer */
+ pk += 4U;
+
+ /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
+ gnext = (fnext2 * k2) + gcurr2;
+
+ /* read next ladder coefficient vN-3 */
+ v4 = *(pv + 3U);
+
+ /* y(n) += gN-4(n) * vN-4 */
+ acc += (gnext * v4);
+
+ /* write gN-3(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* update pointers */
+ px1 += 4U;
+ pv += 4U;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numStages % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numStages;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ gcurr1 = *px1++;
+ /* Process sample for last taps */
+ fnext1 = fnext2 - ((*pk) * gcurr1);
+ gnext = (fnext1 * (*pk++)) + gcurr1;
+ /* Output samples for last taps */
+ acc += (gnext * (*pv++));
+ *px2++ = gnext;
+ fnext2 = fnext1;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* y(n) += g0(n) * v0 */
+ acc += (fnext2 * (*pv));
+
+ *px2++ = fnext2;
+
+ /* write out into pDst */
+ *pDst++ = acc;
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 1U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete. Now copy last S->numStages samples to start of the buffer
+ for the preperation of next frame process */
+
+ /* Points to the start of the state buffer */
+ pStateCur = &S->pState[0];
+ pState = &S->pState[blockSize];
+
+ /* Copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numStages >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numStages % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ tapCnt = numStages;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of IIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_f32.c new file mode 100644 index 0000000..459ed92 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_f32.c @@ -0,0 +1,77 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_iir_lattice_init_f32.c
+ * Description: Floating-point IIR lattice filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup IIR_Lattice
+ @{
+ */
+
+/**
+ @brief Initialization function for the floating-point IIR lattice filter.
+ @param[in] S points to an instance of the floating-point IIR lattice structure
+ @param[in] numStages number of stages in the filter
+ @param[in] pkCoeffs points to reflection coefficient buffer. The array is of length numStages
+ @param[in] pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1
+ @param[in] pState points to state buffer. The array is of length numStages+blockSize
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+void arm_iir_lattice_init_f32(
+ arm_iir_lattice_instance_f32 * S,
+ uint16_t numStages,
+ float32_t * pkCoeffs,
+ float32_t * pvCoeffs,
+ float32_t * pState,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numStages = numStages;
+
+ /* Assign reflection coefficient pointer */
+ S->pkCoeffs = pkCoeffs;
+
+ /* Assign ladder coefficient pointer */
+ S->pvCoeffs = pvCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numStages */
+ memset(pState, 0, (numStages + blockSize) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of IIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q15.c new file mode 100644 index 0000000..1e16932 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q15.c @@ -0,0 +1,77 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_iir_lattice_init_q15.c
+ * Description: Q15 IIR lattice filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup IIR_Lattice
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 IIR lattice filter.
+ @param[in] S points to an instance of the Q15 IIR lattice structure
+ @param[in] numStages number of stages in the filter
+ @param[in] pkCoeffs points to reflection coefficient buffer. The array is of length numStages
+ @param[in] pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1
+ @param[in] pState points to state buffer. The array is of length numStages+blockSize
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+void arm_iir_lattice_init_q15(
+ arm_iir_lattice_instance_q15 * S,
+ uint16_t numStages,
+ q15_t * pkCoeffs,
+ q15_t * pvCoeffs,
+ q15_t * pState,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numStages = numStages;
+
+ /* Assign reflection coefficient pointer */
+ S->pkCoeffs = pkCoeffs;
+
+ /* Assign ladder coefficient pointer */
+ S->pvCoeffs = pvCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numStages */
+ memset(pState, 0, (numStages + blockSize) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of IIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q31.c new file mode 100644 index 0000000..a1efae0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_init_q31.c @@ -0,0 +1,77 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_iir_lattice_init_q31.c
+ * Description: Initialization function for the Q31 IIR lattice filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup IIR_Lattice
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q31 IIR lattice filter.
+ @param[in] S points to an instance of the Q31 IIR lattice structure
+ @param[in] numStages number of stages in the filter
+ @param[in] pkCoeffs points to reflection coefficient buffer. The array is of length numStages
+ @param[in] pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1
+ @param[in] pState points to state buffer. The array is of length numStages+blockSize
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+void arm_iir_lattice_init_q31(
+ arm_iir_lattice_instance_q31 * S,
+ uint16_t numStages,
+ q31_t * pkCoeffs,
+ q31_t * pvCoeffs,
+ q31_t * pState,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numStages = numStages;
+
+ /* Assign reflection coefficient pointer */
+ S->pkCoeffs = pkCoeffs;
+
+ /* Assign ladder coefficient pointer */
+ S->pvCoeffs = pvCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numStages */
+ memset(pState, 0, (numStages + blockSize) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+}
+
+/**
+ @} end of IIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q15.c new file mode 100644 index 0000000..2651dd5 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q15.c @@ -0,0 +1,396 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_iir_lattice_q15.c
+ * Description: Q15 IIR Lattice filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup IIR_Lattice
+ @{
+ */
+
+/**
+ @brief Processing function for the Q15 IIR lattice filter.
+ @param[in] S points to an instance of the Q15 IIR lattice structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+ Lastly, the accumulator is saturated to yield a result in 1.15 format.
+ */
+
+void arm_iir_lattice_q15(
+ const arm_iir_lattice_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pDst,
+ uint32_t blockSize)
+{
+ q15_t *pState = S->pState; /* State pointer */
+ q15_t *pStateCur; /* State current pointer */
+ q31_t fcurr, fnext = 0, gcurr = 0, gnext; /* Temporary variables for lattice stages */
+ q63_t acc; /* Accumlator */
+ q15_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */
+ uint32_t numStages = S->numStages; /* Number of stages */
+ uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
+ q15_t out; /* Temporary variable for output */
+
+#if defined (ARM_MATH_DSP) && defined (ARM_MATH_LOOPUNROLL)
+ q15_t gnext1, gnext2; /* Temporary variables for lattice stages */
+ q31_t v; /* Temporary variable for ladder coefficient */
+#endif
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+#if defined (ARM_MATH_DSP)
+
+ /* Sample processing */
+ while (blkCnt > 0U)
+ {
+ /* Read Sample from input buffer */
+ /* fN(n) = x(n) */
+ fcurr = *pSrc++;
+
+ /* Initialize Ladder coeff pointer */
+ pv = &S->pvCoeffs[0];
+
+ /* Initialize Reflection coeff pointer */
+ pk = &S->pkCoeffs[0];
+
+ /* Initialize state read pointer */
+ px1 = pState;
+
+ /* Initialize state write pointer */
+ px2 = pState;
+
+ /* Set accumulator to zero */
+ acc = 0;
+
+ /* Process sample for first tap */
+ gcurr = *px1++;
+ /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
+ fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
+ fnext = __SSAT(fnext, 16);
+
+ /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
+ gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
+ gnext = __SSAT(gnext, 16);
+
+ /* write gN(n) into state for next sample processing */
+ *px2++ = (q15_t) gnext;
+
+ /* y(n) += gN(n) * vN */
+ acc += (q31_t) ((gnext * (*pv++)));
+
+ /* Update f values for next coefficient processing */
+ fcurr = fnext;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numStages - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Process sample for 2nd, 6th ...taps */
+ /* Read gN-2(n-1) from state buffer */
+ gcurr = *px1++;
+ /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
+ fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
+ fnext = __SSAT(fnext, 16);
+ /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
+ gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
+ gnext1 = (q15_t) __SSAT(gnext, 16);
+ /* write gN-1(n) into state for next sample processing */
+ *px2++ = (q15_t) gnext1;
+
+ /* Process sample for 3nd, 7th ...taps */
+ /* Read gN-3(n-1) from state buffer */
+ gcurr = *px1++;
+ /* Process sample for 3rd, 7th .. taps */
+ /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
+ fcurr = fnext - (((q31_t) gcurr * (*pk)) >> 15);
+ fcurr = __SSAT(fcurr, 16);
+ /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
+ gnext = (((q31_t) fcurr * (*pk++)) >> 15) + gcurr;
+ gnext2 = (q15_t) __SSAT(gnext, 16);
+ /* write gN-2(n) into state */
+ *px2++ = (q15_t) gnext2;
+
+ /* Read vN-1 and vN-2 at a time */
+ v = read_q15x2_ia (&pv);
+
+ /* Pack gN-1(n) and gN-2(n) */
+
+#ifndef ARM_MATH_BIG_ENDIAN
+ gnext = __PKHBT(gnext1, gnext2, 16);
+#else
+ gnext = __PKHBT(gnext2, gnext1, 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* y(n) += gN-1(n) * vN-1 */
+ /* process for gN-5(n) * vN-5, gN-9(n) * vN-9 ... */
+ /* y(n) += gN-2(n) * vN-2 */
+ /* process for gN-6(n) * vN-6, gN-10(n) * vN-10 ... */
+ acc = __SMLALD(gnext, v, acc);
+
+ /* Process sample for 4th, 8th ...taps */
+ /* Read gN-4(n-1) from state buffer */
+ gcurr = *px1++;
+ /* Process sample for 4th, 8th .. taps */
+ /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
+ fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
+ fnext = __SSAT(fnext, 16);
+ /* gN-3(n) = kN-3 * fN-1(n) + gN-1(n-1) */
+ gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
+ gnext1 = (q15_t) __SSAT(gnext, 16);
+ /* write gN-3(n) for the next sample process */
+ *px2++ = (q15_t) gnext1;
+
+ /* Process sample for 5th, 9th ...taps */
+ /* Read gN-5(n-1) from state buffer */
+ gcurr = *px1++;
+ /* Process sample for 5th, 9th .. taps */
+ /* fN-5(n) = fN-4(n) - kN-4 * gN-5(n-1) */
+ fcurr = fnext - (((q31_t) gcurr * (*pk)) >> 15);
+ fcurr = __SSAT(fcurr, 16);
+ /* gN-4(n) = kN-4 * fN-5(n) + gN-5(n-1) */
+ gnext = (((q31_t) fcurr * (*pk++)) >> 15) + gcurr;
+ gnext2 = (q15_t) __SSAT(gnext, 16);
+ /* write gN-4(n) for the next sample process */
+ *px2++ = (q15_t) gnext2;
+
+ /* Read vN-3 and vN-4 at a time */
+ v = read_q15x2_ia (&pv);
+
+ /* Pack gN-3(n) and gN-4(n) */
+#ifndef ARM_MATH_BIG_ENDIAN
+ gnext = __PKHBT(gnext1, gnext2, 16);
+#else
+ gnext = __PKHBT(gnext2, gnext1, 16);
+#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
+
+ /* y(n) += gN-4(n) * vN-4 */
+ /* process for gN-8(n) * vN-8, gN-12(n) * vN-12 ... */
+ /* y(n) += gN-3(n) * vN-3 */
+ /* process for gN-7(n) * vN-7, gN-11(n) * vN-11 ... */
+ acc = __SMLALD(gnext, v, acc);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ fnext = fcurr;
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numStages - 1U) % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ tapCnt = (numStages - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ gcurr = *px1++;
+ /* Process sample for last taps */
+ fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
+ fnext = __SSAT(fnext, 16);
+ gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
+ gnext = __SSAT(gnext, 16);
+
+ /* Output samples for last taps */
+ acc += (q31_t) (((q31_t) gnext * (*pv++)));
+ *px2++ = (q15_t) gnext;
+ fcurr = fnext;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* y(n) += g0(n) * v0 */
+ acc += (q31_t) (((q31_t) fnext * (*pv++)));
+
+ out = (q15_t) __SSAT(acc >> 15, 16);
+ *px2++ = (q15_t) fnext;
+
+ /* write out into pDst */
+ *pDst++ = out;
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 1U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete. Now copy last S->numStages samples to start of the buffer
+ for the preperation of next frame process */
+
+ /* Points to the start of the state buffer */
+ pStateCur = &S->pState[0];
+ pState = &S->pState[blockSize];
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numStages >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+ write_q15x2_ia (&pStateCur, read_q15x2_ia (&pState));
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numStages % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ tapCnt = (numStages - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+ /* Sample processing */
+ while (blkCnt > 0U)
+ {
+ /* Read Sample from input buffer */
+ /* fN(n) = x(n) */
+ fcurr = *pSrc++;
+
+ /* Initialize Ladder coeff pointer */
+ pv = &S->pvCoeffs[0];
+
+ /* Initialize Reflection coeff pointer */
+ pk = &S->pkCoeffs[0];
+
+ /* Initialize state read pointer */
+ px1 = pState;
+
+ /* Initialize state write pointer */
+ px2 = pState;
+
+ /* Set accumulator to zero */
+ acc = 0;
+
+ tapCnt = numStages;
+
+ while (tapCnt > 0U)
+ {
+ gcurr = *px1++;
+ /* Process sample */
+ /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
+ fnext = fcurr - ((gcurr * (*pk)) >> 15);
+ fnext = __SSAT(fnext, 16);
+
+ /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
+ gnext = ((fnext * (*pk++)) >> 15) + gcurr;
+ gnext = __SSAT(gnext, 16);
+
+ /* Output samples */
+ /* y(n) += gN(n) * vN */
+ acc += (q31_t) ((gnext * (*pv++)));
+
+ /* write gN(n) into state for next sample processing */
+ *px2++ = (q15_t) gnext;
+
+ /* Update f values for next coefficient processing */
+ fcurr = fnext;
+
+ tapCnt--;
+ }
+
+ /* y(n) += g0(n) * v0 */
+ acc += (q31_t) ((fnext * (*pv++)));
+
+ out = (q15_t) __SSAT(acc >> 15, 16);
+ *px2++ = (q15_t) fnext;
+
+ /* write out into pDst */
+ *pDst++ = out;
+
+ /* Advance the state pointer by 1 to process the next group of samples */
+ pState = pState + 1U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete. Now copy last S->numStages samples to start of the buffer
+ for the preperation of next frame process */
+
+ /* Points to the start of the state buffer */
+ pStateCur = &S->pState[0];
+ pState = &S->pState[blockSize];
+
+ tapCnt = numStages;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of IIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q31.c new file mode 100644 index 0000000..d0348fe --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_iir_lattice_q31.c @@ -0,0 +1,356 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_iir_lattice_q31.c
+ * Description: Q31 IIR Lattice filter processing function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup IIR_Lattice
+ @{
+ */
+
+/**
+ @brief Processing function for the Q31 IIR lattice filter.
+ @param[in] S points to an instance of the Q31 IIR lattice structure
+ @param[in] pSrc points to the block of input data
+ @param[out] pDst points to the block of output data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by 2*log2(numStages) bits.
+ After all multiply-accumulates are performed, the 2.62 accumulator is saturated to 1.32 format and then truncated to 1.31 format.
+ */
+
+void arm_iir_lattice_q31(
+ const arm_iir_lattice_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pDst,
+ uint32_t blockSize)
+{
+ q31_t *pState = S->pState; /* State pointer */
+ q31_t *pStateCur; /* State current pointer */
+ q31_t fcurr, fnext = 0, gcurr = 0, gnext; /* Temporary variables for lattice stages */
+ q63_t acc; /* Accumlator */
+ q31_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */
+ uint32_t numStages = S->numStages; /* Number of stages */
+ uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
+
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+#if defined (ARM_MATH_DSP)
+
+ /* Sample processing */
+ while (blkCnt > 0U)
+ {
+ /* Read Sample from input buffer */
+ /* fN(n) = x(n) */
+ fcurr = *pSrc++;
+
+ /* Initialize Ladder coeff pointer */
+ pv = &S->pvCoeffs[0];
+
+ /* Initialize Reflection coeff pointer */
+ pk = &S->pkCoeffs[0];
+
+ /* Initialize state read pointer */
+ px1 = pState;
+
+ /* Initialize state write pointer */
+ px2 = pState;
+
+ /* Set accumulator to zero */
+ acc = 0;
+
+ /* Process sample for first tap */
+ gcurr = *px1++;
+ /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
+ fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk )) >> 31));
+
+ /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
+ gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
+
+ /* write gN-1(n-1) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* y(n) += gN(n) * vN */
+ acc += ((q63_t) gnext * *pv++);
+
+ /* Update f values for next coefficient processing */
+ fcurr = fnext;
+
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numStages - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Process sample for 2nd, 6th ...taps */
+ /* Read gN-2(n-1) from state buffer */
+ gcurr = *px1++;
+ /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
+ fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk )) >> 31));
+ /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
+ gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
+ /* y(n) += gN-1(n) * vN-1 */
+ /* process for gN-5(n) * vN-5, gN-9(n) * vN-9 ... */
+ acc += ((q63_t) gnext * *pv++);
+ /* write gN-1(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* Process sample for 3nd, 7th ...taps */
+ /* Read gN-3(n-1) from state buffer */
+ gcurr = *px1++;
+ /* Process sample for 3rd, 7th .. taps */
+ /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
+ fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk )) >> 31));
+ /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
+ gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31));
+ /* y(n) += gN-2(n) * vN-2 */
+ /* process for gN-6(n) * vN-6, gN-10(n) * vN-10 ... */
+ acc += ((q63_t) gnext * *pv++);
+ /* write gN-2(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* Process sample for 4th, 8th ...taps */
+ /* Read gN-4(n-1) from state buffer */
+ gcurr = *px1++;
+ /* Process sample for 4th, 8th .. taps */
+ /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
+ fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk )) >> 31));
+ /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
+ gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
+ /* y(n) += gN-3(n) * vN-3 */
+ /* process for gN-7(n) * vN-7, gN-11(n) * vN-11 ... */
+ acc += ((q63_t) gnext * *pv++);
+ /* write gN-3(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* Process sample for 5th, 9th ...taps */
+ /* Read gN-5(n-1) from state buffer */
+ gcurr = *px1++;
+ /* Process sample for 5th, 9th .. taps */
+ /* fN-5(n) = fN-4(n) - kN-4 * gN-1(n-1) */
+ fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk )) >> 31));
+ /* gN-4(n) = kN-4 * fN-5(n) + gN-5(n-1) */
+ gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31));
+ /* y(n) += gN-4(n) * vN-4 */
+ /* process for gN-8(n) * vN-8, gN-12(n) * vN-12 ... */
+ acc += ((q63_t) gnext * *pv++);
+
+ /* write gN-4(n) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ fnext = fcurr;
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numStages - 1U) % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ tapCnt = (numStages - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ gcurr = *px1++;
+ /* Process sample for last taps */
+ fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk )) >> 31));
+ gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
+
+ /* Output samples for last taps */
+ acc += ((q63_t) gnext * *pv++);
+ *px2++ = gnext;
+ fcurr = fnext;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* y(n) += g0(n) * v0 */
+ acc += ((q63_t) fnext * *pv++);
+
+ *px2++ = fnext;
+
+ /* write out into pDst */
+ *pDst++ = (q31_t) (acc >> 31U);
+
+ /* Advance the state pointer by 4 to process the next group of 4 samples */
+ pState = pState + 1U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete. Now copy last S->numStages samples to start of the buffer
+ for the preperation of next frame process */
+
+ /* Points to the start of the state buffer */
+ pStateCur = &S->pState[0];
+ pState = &S->pState[blockSize];
+
+ /* Copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numStages >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numStages % 0x4U;
+
+#else
+
+ /* Initialize blkCnt with number of samples */
+ tapCnt = (numStages - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#else /* #if defined (ARM_MATH_DSP) */
+
+ /* Sample processing */
+ while (blkCnt > 0U)
+ {
+ /* Read Sample from input buffer */
+ /* fN(n) = x(n) */
+ fcurr = *pSrc++;
+
+ /* Initialize Ladder coeff pointer */
+ pv = &S->pvCoeffs[0];
+
+ /* Initialize Reflection coeff pointer */
+ pk = &S->pkCoeffs[0];
+
+ /* Initialize state read pointer */
+ px1 = pState;
+
+ /* Initialize state write pointer */
+ px2 = pState;
+
+ /* Set accumulator to zero */
+ acc = 0;
+
+ tapCnt = numStages;
+
+ while (tapCnt > 0U)
+ {
+ gcurr = *px1++;
+ /* Process sample */
+ /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
+ fnext = clip_q63_to_q31(((q63_t) fcurr - ((q31_t) (((q63_t) gcurr * (*pk )) >> 31))));
+
+ /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
+ gnext = clip_q63_to_q31(((q63_t) gcurr + ((q31_t) (((q63_t) fnext * (*pk++)) >> 31))));
+
+ /* Output samples */
+ /* y(n) += gN(n) * vN */
+ acc += ((q63_t) gnext * *pv++);
+
+ /* write gN-1(n-1) into state for next sample processing */
+ *px2++ = gnext;
+
+ /* Update f values for next coefficient processing */
+ fcurr = fnext;
+
+ tapCnt--;
+ }
+
+ /* y(n) += g0(n) * v0 */
+ acc += ((q63_t) fnext * *pv++);
+
+ *px2++ = fnext;
+
+ /* write out into pDst */
+ *pDst++ = (q31_t) (acc >> 31U);
+
+ /* Advance the state pointer by 1 to process the next group of samples */
+ pState = pState + 1U;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Processing is complete. Now copy last S->numStages samples to start of the buffer
+ for the preperation of next frame process */
+
+ /* Points to the start of the state buffer */
+ pStateCur = &S->pState[0];
+ pState = &S->pState[blockSize];
+
+ tapCnt = numStages;
+
+ /* Copy data */
+ while (tapCnt > 0U)
+ {
+ *pStateCur++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+#endif /* #if defined (ARM_MATH_DSP) */
+
+}
+
+/**
+ @} end of IIR_Lattice group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_f32.c new file mode 100644 index 0000000..edcb8e3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_f32.c @@ -0,0 +1,533 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_f32.c
+ * Description: Processing function for the floating-point LMS filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup LMS Least Mean Square (LMS) Filters
+
+ LMS filters are a class of adaptive filters that are able to "learn" an unknown transfer functions.
+ LMS filters use a gradient descent method in which the filter coefficients are updated based on the instantaneous error signal.
+ Adaptive filters are often used in communication systems, equalizers, and noise removal.
+ The CMSIS DSP Library contains LMS filter functions that operate on Q15, Q31, and floating-point data types.
+ The library also contains normalized LMS filters in which the filter coefficient adaptation is indepedent of the level of the input signal.
+
+ An LMS filter consists of two components as shown below.
+ The first component is a standard transversal or FIR filter.
+ The second component is a coefficient update mechanism.
+ The LMS filter has two input signals.
+ The "input" feeds the FIR filter while the "reference input" corresponds to the desired output of the FIR filter.
+ That is, the FIR filter coefficients are updated so that the output of the FIR filter matches the reference input.
+ The filter coefficient update mechanism is based on the difference between the FIR filter output and the reference input.
+ This "error signal" tends towards zero as the filter adapts.
+ The LMS processing functions accept the input and reference input signals and generate the filter output and error signal.
+ \image html LMS.gif "Internal structure of the Least Mean Square filter"
+
+ The functions operate on blocks of data and each call to the function processes
+ <code>blockSize</code> samples through the filter.
+ <code>pSrc</code> points to input signal, <code>pRef</code> points to reference signal,
+ <code>pOut</code> points to output signal and <code>pErr</code> points to error signal.
+ All arrays contain <code>blockSize</code> values.
+
+ The functions operate on a block-by-block basis.
+ Internally, the filter coefficients <code>b[n]</code> are updated on a sample-by-sample basis.
+ The convergence of the LMS filter is slower compared to the normalized LMS algorithm.
+
+ @par Algorithm
+ The output signal <code>y[n]</code> is computed by a standard FIR filter:
+ <pre>
+ y[n] = b[0] * x[n] + b[1] * x[n-1] + b[2] * x[n-2] + ...+ b[numTaps-1] * x[n-numTaps+1]
+ </pre>
+
+ @par
+ The error signal equals the difference between the reference signal <code>d[n]</code> and the filter output:
+ <pre>
+ e[n] = d[n] - y[n].
+ </pre>
+
+ @par
+ After each sample of the error signal is computed, the filter coefficients <code>b[k]</code> are updated on a sample-by-sample basis:
+ <pre>
+ b[k] = b[k] + e[n] * mu * x[n-k], for k=0, 1, ..., numTaps-1
+ </pre>
+ where <code>mu</code> is the step size and controls the rate of coefficient convergence.
+ @par
+ In the APIs, <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
+ Coefficients are stored in time reversed order.
+ @par
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to a state array of size <code>numTaps + blockSize - 1</code>.
+ Samples in the state buffer are stored in the order:
+ @par
+ <pre>
+ {x[n-numTaps+1], x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2]....x[0], x[1], ..., x[blockSize-1]}
+ </pre>
+ @par
+ Note that the length of the state buffer exceeds the length of the coefficient array by <code>blockSize-1</code> samples.
+ The increased state buffer length allows circular addressing, which is traditionally used in FIR filters,
+ to be avoided and yields a significant speed improvement.
+ The state variables are updated after each block of data is processed.
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter and
+ coefficient and state arrays cannot be shared among instances.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numTaps, pCoeffs, mu, postShift (not for f32), pState. Also set all of the values in pState to zero.
+
+ @par
+ Use of the initialization function is optional.
+ However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
+ To place an instance structure into a const data section, the instance structure must be manually initialized.
+ Set the values in the state buffer to zeros before static initialization.
+ The code below statically initializes each of the 3 different data type filter instance structures
+ <pre>
+ arm_lms_instance_f32 S = {numTaps, pState, pCoeffs, mu};
+ arm_lms_instance_q31 S = {numTaps, pState, pCoeffs, mu, postShift};
+ arm_lms_instance_q15 S = {numTaps, pState, pCoeffs, mu, postShift};
+ </pre>
+ where <code>numTaps</code> is the number of filter coefficients in the filter; <code>pState</code> is the address of the state buffer;
+ <code>pCoeffs</code> is the address of the coefficient buffer; <code>mu</code> is the step size parameter; and <code>postShift</code> is the shift applied to coefficients.
+
+ @par Fixed-Point Behavior
+ Care must be taken when using the Q15 and Q31 versions of the LMS filter.
+ The following issues must be considered:
+ - Scaling of coefficients
+ - Overflow and saturation
+
+ @par Scaling of Coefficients
+ Filter coefficients are represented as fractional values and
+ coefficients are restricted to lie in the range <code>[-1 +1)</code>.
+ The fixed-point functions have an additional scaling parameter <code>postShift</code>.
+ At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
+ This essentially scales the filter coefficients by <code>2^postShift</code> and
+ allows the filter coefficients to exceed the range <code>[+1 -1)</code>.
+ The value of <code>postShift</code> is set by the user based on the expected gain through the system being modeled.
+
+ @par Overflow and Saturation
+ Overflow and saturation behavior of the fixed-point Q15 and Q31 versions are
+ described separately as part of the function specific documentation below.
+ */
+
+/**
+ @addtogroup LMS
+ @{
+ */
+
+/**
+ @brief Processing function for floating-point LMS filter.
+ @param[in] S points to an instance of the floating-point LMS filter structure
+ @param[in] pSrc points to the block of input data
+ @param[in] pRef points to the block of reference data
+ @param[out] pOut points to the block of output data
+ @param[out] pErr points to the block of error data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+#if defined(ARM_MATH_NEON)
+void arm_lms_f32(
+ const arm_lms_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pRef,
+ float32_t * pOut,
+ float32_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ float32_t mu = S->mu; /* Adaptive factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ float32_t sum, e, d; /* accumulator, error, reference data sample */
+ float32_t w = 0.0f; /* weight factor */
+
+ float32x4_t tempV, sumV, xV, bV;
+ float32x2_t tempV2;
+
+ e = 0.0f;
+ d = 0.0f;
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = (pCoeffs);
+
+ /* Set the accumulator to zero */
+ sum = 0.0f;
+ sumV = vdupq_n_f32(0.0);
+
+ /* Process 4 taps at a time. */
+ tapCnt = numTaps >> 2;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ xV = vld1q_f32(px);
+ bV = vld1q_f32(pb);
+ sumV = vmlaq_f32(sumV, xV, bV);
+
+ px += 4;
+ pb += 4;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+ tempV2 = vpadd_f32(vget_low_f32(sumV),vget_high_f32(sumV));
+ sum = tempV2[0] + tempV2[1];
+
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (*px++) * (*pb++);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* The result in the accumulator, store in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Compute and store error */
+ d = (float32_t) (*pRef++);
+ e = d - sum;
+ *pErr++ = e;
+
+ /* Calculation of Weighting factor for the updating filter coefficients */
+ w = e * mu;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = (pCoeffs);
+
+ /* Process 4 taps at a time. */
+ tapCnt = numTaps >> 2;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ xV = vld1q_f32(px);
+ bV = vld1q_f32(pb);
+ px += 4;
+ bV = vmlaq_n_f32(bV,xV,w);
+
+ vst1q_f32(pb,bV);
+ pb += 4;
+
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ *pb = *pb + (w * (*px++));
+ pb++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+
+
+ /* Processing is complete. Now copy the last numTaps - 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* Process 4 taps at a time for (numTaps - 1U) samples copy */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* copy data */
+ while (tapCnt > 0U)
+ {
+ tempV = vld1q_f32(pState);
+ vst1q_f32(pStateCurnt,tempV);
+ pState += 4;
+ pStateCurnt += 4;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+ /* Copy the remaining q31_t data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+
+}
+#else
+void arm_lms_f32(
+ const arm_lms_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pRef,
+ float32_t * pOut,
+ float32_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ float32_t mu = S->mu; /* Adaptive factor */
+ float32_t acc, e; /* Accumulator, error */
+ float32_t w; /* Weight factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+
+ /* Initializations of error, difference, Coefficient update */
+ e = 0.0f;
+ w = 0.0f;
+
+ /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Set the accumulator to zero */
+ acc = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += (*px++) * (*pb++);
+
+ acc += (*px++) * (*pb++);
+
+ acc += (*px++) * (*pb++);
+
+ acc += (*px++) * (*pb++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += (*px++) * (*pb++);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Store the result from accumulator into the destination buffer. */
+ *pOut++ = acc;
+
+ /* Compute and store error */
+ e = (float32_t) *pRef++ - acc;
+ *pErr++ = e;
+
+ /* Calculation of Weighting factor for updating filter coefficients */
+ w = e * mu;
+
+ /* Initialize pState pointer */
+ /* Advance state pointer by 1 for the next sample */
+ px = pState++;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ *pb += w * (*px++);
+ pb++;
+
+ *pb += w * (*px++);
+ pb++;
+
+ *pb += w * (*px++);
+ pb++;
+
+ *pb += w * (*px++);
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ *pb += w * (*px++);
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Decrement 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+#endif /* #if defined(ARM_MATH_NEON) */
+
+/**
+ @} end of LMS group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_f32.c new file mode 100644 index 0000000..d1618e0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_f32.c @@ -0,0 +1,81 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_init_f32.c
+ * Description: Floating-point LMS filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @addtogroup LMS
+ @{
+ */
+
+/**
+ @brief Initialization function for floating-point LMS filter.
+ @param[in] S points to an instance of the floating-point LMS filter structure
+ @param[in] numTaps number of filter coefficients
+ @param[in] pCoeffs points to coefficient buffer
+ @param[in] pState points to state buffer
+ @param[in] mu step size that controls filter coefficient updates
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ The initial filter coefficients serve as a starting point for the adaptive filter.
+ <code>pState</code> points to an array of length <code>numTaps+blockSize-1</code> samples, where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_lms_f32()</code>.
+ */
+
+void arm_lms_init_f32(
+ arm_lms_instance_f32 * S,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ float32_t mu,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numTaps */
+ memset(pState, 0, (numTaps + (blockSize - 1)) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Step size value */
+ S->mu = mu;
+}
+
+/**
+ @} end of LMS group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q15.c new file mode 100644 index 0000000..56c361b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q15.c @@ -0,0 +1,92 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_init_q15.c
+ * Description: Q15 LMS filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS
+ @{
+ */
+
+/**
+ @brief Initialization function for the Q15 LMS filter.
+ @param[in] S points to an instance of the Q15 LMS filter structure.
+ @param[in] numTaps number of filter coefficients.
+ @param[in] pCoeffs points to coefficient buffer.
+ @param[in] pState points to state buffer.
+ @param[in] mu step size that controls filter coefficient updates.
+ @param[in] blockSize number of samples to process.
+ @param[in] postShift bit shift applied to coefficients.
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ The initial filter coefficients serve as a starting point for the adaptive filter.
+ <code>pState</code> points to the array of state variables and size of array is
+ <code>numTaps+blockSize-1</code> samples, where <code>blockSize</code> is the number of
+ input samples processed by each call to <code>arm_lms_q15()</code>.
+ */
+
+void arm_lms_init_q15(
+ arm_lms_instance_q15 * S,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ q15_t mu,
+ uint32_t blockSize,
+ uint32_t postShift)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numTaps - 1 */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q15_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Step size value */
+ S->mu = mu;
+
+ /* Assign postShift value to be applied */
+ S->postShift = postShift;
+}
+
+/**
+ @} end of LMS group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q31.c new file mode 100644 index 0000000..559296a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_init_q31.c @@ -0,0 +1,92 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_init_q31.c
+ * Description: Q31 LMS filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS
+ @{
+ */
+
+/**
+ @brief Initialization function for Q31 LMS filter.
+ @param[in] S points to an instance of the Q31 LMS filter structure
+ @param[in] numTaps number of filter coefficients
+ @param[in] pCoeffs points to coefficient buffer
+ @param[in] pState points to state buffer
+ @param[in] mu step size that controls filter coefficient updates
+ @param[in] blockSize number of samples to process
+ @param[in] postShift bit shift applied to coefficients
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ The initial filter coefficients serve as a starting point for the adaptive filter.
+ <code>pState</code> points to an array of length <code>numTaps+blockSize-1</code> samples,
+ where <code>blockSize</code> is the number of input samples processed by each call to
+ <code>arm_lms_q31()</code>.
+ */
+
+void arm_lms_init_q31(
+ arm_lms_instance_q31 * S,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ q31_t mu,
+ uint32_t blockSize,
+ uint32_t postShift)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numTaps - 1 */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q31_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Step size value */
+ S->mu = mu;
+
+ /* Assign postShift value to be applied */
+ S->postShift = postShift;
+}
+
+/**
+ @} end of LMS group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_f32.c new file mode 100644 index 0000000..244de7f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_f32.c @@ -0,0 +1,564 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_norm_f32.c
+ * Description: Processing function for the floating-point NLMS filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @defgroup LMS_NORM Normalized LMS Filters
+
+ This set of functions implements a commonly used adaptive filter.
+ It is related to the Least Mean Square (LMS) adaptive filter and includes an additional normalization
+ factor which increases the adaptation rate of the filter.
+ The CMSIS DSP Library contains normalized LMS filter functions that operate on Q15, Q31, and floating-point data types.
+
+ A normalized least mean square (NLMS) filter consists of two components as shown below.
+ The first component is a standard transversal or FIR filter.
+ The second component is a coefficient update mechanism.
+ The NLMS filter has two input signals.
+ The "input" feeds the FIR filter while the "reference input" corresponds to the desired output of the FIR filter.
+ That is, the FIR filter coefficients are updated so that the output of the FIR filter matches the reference input.
+ The filter coefficient update mechanism is based on the difference between the FIR filter output and the reference input.
+ This "error signal" tends towards zero as the filter adapts.
+ The NLMS processing functions accept the input and reference input signals and generate the filter output and error signal.
+ \image html LMS.gif "Internal structure of the NLMS adaptive filter"
+
+ The functions operate on blocks of data and each call to the function processes
+ <code>blockSize</code> samples through the filter.
+ <code>pSrc</code> points to input signal, <code>pRef</code> points to reference signal,
+ <code>pOut</code> points to output signal and <code>pErr</code> points to error signal.
+ All arrays contain <code>blockSize</code> values.
+
+ The functions operate on a block-by-block basis.
+ Internally, the filter coefficients <code>b[n]</code> are updated on a sample-by-sample basis.
+ The convergence of the LMS filter is slower compared to the normalized LMS algorithm.
+
+ @par Algorithm
+ The output signal <code>y[n]</code> is computed by a standard FIR filter:
+ <pre>
+ y[n] = b[0] * x[n] + b[1] * x[n-1] + b[2] * x[n-2] + ...+ b[numTaps-1] * x[n-numTaps+1]
+ </pre>
+
+ @par
+ The error signal equals the difference between the reference signal <code>d[n]</code> and the filter output:
+ <pre>
+ e[n] = d[n] - y[n].
+ </pre>
+
+ @par
+ After each sample of the error signal is computed the instanteous energy of the filter state variables is calculated:
+ <pre>
+ E = x[n]^2 + x[n-1]^2 + ... + x[n-numTaps+1]^2.
+ </pre>
+ The filter coefficients <code>b[k]</code> are then updated on a sample-by-sample basis:
+ <pre>
+ b[k] = b[k] + e[n] * (mu/E) * x[n-k], for k=0, 1, ..., numTaps-1
+ </pre>
+ where <code>mu</code> is the step size and controls the rate of coefficient convergence.
+ @par
+ In the APIs, <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
+ Coefficients are stored in time reversed order.
+ @par
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ @par
+ <code>pState</code> points to a state array of size <code>numTaps + blockSize - 1</code>.
+ Samples in the state buffer are stored in the order:
+ @par
+ <pre>
+ {x[n-numTaps+1], x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2]....x[0], x[1], ..., x[blockSize-1]}
+ </pre>
+ @par
+ Note that the length of the state buffer exceeds the length of the coefficient array by <code>blockSize-1</code> samples.
+ The increased state buffer length allows circular addressing, which is traditionally used in FIR filters,
+ to be avoided and yields a significant speed improvement.
+ The state variables are updated after each block of data is processed.
+
+ @par Instance Structure
+ The coefficients and state variables for a filter are stored together in an instance data structure.
+ A separate instance structure must be defined for each filter and
+ coefficient and state arrays cannot be shared among instances.
+ There are separate instance structure declarations for each of the 3 supported data types.
+
+ @par Initialization Functions
+ There is also an associated initialization function for each data type.
+ The initialization function performs the following operations:
+ - Sets the values of the internal structure fields.
+ - Zeros out the values in the state buffer.
+ To do this manually without calling the init function, assign the follow subfields of the instance structure:
+ numTaps, pCoeffs, mu, energy, x0, pState. Also set all of the values in pState to zero.
+ For Q7, Q15, and Q31 the following fields must also be initialized;
+ recipTable, postShift
+ @par
+ Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
+ @par Fixed-Point Behavior
+ Care must be taken when using the Q15 and Q31 versions of the normalised LMS filter.
+ The following issues must be considered:
+ - Scaling of coefficients
+ - Overflow and saturation
+
+ @par Scaling of Coefficients
+ Filter coefficients are represented as fractional values and
+ coefficients are restricted to lie in the range <code>[-1 +1)</code>.
+ The fixed-point functions have an additional scaling parameter <code>postShift</code>.
+ At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
+ This essentially scales the filter coefficients by <code>2^postShift</code> and
+ allows the filter coefficients to exceed the range <code>[+1 -1)</code>.
+ The value of <code>postShift</code> is set by the user based on the expected gain through the system being modeled.
+
+ @par Overflow and Saturation
+ Overflow and saturation behavior of the fixed-point Q15 and Q31 versions are
+ described separately as part of the function specific documentation below.
+ */
+
+/**
+ @addtogroup LMS_NORM
+ @{
+ */
+
+/**
+ @brief Processing function for floating-point normalized LMS filter.
+ @param[in] S points to an instance of the floating-point normalized LMS filter structure
+ @param[in] pSrc points to the block of input data
+ @param[in] pRef points to the block of reference data
+ @param[out] pOut points to the block of output data
+ @param[out] pErr points to the block of error data
+ @param[in] blockSize number of samples to process
+ @return none
+ */
+
+#if defined(ARM_MATH_NEON)
+void arm_lms_norm_f32(
+ arm_lms_norm_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pRef,
+ float32_t * pOut,
+ float32_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ float32_t mu = S->mu; /* Adaptive factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ float32_t energy; /* Energy of the input */
+ float32_t sum, e, d; /* accumulator, error, reference data sample */
+ float32_t w, x0, in; /* weight factor, temporary variable to hold input sample and state */
+
+ float32x4_t tempV, sumV, xV, bV;
+ float32x2_t tempV2;
+
+ /* Initializations of error, difference, Coefficient update */
+ e = 0.0f;
+ d = 0.0f;
+ w = 0.0f;
+
+ energy = S->energy;
+ x0 = S->x0;
+
+ /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* Loop over blockSize number of values */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = (pCoeffs);
+
+ /* Read the sample from input buffer */
+ in = *pSrc++;
+
+ /* Update the energy calculation */
+ energy -= x0 * x0;
+ energy += in * in;
+
+ /* Set the accumulator to zero */
+ sum = 0.0f;
+ sumV = vdupq_n_f32(0.0);
+
+ /* Process 4 taps at a time. */
+ tapCnt = numTaps >> 2;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ xV = vld1q_f32(px);
+ bV = vld1q_f32(pb);
+ sumV = vmlaq_f32(sumV, xV, bV);
+
+ px += 4;
+ pb += 4;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+ tempV2 = vpadd_f32(vget_low_f32(sumV),vget_high_f32(sumV));
+ sum = tempV2[0] + tempV2[1];
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ sum += (*px++) * (*pb++);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* The result in the accumulator, store in the destination buffer. */
+ *pOut++ = sum;
+
+ /* Compute and store error */
+ d = (float32_t) (*pRef++);
+ e = d - sum;
+ *pErr++ = e;
+
+ /* Calculation of Weighting factor for updating filter coefficients */
+ /* epsilon value 0.000000119209289f */
+ w = (e * mu) / (energy + 0.000000119209289f);
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coeff pointer */
+ pb = (pCoeffs);
+
+ /* Process 4 taps at a time. */
+ tapCnt = numTaps >> 2;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ xV = vld1q_f32(px);
+ bV = vld1q_f32(pb);
+ px += 4;
+ bV = vmlaq_n_f32(bV,xV,w);
+
+ vst1q_f32(pb,bV);
+ pb += 4;
+
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* If the filter length is not a multiple of 4, compute the remaining filter taps */
+ tapCnt = numTaps % 0x4U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ *pb += w * (*px++);
+ pb++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ x0 = *pState;
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1;
+
+ /* Decrement the loop counter */
+ blkCnt--;
+ }
+
+ S->energy = energy;
+ S->x0 = x0;
+
+ /* Processing is complete. Now copy the last numTaps - 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* Process 4 taps at a time for (numTaps - 1U)/4 samples copy */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ /* copy data */
+ while (tapCnt > 0U)
+ {
+ tempV = vld1q_f32(pState);
+ vst1q_f32(pStateCurnt,tempV);
+ pState += 4;
+ pStateCurnt += 4;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Calculate remaining number of copies */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+ /* Copy the remaining q31_t data */
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+}
+#else
+void arm_lms_norm_f32(
+ arm_lms_norm_instance_f32 * S,
+ const float32_t * pSrc,
+ float32_t * pRef,
+ float32_t * pOut,
+ float32_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ float32_t mu = S->mu; /* Adaptive factor */
+ float32_t acc, e; /* Accumulator, error */
+ float32_t w; /* Weight factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ float32_t energy; /* Energy of the input */
+ float32_t x0, in; /* Temporary variable to hold input sample and state */
+
+ /* Initializations of error, difference, Coefficient update */
+ e = 0.0f;
+ w = 0.0f;
+
+ energy = S->energy;
+ x0 = S->x0;
+
+ /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Read the sample from input buffer */
+ in = *pSrc++;
+
+ /* Update the energy calculation */
+ energy -= x0 * x0;
+ energy += in * in;
+
+ /* Set the accumulator to zero */
+ acc = 0.0f;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += (*px++) * (*pb++);
+
+ acc += (*px++) * (*pb++);
+
+ acc += (*px++) * (*pb++);
+
+ acc += (*px++) * (*pb++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += (*px++) * (*pb++);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Store the result from accumulator into the destination buffer. */
+ *pOut++ = acc;
+
+ /* Compute and store error */
+ e = (float32_t) *pRef++ - acc;
+ *pErr++ = e;
+
+ /* Calculation of Weighting factor for updating filter coefficients */
+ /* epsilon value 0.000000119209289f */
+ w = (e * mu) / (energy + 0.000000119209289f);
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ *pb += w * (*px++);
+ pb++;
+
+ *pb += w * (*px++);
+ pb++;
+
+ *pb += w * (*px++);
+ pb++;
+
+ *pb += w * (*px++);
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ *pb += w * (*px++);
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ x0 = *pState;
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Save energy and x0 values for the next frame */
+ S->energy = energy;
+ S->x0 = x0;
+
+ /* 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+#endif /* #if defined(ARM_MATH_NEON) */
+/**
+ @} end of LMS_NORM group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_f32.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_f32.c new file mode 100644 index 0000000..632510f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_f32.c @@ -0,0 +1,92 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_norm_init_f32.c
+ * Description: Floating-point NLMS filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS_NORM
+ @{
+ */
+
+/**
+ @brief Initialization function for floating-point normalized LMS filter.
+ @param[in] S points to an instance of the floating-point LMS filter structure
+ @param[in] numTaps number of filter coefficients
+ @param[in] pCoeffs points to coefficient buffer
+ @param[in] pState points to state buffer
+ @param[in] mu step size that controls filter coefficient updates
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ The initial filter coefficients serve as a starting point for the adaptive filter.
+ <code>pState</code> points to an array of length <code>numTaps+blockSize-1</code> samples,
+ where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_lms_norm_f32()</code>.
+ */
+
+void arm_lms_norm_init_f32(
+ arm_lms_norm_instance_f32 * S,
+ uint16_t numTaps,
+ float32_t * pCoeffs,
+ float32_t * pState,
+ float32_t mu,
+ uint32_t blockSize)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numTaps - 1 */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(float32_t));
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Step size value */
+ S->mu = mu;
+
+ /* Initialise Energy to zero */
+ S->energy = 0.0f;
+
+ /* Initialise x0 to zero */
+ S->x0 = 0.0f;
+}
+
+/**
+ @} end of LMS_NORM group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q15.c new file mode 100644 index 0000000..11b5f39 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q15.c @@ -0,0 +1,98 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_norm_init_q15.c
+ * Description: Q15 NLMS filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+/**
+ @addtogroup LMS_NORM
+ @{
+ */
+
+/**
+ @brief Initialization function for Q15 normalized LMS filter.
+ @param[in] S points to an instance of the Q15 normalized LMS filter structure.
+ @param[in] numTaps number of filter coefficients.
+ @param[in] pCoeffs points to coefficient buffer.
+ @param[in] pState points to state buffer.
+ @param[in] mu step size that controls filter coefficient updates.
+ @param[in] blockSize number of samples to process.
+ @param[in] postShift bit shift applied to coefficients.
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ The initial filter coefficients serve as a starting point for the adaptive filter.
+ <code>pState</code> points to the array of state variables and size of array is
+ <code>numTaps+blockSize-1</code> samples, where <code>blockSize</code> is the number of input samples processed
+ by each call to <code>arm_lms_norm_q15()</code>.
+ */
+
+void arm_lms_norm_init_q15(
+ arm_lms_norm_instance_q15 * S,
+ uint16_t numTaps,
+ q15_t * pCoeffs,
+ q15_t * pState,
+ q15_t mu,
+ uint32_t blockSize,
+ uint8_t postShift)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numTaps - 1 */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q15_t));
+
+ /* Assign post Shift value applied to coefficients */
+ S->postShift = postShift;
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Step size value */
+ S->mu = mu;
+
+ /* Initialize reciprocal pointer table */
+ S->recipTable = (q15_t *) armRecipTableQ15;
+
+ /* Initialise Energy to zero */
+ S->energy = 0;
+
+ /* Initialise x0 to zero */
+ S->x0 = 0;
+}
+
+/**
+ @} end of LMS_NORM group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q31.c new file mode 100644 index 0000000..79d629f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_init_q31.c @@ -0,0 +1,97 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_norm_init_q31.c
+ * Description: Q31 NLMS filter initialization function
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+/**
+ @addtogroup LMS_NORM
+ @{
+ */
+
+/**
+ @brief Initialization function for Q31 normalized LMS filter.
+ @param[in] S points to an instance of the Q31 normalized LMS filter structure.
+ @param[in] numTaps number of filter coefficients.
+ @param[in] pCoeffs points to coefficient buffer.
+ @param[in] pState points to state buffer.
+ @param[in] mu step size that controls filter coefficient updates.
+ @param[in] blockSize number of samples to process.
+ @param[in] postShift bit shift applied to coefficients.
+ @return none
+
+ @par Details
+ <code>pCoeffs</code> points to the array of filter coefficients stored in time reversed order:
+ <pre>
+ {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
+ </pre>
+ The initial filter coefficients serve as a starting point for the adaptive filter.
+ <code>pState</code> points to an array of length <code>numTaps+blockSize-1</code> samples,
+ where <code>blockSize</code> is the number of input samples processed by each call to <code>arm_lms_norm_q31()</code>.
+ */
+
+void arm_lms_norm_init_q31(
+ arm_lms_norm_instance_q31 * S,
+ uint16_t numTaps,
+ q31_t * pCoeffs,
+ q31_t * pState,
+ q31_t mu,
+ uint32_t blockSize,
+ uint8_t postShift)
+{
+ /* Assign filter taps */
+ S->numTaps = numTaps;
+
+ /* Assign coefficient pointer */
+ S->pCoeffs = pCoeffs;
+
+ /* Clear state buffer and size is always blockSize + numTaps - 1 */
+ memset(pState, 0, (numTaps + (blockSize - 1U)) * sizeof(q31_t));
+
+ /* Assign post Shift value applied to coefficients */
+ S->postShift = postShift;
+
+ /* Assign state pointer */
+ S->pState = pState;
+
+ /* Assign Step size value */
+ S->mu = mu;
+
+ /* Initialize reciprocal pointer table */
+ S->recipTable = (q31_t *) armRecipTableQ31;
+
+ /* Initialise Energy to zero */
+ S->energy = 0;
+
+ /* Initialise x0 to zero */
+ S->x0 = 0;
+}
+
+/**
+ @} end of LMS_NORM group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q15.c new file mode 100644 index 0000000..14bbdc7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q15.c @@ -0,0 +1,297 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_norm_q15.c
+ * Description: Processing function for Q15 normalized LMS filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS_NORM
+ @{
+ */
+
+/**
+ @brief Processing function for Q15 normalized LMS filter.
+ @param[in] S points to an instance of the Q15 normalized LMS filter structure
+ @param[in] pSrc points to the block of input data
+ @param[in] pRef points to the block of reference data
+ @param[out] pOut points to the block of output data
+ @param[out] pErr points to the block of error data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using a 64-bit internal accumulator.
+ Both coefficients and state variables are represented in 1.15 format and
+ multiplications yield a 2.30 result. The 2.30 intermediate results are
+ accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full
+ precision of intermediate multiplications is preserved. After all additions
+ have been performed, the accumulator is truncated to 34.15 format by
+ discarding low 15 bits. Lastly, the accumulator is saturated to yield a
+ result in 1.15 format.
+ @par
+ In this filter, filter coefficients are updated for each sample and the
+ updation of filter cofficients are saturted.
+ */
+
+void arm_lms_norm_q15(
+ arm_lms_norm_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pRef,
+ q15_t * pOut,
+ q15_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ q15_t mu = S->mu; /* Adaptive factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ q63_t acc; /* Accumulator */
+ q31_t energy; /* Energy of the input */
+ q15_t e = 0, d = 0; /* Error, reference data sample */
+ q15_t w = 0, in; /* Weight factor and state */
+ q15_t x0; /* Temporary variable to hold input sample */
+ q15_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */
+ q15_t postShift; /* Post shift to be applied to weight after reciprocal calculation */
+ q31_t coef; /* Temporary variable for coefficient */
+ q31_t acc_l, acc_h; /* Temporary input */
+ int32_t lShift = (15 - (int32_t) S->postShift); /* Post shift */
+ int32_t uShift = (32 - lShift);
+
+ energy = S->energy;
+ x0 = S->x0;
+
+ /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Read the sample from input buffer */
+ in = *pSrc++;
+
+ /* Update the energy calculation */
+ energy -= (((q31_t) x0 * (x0)) >> 15);
+ energy += (((q31_t) in * (in)) >> 15);
+
+ /* Set the accumulator to zero */
+ acc = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* acc += b[N] * x[n-N] + b[N-1] * x[n-N-1] */
+ acc = __SMLALD(read_q15x2_ia (&px), read_q15x2_ia (&pb), acc);
+ acc = __SMLALD(read_q15x2_ia (&px), read_q15x2_ia (&pb), acc);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += (q63_t) (((q31_t) (*px++) * (*pb++)));
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Converting the result to 1.15 format and saturate the output */
+ acc = __SSAT(acc, 16U);
+
+ /* Store the result from accumulator into the destination buffer. */
+ *pOut++ = (q15_t) acc;
+
+ /* Compute and store error */
+ d = *pRef++;
+ e = d - (q15_t) acc;
+ *pErr++ = e;
+
+ /* Calculation of 1/energy */
+ postShift = arm_recip_q15((q15_t) energy + DELTA_Q15, &oneByEnergy, S->recipTable);
+
+ /* Calculation of e * mu value */
+ errorXmu = (q15_t) (((q31_t) e * mu) >> 15);
+
+ /* Calculation of (e * mu) * (1/energy) value */
+ acc = (((q31_t) errorXmu * oneByEnergy) >> (15 - postShift));
+
+ /* Weighting factor for the normalized version */
+ w = (q15_t) __SSAT((q31_t) acc, 16);
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ coef = (q31_t) *pb + (((q31_t) w * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT(coef, 16);
+
+ coef = (q31_t) *pb + (((q31_t) w * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT(coef, 16);
+
+ coef = (q31_t) *pb + (((q31_t) w * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT(coef, 16);
+
+ coef = (q31_t) *pb + (((q31_t) w * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT(coef, 16);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ coef = (q31_t) *pb + (((q31_t) w * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT(coef, 16);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ x0 = *pState;
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Save energy and x0 values for the next frame */
+ S->energy = (q15_t) energy;
+ S->x0 = x0;
+
+ /* 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ write_q15x2_ia (&pStateCurnt, read_q15x2_ia (&pState));
+ write_q15x2_ia (&pStateCurnt, read_q15x2_ia (&pState));
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of LMS_NORM group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q31.c new file mode 100644 index 0000000..34cc34a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_norm_q31.c @@ -0,0 +1,311 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_norm_q31.c
+ * Description: Processing function for the Q31 NLMS filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS_NORM
+ @{
+ */
+
+/**
+ @brief Processing function for Q31 normalized LMS filter.
+ @param[in] S points to an instance of the Q31 normalized LMS filter structure
+ @param[in] pSrc points to the block of input data
+ @param[in] pRef points to the block of reference data
+ @param[out] pOut points to the block of output data
+ @param[out] pErr points to the block of error data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate
+ multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clip.
+ In order to avoid overflows completely the input signal must be scaled down by
+ log2(numTaps) bits. The reference signal should not be scaled down.
+ After all multiply-accumulates are performed, the 2.62 accumulator is shifted
+ and saturated to 1.31 format to yield the final result.
+ The output signal and error signal are in 1.31 format.
+ @par
+ In this filter, filter coefficients are updated for each sample and the
+ updation of filter cofficients are saturted.
+ */
+
+void arm_lms_norm_q31(
+ arm_lms_norm_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pRef,
+ q31_t * pOut,
+ q31_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ q31_t mu = S->mu; /* Adaptive factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ q63_t acc; /* Accumulator */
+ q63_t energy; /* Energy of the input */
+ q31_t e = 0; /* Error data sample */
+ q31_t w = 0, in; /* Weight factor and state */
+ q31_t x0; /* Temporary variable to hold input sample */
+ q31_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */
+ q31_t postShift; /* Post shift to be applied to weight after reciprocal calculation */
+ q31_t coef; /* Temporary variable for coef */
+ q31_t acc_l, acc_h; /* Temporary input */
+ uint32_t uShift = ((uint32_t) S->postShift + 1U);
+ uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
+
+ energy = S->energy;
+ x0 = S->x0;
+
+ /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Read the sample from input buffer */
+ in = *pSrc++;
+
+ /* Update the energy calculation */
+ energy = (q31_t) ((((q63_t) energy << 32) - (((q63_t) x0 * x0) << 1)) >> 32);
+ energy = (q31_t) (((((q63_t) in * in) << 1) + (energy << 32)) >> 32);
+
+ /* Set the accumulator to zero */
+ acc = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* acc += b[N] * x[n-N] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* acc += b[N-1] * x[n-N-1] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* acc += b[N-2] * x[n-N-2] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* acc += b[N-3] * x[n-N-3] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Converting the result to 1.31 format */
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store the result from accumulator into the destination buffer. */
+ *pOut++ = (q31_t) acc;
+
+ /* Compute and store error */
+ e = *pRef++ - (q31_t) acc;
+ *pErr++ = e;
+
+ /* Calculates the reciprocal of energy */
+ postShift = arm_recip_q31(energy + DELTA_Q31, &oneByEnergy, &S->recipTable[0]);
+
+ /* Calculation of product of (e * mu) */
+ errorXmu = (q31_t) (((q63_t) e * mu) >> 31);
+
+ /* Weighting factor for the normalized version */
+ w = clip_q63_to_q31(((q63_t) errorXmu * oneByEnergy) >> (31 - postShift));
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+
+ /* coef is in 2.30 format */
+ coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
+ /* get coef in 1.31 format by left shifting */
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ /* update coefficient buffer to next coefficient */
+ pb++;
+
+ coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Read the sample from state buffer */
+ x0 = *pState;
+
+ /* Advance state pointer by 1 for the next sample */
+ pState = pState + 1;
+
+ /* Decrement loop counter */
+ blkCnt--;
+ }
+
+ /* Save energy and x0 values for the next frame */
+ S->energy = (q31_t) energy;
+ S->x0 = x0;
+
+ /* 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of LMS_NORM group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q15.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q15.c new file mode 100644 index 0000000..ce4fd31 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q15.c @@ -0,0 +1,262 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_q15.c
+ * Description: Processing function for Q15 LMS filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS
+ @{
+ */
+
+/**
+ @brief Processing function for Q15 LMS filter.
+ @param[in] S points to an instance of the Q15 LMS filter structure
+ @param[in] pSrc points to the block of input data
+ @param[in] pRef points to the block of reference data
+ @param[out] pOut points to the block of output data
+ @param[out] pErr points to the block of error data
+ @param[in] blockSize number of samples to process
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
+ The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
+ There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
+ After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
+ Lastly, the accumulator is saturated to yield a result in 1.15 format.
+ @par
+ In this filter, filter coefficients are updated for each sample and
+ the updation of filter cofficients are saturted.
+ */
+
+void arm_lms_q15(
+ const arm_lms_instance_q15 * S,
+ const q15_t * pSrc,
+ q15_t * pRef,
+ q15_t * pOut,
+ q15_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ q15_t mu = S->mu; /* Adaptive factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ q63_t acc; /* Accumulator */
+ q15_t e = 0; /* Error of data sample */
+ q15_t alpha; /* Intermediate constant for taps update */
+ q31_t coef; /* Temporary variable for coefficient */
+ q31_t acc_l, acc_h; /* Temporary input */
+ int32_t lShift = (15 - (int32_t) S->postShift); /* Post shift */
+ int32_t uShift = (32 - lShift);
+
+ /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Set the accumulator to zero */
+ acc = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* acc += b[N] * x[n-N] + b[N-1] * x[n-N-1] */
+ acc = __SMLALD(read_q15x2_ia (&px), read_q15x2_ia (&pb), acc);
+ acc = __SMLALD(read_q15x2_ia (&px), read_q15x2_ia (&pb), acc);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += (q63_t) (((q31_t) (*px++) * (*pb++)));
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ /* Apply shift for lower part of acc and upper part of acc */
+ acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Converting the result to 1.15 format and saturate the output */
+ acc = __SSAT(acc, 16U);
+
+ /* Store the result from accumulator into the destination buffer. */
+ *pOut++ = (q15_t) acc;
+
+ /* Compute and store error */
+ e = *pRef++ - (q15_t) acc;
+ *pErr++ = (q15_t) e;
+
+ /* Compute alpha i.e. intermediate constant for taps update */
+ alpha = (q15_t) (((q31_t) e * (mu)) >> 15);
+
+ /* Initialize pState pointer */
+ /* Advance state pointer by 1 for the next sample */
+ px = pState++;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT((coef), 16);
+
+ coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT((coef), 16);
+
+ coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT((coef), 16);
+
+ coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT((coef), 16);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
+ *pb++ = (q15_t) __SSAT((coef), 16);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Decrement 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ write_q15x2_ia (&pStateCurnt, read_q15x2_ia (&pState));
+ write_q15x2_ia (&pStateCurnt, read_q15x2_ia (&pState));
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of LMS group
+ */
diff --git a/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q31.c b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q31.c new file mode 100644 index 0000000..b9f7a73 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/FilteringFunctions/arm_lms_q31.c @@ -0,0 +1,283 @@ +/* ----------------------------------------------------------------------
+ * Project: CMSIS DSP Library
+ * Title: arm_lms_q31.c
+ * Description: Processing function for the Q31 LMS filter
+ *
+ * $Date: 18. March 2019
+ * $Revision: V1.6.0
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "arm_math.h"
+
+/**
+ @ingroup groupFilters
+ */
+
+/**
+ @addtogroup LMS
+ @{
+ */
+
+/**
+ @brief Processing function for Q31 LMS filter.
+ @param[in] S points to an instance of the Q31 LMS filter structure.
+ @param[in] pSrc points to the block of input data.
+ @param[in] pRef points to the block of reference data.
+ @param[out] pOut points to the block of output data.
+ @param[out] pErr points to the block of error data.
+ @param[in] blockSize number of samples to process.
+ @return none
+
+ @par Scaling and Overflow Behavior
+ The function is implemented using an internal 64-bit accumulator.
+ The accumulator has a 2.62 format and maintains full precision of the intermediate
+ multiplication results but provides only a single guard bit.
+ Thus, if the accumulator result overflows it wraps around rather than clips.
+ In order to avoid overflows completely the input signal must be scaled down by
+ log2(numTaps) bits.
+ The reference signal should not be scaled down.
+ After all multiply-accumulates are performed, the 2.62 accumulator is shifted
+ and saturated to 1.31 format to yield the final result.
+ The output signal and error signal are in 1.31 format.
+ @par
+ In this filter, filter coefficients are updated for each sample and
+ the updation of filter cofficients are saturted.
+ */
+
+void arm_lms_q31(
+ const arm_lms_instance_q31 * S,
+ const q31_t * pSrc,
+ q31_t * pRef,
+ q31_t * pOut,
+ q31_t * pErr,
+ 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 *px, *pb; /* Temporary pointers for state and coefficient buffers */
+ q31_t mu = S->mu; /* Adaptive factor */
+ uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
+ uint32_t tapCnt, blkCnt; /* Loop counters */
+ q63_t acc; /* Accumulator */
+ q31_t e = 0; /* Error of data sample */
+ q31_t alpha; /* Intermediate constant for taps update */
+ q31_t coef; /* Temporary variable for coef */
+ q31_t acc_l, acc_h; /* Temporary input */
+ uint32_t uShift = ((uint32_t) S->postShift + 1U);
+ uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
+
+ /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
+ /* pStateCurnt points to the location where the new input data should be written */
+ pStateCurnt = &(S->pState[(numTaps - 1U)]);
+
+ /* initialise loop count */
+ blkCnt = blockSize;
+
+ while (blkCnt > 0U)
+ {
+ /* Copy the new input sample into the state buffer */
+ *pStateCurnt++ = *pSrc++;
+
+ /* Initialize pState pointer */
+ px = pState;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+ /* Set the accumulator to zero */
+ acc = 0;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ /* acc += b[N] * x[n-N] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* acc += b[N-1] * x[n-N-1] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* acc += b[N-2] * x[n-N-2] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* acc += b[N-3] * x[n-N-3] */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ acc += ((q63_t) (*px++)) * (*pb++);
+
+ /* Decrement the loop counter */
+ tapCnt--;
+ }
+
+ /* Converting the result to 1.31 format */
+ /* Calc lower part of acc */
+ acc_l = acc & 0xffffffff;
+
+ /* Calc upper part of acc */
+ acc_h = (acc >> 32) & 0xffffffff;
+
+ acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
+
+ /* Store the result from accumulator into the destination buffer. */
+ *pOut++ = (q31_t) acc;
+
+ /* Compute and store error */
+ e = *pRef++ - (q31_t) acc;
+ *pErr++ = e;
+
+ /* Compute alpha i.e. intermediate constant for taps update */
+ alpha = (q31_t) (((q63_t) e * mu) >> 31);
+
+ /* Initialize pState pointer */
+ /* Advance state pointer by 1 for the next sample */
+ px = pState++;
+
+ /* Initialize coefficient pointer */
+ pb = pCoeffs;
+
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = numTaps >> 2U;
+
+ /* Update filter coefficients */
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+
+ /* coef is in 2.30 format */
+ coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
+ /* get coef in 1.31 format by left shifting */
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ /* update coefficient buffer to next coefficient */
+ pb++;
+
+ coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = numTaps % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = numTaps;
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ /* Perform the multiply-accumulate */
+ coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32));
+ *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
+ pb++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Decrement 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 pState buffer */
+ pStateCurnt = S->pState;
+
+ /* copy data */
+#if defined (ARM_MATH_LOOPUNROLL)
+
+ /* Loop unrolling: Compute 4 taps at a time. */
+ tapCnt = (numTaps - 1U) >> 2U;
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+ /* Loop unrolling: Compute remaining taps */
+ tapCnt = (numTaps - 1U) % 0x4U;
+
+#else
+
+ /* Initialize tapCnt with number of samples */
+ tapCnt = (numTaps - 1U);
+
+#endif /* #if defined (ARM_MATH_LOOPUNROLL) */
+
+ while (tapCnt > 0U)
+ {
+ *pStateCurnt++ = *pState++;
+
+ /* Decrement loop counter */
+ tapCnt--;
+ }
+
+}
+
+/**
+ @} end of LMS group
+ */
|