diff options
Diffstat (limited to 'Drivers/CMSIS/DSP/Source/ComplexMathFunctions')
31 files changed, 6342 insertions, 0 deletions
diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/CMakeLists.txt b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/CMakeLists.txt new file mode 100644 index 0000000..5251dd3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required (VERSION 3.14) + +project(CMSISDSPComplexMath) + +include(configLib) +include(configDsp) + +add_library(CMSISDSPComplexMath STATIC) + +configLib(CMSISDSPComplexMath ${ROOT}) +configDsp(CMSISDSPComplexMath ${ROOT}) + + +include(interpol) +interpol(CMSISDSPComplexMath) + + +if (CONFIGTABLE AND ALLFAST) + target_compile_definitions(CMSISDSPComplexMath PUBLIC ARM_ALL_FAST_TABLES) +endif() + +# Vectorized code is defining sqrt +# so fast tables required even if Fast Math not built. +if (CONFIGTABLE AND (HELIUM OR MVEF OR MVEI)) + target_compile_definitions(CMSISDSPComplexMath PUBLIC ARM_FAST_ALLOW_TABLES) +endif() + + +# MVE code is using a table for computing the fast sqrt arm_cmplx_mag_q31 +# There is the possibility of not compiling this function and not including +# the table. +if (NOT CONFIGTABLE OR ALLFAST OR ARM_CMPLX_MAG_Q31 OR (NOT HELIUM AND NOT MVEI)) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_q31.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_CMPLX_MAG_Q15 OR (NOT HELIUM AND NOT MVEI)) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_q15.c) +endif() + +if (NOT CONFIGTABLE OR ALLFAST OR ARM_CMPLX_MAG_FAST_Q15 OR (NOT HELIUM AND NOT MVEI)) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_fast_q15.c) +endif() + +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_conj_f32.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_conj_q15.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_conj_q31.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_dot_prod_f32.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_dot_prod_q15.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_dot_prod_q31.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_f32.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_f64.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_squared_f32.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_squared_f64.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_squared_q15.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_squared_q31.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_cmplx_f32.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_cmplx_f64.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_cmplx_q15.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_cmplx_q31.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_real_f32.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_real_q15.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_real_q31.c) + +if ((NOT ARMAC5) AND (NOT DISABLEFLOAT16)) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_conj_f16.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_dot_prod_f16.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_f16.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mag_squared_f16.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_cmplx_f16.c) +target_sources(CMSISDSPComplexMath PRIVATE arm_cmplx_mult_real_f16.c) +endif() + +### Includes +target_include_directories(CMSISDSPComplexMath PUBLIC "${DSP}/Include") diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctions.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctions.c new file mode 100644 index 0000000..9b46bb3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctions.c @@ -0,0 +1,66 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: CompexMathFunctions.c + * Description: Combination of all comlex math 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_cmplx_conj_f32.c" +#include "arm_cmplx_conj_q15.c" +#include "arm_cmplx_conj_q31.c" +#include "arm_cmplx_dot_prod_f32.c" +#include "arm_cmplx_dot_prod_q15.c" +#include "arm_cmplx_dot_prod_q31.c" +#include "arm_cmplx_mag_f32.c" +#include "arm_cmplx_mag_f64.c" + +#if (defined (ARM_MATH_HELIUM) || defined(ARM_MATH_MVEI)) && !defined(ARM_MATH_AUTOVECTORIZE) + #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_FAST_SQRT_Q31_MVE) + #include "arm_cmplx_mag_q15.c" + #endif + + #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_FAST_SQRT_Q15_MVE) + #include "arm_cmplx_mag_fast_q15.c" + #endif + + #if !defined(ARM_DSP_CONFIG_TABLES) || defined(ARM_ALL_FAST_TABLES) || defined(ARM_TABLE_FAST_SQRT_Q31_MVE) + #include "arm_cmplx_mag_q31.c" + #endif +#else + #include "arm_cmplx_mag_q15.c" + #include "arm_cmplx_mag_fast_q15.c" + #include "arm_cmplx_mag_q31.c" +#endif + +#include "arm_cmplx_mag_squared_f32.c" +#include "arm_cmplx_mag_squared_f64.c" +#include "arm_cmplx_mag_squared_q15.c" +#include "arm_cmplx_mag_squared_q31.c" +#include "arm_cmplx_mult_cmplx_f32.c" +#include "arm_cmplx_mult_cmplx_f64.c" +#include "arm_cmplx_mult_cmplx_q15.c" +#include "arm_cmplx_mult_cmplx_q31.c" +#include "arm_cmplx_mult_real_f32.c" +#include "arm_cmplx_mult_real_q15.c" +#include "arm_cmplx_mult_real_q31.c" diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctionsF16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctionsF16.c new file mode 100644 index 0000000..d2c7784 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/ComplexMathFunctionsF16.c @@ -0,0 +1,32 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: CompexMathFunctionsF16.c + * Description: Combination of all complex math function f16 source files. + * + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 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 "arm_cmplx_conj_f16.c" +#include "arm_cmplx_dot_prod_f16.c" +#include "arm_cmplx_mag_f16.c" +#include "arm_cmplx_mag_squared_f16.c" +#include "arm_cmplx_mult_cmplx_f16.c" +#include "arm_cmplx_mult_real_f16.c" diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f16.c new file mode 100644 index 0000000..6cf871e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f16.c @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_conj_f16.c + * Description: Floating-point complex conjugate + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions_f16.h" + +#if defined(ARM_FLOAT16_SUPPORTED) +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_conj Complex Conjugate + + Conjugates the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the destination data where the result should be written. + <code>numSamples</code> specifies the number of complex samples + and the data in each array is stored in an interleaved fashion + (real, imag, real, imag, ...). + Each array has a total of <code>2*numSamples</code> values. + + The underlying algorithm is used: + <pre> + for (n = 0; n < numSamples; n++) { + pDst[(2*n) ] = pSrc[(2*n) ]; // real part + pDst[(2*n)+1] = -pSrc[(2*n)+1]; // imag part + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_conj + @{ + */ + +/** + @brief Floating-point complex conjugate. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_conj_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t numSamples) +{ + static const float16_t cmplx_conj_sign[8] = { 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f }; + uint32_t blockSize = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + f16x8_t vecSrc; + f16x8_t vecSign; + + /* + * load sign vector + */ + vecSign = *(f16x8_t *) cmplx_conj_sign; + + /* Compute 4 real samples at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + vecSrc = vld1q(pSrc); + vst1q(pDst,vmulq(vecSrc, vecSign)); + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + blkCnt--; + } + + /* Tail */ + blkCnt = (blockSize & 0x7) >> 1; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + *pDst++ = -(_Float16)*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#else +void arm_cmplx_conj_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + *pDst++ = -(_Float16)*pSrc++; + + *pDst++ = *pSrc++; + *pDst++ = -(_Float16)*pSrc++; + + *pDst++ = *pSrc++; + *pDst++ = -(_Float16)*pSrc++; + + *pDst++ = *pSrc++; + *pDst++ = -(_Float16)*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + *pDst++ = -(_Float16)*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_conj group + */ +#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c new file mode 100644 index 0000000..5920a77 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_f32.c @@ -0,0 +1,213 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_conj_f32.c + * Description: Floating-point complex conjugate + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_conj Complex Conjugate + + Conjugates the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the destination data where the result should be written. + <code>numSamples</code> specifies the number of complex samples + and the data in each array is stored in an interleaved fashion + (real, imag, real, imag, ...). + Each array has a total of <code>2*numSamples</code> values. + + The underlying algorithm is used: + <pre> + for (n = 0; n < numSamples; n++) { + pDst[(2*n) ] = pSrc[(2*n) ]; // real part + pDst[(2*n)+1] = -pSrc[(2*n)+1]; // imag part + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_conj + @{ + */ + +/** + @brief Floating-point complex conjugate. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_conj_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples) +{ + static const float32_t cmplx_conj_sign[4] = { 1.0f, -1.0f, 1.0f, -1.0f }; + uint32_t blockSize = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + f32x4_t vecSrc; + f32x4_t vecSign; + + /* + * load sign vector + */ + vecSign = *(f32x4_t *) cmplx_conj_sign; + + /* Compute 4 real samples at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + vecSrc = vld1q(pSrc); + vst1q(pDst,vmulq(vecSrc, vecSign)); + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrc += 4; + pDst += 4; + blkCnt--; + } + + /* Tail */ + blkCnt = (blockSize & 0x3) >> 1; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + *pDst++ = -*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#else +void arm_cmplx_conj_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + float32x4_t zero; + float32x4x2_t vec; + + zero = vdupq_n_f32(0.0f); + + /* Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0]+jC[1] = A[0]+(-1)*jA[1] */ + /* Calculate Complex Conjugate and then store the results in the destination buffer. */ + vec = vld2q_f32(pSrc); + vec.val[1] = vsubq_f32(zero,vec.val[1]); + vst2q_f32(pDst,vec); + + /* Increment pointers */ + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = numSamples & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + *pDst++ = -*pSrc++; + + *pDst++ = *pSrc++; + *pDst++ = -*pSrc++; + + *pDst++ = *pSrc++; + *pDst++ = -*pSrc++; + + *pDst++ = *pSrc++; + *pDst++ = -*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined (ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + *pDst++ = -*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_conj group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c new file mode 100644 index 0000000..fb350a0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q15.c @@ -0,0 +1,207 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_conj_q15.c + * Description: Q15 complex conjugate + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_conj + @{ + */ + +/** + @brief Q15 complex conjugate. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + The Q15 value -1 (0x8000) is saturated to the maximum allowable positive value 0x7FFF. + */ + + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) +void arm_cmplx_conj_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + uint32_t blockSize = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + q31_t in1; + + q15x8x2_t vecSrc; + q15x8_t zero; + + zero = vdupq_n_s16(0); + + /* Compute 8 real samples at a time */ + blkCnt = blockSize >> 4U; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + vecSrc.val[1] = vqsubq(zero, vecSrc.val[1]); + vst2q(pDst,vecSrc); + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrc += 16; + pDst += 16; + blkCnt --; + } + + /* Tail */ + blkCnt = (blockSize & 0xF) >> 1; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + in1 = *pSrc++; + *pDst++ = __SSAT(-in1, 16); + + /* Decrement loop counter */ + blkCnt--; + } +} +#else +void arm_cmplx_conj_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t in1; /* Temporary input variable */ + +#if defined (ARM_MATH_LOOPUNROLL) && defined (ARM_MATH_DSP) + q31_t in2, in3, in4; /* Temporary input variables */ +#endif + + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + +#if defined (ARM_MATH_DSP) + in1 = read_q15x2_ia (&pSrc); + in2 = read_q15x2_ia (&pSrc); + in3 = read_q15x2_ia (&pSrc); + in4 = read_q15x2_ia (&pSrc); + +#ifndef ARM_MATH_BIG_ENDIAN + in1 = __QASX(0, in1); + in2 = __QASX(0, in2); + in3 = __QASX(0, in3); + in4 = __QASX(0, in4); +#else + in1 = __QSAX(0, in1); + in2 = __QSAX(0, in2); + in3 = __QSAX(0, in3); + in4 = __QSAX(0, in4); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + in1 = ((uint32_t) in1 >> 16) | ((uint32_t) in1 << 16); + in2 = ((uint32_t) in2 >> 16) | ((uint32_t) in2 << 16); + in3 = ((uint32_t) in3 >> 16) | ((uint32_t) in3 << 16); + in4 = ((uint32_t) in4 >> 16) | ((uint32_t) in4 << 16); + + write_q15x2_ia (&pDst, in1); + write_q15x2_ia (&pDst, in2); + write_q15x2_ia (&pDst, in3); + write_q15x2_ia (&pDst, in4); +#else + *pDst++ = *pSrc++; + in1 = *pSrc++; + *pDst++ = (in1 == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in1; + + *pDst++ = *pSrc++; + in1 = *pSrc++; + *pDst++ = (in1 == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in1; + + *pDst++ = *pSrc++; + in1 = *pSrc++; + *pDst++ = (in1 == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in1; + + *pDst++ = *pSrc++; + in1 = *pSrc++; + *pDst++ = (in1 == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in1; + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + in1 = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __SSAT(-in1, 16); +#else + *pDst++ = (in1 == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in1; +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_conj group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c new file mode 100644 index 0000000..ec1b5b7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_conj_q31.c @@ -0,0 +1,193 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_conj_q31.c + * Description: Q31 complex conjugate + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_conj + @{ + */ + +/** + @brief Q31 complex conjugate. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + The Q31 value -1 (0x80000000) is saturated to the maximum allowable positive value 0x7FFFFFFF. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_conj_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples) +{ + + uint32_t blockSize = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + q31x4x2_t vecSrc; + q31_t in; /* Temporary input variable */ + q31x4_t zero; + + zero = vdupq_n_s32(0); + + + /* Compute 4 real samples at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + + vecSrc = vld2q(pSrc); + vecSrc.val[1] = vqsubq(zero, vecSrc.val[1]); + vst2q(pDst,vecSrc); + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + blkCnt --; + } + + /* Tail */ + blkCnt = (blockSize & 0x7) >> 1; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + in = *pSrc++; + *pDst++ = __QSUB(0, in); + + /* Decrement loop counter */ + blkCnt--; + } + + +} +#else + +void arm_cmplx_conj_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t in; /* Temporary input variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + *pDst++ = *pSrc++; + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + *pDst++ = *pSrc++; + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + *pDst++ = *pSrc++; + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] + jC[1] = A[0]+ j(-1)A[1] */ + + /* Calculate Complex Conjugate and store result in destination buffer. */ + *pDst++ = *pSrc++; + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_conj group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f16.c new file mode 100644 index 0000000..ff45a9f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f16.c @@ -0,0 +1,288 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_dot_prod_f16.c + * Description: Floating-point complex dot product + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions_f16.h" + +#if defined(ARM_FLOAT16_SUPPORTED) + + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_dot_prod Complex Dot Product + + Computes the dot product of two complex vectors. + The vectors are multiplied element-by-element and then summed. + + The <code>pSrcA</code> points to the first complex input vector and + <code>pSrcB</code> points to the second complex input vector. + <code>numSamples</code> specifies the number of complex samples + and the data in each array is stored in an interleaved fashion + (real, imag, real, imag, ...). + Each array has a total of <code>2*numSamples</code> values. + + The underlying algorithm is used: + + <pre> + realResult = 0; + imagResult = 0; + for (n = 0; n < numSamples; n++) { + realResult += pSrcA[(2*n)+0] * pSrcB[(2*n)+0] - pSrcA[(2*n)+1] * pSrcB[(2*n)+1]; + imagResult += pSrcA[(2*n)+0] * pSrcB[(2*n)+1] + pSrcA[(2*n)+1] * pSrcB[(2*n)+0]; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_dot_prod + @{ + */ + +/** + @brief Floating-point complex dot product. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[in] numSamples number of samples in each vector + @param[out] realResult real part of the result returned here + @param[out] imagResult imaginary part of the result returned here + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_cmplx_dot_prod_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + uint32_t numSamples, + float16_t * realResult, + float16_t * imagResult) +{ + int32_t blkCnt; + float16_t real_sum, imag_sum; + f16x8_t vecSrcA, vecSrcB; + f16x8_t vec_acc = vdupq_n_f16(0.0f16); + f16x8_t vecSrcC, vecSrcD; + + blkCnt = (numSamples >> 3); + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q( pSrcA); + vecSrcB = vld1q( pSrcB); + pSrcA += 8; + pSrcB += 8; + + while (blkCnt > 0) { + vec_acc = vcmlaq(vec_acc, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 8; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 8; + + vec_acc = vcmlaq(vec_acc, vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 8; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 8; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + vec_acc = vcmlaq(vec_acc, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + + vec_acc = vcmlaq(vec_acc, vecSrcC, vecSrcD); + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 7); + while (blkCnt > 0) { + mve_pred16_t p = vctp16q(blkCnt); + pSrcA += 8; + pSrcB += 8; + + vecSrcA = vldrhq_z_f16(pSrcA, p); + vecSrcB = vldrhq_z_f16(pSrcB, p); + vec_acc = vcmlaq_m(vec_acc, vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + + blkCnt -= 8; + } + } else { + /* small vector */ + blkCnt = numSamples * CMPLX_DIM; + vec_acc = vdupq_n_f16(0.0f16); + + do { + mve_pred16_t p = vctp16q(blkCnt); + + vecSrcA = vldrhq_z_f16(pSrcA, p); + vecSrcB = vldrhq_z_f16(pSrcB, p); + + vec_acc = vcmlaq_m(vec_acc, vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + blkCnt -= 8; + } + while (blkCnt > 0); + } + + /* Sum the partial parts */ + mve_cmplx_sum_intra_r_i_f16(vec_acc, real_sum, imag_sum); + + /* + * Store the real and imaginary results in the destination buffers + */ + *realResult = real_sum; + *imagResult = imag_sum; +} + +#else +void arm_cmplx_dot_prod_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + uint32_t numSamples, + float16_t * realResult, + float16_t * imagResult) +{ + uint32_t blkCnt; /* Loop counter */ + _Float16 real_sum = 0.0f, imag_sum = 0.0f; /* Temporary result variables */ + _Float16 a0,b0,c0,d0; + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store real and imaginary result in destination buffer. */ + *realResult = real_sum; + *imagResult = imag_sum; +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_dot_prod group + */ + +#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c new file mode 100644 index 0000000..af60d32 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_f32.c @@ -0,0 +1,340 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_dot_prod_f32.c + * Description: Floating-point complex dot product + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_dot_prod Complex Dot Product + + Computes the dot product of two complex vectors. + The vectors are multiplied element-by-element and then summed. + + The <code>pSrcA</code> points to the first complex input vector and + <code>pSrcB</code> points to the second complex input vector. + <code>numSamples</code> specifies the number of complex samples + and the data in each array is stored in an interleaved fashion + (real, imag, real, imag, ...). + Each array has a total of <code>2*numSamples</code> values. + + The underlying algorithm is used: + + <pre> + realResult = 0; + imagResult = 0; + for (n = 0; n < numSamples; n++) { + realResult += pSrcA[(2*n)+0] * pSrcB[(2*n)+0] - pSrcA[(2*n)+1] * pSrcB[(2*n)+1]; + imagResult += pSrcA[(2*n)+0] * pSrcB[(2*n)+1] + pSrcA[(2*n)+1] * pSrcB[(2*n)+0]; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_dot_prod + @{ + */ + +/** + @brief Floating-point complex dot product. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[in] numSamples number of samples in each vector + @param[out] realResult real part of the result returned here + @param[out] imagResult imaginary part of the result returned here + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_dot_prod_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + uint32_t numSamples, + float32_t * realResult, + float32_t * imagResult) +{ + int32_t blkCnt; + float32_t real_sum, imag_sum; + f32x4_t vecSrcA, vecSrcB; + f32x4_t vec_acc = vdupq_n_f32(0.0f); + f32x4_t vecSrcC, vecSrcD; + + blkCnt = numSamples >> 2; + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 4; + pSrcB += 4; + + while (blkCnt > 0) { + vec_acc = vcmlaq(vec_acc, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 4; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 4; + + vec_acc = vcmlaq(vec_acc, vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 4; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 4; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + vec_acc = vcmlaq(vec_acc, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + + vec_acc = vcmlaq(vec_acc, vecSrcC, vecSrcD); + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 3); + while (blkCnt > 0) { + mve_pred16_t p = vctp32q(blkCnt); + pSrcA += 4; + pSrcB += 4; + vecSrcA = vldrwq_z_f32(pSrcA, p); + vecSrcB = vldrwq_z_f32(pSrcB, p); + vec_acc = vcmlaq_m(vec_acc, vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + blkCnt -= 4; + } + } else { + /* small vector */ + blkCnt = numSamples * CMPLX_DIM; + vec_acc = vdupq_n_f32(0.0f); + + do { + mve_pred16_t p = vctp32q(blkCnt); + + vecSrcA = vldrwq_z_f32(pSrcA, p); + vecSrcB = vldrwq_z_f32(pSrcB, p); + + vec_acc = vcmlaq_m(vec_acc, vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + blkCnt -= 4; + } + while (blkCnt > 0); + } + + real_sum = vgetq_lane(vec_acc, 0) + vgetq_lane(vec_acc, 2); + imag_sum = vgetq_lane(vec_acc, 1) + vgetq_lane(vec_acc, 3); + + /* + * Store the real and imaginary results in the destination buffers + */ + *realResult = real_sum; + *imagResult = imag_sum; +} + +#else +void arm_cmplx_dot_prod_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + uint32_t numSamples, + float32_t * realResult, + float32_t * imagResult) +{ + uint32_t blkCnt; /* Loop counter */ + float32_t real_sum = 0.0f, imag_sum = 0.0f; /* Temporary result variables */ + float32_t a0,b0,c0,d0; + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + float32x4x2_t vec1,vec2,vec3,vec4; + float32x4_t accR,accI; + float32x2_t accum = vdup_n_f32(0); + + accR = vdupq_n_f32(0.0f); + accI = vdupq_n_f32(0.0f); + + /* Loop unrolling: Compute 8 outputs at a time */ + blkCnt = numSamples >> 3U; + + while (blkCnt > 0U) + { + /* C = (A[0]+jA[1])*(B[0]+jB[1]) + ... */ + /* Calculate dot product and then store the result in a temporary buffer. */ + + vec1 = vld2q_f32(pSrcA); + vec2 = vld2q_f32(pSrcB); + + /* Increment pointers */ + pSrcA += 8; + pSrcB += 8; + + /* Re{C} = Re{A}*Re{B} - Im{A}*Im{B} */ + accR = vmlaq_f32(accR,vec1.val[0],vec2.val[0]); + accR = vmlsq_f32(accR,vec1.val[1],vec2.val[1]); + + /* Im{C} = Re{A}*Im{B} + Im{A}*Re{B} */ + accI = vmlaq_f32(accI,vec1.val[1],vec2.val[0]); + accI = vmlaq_f32(accI,vec1.val[0],vec2.val[1]); + + vec3 = vld2q_f32(pSrcA); + vec4 = vld2q_f32(pSrcB); + + /* Increment pointers */ + pSrcA += 8; + pSrcB += 8; + + /* Re{C} = Re{A}*Re{B} - Im{A}*Im{B} */ + accR = vmlaq_f32(accR,vec3.val[0],vec4.val[0]); + accR = vmlsq_f32(accR,vec3.val[1],vec4.val[1]); + + /* Im{C} = Re{A}*Im{B} + Im{A}*Re{B} */ + accI = vmlaq_f32(accI,vec3.val[1],vec4.val[0]); + accI = vmlaq_f32(accI,vec3.val[0],vec4.val[1]); + + /* Decrement the loop counter */ + blkCnt--; + } + + accum = vpadd_f32(vget_low_f32(accR), vget_high_f32(accR)); + real_sum += vget_lane_f32(accum, 0) + vget_lane_f32(accum, 1); + + accum = vpadd_f32(vget_low_f32(accI), vget_high_f32(accI)); + imag_sum += vget_lane_f32(accum, 0) + vget_lane_f32(accum, 1); + + /* Tail */ + blkCnt = numSamples & 0x7; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += a0 * c0; + imag_sum += a0 * d0; + real_sum -= b0 * d0; + imag_sum += b0 * c0; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store real and imaginary result in destination buffer. */ + *realResult = real_sum; + *imagResult = imag_sum; +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_dot_prod group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c new file mode 100644 index 0000000..1910d8a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q15.c @@ -0,0 +1,256 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_dot_prod_q15.c + * Description: Processing function for the Q15 Complex Dot product + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_dot_prod + @{ + */ + +/** + @brief Q15 complex dot product. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[in] numSamples number of samples in each vector + @param[out] realResult real part of the result returned here + @param[out] imagResult imaginary part of the result returned her + @return none + + @par Scaling and Overflow Behavior + The function is implemented using an internal 64-bit accumulator. + The intermediate 1.15 by 1.15 multiplications are performed with full precision and yield a 2.30 result. + These are accumulated in a 64-bit accumulator with 34.30 precision. + As a final step, the accumulators are converted to 8.24 format. + The return results <code>realResult</code> and <code>imagResult</code> are in 8.24 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) +void arm_cmplx_dot_prod_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + uint32_t numSamples, + q31_t * realResult, + q31_t * imagResult) +{ + int32_t blkCnt; + q63_t accReal = 0LL; + q63_t accImag = 0LL; + q15x8_t vecSrcA, vecSrcB; + q15x8_t vecSrcC, vecSrcD; + + blkCnt = (numSamples >> 3); + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 8; + pSrcB += 8; + + while (blkCnt > 0) { + + accReal = vmlsldavaq(accReal, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 8; + + accImag = vmlaldavaxq(accImag, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 8; + + accReal = vmlsldavaq(accReal, vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 8; + + accImag = vmlaldavaxq(accImag, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 8; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + accReal = vmlsldavaq(accReal, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + accImag = vmlaldavaxq(accImag, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + + accReal = vmlsldavaq(accReal, vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + + accImag = vmlaldavaxq(accImag, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 7); + do { + mve_pred16_t p = vctp16q(blkCnt); + + pSrcA += 8; + pSrcB += 8; + + vecSrcA = vldrhq_z_s16(pSrcA, p); + vecSrcB = vldrhq_z_s16(pSrcB, p); + + accReal = vmlsldavaq_p(accReal, vecSrcA, vecSrcB, p); + accImag = vmlaldavaxq_p(accImag, vecSrcA, vecSrcB, p); + + blkCnt -= 8; + } + while ((int32_t) blkCnt > 0); + } else { + blkCnt = numSamples * CMPLX_DIM; + while (blkCnt > 0) { + mve_pred16_t p = vctp16q(blkCnt); + + vecSrcA = vldrhq_z_s16(pSrcA, p); + vecSrcB = vldrhq_z_s16(pSrcB, p); + + accReal = vmlsldavaq_p(accReal, vecSrcA, vecSrcB, p); + accImag = vmlaldavaxq_p(accImag, vecSrcA, vecSrcB, p); + + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + blkCnt -= 8; + } + } + *realResult = asrl(accReal, (14 - 8)); + *imagResult = asrl(accImag, (14 - 8)); +} +#else +void arm_cmplx_dot_prod_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + uint32_t numSamples, + q31_t * realResult, + q31_t * imagResult) +{ + uint32_t blkCnt; /* Loop counter */ + q63_t real_sum = 0, imag_sum = 0; /* Temporary result variables */ + q15_t a0,b0,c0,d0; + +#if defined (ARM_MATH_LOOPUNROLL) + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += (q31_t)a0 * c0; + imag_sum += (q31_t)a0 * d0; + real_sum -= (q31_t)b0 * d0; + imag_sum += (q31_t)b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += (q31_t)a0 * c0; + imag_sum += (q31_t)a0 * d0; + real_sum -= (q31_t)b0 * d0; + imag_sum += (q31_t)b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += (q31_t)a0 * c0; + imag_sum += (q31_t)a0 * d0; + real_sum -= (q31_t)b0 * d0; + imag_sum += (q31_t)b0 * c0; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += (q31_t)a0 * c0; + imag_sum += (q31_t)a0 * d0; + real_sum -= (q31_t)b0 * d0; + imag_sum += (q31_t)b0 * c0; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += (q31_t)a0 * c0; + imag_sum += (q31_t)a0 * d0; + real_sum -= (q31_t)b0 * d0; + imag_sum += (q31_t)b0 * c0; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store real and imaginary result in 8.24 format */ + /* Convert real data in 34.30 to 8.24 by 6 right shifts */ + *realResult = (q31_t) (real_sum >> 6); + /* Convert imaginary data in 34.30 to 8.24 by 6 right shifts */ + *imagResult = (q31_t) (imag_sum >> 6); +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_dot_prod group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c new file mode 100644 index 0000000..fac85bd --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_dot_prod_q31.c @@ -0,0 +1,259 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_dot_prod_q31.c + * Description: Q31 complex dot product + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_dot_prod + @{ + */ + +/** + @brief Q31 complex dot product. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[in] numSamples number of samples in each vector + @param[out] realResult real part of the result returned here + @param[out] imagResult imaginary part of the result returned here + @return none + + @par Scaling and Overflow Behavior + The function is implemented using an internal 64-bit accumulator. + The intermediate 1.31 by 1.31 multiplications are performed with 64-bit precision and then shifted to 16.48 format. + The internal real and imaginary accumulators are in 16.48 format and provide 15 guard bits. + Additions are nonsaturating and no overflow will occur as long as <code>numSamples</code> is less than 32768. + The return results <code>realResult</code> and <code>imagResult</code> are in 16.48 format. + Input down scaling is not required. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_dot_prod_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + uint32_t numSamples, + q63_t * realResult, + q63_t * imagResult) +{ + int32_t blkCnt; + q63_t accReal = 0LL; + q63_t accImag = 0LL; + q31x4_t vecSrcA, vecSrcB; + q31x4_t vecSrcC, vecSrcD; + + blkCnt = numSamples >> 2; + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 4; + pSrcB += 4; + + while (blkCnt > 0) { + + accReal = vrmlsldavhaq(accReal, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 4; + + accImag = vrmlaldavhaxq(accImag, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 4; + + accReal = vrmlsldavhaq(accReal, vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 4; + + accImag = vrmlaldavhaxq(accImag, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 4; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + accReal = vrmlsldavhaq(accReal, vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + accImag = vrmlaldavhaxq(accImag, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + + accReal = vrmlsldavhaq(accReal, vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + + accImag = vrmlaldavhaxq(accImag, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 3); + do { + mve_pred16_t p = vctp32q(blkCnt); + + pSrcA += 4; + pSrcB += 4; + + vecSrcA = vldrwq_z_s32(pSrcA, p); + vecSrcB = vldrwq_z_s32(pSrcB, p); + + accReal = vrmlsldavhaq_p(accReal, vecSrcA, vecSrcB, p); + accImag = vrmlaldavhaxq_p(accImag, vecSrcA, vecSrcB, p); + + blkCnt -= 4; + } + while ((int32_t) blkCnt > 0); + } else { + blkCnt = numSamples * CMPLX_DIM; + while (blkCnt > 0) { + mve_pred16_t p = vctp32q(blkCnt); + + vecSrcA = vldrwq_z_s32(pSrcA, p); + vecSrcB = vldrwq_z_s32(pSrcB, p); + + accReal = vrmlsldavhaq_p(accReal, vecSrcA, vecSrcB, p); + accImag = vrmlaldavhaxq_p(accImag, vecSrcA, vecSrcB, p); + + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + blkCnt -= 4; + } + } + *realResult = asrl(accReal, (14 - 8)); + *imagResult = asrl(accImag, (14 - 8)); + +} + +#else +void arm_cmplx_dot_prod_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + uint32_t numSamples, + q63_t * realResult, + q63_t * imagResult) +{ + uint32_t blkCnt; /* Loop counter */ + q63_t real_sum = 0, imag_sum = 0; /* Temporary result variables */ + q31_t a0,b0,c0,d0; + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += ((q63_t)a0 * c0) >> 14; + imag_sum += ((q63_t)a0 * d0) >> 14; + real_sum -= ((q63_t)b0 * d0) >> 14; + imag_sum += ((q63_t)b0 * c0) >> 14; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += ((q63_t)a0 * c0) >> 14; + imag_sum += ((q63_t)a0 * d0) >> 14; + real_sum -= ((q63_t)b0 * d0) >> 14; + imag_sum += ((q63_t)b0 * c0) >> 14; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += ((q63_t)a0 * c0) >> 14; + imag_sum += ((q63_t)a0 * d0) >> 14; + real_sum -= ((q63_t)b0 * d0) >> 14; + imag_sum += ((q63_t)b0 * c0) >> 14; + + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += ((q63_t)a0 * c0) >> 14; + imag_sum += ((q63_t)a0 * d0) >> 14; + real_sum -= ((q63_t)b0 * d0) >> 14; + imag_sum += ((q63_t)b0 * c0) >> 14; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + a0 = *pSrcA++; + b0 = *pSrcA++; + c0 = *pSrcB++; + d0 = *pSrcB++; + + real_sum += ((q63_t)a0 * c0) >> 14; + imag_sum += ((q63_t)a0 * d0) >> 14; + real_sum -= ((q63_t)b0 * d0) >> 14; + imag_sum += ((q63_t)b0 * c0) >> 14; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store real and imaginary result in 16.48 format */ + *realResult = real_sum; + *imagResult = imag_sum; +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_dot_prod group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f16.c new file mode 100644 index 0000000..1f93598 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f16.c @@ -0,0 +1,241 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_f16.c + * Description: Floating-point complex magnitude + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions_f16.h" + +#if defined(ARM_FLOAT16_SUPPORTED) +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_mag Complex Magnitude + + Computes the magnitude of the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the where the result should be written. + <code>numSamples</code> specifies the number of complex samples + in the input array and the data is stored in an interleaved fashion + (real, imag, real, imag, ...). + The input array has a total of <code>2*numSamples</code> values; + the output array has a total of <code>numSamples</code> values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2); + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_mag + @{ + */ + +/** + @brief Floating-point complex magnitude. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + + +void arm_cmplx_mag_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + f16x8x2_t vecSrc; + f16x8_t sum; + + /* Compute 4 complex samples at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + q15x8_t newtonStartVec; + f16x8_t sumHalf, invSqrt; + + vecSrc = vld2q(pSrc); + pSrc += 16; + sum = vmulq(vecSrc.val[0], vecSrc.val[0]); + sum = vfmaq(sum, vecSrc.val[1], vecSrc.val[1]); + + /* + * inlined Fast SQRT using inverse SQRT newton-raphson method + */ + + /* compute initial value */ + newtonStartVec = vdupq_n_s16(INVSQRT_MAGIC_F16) - vshrq((q15x8_t) sum, 1); + sumHalf = sum * 0.5f; + /* + * compute 3 x iterations + * + * The more iterations, the more accuracy. + * If you need to trade a bit of accuracy for more performance, + * you can comment out the 3rd use of the macro. + */ + INVSQRT_NEWTON_MVE_F16(invSqrt, sumHalf, (f16x8_t) newtonStartVec); + INVSQRT_NEWTON_MVE_F16(invSqrt, sumHalf, invSqrt); + INVSQRT_NEWTON_MVE_F16(invSqrt, sumHalf, invSqrt); + /* + * set negative values to 0 + */ + invSqrt = vdupq_m(invSqrt, (float16_t)0.0f, vcmpltq(invSqrt, (float16_t)0.0f)); + /* + * sqrt(x) = x * invSqrt(x) + */ + sum = vmulq(sum, invSqrt); + vstrhq_f16(pDst, sum); + pDst += 8; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + q15x8_t newtonStartVec; + f16x8_t sumHalf, invSqrt; + + vecSrc = vld2q((float16_t const *)pSrc); + sum = vmulq(vecSrc.val[0], vecSrc.val[0]); + sum = vfmaq(sum, vecSrc.val[1], vecSrc.val[1]); + + /* + * inlined Fast SQRT using inverse SQRT newton-raphson method + */ + + /* compute initial value */ + newtonStartVec = vdupq_n_s16(INVSQRT_MAGIC_F16) - vshrq((q15x8_t) sum, 1); + sumHalf = vmulq(sum, (float16_t)0.5); + /* + * compute 2 x iterations + */ + INVSQRT_NEWTON_MVE_F16(invSqrt, sumHalf, (f16x8_t) newtonStartVec); + INVSQRT_NEWTON_MVE_F16(invSqrt, sumHalf, invSqrt); + /* + * set negative values to 0 + */ + invSqrt = vdupq_m(invSqrt, (float16_t)0.0, vcmpltq(invSqrt, (float16_t)0.0)); + /* + * sqrt(x) = x * invSqrt(x) + */ + sum = vmulq(sum, invSqrt); + vstrhq_p_f16(pDst, sum, p0); + } +} + +#else +void arm_cmplx_mag_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* loop counter */ + _Float16 real, imag; /* Temporary variables to hold input values */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + arm_sqrt_f16((real * real) + (imag * imag), pDst++); + + real = *pSrc++; + imag = *pSrc++; + arm_sqrt_f16((real * real) + (imag * imag), pDst++); + + real = *pSrc++; + imag = *pSrc++; + arm_sqrt_f16((real * real) + (imag * imag), pDst++); + + real = *pSrc++; + imag = *pSrc++; + arm_sqrt_f16((real * real) + (imag * imag), pDst++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + arm_sqrt_f16((real * real) + (imag * imag), pDst++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_mag group + */ + +#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c new file mode 100644 index 0000000..aac6fba --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c @@ -0,0 +1,273 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_f32.c + * Description: Floating-point complex magnitude + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_mag Complex Magnitude + + Computes the magnitude of the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the where the result should be written. + <code>numSamples</code> specifies the number of complex samples + in the input array and the data is stored in an interleaved fashion + (real, imag, real, imag, ...). + The input array has a total of <code>2*numSamples</code> values; + the output array has a total of <code>numSamples</code> values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2); + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_mag + @{ + */ + +/** + @brief Floating-point complex magnitude. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) +#include "arm_vec_math.h" +#endif + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + + +void arm_cmplx_mag_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + f32x4x2_t vecSrc; + f32x4_t sum; + float32_t real, imag; /* Temporary variables to hold input values */ + + /* Compute 4 complex samples at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + q31x4_t newtonStartVec; + f32x4_t sumHalf, invSqrt; + + vecSrc = vld2q(pSrc); + pSrc += 8; + sum = vmulq(vecSrc.val[0], vecSrc.val[0]); + sum = vfmaq(sum, vecSrc.val[1], vecSrc.val[1]); + + /* + * inlined Fast SQRT using inverse SQRT newton-raphson method + */ + + /* compute initial value */ + newtonStartVec = vdupq_n_s32(INVSQRT_MAGIC_F32) - vshrq((q31x4_t) sum, 1); + sumHalf = sum * 0.5f; + /* + * compute 3 x iterations + * + * The more iterations, the more accuracy. + * If you need to trade a bit of accuracy for more performance, + * you can comment out the 3rd use of the macro. + */ + INVSQRT_NEWTON_MVE_F32(invSqrt, sumHalf, (f32x4_t) newtonStartVec); + INVSQRT_NEWTON_MVE_F32(invSqrt, sumHalf, invSqrt); + INVSQRT_NEWTON_MVE_F32(invSqrt, sumHalf, invSqrt); + /* + * set negative values to 0 + */ + invSqrt = vdupq_m(invSqrt, 0.0f, vcmpltq(invSqrt, 0.0f)); + /* + * sqrt(x) = x * invSqrt(x) + */ + sum = vmulq(sum, invSqrt); + vst1q(pDst, sum); + pDst += 4; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + /* + * tail + */ + blkCnt = blockSize & 3; + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + arm_sqrt_f32((real * real) + (imag * imag), pDst++); + + /* Decrement loop counter */ + blkCnt--; + } +} + +#else +void arm_cmplx_mag_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* loop counter */ + float32_t real, imag; /* Temporary variables to hold input values */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + + float32x4x2_t vecA; + float32x4_t vRealA; + float32x4_t vImagA; + float32x4_t vMagSqA; + + float32x4x2_t vecB; + float32x4_t vRealB; + float32x4_t vImagB; + float32x4_t vMagSqB; + + /* Loop unrolling: Compute 8 outputs at a time */ + blkCnt = numSamples >> 3; + + while (blkCnt > 0U) + { + /* out = sqrt((real * real) + (imag * imag)) */ + + vecA = vld2q_f32(pSrc); + pSrc += 8; + + vecB = vld2q_f32(pSrc); + pSrc += 8; + + vRealA = vmulq_f32(vecA.val[0], vecA.val[0]); + vImagA = vmulq_f32(vecA.val[1], vecA.val[1]); + vMagSqA = vaddq_f32(vRealA, vImagA); + + vRealB = vmulq_f32(vecB.val[0], vecB.val[0]); + vImagB = vmulq_f32(vecB.val[1], vecB.val[1]); + vMagSqB = vaddq_f32(vRealB, vImagB); + + /* Store the result in the destination buffer. */ + vst1q_f32(pDst, __arm_vec_sqrt_f32_neon(vMagSqA)); + pDst += 4; + + vst1q_f32(pDst, __arm_vec_sqrt_f32_neon(vMagSqB)); + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + blkCnt = numSamples & 7; + +#else + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + arm_sqrt_f32((real * real) + (imag * imag), pDst++); + + real = *pSrc++; + imag = *pSrc++; + arm_sqrt_f32((real * real) + (imag * imag), pDst++); + + real = *pSrc++; + imag = *pSrc++; + arm_sqrt_f32((real * real) + (imag * imag), pDst++); + + real = *pSrc++; + imag = *pSrc++; + arm_sqrt_f32((real * real) + (imag * imag), pDst++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + arm_sqrt_f32((real * real) + (imag * imag), pDst++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_mag group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f64.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f64.c new file mode 100644 index 0000000..c2bcd56 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_f64.c @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_f64.c + * Description: Floating-point complex magnitude + * + * $Date: 13 September 2021 + * $Revision: V1.10.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_mag Complex Magnitude + + Computes the magnitude of the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the where the result should be written. + <code>numSamples</code> specifies the number of complex samples + in the input array and the data is stored in an interleaved fashion + (real, imag, real, imag, ...). + The input array has a total of <code>2*numSamples</code> values; + the output array has a total of <code>numSamples</code> values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[n] = sqrt(pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2); + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_mag + @{ + */ + +/** + @brief Floating-point complex magnitude. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ +void arm_cmplx_mag_f64( + const float64_t * pSrc, + float64_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* loop counter */ + float64_t real, imag; /* Temporary variables to hold input values */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + *pDst++ = sqrt((real * real) + (imag * imag)); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of cmplx_mag group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_fast_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_fast_q15.c new file mode 100644 index 0000000..eac2a14 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_fast_q15.c @@ -0,0 +1,223 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_fast_q15.c + * Description: Q15 complex magnitude + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_mag + @{ + */ + +/** + @brief Q15 complex magnitude. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.15 by 1.15 multiplications and finally output is converted into 2.14 format. + Fast functions are less accurate. This function will tend to clamp to 0 + the too small values. So sqrt(x*x) = x will not always be true. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_cmplx_mag_fast_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + q15x8x2_t vecSrc; + q15x8_t sum; + q31_t in; + q31_t acc0; + + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + pSrc += 16; + sum = vqaddq(vmulhq(vecSrc.val[0], vecSrc.val[0]), + vmulhq(vecSrc.val[1], vecSrc.val[1])); + + sum = vshrq(sum, 1); + + sum = FAST_VSQRT_Q15(sum); + + vst1q(pDst, sum); + pDst += 8; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* + * tail + */ + blkCnt = blockSize & 7; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + in = read_q15x2_ia ((q15_t **) &pSrc); + acc0 = __SMUAD(in, in); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++); + + + /* Decrement loop counter */ + blkCnt--; + } +} + +#else +void arm_cmplx_mag_fast_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_DSP) + q31_t in; + q31_t acc0; /* Accumulators */ +#else + q15_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ +#endif + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + +#if defined (ARM_MATH_DSP) + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++); + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++); + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++); + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++); +#else + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++); + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++); + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++); + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++); +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + +#if defined (ARM_MATH_DSP) + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q15((q15_t) (acc0 >> 17), pDst++); +#else + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q15((q15_t) (((q63_t) acc0 + acc1) >> 17), pDst++); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_mag group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c new file mode 100644 index 0000000..10ce335 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q15.c @@ -0,0 +1,268 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_q15.c + * Description: Q15 complex magnitude + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_mag + @{ + */ + +/** + @brief Q15 complex magnitude. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.15 by 1.15 multiplications and finally output is converted into 2.14 format. + */ + +/* Sqrt q31 is used otherwise accuracy is not good enough + for small values and for some applications it is + an issue. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_cmplx_mag_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + q15x8x2_t vecSrc; + q31x4_t prod0; + q31x4_t prod1; + + q31_t in; + q31_t acc0; + q31x4_t acc0V; + q31x4_t acc1V; + + q31_t res; + q15x8_t resV; + + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + pSrc += 16; + + acc0V = vdupq_n_s32(0); + acc1V = vdupq_n_s32(0); + + prod0 = vmullbq_int_s16(vecSrc.val[0], vecSrc.val[0]); + acc0V = vqaddq_s32(acc0V,prod0); + + prod0 = vmullbq_int_s16(vecSrc.val[1], vecSrc.val[1]); + acc0V = vqaddq_s32(acc0V,prod0); + + + prod1 = vmulltq_int_s16(vecSrc.val[0], vecSrc.val[0]); + acc1V = vqaddq_s32(acc1V,prod1); + + prod1 = vmulltq_int_s16(vecSrc.val[1], vecSrc.val[1]); + acc1V = vqaddq_s32(acc1V,prod1); + + + + acc0V = vshrq(acc0V, 1); + acc1V = vshrq(acc1V, 1); + + acc0V = FAST_VSQRT_Q31(acc0V); + acc1V = FAST_VSQRT_Q31(acc1V); + + resV = vdupq_n_s16(0); + resV = vqshrnbq_n_s32(resV,acc0V,16); + resV = vqshrntq_n_s32(resV,acc1V,16); + + vst1q(pDst, resV); + pDst += 8; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* + * tail + */ + blkCnt = blockSize & 7; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + in = read_q15x2_ia ((q15_t **) &pSrc); + acc0 = __SMUAD(in, in); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q31(acc0 >> 1 , &res); + *pDst++ = res >> 16; + + + /* Decrement loop counter */ + blkCnt--; + } +} + +#else +void arm_cmplx_mag_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + q31_t res; /* temporary result */ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_DSP) + q31_t in; + q31_t acc0; /* Accumulators */ +#else + q15_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ +#endif + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + +#if defined (ARM_MATH_DSP) + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q31(acc0 >> 1 , &res); + *pDst++ = res >> 16; + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + arm_sqrt_q31(acc0 >> 1 , &res); + *pDst++ = res >> 16; + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + arm_sqrt_q31(acc0 >> 1 , &res); + *pDst++ = res >> 16; + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + arm_sqrt_q31(acc0 >> 1 , &res); + *pDst++ = res >> 16; +#else + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q31((acc0 + acc1) >> 1 , &res); + *pDst++ = res >> 16; + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + arm_sqrt_q31((acc0 + acc1) >> 1 , &res); + *pDst++ = res >> 16; + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + arm_sqrt_q31((acc0 + acc1) >> 1 , &res); + *pDst++ = res >> 16; + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + arm_sqrt_q31((acc0 + acc1) >> 1 , &res); + *pDst++ = res >> 16; +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + +#if defined (ARM_MATH_DSP) + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q31(acc0 >> 1 , &res); + *pDst++ = res >> 16; +#else + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + + /* store result in 2.14 format in destination buffer. */ + arm_sqrt_q31((acc0 + acc1) >> 1 , &res); + *pDst++ = res >> 16; + +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_mag group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c new file mode 100644 index 0000000..57a72f5 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_q31.c @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_q31.c + * Description: Q31 complex magnitude + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_mag + @{ + */ + +/** + @brief Q31 complex magnitude. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.31 by 1.31 multiplications and finally output is converted into 2.30 format. + Input down scaling is not required. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_cmplx_mag_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + + q31x4x2_t vecSrc; + q31x4_t sum; + + q31_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ + + /* Compute 4 complex samples at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + + sum = vqaddq(vmulhq(vecSrc.val[0], vecSrc.val[0]), + vmulhq(vecSrc.val[1], vecSrc.val[1])); + + sum = vshrq(sum, 1); + + /* + + This function is using a table. There are compilations flags to avoid + including this table (and in this case, arm_cmplx_maq_q31 must not + be built and linked.) + + */ + sum = FAST_VSQRT_Q31(sum); + + vst1q(pDst, sum); + + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + pSrc += 8; + pDst += 4; + } + + /* + * tail + */ + blkCnt = blockSize & 3; + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + + /* store result in 2.30 format in destination buffer. */ + arm_sqrt_q31(acc0 + acc1, pDst++); + + /* Decrement loop counter */ + blkCnt--; + } +} + +#else +void arm_cmplx_mag_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + + /* store result in 2.30 format in destination buffer. */ + arm_sqrt_q31(acc0 + acc1, pDst++); + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + arm_sqrt_q31(acc0 + acc1, pDst++); + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + arm_sqrt_q31(acc0 + acc1, pDst++); + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + arm_sqrt_q31(acc0 + acc1, pDst++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + + /* store result in 2.30 format in destination buffer. */ + arm_sqrt_q31(acc0 + acc1, pDst++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_mag group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f16.c new file mode 100644 index 0000000..df3cbec --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f16.c @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_squared_f16.c + * Description: Floating-point complex magnitude squared + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions_f16.h" + +#if defined(ARM_FLOAT16_SUPPORTED) + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_mag_squared Complex Magnitude Squared + + Computes the magnitude squared of the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the where the result should be written. + <code>numSamples</code> specifies the number of complex samples + in the input array and the data is stored in an interleaved fashion + (real, imag, real, imag, ...). + The input array has a total of <code>2*numSamples</code> values; + the output array has a total of <code>numSamples</code> values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[n] = pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_mag_squared + @{ + */ + +/** + @brief Floating-point complex magnitude squared. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mag_squared_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + f16x8x2_t vecSrc; + f16x8_t sum; + + /* Compute 4 complex samples at a time */ + while (blockSize > 0) + { + mve_pred16_t p = vctp16q(blockSize); + vecSrc = vld2q(pSrc); + sum = vmulq_m(vuninitializedq_f16(),vecSrc.val[0], vecSrc.val[0],p); + sum = vfmaq_m(sum, vecSrc.val[1], vecSrc.val[1],p); + vstrhq_p_f16(pDst, sum,p); + + pSrc += 16; + pDst += 8; + + /* + * Decrement the blockSize loop counter + */ + blockSize-= 8; + } + +} + +#else +void arm_cmplx_mag_squared_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + _Float16 real, imag; /* Temporary input variables */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + *pDst++ = (real * real) + (imag * imag); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_mag_squared group + */ + +#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c new file mode 100644 index 0000000..861585f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f32.c @@ -0,0 +1,235 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_squared_f32.c + * Description: Floating-point complex magnitude squared + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup cmplx_mag_squared Complex Magnitude Squared + + Computes the magnitude squared of the elements of a complex data vector. + + The <code>pSrc</code> points to the source data and + <code>pDst</code> points to the where the result should be written. + <code>numSamples</code> specifies the number of complex samples + in the input array and the data is stored in an interleaved fashion + (real, imag, real, imag, ...). + The input array has a total of <code>2*numSamples</code> values; + the output array has a total of <code>numSamples</code> values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[n] = pSrc[(2*n)+0]^2 + pSrc[(2*n)+1]^2; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup cmplx_mag_squared + @{ + */ + +/** + @brief Floating-point complex magnitude squared. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mag_squared_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + f32x4x2_t vecSrc; + f32x4_t sum; + float32_t real, imag; /* Temporary input variables */ + + /* Compute 4 complex samples at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + sum = vmulq(vecSrc.val[0], vecSrc.val[0]); + sum = vfmaq(sum, vecSrc.val[1], vecSrc.val[1]); + vst1q(pDst, sum); + + pSrc += 8; + pDst += 4; + + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + *pDst++ = (real * real) + (imag * imag); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#else +void arm_cmplx_mag_squared_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + float32_t real, imag; /* Temporary input variables */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + float32x4x2_t vecA; + float32x4_t vRealA; + float32x4_t vImagA; + float32x4_t vMagSqA; + + float32x4x2_t vecB; + float32x4_t vRealB; + float32x4_t vImagB; + float32x4_t vMagSqB; + + /* Loop unrolling: Compute 8 outputs at a time */ + blkCnt = numSamples >> 3; + + while (blkCnt > 0U) + { + /* out = sqrt((real * real) + (imag * imag)) */ + + vecA = vld2q_f32(pSrc); + pSrc += 8; + + vRealA = vmulq_f32(vecA.val[0], vecA.val[0]); + vImagA = vmulq_f32(vecA.val[1], vecA.val[1]); + vMagSqA = vaddq_f32(vRealA, vImagA); + + vecB = vld2q_f32(pSrc); + pSrc += 8; + + vRealB = vmulq_f32(vecB.val[0], vecB.val[0]); + vImagB = vmulq_f32(vecB.val[1], vecB.val[1]); + vMagSqB = vaddq_f32(vRealB, vImagB); + + /* Store the result in the destination buffer. */ + vst1q_f32(pDst, vMagSqA); + pDst += 4; + + vst1q_f32(pDst, vMagSqB); + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + blkCnt = numSamples & 7; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + real = *pSrc++; + imag = *pSrc++; + *pDst++ = (real * real) + (imag * imag); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + *pDst++ = (real * real) + (imag * imag); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of cmplx_mag_squared group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f64.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f64.c new file mode 100644 index 0000000..fde0321 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_f64.c @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_squared_f64.c + * Description: Floating-point complex magnitude squared + * + * $Date: 13 September 2021 + * $Revision: V1.10.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_mag_squared + @{ + */ + +/** + @brief Floating-point complex magnitude squared. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ +void arm_cmplx_mag_squared_f64( + const float64_t * pSrc, + float64_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + float64_t real, imag; /* Temporary input variables */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + + /* store result in destination buffer. */ + *pDst++ = (real * real) + (imag * imag); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of cmplx_mag_squared group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c new file mode 100644 index 0000000..727699e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q15.c @@ -0,0 +1,221 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_squared_q15.c + * Description: Q15 complex magnitude squared + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_mag_squared + @{ + */ + +/** + @brief Q15 complex magnitude squared. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.15 by 1.15 multiplications and finally output is converted into 3.13 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mag_squared_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + q31_t in; + q31_t acc0; /* Accumulators */ + q15x8x2_t vecSrc; + q15x8_t vReal, vImag; + q15x8_t vMagSq; + + + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + vReal = vmulhq(vecSrc.val[0], vecSrc.val[0]); + vImag = vmulhq(vecSrc.val[1], vecSrc.val[1]); + vMagSq = vqaddq(vReal, vImag); + vMagSq = vshrq(vMagSq, 1); + + vst1q(pDst, vMagSq); + + pSrc += 16; + pDst += 8; + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + blkCnt --; + } + + /* + * tail + */ + blkCnt = blockSize & 7; + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + in = read_q15x2_ia ((q15_t **) &pSrc); + acc0 = __SMUAD(in, in); + + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) (acc0 >> 17); + + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#else +void arm_cmplx_mag_squared_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_DSP) + q31_t in; + q31_t acc0; /* Accumulators */ +#else + q15_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ +#endif + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + +#if defined (ARM_MATH_DSP) + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) (acc0 >> 17); + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + *pDst++ = (q15_t) (acc0 >> 17); + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + *pDst++ = (q15_t) (acc0 >> 17); + + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + *pDst++ = (q15_t) (acc0 >> 17); +#else + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17); + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17); + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17); + + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17); +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + +#if defined (ARM_MATH_DSP) + in = read_q15x2_ia (&pSrc); + acc0 = __SMUAD(in, in); + + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) (acc0 >> 17); +#else + real = *pSrc++; + imag = *pSrc++; + acc0 = ((q31_t) real * real); + acc1 = ((q31_t) imag * imag); + + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) (((q63_t) acc0 + acc1) >> 17); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_mag_squared group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c new file mode 100644 index 0000000..4ccde36 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mag_squared_q31.c @@ -0,0 +1,187 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mag_squared_q31.c + * Description: Q31 complex magnitude squared + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup cmplx_mag_squared + @{ + */ + +/** + @brief Q31 complex magnitude squared. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.31 by 1.31 multiplications and finally output is converted into 3.29 format. + Input down scaling is not required. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mag_squared_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples) +{ + int32_t blockSize = numSamples; /* loop counters */ + uint32_t blkCnt; /* loop counters */ + q31x4x2_t vecSrc; + q31x4_t vReal, vImag; + q31x4_t vMagSq; + q31_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ + + /* Compute 4 complex samples at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + vecSrc = vld2q(pSrc); + vReal = vmulhq(vecSrc.val[0], vecSrc.val[0]); + vImag = vmulhq(vecSrc.val[1], vecSrc.val[1]); + vMagSq = vqaddq(vReal, vImag); + vMagSq = vshrq(vMagSq, 1); + + vst1q(pDst, vMagSq); + + pSrc += 8; + pDst += 4; + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + blkCnt --; + } + + /* Tail */ + blkCnt = blockSize & 3; + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + + /* store result in 3.29 format in destination buffer. */ + *pDst++ = acc0 + acc1; + + /* Decrement loop counter */ + blkCnt--; + } +} + +#else +void arm_cmplx_mag_squared_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t real, imag; /* Temporary input variables */ + q31_t acc0, acc1; /* Accumulators */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + /* store the result in 3.29 format in the destination buffer. */ + *pDst++ = acc0 + acc1; + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + *pDst++ = acc0 + acc1; + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + *pDst++ = acc0 + acc1; + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + *pDst++ = acc0 + acc1; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[0] = (A[0] * A[0] + A[1] * A[1]) */ + + real = *pSrc++; + imag = *pSrc++; + acc0 = (q31_t) (((q63_t) real * real) >> 33); + acc1 = (q31_t) (((q63_t) imag * imag) >> 33); + + /* store result in 3.29 format in destination buffer. */ + *pDst++ = acc0 + acc1; + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of cmplx_mag_squared group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f16.c new file mode 100644 index 0000000..68e1d1b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f16.c @@ -0,0 +1,271 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_cmplx_f16.c + * Description: Floating-point complex-by-complex multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions_f16.h" + +#if defined(ARM_FLOAT16_SUPPORTED) + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup CmplxByCmplxMult Complex-by-Complex Multiplication + + Multiplies a complex vector by another complex vector and generates a complex result. + The data in the complex arrays is stored in an interleaved fashion + (real, imag, real, imag, ...). + The parameter <code>numSamples</code> represents the number of complex + samples processed. The complex arrays have a total of <code>2*numSamples</code> + real values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[(2*n)+0] = pSrcA[(2*n)+0] * pSrcB[(2*n)+0] - pSrcA[(2*n)+1] * pSrcB[(2*n)+1]; + pDst[(2*n)+1] = pSrcA[(2*n)+0] * pSrcB[(2*n)+1] + pSrcA[(2*n)+1] * pSrcB[(2*n)+0]; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup CmplxByCmplxMult + @{ + */ + +/** + @brief Floating-point complex-by-complex multiplication. + @param[in] pSrcA points to first input vector + @param[in] pSrcB points to second input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mult_cmplx_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t numSamples) +{ + int32_t blkCnt; + f16x8_t vecSrcA, vecSrcB; + f16x8_t vecSrcC, vecSrcD; + f16x8_t vec_acc; + + blkCnt = (numSamples >> 3); + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 8; + pSrcB += 8; + + while (blkCnt > 0) { + vec_acc = vcmulq(vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 8; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 8; + vst1q(pDst, vec_acc); + pDst += 8; + + vec_acc = vcmulq(vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 8; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 8; + vst1q(pDst, vec_acc); + pDst += 8; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + vec_acc = vcmulq(vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + vst1q(pDst, vec_acc); + pDst += 8; + + vec_acc = vcmulq(vecSrcC, vecSrcD); + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + vst1q(pDst, vec_acc); + pDst += 8; + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 7); + while (blkCnt > 0) { + mve_pred16_t p = vctp16q(blkCnt); + pSrcA += 8; + pSrcB += 8; + + vecSrcA = vldrhq_z_f16(pSrcA, p); + vecSrcB = vldrhq_z_f16(pSrcB, p); + vec_acc = vcmulq_m(vuninitializedq_f16(),vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + + vstrhq_p_f16(pDst, vec_acc, p); + pDst += 8; + + blkCnt -= 8; + } + } else { + /* small vector */ + blkCnt = numSamples * CMPLX_DIM; + + do { + mve_pred16_t p = vctp16q(blkCnt); + + vecSrcA = vldrhq_z_f16(pSrcA, p); + vecSrcB = vldrhq_z_f16(pSrcB, p); + + vec_acc = vcmulq_m(vuninitializedq_f16(),vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + vstrhq_p_f16(pDst, vec_acc, p); + pDst += 8; + + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + blkCnt -= 8; + } + while (blkCnt > 0); + } + +} + + +#else +void arm_cmplx_mult_cmplx_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + _Float16 a, b, c, d; /* Temporary variables to store real and imaginary values */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + /* store result in destination buffer. */ + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + + /* store result in destination buffer. */ + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of CmplxByCmplxMult group + */ + +#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c new file mode 100644 index 0000000..5d2c66c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f32.c @@ -0,0 +1,305 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_cmplx_f32.c + * Description: Floating-point complex-by-complex multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup CmplxByCmplxMult Complex-by-Complex Multiplication + + Multiplies a complex vector by another complex vector and generates a complex result. + The data in the complex arrays is stored in an interleaved fashion + (real, imag, real, imag, ...). + The parameter <code>numSamples</code> represents the number of complex + samples processed. The complex arrays have a total of <code>2*numSamples</code> + real values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pDst[(2*n)+0] = pSrcA[(2*n)+0] * pSrcB[(2*n)+0] - pSrcA[(2*n)+1] * pSrcB[(2*n)+1]; + pDst[(2*n)+1] = pSrcA[(2*n)+0] * pSrcB[(2*n)+1] + pSrcA[(2*n)+1] * pSrcB[(2*n)+0]; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup CmplxByCmplxMult + @{ + */ + +/** + @brief Floating-point complex-by-complex multiplication. + @param[in] pSrcA points to first input vector + @param[in] pSrcB points to second input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mult_cmplx_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t numSamples) +{ + int32_t blkCnt; + f32x4_t vecSrcA, vecSrcB; + f32x4_t vecSrcC, vecSrcD; + f32x4_t vec_acc; + + blkCnt = numSamples >> 2; + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 4; + pSrcB += 4; + + while (blkCnt > 0) { + vec_acc = vcmulq(vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 4; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 4; + vst1q(pDst, vec_acc); + pDst += 4; + + vec_acc = vcmulq(vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 4; + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 4; + vst1q(pDst, vec_acc); + pDst += 4; + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + vec_acc = vcmulq(vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + vec_acc = vcmlaq_rot90(vec_acc, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + vst1q(pDst, vec_acc); + pDst += 4; + + vec_acc = vcmulq(vecSrcC, vecSrcD); + vec_acc = vcmlaq_rot90(vec_acc, vecSrcC, vecSrcD); + vst1q(pDst, vec_acc); + pDst += 4; + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 3); + while (blkCnt > 0) { + mve_pred16_t p = vctp32q(blkCnt); + pSrcA += 4; + pSrcB += 4; + + vecSrcA = vldrwq_z_f32(pSrcA, p); + vecSrcB = vldrwq_z_f32(pSrcB, p); + vec_acc = vcmulq_m(vuninitializedq_f32(),vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + + vstrwq_p_f32(pDst, vec_acc, p); + pDst += 4; + + blkCnt -= 4; + } + } else { + /* small vector */ + blkCnt = numSamples * CMPLX_DIM; + vec_acc = vdupq_n_f32(0.0f); + + do { + mve_pred16_t p = vctp32q(blkCnt); + + vecSrcA = vldrwq_z_f32(pSrcA, p); + vecSrcB = vldrwq_z_f32(pSrcB, p); + + vec_acc = vcmulq_m(vuninitializedq_f32(),vecSrcA, vecSrcB, p); + vec_acc = vcmlaq_rot90_m(vec_acc, vecSrcA, vecSrcB, p); + vstrwq_p_f32(pDst, vec_acc, p); + pDst += 4; + + /* + * Decrement the blkCnt loop counter + * Advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + blkCnt -= 4; + } + while (blkCnt > 0); + } + +} + +#else +void arm_cmplx_mult_cmplx_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + float32_t a, b, c, d; /* Temporary variables to store real and imaginary values */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + float32x4x2_t va, vb; + float32x4x2_t outCplx; + + /* Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + va = vld2q_f32(pSrcA); // load & separate real/imag pSrcA (de-interleave 2) + vb = vld2q_f32(pSrcB); // load & separate real/imag pSrcB + + /* Increment pointers */ + pSrcA += 8; + pSrcB += 8; + + /* Re{C} = Re{A}*Re{B} - Im{A}*Im{B} */ + outCplx.val[0] = vmulq_f32(va.val[0], vb.val[0]); + outCplx.val[0] = vmlsq_f32(outCplx.val[0], va.val[1], vb.val[1]); + + /* Im{C} = Re{A}*Im{B} + Im{A}*Re{B} */ + outCplx.val[1] = vmulq_f32(va.val[0], vb.val[1]); + outCplx.val[1] = vmlaq_f32(outCplx.val[1], va.val[1], vb.val[0]); + + vst2q_f32(pDst, outCplx); + + /* Increment pointer */ + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = numSamples & 3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + /* store result in destination buffer. */ + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + + /* store result in destination buffer. */ + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of CmplxByCmplxMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f64.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f64.c new file mode 100644 index 0000000..75a044c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_f64.c @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_cmplx_f64.c + * Description: Floating-point complex-by-complex multiplication + * + * $Date: 13 September 2021 + * $Revision: V1.10.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup CmplxByCmplxMult + @{ + */ + +/** + @brief Floating-point complex-by-complex multiplication. + @param[in] pSrcA points to first input vector + @param[in] pSrcB points to second input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +void arm_cmplx_mult_cmplx_f64( + const float64_t * pSrcA, + const float64_t * pSrcB, + float64_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + float64_t a, b, c, d; /* Temporary variables to store real and imaginary values */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + + /* store result in destination buffer. */ + *pDst++ = (a * c) - (b * d); + *pDst++ = (a * d) + (b * c); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of CmplxByCmplxMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c new file mode 100644 index 0000000..53fbe58 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q15.c @@ -0,0 +1,258 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_cmplx_q15.c + * Description: Q15 complex-by-complex multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup CmplxByCmplxMult + @{ + */ + +/** + @brief Q15 complex-by-complex multiplication. + @param[in] pSrcA points to first input vector + @param[in] pSrcB points to second input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.15 by 1.15 multiplications and finally output is converted into 3.13 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mult_cmplx_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t numSamples) +{ + int32_t blkCnt; + q15x8_t vecSrcA, vecSrcB; + q15x8_t vecSrcC, vecSrcD; + q15x8_t vecDst; + + blkCnt = (numSamples >> 3); + blkCnt -= 1; + if (blkCnt > 0) + { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 8; + pSrcB += 8; + + while (blkCnt > 0) + { + + /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1]. */ + vecDst = vqdmlsdhq(vuninitializedq_s16(), vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 8; + + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i]. */ + vecDst = vqdmladhxq(vecDst, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 8; + + vstrhq_s16(pDst, vshrq(vecDst, 2)); + pDst += 8; + + vecDst = vqdmlsdhq(vuninitializedq_s16(), vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 8; + + vecDst = vqdmladhxq(vecDst, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 8; + + vstrhq_s16(pDst, vshrq(vecDst, 2)); + pDst += 8; + + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + vecDst = vqdmlsdhq(vuninitializedq_s16(), vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + vecDst = vqdmladhxq(vecDst, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + + vstrhq_s16(pDst, vshrq(vecDst, 2)); + pDst += 8; + + vecDst = vqdmlsdhq(vuninitializedq_s16(), vecSrcC, vecSrcD); + vecDst = vqdmladhxq(vecDst, vecSrcC, vecSrcD); + + vstrhq_s16(pDst, vshrq(vecDst, 2)); + pDst += 8; + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 7); + do + { + mve_pred16_t p = vctp16q(blkCnt); + + pSrcA += 8; + pSrcB += 8; + + vecSrcA = vldrhq_z_s16(pSrcA, p); + vecSrcB = vldrhq_z_s16(pSrcB, p); + + vecDst = vqdmlsdhq_m(vuninitializedq_s16(), vecSrcA, vecSrcB, p); + vecDst = vqdmladhxq_m(vecDst, vecSrcA, vecSrcB, p); + + vecDst = vshrq_m(vuninitializedq_s16(), vecDst, 2, p); + vstrhq_p_s16(pDst, vecDst, p); + pDst += 8; + + blkCnt -= 8; + } + while ((int32_t) blkCnt > 0); + } + else + { + blkCnt = numSamples * CMPLX_DIM; + while (blkCnt > 0) { + mve_pred16_t p = vctp16q(blkCnt); + + vecSrcA = vldrhq_z_s16(pSrcA, p); + vecSrcB = vldrhq_z_s16(pSrcB, p); + + vecDst = vqdmlsdhq_m(vuninitializedq_s16(), vecSrcA, vecSrcB, p); + vecDst = vqdmladhxq_m(vecDst, vecSrcA, vecSrcB, p); + + vecDst = vshrq_m(vuninitializedq_s16(), vecDst, 2, p); + vstrhq_p_s16(pDst, vecDst, p); + + pDst += 8; + pSrcA += 8; + pSrcB += 8; + + blkCnt -= 8; + } + } +} +#else +void arm_cmplx_mult_cmplx_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q15_t a, b, c, d; /* Temporary variables */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) ( (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17) ); + *pDst++ = (q15_t) ( (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17) ); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (q15_t) ( (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17) ); + *pDst++ = (q15_t) ( (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17) ); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (q15_t) ( (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17) ); + *pDst++ = (q15_t) ( (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17) ); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (q15_t) ( (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17) ); + *pDst++ = (q15_t) ( (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17) ); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + + /* store result in 3.13 format in destination buffer. */ + *pDst++ = (q15_t) ( (((q31_t) a * c) >> 17) - (((q31_t) b * d) >> 17) ); + *pDst++ = (q15_t) ( (((q31_t) a * d) >> 17) + (((q31_t) b * c) >> 17) ); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of CmplxByCmplxMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c new file mode 100644 index 0000000..7f94561 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_cmplx_q31.c @@ -0,0 +1,253 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_cmplx_q31.c + * Description: Q31 complex-by-complex multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup CmplxByCmplxMult + @{ + */ + +/** + @brief Q31 complex-by-complex multiplication. + @param[in] pSrcA points to first input vector + @param[in] pSrcB points to second input vector + @param[out] pDst points to output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function implements 1.31 by 1.31 multiplications and finally output is converted into 3.29 format. + Input down scaling is not required. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) +void arm_cmplx_mult_cmplx_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t numSamples) +{ + int32_t blkCnt; + q31x4_t vecSrcA, vecSrcB; + q31x4_t vecSrcC, vecSrcD; + q31x4_t vecDst; + + blkCnt = numSamples >> 2; + blkCnt -= 1; + if (blkCnt > 0) { + /* should give more freedom to generate stall free code */ + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + pSrcA += 4; + pSrcB += 4; + + while (blkCnt > 0) { + + /* C[2 * i] = A[2 * i] * B[2 * i] - A[2 * i + 1] * B[2 * i + 1]. */ + vecDst = vqdmlsdhq(vuninitializedq_s32(), vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + pSrcA += 4; + + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i]. */ + vecDst = vqdmladhxq(vecDst, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + pSrcB += 4; + + vst1q(pDst, vshrq(vecDst, 2)); + pDst += 4; + + vecDst = vqdmlsdhq(vuninitializedq_s32(), vecSrcC, vecSrcD); + vecSrcA = vld1q(pSrcA); + pSrcA += 4; + + vecDst = vqdmladhxq(vecDst, vecSrcC, vecSrcD); + vecSrcB = vld1q(pSrcB); + pSrcB += 4; + + vst1q(pDst, vshrq(vecDst, 2)); + pDst += 4; + + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + } + + /* process last elements out of the loop avoid the armclang breaking the SW pipeline */ + vecDst = vqdmlsdhq(vuninitializedq_s32(), vecSrcA, vecSrcB); + vecSrcC = vld1q(pSrcA); + + vecDst = vqdmladhxq(vecDst, vecSrcA, vecSrcB); + vecSrcD = vld1q(pSrcB); + + vst1q(pDst, vshrq(vecDst, 2)); + pDst += 4; + + vecDst = vqdmlsdhq(vuninitializedq_s32(), vecSrcC, vecSrcD); + vecDst = vqdmladhxq(vecDst, vecSrcC, vecSrcD); + + vst1q(pDst, vshrq(vecDst, 2)); + pDst += 4; + + /* + * tail + */ + blkCnt = CMPLX_DIM * (numSamples & 3); + do { + mve_pred16_t p = vctp32q(blkCnt); + + pSrcA += 4; + pSrcB += 4; + + vecSrcA = vldrwq_z_s32(pSrcA, p); + vecSrcB = vldrwq_z_s32(pSrcB, p); + + vecDst = vqdmlsdhq_m(vuninitializedq_s32(), vecSrcA, vecSrcB, p); + vecDst = vqdmladhxq_m(vecDst, vecSrcA, vecSrcB, p); + + vecDst = vshrq_m(vuninitializedq_s32(), vecDst, 2, p); + vstrwq_p_s32(pDst, vecDst, p); + pDst += 4; + + blkCnt -= 4; + } + while ((int32_t) blkCnt > 0); + } else { + blkCnt = numSamples * CMPLX_DIM; + while (blkCnt > 0) { + mve_pred16_t p = vctp32q(blkCnt); + + vecSrcA = vldrwq_z_s32(pSrcA, p); + vecSrcB = vldrwq_z_s32(pSrcB, p); + + vecDst = vqdmlsdhq_m(vuninitializedq_s32(), vecSrcA, vecSrcB, p); + vecDst = vqdmladhxq_m(vecDst, vecSrcA, vecSrcB, p); + + vecDst = vshrq_m(vuninitializedq_s32(), vecDst, 2, p); + vstrwq_p_s32(pDst, vecDst, p); + + pDst += 4; + pSrcA += 4; + pSrcB += 4; + + blkCnt -= 4; + } + } +} +#else +void arm_cmplx_mult_cmplx_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t a, b, c, d; /* Temporary variables */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + /* store result in 3.29 format in destination buffer. */ + *pDst++ = (q31_t) ( (((q63_t) a * c) >> 33) - (((q63_t) b * d) >> 33) ); + *pDst++ = (q31_t) ( (((q63_t) a * d) >> 33) + (((q63_t) b * c) >> 33) ); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (q31_t) ( (((q63_t) a * c) >> 33) - (((q63_t) b * d) >> 33) ); + *pDst++ = (q31_t) ( (((q63_t) a * d) >> 33) + (((q63_t) b * c) >> 33) ); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (q31_t) ( (((q63_t) a * c) >> 33) - (((q63_t) b * d) >> 33) ); + *pDst++ = (q31_t) ( (((q63_t) a * d) >> 33) + (((q63_t) b * c) >> 33) ); + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + *pDst++ = (q31_t) ( (((q63_t) a * c) >> 33) - (((q63_t) b * d) >> 33) ); + *pDst++ = (q31_t) ( (((q63_t) a * d) >> 33) + (((q63_t) b * c) >> 33) ); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i] * B[2 * i ] - A[2 * i + 1] * B[2 * i + 1]. */ + /* C[2 * i + 1] = A[2 * i] * B[2 * i + 1] + A[2 * i + 1] * B[2 * i ]. */ + + a = *pSrcA++; + b = *pSrcA++; + c = *pSrcB++; + d = *pSrcB++; + + /* store result in 3.29 format in destination buffer. */ + *pDst++ = (q31_t) ( (((q63_t) a * c) >> 33) - (((q63_t) b * d) >> 33) ); + *pDst++ = (q31_t) ( (((q63_t) a * d) >> 33) + (((q63_t) b * c) >> 33) ); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of CmplxByCmplxMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f16.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f16.c new file mode 100644 index 0000000..f5909fc --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f16.c @@ -0,0 +1,194 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_real_f16.c + * Description: Floating-point complex by real multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions_f16.h" + +#if defined(ARM_FLOAT16_SUPPORTED) + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup CmplxByRealMult Complex-by-Real Multiplication + + Multiplies a complex vector by a real vector and generates a complex result. + The data in the complex arrays is stored in an interleaved fashion + (real, imag, real, imag, ...). + The parameter <code>numSamples</code> represents the number of complex + samples processed. The complex arrays have a total of <code>2*numSamples</code> + real values while the real array has a total of <code>numSamples</code> + real values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pCmplxDst[(2*n)+0] = pSrcCmplx[(2*n)+0] * pSrcReal[n]; + pCmplxDst[(2*n)+1] = pSrcCmplx[(2*n)+1] * pSrcReal[n]; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup CmplxByRealMult + @{ + */ + +/** + @brief Floating-point complex-by-real multiplication. + @param[in] pSrcCmplx points to complex input vector + @param[in] pSrcReal points to real input vector + @param[out] pCmplxDst points to complex output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mult_real_f16( + const float16_t * pSrcCmplx, + const float16_t * pSrcReal, + float16_t * pCmplxDst, + uint32_t numSamples) +{ + static const uint16_t stride_cmplx_x_real_16[8] = { + 0, 0, 1, 1, 2, 2, 3, 3 + }; + uint32_t blockSizeC = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + f16x8_t rVec; + f16x8_t cmplxVec; + f16x8_t dstVec; + uint16x8_t strideVec; + + + /* stride vector for pairs of real generation */ + strideVec = vld1q(stride_cmplx_x_real_16); + + /* Compute 4 complex outputs at a time */ + blkCnt = blockSizeC >> 3; + while (blkCnt > 0U) + { + cmplxVec = vld1q(pSrcCmplx); + rVec = vldrhq_gather_shifted_offset_f16(pSrcReal, strideVec); + dstVec = vmulq(cmplxVec, rVec); + vst1q(pCmplxDst, dstVec); + + pSrcReal += 4; + pSrcCmplx += 8; + pCmplxDst += 8; + blkCnt--; + } + + blkCnt = blockSizeC & 7; + if (blkCnt > 0U) { + mve_pred16_t p0 = vctp16q(blkCnt); + + cmplxVec = vld1q(pSrcCmplx); + rVec = vldrhq_gather_shifted_offset_f16(pSrcReal, strideVec); + dstVec = vmulq(cmplxVec, rVec); + vstrhq_p_f16(pCmplxDst, dstVec, p0); + } +} + +#else +void arm_cmplx_mult_real_f16( + const float16_t * pSrcCmplx, + const float16_t * pSrcReal, + float16_t * pCmplxDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + float16_t in; /* Temporary variable */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store result in destination buffer. */ + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + + in = *pSrcReal++; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + + in = *pSrcReal++; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + + in = *pSrcReal++; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store result in destination buffer. */ + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + *pCmplxDst++ = (_Float16)*pSrcCmplx++ * (_Float16)in; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of CmplxByRealMult group + */ + +#endif /* #if defined(ARM_FLOAT16_SUPPORTED) */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c new file mode 100644 index 0000000..bc0f59b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_f32.c @@ -0,0 +1,224 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_real_f32.c + * Description: Floating-point complex by real multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @defgroup CmplxByRealMult Complex-by-Real Multiplication + + Multiplies a complex vector by a real vector and generates a complex result. + The data in the complex arrays is stored in an interleaved fashion + (real, imag, real, imag, ...). + The parameter <code>numSamples</code> represents the number of complex + samples processed. The complex arrays have a total of <code>2*numSamples</code> + real values while the real array has a total of <code>numSamples</code> + real values. + + The underlying algorithm is used: + + <pre> + for (n = 0; n < numSamples; n++) { + pCmplxDst[(2*n)+0] = pSrcCmplx[(2*n)+0] * pSrcReal[n]; + pCmplxDst[(2*n)+1] = pSrcCmplx[(2*n)+1] * pSrcReal[n]; + } + </pre> + + There are separate functions for floating-point, Q15, and Q31 data types. + */ + +/** + @addtogroup CmplxByRealMult + @{ + */ + +/** + @brief Floating-point complex-by-real multiplication. + @param[in] pSrcCmplx points to complex input vector + @param[in] pSrcReal points to real input vector + @param[out] pCmplxDst points to complex output vector + @param[in] numSamples number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mult_real_f32( + const float32_t * pSrcCmplx, + const float32_t * pSrcReal, + float32_t * pCmplxDst, + uint32_t numSamples) +{ + static const uint32_t stride_cmplx_x_real_32[4] = { 0, 0, 1, 1 }; + + uint32_t blockSizeC = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + f32x4_t rVec; + f32x4_t cmplxVec; + f32x4_t dstVec; + uint32x4_t strideVec; + float32_t in; + + + /* stride vector for pairs of real generation */ + strideVec = vld1q(stride_cmplx_x_real_32); + + /* Compute 4 complex outputs at a time */ + blkCnt = blockSizeC >> 2; + while (blkCnt > 0U) + { + cmplxVec = vld1q(pSrcCmplx); + rVec = vldrwq_gather_shifted_offset_f32(pSrcReal, strideVec); + dstVec = vmulq(cmplxVec, rVec); + vst1q(pCmplxDst, dstVec); + + pSrcReal += 2; + pSrcCmplx += 4; + pCmplxDst += 4; + blkCnt--; + } + + blkCnt = (blockSizeC & 3) >> 1; + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store result in destination buffer. */ + *pCmplxDst++ = *pSrcCmplx++ * in; + *pCmplxDst++ = *pSrcCmplx++ * in; + + /* Decrement loop counter */ + blkCnt--; + } +} + +#else +void arm_cmplx_mult_real_f32( + const float32_t * pSrcCmplx, + const float32_t * pSrcReal, + float32_t * pCmplxDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + float32_t in; /* Temporary variable */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + float32x4_t r; + float32x4x2_t ab,outCplx; + + /* Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + ab = vld2q_f32(pSrcCmplx); // load & separate real/imag pSrcA (de-interleave 2) + r = vld1q_f32(pSrcReal); // load & separate real/imag pSrcB + + /* Increment pointers */ + pSrcCmplx += 8; + pSrcReal += 4; + + outCplx.val[0] = vmulq_f32(ab.val[0], r); + outCplx.val[1] = vmulq_f32(ab.val[1], r); + + vst2q_f32(pCmplxDst, outCplx); + pCmplxDst += 8; + + blkCnt--; + } + + /* Tail */ + blkCnt = numSamples & 3; +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store result in destination buffer. */ + *pCmplxDst++ = *pSrcCmplx++ * in; + *pCmplxDst++ = *pSrcCmplx++ * in; + + in = *pSrcReal++; + *pCmplxDst++ = *pSrcCmplx++ * in; + *pCmplxDst++ = *pSrcCmplx++ * in; + + in = *pSrcReal++; + *pCmplxDst++ = *pSrcCmplx++ * in; + *pCmplxDst++ = *pSrcCmplx++ * in; + + in = *pSrcReal++; + *pCmplxDst++ = *pSrcCmplx++* in; + *pCmplxDst++ = *pSrcCmplx++ * in; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store result in destination buffer. */ + *pCmplxDst++ = *pSrcCmplx++ * in; + *pCmplxDst++ = *pSrcCmplx++ * in; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of CmplxByRealMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c new file mode 100644 index 0000000..84e5ae3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q15.c @@ -0,0 +1,238 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_real_q15.c + * Description: Q15 complex by real multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup CmplxByRealMult + @{ + */ + +/** + @brief Q15 complex-by-real multiplication. + @param[in] pSrcCmplx points to complex input vector + @param[in] pSrcReal points to real input vector + @param[out] pCmplxDst points to complex output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +void arm_cmplx_mult_real_q15( + const q15_t * pSrcCmplx, + const q15_t * pSrcReal, + q15_t * pCmplxDst, + uint32_t numSamples) +{ + static const uint16_t stride_cmplx_x_real_16[8] = { + 0, 0, 1, 1, 2, 2, 3, 3 + }; + q15x8_t rVec; + q15x8_t cmplxVec; + q15x8_t dstVec; + uint16x8_t strideVec; + uint32_t blockSizeC = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + q15_t in; + + /* + * stride vector for pairs of real generation + */ + strideVec = vld1q(stride_cmplx_x_real_16); + + blkCnt = blockSizeC >> 3; + + while (blkCnt > 0U) + { + cmplxVec = vld1q(pSrcCmplx); + rVec = vldrhq_gather_shifted_offset_s16(pSrcReal, strideVec); + dstVec = vqdmulhq(cmplxVec, rVec); + vst1q(pCmplxDst, dstVec); + + pSrcReal += 4; + pSrcCmplx += 8; + pCmplxDst += 8; + blkCnt --; + } + + /* Tail */ + blkCnt = (blockSizeC & 7) >> 1; + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store the result in the destination buffer. */ + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + + /* Decrement loop counter */ + blkCnt--; + } +} +#else +void arm_cmplx_mult_real_q15( + const q15_t * pSrcCmplx, + const q15_t * pSrcReal, + q15_t * pCmplxDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q15_t in; /* Temporary variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t inA1, inA2; /* Temporary variables to hold input data */ + q31_t inB1; /* Temporary variables to hold input data */ + q15_t out1, out2, out3, out4; /* Temporary variables to hold output data */ + q31_t mul1, mul2, mul3, mul4; /* Temporary variables to hold intermediate data */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + +#if defined (ARM_MATH_DSP) + /* read 2 complex numbers both real and imaginary from complex input buffer */ + inA1 = read_q15x2_ia (&pSrcCmplx); + inA2 = read_q15x2_ia (&pSrcCmplx); + /* read 2 real values at a time from real input buffer */ + inB1 = read_q15x2_ia (&pSrcReal); + + /* multiply complex number with real numbers */ +#ifndef ARM_MATH_BIG_ENDIAN + mul1 = (q31_t) ((q15_t) (inA1) * (q15_t) (inB1)); + mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1)); + mul3 = (q31_t) ((q15_t) (inA2) * (q15_t) (inB1 >> 16)); + mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB1 >> 16)); +#else + mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16)); + mul1 = (q31_t) ((q15_t) inA1 * (q15_t) (inB1 >> 16)); + mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) inB1); + mul3 = (q31_t) ((q15_t) inA2 * (q15_t) inB1); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* saturate the result */ + out1 = (q15_t) __SSAT(mul1 >> 15U, 16); + out2 = (q15_t) __SSAT(mul2 >> 15U, 16); + out3 = (q15_t) __SSAT(mul3 >> 15U, 16); + out4 = (q15_t) __SSAT(mul4 >> 15U, 16); + + /* pack real and imaginary outputs and store them to destination */ + write_q15x2_ia (&pCmplxDst, __PKHBT(out1, out2, 16)); + write_q15x2_ia (&pCmplxDst, __PKHBT(out3, out4, 16)); + + inA1 = read_q15x2_ia (&pSrcCmplx); + inA2 = read_q15x2_ia (&pSrcCmplx); + inB1 = read_q15x2_ia (&pSrcReal); + +#ifndef ARM_MATH_BIG_ENDIAN + mul1 = (q31_t) ((q15_t) (inA1) * (q15_t) (inB1)); + mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1)); + mul3 = (q31_t) ((q15_t) (inA2) * (q15_t) (inB1 >> 16)); + mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB1 >> 16)); +#else + mul2 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16)); + mul1 = (q31_t) ((q15_t) inA1 * (q15_t) (inB1 >> 16)); + mul4 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) inB1); + mul3 = (q31_t) ((q15_t) inA2 * (q15_t) inB1); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + out1 = (q15_t) __SSAT(mul1 >> 15U, 16); + out2 = (q15_t) __SSAT(mul2 >> 15U, 16); + out3 = (q15_t) __SSAT(mul3 >> 15U, 16); + out4 = (q15_t) __SSAT(mul4 >> 15U, 16); + + write_q15x2_ia (&pCmplxDst, __PKHBT(out1, out2, 16)); + write_q15x2_ia (&pCmplxDst, __PKHBT(out3, out4, 16)); +#else + in = *pSrcReal++; + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + + in = *pSrcReal++; + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + + in = *pSrcReal++; + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + + in = *pSrcReal++; + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store the result in the destination buffer. */ + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + *pCmplxDst++ = (q15_t) __SSAT((((q31_t) *pSrcCmplx++ * in) >> 15), 16); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of CmplxByRealMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c new file mode 100644 index 0000000..de13757 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/ComplexMathFunctions/arm_cmplx_mult_real_q31.c @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mult_real_q31.c + * Description: Q31 complex by real multiplication + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2021 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/complex_math_functions.h" + +/** + @ingroup groupCmplxMath + */ + +/** + @addtogroup CmplxByRealMult + @{ + */ + +/** + @brief Q31 complex-by-real multiplication. + @param[in] pSrcCmplx points to complex input vector + @param[in] pSrcReal points to real input vector + @param[out] pCmplxDst points to complex output vector + @param[in] numSamples number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q31 range[0x80000000 0x7FFFFFFF] are saturated. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) +void arm_cmplx_mult_real_q31( + const q31_t * pSrcCmplx, + const q31_t * pSrcReal, + q31_t * pCmplxDst, + uint32_t numSamples) +{ + + static const uint32_t stride_cmplx_x_real_32[4] = { + 0, 0, 1, 1 + }; + q31x4_t rVec; + q31x4_t cmplxVec; + q31x4_t dstVec; + uint32x4_t strideVec; + uint32_t blockSizeC = numSamples * CMPLX_DIM; /* loop counters */ + uint32_t blkCnt; + q31_t in; + + /* + * stride vector for pairs of real generation + */ + strideVec = vld1q(stride_cmplx_x_real_32); + + /* Compute 4 complex outputs at a time */ + blkCnt = blockSizeC >> 2; + while (blkCnt > 0U) + { + cmplxVec = vld1q(pSrcCmplx); + rVec = vldrwq_gather_shifted_offset_s32(pSrcReal, strideVec); + dstVec = vqdmulhq(cmplxVec, rVec); + vst1q(pCmplxDst, dstVec); + + pSrcReal += 2; + pSrcCmplx += 4; + pCmplxDst += 4; + blkCnt --; + } + + blkCnt = (blockSizeC & 3) >> 1; + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; + /* store saturated result in 1.31 format to destination buffer */ + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + + /* Decrement loop counter */ + blkCnt--; + } +} +#else +void arm_cmplx_mult_real_q31( + const q31_t * pSrcCmplx, + const q31_t * pSrcReal, + q31_t * pCmplxDst, + uint32_t numSamples) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t in; /* Temporary variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; +#if defined (ARM_MATH_DSP) + /* store saturated result in 1.31 format to destination buffer */ + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); +#else + /* store result in destination buffer. */ + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); +#endif + + in = *pSrcReal++; +#if defined (ARM_MATH_DSP) + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); +#else + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); +#endif + + in = *pSrcReal++; +#if defined (ARM_MATH_DSP) + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); +#else + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); +#endif + + in = *pSrcReal++; +#if defined (ARM_MATH_DSP) + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); +#else + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C[2 * i ] = A[2 * i ] * B[i]. */ + /* C[2 * i + 1] = A[2 * i + 1] * B[i]. */ + + in = *pSrcReal++; +#if defined (ARM_MATH_DSP) + /* store saturated result in 1.31 format to destination buffer */ + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); + *pCmplxDst++ = (__SSAT((q31_t) (((q63_t) *pSrcCmplx++ * in) >> 32), 31) << 1); +#else + /* store result in destination buffer. */ + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); + *pCmplxDst++ = (q31_t) clip_q63_to_q31(((q63_t) *pSrcCmplx++ * in) >> 31); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of CmplxByRealMult group + */ |