diff options
Diffstat (limited to 'Drivers/CMSIS/DSP/Source/BasicMathFunctions')
71 files changed, 11019 insertions, 0 deletions
diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctions.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctions.c new file mode 100644 index 0000000..7ade747 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctions.c @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: BasicMathFunctions.c + * Description: Combination of all basic math function source files. + * + * $Date: 16. March 2020 + * $Revision: V1.1.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2019-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_abs_f32.c" +#include "arm_abs_f64.c" +#include "arm_abs_q15.c" +#include "arm_abs_q31.c" +#include "arm_abs_q7.c" +#include "arm_add_f32.c" +#include "arm_add_f64.c" +#include "arm_add_q15.c" +#include "arm_add_q31.c" +#include "arm_add_q7.c" +#include "arm_and_u16.c" +#include "arm_and_u32.c" +#include "arm_and_u8.c" +#include "arm_dot_prod_f32.c" +#include "arm_dot_prod_f64.c" +#include "arm_dot_prod_q15.c" +#include "arm_dot_prod_q31.c" +#include "arm_dot_prod_q7.c" +#include "arm_mult_f32.c" +#include "arm_mult_f64.c" +#include "arm_mult_q15.c" +#include "arm_mult_q31.c" +#include "arm_mult_q7.c" +#include "arm_negate_f32.c" +#include "arm_negate_f64.c" +#include "arm_negate_q15.c" +#include "arm_negate_q31.c" +#include "arm_negate_q7.c" +#include "arm_not_u16.c" +#include "arm_not_u32.c" +#include "arm_not_u8.c" +#include "arm_offset_f32.c" +#include "arm_offset_f64.c" +#include "arm_offset_q15.c" +#include "arm_offset_q31.c" +#include "arm_offset_q7.c" +#include "arm_or_u16.c" +#include "arm_or_u32.c" +#include "arm_or_u8.c" +#include "arm_scale_f32.c" +#include "arm_scale_f64.c" +#include "arm_scale_q15.c" +#include "arm_scale_q31.c" +#include "arm_scale_q7.c" +#include "arm_shift_q15.c" +#include "arm_shift_q31.c" +#include "arm_shift_q7.c" +#include "arm_sub_f32.c" +#include "arm_sub_f64.c" +#include "arm_sub_q15.c" +#include "arm_sub_q31.c" +#include "arm_sub_q7.c" +#include "arm_xor_u16.c" +#include "arm_xor_u32.c" +#include "arm_xor_u8.c" +#include "arm_clip_f32.c" +#include "arm_clip_q31.c" +#include "arm_clip_q15.c" +#include "arm_clip_q7.c" diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctionsF16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctionsF16.c new file mode 100644 index 0000000..f185db4 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/BasicMathFunctionsF16.c @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: BasicMathFunctionsF16.c + * Description: Combination of all basic math function f16 source files. + * + * $Date: 20. April 2020 + * $Revision: V1.1.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2019-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_abs_f16.c" +#include "arm_add_f16.c" +#include "arm_dot_prod_f16.c" +#include "arm_mult_f16.c" +#include "arm_negate_f16.c" +#include "arm_offset_f16.c" +#include "arm_scale_f16.c" +#include "arm_sub_f16.c" +#include "arm_clip_f16.c" diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/CMakeLists.txt b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/CMakeLists.txt new file mode 100644 index 0000000..37802a8 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required (VERSION 3.14) + +project(CMSISDSPBasicMath) + +include(configLib) +include(configDsp) + +file(GLOB SRCF64 "./*_f64.c") +file(GLOB SRCF32 "./*_f32.c") +file(GLOB SRCF16 "./*_f16.c") +file(GLOB SRCQ31 "./*_q31.c") +file(GLOB SRCQ15 "./*_q15.c") +file(GLOB SRCQ7 "./*_q7.c") + +file(GLOB SRCU32 "./*_u32.c") +file(GLOB SRCU16 "./*_u16.c") +file(GLOB SRCU8 "./*_u8.c") + +add_library(CMSISDSPBasicMath STATIC ${SRCF64}) +target_sources(CMSISDSPBasicMath PRIVATE ${SRCF32}) + +if ((NOT ARMAC5) AND (NOT DISABLEFLOAT16)) +target_sources(CMSISDSPBasicMath PRIVATE ${SRCF16}) +endif() + +target_sources(CMSISDSPBasicMath PRIVATE ${SRCQ31}) +target_sources(CMSISDSPBasicMath PRIVATE ${SRCQ15}) +target_sources(CMSISDSPBasicMath PRIVATE ${SRCQ7}) + +target_sources(CMSISDSPBasicMath PRIVATE ${SRCU32}) +target_sources(CMSISDSPBasicMath PRIVATE ${SRCU16}) +target_sources(CMSISDSPBasicMath PRIVATE ${SRCU8}) + +configLib(CMSISDSPBasicMath ${ROOT}) +configDsp(CMSISDSPBasicMath ${ROOT}) + +### Includes +target_include_directories(CMSISDSPBasicMath PUBLIC "${DSP}/Include") + + + diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f16.c new file mode 100644 index 0000000..0d6568b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f16.c @@ -0,0 +1,198 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_abs_f16.c + * Description: Floating-point vector absolute value + * + * $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/basic_math_functions_f16.h" +#include <math.h> + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicAbs Vector Absolute Value + + Computes the absolute value of a vector on an element-by-element basis. + + <pre> + pDst[n] = abs(pSrc[n]), 0 <= n < blockSize. + </pre> + + The functions support in-place computation allowing the source and + destination pointers to reference the same memory buffer. + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicAbs + @{ + */ + +/** + @brief Floating-point vector absolute value. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] blockSize 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_abs_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + f16x8_t vec1; + f16x8_t res; + + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute values and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vabsq(vec1); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + + + if (blkCnt > 0U) + { + /* C = |A| */ + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q(pSrc); + vstrhq_p(pDst, vabsq(vec1), p0); + } + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_abs_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + f16x8_t vec1; + f16x8_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute values and then store the results in the destination buffer. */ + vec1 = vld1q_f16(pSrc); + res = vabsq_f16(vec1); + vst1q_f16(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute and store result in destination buffer. */ + *pDst++ = (_Float16)fabsf((float32_t)*pSrc++); + + *pDst++ = (_Float16)fabsf((float32_t)*pSrc++); + + *pDst++ = (_Float16)fabsf((float32_t)*pSrc++); + + *pDst++ = (_Float16)fabsf((float32_t)*pSrc++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute and store result in destination buffer. */ + *pDst++ = (_Float16)fabsf((float32_t)*pSrc++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_FLOAT16_SUPPORTED */ +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ +/** + @} end of BasicAbs group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f32.c new file mode 100644 index 0000000..1e67f4e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f32.c @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_abs_f32.c + * Description: Floating-point vector absolute value + * + * $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/basic_math_functions.h" +#include <math.h> + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicAbs Vector Absolute Value + + Computes the absolute value of a vector on an element-by-element basis. + + <pre> + pDst[n] = abs(pSrc[n]), 0 <= n < blockSize. + </pre> + + The functions support in-place computation allowing the source and + destination pointers to reference the same memory buffer. + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicAbs + @{ + */ + +/** + @brief Floating-point vector absolute value. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_abs_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + f32x4_t vec1; + f32x4_t res; + + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute values and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vabsq(vec1); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + + + if (blkCnt > 0U) + { + /* C = |A| */ + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q(pSrc); + vstrwq_p(pDst, vabsq(vec1), p0); + } + +} + +#else +void arm_abs_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute values and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrc); + res = vabsq_f32(vec1); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute and store result in destination buffer. */ + *pDst++ = fabsf(*pSrc++); + + *pDst++ = fabsf(*pSrc++); + + *pDst++ = fabsf(*pSrc++); + + *pDst++ = fabsf(*pSrc++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute and store result in destination buffer. */ + *pDst++ = fabsf(*pSrc++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ +/** + @} end of BasicAbs group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f64.c new file mode 100644 index 0000000..04a79f7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_f64.c @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_abs_f64.c + * Description: Floating-point vector absolute value + * + * $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/basic_math_functions.h" +#include <math.h> + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAbs + @{ + */ + +/** + @brief Floating-point vector absolute value. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_abs_f64( + const float64_t * pSrc, + float64_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute and store result in destination buffer. */ + *pDst++ = fabs(*pSrc++); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicAbs group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q15.c new file mode 100644 index 0000000..7c38ef5 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q15.c @@ -0,0 +1,178 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_abs_q15.c + * Description: Q15 vector absolute value + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAbs + @{ + */ + +/** + @brief Q15 vector absolute value. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + The Q15 value -1 (0x8000) will be saturated to the maximum allowable positive value 0x7FFF. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_abs_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecSrc; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = |A| + * Calculate absolute and then store the results in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqabsq(vecSrc)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrc = vld1q(pSrc); + vstrhq_p(pDst, vqabsq(vecSrc), p0); + } +} + +#else +void arm_abs_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q15_t in; /* Temporary input variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute of input (if -1 then saturated to 0x7fff) and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q15_t)__QSUB16(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q15_t)__QSUB16(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q15_t)__QSUB16(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q15_t)__QSUB16(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute of input (if -1 then saturated to 0x7fff) and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q15_t)__QSUB16(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q15_t) 0x8000) ? 0x7fff : -in); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicAbs group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q31.c new file mode 100644 index 0000000..7043aa0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q31.c @@ -0,0 +1,208 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_abs_q31.c + * Description: Q31 vector absolute value + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAbs + @{ + */ + +/** + @brief Q31 vector absolute value. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + The Q31 value -1 (0x80000000) will be saturated to the maximum allowable positive value 0x7FFFFFFF. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_abs_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counters */ + q31x4_t vecSrc; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + + while (blkCnt > 0U) + { + /* + * C = |A| + * Calculate absolute and then store the results in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqabsq(vecSrc)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * Advance vector source and destination pointers + */ + pSrc += 4; + pDst += 4; + } + /* + * Tail + */ + blkCnt = blockSize & 3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrc = vld1q(pSrc); + vstrwq_p(pDst, vqabsq(vecSrc), p0); + } +} + +#else +void arm_abs_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t in; /* Temporary variable */ + +#if defined(ARM_MATH_NEON) + int32x4_t vec1; + int32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + /* Calculate absolute and then store the results in the destination buffer. */ + + vec1 = vld1q_s32(pSrc); + res = vqabsq_s32(vec1); + vst1q_s32(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the blockSize loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute of input (if -1 then saturated to 0x7fffffff) and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q31_t)__QSUB(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == INT32_MIN) ? INT32_MAX : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q31_t)__QSUB(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == INT32_MIN) ? INT32_MAX : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q31_t)__QSUB(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == INT32_MIN) ? INT32_MAX : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q31_t)__QSUB(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == INT32_MIN) ? INT32_MAX : -in); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined (ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute of input (if -1 then saturated to 0x7fffffff) and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q31_t)__QSUB(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == INT32_MIN) ? INT32_MAX : -in); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* #if defined (ARM_MATH_MVEI) */ +/** + @} end of BasicAbs group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q7.c new file mode 100644 index 0000000..bd79582 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_abs_q7.c @@ -0,0 +1,180 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_abs_q7.c + * Description: Q7 vector absolute value + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAbs + @{ + */ + +/** + @brief Q7 vector absolute value. + @param[in] pSrc points to the input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Conditions for optimum performance + Input and output buffers should be aligned by 32-bit + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + The Q7 value -1 (0x80) will be saturated to the maximum allowable positive value 0x7F. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_abs_q7( + const q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecSrc; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = |A| + * Calculate absolute and then store the results in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqabsq(vecSrc)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrc = vld1q(pSrc); + vstrbq_p(pDst, vqabsq(vecSrc), p0); + } +} + +#else +void arm_abs_q7( + const q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q7_t in; /* Temporary input variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute of input (if -1 then saturated to 0x7f) and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q7_t)__QSUB8(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? (q7_t) 0x7f : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q7_t)__QSUB8(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? (q7_t) 0x7f : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q7_t)__QSUB8(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? (q7_t) 0x7f : -in); +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q7_t)__QSUB8(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? (q7_t) 0x7f : -in); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute of input (if -1 then saturated to 0x7f) and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = (in > 0) ? in : (q7_t) __QSUB8(0, in); +#else + *pDst++ = (in > 0) ? in : ((in == (q7_t) 0x80) ? (q7_t) 0x7f : -in); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicAbs group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f16.c new file mode 100644 index 0000000..2b2ae8f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f16.c @@ -0,0 +1,169 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_add_f16.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicAdd Vector Addition + + Element-by-element addition of two vectors. + + <pre> + pDst[n] = pSrcA[n] + pSrcB[n], 0 <= n < blockSize. + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicAdd + @{ + */ + +/** + @brief Floating-point vector addition. + @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] blockSize 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_add_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f16x8_t vec1; + f16x8_t vec2; + f16x8_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + res = vaddq(vec1, vec2); + vst1q(pDst, res); + + /* Increment pointers */ + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + + if (blkCnt > 0U) + { + /* C = A + B */ + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + vstrhq_p(pDst, vaddq(vec1,vec2), p0); + } + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_add_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrcA++) + (_Float16)(*pSrcB++); + *pDst++ = (_Float16)(*pSrcA++) + (_Float16)(*pSrcB++); + *pDst++ = (_Float16)(*pSrcA++) + (_Float16)(*pSrcB++); + *pDst++ = (_Float16)(*pSrcA++) + (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrcA++) + (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_FLOAT16_SUPPORTED) */ +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicAdd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f32.c new file mode 100644 index 0000000..9741e4a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f32.c @@ -0,0 +1,199 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_add_f32.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicAdd Vector Addition + + Element-by-element addition of two vectors. + + <pre> + pDst[n] = pSrcA[n] + pSrcB[n], 0 <= n < blockSize. + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicAdd + @{ + */ + +/** + @brief Floating-point vector addition. + @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] blockSize number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_add_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f32x4_t vec1; + f32x4_t vec2; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + res = vaddq(vec1, vec2); + vst1q(pDst, res); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + + if (blkCnt > 0U) + { + /* C = A + B */ + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + vstrwq_p(pDst, vaddq(vec1,vec2), p0); + } + +} + +#else +void arm_add_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t vec2; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrcA); + vec2 = vld1q_f32(pSrcB); + res = vaddq_f32(vec1, vec2); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = (*pSrcA++) + (*pSrcB++); + *pDst++ = (*pSrcA++) + (*pSrcB++); + *pDst++ = (*pSrcA++) + (*pSrcB++); + *pDst++ = (*pSrcA++) + (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = (*pSrcA++) + (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicAdd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f64.c new file mode 100644 index 0000000..2d77233 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f64.c @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_add_f64.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAdd + @{ + */ + +/** + @brief Floating-point vector addition. + @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] blockSize number of samples in each vector + @return none + */ + +void arm_add_f64( + const float64_t * pSrcA, + const float64_t * pSrcB, + float64_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = (*pSrcA++) + (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicAdd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q15.c new file mode 100644 index 0000000..0bf9d06 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q15.c @@ -0,0 +1,176 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_add_q15.c + * Description: Q15 vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAdd + @{ + */ + +/** + @brief Q15 vector addition. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_add_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecA; + q15x8_t vecB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = A + B + * Add and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqaddq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrhq_p(pDst, vqaddq(vecA, vecB), p0); + } +} + +#else +void arm_add_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t inA1, inA2; + q31_t inB1, inB2; +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + +#if defined (ARM_MATH_DSP) + /* read 2 times 2 samples at a time from sourceA */ + inA1 = read_q15x2_ia (&pSrcA); + inA2 = read_q15x2_ia (&pSrcA); + /* read 2 times 2 samples at a time from sourceB */ + inB1 = read_q15x2_ia (&pSrcB); + inB2 = read_q15x2_ia (&pSrcB); + + /* Add and store 2 times 2 samples at a time */ + write_q15x2_ia (&pDst, __QADD16(inA1, inB1)); + write_q15x2_ia (&pDst, __QADD16(inA2, inB2)); +#else + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ + *pSrcB++), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ + *pSrcB++), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ + *pSrcB++), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ + *pSrcB++), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + *pDst++ = (q15_t) __QADD16(*pSrcA++, *pSrcB++); +#else + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ + *pSrcB++), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ +/** + @} end of BasicAdd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q31.c new file mode 100644 index 0000000..a6783e7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q31.c @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_add_q31.c + * Description: Q31 vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAdd + @{ + */ + +/** + @brief Q31 vector addition. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_add_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; + q31x4_t vecA; + q31x4_t vecB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A + B + * Add and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqaddq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrwq_p(pDst, vqaddq(vecA, vecB), p0); + } +} + +#else +void arm_add_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = __QADD(*pSrcA++, *pSrcB++); + + *pDst++ = __QADD(*pSrcA++, *pSrcB++); + + *pDst++ = __QADD(*pSrcA++, *pSrcB++); + + *pDst++ = __QADD(*pSrcA++, *pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = __QADD(*pSrcA++, *pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +#endif /* defined(ARM_MATH_MVEI) */ +/** + @} end of BasicAdd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q7.c new file mode 100644 index 0000000..f58ff86 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_add_q7.c @@ -0,0 +1,159 @@ +/* + * 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. + */ + +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_add_q7.c + * Description: Q7 vector addition + * + * $Date: 23 April 2021 + * $Revision: V1.9.0 + * + * Target Processor: Cortex-M and Cortex-A cores + * -------------------------------------------------------------------- */ + +#include "dsp/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicAdd + @{ + */ + +/** + @brief Q7 vector addition. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q7 range [0x80 0x7F] are saturated. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_add_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecA; + q7x16_t vecB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = A + B + * Add and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqaddq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 16; + pSrcB += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrbq_p(pDst, vqaddq(vecA, vecB), p0); + } +} +#else +void arm_add_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + +#if defined (ARM_MATH_DSP) + /* Add and store result in destination buffer (4 samples at a time). */ + write_q7x4_ia (&pDst, __QADD8 (read_q7x4_ia (&pSrcA), read_q7x4_ia (&pSrcB))); +#else + *pDst++ = (q7_t) __SSAT ((q15_t) *pSrcA++ + *pSrcB++, 8); + *pDst++ = (q7_t) __SSAT ((q15_t) *pSrcA++ + *pSrcB++, 8); + *pDst++ = (q7_t) __SSAT ((q15_t) *pSrcA++ + *pSrcB++, 8); + *pDst++ = (q7_t) __SSAT ((q15_t) *pSrcA++ + *pSrcB++, 8); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and store result in destination buffer. */ + *pDst++ = (q7_t) __SSAT((q15_t) *pSrcA++ + *pSrcB++, 8); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ +/** + @} end of BasicAdd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u16.c new file mode 100644 index 0000000..7a7427d --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u16.c @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_and_u16.c + * Description: uint16_t bitwise AND + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup And Vector bitwise AND + + Compute the logical bitwise AND. + + There are separate functions for uint32_t, uint16_t, and uint7_t data types. + */ + +/** + @addtogroup And + @{ + */ + +/** + @brief Compute the logical bitwise AND of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_and_u16( + const uint16_t * pSrcA, + const uint16_t * pSrcB, + uint16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecSrcA, vecSrcB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, vandq_u16(vecSrcA, vecSrcB) ); + + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrhq_p(pDst, vandq_u16(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecA, vecB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + vecA = vld1q_u16(pSrcA); + vecB = vld1q_u16(pSrcB); + + vst1q_u16(pDst, vandq_u16(vecA, vecB) ); + + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)&(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of And group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u32.c new file mode 100644 index 0000000..ddf838c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u32.c @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_and_u32.c + * Description: uint32_t bitwise AND + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup And + @{ + */ + +/** + @brief Compute the logical bitwise AND of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_and_u32( + const uint32_t * pSrcA, + const uint32_t * pSrcB, + uint32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecSrcA, vecSrcB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, vandq_u32(vecSrcA, vecSrcB) ); + + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrwq_p(pDst, vandq_u32(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecA, vecB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + vecA = vld1q_u32(pSrcA); + vecB = vld1q_u32(pSrcB); + + vst1q_u32(pDst, vandq_u32(vecA, vecB) ); + + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)&(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of And group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u8.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u8.c new file mode 100644 index 0000000..bd1a1ab --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_and_u8.c @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_and_u8.c + * Description: uint8_t bitwise AND + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + + +/** + @addtogroup And + @{ + */ + +/** + @brief Compute the logical bitwise AND of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_and_u8( + const uint8_t * pSrcA, + const uint8_t * pSrcB, + uint8_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecSrcA, vecSrcB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, vandq_u8(vecSrcA, vecSrcB) ); + + pSrcA += 16; + pSrcB += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrbq_p(pDst, vandq_u8(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecA, vecB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4U; + + while (blkCnt > 0U) + { + vecA = vld1q_u8(pSrcA); + vecB = vld1q_u8(pSrcB); + + vst1q_u8(pDst, vandq_u8(vecA, vecB) ); + + pSrcA += 16; + pSrcB += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)&(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of And group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_f16.c new file mode 100644 index 0000000..f784a23 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_f16.c @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_clip_f16.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + + +/** + @addtogroup BasicClip + @{ + */ + +/** + @brief Elementwise floating-point clipping + @param[in] pSrc points to input values + @param[out] pDst points to output clipped values + @param[in] low lower bound + @param[in] high higher bound + @param[in] numSamples number of samples to clip + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_clip_f16(const float16_t * pSrc, + float16_t * pDst, + float16_t low, + float16_t high, + uint32_t numSamples) +{ + uint32_t blkCnt; + f16x8_t curVec0, curVec1; + f16x8_t vecLow, vecHigh; + + vecLow = vdupq_n_f16(low); + vecHigh = vdupq_n_f16(high); + + curVec0 = vld1q(pSrc); + pSrc += 8; + /* + * unrolled x 2 to allow + * vldr/vstr/vmin/vmax + * stall free interleaving + */ + blkCnt = numSamples >> 4; + while (blkCnt--) + { + curVec0 = vmaxnmq(curVec0, vecLow); + curVec1 = vld1q(pSrc); + pSrc += 8; + curVec0 = vminnmq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 8; + curVec1 = vmaxnmq(curVec1, vecLow); + curVec0 = vld1q(pSrc); + pSrc += 8; + curVec1 = vminnmq(curVec1, vecHigh); + vst1q(pDst, curVec1); + pDst += 8; + } + /* + * Tail handling + */ + blkCnt = numSamples - ((numSamples >> 4) << 4); + if (blkCnt >= 8) + { + curVec0 = vmaxnmq(curVec0, vecLow); + curVec0 = vminnmq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 8; + curVec0 = vld1q(pSrc); + pSrc += 8; + } + + if (blkCnt > 0) + { + mve_pred16_t p0 = vctp16q(blkCnt & 7); + curVec0 = vmaxnmq(curVec0, vecLow); + curVec0 = vminnmq(curVec0, vecHigh); + vstrhq_p(pDst, curVec0, p0); + } +} + +#else + +#if defined(ARM_FLOAT16_SUPPORTED) + +void arm_clip_f16(const float16_t * pSrc, + float16_t * pDst, + float16_t low, + float16_t high, + uint32_t numSamples) +{ + for (uint32_t i = 0; i < numSamples; i++) + { + if ((_Float16)pSrc[i] > (_Float16)high) + pDst[i] = high; + else if ((_Float16)pSrc[i] < (_Float16)low) + pDst[i] = low; + else + pDst[i] = pSrc[i]; + } +} +#endif /* defined(ARM_FLOAT16_SUPPORTED */ + +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + + +/** + @} end of BasicClip group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_f32.c new file mode 100644 index 0000000..4cc2799 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_f32.c @@ -0,0 +1,144 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_clip_f32.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicClip Elementwise clipping + + Element-by-element clipping of a value. + + The value is constrained between 2 bounds. + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicClip + @{ + */ + +/** + @brief Elementwise floating-point clipping + @param[in] pSrc points to input values + @param[out] pDst points to output clipped values + @param[in] low lower bound + @param[in] high higher bound + @param[in] numSamples number of samples to clip + @return none + */ +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_clip_f32(const float32_t * pSrc, + float32_t * pDst, + float32_t low, + float32_t high, + uint32_t numSamples) +{ + uint32_t blkCnt; + f32x4_t curVec0, curVec1; + f32x4_t vecLow, vecHigh; + + vecLow = vdupq_n_f32(low); + vecHigh = vdupq_n_f32(high); + + curVec0 = vld1q(pSrc); + pSrc += 4; + /* + * unrolled x 2 to allow + * vldr/vstr/vmin/vmax + * stall free interleaving + */ + blkCnt = numSamples >> 3; + while (blkCnt--) + { + curVec0 = vmaxnmq(curVec0, vecLow); + curVec1 = vld1q(pSrc); + pSrc += 4; + curVec0 = vminnmq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 4; + curVec1 = vmaxnmq(curVec1, vecLow); + curVec0 = vld1q(pSrc); + pSrc += 4; + curVec1 = vminnmq(curVec1, vecHigh); + vst1q(pDst, curVec1); + pDst += 4; + } + /* + * Tail handling + */ + blkCnt = numSamples - ((numSamples >> 3) << 3); + if (blkCnt >= 4) + { + curVec0 = vmaxnmq(curVec0, vecLow); + curVec0 = vminnmq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 4; + curVec0 = vld1q(pSrc); + pSrc += 4; + } + + if (blkCnt > 0) + { + mve_pred16_t p0 = vctp32q(blkCnt & 3); + curVec0 = vmaxnmq(curVec0, vecLow); + curVec0 = vminnmq(curVec0, vecHigh); + vstrwq_p(pDst, curVec0, p0); + } +} + +#else +void arm_clip_f32(const float32_t * pSrc, + float32_t * pDst, + float32_t low, + float32_t high, + uint32_t numSamples) +{ + uint32_t i; + for (i = 0; i < numSamples; i++) + { + if (pSrc[i] > high) + pDst[i] = high; + else if (pSrc[i] < low) + pDst[i] = low; + else + pDst[i] = pSrc[i]; + } +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicClip group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q15.c new file mode 100644 index 0000000..0d67c37 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q15.c @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_clip_q15.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + + +/** + @addtogroup BasicClip + @{ + */ + +/** + @brief Elementwise fixed-point clipping + @param[in] pSrc points to input values + @param[out] pDst points to output clipped values + @param[in] low lower bound + @param[in] high higher bound + @param[in] numSamples number of samples to clip + @return none + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" +void arm_clip_q15(const q15_t * pSrc, + q15_t * pDst, + q15_t low, + q15_t high, + uint32_t numSamples) +{ + uint32_t blkCnt; + q15x8_t curVec0, curVec1; + q15x8_t vecLow, vecHigh; + + vecLow = vdupq_n_s16(low); + vecHigh = vdupq_n_s16(high); + + curVec0 = vld1q(pSrc); + pSrc += 8; + /* + * unrolled x 2 to allow + * vldr/vstr/vmin/vmax + * stall free interleaving + */ + blkCnt = numSamples >> 4; + while (blkCnt--) + { + curVec0 = vmaxq(curVec0, vecLow); + curVec1 = vld1q(pSrc); + pSrc += 8; + curVec0 = vminq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 8; + curVec1 = vmaxq(curVec1, vecLow); + curVec0 = vld1q(pSrc); + pSrc += 8; + curVec1 = vminq(curVec1, vecHigh); + vst1q(pDst, curVec1); + pDst += 8; + } + /* + * Tail handling + */ + blkCnt = numSamples - ((numSamples >> 4) << 4); + if (blkCnt >= 8) + { + curVec0 = vmaxq(curVec0, vecLow); + curVec0 = vminq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 8; + curVec0 = vld1q(pSrc); + pSrc += 8; + } + + if (blkCnt > 0) + { + mve_pred16_t p0 = vctp16q(blkCnt & 7); + curVec0 = vmaxq(curVec0, vecLow); + curVec0 = vminq(curVec0, vecHigh); + vstrhq_p(pDst, curVec0, p0); + } +} + +#else +void arm_clip_q15(const q15_t * pSrc, + q15_t * pDst, + q15_t low, + q15_t high, + uint32_t numSamples) +{ + uint32_t i; + for (i = 0; i < numSamples; i++) + { + if (pSrc[i] > high) + pDst[i] = high; + else if (pSrc[i] < low) + pDst[i] = low; + else + pDst[i] = pSrc[i]; + } +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicClip group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q31.c new file mode 100644 index 0000000..72b80d3 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q31.c @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_clip_q31.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + + +/** + @addtogroup BasicClip + @{ + */ + +/** + @brief Elementwise fixed-point clipping + @param[in] pSrc points to input values + @param[out] pDst points to output clipped values + @param[in] low lower bound + @param[in] high higher bound + @param[in] numSamples number of samples to clip + @return none + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" +void arm_clip_q31(const q31_t * pSrc, + q31_t * pDst, + q31_t low, + q31_t high, + uint32_t numSamples) +{ + uint32_t blkCnt; + q31x4_t curVec0, curVec1; + q31x4_t vecLow, vecHigh; + + vecLow = vdupq_n_s32(low); + vecHigh = vdupq_n_s32(high); + + curVec0 = vld1q(pSrc); + pSrc += 4; + /* + * unrolled x 2 to allow + * vldr/vstr/vmin/vmax + * stall free interleaving + */ + blkCnt = numSamples >> 3; + while (blkCnt--) + { + curVec0 = vmaxq(curVec0, vecLow); + curVec1 = vld1q(pSrc); + pSrc += 4; + curVec0 = vminq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 4; + curVec1 = vmaxq(curVec1, vecLow); + curVec0 = vld1q(pSrc); + pSrc += 4; + curVec1 = vminq(curVec1, vecHigh); + vst1q(pDst, curVec1); + pDst += 4; + } + /* + * Tail handling + */ + blkCnt = numSamples - ((numSamples >> 3) << 3); + if (blkCnt >= 4) + { + curVec0 = vmaxq(curVec0, vecLow); + curVec0 = vminq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 4; + curVec0 = vld1q(pSrc); + pSrc += 4; + } + + if (blkCnt > 0) + { + mve_pred16_t p0 = vctp32q(blkCnt & 3); + curVec0 = vmaxq(curVec0, vecLow); + curVec0 = vminq(curVec0, vecHigh); + vstrwq_p(pDst, curVec0, p0); + } +} + +#else +void arm_clip_q31(const q31_t * pSrc, + q31_t * pDst, + q31_t low, + q31_t high, + uint32_t numSamples) +{ + uint32_t i; + for (i = 0; i < numSamples; i++) + { + if (pSrc[i] > high) + pDst[i] = high; + else if (pSrc[i] < low) + pDst[i] = low; + else + pDst[i] = pSrc[i]; + } +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicClip group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q7.c new file mode 100644 index 0000000..a41e0c9 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_clip_q7.c @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_clip_q7.c + * Description: Floating-point vector addition + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + + +/** + @addtogroup BasicClip + @{ + */ + +/** + @brief Elementwise fixed-point clipping + @param[in] pSrc points to input values + @param[out] pDst points to output clipped values + @param[in] low lower bound + @param[in] high higher bound + @param[in] numSamples number of samples to clip + @return none + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" +void arm_clip_q7(const q7_t * pSrc, + q7_t * pDst, + q7_t low, + q7_t high, + uint32_t numSamples) +{ + uint32_t blkCnt; + q7x16_t curVec0, curVec1; + q7x16_t vecLow, vecHigh; + + vecLow = vdupq_n_s8(low); + vecHigh = vdupq_n_s8(high); + + curVec0 = vld1q(pSrc); + pSrc += 16; + /* + * unrolled x 2 to allow + * vldr/vstr/vmin/vmax + * stall free interleaving + */ + blkCnt = numSamples >> 5; + while (blkCnt--) + { + curVec0 = vmaxq(curVec0, vecLow); + curVec1 = vld1q(pSrc); + pSrc += 16; + curVec0 = vminq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 16; + curVec1 = vmaxq(curVec1, vecLow); + curVec0 = vld1q(pSrc); + pSrc += 16; + curVec1 = vminq(curVec1, vecHigh); + vst1q(pDst, curVec1); + pDst += 16; + } + /* + * Tail handling + */ + blkCnt = numSamples - ((numSamples >> 5) << 5); + if (blkCnt >= 16) + { + curVec0 = vmaxq(curVec0, vecLow); + curVec0 = vminq(curVec0, vecHigh); + vst1q(pDst, curVec0); + pDst += 16; + curVec0 = vld1q(pSrc); + pSrc += 16; + } + + if (blkCnt > 0) + { + mve_pred16_t p0 = vctp8q(blkCnt & 0xf); + curVec0 = vmaxq(curVec0, vecLow); + curVec0 = vminq(curVec0, vecHigh); + vstrbq_p(pDst, curVec0, p0); + } +} + +#else +void arm_clip_q7(const q7_t * pSrc, + q7_t * pDst, + q7_t low, + q7_t high, + uint32_t numSamples) +{ + uint32_t i; + for (i = 0; i < numSamples; i++) + { + if (pSrc[i] > high) + pDst[i] = high; + else if (pSrc[i] < low) + pDst[i] = low; + else + pDst[i] = pSrc[i]; + } +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicClip group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f16.c new file mode 100644 index 0000000..55c88df --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f16.c @@ -0,0 +1,184 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_dot_prod_f16.c + * Description: Floating-point 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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicDotProd Vector Dot Product + + Computes the dot product of two vectors. + The vectors are multiplied element-by-element and then summed. + + <pre> + sum = pSrcA[0]*pSrcB[0] + pSrcA[1]*pSrcB[1] + ... + pSrcA[blockSize-1]*pSrcB[blockSize-1] + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicDotProd + @{ + */ + +/** + @brief Dot product of floating-point vectors. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[in] blockSize number of samples in each vector. + @param[out] result output result returned here. + @return none + */ + +#if defined(ARM_MATH_MVE_FLOAT16) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + + +void arm_dot_prod_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + uint32_t blockSize, + float16_t * result) +{ + f16x8_t vecA, vecB; + f16x8_t vecSum; + uint32_t blkCnt; + float16_t sum = 0.0f; + vecSum = vdupq_n_f16(0.0f); + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + while (blkCnt > 0U) + { + /* + * C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] + * Calculate dot product and then store the result in a temporary buffer. + * and advance vector source and destination pointers + */ + vecA = vld1q(pSrcA); + pSrcA += 8; + + vecB = vld1q(pSrcB); + pSrcB += 8; + + vecSum = vfmaq(vecSum, vecA, vecB); + /* + * Decrement the blockSize loop counter + */ + blkCnt --; + } + + + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + mve_pred16_t p0 = vctp16q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vecSum = vfmaq_m(vecSum, vecA, vecB, p0); + } + + sum = vecAddAcrossF16Mve(vecSum); + + /* Store result in destination buffer */ + *result = sum; + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_dot_prod_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + uint32_t blockSize, + float16_t * result) +{ + uint32_t blkCnt; /* Loop counter */ + _Float16 sum = 0.0f; /* Temporary return variable */ + + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + /* First part of the processing with loop unrolling. Compute 4 outputs at a time. + ** a second loop below computes the remaining 1 to 3 samples. */ + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + sum += (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + sum += (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + sum += (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store result in destination buffer */ + *result = sum; +} +#endif +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ +/** + @} end of BasicDotProd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f32.c new file mode 100644 index 0000000..62b8657 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f32.c @@ -0,0 +1,228 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_dot_prod_f32.c + * Description: Floating-point dot product + * + * $Date: 05 October 2021 + * $Revision: V1.9.1 + * + * 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicDotProd Vector Dot Product + + Computes the dot product of two vectors. + The vectors are multiplied element-by-element and then summed. + + <pre> + sum = pSrcA[0]*pSrcB[0] + pSrcA[1]*pSrcB[1] + ... + pSrcA[blockSize-1]*pSrcB[blockSize-1] + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicDotProd + @{ + */ + +/** + @brief Dot product of floating-point vectors. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[in] blockSize number of samples in each vector. + @param[out] result output result returned here. + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + + +void arm_dot_prod_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + uint32_t blockSize, + float32_t * result) +{ + f32x4_t vecA, vecB; + f32x4_t vecSum; + uint32_t blkCnt; + float32_t sum = 0.0f; + vecSum = vdupq_n_f32(0.0f); + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + while (blkCnt > 0U) + { + /* + * C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] + * Calculate dot product and then store the result in a temporary buffer. + * and advance vector source and destination pointers + */ + vecA = vld1q(pSrcA); + pSrcA += 4; + + vecB = vld1q(pSrcB); + pSrcB += 4; + + vecSum = vfmaq(vecSum, vecA, vecB); + /* + * Decrement the blockSize loop counter + */ + blkCnt --; + } + + + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + mve_pred16_t p0 = vctp32q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vecSum = vfmaq_m(vecSum, vecA, vecB, p0); + } + + sum = vecAddAcrossF32Mve(vecSum); + + /* Store result in destination buffer */ + *result = sum; + +} + +#else + +void arm_dot_prod_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + uint32_t blockSize, + float32_t * result) +{ + uint32_t blkCnt; /* Loop counter */ + float32_t sum = 0.0f; /* Temporary return variable */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t vec2; + f32x4_t accum = vdupq_n_f32(0); +#if !defined(__aarch64__) + f32x2_t tmp = vdup_n_f32(0); +#endif + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + vec1 = vld1q_f32(pSrcA); + vec2 = vld1q_f32(pSrcB); + + while (blkCnt > 0U) + { + /* C = A[0]*B[0] + A[1]*B[1] + A[2]*B[2] + ... + A[blockSize-1]*B[blockSize-1] */ + /* Calculate dot product and then store the result in a temporary buffer. */ + + accum = vmlaq_f32(accum, vec1, vec2); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + + vec1 = vld1q_f32(pSrcA); + vec2 = vld1q_f32(pSrcB); + + /* Decrement the loop counter */ + blkCnt--; + } + +#if defined(__aarch64__) + sum = vpadds_f32(vpadd_f32(vget_low_f32(accum), vget_high_f32(accum))); +#else + tmp = vpadd_f32(vget_low_f32(accum), vget_high_f32(accum)); + sum = vget_lane_f32(tmp, 0) + vget_lane_f32(tmp, 1); + +#endif + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + /* First part of the processing with loop unrolling. Compute 4 outputs at a time. + ** a second loop below computes the remaining 1 to 3 samples. */ + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += (*pSrcA++) * (*pSrcB++); + + sum += (*pSrcA++) * (*pSrcB++); + + sum += (*pSrcA++) * (*pSrcB++); + + sum += (*pSrcA++) * (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += (*pSrcA++) * (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store result in destination buffer */ + *result = sum; +} + +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ +/** + @} end of BasicDotProd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f64.c new file mode 100644 index 0000000..a4cd07c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f64.c @@ -0,0 +1,78 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_dot_prod_f64.c + * Description: Floating-point dot product + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicDotProd + @{ + */ + +/** + @brief Dot product of floating-point vectors. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[in] blockSize number of samples in each vector. + @param[out] result output result returned here. + @return none + */ + +void arm_dot_prod_f64( + const float64_t * pSrcA, + const float64_t * pSrcB, + uint32_t blockSize, + float64_t * result) +{ + uint32_t blkCnt; /* Loop counter */ + float64_t sum = 0.; /* Temporary return variable */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += (*pSrcA++) * (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store result in destination buffer */ + *result = sum; +} + +/** + @} end of BasicDotProd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q15.c new file mode 100644 index 0000000..cb42609 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q15.c @@ -0,0 +1,172 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_dot_prod_q15.c + * Description: Q15 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicDotProd + @{ + */ + +/** + @brief Dot product of Q15 vectors. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[in] blockSize number of samples in each vector + @param[out] result output result returned here + @return none + + @par Scaling and Overflow Behavior + The intermediate multiplications are in 1.15 x 1.15 = 2.30 format and these + results are added to a 64-bit accumulator in 34.30 format. + Nonsaturating additions are used and given that there are 33 guard bits in the accumulator + there is no risk of overflow. + The return result is in 34.30 format. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_dot_prod_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + uint32_t blockSize, + q63_t * result) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecA; + q15x8_t vecB; + q63_t sum = 0LL; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] + * Calculate dot product and then store the result in a temporary buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + sum = vmlaldavaq(sum, vecA, vecB); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + sum = vmlaldavaq_p(sum, vecA, vecB, p0); + } + + *result = sum; +} + +#else +void arm_dot_prod_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + uint32_t blockSize, + q63_t * result) +{ + uint32_t blkCnt; /* Loop counter */ + q63_t sum = 0; /* Temporary return variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + +#if defined (ARM_MATH_DSP) + /* Calculate dot product and store result in a temporary buffer. */ + sum = __SMLALD(read_q15x2_ia (&pSrcA), read_q15x2_ia (&pSrcB), sum); + sum = __SMLALD(read_q15x2_ia (&pSrcA), read_q15x2_ia (&pSrcB), sum); +#else + sum += (q63_t)((q31_t) *pSrcA++ * *pSrcB++); + sum += (q63_t)((q31_t) *pSrcA++ * *pSrcB++); + sum += (q63_t)((q31_t) *pSrcA++ * *pSrcB++); + sum += (q63_t)((q31_t) *pSrcA++ * *pSrcB++); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ +//#if defined (ARM_MATH_DSP) +// sum = __SMLALD(*pSrcA++, *pSrcB++, sum); +//#else + sum += (q63_t)((q31_t) *pSrcA++ * *pSrcB++); +//#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store result in destination buffer in 34.30 format */ + *result = sum; +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicDotProd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q31.c new file mode 100644 index 0000000..b85da37 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q31.c @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_dot_prod_q31.c + * Description: Q31 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicDotProd + @{ + */ + +/** + @brief Dot product of Q31 vectors. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[in] blockSize number of samples in each vector. + @param[out] result output result returned here. + @return none + + @par Scaling and Overflow Behavior + The intermediate multiplications are in 1.31 x 1.31 = 2.62 format and these + are truncated to 2.48 format by discarding the lower 14 bits. + The 2.48 result is then added without saturation to a 64-bit accumulator in 16.48 format. + There are 15 guard bits in the accumulator and there is no risk of overflow as long as + the length of the vectors is less than 2^16 elements. + The return result is in 16.48 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_dot_prod_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + uint32_t blockSize, + q63_t * result) +{ + uint32_t blkCnt; /* loop counters */ + q31x4_t vecA; + q31x4_t vecB; + q63_t sum = 0LL; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] + * Calculate dot product and then store the result in a temporary buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + sum = vrmlaldavhaq(sum, vecA, vecB); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + sum = vrmlaldavhaq_p(sum, vecA, vecB, p0); + } + + /* + * vrmlaldavhaq provides extra intermediate accumulator headroom. + * limiting the need of intermediate scaling + * Scalar variant uses 2.48 accu format by right shifting accumulators by 14. + * 16.48 output conversion is performed outside the loop by scaling accu. by 6 + */ + *result = asrl(sum, (14 - 8)); +} + +#else +void arm_dot_prod_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + uint32_t blockSize, + q63_t * result) +{ + uint32_t blkCnt; /* Loop counter */ + q63_t sum = 0; /* Temporary return variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += ((q63_t) *pSrcA++ * *pSrcB++) >> 14U; + + sum += ((q63_t) *pSrcA++ * *pSrcB++) >> 14U; + + sum += ((q63_t) *pSrcA++ * *pSrcB++) >> 14U; + + sum += ((q63_t) *pSrcA++ * *pSrcB++) >> 14U; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ + sum += ((q63_t) *pSrcA++ * *pSrcB++) >> 14U; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store result in destination buffer in 16.48 format */ + *result = sum; +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicDotProd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q7.c new file mode 100644 index 0000000..2de4e27 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_q7.c @@ -0,0 +1,191 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_dot_prod_q7.c + * Description: Q7 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicDotProd + @{ + */ + +/** + @brief Dot product of Q7 vectors. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[in] blockSize number of samples in each vector + @param[out] result output result returned here + @return none + + @par Scaling and Overflow Behavior + The intermediate multiplications are in 1.7 x 1.7 = 2.14 format and these + results are added to an accumulator in 18.14 format. + Nonsaturating additions are used and there is no danger of wrap around as long as + the vectors are less than 2^18 elements long. + The return result is in 18.14 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_dot_prod_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + uint32_t blockSize, + q31_t * result) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecA; + q7x16_t vecB; + q31_t sum = 0; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] + * Calculate dot product and then store the result in a temporary buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + sum = vmladavaq(sum, vecA, vecB); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 16; + pSrcB += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + sum = vmladavaq_p(sum, vecA, vecB, p0); + } + + *result = sum; +} +#else +void arm_dot_prod_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + uint32_t blockSize, + q31_t * result) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t sum = 0; /* Temporary return variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t input1, input2; /* Temporary variables */ + q31_t inA1, inA2, inB1, inB2; /* Temporary variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + +#if defined (ARM_MATH_DSP) + /* read 4 samples at a time from sourceA */ + input1 = read_q7x4_ia (&pSrcA); + /* read 4 samples at a time from sourceB */ + input2 = read_q7x4_ia (&pSrcB); + + /* extract two q7_t samples to q15_t samples */ + inA1 = __SXTB16(__ROR(input1, 8)); + /* extract reminaing two samples */ + inA2 = __SXTB16(input1); + /* extract two q7_t samples to q15_t samples */ + inB1 = __SXTB16(__ROR(input2, 8)); + /* extract reminaing two samples */ + inB2 = __SXTB16(input2); + + /* multiply and accumulate two samples at a time */ + sum = __SMLAD(inA1, inB1, sum); + sum = __SMLAD(inA2, inB2, sum); +#else + sum += (q31_t) ((q15_t) *pSrcA++ * *pSrcB++); + sum += (q31_t) ((q15_t) *pSrcA++ * *pSrcB++); + sum += (q31_t) ((q15_t) *pSrcA++ * *pSrcB++); + sum += (q31_t) ((q15_t) *pSrcA++ * *pSrcB++); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A[0]* B[0] + A[1]* B[1] + A[2]* B[2] + .....+ A[blockSize-1]* B[blockSize-1] */ + + /* Calculate dot product and store result in a temporary buffer. */ +//#if defined (ARM_MATH_DSP) +// sum = __SMLAD(*pSrcA++, *pSrcB++, sum); +//#else + sum += (q31_t) ((q15_t) *pSrcA++ * *pSrcB++); +//#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Store result in destination buffer in 18.14 format */ + *result = sum; +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicDotProd group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f16.c new file mode 100644 index 0000000..c1115ac --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f16.c @@ -0,0 +1,171 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mult_f16.c + * Description: Floating-point vector 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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicMult Vector Multiplication + + Element-by-element multiplication of two vectors. + + <pre> + pDst[n] = pSrcA[n] * pSrcB[n], 0 <= n < blockSize. + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicMult + @{ + */ + +/** + @brief Floating-point vector multiplication. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize 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_mult_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f16x8_t vec1; + f16x8_t vec2; + f16x8_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + res = vmulq(vec1, vec2); + vst1q(pDst, res); + + /* Increment pointers */ + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + if (blkCnt > 0U) + { + /* C = A + B */ + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + vstrhq_p(pDst, vmulq(vec1,vec2), p0); + } + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_mult_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply inputs and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + *pDst++ = (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + *pDst++ = (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + *pDst++ = (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply input and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrcA++) * (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f32.c new file mode 100644 index 0000000..4df04e9 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f32.c @@ -0,0 +1,200 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mult_f32.c + * Description: Floating-point vector 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicMult Vector Multiplication + + Element-by-element multiplication of two vectors. + + <pre> + pDst[n] = pSrcA[n] * pSrcB[n], 0 <= n < blockSize. + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicMult + @{ + */ + +/** + @brief Floating-point vector multiplication. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize number of samples in each vector. + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_mult_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f32x4_t vec1; + f32x4_t vec2; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + res = vmulq(vec1, vec2); + vst1q(pDst, res); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + if (blkCnt > 0U) + { + /* C = A + B */ + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + vstrwq_p(pDst, vmulq(vec1,vec2), p0); + } + +} + +#else +void arm_mult_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t vec2; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply the inputs and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrcA); + vec2 = vld1q_f32(pSrcB); + res = vmulq_f32(vec1, vec2); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply inputs and store result in destination buffer. */ + *pDst++ = (*pSrcA++) * (*pSrcB++); + + *pDst++ = (*pSrcA++) * (*pSrcB++); + + *pDst++ = (*pSrcA++) * (*pSrcB++); + + *pDst++ = (*pSrcA++) * (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply input and store result in destination buffer. */ + *pDst++ = (*pSrcA++) * (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f64.c new file mode 100644 index 0000000..1a5afd9 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_f64.c @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mult_f64.c + * Description: Floating-point vector 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicMult + @{ + */ + +/** + @brief Floating-point vector multiplication. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize number of samples in each vector. + @return none + */ + +void arm_mult_f64( + const float64_t * pSrcA, + const float64_t * pSrcB, + float64_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply input and store result in destination buffer. */ + *pDst++ = (*pSrcA++) * (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q15.c new file mode 100644 index 0000000..17951c7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q15.c @@ -0,0 +1,192 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mult_q15.c + * Description: Q15 vector 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicMult + @{ + */ + +/** + @brief Q15 vector 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] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_mult_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecA, vecB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = A * B + * Multiply the inputs and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqdmulhq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrhq_p(pDst, vqdmulhq(vecA, vecB), p0); + } +} + +#else +void arm_mult_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t inA1, inA2, inB1, inB2; /* Temporary input variables */ + q15_t out1, out2, out3, out4; /* Temporary output variables */ + q31_t mul1, mul2, mul3, mul4; /* Temporary variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * B */ + +#if defined (ARM_MATH_DSP) + /* read 2 samples at a time from sourceA */ + inA1 = read_q15x2_ia (&pSrcA); + /* read 2 samples at a time from sourceB */ + inB1 = read_q15x2_ia (&pSrcB); + /* read 2 samples at a time from sourceA */ + inA2 = read_q15x2_ia (&pSrcA); + /* read 2 samples at a time from sourceB */ + inB2 = read_q15x2_ia (&pSrcB); + + /* multiply mul = sourceA * sourceB */ + mul1 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16)); + mul2 = (q31_t) ((q15_t) (inA1 ) * (q15_t) (inB1 )); + mul3 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB2 >> 16)); + mul4 = (q31_t) ((q15_t) (inA2 ) * (q15_t) (inB2 )); + + /* saturate result to 16 bit */ + out1 = (q15_t) __SSAT(mul1 >> 15, 16); + out2 = (q15_t) __SSAT(mul2 >> 15, 16); + out3 = (q15_t) __SSAT(mul3 >> 15, 16); + out4 = (q15_t) __SSAT(mul4 >> 15, 16); + + /* store result to destination */ +#ifndef ARM_MATH_BIG_ENDIAN + write_q15x2_ia (&pDst, __PKHBT(out2, out1, 16)); + write_q15x2_ia (&pDst, __PKHBT(out4, out3, 16)); +#else + write_q15x2_ia (&pDst, __PKHBT(out1, out2, 16)); + write_q15x2_ia (&pDst, __PKHBT(out3, out4, 16)); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + +#else + *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16); + *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16); + *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16); + *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply inputs and store result in destination buffer. */ + *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q31.c new file mode 100644 index 0000000..63ad73c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q31.c @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mult_q31.c + * Description: Q31 vector 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicMult + @{ + */ + +/** + @brief Q31 vector multiplication. + @param[in] pSrcA points to the first input vector. + @param[in] pSrcB points to the second input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_mult_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q31x4_t vecA, vecB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A * B + * Multiply the inputs and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqdmulhq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrwq_p(pDst, vqdmulhq(vecA, vecB), p0); + } +} + +#else +void arm_mult_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t out; /* Temporary output variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply inputs and store result in destination buffer. */ + out = ((q63_t) *pSrcA++ * *pSrcB++) >> 32; + out = __SSAT(out, 31); + *pDst++ = out << 1U; + + out = ((q63_t) *pSrcA++ * *pSrcB++) >> 32; + out = __SSAT(out, 31); + *pDst++ = out << 1U; + + out = ((q63_t) *pSrcA++ * *pSrcB++) >> 32; + out = __SSAT(out, 31); + *pDst++ = out << 1U; + + out = ((q63_t) *pSrcA++ * *pSrcB++) >> 32; + out = __SSAT(out, 31); + *pDst++ = out << 1U; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply inputs and store result in destination buffer. */ + out = ((q63_t) *pSrcA++ * *pSrcB++) >> 32; + out = __SSAT(out, 31); + *pDst++ = out << 1U; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q7.c new file mode 100644 index 0000000..7be80db --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_mult_q7.c @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mult_q7.c + * Description: Q7 vector 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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicMult + @{ + */ + +/** + @brief Q7 vector multiplication + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q7 range [0x80 0x7F] are saturated. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_mult_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecA, vecB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = A * B + * Multiply the inputs and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqdmulhq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 16; + pSrcB += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrbq_p(pDst, vqdmulhq(vecA, vecB), p0); + } +} + +#else +void arm_mult_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q7_t out1, out2, out3, out4; /* Temporary output variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * B */ + +#if defined (ARM_MATH_DSP) + /* Multiply inputs and store results in temporary variables */ + out1 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + out2 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + out3 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + out4 = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + + /* Pack and store result in destination buffer (in single write) */ + write_q7x4_ia (&pDst, __PACKq7(out1, out2, out3, out4)); +#else + *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * B */ + + /* Multiply input and store result in destination buffer. */ + *pDst++ = (q7_t) __SSAT((((q15_t) (*pSrcA++) * (*pSrcB++)) >> 7), 8); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicMult group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f16.c new file mode 100644 index 0000000..187e511 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f16.c @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_negate_f16.c + * Description: Negates floating-point vectors + * + * $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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicNegate Vector Negate + + Negates the elements of a vector. + + <pre> + pDst[n] = -pSrc[n], 0 <= n < blockSize. + </pre> + + The functions support in-place computation allowing the source and + destination pointers to reference the same memory buffer. + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicNegate + @{ + */ + +/** + @brief Negates the elements of a floating-point vector. + @param[in] pSrc points to input vector. + @param[out] pDst points to output vector. + @param[in] blockSize 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_negate_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + f16x8_t vec1; + f16x8_t res; + + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute values and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vnegq(vec1); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + if (blkCnt > 0U) + { + /* C = |A| */ + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q((float16_t const *) pSrc); + vstrhq_p(pDst, vnegq(vec1), p0); + } + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_negate_f16( + const float16_t * pSrc, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + *pDst++ = -(_Float16)*pSrc++; + + *pDst++ = -(_Float16)*pSrc++; + + *pDst++ = -(_Float16)*pSrc++; + + *pDst++ = -(_Float16)*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + *pDst++ = -(_Float16)*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicNegate group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f32.c new file mode 100644 index 0000000..4f243cd --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f32.c @@ -0,0 +1,192 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_negate_f32.c + * Description: Negates floating-point vectors + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicNegate Vector Negate + + Negates the elements of a vector. + + <pre> + pDst[n] = -pSrc[n], 0 <= n < blockSize. + </pre> + + The functions support in-place computation allowing the source and + destination pointers to reference the same memory buffer. + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicNegate + @{ + */ + +/** + @brief Negates the elements of a floating-point vector. + @param[in] pSrc points to input vector. + @param[out] pDst points to output vector. + @param[in] blockSize number of samples in each vector. + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_negate_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + f32x4_t vec1; + f32x4_t res; + + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + while (blkCnt > 0U) + { + /* C = |A| */ + + /* Calculate absolute values and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vnegq(vec1); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + if (blkCnt > 0U) + { + /* C = |A| */ + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q((float32_t const *) pSrc); + vstrwq_p(pDst, vnegq(vec1), p0); + } + +} + +#else +void arm_negate_f32( + const float32_t * pSrc, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON_EXPERIMENTAL) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrc); + res = vnegq_f32(vec1); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + *pDst++ = -*pSrc++; + + *pDst++ = -*pSrc++; + + *pDst++ = -*pSrc++; + + *pDst++ = -*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON_EXPERIMENTAL) */ + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + *pDst++ = -*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicNegate group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f64.c new file mode 100644 index 0000000..c8a7341 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_f64.c @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_negate_f64.c + * Description: Negates floating-point vectors + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicNegate + @{ + */ + +/** + @brief Negates the elements of a floating-point vector. + @param[in] pSrc points to input vector. + @param[out] pDst points to output vector. + @param[in] blockSize number of samples in each vector. + @return none + */ + +void arm_negate_f64( + const float64_t * pSrc, + float64_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + *pDst++ = -*pSrc++; + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicNegate group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q15.c new file mode 100644 index 0000000..43d5b0d --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q15.c @@ -0,0 +1,171 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_negate_q15.c + * Description: Negates Q15 vectors + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicNegate + @{ + */ + +/** + @brief Negates the elements of a Q15 vector. + @param[in] pSrc points to the input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize number of samples in each vector. + @return none + + @par Conditions for optimum performance + Input and output buffers should be aligned by 32-bit + @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) + +#include "arm_helium_utils.h" + +void arm_negate_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecSrc; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = -A + * Negate and then store the results in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqnegq(vecSrc)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrc = vld1q(pSrc); + vstrhq_p(pDst, vqnegq(vecSrc), p0); + } +} + +#else +void arm_negate_q15( + const q15_t * pSrc, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q15_t in; /* Temporary input variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t in1; /* Temporary input variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = -A */ + +#if defined (ARM_MATH_DSP) + /* Negate and store result in destination buffer (2 samples at a time). */ + in1 = read_q15x2_ia (&pSrc); + write_q15x2_ia (&pDst, __QSUB16(0, in1)); + + in1 = read_q15x2_ia (&pSrc); + write_q15x2_ia (&pDst, __QSUB16(0, in1)); +#else + in = *pSrc++; + *pDst++ = (in == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in; + + in = *pSrc++; + *pDst++ = (in == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in; + + in = *pSrc++; + *pDst++ = (in == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in; + + in = *pSrc++; + *pDst++ = (in == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in; +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + in = *pSrc++; + *pDst++ = (in == (q15_t) 0x8000) ? (q15_t) 0x7fff : -in; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicNegate group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q31.c new file mode 100644 index 0000000..3ee77ed --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q31.c @@ -0,0 +1,178 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_negate_q31.c + * Description: Negates Q31 vectors + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicNegate + @{ + */ + +/** + @brief Negates the elements of a Q31 vector. + @param[in] pSrc points to the input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_negate_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q31x4_t vecSrc; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = -A + * Negate and then store the results in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqnegq(vecSrc)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrc = vld1q(pSrc); + vstrwq_p(pDst, vqnegq(vecSrc), p0); + } +} + +#else +void arm_negate_q31( + const q31_t * pSrc, + q31_t * pDst, + uint32_t blockSize) +{ + 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 = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + in = *pSrc++; +#if defined (ARM_MATH_DSP) + *pDst++ = __QSUB(0, in); +#else + *pDst++ = (in == INT32_MIN) ? INT32_MAX : -in; +#endif + + 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 = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + 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 BasicNegate group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q7.c new file mode 100644 index 0000000..9fd3122 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_negate_q7.c @@ -0,0 +1,171 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_negate_q7.c + * Description: Negates Q7 vectors + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicNegate + @{ + */ + +/** + @brief Negates the elements of a Q7 vector. + @param[in] pSrc points to the input vector. + @param[out] pDst points to the output vector. + @param[in] blockSize number of samples in each vector. + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + The Q7 value -1 (0x80) is saturated to the maximum allowable positive value 0x7F. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_negate_q7( + const q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecSrc; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = -A + * Negate and then store the results in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqnegq(vecSrc)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrc = vld1q(pSrc); + vstrbq_p(pDst, vqnegq(vecSrc), p0); + } +} + +#else +void arm_negate_q7( + const q7_t * pSrc, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q7_t in; /* Temporary input variable */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t in1; /* Temporary input variable */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = -A */ + +#if defined (ARM_MATH_DSP) + /* Negate and store result in destination buffer (4 samples at a time). */ + in1 = read_q7x4_ia (&pSrc); + write_q7x4_ia (&pDst, __QSUB8(0, in1)); +#else + in = *pSrc++; + *pDst++ = (in == (q7_t) 0x80) ? (q7_t) 0x7f : -in; + + in = *pSrc++; + *pDst++ = (in == (q7_t) 0x80) ? (q7_t) 0x7f : -in; + + in = *pSrc++; + *pDst++ = (in == (q7_t) 0x80) ? (q7_t) 0x7f : -in; + + in = *pSrc++; + *pDst++ = (in == (q7_t) 0x80) ? (q7_t) 0x7f : -in; +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = -A */ + + /* Negate and store result in destination buffer. */ + in = *pSrc++; + +#if defined (ARM_MATH_DSP) + *pDst++ = (q7_t) __QSUB8(0, in); +#else + *pDst++ = (in == (q7_t) 0x80) ? (q7_t) 0x7f : -in; +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicNegate group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u16.c new file mode 100644 index 0000000..e553d28 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u16.c @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_not_u16.c + * Description: uint16_t bitwise NOT + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup Not Vector bitwise NOT + + Compute the logical bitwise NOT. + + There are separate functions for uint32_t, uint16_t, and uint8_t data types. + */ + +/** + @addtogroup Not + @{ + */ + +/** + @brief Compute the logical bitwise NOT of a fixed-point vector. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_not_u16( + const uint16_t * pSrc, + uint16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecSrc; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + + while (blkCnt > 0U) + { + vecSrc = vld1q(pSrc); + + vst1q(pDst, vmvnq_u16(vecSrc) ); + + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrc = vld1q(pSrc); + vstrhq_p(pDst, vmvnq_u16(vecSrc), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t inV; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + inV = vld1q_u16(pSrc); + + vst1q_u16(pDst, vmvnq_u16(inV) ); + + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = ~(*pSrc++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Not group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u32.c new file mode 100644 index 0000000..f46284c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u32.c @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_not_u32.c + * Description: uint32_t bitwise NOT + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup Not + @{ + */ + +/** + @brief Compute the logical bitwise NOT of a fixed-point vector. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_not_u32( + const uint32_t * pSrc, + uint32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecSrc; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 2; + + while (blkCnt > 0U) + { + vecSrc = vld1q(pSrc); + + vst1q(pDst, vmvnq_u32(vecSrc) ); + + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrc = vld1q(pSrc); + vstrwq_p(pDst, vmvnq_u32(vecSrc), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t inV; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + inV = vld1q_u32(pSrc); + + vst1q_u32(pDst, vmvnq_u32(inV) ); + + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = ~(*pSrc++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Not group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u8.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u8.c new file mode 100644 index 0000000..e068283 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_not_u8.c @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_not_u8.c + * Description: uint8_t bitwise NOT + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup Not + @{ + */ + +/** + @brief Compute the logical bitwise NOT of a fixed-point vector. + @param[in] pSrc points to input vector + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_not_u8( + const uint8_t * pSrc, + uint8_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecSrc; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + + while (blkCnt > 0U) + { + vecSrc = vld1q(pSrc); + + vst1q(pDst, vmvnq_u8(vecSrc) ); + + pSrc += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrc = vld1q(pSrc); + vstrbq_p(pDst, vmvnq_u8(vecSrc), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t inV; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4U; + + while (blkCnt > 0U) + { + inV = vld1q_u8(pSrc); + + vst1q_u8(pDst, vmvnq_u8(inV) ); + + pSrc += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = ~(*pSrc++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Not group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f16.c new file mode 100644 index 0000000..9aeb1e7 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f16.c @@ -0,0 +1,170 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_offset_f16.c + * Description: Floating-point vector offset + * + * $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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicOffset Vector Offset + + Adds a constant offset to each element of a vector. + + <pre> + pDst[n] = pSrc[n] + offset, 0 <= n < blockSize. + </pre> + + The functions support in-place computation allowing the source and + destination pointers to reference the same memory buffer. + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicOffset + @{ + */ + +/** + @brief Adds a constant offset to a floating-point vector. + @param[in] pSrc points to the input vector + @param[in] offset is the offset to be added + @param[out] pDst points to the output vector + @param[in] blockSize 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_offset_f16( + const float16_t * pSrc, + float16_t offset, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f16x8_t vec1; + f16x8_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vaddq(vec1,offset); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q((float16_t const *) pSrc); + vstrhq_p(pDst, vaddq(vec1, offset), p0); + } + + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_offset_f16( + const float16_t * pSrc, + float16_t offset, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrc++) + (_Float16)offset; + + *pDst++ = (_Float16)(*pSrc++) + (_Float16)offset; + + *pDst++ = (_Float16)(*pSrc++) + (_Float16)offset; + + *pDst++ = (_Float16)(*pSrc++) + (_Float16)offset; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrc++) + (_Float16)offset; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicOffset group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f32.c new file mode 100644 index 0000000..a68df05 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f32.c @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_offset_f32.c + * Description: Floating-point vector offset + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicOffset Vector Offset + + Adds a constant offset to each element of a vector. + + <pre> + pDst[n] = pSrc[n] + offset, 0 <= n < blockSize. + </pre> + + The functions support in-place computation allowing the source and + destination pointers to reference the same memory buffer. + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicOffset + @{ + */ + +/** + @brief Adds a constant offset to a floating-point vector. + @param[in] pSrc points to the input vector + @param[in] offset is the offset to be added + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_offset_f32( + const float32_t * pSrc, + float32_t offset, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f32x4_t vec1; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vaddq(vec1,offset); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q((float32_t const *) pSrc); + vstrwq_p(pDst, vaddq(vec1, offset), p0); + } + + +} + +#else +void arm_offset_f32( + const float32_t * pSrc, + float32_t offset, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON_EXPERIMENTAL) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrc); + res = vaddq_f32(vec1,vdupq_n_f32(offset)); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = (*pSrc++) + offset; + + *pDst++ = (*pSrc++) + offset; + + *pDst++ = (*pSrc++) + offset; + + *pDst++ = (*pSrc++) + offset; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON_EXPERIMENTAL) */ + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = (*pSrc++) + offset; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicOffset group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f64.c new file mode 100644 index 0000000..0b39da4 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_f64.c @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_offset_f64.c + * Description: Floating-point vector offset + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicOffset + @{ + */ + +/** + @brief Adds a constant offset to a floating-point vector. + @param[in] pSrc points to the input vector + @param[in] offset is the offset to be added + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_offset_f64( + const float64_t * pSrc, + float64_t offset, + float64_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = (*pSrc++) + offset; + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicOffset group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q15.c new file mode 100644 index 0000000..211776d --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q15.c @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_offset_q15.c + * Description: Q15 vector offset + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicOffset + @{ + */ + +/** + @brief Adds a constant offset to a Q15 vector. + @param[in] pSrc points to the input vector + @param[in] offset is the offset to be added + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_offset_q15( + const q15_t * pSrc, + q15_t offset, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecSrc; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = A + offset + * Add offset and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqaddq(vecSrc, offset)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrc = vld1q(pSrc); + vstrhq_p(pDst, vqaddq(vecSrc, offset), p0); + } +} + + +#else +void arm_offset_q15( + const q15_t * pSrc, + q15_t offset, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t offset_packed; /* Offset packed to 32 bit */ + + /* Offset is packed to 32 bit in order to use SIMD32 for addition */ + offset_packed = __PKHBT(offset, offset, 16); +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + +#if defined (ARM_MATH_DSP) + /* Add offset and store result in destination buffer (2 samples at a time). */ + write_q15x2_ia (&pDst, __QADD16(read_q15x2_ia (&pSrc), offset_packed)); + write_q15x2_ia (&pDst, __QADD16(read_q15x2_ia (&pSrc), offset_packed)); +#else + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrc++ + offset), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrc++ + offset), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrc++ + offset), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrc++ + offset), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + *pDst++ = (q15_t) __QADD16(*pSrc++, offset); +#else + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrc++ + offset), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicOffset group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q31.c new file mode 100644 index 0000000..2f702b0 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q31.c @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_offset_q31.c + * Description: Q31 vector offset + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicOffset + @{ + */ + +/** + @brief Adds a constant offset to a Q31 vector. + @param[in] pSrc points to the input vector + @param[in] offset is the offset to be added + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_offset_q31( + const q31_t * pSrc, + q31_t offset, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q31x4_t vecSrc; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A + offset + * Add offset and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqaddq(vecSrc, offset)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrc = vld1q(pSrc); + vstrwq_p(pDst, vqaddq(vecSrc, offset), p0); + } +} + +#else +void arm_offset_q31( + const q31_t * pSrc, + q31_t offset, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = __QADD(*pSrc++, offset); + + *pDst++ = __QADD(*pSrc++, offset); + + *pDst++ = __QADD(*pSrc++, offset); + + *pDst++ = __QADD(*pSrc++, offset); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + *pDst++ = __QADD(*pSrc++, offset); +#else + *pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicOffset group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q7.c new file mode 100644 index 0000000..b53229d --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_offset_q7.c @@ -0,0 +1,162 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_offset_q7.c + * Description: Q7 vector offset + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicOffset + @{ + */ + +/** + @brief Adds a constant offset to a Q7 vector. + @param[in] pSrc points to the input vector + @param[in] offset is the offset to be added + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q7 range [0x80 0x7F] are saturated. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_offset_q7( + const q7_t * pSrc, + q7_t offset, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecSrc; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = A + offset + * Add offset and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vst1q(pDst, vqaddq(vecSrc, offset)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrc = vld1q(pSrc); + vstrbq_p(pDst, vqaddq(vecSrc, offset), p0); + } +} + +#else +void arm_offset_q7( + const q7_t * pSrc, + q7_t offset, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t offset_packed; /* Offset packed to 32 bit */ + + /* Offset is packed to 32 bit in order to use SIMD32 for addition */ + offset_packed = __PACKq7(offset, offset, offset, offset); +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + +#if defined (ARM_MATH_DSP) + /* Add offset and store result in destination buffer (4 samples at a time). */ + write_q7x4_ia (&pDst, __QADD8(read_q7x4_ia (&pSrc), offset_packed)); +#else + *pDst++ = (q7_t) __SSAT((q15_t) *pSrc++ + offset, 8); + *pDst++ = (q7_t) __SSAT((q15_t) *pSrc++ + offset, 8); + *pDst++ = (q7_t) __SSAT((q15_t) *pSrc++ + offset, 8); + *pDst++ = (q7_t) __SSAT((q15_t) *pSrc++ + offset, 8); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and store result in destination buffer. */ + *pDst++ = (q7_t) __SSAT((q15_t) *pSrc++ + offset, 8); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicOffset group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u16.c new file mode 100644 index 0000000..92d2803 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u16.c @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_or_u16.c + * Description: uint16_t bitwise inclusive OR + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup Or Vector bitwise inclusive OR + + Compute the logical bitwise OR. + + There are separate functions for uint32_t, uint16_t, and uint8_t data types. + */ + +/** + @addtogroup Or + @{ + */ + +/** + @brief Compute the logical bitwise OR of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_or_u16( + const uint16_t * pSrcA, + const uint16_t * pSrcB, + uint16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecSrcA, vecSrcB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, vorrq_u16(vecSrcA, vecSrcB) ); + + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrhq_p(pDst, vorrq_u16(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecA, vecB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + vecA = vld1q_u16(pSrcA); + vecB = vld1q_u16(pSrcB); + + vst1q_u16(pDst, vorrq_u16(vecA, vecB) ); + + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)|(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Or group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u32.c new file mode 100644 index 0000000..14cc83b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u32.c @@ -0,0 +1,128 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_or_u32.c + * Description: uint32_t bitwise inclusive OR + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup Or + @{ + */ + +/** + @brief Compute the logical bitwise OR of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_or_u32( + const uint32_t * pSrcA, + const uint32_t * pSrcB, + uint32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecSrcA, vecSrcB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, vorrq_u32(vecSrcA, vecSrcB) ); + + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrwq_p(pDst, vorrq_u32(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecA, vecB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + vecA = vld1q_u32(pSrcA); + vecB = vld1q_u32(pSrcB); + + vst1q_u32(pDst, vorrq_u32(vecA, vecB) ); + + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)|(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} +/** + @} end of Or group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u8.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u8.c new file mode 100644 index 0000000..beee07b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_or_u8.c @@ -0,0 +1,128 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_or_u8.c + * Description: uint8_t bitwise inclusive OR + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup Or + @{ + */ + +/** + @brief Compute the logical bitwise OR of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_or_u8( + const uint8_t * pSrcA, + const uint8_t * pSrcB, + uint8_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecSrcA, vecSrcB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, vorrq_u8(vecSrcA, vecSrcB) ); + + pSrcA += 16; + pSrcB += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrbq_p(pDst, vorrq_u8(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecA, vecB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4U; + + while (blkCnt > 0U) + { + vecA = vld1q_u8(pSrcA); + vecB = vld1q_u8(pSrcB); + + vst1q_u8(pDst, vorrq_u8(vecA, vecB) ); + + pSrcA += 16; + pSrcB += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)|(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} +/** + @} end of Or group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f16.c new file mode 100644 index 0000000..b4b1c33 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f16.c @@ -0,0 +1,183 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_scale_f16.c + * Description: Multiplies a floating-point vector by a scalar + * + * $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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicScale Vector Scale + + Multiply a vector by a scalar value. For floating-point data, the algorithm used is: + + <pre> + pDst[n] = pSrc[n] * scale, 0 <= n < blockSize. + </pre> + + In the fixed-point Q7, Q15, and Q31 functions, <code>scale</code> is represented by + a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>. + The shift allows the gain of the scaling operation to exceed 1.0. + The algorithm used with fixed-point data is: + + <pre> + pDst[n] = (pSrc[n] * scaleFract) << shift, 0 <= n < blockSize. + </pre> + + The overall scale factor applied to the fixed-point data is + <pre> + scale = scaleFract * 2^shift. + </pre> + + The functions support in-place computation allowing the source and destination + pointers to reference the same memory buffer. + */ + +/** + @addtogroup BasicScale + @{ + */ + +/** + @brief Multiplies a floating-point vector by a scalar. + @param[in] pSrc points to the input vector + @param[in] scale scale factor to be applied + @param[out] pDst points to the output vector + @param[in] blockSize 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_scale_f16( + const float16_t * pSrc, + float16_t scale, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f16x8_t vec1; + f16x8_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vmulq(vec1,scale); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q((float16_t const *) pSrc); + vstrhq_p(pDst, vmulq(vec1, scale), p0); + } + + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_scale_f16( + const float16_t *pSrc, + float16_t scale, + float16_t *pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrc++) * (_Float16)scale; + + *pDst++ = (_Float16)(*pSrc++) * (_Float16)scale; + + *pDst++ = (_Float16)(*pSrc++) * (_Float16)scale; + + *pDst++ = (_Float16)(*pSrc++) * (_Float16)scale; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrc++) * (_Float16)scale; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicScale group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f32.c new file mode 100644 index 0000000..59bc813 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f32.c @@ -0,0 +1,216 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_scale_f32.c + * Description: Multiplies a floating-point vector by a scalar + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicScale Vector Scale + + Multiply a vector by a scalar value. For floating-point data, the algorithm used is: + + <pre> + pDst[n] = pSrc[n] * scale, 0 <= n < blockSize. + </pre> + + In the fixed-point Q7, Q15, and Q31 functions, <code>scale</code> is represented by + a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>. + The shift allows the gain of the scaling operation to exceed 1.0. + The algorithm used with fixed-point data is: + + <pre> + pDst[n] = (pSrc[n] * scaleFract) << shift, 0 <= n < blockSize. + </pre> + + The overall scale factor applied to the fixed-point data is + <pre> + scale = scaleFract * 2^shift. + </pre> + + The functions support in-place computation allowing the source and destination + pointers to reference the same memory buffer. + */ + +/** + @addtogroup BasicScale + @{ + */ + +/** + @brief Multiplies a floating-point vector by a scalar. + @param[in] pSrc points to the input vector + @param[in] scale scale factor to be applied + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_scale_f32( + const float32_t * pSrc, + float32_t scale, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f32x4_t vec1; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + offset */ + + /* Add offset and then store the results in the destination buffer. */ + vec1 = vld1q(pSrc); + res = vmulq(vec1,scale); + vst1q(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q((float32_t const *) pSrc); + vstrwq_p(pDst, vmulq(vec1, scale), p0); + } + + +} + +#else +void arm_scale_f32( + const float32_t *pSrc, + float32_t scale, + float32_t *pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ +#if defined(ARM_MATH_NEON_EXPERIMENTAL) + f32x4_t vec1; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale the input and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrc); + res = vmulq_f32(vec1, vdupq_n_f32(scale)); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrc += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + float32_t in1, in2, in3, in4; + + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + in1 = (*pSrc++) * scale; + + in2 = (*pSrc++) * scale; + + in3 = (*pSrc++) * scale; + + in4 = (*pSrc++) * scale; + + *pDst++ = in1; + *pDst++ = in2; + *pDst++ = in3; + *pDst++ = in4; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON_EXPERIMENTAL) */ + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + *pDst++ = (*pSrc++) * scale; + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicScale group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f64.c new file mode 100644 index 0000000..cb2452b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f64.c @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_scale_f64.c + * Description: Multiplies a floating-point vector by a scalar + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicScale + @{ + */ + +/** + @brief Multiplies a floating-point vector by a scalar. + @param[in] pSrc points to the input vector + @param[in] scale scale factor to be applied + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_scale_f64( + const float64_t *pSrc, + float64_t scale, + float64_t *pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + *pDst++ = (*pSrc++) * scale; + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicScale group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q15.c new file mode 100644 index 0000000..fbad607 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q15.c @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_scale_q15.c + * Description: Multiplies a Q15 vector by a scalar + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicScale + @{ + */ + +/** + @brief Multiplies a Q15 vector by a scalar. + @param[in] pSrc points to the input vector + @param[in] scaleFract fractional portion of the scale value + @param[in] shift number of bits to shift the result by + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format. + These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_scale_q15( + const q15_t * pSrc, + q15_t scaleFract, + int8_t shift, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecSrc; + q15x8_t vecDst; + + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + + while (blkCnt > 0U) + { + /* + * C = A * scale + * Scale the input and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vecDst = vmulhq(vecSrc, vdupq_n_s16(scaleFract)); + vecDst = vqshlq_r(vecDst, shift + 1); + vst1q(pDst, vecDst); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt);; + vecSrc = vld1q(pSrc); + vecDst = vmulhq(vecSrc, vdupq_n_s16(scaleFract)); + vecDst = vqshlq_r(vecDst, shift + 1); + vstrhq_p(pDst, vecDst, p0); + } + +} + + +#else +void arm_scale_q15( + const q15_t *pSrc, + q15_t scaleFract, + int8_t shift, + q15_t *pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + int8_t kShift = 15 - shift; /* Shift to apply after scaling */ + +#if defined (ARM_MATH_LOOPUNROLL) +#if defined (ARM_MATH_DSP) + q31_t inA1, inA2; + q31_t out1, out2, out3, out4; /* Temporary output variables */ + q15_t in1, in2, in3, in4; /* Temporary input variables */ +#endif +#endif + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * scale */ + +#if defined (ARM_MATH_DSP) + /* read 2 times 2 samples at a time from source */ + inA1 = read_q15x2_ia (&pSrc); + inA2 = read_q15x2_ia (&pSrc); + + /* Scale inputs and store result in temporary variables + * in single cycle by packing the outputs */ + out1 = (q31_t) ((q15_t) (inA1 >> 16) * scaleFract); + out2 = (q31_t) ((q15_t) (inA1 ) * scaleFract); + out3 = (q31_t) ((q15_t) (inA2 >> 16) * scaleFract); + out4 = (q31_t) ((q15_t) (inA2 ) * scaleFract); + + /* apply shifting */ + out1 = out1 >> kShift; + out2 = out2 >> kShift; + out3 = out3 >> kShift; + out4 = out4 >> kShift; + + /* saturate the output */ + in1 = (q15_t) (__SSAT(out1, 16)); + in2 = (q15_t) (__SSAT(out2, 16)); + in3 = (q15_t) (__SSAT(out3, 16)); + in4 = (q15_t) (__SSAT(out4, 16)); + + /* store result to destination */ + write_q15x2_ia (&pDst, __PKHBT(in2, in1, 16)); + write_q15x2_ia (&pDst, __PKHBT(in4, in3, 16)); +#else + *pDst++ = (q15_t) (__SSAT(((q31_t) *pSrc++ * scaleFract) >> kShift, 16)); + *pDst++ = (q15_t) (__SSAT(((q31_t) *pSrc++ * scaleFract) >> kShift, 16)); + *pDst++ = (q15_t) (__SSAT(((q31_t) *pSrc++ * scaleFract) >> kShift, 16)); + *pDst++ = (q15_t) (__SSAT(((q31_t) *pSrc++ * scaleFract) >> kShift, 16)); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + *pDst++ = (q15_t) (__SSAT(((q31_t) *pSrc++ * scaleFract) >> kShift, 16)); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicScale group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q31.c new file mode 100644 index 0000000..4e28441 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q31.c @@ -0,0 +1,244 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_scale_q31.c + * Description: Multiplies a Q31 vector by a scalar + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicScale + @{ + */ + +/** + @brief Multiplies a Q31 vector by a scalar. + @param[in] pSrc points to the input vector + @param[in] scaleFract fractional portion of the scale value + @param[in] shift number of bits to shift the result by + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format. + These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_scale_q31( + const q31_t * pSrc, + q31_t scaleFract, + int8_t shift, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q31x4_t vecSrc; + q31x4_t vecDst; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A * scale + * Scale the input and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vecDst = vmulhq(vecSrc, vdupq_n_s32(scaleFract)); + vecDst = vqshlq_r(vecDst, shift + 1); + vst1q(pDst, vecDst); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrc = vld1q(pSrc); + vecDst = vmulhq(vecSrc, vdupq_n_s32(scaleFract)); + vecDst = vqshlq_r(vecDst, shift + 1); + vstrwq_p(pDst, vecDst, p0); + } +} + +#else +void arm_scale_q31( + const q31_t *pSrc, + q31_t scaleFract, + int8_t shift, + q31_t *pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + q31_t in, out; /* Temporary variables */ + int8_t kShift = shift + 1; /* Shift to apply after scaling */ + int8_t sign = (kShift & 0x80); + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + in = *pSrc++; /* read input from source */ + in = ((q63_t) in * scaleFract) >> 32; /* multiply input with scaler value */ + out = in << kShift; /* apply shifting */ + if (in != (out >> kShift)) /* saturate the result */ + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; /* Store result destination */ + + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + in = *pSrc++; /* read four inputs from source */ + in = ((q63_t) in * scaleFract) >> 32; /* multiply input with scaler value */ + out = in >> -kShift; /* apply shifting */ + *pDst++ = out; /* Store result destination */ + + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in >> -kShift; + *pDst++ = out; + + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in >> -kShift; + *pDst++ = out; + + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in >> -kShift; + *pDst++ = out; + + /* Decrement loop counter */ + blkCnt--; + } + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + in = *pSrc++; + in = ((q63_t) in * scaleFract) >> 32; + out = in >> -kShift; + *pDst++ = out; + + /* Decrement loop counter */ + blkCnt--; + } + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicScale group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q7.c new file mode 100644 index 0000000..5bb4580 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_q7.c @@ -0,0 +1,186 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_scale_q7.c + * Description: Multiplies a Q7 vector by a scalar + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicScale + @{ + */ + +/** + @brief Multiplies a Q7 vector by a scalar. + @param[in] pSrc points to the input vector + @param[in] scaleFract fractional portion of the scale value + @param[in] shift number of bits to shift the result by + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.7 format. + These are multiplied to yield a 2.14 intermediate result and this is shifted with saturation to 1.7 format. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + + +void arm_scale_q7( + const q7_t * pSrc, + q7_t scaleFract, + int8_t shift, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecSrc; + q7x16_t vecDst; + + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + + while (blkCnt > 0U) + { + /* + * C = A * scale + * Scale the input and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vecDst = vmulhq(vecSrc, vdupq_n_s8(scaleFract)); + vecDst = vqshlq_r(vecDst, shift + 1); + vst1q(pDst, vecDst); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrc = vld1q(pSrc); + vecDst = vmulhq(vecSrc, vdupq_n_s8(scaleFract)); + vecDst = vqshlq_r(vecDst, shift + 1); + vstrbq_p(pDst, vecDst, p0); + } + +} + +#else +void arm_scale_q7( + const q7_t * pSrc, + q7_t scaleFract, + int8_t shift, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + int8_t kShift = 7 - shift; /* Shift to apply after scaling */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q7_t in1, in2, in3, in4; /* Temporary input variables */ + q7_t out1, out2, out3, out4; /* Temporary output variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A * scale */ + +#if defined (ARM_MATH_DSP) + /* Reading 4 inputs from memory */ + in1 = *pSrc++; + in2 = *pSrc++; + in3 = *pSrc++; + in4 = *pSrc++; + + /* Scale inputs and store result in the temporary variable. */ + out1 = (q7_t) (__SSAT(((in1) * scaleFract) >> kShift, 8)); + out2 = (q7_t) (__SSAT(((in2) * scaleFract) >> kShift, 8)); + out3 = (q7_t) (__SSAT(((in3) * scaleFract) >> kShift, 8)); + out4 = (q7_t) (__SSAT(((in4) * scaleFract) >> kShift, 8)); + + /* Pack and store result in destination buffer (in single write) */ + write_q7x4_ia (&pDst, __PACKq7(out1, out2, out3, out4)); +#else + *pDst++ = (q7_t) (__SSAT((((q15_t) *pSrc++ * scaleFract) >> kShift), 8)); + *pDst++ = (q7_t) (__SSAT((((q15_t) *pSrc++ * scaleFract) >> kShift), 8)); + *pDst++ = (q7_t) (__SSAT((((q15_t) *pSrc++ * scaleFract) >> kShift), 8)); + *pDst++ = (q7_t) (__SSAT((((q15_t) *pSrc++ * scaleFract) >> kShift), 8)); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A * scale */ + + /* Scale input and store result in destination buffer. */ + *pDst++ = (q7_t) (__SSAT((((q15_t) *pSrc++ * scaleFract) >> kShift), 8)); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicScale group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q15.c new file mode 100644 index 0000000..2de3b2b --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q15.c @@ -0,0 +1,251 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_shift_q15.c + * Description: Shifts the elements of a Q15 vector by a specified number of bits + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicShift + @{ + */ + +/** + @brief Shifts the elements of a Q15 vector a specified number of bits + @param[in] pSrc points to the input vector + @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_shift_q15( + const q15_t * pSrc, + int8_t shiftBits, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecSrc; + q15x8_t vecDst; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = A (>> or <<) shiftBits + * Shift the input and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vecDst = vqshlq_r(vecSrc, shiftBits); + vst1q(pDst, vecDst); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrc = vld1q(pSrc); + vecDst = vqshlq_r(vecSrc, shiftBits); + vstrhq_p(pDst, vecDst, p0); + } +} + +#else +void arm_shift_q15( + const q15_t * pSrc, + int8_t shiftBits, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q15_t in1, in2; /* Temporary input variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + /* If the shift value is positive then do right shift else left shift */ + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A << shiftBits */ + +#if defined (ARM_MATH_DSP) + /* read 2 samples from source */ + in1 = *pSrc++; + in2 = *pSrc++; + + /* Shift the inputs and then store the results in the destination buffer. */ +#ifndef ARM_MATH_BIG_ENDIAN + write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in1 << shiftBits), 16), + __SSAT(((q31_t) in2 << shiftBits), 16), 16)); +#else + write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in2 << shiftBits), 16), + __SSAT(((q31_t) in1 << shiftBits), 16), 16)); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* read 2 samples from source */ + in1 = *pSrc++; + in2 = *pSrc++; + +#ifndef ARM_MATH_BIG_ENDIAN + write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in1 << shiftBits), 16), + __SSAT(((q31_t) in2 << shiftBits), 16), 16)); +#else + write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in2 << shiftBits), 16), + __SSAT(((q31_t) in1 << shiftBits), 16), 16)); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + +#else + *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); + *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); + *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); + *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A >> shiftBits */ + +#if defined (ARM_MATH_DSP) + /* read 2 samples from source */ + in1 = *pSrc++; + in2 = *pSrc++; + + /* Shift the inputs and then store the results in the destination buffer. */ +#ifndef ARM_MATH_BIG_ENDIAN + write_q15x2_ia (&pDst, __PKHBT((in1 >> -shiftBits), + (in2 >> -shiftBits), 16)); +#else + write_q15x2_ia (&pDst, __PKHBT((in2 >> -shiftBits), + (in1 >> -shiftBits), 16)); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* read 2 samples from source */ + in1 = *pSrc++; + in2 = *pSrc++; + +#ifndef ARM_MATH_BIG_ENDIAN + write_q15x2_ia (&pDst, __PKHBT((in1 >> -shiftBits), + (in2 >> -shiftBits), 16)); +#else + write_q15x2_ia (&pDst, __PKHBT((in2 >> -shiftBits), + (in1 >> -shiftBits), 16)); +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + +#else + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + /* If the shift value is positive then do right shift else left shift */ + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A << shiftBits */ + + /* Shift input and store result in destination buffer. */ + *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A >> shiftBits */ + + /* Shift input and store result in destination buffer. */ + *pDst++ = (*pSrc++ >> -shiftBits); + + /* Decrement loop counter */ + blkCnt--; + } + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicShift group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q31.c new file mode 100644 index 0000000..5405c24 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q31.c @@ -0,0 +1,232 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_shift_q31.c + * Description: Shifts the elements of a Q31 vector by a specified number of bits + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ +/** + @defgroup BasicShift Vector Shift + + Shifts the elements of a fixed-point vector by a specified number of bits. + There are separate functions for Q7, Q15, and Q31 data types. + The underlying algorithm used is: + + <pre> + pDst[n] = pSrc[n] << shift, 0 <= n < blockSize. + </pre> + + If <code>shift</code> is positive then the elements of the vector are shifted to the left. + If <code>shift</code> is negative then the elements of the vector are shifted to the right. + + The functions support in-place computation allowing the source and destination + pointers to reference the same memory buffer. + */ + +/** + @addtogroup BasicShift + @{ + */ + +/** + @brief Shifts the elements of a Q31 vector a specified number of bits. + @param[in] pSrc points to the input vector + @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in the 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) + +#include "arm_helium_utils.h" + +void arm_shift_q31( + const q31_t * pSrc, + int8_t shiftBits, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q31x4_t vecSrc; + q31x4_t vecDst; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A (>> or <<) shiftBits + * Shift the input and then store the result in the destination buffer. + */ + vecSrc = vld1q((q31_t const *) pSrc); + vecDst = vqshlq_r(vecSrc, shiftBits); + vst1q(pDst, vecDst); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrc = vld1q((q31_t const *) pSrc); + vecDst = vqshlq_r(vecSrc, shiftBits); + vstrwq_p(pDst, vecDst, p0); + } +} + + +#else +void arm_shift_q31( + const q31_t * pSrc, + int8_t shiftBits, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */ + +#if defined (ARM_MATH_LOOPUNROLL) + + q31_t in, out; /* Temporary variables */ + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + /* If the shift value is positive then do right shift else left shift */ + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A << shiftBits */ + + /* Shift input and store result in destination buffer. */ + in = *pSrc++; + out = in << shiftBits; + if (in != (out >> shiftBits)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + in = *pSrc++; + out = in << shiftBits; + if (in != (out >> shiftBits)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + in = *pSrc++; + out = in << shiftBits; + if (in != (out >> shiftBits)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + in = *pSrc++; + out = in << shiftBits; + if (in != (out >> shiftBits)) + out = 0x7FFFFFFF ^ (in >> 31); + *pDst++ = out; + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A >> shiftBits */ + + /* Shift input and store results in destination buffer. */ + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + + /* Decrement loop counter */ + blkCnt--; + } + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + /* If the shift value is positive then do right shift else left shift */ + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A << shiftBits */ + + /* Shift input and store result in destination buffer. */ + *pDst++ = clip_q63_to_q31((q63_t) *pSrc++ << shiftBits); + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A >> shiftBits */ + + /* Shift input and store result in destination buffer. */ + *pDst++ = (*pSrc++ >> -shiftBits); + + /* Decrement loop counter */ + blkCnt--; + } + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicShift group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q7.c new file mode 100644 index 0000000..4f83edc --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_shift_q7.c @@ -0,0 +1,225 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_shift_q7.c + * Description: Processing function for the Q7 Shifting + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicShift + @{ + */ + +/** + @brief Shifts the elements of a Q7 vector a specified number of bits + @param[in] pSrc points to the input vector + @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par onditions for optimum performance + Input and output buffers should be aligned by 32-bit + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q7 range [0x80 0x7F] are saturated. + */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_shift_q7( + const q7_t * pSrc, + int8_t shiftBits, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecSrc; + q7x16_t vecDst; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = A (>> or <<) shiftBits + * Shift the input and then store the result in the destination buffer. + */ + vecSrc = vld1q(pSrc); + vecDst = vqshlq_r(vecSrc, shiftBits); + vst1q(pDst, vecDst); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrc += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrc = vld1q(pSrc); + vecDst = vqshlq_r(vecSrc, shiftBits); + vstrbq_p(pDst, vecDst, p0); + } +} + +#else +void arm_shift_q7( + const q7_t * pSrc, + int8_t shiftBits, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q7_t in1, in2, in3, in4; /* Temporary input variables */ +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + /* If the shift value is positive then do right shift else left shift */ + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A << shiftBits */ + +#if defined (ARM_MATH_DSP) + /* Read 4 inputs */ + in1 = *pSrc++; + in2 = *pSrc++; + in3 = *pSrc++; + in4 = *pSrc++; + + /* Pack and store result in destination buffer (in single write) */ + write_q7x4_ia (&pDst, __PACKq7(__SSAT(((q15_t) in1 << shiftBits), 8), + __SSAT(((q15_t) in2 << shiftBits), 8), + __SSAT(((q15_t) in3 << shiftBits), 8), + __SSAT(((q15_t) in4 << shiftBits), 8) )); +#else + *pDst++ = (q7_t) __SSAT(((q15_t) *pSrc++ << shiftBits), 8); + *pDst++ = (q7_t) __SSAT(((q15_t) *pSrc++ << shiftBits), 8); + *pDst++ = (q7_t) __SSAT(((q15_t) *pSrc++ << shiftBits), 8); + *pDst++ = (q7_t) __SSAT(((q15_t) *pSrc++ << shiftBits), 8); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A >> shiftBits */ + +#if defined (ARM_MATH_DSP) + /* Read 4 inputs */ + in1 = *pSrc++; + in2 = *pSrc++; + in3 = *pSrc++; + in4 = *pSrc++; + + /* Pack and store result in destination buffer (in single write) */ + write_q7x4_ia (&pDst, __PACKq7((in1 >> -shiftBits), + (in2 >> -shiftBits), + (in3 >> -shiftBits), + (in4 >> -shiftBits) )); +#else + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); + *pDst++ = (*pSrc++ >> -shiftBits); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + /* If the shift value is positive then do right shift else left shift */ + if (sign == 0U) + { + while (blkCnt > 0U) + { + /* C = A << shiftBits */ + + /* Shift input and store result in destination buffer. */ + *pDst++ = (q7_t) __SSAT(((q15_t) *pSrc++ << shiftBits), 8); + + /* Decrement loop counter */ + blkCnt--; + } + } + else + { + while (blkCnt > 0U) + { + /* C = A >> shiftBits */ + + /* Shift input and store result in destination buffer. */ + *pDst++ = (*pSrc++ >> -shiftBits); + + /* Decrement loop counter */ + blkCnt--; + } + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicShift group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f16.c new file mode 100644 index 0000000..836ad43 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f16.c @@ -0,0 +1,171 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sub_f16.c + * Description: Floating-point vector subtraction + * + * $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/basic_math_functions_f16.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicSub Vector Subtraction + + Element-by-element subtraction of two vectors. + + <pre> + pDst[n] = pSrcA[n] - pSrcB[n], 0 <= n < blockSize. + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicSub + @{ + */ + +/** + @brief Floating-point vector subtraction. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize 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_sub_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f16x8_t vec1; + f16x8_t vec2; + f16x8_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + res = vsubq(vec1, vec2); + vst1q(pDst, res); + + /* Increment pointers */ + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x7; + + if (blkCnt > 0U) + { + /* C = A + B */ + mve_pred16_t p0 = vctp16q(blkCnt); + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + vstrhq_p(pDst, vsubq(vec1,vec2), p0); + } + +} + +#else +#if defined(ARM_FLOAT16_SUPPORTED) +void arm_sub_f16( + const float16_t * pSrcA, + const float16_t * pSrcB, + float16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrcA++) - (_Float16)(*pSrcB++); + + *pDst++ = (_Float16)(*pSrcA++) - (_Float16)(*pSrcB++); + + *pDst++ = (_Float16)(*pSrcA++) - (_Float16)(*pSrcB++); + + *pDst++ = (_Float16)(*pSrcA++) - (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = (_Float16)(*pSrcA++) - (_Float16)(*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicSub group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f32.c new file mode 100644 index 0000000..2a07c0c --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f32.c @@ -0,0 +1,202 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sub_f32.c + * Description: Floating-point vector subtraction + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup BasicSub Vector Subtraction + + Element-by-element subtraction of two vectors. + + <pre> + pDst[n] = pSrcA[n] - pSrcB[n], 0 <= n < blockSize. + </pre> + + There are separate functions for floating-point, Q7, Q15, and Q31 data types. + */ + +/** + @addtogroup BasicSub + @{ + */ + +/** + @brief Floating-point vector subtraction. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +#if defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_sub_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + f32x4_t vec1; + f32x4_t vec2; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A + B */ + + /* Add and then store the results in the destination buffer. */ + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + res = vsubq(vec1, vec2); + vst1q(pDst, res); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + + if (blkCnt > 0U) + { + /* C = A + B */ + mve_pred16_t p0 = vctp32q(blkCnt); + vec1 = vld1q(pSrcA); + vec2 = vld1q(pSrcB); + vstrwq_p(pDst, vsubq(vec1,vec2), p0); + } + +} + +#else +void arm_sub_f32( + const float32_t * pSrcA, + const float32_t * pSrcB, + float32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + f32x4_t vec1; + f32x4_t vec2; + f32x4_t res; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and then store the results in the destination buffer. */ + vec1 = vld1q_f32(pSrcA); + vec2 = vld1q_f32(pSrcB); + res = vsubq_f32(vec1, vec2); + vst1q_f32(pDst, res); + + /* Increment pointers */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0x3; + +#else +#if defined (ARM_MATH_LOOPUNROLL) && !defined(ARM_MATH_AUTOVECTORIZE) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = (*pSrcA++) - (*pSrcB++); + + *pDst++ = (*pSrcA++) - (*pSrcB++); + + *pDst++ = (*pSrcA++) - (*pSrcB++); + + *pDst++ = (*pSrcA++) - (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ +#endif /* #if defined(ARM_MATH_NEON) */ + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = (*pSrcA++) - (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEF) && !defined(ARM_MATH_AUTOVECTORIZE) */ + +/** + @} end of BasicSub group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f64.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f64.c new file mode 100644 index 0000000..e409e2a --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f64.c @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sub_f64.c + * Description: Floating-point vector subtraction + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicSub + @{ + */ + +/** + @brief Floating-point vector subtraction. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_sub_f64( + const float64_t * pSrcA, + const float64_t * pSrcB, + float64_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = (*pSrcA++) - (*pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} + +/** + @} end of BasicSub group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q15.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q15.c new file mode 100644 index 0000000..575bd9f --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q15.c @@ -0,0 +1,178 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sub_q15.c + * Description: Q15 vector subtraction + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicSub + @{ + */ + +/** + @brief Q15 vector subtraction. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_sub_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q15x8_t vecA; + q15x8_t vecB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + while (blkCnt > 0U) + { + /* + * C = A - B + * Subtract and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqsubq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 8; + pSrcB += 8; + pDst += 8; + } + /* + * tail + */ + blkCnt = blockSize & 7; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrhq_p(pDst, vqsubq(vecA, vecB), p0); + } +} + + +#else +void arm_sub_q15( + const q15_t * pSrcA, + const q15_t * pSrcB, + q15_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + +#if defined (ARM_MATH_DSP) + q31_t inA1, inA2; + q31_t inB1, inB2; +#endif + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A - B */ + +#if defined (ARM_MATH_DSP) + /* read 2 times 2 samples at a time from sourceA */ + inA1 = read_q15x2_ia (&pSrcA); + inA2 = read_q15x2_ia (&pSrcA); + /* read 2 times 2 samples at a time from sourceB */ + inB1 = read_q15x2_ia (&pSrcB); + inB2 = read_q15x2_ia (&pSrcB); + + /* Subtract and store 2 times 2 samples at a time */ + write_q15x2_ia (&pDst, __QSUB16(inA1, inB1)); + write_q15x2_ia (&pDst, __QSUB16(inA2, inB2)); +#else + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ - *pSrcB++), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ - *pSrcB++), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ - *pSrcB++), 16); + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ - *pSrcB++), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + *pDst++ = (q15_t) __QSUB16(*pSrcA++, *pSrcB++); +#else + *pDst++ = (q15_t) __SSAT(((q31_t) *pSrcA++ - *pSrcB++), 16); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicSub group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q31.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q31.c new file mode 100644 index 0000000..ed879a1 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q31.c @@ -0,0 +1,159 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sub_q31.c + * Description: Q31 vector subtraction + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicSub + @{ + */ + +/** + @brief Q31 vector subtraction. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize 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) + +#include "arm_helium_utils.h" + +void arm_sub_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; + q31x4_t vecA; + q31x4_t vecB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + while (blkCnt > 0U) + { + /* + * C = A + B + * Add and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqsubq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 4; + pSrcB += 4; + pDst += 4; + } + /* + * tail + */ + blkCnt = blockSize & 3; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrwq_p(pDst, vqsubq(vecA, vecB), p0); + } +} + +#else +void arm_sub_q31( + const q31_t * pSrcA, + const q31_t * pSrcB, + q31_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = __QSUB(*pSrcA++, *pSrcB++); + + *pDst++ = __QSUB(*pSrcA++, *pSrcB++); + + *pDst++ = __QSUB(*pSrcA++, *pSrcB++); + + *pDst++ = __QSUB(*pSrcA++, *pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = __QSUB(*pSrcA++, *pSrcB++); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicSub group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q7.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q7.c new file mode 100644 index 0000000..1de152e --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_q7.c @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sub_q7.c + * Description: Q7 vector subtraction + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup BasicSub + @{ + */ + +/** + @brief Q7 vector subtraction. + @param[in] pSrcA points to the first input vector + @param[in] pSrcB points to the second input vector + @param[out] pDst points to the output vector + @param[in] blockSize number of samples in each vector + @return none + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q7 range [0x80 0x7F] will be saturated. + */ +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + +#include "arm_helium_utils.h" + +void arm_sub_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* loop counters */ + q7x16_t vecA; + q7x16_t vecB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + while (blkCnt > 0U) + { + /* + * C = A - B + * Subtract and then store the results in the destination buffer. + */ + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vst1q(pDst, vqsubq(vecA, vecB)); + /* + * Decrement the blockSize loop counter + */ + blkCnt--; + /* + * advance vector source and destination pointers + */ + pSrcA += 16; + pSrcB += 16; + pDst += 16; + } + /* + * tail + */ + blkCnt = blockSize & 0xF; + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecA = vld1q(pSrcA); + vecB = vld1q(pSrcB); + vstrbq_p(pDst, vqsubq(vecA, vecB), p0); + } +} +#else +void arm_sub_q7( + const q7_t * pSrcA, + const q7_t * pSrcB, + q7_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + /* C = A - B */ + +#if defined (ARM_MATH_DSP) + /* Subtract and store result in destination buffer (4 samples at a time). */ + write_q7x4_ia (&pDst, __QSUB8(read_q7x4_ia (&pSrcA), read_q7x4_ia (&pSrcB))); +#else + *pDst++ = (q7_t) __SSAT((q15_t) *pSrcA++ - *pSrcB++, 8); + *pDst++ = (q7_t) __SSAT((q15_t) *pSrcA++ - *pSrcB++, 8); + *pDst++ = (q7_t) __SSAT((q15_t) *pSrcA++ - *pSrcB++, 8); + *pDst++ = (q7_t) __SSAT((q15_t) *pSrcA++ - *pSrcB++, 8); +#endif + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = blockSize % 0x4U; + +#else + + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (blkCnt > 0U) + { + /* C = A - B */ + + /* Subtract and store result in destination buffer. */ + *pDst++ = (q7_t) __SSAT((q15_t) *pSrcA++ - *pSrcB++, 8); + + /* Decrement loop counter */ + blkCnt--; + } + +} +#endif /* defined(ARM_MATH_MVEI) */ + +/** + @} end of BasicSub group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u16.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u16.c new file mode 100644 index 0000000..54042ea --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u16.c @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_xor_u16.c + * Description: uint16_t bitwise exclusive OR + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @defgroup Xor Vector bitwise exclusive OR + + Compute the logical bitwise XOR. + + There are separate functions for uint32_t, uint16_t, and uint8_t data types. + */ + +/** + @addtogroup Xor + @{ + */ + +/** + @brief Compute the logical bitwise XOR of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_xor_u16( + const uint16_t * pSrcA, + const uint16_t * pSrcB, + uint16_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecSrcA, vecSrcB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, veorq_u16(vecSrcA, vecSrcB) ); + + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp16q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrhq_p(pDst, veorq_u16(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint16x8_t vecA, vecB; + + /* Compute 8 outputs at a time */ + blkCnt = blockSize >> 3U; + + while (blkCnt > 0U) + { + vecA = vld1q_u16(pSrcA); + vecB = vld1q_u16(pSrcB); + + vst1q_u16(pDst, veorq_u16(vecA, vecB) ); + + pSrcA += 8; + pSrcB += 8; + pDst += 8; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 7; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)^(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Xor group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u32.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u32.c new file mode 100644 index 0000000..9d14908 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u32.c @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_xor_u32.c + * Description: uint32_t bitwise exclusive OR + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup Xor + @{ + */ + +/** + @brief Compute the logical bitwise XOR of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_xor_u32( + const uint32_t * pSrcA, + const uint32_t * pSrcB, + uint32_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecSrcA, vecSrcB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, veorq_u32(vecSrcA, vecSrcB) ); + + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp32q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrwq_p(pDst, veorq_u32(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint32x4_t vecA, vecB; + + /* Compute 4 outputs at a time */ + blkCnt = blockSize >> 2U; + + while (blkCnt > 0U) + { + vecA = vld1q_u32(pSrcA); + vecB = vld1q_u32(pSrcB); + + vst1q_u32(pDst, veorq_u32(vecA, vecB) ); + + pSrcA += 4; + pSrcB += 4; + pDst += 4; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 3; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)^(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Xor group + */ diff --git a/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u8.c b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u8.c new file mode 100644 index 0000000..d708cd4 --- /dev/null +++ b/Drivers/CMSIS/DSP/Source/BasicMathFunctions/arm_xor_u8.c @@ -0,0 +1,129 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_xor_u8.c + * Description: uint8_t bitwise exclusive OR + * + * $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/basic_math_functions.h" + +/** + @ingroup groupMath + */ + +/** + @addtogroup Xor + @{ + */ + +/** + @brief Compute the logical bitwise XOR of two fixed-point vectors. + @param[in] pSrcA points to input vector A + @param[in] pSrcB points to input vector B + @param[out] pDst points to output vector + @param[in] blockSize number of samples in each vector + @return none + */ + +void arm_xor_u8( + const uint8_t * pSrcA, + const uint8_t * pSrcB, + uint8_t * pDst, + uint32_t blockSize) +{ + uint32_t blkCnt; /* Loop counter */ + +#if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecSrcA, vecSrcB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4; + + while (blkCnt > 0U) + { + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + + vst1q(pDst, veorq_u8(vecSrcA, vecSrcB) ); + + pSrcA += 16; + pSrcB += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; + + if (blkCnt > 0U) + { + mve_pred16_t p0 = vctp8q(blkCnt); + vecSrcA = vld1q(pSrcA); + vecSrcB = vld1q(pSrcB); + vstrbq_p(pDst, veorq_u8(vecSrcA, vecSrcB), p0); + } +#else +#if defined(ARM_MATH_NEON) && !defined(ARM_MATH_AUTOVECTORIZE) + uint8x16_t vecA, vecB; + + /* Compute 16 outputs at a time */ + blkCnt = blockSize >> 4U; + + while (blkCnt > 0U) + { + vecA = vld1q_u8(pSrcA); + vecB = vld1q_u8(pSrcB); + + vst1q_u8(pDst, veorq_u8(vecA, vecB) ); + + pSrcA += 16; + pSrcB += 16; + pDst += 16; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Tail */ + blkCnt = blockSize & 0xF; +#else + /* Initialize blkCnt with number of samples */ + blkCnt = blockSize; +#endif + + while (blkCnt > 0U) + { + *pDst++ = (*pSrcA++)^(*pSrcB++); + + /* Decrement the loop counter */ + blkCnt--; + } +#endif /* if defined(ARM_MATH_MVEI) */ +} + +/** + @} end of Xor group + */ |