From f0750e92c2df85f7ab399e231497459d27a91291 Mon Sep 17 00:00:00 2001 From: Christophe Favergeon Date: Tue, 4 Aug 2020 14:28:08 +0200 Subject: [PATCH] CMSIS-DSP: Added f16 version of biquads. Corrected few doxygen issues. --- Include/dsp/fast_math_functions_f16.h | 5 +- Include/dsp/filtering_functions_f16.h | 120 ++ Source/FilteringFunctions/CMakeLists.txt | 6 + .../FilteringFunctionsF16.c | 7 + .../arm_biquad_cascade_df1_f16.c | 488 ++++++ .../arm_biquad_cascade_df1_init_f16.c | 158 ++ .../arm_biquad_cascade_df2T_f16.c | 492 ++++++ .../arm_biquad_cascade_df2T_init_f16.c | 111 ++ .../arm_biquad_cascade_stereo_df2T_f16.c | 426 +++++ .../arm_biquad_cascade_stereo_df2T_init_f16.c | 86 + Source/TransformFunctions/arm_rfft_fast_f16.c | 3 +- Testing/CMakeLists.txt | 5 +- Testing/DebugScripts/debugbiquad.py | 8 +- Testing/Include/Tests/BIQUADF16.h | 30 + Testing/PatternGeneration/BIQUAD.py | 8 +- Testing/PatternGeneration/DebugTools.py | 21 + .../BIQUAD/BIQUADF16/AllBiquadCoefs2_f16.txt | 1382 +++++++++++++++++ .../BIQUADF16/AllBiquadConfigs2_s16.txt | 38 + .../BIQUAD/BIQUADF16/AllBiquadInputs2_f16.txt | 278 ++++ .../BIQUAD/BIQUADF16/AllBiquadRefs2_f16.txt | 278 ++++ .../BIQUADF16/AllBiquadStereoInputs2_f16.txt | 554 +++++++ .../BIQUADF16/AllBiquadStereoRefs2_f16.txt | 554 +++++++ .../BIQUAD/BIQUADF16/BiquadCoefs1_f16.txt | 32 + .../BIQUAD/BIQUADF16/BiquadInput1_f16.txt | 202 +++ .../BIQUAD/BIQUADF16/BiquadOutput1_f16.txt | 202 +++ .../Filtering/BIQUAD/BIQUADF16/Coefs1_f16.txt | 42 + .../BIQUAD/BIQUADF16/Samples1_f16.txt | 1026 ++++++++++++ Testing/Source/Tests/BIQUADF16.cpp | 449 ++++++ Testing/desc_f16.txt | 33 + 29 files changed, 7034 insertions(+), 10 deletions(-) create mode 100755 Source/FilteringFunctions/arm_biquad_cascade_df1_f16.c create mode 100755 Source/FilteringFunctions/arm_biquad_cascade_df1_init_f16.c create mode 100755 Source/FilteringFunctions/arm_biquad_cascade_df2T_f16.c create mode 100755 Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f16.c create mode 100755 Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f16.c create mode 100755 Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f16.c create mode 100755 Testing/Include/Tests/BIQUADF16.h create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/AllBiquadCoefs2_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/AllBiquadConfigs2_s16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/AllBiquadInputs2_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/AllBiquadRefs2_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/AllBiquadStereoInputs2_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/AllBiquadStereoRefs2_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/BiquadCoefs1_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/BiquadInput1_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/BiquadOutput1_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/Coefs1_f16.txt create mode 100755 Testing/Patterns/DSP/Filtering/BIQUAD/BIQUADF16/Samples1_f16.txt create mode 100755 Testing/Source/Tests/BIQUADF16.cpp diff --git a/Include/dsp/fast_math_functions_f16.h b/Include/dsp/fast_math_functions_f16.h index 97e0343a..cafeac0c 100755 --- a/Include/dsp/fast_math_functions_f16.h +++ b/Include/dsp/fast_math_functions_f16.h @@ -65,7 +65,10 @@ __STATIC_FORCEINLINE arm_status arm_sqrt_f16( } - +/** + @} end of SQRT group + */ + #endif /*defined(ARM_FLOAT16_SUPPORTED)*/ #ifdef __cplusplus } diff --git a/Include/dsp/filtering_functions_f16.h b/Include/dsp/filtering_functions_f16.h index f80dbe71..9cc10cc2 100755 --- a/Include/dsp/filtering_functions_f16.h +++ b/Include/dsp/filtering_functions_f16.h @@ -79,6 +79,126 @@ extern "C" uint32_t blockSize); + /** + * @brief Instance structure for the floating-point Biquad cascade filter. + */ + typedef struct + { + uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float16_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */ + const float16_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_casd_df1_inst_f16; + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + /** + * @brief Instance structure for the modified Biquad coefs required by vectorized code. + */ + typedef struct + { + float16_t coeffs[12][8]; /**< Points to the array of modified coefficients. The array is of length 32. There is one per stage */ + } arm_biquad_mod_coef_f16; +#endif + + /** + * @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. + */ + void arm_biquad_cascade_df1_f16( + const arm_biquad_casd_df1_inst_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize); + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + void arm_biquad_cascade_df1_mve_init_f16( + arm_biquad_casd_df1_inst_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + arm_biquad_mod_coef_f16 * pCoeffsMod, + float16_t * pState); +#endif + + void arm_biquad_cascade_df1_init_f16( + arm_biquad_casd_df1_inst_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + float16_t * pState); + + /** + * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. + */ + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float16_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */ + const float16_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_cascade_df2T_instance_f16; + + /** + * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter. + */ + typedef struct + { + uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */ + float16_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */ + const float16_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */ + } arm_biquad_cascade_stereo_df2T_instance_f16; + + /** + * @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. + */ + void arm_biquad_cascade_df2T_f16( + const arm_biquad_cascade_df2T_instance_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize); + + /** + * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. 2 channels + * @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. + */ + void arm_biquad_cascade_stereo_df2T_f16( + const arm_biquad_cascade_stereo_df2T_instance_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize); + + /** + * @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. + */ + void arm_biquad_cascade_df2T_init_f16( + arm_biquad_cascade_df2T_instance_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + float16_t * pState); + + /** + * @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. + */ + void arm_biquad_cascade_stereo_df2T_init_f16( + arm_biquad_cascade_stereo_df2T_instance_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + float16_t * pState); + #endif /*defined(ARM_FLOAT16_SUPPORTED)*/ #ifdef __cplusplus } diff --git a/Source/FilteringFunctions/CMakeLists.txt b/Source/FilteringFunctions/CMakeLists.txt index 3310a866..0f239bc6 100644 --- a/Source/FilteringFunctions/CMakeLists.txt +++ b/Source/FilteringFunctions/CMakeLists.txt @@ -126,6 +126,12 @@ target_sources(CMSISDSPFiltering PRIVATE arm_lms_q31.c) if ((NOT ARMAC5) AND (NOT DISABLEFLOAT16)) target_sources(CMSISDSPFiltering PRIVATE arm_fir_f16.c) target_sources(CMSISDSPFiltering PRIVATE arm_fir_init_f16.c) +target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_f16.c) +target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df1_init_f16.c) +target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df2T_f16.c) +target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_df2T_init_f16.c) +target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_stereo_df2T_f16.c) +target_sources(CMSISDSPFiltering PRIVATE arm_biquad_cascade_stereo_df2T_init_f16.c) endif() ### Includes diff --git a/Source/FilteringFunctions/FilteringFunctionsF16.c b/Source/FilteringFunctions/FilteringFunctionsF16.c index d21389ed..514b32ba 100755 --- a/Source/FilteringFunctions/FilteringFunctionsF16.c +++ b/Source/FilteringFunctions/FilteringFunctionsF16.c @@ -26,3 +26,10 @@ #include "arm_fir_f16.c" #include "arm_fir_init_f16.c" +#include "arm_biquad_cascade_df1_f16.c" +#include "arm_biquad_cascade_df1_init_f16.c" +#include "arm_biquad_cascade_df2T_f16.c" +#include "arm_biquad_cascade_df2T_init_f16.c" +#include "arm_biquad_cascade_stereo_df2T_f16.c" +#include "arm_biquad_cascade_stereo_df2T_init_f16.c" + diff --git a/Source/FilteringFunctions/arm_biquad_cascade_df1_f16.c b/Source/FilteringFunctions/arm_biquad_cascade_df1_f16.c new file mode 100755 index 00000000..cda045ba --- /dev/null +++ b/Source/FilteringFunctions/arm_biquad_cascade_df1_f16.c @@ -0,0 +1,488 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_biquad_cascade_df1_f16.c + * Description: Processing function for the floating-point Biquad cascade DirectFormI(DF1) filter + * + * $Date: 18. March 2020 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2020 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 "dsp/filtering_functions_f16.h" + +/** + @ingroup groupFilters + */ + + +/** + @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_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_biquad_cascade_df1_f16( + const arm_biquad_casd_df1_inst_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + float16_t *pIn = (float16_t *)pSrc; /* source pointer */ + float16_t *pOut = pDst; /* destination pointer */ + float16_t *pState = S->pState; /* pState pointer */ + const float16_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ + float16_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */ + float16_t X0, X1, X2, X3; /* temporary input */ + float16_t X4, X5, X6, X7; /* temporary input */ + float16_t lastX, lastY; /* X,Y history for tail handling */ + f16x8_t coeffs; + f16x8_t accVec; /* accumultor vector */ + uint32_t sample, stage = S->numStages; /* loop counters */ + + do + { + /* + * Reading the pState values + */ + Xn1 = pState[0]; + Xn2 = pState[1]; + Yn1 = pState[2]; + Yn2 = pState[3]; + + sample = blockSize >> 3U; + + /* + * First part of the processing with loop unrolling. Compute 8 outputs at a time. + */ + while (sample > 0U) + { + X0 = *pIn++; + X1 = *pIn++; + X2 = *pIn++; + X3 = *pIn++; + X4 = *pIn++; + X5 = *pIn++; + X6 = *pIn++; + X7 = *pIn++; + + coeffs = vld1q(pCoeffs); + accVec = vmulq(coeffs, X7); + + coeffs = vld1q(&pCoeffs[8]); + accVec = vfmaq(accVec, coeffs, X6); + + coeffs = vld1q(&pCoeffs[16]); + accVec = vfmaq(accVec, coeffs, X5); + + coeffs = vld1q(&pCoeffs[24]); + accVec = vfmaq(accVec, coeffs, X4); + + coeffs = vld1q(&pCoeffs[32]); + accVec = vfmaq(accVec, coeffs, X3); + + coeffs = vld1q(&pCoeffs[40]); + accVec = vfmaq(accVec, coeffs, X2); + + coeffs = vld1q(&pCoeffs[48]); + accVec = vfmaq(accVec, coeffs, X1); + + coeffs = vld1q(&pCoeffs[56]); + accVec = vfmaq(accVec, coeffs, X0); + + coeffs = vld1q(&pCoeffs[64]); + accVec = vfmaq(accVec, coeffs, Xn1); + + coeffs = vld1q(&pCoeffs[72]); + accVec = vfmaq(accVec, coeffs, Xn2); + + coeffs = vld1q(&pCoeffs[80]); + accVec = vfmaq(accVec, coeffs, Yn1); + + coeffs = vld1q(&pCoeffs[88]); + accVec = vfmaq(accVec, coeffs, Yn2); + /* + * Store the result in the accumulator in the destination buffer. + */ + vst1q(pOut, accVec); + pOut += 8; + + /* + * update recurrence + */ + Xn1 = X7; + Xn2 = X6; + Yn1 = vgetq_lane(accVec, 7); + Yn2 = vgetq_lane(accVec, 6); + /* + * decrement the loop counter + */ + sample--; + } + + /* + * If the blockSize is not a multiple of 8, + * compute any remaining output samples here. + */ + sample = blockSize & 0x7U; + if (sample) + { + /* save previous X, Y for modulo 1 length case */ + lastX = X7; + lastY = Yn1; + + X0 = *pIn++; + X1 = *pIn++; + X2 = *pIn++; + X3 = *pIn++; + X4 = *pIn++; + X5 = *pIn++; + X6 = *pIn++; + X7 = *pIn++; + + coeffs = vld1q(pCoeffs); + accVec = vmulq(coeffs, X7); + + coeffs = vld1q(&pCoeffs[8]); + accVec = vfmaq(accVec, coeffs, X6); + + coeffs = vld1q(&pCoeffs[16]); + accVec = vfmaq(accVec, coeffs, X5); + + coeffs = vld1q(&pCoeffs[24]); + accVec = vfmaq(accVec, coeffs, X4); + + coeffs = vld1q(&pCoeffs[32]); + accVec = vfmaq(accVec, coeffs, X3); + + coeffs = vld1q(&pCoeffs[40]); + accVec = vfmaq(accVec, coeffs, X2); + + coeffs = vld1q(&pCoeffs[48]); + accVec = vfmaq(accVec, coeffs, X1); + + coeffs = vld1q(&pCoeffs[56]); + accVec = vfmaq(accVec, coeffs, X0); + + coeffs = vld1q(&pCoeffs[64]); + accVec = vfmaq(accVec, coeffs, Xn1); + + coeffs = vld1q(&pCoeffs[72]); + accVec = vfmaq(accVec, coeffs, Xn2); + + coeffs = vld1q(&pCoeffs[80]); + accVec = vfmaq(accVec, coeffs, Yn1); + + coeffs = vld1q(&pCoeffs[88]); + accVec = vfmaq(accVec, coeffs, Yn2); + + switch(sample) + { + case 1: + *pOut++ = vgetq_lane(accVec, 0); + Xn1 = X0; + Xn2 = lastX; + Yn1 = vgetq_lane(accVec, 0); + Yn2 = lastY; + break; + case 2: + *pOut++ = vgetq_lane(accVec, 0); + *pOut++ = vgetq_lane(accVec, 1); + Xn1 = X1; + Xn2 = X0; + Yn1 = vgetq_lane(accVec, 1); + Yn2 = vgetq_lane(accVec, 0); + break; + case 3: + *pOut++ = vgetq_lane(accVec, 0); + *pOut++ = vgetq_lane(accVec, 1); + *pOut++ = vgetq_lane(accVec, 2); + Xn1 = X2; + Xn2 = X1; + Yn1 = vgetq_lane(accVec, 2); + Yn2 = vgetq_lane(accVec, 1); + break; + + case 4: + *pOut++ = vgetq_lane(accVec, 0); + *pOut++ = vgetq_lane(accVec, 1); + *pOut++ = vgetq_lane(accVec, 2); + *pOut++ = vgetq_lane(accVec, 3); + Xn1 = X3; + Xn2 = X2; + Yn1 = vgetq_lane(accVec, 3); + Yn2 = vgetq_lane(accVec, 2); + break; + + case 5: + *pOut++ = vgetq_lane(accVec, 0); + *pOut++ = vgetq_lane(accVec, 1); + *pOut++ = vgetq_lane(accVec, 2); + *pOut++ = vgetq_lane(accVec, 3); + *pOut++ = vgetq_lane(accVec, 4); + Xn1 = X4; + Xn2 = X3; + Yn1 = vgetq_lane(accVec, 4); + Yn2 = vgetq_lane(accVec, 3); + break; + + case 6: + *pOut++ = vgetq_lane(accVec, 0); + *pOut++ = vgetq_lane(accVec, 1); + *pOut++ = vgetq_lane(accVec, 2); + *pOut++ = vgetq_lane(accVec, 3); + *pOut++ = vgetq_lane(accVec, 4); + *pOut++ = vgetq_lane(accVec, 5); + Xn1 = X5; + Xn2 = X4; + Yn1 = vgetq_lane(accVec, 5); + Yn2 = vgetq_lane(accVec, 4); + break; + + case 7: + *pOut++ = vgetq_lane(accVec, 0); + *pOut++ = vgetq_lane(accVec, 1); + *pOut++ = vgetq_lane(accVec, 2); + *pOut++ = vgetq_lane(accVec, 3); + *pOut++ = vgetq_lane(accVec, 4); + *pOut++ = vgetq_lane(accVec, 5); + *pOut++ = vgetq_lane(accVec, 6); + Xn1 = X6; + Xn2 = X5; + Yn1 = vgetq_lane(accVec, 6); + Yn2 = vgetq_lane(accVec, 5); + break; + } + } + /* + * Store the updated state variables back into the pState array + */ + *pState++ = Xn1; + *pState++ = Xn2; + *pState++ = Yn1; + *pState++ = Yn2; + + pCoeffs += sizeof(arm_biquad_mod_coef_f16) / sizeof(float16_t); + /* + * 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--; + } + while (stage > 0U); +} + +#else +void arm_biquad_cascade_df1_f16( + const arm_biquad_casd_df1_inst_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + const float16_t *pIn = pSrc; /* Source pointer */ + float16_t *pOut = pDst; /* Destination pointer */ + float16_t *pState = S->pState; /* pState pointer */ + const float16_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ + float16_t acc; /* Accumulator */ + float16_t b0, b1, b2, a1, a2; /* Filter coefficients */ + float16_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */ + float16_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) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* 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); + +} + +/** + @} end of BiquadCascadeDF1 group + */ +#endif /* #if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) */ \ No newline at end of file diff --git a/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f16.c b/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f16.c new file mode 100755 index 00000000..2bf15f37 --- /dev/null +++ b/Source/FilteringFunctions/arm_biquad_cascade_df1_init_f16.c @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_biquad_cascade_df1_init_f16.c + * Description: Floating-point Biquad cascade DirectFormI(DF1) filter initialization function + * + * $Date: 18. March 2020 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2020 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 "dsp/filtering_functions_f16.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 pCoeffs in the following order: + 
+      {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+
+ + @par + where b1x and a1x are the coefficients for the first stage, + b2x and a2x are the coefficients for the second stage, + and so on. The pCoeffs array contains a total of 5*numStages values. + @par + The pState is a pointer to state array. + Each Biquad stage has 4 state variables x[n-1], x[n-2], y[n-1], and y[n-2]. + The state variables are arranged in the pState array as: + 
+      {x[n-1], x[n-2], y[n-1], y[n-2]}
+
+ 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 4*numStages values. + The state variables are updated after each block of data is processed; the coefficients are untouched. + + @par For MVE code, an additional buffer of modified coefficients is required. + Its size is numStages and each element of this buffer has type arm_biquad_mod_coef_f16. + So, its total size is 96*numStages float16_t elements. + + The initialization function which must be used is arm_biquad_cascade_df1_mve_init_f16. + */ + + +void arm_biquad_cascade_df1_init_f16( + arm_biquad_casd_df1_inst_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + float16_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(float16_t)); + + /* Assign state pointer */ + S->pState = pState; +} + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +static void generateCoefsFastBiquadF16(float16_t b0, float16_t b1, float16_t b2, float16_t a1, float16_t a2, + arm_biquad_mod_coef_f16 * newCoef) +{ + float32_t coeffs[8][12] = { + {0, 0, 0, 0, 0, 0, 0, b0, b1, b2, a1, a2}, + {0, 0, 0, 0, 0, 0, b0, b1, b2, 0, a2, 0}, + {0, 0, 0, 0, 0, b0, b1, b2, 0, 0, 0, 0}, + {0, 0, 0, 0, b0, b1, b2, 0, 0, 0, 0, 0}, + {0, 0, 0, b0, b1, b2, 0, 0, 0, 0, 0, 0}, + {0, 0, b0, b1, b2, 0, 0, 0, 0, 0, 0, 0}, + {0, b0, b1, b2, 0, 0, 0, 0, 0, 0, 0, 0}, + {b0, b1, b2, 0, 0, 0, 0, 0, 0, 0, 0, 0} + }; + + for (int i = 0; i < 12; i++) + { + coeffs[1][i] += (a1 * coeffs[0][i]); + coeffs[2][i] += (a1 * coeffs[1][i]) + (a2 * coeffs[0][i]); + coeffs[3][i] += (a1 * coeffs[2][i]) + (a2 * coeffs[1][i]); + coeffs[4][i] += (a1 * coeffs[3][i]) + (a2 * coeffs[2][i]); + coeffs[5][i] += (a1 * coeffs[4][i]) + (a2 * coeffs[3][i]); + coeffs[6][i] += (a1 * coeffs[5][i]) + (a2 * coeffs[4][i]); + coeffs[7][i] += (a1 * coeffs[6][i]) + (a2 * coeffs[5][i]); + + /* + * transpose + */ + newCoef->coeffs[i][0] = (float16_t) coeffs[0][i]; + newCoef->coeffs[i][1] = (float16_t) coeffs[1][i]; + newCoef->coeffs[i][2] = (float16_t) coeffs[2][i]; + newCoef->coeffs[i][3] = (float16_t) coeffs[3][i]; + newCoef->coeffs[i][4] = (float16_t) coeffs[4][i]; + newCoef->coeffs[i][5] = (float16_t) coeffs[5][i]; + newCoef->coeffs[i][6] = (float16_t) coeffs[6][i]; + newCoef->coeffs[i][7] = (float16_t) coeffs[7][i]; + + } +} + +void arm_biquad_cascade_df1_mve_init_f16(arm_biquad_casd_df1_inst_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + arm_biquad_mod_coef_f16 * pCoeffsMod, + float16_t * pState) +{ + arm_biquad_cascade_df1_init_f16(S, numStages, (float16_t *)pCoeffsMod, pState); + + /* Generate SIMD friendly modified coefs */ + for (int i = 0; i < numStages; i++) + { + generateCoefsFastBiquadF16(pCoeffs[0], pCoeffs[1], pCoeffs[2], pCoeffs[3], pCoeffs[4], pCoeffsMod); + pCoeffs += 5; + pCoeffsMod++; + } +} + +#endif + +/** + @} end of BiquadCascadeDF1 group + */ diff --git a/Source/FilteringFunctions/arm_biquad_cascade_df2T_f16.c b/Source/FilteringFunctions/arm_biquad_cascade_df2T_f16.c new file mode 100755 index 00000000..535be236 --- /dev/null +++ b/Source/FilteringFunctions/arm_biquad_cascade_df2T_f16.c @@ -0,0 +1,492 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_biquad_cascade_df2T_f16.c + * Description: Processing function for floating-point transposed direct form II Biquad cascade filter + * + * $Date: 18. March 2020 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2020 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 "dsp/filtering_functions_f16.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_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) +void arm_biquad_cascade_df2T_f16( + const arm_biquad_cascade_df2T_instance_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + float16_t *pIn = (float16_t *)pSrc; /* source pointer */ + float16_t Xn0, Xn1; + float16_t acc0, acc1; + float16_t *pOut = pDst; /* destination pointer */ + float16_t *pState = S->pState; /* State pointer */ + uint32_t sample, stage = S->numStages; /* loop counters */ + float16_t const *pCurCoeffs = /* coefficient pointer */ + (float16_t const *) S->pCoeffs; + f16x8_t b0Coeffs, a0Coeffs; /* Coefficients vector */ + f16x8_t b1Coeffs, a1Coeffs; /* Modified coef. vector */ + f16x8_t state; /* State vector */ + + do + { + /* + * temporary carry variable for feeding the 128-bit vector shifter + */ + uint32_t tmp = 0; + /* + * Reading the coefficients + * b0Coeffs = {b0, b1, b2, x, x, x, x, x} + * a0Coeffs = { x, a1, a2, x, x, x, x, x} + */ + b0Coeffs = vld1q(pCurCoeffs); pCurCoeffs += 2; + a0Coeffs = vld1q(pCurCoeffs); pCurCoeffs += 3; + /* + * Reading the state values + * state = {d1, d2, 0, 0, x, x, x, x} + */ + state = *(f16x8_t *) pState; + state = vsetq_lane((float16_t)0.0, state, 2); + state = vsetq_lane((float16_t)0.0, state, 3); + + /* b1Coeffs = {0, b0, b1, b2, x, x, x, x} */ + /* b1Coeffs = { x, x, a1, a2, x, x, x, x} */ + b1Coeffs = (f16x8_t)vshlcq_s16((int16x8_t)b0Coeffs, &tmp, 16); + a1Coeffs = (f16x8_t)vshlcq_s16((int16x8_t)a0Coeffs, &tmp, 16); + + sample = blockSize / 2; + + /* unrolled 2 x */ + while (sample > 0U) + { + /* + * Read 2 inputs + */ + Xn0 = *pIn++; + Xn1 = *pIn++; + + /* + * 1st half: + * / acc1 \ / b0 \ / d1 \ / 0 \ + * | d1 | | b1 | | d2 | | a1 | + * | d2 | | b2 | | 0 | | a2 | + * | x | = | x | * Xn1 + | x | + | x | x acc1 + * ... ... ... ... + * \ x / \ x / \ x / \ x / + */ + + state = vfmaq(state, b0Coeffs, Xn0); + acc0 = vgetq_lane(state, 0); + state = vfmaq(state, a0Coeffs, acc0); + state = vsetq_lane((float16_t)0.0, state, 3); + + /* + * 2nd half: + * same as 1st half, but all vector elements shifted down. + * / x \ / x \ / x \ / x \ + * | acc1 | | b0 | | d1 | | 0 | + * | d1 | | b1 | | d2 | | a1 | + * | d2 | | b2 | | 0 | | a2 | + * | x | = | x | * Xn1 + | x | + | x | x acc1 + * ... ... ... ... + * \ x / \ x / \ x / \ x / + */ + + state = vfmaq(state, b1Coeffs, Xn1); + acc1 = vgetq_lane(state, 1); + state = vfmaq(state, a1Coeffs, acc1); + + /* move d1, d2 up + clearing */ + /* expect dual move or long move */ + state = vsetq_lane(vgetq_lane(state, 2), state, 0); + state = vsetq_lane(vgetq_lane(state, 3), state, 1); + state = vsetq_lane((float16_t)0.0, state, 2); + /* + * Store the results in the destination buffer. + */ + *pOut++ = acc0; + *pOut++ = acc1; + /* + * decrement the loop counter + */ + sample--; + } + + /* compiler does not come back when enabled */ + /* + * tail handling + */ + if (blockSize & 1) + { + Xn0 = *pIn++; + state = vfmaq_n_f16(state, b0Coeffs, Xn0); + acc0 = vgetq_lane(state, 0); + + state = vfmaq_n_f16(state, a0Coeffs, acc0); + *pOut++ = acc0; + *pState++ = vgetq_lane(state, 1); + *pState++ = vgetq_lane(state, 2); + } + else + { + *pState++ = vgetq_lane(state, 0); + *pState++ = vgetq_lane(state, 1); + } + /* + * The current stage input is given as the output to the next stage + */ + pIn = pDst; + /* + * Reset the output working pointer + */ + pOut = pDst; + /* + * decrement the loop counter + */ + stage--; + } + while (stage > 0U); +} +#else +LOW_OPTIMIZATION_ENTER +void arm_biquad_cascade_df2T_f16( + const arm_biquad_cascade_df2T_instance_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + const float16_t *pIn = pSrc; /* Source pointer */ + float16_t *pOut = pDst; /* Destination pointer */ + float16_t *pState = S->pState; /* State pointer */ + const float16_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ + float16_t acc1; /* Accumulator */ + float16_t b0, b1, b2, a1, a2; /* Filter coefficients */ + float16_t Xn1; /* Temporary input */ + float16_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 output is given as the input 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_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ +/** + @} end of BiquadCascadeDF2T group + */ diff --git a/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f16.c b/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f16.c new file mode 100755 index 00000000..2e70325a --- /dev/null +++ b/Source/FilteringFunctions/arm_biquad_cascade_df2T_init_f16.c @@ -0,0 +1,111 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_biquad_cascade_df2T_init_f16.c + * Description: Initialization function for floating-point transposed direct form II Biquad cascade filter + * + * $Date: 18. March 2020 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2020 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 "dsp/filtering_functions_f16.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 pCoeffs in the following order + in the not Neon version. + 
+      {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+
+ + @par + where b1x and a1x are the coefficients for the first stage, + b2x and a2x are the coefficients for the second stage, + and so on. The pCoeffs array contains a total of 5*numStages 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_f16 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: + 
+                   arm_biquad_cascade_df2T_init_f16(&SNeon, nbCascade, neonCoefs, stateNeon);
+                   arm_biquad_cascade_df2T_compute_coefs_f16(&SNeon,nbCascade,coefs);
+
+ + @par In this example, neonCoefs is a bigger array of size 8 * numStages. + coefs is the standard array: + + 
+      {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+
+ + + @par + The pState is a pointer to state array. + Each Biquad stage has 2 state variables d1, and d2. + 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 2*numStages values. + The state variables are updated after each block of data is processed; the coefficients are untouched. + */ + +void arm_biquad_cascade_df2T_init_f16( + arm_biquad_cascade_df2T_instance_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + float16_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(float16_t)); + + /* Assign state pointer */ + S->pState = pState; +} + +/** + @} end of BiquadCascadeDF2T group + */ diff --git a/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f16.c b/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f16.c new file mode 100755 index 00000000..889e1546 --- /dev/null +++ b/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_f16.c @@ -0,0 +1,426 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_biquad_cascade_stereo_df2T_f16.c + * Description: Processing function for floating-point transposed direct form II Biquad cascade filter. 2 channels + * + * $Date: 18. March 2020 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2020 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 "dsp/filtering_functions_f16.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_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) +void arm_biquad_cascade_stereo_df2T_f16( + const arm_biquad_cascade_stereo_df2T_instance_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + float16_t *pIn = (float16_t *)pSrc; /* source pointer */ + float16_t *pOut = pDst; /* destination pointer */ + float16_t *pState = S->pState; /* State pointer */ + const float16_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ + float16_t b0, b1, b2, a1, a2; /* Filter coefficients */ + uint32_t sample, stage = S->numStages; /* loop counters */ + static const uint16_t idx2[] = {2, 3, 8, 9, 2, 3, 8, 9}; + f16x8_t aCoeffs, bCoeffs; + float16_t scratch[16]; + uint16x8_t loadIdxVec; + uint16x8_t reshufledIdxVec; + uint16_t startIdx = 0; + f16x8_t stateVec0, stateVec1; + f16x8_t inVec; + + /* + * {0, 1, 0, 1, 0, 1, 0, 1} generator + */ + loadIdxVec = viwdupq_u16(startIdx, 2, 1); + reshufledIdxVec = *(uint16x8_t *)&idx2; + + /* + * scratch top clearing + * layout : [d1a d1b d2a d2b d1a d1b d2a d2b 0 0] + */ + scratch[8] = (float16_t)0.0; + scratch[9] = (float16_t)0.0; + + do + { + /* + * Reading the coefficients + */ + b0 = *pCoeffs++; + b1 = *pCoeffs++; + b2 = *pCoeffs++; + a1 = *pCoeffs++; + a2 = *pCoeffs++; + + /* aCoeffs = {a1 a1 a2 a2 a1 a1 a2 a2} */ + aCoeffs = vdupq_n_f16(a1); + aCoeffs = vsetq_lane(a2, aCoeffs, 2); + aCoeffs = vsetq_lane(a2, aCoeffs, 3); + aCoeffs = vsetq_lane(a2, aCoeffs, 6); + aCoeffs = vsetq_lane(a2, aCoeffs, 7); + + /* bCoeffs = {b1 b1 b2 b2 b1 b1 b2 b2} */ + bCoeffs = vdupq_n_f16(b1); + bCoeffs = vsetq_lane(b2, bCoeffs, 2); + bCoeffs = vsetq_lane(b2, bCoeffs, 3); + bCoeffs = vsetq_lane(b2, bCoeffs, 6); + bCoeffs = vsetq_lane(b2, bCoeffs, 7); + + /* + * Reading the state values + * Save into scratch + */ + *(f16x8_t *) scratch = *(f16x8_t *) pState; + + sample = blockSize; + + while (sample > 0U) + { + /* + * step 1 + * + * 0 | acc1a = xn1a * b0 + d1a + * 1 | acc1b = xn1b * b0 + d1b + * 2 | acc1a = xn1a * b0 + d1a + * 3 | acc1b = xn1b * b0 + d1b + * 4 | + * 5 | ... + */ + + /* + * load {d1a, d1b, d1a, d1b, d1a, d1b, d1a, d1b} + */ + stateVec0 = vldrhq_gather_shifted_offset((float16_t const *) scratch, loadIdxVec); + /* + * load {in0 in1 in0 in1 in0 in1 in0 in1} + */ + inVec = vldrhq_gather_shifted_offset_f16(pIn, loadIdxVec); + + stateVec0 = vfmaq(stateVec0, inVec, b0); + *pOut++ = vgetq_lane(stateVec0, 0); + *pOut++ = vgetq_lane(stateVec0, 1); + + /* + * step 2 + * + * 0 | d1a = b1 * xn1a + a1 * acc1a + d2a + * 1 | d1b = b1 * xn1b + a1 * acc1b + d2b + * 2 | d2a = b2 * xn1a + a2 * acc1a + 0 + * 3 | d2b = b2 * xn1b + a2 * acc1b + 0 + * 4 | + * 5 | ... + */ + + /* + * load {d2a, d2b, 0, 0, d2a, d2b, 0, 0} + */ + stateVec1 = vldrhq_gather_shifted_offset((float16_t const *) scratch, reshufledIdxVec); + stateVec1 = vfmaq(stateVec1, stateVec0, aCoeffs); + stateVec1 = vfmaq(stateVec1, inVec, bCoeffs); + *(f16x8_t *) scratch = stateVec1; + + pIn = pIn + 2; + sample--; + } + + /* + * Store the updated state variables back into the state array + */ + *pState++ = vgetq_lane(stateVec1, 0); + *pState++ = vgetq_lane(stateVec1, 1); + *pState++ = vgetq_lane(stateVec1, 2); + *pState++ = vgetq_lane(stateVec1, 3); + + /* + * The current stage input is given as the output to the next stage + */ + pIn = pDst; + /* + * Reset the output working pointer + */ + pOut = pDst; + /* + * decrement the loop counter + */ + stage--; + } + while (stage > 0U); +} +#else +LOW_OPTIMIZATION_ENTER +void arm_biquad_cascade_stereo_df2T_f16( + const arm_biquad_cascade_stereo_df2T_instance_f16 * S, + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + const float16_t *pIn = pSrc; /* Source pointer */ + float16_t *pOut = pDst; /* Destination pointer */ + float16_t *pState = S->pState; /* State pointer */ + const float16_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ + float16_t acc1a, acc1b; /* Accumulator */ + float16_t b0, b1, b2, a1, a2; /* Filter coefficients */ + float16_t Xn1a, Xn1b; /* Temporary input */ + float16_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 output is given as the input to the next stage */ + pIn = pDst; + + /* Reset the output working pointer */ + pOut = pDst; + + /* Decrement the loop counter */ + stage--; + + } while (stage > 0U); + +} +LOW_OPTIMIZATION_EXIT +#endif /* #if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) */ +/** + @} end of BiquadCascadeDF2T group + */ diff --git a/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f16.c b/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f16.c new file mode 100755 index 00000000..d04d8d08 --- /dev/null +++ b/Source/FilteringFunctions/arm_biquad_cascade_stereo_df2T_init_f16.c @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_biquad_cascade_stereo_df2T_init_f16.c + * Description: Initialization function for floating-point transposed direct form II Biquad cascade filter + * + * $Date: 18. March 2020 + * $Revision: V1.6.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2020 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 "dsp/filtering_functions_f16.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 pCoeffs in the following order: + 
+      {b10, b11, b12, a11, a12, b20, b21, b22, a21, a22, ...}
+
+ @par + where b1x and a1x are the coefficients for the first stage, + b2x and a2x are the coefficients for the second stage, + and so on. The pCoeffs array contains a total of 5*numStages values. + @par + The pState is a pointer to state array. + Each Biquad stage has 2 state variables d1, and d2 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 2*numStages values. + The state variables are updated after each block of data is processed; the coefficients are untouched. + */ + +void arm_biquad_cascade_stereo_df2T_init_f16( + arm_biquad_cascade_stereo_df2T_instance_f16 * S, + uint8_t numStages, + const float16_t * pCoeffs, + float16_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(float16_t)); + + /* Assign state pointer */ + S->pState = pState; +} + +/** + @} end of BiquadCascadeDF2T group + */ diff --git a/Source/TransformFunctions/arm_rfft_fast_f16.c b/Source/TransformFunctions/arm_rfft_fast_f16.c index b7f27a23..2a9bfe8f 100755 --- a/Source/TransformFunctions/arm_rfft_fast_f16.c +++ b/Source/TransformFunctions/arm_rfft_fast_f16.c @@ -497,8 +497,7 @@ void merge_rfft_f16( and we describe each algorithm in turn. @par Floating-point The main functions are \ref arm_rfft_fast_f16() and \ref arm_rfft_fast_init_f16(). - The older functions \ref arm_rfft_f16() and \ref arm_rfft_init_f16() have been deprecated - but are still documented. + @par The FFT of a real N-point sequence has even symmetry in the frequency domain. The second half of the data equals the conjugate of the first half flipped in frequency. diff --git a/Testing/CMakeLists.txt b/Testing/CMakeLists.txt index 4628d6d6..6f6ed4b3 100644 --- a/Testing/CMakeLists.txt +++ b/Testing/CMakeLists.txt @@ -329,6 +329,7 @@ set(TESTSRC16 Source/Tests/BasicTestsF16.cpp Source/Tests/ComplexTestsF16.cpp Source/Tests/FIRF16.cpp + Source/Tests/BIQUADF16.cpp Source/Tests/TransformCF16.cpp Source/Tests/TransformRF16.cpp ) @@ -483,7 +484,9 @@ if(DOXYGEN_FOUND) set(DOXYGEN_ENABLE_PREPROCESSING YES) set(DOXYGEN_MACRO_EXPANSION YES) set(DOXYGEN_EXPAND_ONLY_PREDEF YES) - set(DOXYGEN_PREDEFINED "ARM_FLOAT16_SUPPORTED=1 __ALIGNED(x)=") + set(DOXYGEN_QUIET YES) + set(DOXYGEN_EXCLUDE_PATTERNS "*/RTE/*") + set(DOXYGEN_PREDEFINED "ARM_FLOAT16_SUPPORTED=1;__ALIGNED(x)=") set(DOXYGEN_EXAMPLE_PATH "${ROOT}/CMSIS/DSP/Examples/ARM") set(DOXYGEN_IMAGE_PATH "${ROOT}/CMSIS/DoxyGen/DSP/src/images") set(DOXYGEN_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/docs") diff --git a/Testing/DebugScripts/debugbiquad.py b/Testing/DebugScripts/debugbiquad.py index 6166ffeb..e64ef6a4 100755 --- a/Testing/DebugScripts/debugbiquad.py +++ b/Testing/DebugScripts/debugbiquad.py @@ -15,7 +15,7 @@ sys.path.insert(0, parent_dir) import PatternGeneration.DebugTools as d -f = "f32" +f = "f16" inputPath = os.path.join(parent_dir,"Patterns","DSP","Filtering","BIQUAD","BIQUAD%s" % f.upper(),"BiquadInput1_%s.txt" % f ) refPath = os.path.join(parent_dir,"Patterns","DSP","Filtering","BIQUAD","BIQUAD%s" % f.upper(),"BiquadOutput1_%s.txt" % f) @@ -24,11 +24,11 @@ outputPath= os.path.join(parent_dir,"Output","DSP","Filtering","BIQUAD","BIQUAD% -inSig = d.readF32Pattern(inputPath) +inSig = d.readF16Pattern(inputPath) -refSig = d.readF32Pattern(refPath) +refSig = d.readF16Pattern(refPath) -sig = d.readF32Output(outputPath) +sig = d.readF16Output(outputPath) figure() diff --git a/Testing/Include/Tests/BIQUADF16.h b/Testing/Include/Tests/BIQUADF16.h new file mode 100755 index 00000000..59f46f87 --- /dev/null +++ b/Testing/Include/Tests/BIQUADF16.h @@ -0,0 +1,30 @@ +#include "Test.h" +#include "Pattern.h" + +#include "dsp/filtering_functions_f16.h" + +class BIQUADF16:public Client::Suite + { + public: + BIQUADF16(Testing::testID_t id); + virtual void setUp(Testing::testID_t,std::vector& params,Client::PatternMgr *mgr); + virtual void tearDown(Testing::testID_t,Client::PatternMgr *mgr); + private: + #include "BIQUADF16_decl.h" + + Client::Pattern coefs; + Client::Pattern inputs; + Client::Pattern configs; + Client::LocalPattern output; + Client::LocalPattern state; + Client::LocalPattern debugstate; + Client::LocalPattern vecCoefs; + // Reference patterns are not loaded when we are in dump mode + Client::RefPattern ref; + + + arm_biquad_casd_df1_inst_f16 Sdf1; + arm_biquad_cascade_df2T_instance_f16 Sdf2T; + arm_biquad_cascade_stereo_df2T_instance_f16 SStereodf2T; + + }; diff --git a/Testing/PatternGeneration/BIQUAD.py b/Testing/PatternGeneration/BIQUAD.py index 885cd5ef..9848bd59 100755 --- a/Testing/PatternGeneration/BIQUAD.py +++ b/Testing/PatternGeneration/BIQUAD.py @@ -3,7 +3,7 @@ import numpy as np import itertools import Tools from scipy import signal -from pylab import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show,semilogx, semilogy +#from pylab import figure, clf, plot, xlabel, ylabel, xlim, ylim, title, grid, axes, show,semilogx, semilogy import math # Those patterns are used for tests and benchmarks. @@ -165,7 +165,8 @@ def writeTests(config,format): config.writeInput(2,allsamples,"AllBiquadInputs") config.writeInput(2,allcoefs,"AllBiquadCoefs") config.writeReference(2,alloutputs,"AllBiquadRefs") - if format==0: + # Stereo version only for floats + if format==0 or format==16: config.writeInput(2,allStereo,"AllBiquadStereoInputs") config.writeReference(2,allStereoOutputs,"AllBiquadStereoRefs") @@ -177,6 +178,7 @@ def generatePatterns(): configf64=Tools.Config(PATTERNDIR,PARAMDIR,"f64") configf32=Tools.Config(PATTERNDIR,PARAMDIR,"f32") + configf16=Tools.Config(PATTERNDIR,PARAMDIR,"f16") configq31=Tools.Config(PATTERNDIR,PARAMDIR,"q31") configq15=Tools.Config(PATTERNDIR,PARAMDIR,"q15") #configq7=Tools.Config(PATTERNDIR,PARAMDIR,"q7") @@ -184,11 +186,13 @@ def generatePatterns(): writeBenchmarks(configf32) + writeBenchmarks(configf16) writeBenchmarks(configq31) writeBenchmarks(configq15) writeBenchmarks(configf64) writeTests(configf32,0) + writeTests(configf16,16) writeTests(configq31,31) writeTests(configq15,15) writeTests(configf64,64) diff --git a/Testing/PatternGeneration/DebugTools.py b/Testing/PatternGeneration/DebugTools.py index aaca0bae..74d57cb6 100755 --- a/Testing/PatternGeneration/DebugTools.py +++ b/Testing/PatternGeneration/DebugTools.py @@ -35,6 +35,10 @@ def readQ15Pattern(r): def hex2float(h): return(struct.unpack(' +#include "Error.h" + +#define SNR_THRESHOLD 32 + +/* + +Reference patterns are generated with +a double precision computation. + +*/ +#define REL_ERROR (3.0e-3) +#define ABS_ERROR (3.5e-2) + + void BIQUADF16::test_biquad_cascade_df1_ref() + { + + + float16_t *statep = state.ptr(); + float16_t *debugstatep = debugstate.ptr(); + + const float16_t *coefsp = coefs.ptr(); + + const float16_t *inputp = inputs.ptr(); + float16_t *outp = output.ptr(); + + #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + arm_biquad_mod_coef_f16 *coefsmodp = (arm_biquad_mod_coef_f16*)vecCoefs.ptr(); + #endif + + int blockSize; + + + + /* + + Python script is generating different tests with + different blockSize and numTaps. + + We loop on those configs. + + */ + + blockSize = inputs.nbSamples() >> 1; + + + /* + + The filter is initialized with the coefs, blockSize and numTaps. + + */ +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + arm_biquad_cascade_df1_mve_init_f16(&this->Sdf1,3,coefsp,coefsmodp,statep); +#else + arm_biquad_cascade_df1_init_f16(&this->Sdf1,3,coefsp,statep); +#endif + + /* + + Python script is filtering a 2*blockSize number of samples. + We do the same filtering in two pass to check (indirectly that + the state management of the fir is working.) + + */ + + arm_biquad_cascade_df1_f16(&this->Sdf1,inputp,outp,blockSize); + + memcpy(debugstatep,statep,3*4*sizeof(float16_t)); + debugstatep += 3*4; + + outp += blockSize; + + inputp += blockSize; + arm_biquad_cascade_df1_f16(&this->Sdf1,inputp,outp,blockSize); + outp += blockSize; + + memcpy(debugstatep,statep,3*4*sizeof(float16_t)); + debugstatep += 3*4; + + ASSERT_EMPTY_TAIL(output); + + ASSERT_SNR(output,ref,(float16_t)SNR_THRESHOLD); + + ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR); + + + } + + void BIQUADF16::test_biquad_cascade_df2T_ref() + { + + + float16_t *statep = state.ptr(); + + + float16_t *coefsp = coefs.ptr(); + + const float16_t *inputp = inputs.ptr(); + float16_t *outp = output.ptr(); + + int blockSize; + + + + /* + + Python script is generating different tests with + different blockSize and numTaps. + + We loop on those configs. + + */ + + blockSize = inputs.nbSamples() >> 1; + + /* + + The filter is initialized with the coefs, blockSize and numTaps. + + */ + + arm_biquad_cascade_df2T_init_f16(&this->Sdf2T,3,coefsp,statep); + + + + /* + + Python script is filtering a 2*blockSize number of samples. + We do the same filtering in two pass to check (indirectly that + the state management of the fir is working.) + + */ + + arm_biquad_cascade_df2T_f16(&this->Sdf2T,inputp,outp,blockSize); + outp += blockSize; + + + inputp += blockSize; + arm_biquad_cascade_df2T_f16(&this->Sdf2T,inputp,outp,blockSize); + outp += blockSize; + + + ASSERT_EMPTY_TAIL(output); + + ASSERT_SNR(output,ref,(float16_t)SNR_THRESHOLD); + + ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR); + + + } + + void BIQUADF16::test_biquad_cascade_df1_rand() + { + + + float16_t *statep = state.ptr(); + + const float16_t *coefsp = coefs.ptr(); + const int16_t *configsp = configs.ptr(); + + const float16_t *inputp = inputs.ptr(); + float16_t *outp = output.ptr(); + + #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + arm_biquad_mod_coef_f16 *coefsmodp = (arm_biquad_mod_coef_f16*)vecCoefs.ptr(); + #endif + + int blockSize; + int numStages; + unsigned long i; + + + + for(i=0;i < configs.nbSamples(); i+=2) + { + /* + + Python script is generating different tests with + different blockSize and numTaps. + + We loop on those configs. + + */ + + + numStages = configsp[0]; + blockSize = configsp[1]; + + configsp += 2; + + /* + + The filter is initialized with the coefs, blockSize and numTaps. + + */ +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + arm_biquad_cascade_df1_mve_init_f16(&this->Sdf1,numStages,coefsp,coefsmodp,statep); +#else + arm_biquad_cascade_df1_init_f16(&this->Sdf1,numStages,coefsp,statep); +#endif + + + /* + + Python script is filtering a 2*blockSize number of samples. + We do the same filtering in two pass to check (indirectly that + the state management of the fir is working.) + + */ + + arm_biquad_cascade_df1_f16(&this->Sdf1,inputp,outp,blockSize); + + inputp += blockSize; + outp += blockSize; + coefsp += numStages * 5; + + + + } + + ASSERT_EMPTY_TAIL(output); + ASSERT_SNR(output,ref,(float16_t)SNR_THRESHOLD); + ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR); + + + } + + void BIQUADF16::test_biquad_cascade_df2T_rand() + { + + + float16_t *statep = state.ptr(); + const int16_t *configsp = configs.ptr(); + + + float16_t *coefsp = coefs.ptr(); + + const float16_t *inputp = inputs.ptr(); + float16_t *outp = output.ptr(); + + int blockSize; + int numStages; + + unsigned long i; + + + + for(i=0;i < configs.nbSamples(); i+=2) + { + + /* + + Python script is generating different tests with + different blockSize and numTaps. + + We loop on those configs. + + */ + + numStages = configsp[0]; + blockSize = configsp[1]; + + configsp += 2; + + + + /* + + The filter is initialized with the coefs, blockSize and numTaps. + + */ + + arm_biquad_cascade_df2T_init_f16(&this->Sdf2T,numStages,coefsp,statep); + + coefsp += numStages * 5; + + /* + + Python script is filtering a 2*blockSize number of samples. + We do the same filtering in two pass to check (indirectly that + the state management of the fir is working.) + + */ + + arm_biquad_cascade_df2T_f16(&this->Sdf2T,inputp,outp,blockSize); + outp += blockSize; + inputp += blockSize; + + } + + ASSERT_EMPTY_TAIL(output); + + ASSERT_SNR(output,ref,(float16_t)SNR_THRESHOLD); + + ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR); + + + } + + void BIQUADF16::test_biquad_cascade_stereo_df2T_rand() + { + + + float16_t *statep = state.ptr(); + const int16_t *configsp = configs.ptr(); + + const float16_t *coefsp = coefs.ptr(); + + + const float16_t *inputp = inputs.ptr(); + float16_t *outp = output.ptr(); + + int blockSize; + int numStages; + + unsigned long i; + + + + for(i=0;i < configs.nbSamples(); i+=2) + { + + /* + + Python script is generating different tests with + different blockSize and numTaps. + + We loop on those configs. + + */ + + numStages = configsp[0]; + blockSize = configsp[1]; + + configsp += 2; + + + + /* + + The filter is initialized with the coefs, blockSize and numTaps. + + */ + arm_biquad_cascade_stereo_df2T_init_f16(&this->SStereodf2T,numStages,coefsp,statep); + + coefsp += numStages * 5; + + /* + + Python script is filtering a 2*blockSize number of samples. + We do the same filtering in two pass to check (indirectly that + the state management of the fir is working.) + + */ + + arm_biquad_cascade_stereo_df2T_f16(&this->SStereodf2T,inputp,outp,blockSize); + outp += 2*blockSize; + inputp += 2*blockSize; + + } + + ASSERT_EMPTY_TAIL(output); + + ASSERT_SNR(output,ref,(float16_t)SNR_THRESHOLD); + + ASSERT_CLOSE_ERROR(output,ref,ABS_ERROR,REL_ERROR); + + + } + + void BIQUADF16::setUp(Testing::testID_t id,std::vector& params,Client::PatternMgr *mgr) + { + + (void)params; + switch(id) + { + case BIQUADF16::TEST_BIQUAD_CASCADE_DF1_REF_1: + debugstate.create(2*64,BIQUADF16::STATE_F16_ID,mgr); + + inputs.reload(BIQUADF16::BIQUADINPUTS_F16_ID,mgr); + coefs.reload(BIQUADF16::BIQUADCOEFS_F16_ID,mgr); + ref.reload(BIQUADF16::BIQUADREFS_F16_ID,mgr); + #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + /* Max num stages is 47 in Python script */ + vecCoefs.create(96*47,BIQUADF16::OUT_F16_ID,mgr); + #endif + break; + + case BIQUADF16::TEST_BIQUAD_CASCADE_DF2T_REF_2: + vecCoefs.create(64,BIQUADF16::OUT_F16_ID,mgr); + + inputs.reload(BIQUADF16::BIQUADINPUTS_F16_ID,mgr); + coefs.reload(BIQUADF16::BIQUADCOEFS_F16_ID,mgr); + ref.reload(BIQUADF16::BIQUADREFS_F16_ID,mgr); + break; + + case BIQUADF16::TEST_BIQUAD_CASCADE_DF1_RAND_3: + + inputs.reload(BIQUADF16::ALLBIQUADINPUTS_F16_ID,mgr); + coefs.reload(BIQUADF16::ALLBIQUADCOEFS_F16_ID,mgr); + ref.reload(BIQUADF16::ALLBIQUADREFS_F16_ID,mgr); + configs.reload(BIQUADF16::ALLBIQUADCONFIGS_S16_ID,mgr); + #if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + /* Max num stages is 47 in Python script */ + vecCoefs.create(96*47,BIQUADF16::OUT_F16_ID,mgr); + #endif + break; + + case BIQUADF16::TEST_BIQUAD_CASCADE_DF2T_RAND_4: + vecCoefs.create(512,BIQUADF16::OUT_F16_ID,mgr); + + inputs.reload(BIQUADF16::ALLBIQUADINPUTS_F16_ID,mgr); + coefs.reload(BIQUADF16::ALLBIQUADCOEFS_F16_ID,mgr); + ref.reload(BIQUADF16::ALLBIQUADREFS_F16_ID,mgr); + configs.reload(BIQUADF16::ALLBIQUADCONFIGS_S16_ID,mgr); + break; + + case BIQUADF16::TEST_BIQUAD_CASCADE_STEREO_DF2T_RAND_5: + + inputs.reload(BIQUADF16::ALLBIQUADSTEREOINPUTS_F16_ID,mgr); + coefs.reload(BIQUADF16::ALLBIQUADCOEFS_F16_ID,mgr); + ref.reload(BIQUADF16::ALLBIQUADSTEREOREFS_F16_ID,mgr); + configs.reload(BIQUADF16::ALLBIQUADCONFIGS_S16_ID,mgr); + break; + + } + + + + + output.create(ref.nbSamples(),BIQUADF16::OUT_F16_ID,mgr); + + state.create(128,BIQUADF16::STATE_F16_ID,mgr); + + + } + + void BIQUADF16::tearDown(Testing::testID_t id,Client::PatternMgr *mgr) + { + (void)id; + output.dump(mgr); + switch(id) + { + case BIQUADF16::TEST_BIQUAD_CASCADE_DF1_REF_1: + debugstate.dump(mgr); + break; + } + } diff --git a/Testing/desc_f16.txt b/Testing/desc_f16.txt index 13202678..d82e2a17 100755 --- a/Testing/desc_f16.txt +++ b/Testing/desc_f16.txt @@ -166,6 +166,39 @@ group Root { } } + + group BIQUAD { + class = BIQUAD + folder = BIQUAD + + suite BIQUAD F16 { + class = BIQUADF16 + folder = BIQUADF16 + + Pattern BIQUADINPUTS_F16_ID : BiquadInput1_f16.txt + Pattern BIQUADCOEFS_F16_ID : BiquadCoefs1_f16.txt + Pattern BIQUADREFS_F16_ID : BiquadOutput1_f16.txt + + Pattern ALLBIQUADINPUTS_F16_ID : AllBiquadInputs2_f16.txt + Pattern ALLBIQUADSTEREOINPUTS_F16_ID : AllBiquadStereoInputs2_f16.txt + Pattern ALLBIQUADCOEFS_F16_ID : AllBiquadCoefs2_f16.txt + Pattern ALLBIQUADREFS_F16_ID : AllBiquadRefs2_f16.txt + Pattern ALLBIQUADSTEREOREFS_F16_ID : AllBiquadStereoRefs2_f16.txt + Pattern ALLBIQUADCONFIGS_S16_ID : AllBiquadConfigs2_s16.txt + + Output OUT_F16_ID : Output + Output STATE_F16_ID : State + + Functions { + arm_biquad_cascade_df1 ref pattern:test_biquad_cascade_df1_ref + arm_biquad_cascade_df2T ref pattern:test_biquad_cascade_df2T_ref + arm_biquad_cascade_df1 random pattern:test_biquad_cascade_df1_rand + arm_biquad_cascade_df2T random pattern:test_biquad_cascade_df2T_rand + arm_biquad_cascade_stereo_df2T random pattern:test_biquad_cascade_stereo_df2T_rand + } + + } + } } group Transform Tests {