From a81fc41c3eb99e8fc52aa734ee63e13c937aab81 Mon Sep 17 00:00:00 2001 From: JanHenrik Date: Sun, 19 Jan 2020 00:56:37 +0100 Subject: added blink example --- .../DSP/Source/MatrixFunctions/arm_mat_add_f32.c | 196 ++++++ .../DSP/Source/MatrixFunctions/arm_mat_add_q15.c | 151 +++++ .../DSP/Source/MatrixFunctions/arm_mat_add_q31.c | 195 ++++++ .../MatrixFunctions/arm_mat_cmplx_mult_f32.c | 272 ++++++++ .../MatrixFunctions/arm_mat_cmplx_mult_q15.c | 413 ++++++++++++ .../MatrixFunctions/arm_mat_cmplx_mult_q31.c | 282 +++++++++ .../DSP/Source/MatrixFunctions/arm_mat_init_f32.c | 76 +++ .../DSP/Source/MatrixFunctions/arm_mat_init_q15.c | 67 ++ .../DSP/Source/MatrixFunctions/arm_mat_init_q31.c | 72 +++ .../Source/MatrixFunctions/arm_mat_inverse_f32.c | 691 +++++++++++++++++++++ .../Source/MatrixFunctions/arm_mat_inverse_f64.c | 691 +++++++++++++++++++++ .../DSP/Source/MatrixFunctions/arm_mat_mult_f32.c | 274 ++++++++ .../Source/MatrixFunctions/arm_mat_mult_fast_q15.c | 525 ++++++++++++++++ .../Source/MatrixFunctions/arm_mat_mult_fast_q31.c | 384 ++++++++++++ .../DSP/Source/MatrixFunctions/arm_mat_mult_q15.c | 457 ++++++++++++++ .../DSP/Source/MatrixFunctions/arm_mat_mult_q31.c | 282 +++++++++ .../DSP/Source/MatrixFunctions/arm_mat_scale_f32.c | 169 +++++ .../DSP/Source/MatrixFunctions/arm_mat_scale_q15.c | 171 +++++ .../DSP/Source/MatrixFunctions/arm_mat_scale_q31.c | 191 ++++++ .../DSP/Source/MatrixFunctions/arm_mat_sub_f32.c | 197 ++++++ .../DSP/Source/MatrixFunctions/arm_mat_sub_q15.c | 148 +++++ .../DSP/Source/MatrixFunctions/arm_mat_sub_q31.c | 196 ++++++ .../DSP/Source/MatrixFunctions/arm_mat_trans_f32.c | 206 ++++++ .../DSP/Source/MatrixFunctions/arm_mat_trans_q15.c | 272 ++++++++ .../DSP/Source/MatrixFunctions/arm_mat_trans_q31.c | 198 ++++++ 25 files changed, 6776 insertions(+) create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c create mode 100644 Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c (limited to 'Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions') diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c new file mode 100644 index 0000000..9b609be --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_f32.c @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_add_f32.c + * Description: Floating-point matrix addition + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixAdd Matrix Addition + * + * Adds two matrices. + * \image html MatrixAddition.gif "Addition of two 3 x 3 matrices" + * + * The functions check to make sure that + * pSrcA, pSrcB, and pDst have the same + * number of rows and columns. + */ + +/** + * @addtogroup MatrixAdd + * @{ + */ + + +/** + * @brief Floating-point matrix addition. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + +arm_status arm_mat_add_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + +#if defined (ARM_MATH_DSP) + + float32_t inA1, inA2, inB1, inB2, out1, out2; /* temporary variables */ + +#endif // #if defined (ARM_MATH_DSP) + + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix addition */ + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numRows != pSrcB->numRows) || + (pSrcA->numCols != pSrcB->numCols) || + (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif + { + + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + +#if defined (ARM_MATH_DSP) + + /* Loop unrolling */ + blkCnt = numSamples >> 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(m,n) = A(m,n) + B(m,n) */ + /* Add and then store the results in the destination buffer. */ + /* Read values from source A */ + inA1 = pIn1[0]; + + /* Read values from source B */ + inB1 = pIn2[0]; + + /* Read values from source A */ + inA2 = pIn1[1]; + + /* out = sourceA + sourceB */ + out1 = inA1 + inB1; + + /* Read values from source B */ + inB2 = pIn2[1]; + + /* Read values from source A */ + inA1 = pIn1[2]; + + /* out = sourceA + sourceB */ + out2 = inA2 + inB2; + + /* Read values from source B */ + inB1 = pIn2[2]; + + /* Store result in destination */ + pOut[0] = out1; + pOut[1] = out2; + + /* Read values from source A */ + inA2 = pIn1[3]; + + /* Read values from source B */ + inB2 = pIn2[3]; + + /* out = sourceA + sourceB */ + out1 = inA1 + inB1; + + /* out = sourceA + sourceB */ + out2 = inA2 + inB2; + + /* Store result in destination */ + pOut[2] = out1; + + /* Store result in destination */ + pOut[3] = out2; + + + /* update pointers to process next sampels */ + pIn1 += 4U; + pIn2 += 4U; + pOut += 4U; + /* Decrement the loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) + B(m,n) */ + /* Add and then store the results in the destination buffer. */ + *pOut++ = (*pIn1++) + (*pIn2++); + + /* Decrement the loop counter */ + blkCnt--; + } + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixAdd group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q15.c new file mode 100644 index 0000000..e6737fa --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q15.c @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_add_q15.c + * Description: Q15 matrix addition + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixAdd + * @{ + */ + +/** + * @brief Q15 matrix addition. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * Scaling and Overflow Behavior: + * \par + * The function uses saturating arithmetic. + * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated. + */ + +arm_status arm_mat_add_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst) +{ + q15_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + q15_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + q15_t *pOut = pDst->pData; /* output data matrix pointer */ + uint16_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix addition */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrcA->numRows != pSrcB->numRows) || + (pSrcA->numCols != pSrcB->numCols) || + (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Total number of samples in the input matrix */ + numSamples = (uint16_t) (pSrcA->numRows * pSrcA->numCols); + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Loop unrolling */ + blkCnt = (uint32_t) numSamples >> 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(m,n) = A(m,n) + B(m,n) */ + /* Add, Saturate and then store the results in the destination buffer. */ + *__SIMD32(pOut)++ = __QADD16(*__SIMD32(pInA)++, *__SIMD32(pInB)++); + *__SIMD32(pOut)++ = __QADD16(*__SIMD32(pInA)++, *__SIMD32(pInB)++); + + /* Decrement the loop counter */ + blkCnt--; + } + + /* If the blockSize is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = (uint32_t) numSamples % 0x4U; + + /* q15 pointers of input and output are initialized */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) + B(m,n) */ + /* Add, Saturate and then store the results in the destination buffer. */ + *pOut++ = (q15_t) __QADD16(*pInA++, *pInB++); + + /* Decrement the loop counter */ + blkCnt--; + } + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = (uint32_t) numSamples; + + + /* q15 pointers of input and output are initialized */ + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) + B(m,n) */ + /* Add, Saturate and then store the results in the destination buffer. */ + *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ + *pInB++), 16); + + /* Decrement the loop counter */ + blkCnt--; + } + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixAdd group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.c new file mode 100644 index 0000000..4119563 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.c @@ -0,0 +1,195 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_add_q31.c + * Description: Q31 matrix addition + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixAdd + * @{ + */ + +/** + * @brief Q31 matrix addition. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * Scaling and Overflow Behavior: + * \par + * The function uses saturating arithmetic. + * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated. + */ + +arm_status arm_mat_add_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + q31_t *pOut = pDst->pData; /* output data matrix pointer */ + q31_t inA1, inB1; /* temporary variables */ + +#if defined (ARM_MATH_DSP) + + q31_t inA2, inB2; /* temporary variables */ + q31_t out1, out2; /* temporary variables */ + +#endif // #if defined (ARM_MATH_DSP) + + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix addition */ + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numRows != pSrcB->numRows) || + (pSrcA->numCols != pSrcB->numCols) || + (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Loop Unrolling */ + blkCnt = numSamples >> 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(m,n) = A(m,n) + B(m,n) */ + /* Add, saturate and then store the results in the destination buffer. */ + /* Read values from source A */ + inA1 = pIn1[0]; + + /* Read values from source B */ + inB1 = pIn2[0]; + + /* Read values from source A */ + inA2 = pIn1[1]; + + /* Add and saturate */ + out1 = __QADD(inA1, inB1); + + /* Read values from source B */ + inB2 = pIn2[1]; + + /* Read values from source A */ + inA1 = pIn1[2]; + + /* Add and saturate */ + out2 = __QADD(inA2, inB2); + + /* Read values from source B */ + inB1 = pIn2[2]; + + /* Store result in destination */ + pOut[0] = out1; + pOut[1] = out2; + + /* Read values from source A */ + inA2 = pIn1[3]; + + /* Read values from source B */ + inB2 = pIn2[3]; + + /* Add and saturate */ + out1 = __QADD(inA1, inB1); + out2 = __QADD(inA2, inB2); + + /* Store result in destination */ + pOut[2] = out1; + pOut[3] = out2; + + /* update pointers to process next sampels */ + pIn1 += 4U; + pIn2 += 4U; + pOut += 4U; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) + B(m,n) */ + /* Add, saturate and then store the results in the destination buffer. */ + inA1 = *pIn1++; + inB1 = *pIn2++; + + inA1 = __QADD(inA1, inB1); + + /* Decrement the loop counter */ + blkCnt--; + + *pOut++ = inA1; + + } + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixAdd group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c new file mode 100644 index 0000000..9b2f532 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_cmplx_mult_f32.c + * Description: Floating-point matrix multiplication + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup CmplxMatrixMult Complex Matrix Multiplication + * + * Complex Matrix multiplication is only defined if the number of columns of the + * first matrix equals the number of rows of the second matrix. + * Multiplying an M x N matrix with an N x P matrix results + * in an M x P matrix. + * When matrix size checking is enabled, the functions check: (1) that the inner dimensions of + * pSrcA and pSrcB are equal; and (2) that the size of the output + * matrix equals the outer dimensions of pSrcA and pSrcB. + */ + + +/** + * @addtogroup CmplxMatrixMult + * @{ + */ + +/** + * @brief Floating-point Complex matrix multiplication. + * @param[in] *pSrcA points to the first input complex matrix structure + * @param[in] *pSrcB points to the second input complex matrix structure + * @param[out] *pDst points to output complex matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + +arm_status arm_mat_cmplx_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + float32_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + float32_t *px; /* Temporary output data matrix pointer */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + float32_t sumReal1, sumImag1; /* accumulator */ + float32_t a0, b0, c0, d0; + float32_t a1, b1, c1, d1; + float32_t sumReal2, sumImag2; /* accumulator */ + + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + uint16_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + 2 * i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + j = 0U; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sumReal1 = 0.0f; + sumImag1 = 0.0f; + + sumReal2 = 0.0f; + sumImag2 = 0.0f; + + /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + pIn1 = pInA; + + /* Apply loop unrolling and compute 4 MACs simultaneously. */ + colCnt = numColsA >> 2; + + /* matrix multiplication */ + while (colCnt > 0U) + { + + /* Reading real part of complex matrix A */ + a0 = *pIn1; + + /* Reading real part of complex matrix B */ + c0 = *pIn2; + + /* Reading imaginary part of complex matrix A */ + b0 = *(pIn1 + 1U); + + /* Reading imaginary part of complex matrix B */ + d0 = *(pIn2 + 1U); + + sumReal1 += a0 * c0; + sumImag1 += b0 * c0; + + pIn1 += 2U; + pIn2 += 2 * numColsB; + + sumReal2 -= b0 * d0; + sumImag2 += a0 * d0; + + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + + a1 = *pIn1; + c1 = *pIn2; + + b1 = *(pIn1 + 1U); + d1 = *(pIn2 + 1U); + + sumReal1 += a1 * c1; + sumImag1 += b1 * c1; + + pIn1 += 2U; + pIn2 += 2 * numColsB; + + sumReal2 -= b1 * d1; + sumImag2 += a1 * d1; + + a0 = *pIn1; + c0 = *pIn2; + + b0 = *(pIn1 + 1U); + d0 = *(pIn2 + 1U); + + sumReal1 += a0 * c0; + sumImag1 += b0 * c0; + + pIn1 += 2U; + pIn2 += 2 * numColsB; + + sumReal2 -= b0 * d0; + sumImag2 += a0 * d0; + + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + + a1 = *pIn1; + c1 = *pIn2; + + b1 = *(pIn1 + 1U); + d1 = *(pIn2 + 1U); + + sumReal1 += a1 * c1; + sumImag1 += b1 * c1; + + pIn1 += 2U; + pIn2 += 2 * numColsB; + + sumReal2 -= b1 * d1; + sumImag2 += a1 * d1; + + /* Decrement the loop count */ + colCnt--; + } + + /* If the columns of pSrcA is not a multiple of 4, compute any remaining MACs here. + ** No loop unrolling is used. */ + colCnt = numColsA % 0x4U; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + a1 = *pIn1; + c1 = *pIn2; + + b1 = *(pIn1 + 1U); + d1 = *(pIn2 + 1U); + + sumReal1 += a1 * c1; + sumImag1 += b1 * c1; + + pIn1 += 2U; + pIn2 += 2 * numColsB; + + sumReal2 -= b1 * d1; + sumImag2 += a1 * d1; + + /* Decrement the loop counter */ + colCnt--; + } + + sumReal1 += sumReal2; + sumImag1 += sumImag2; + + /* Store the result in the destination buffer */ + *px++ = sumReal1; + *px++ = sumImag1; + + /* Update the pointer pIn2 to point to the starting address of the next column */ + j++; + pIn2 = pSrcB->pData + 2U * j; + + /* Decrement the column loop counter */ + col--; + + } while (col > 0U); + + /* Update the pointer pInA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + 2 * numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c new file mode 100644 index 0000000..b1578a5 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c @@ -0,0 +1,413 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cmplx_mat_mult_q15.c + * Description: Q15 complex matrix multiplication + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup CmplxMatrixMult + * @{ + */ + + +/** + * @brief Q15 Complex matrix multiplication + * @param[in] *pSrcA points to the first input complex matrix structure + * @param[in] *pSrcB points to the second input complex matrix structure + * @param[out] *pDst points to output complex matrix structure + * @param[in] *pScratch points to the array for storing intermediate results + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * \par Conditions for optimum performance + * Input, output and state buffers should be aligned by 32-bit + * + * \par Restrictions + * If the silicon does not support unaligned memory access enable the macro UNALIGNED_SUPPORT_DISABLE + * In this case input, output, scratch buffers should be aligned by 32-bit + * + * @details + * Scaling and Overflow Behavior: + * + * \par + * The function is implemented using a 64-bit internal accumulator. The inputs to the + * multiplications are in 1.15 format and multiplications yield a 2.30 result. + * The 2.30 intermediate + * results are accumulated in a 64-bit accumulator in 34.30 format. This approach + * provides 33 guard bits and there is no risk of overflow. The 34.30 result is then + * truncated to 34.15 format by discarding the low 15 bits and then saturated to + * 1.15 format. + * + * \par + * Refer to arm_mat_mult_fast_q15() for a faster but less precise version of this function. + * + */ + + + + +arm_status arm_mat_cmplx_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pScratch) +{ + /* accumulator */ + q15_t *pSrcBT = pScratch; /* input data matrix pointer for transpose */ + q15_t *pInA = pSrcA->pData; /* input data matrix pointer A of Q15 type */ + q15_t *pInB = pSrcB->pData; /* input data matrix pointer B of Q15 type */ + q15_t *px; /* Temporary output data matrix pointer */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + uint16_t numRowsB = pSrcB->numRows; /* number of rows of input matrix A */ + uint16_t col, i = 0U, row = numRowsB, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + q63_t sumReal, sumImag; + +#ifdef UNALIGNED_SUPPORT_DISABLE + q15_t in; /* Temporary variable to hold the input value */ + q15_t a, b, c, d; +#else + q31_t in; /* Temporary variable to hold the input value */ + q31_t prod1, prod2; + q31_t pSourceA, pSourceB; +#endif + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif + { + /* Matrix transpose */ + do + { + /* Apply loop unrolling and exchange the columns with row elements */ + col = numColsB >> 2; + + /* The pointer px is set to starting address of the column being processed */ + px = pSrcBT + i; + + /* 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 (col > 0U) + { +#ifdef UNALIGNED_SUPPORT_DISABLE + /* Read two elements from the row */ + in = *pInB++; + *px = in; + in = *pInB++; + px[1] = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Read two elements from the row */ + in = *pInB++; + *px = in; + in = *pInB++; + px[1] = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Read two elements from the row */ + in = *pInB++; + *px = in; + in = *pInB++; + px[1] = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Read two elements from the row */ + in = *pInB++; + *px = in; + in = *pInB++; + px[1] = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Decrement the column loop counter */ + col--; + } + + /* If the columns of pSrcB is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + col = numColsB % 0x4U; + + while (col > 0U) + { + /* Read two elements from the row */ + in = *pInB++; + *px = in; + in = *pInB++; + px[1] = in; +#else + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + *__SIMD32(px) = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + *__SIMD32(px) = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + *__SIMD32(px) = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + *__SIMD32(px) = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Decrement the column loop counter */ + col--; + } + + /* If the columns of pSrcB is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + col = numColsB % 0x4U; + + while (col > 0U) + { + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + *__SIMD32(px) = in; +#endif + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB * 2; + + /* Decrement the column loop counter */ + col--; + } + + i = i + 2U; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* Reset the variables for the usage in the following multiplication process */ + row = numRowsA; + i = 0U; + px = pDst->pData; + + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the transposed pSrcB data */ + pInB = pSrcBT; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sumReal = 0; + sumImag = 0; + + /* Apply loop unrolling and compute 2 MACs simultaneously. */ + colCnt = numColsA >> 1; + + /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + pInA = pSrcA->pData + i * 2; + + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + +#ifdef UNALIGNED_SUPPORT_DISABLE + + /* read real and imag values from pSrcA buffer */ + a = *pInA; + b = *(pInA + 1U); + /* read real and imag values from pSrcB buffer */ + c = *pInB; + d = *(pInB + 1U); + + /* Multiply and Accumlates */ + sumReal += (q31_t) a *c; + sumImag += (q31_t) a *d; + sumReal -= (q31_t) b *d; + sumImag += (q31_t) b *c; + + /* read next real and imag values from pSrcA buffer */ + a = *(pInA + 2U); + b = *(pInA + 3U); + /* read next real and imag values from pSrcB buffer */ + c = *(pInB + 2U); + d = *(pInB + 3U); + + /* update pointer */ + pInA += 4U; + + /* Multiply and Accumlates */ + sumReal += (q31_t) a *c; + sumImag += (q31_t) a *d; + sumReal -= (q31_t) b *d; + sumImag += (q31_t) b *c; + /* update pointer */ + pInB += 4U; +#else + /* read real and imag values from pSrcA and pSrcB buffer */ + pSourceA = *__SIMD32(pInA)++; + pSourceB = *__SIMD32(pInB)++; + + /* Multiply and Accumlates */ +#ifdef ARM_MATH_BIG_ENDIAN + prod1 = -__SMUSD(pSourceA, pSourceB); +#else + prod1 = __SMUSD(pSourceA, pSourceB); +#endif + prod2 = __SMUADX(pSourceA, pSourceB); + sumReal += (q63_t) prod1; + sumImag += (q63_t) prod2; + + /* read real and imag values from pSrcA and pSrcB buffer */ + pSourceA = *__SIMD32(pInA)++; + pSourceB = *__SIMD32(pInB)++; + + /* Multiply and Accumlates */ +#ifdef ARM_MATH_BIG_ENDIAN + prod1 = -__SMUSD(pSourceA, pSourceB); +#else + prod1 = __SMUSD(pSourceA, pSourceB); +#endif + prod2 = __SMUADX(pSourceA, pSourceB); + sumReal += (q63_t) prod1; + sumImag += (q63_t) prod2; + +#endif /* #ifdef UNALIGNED_SUPPORT_DISABLE */ + + /* Decrement the loop counter */ + colCnt--; + } + + /* process odd column samples */ + if ((numColsA & 0x1U) > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + +#ifdef UNALIGNED_SUPPORT_DISABLE + + /* read real and imag values from pSrcA and pSrcB buffer */ + a = *pInA++; + b = *pInA++; + c = *pInB++; + d = *pInB++; + + /* Multiply and Accumlates */ + sumReal += (q31_t) a *c; + sumImag += (q31_t) a *d; + sumReal -= (q31_t) b *d; + sumImag += (q31_t) b *c; + +#else + /* read real and imag values from pSrcA and pSrcB buffer */ + pSourceA = *__SIMD32(pInA)++; + pSourceB = *__SIMD32(pInB)++; + + /* Multiply and Accumlates */ +#ifdef ARM_MATH_BIG_ENDIAN + prod1 = -__SMUSD(pSourceA, pSourceB); +#else + prod1 = __SMUSD(pSourceA, pSourceB); +#endif + prod2 = __SMUADX(pSourceA, pSourceB); + sumReal += (q63_t) prod1; + sumImag += (q63_t) prod2; + +#endif /* #ifdef UNALIGNED_SUPPORT_DISABLE */ + + } + + /* Saturate and store the result in the destination buffer */ + + *px++ = (q15_t) (__SSAT(sumReal >> 15, 16)); + *px++ = (q15_t) (__SSAT(sumImag >> 15, 16)); + + /* Decrement the column loop counter */ + col--; + + } while (col > 0U); + + i = i + numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c new file mode 100644 index 0000000..a05440e --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c @@ -0,0 +1,282 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_cmplx_mult_q31.c + * Description: Floating-point matrix multiplication + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup CmplxMatrixMult + * @{ + */ + +/** + * @brief Q31 Complex matrix multiplication + * @param[in] *pSrcA points to the first input complex matrix structure + * @param[in] *pSrcB points to the second input complex matrix structure + * @param[out] *pDst points to output complex matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * + * \par + * The function is implemented using an internal 64-bit accumulator. + * The accumulator has a 2.62 format and maintains full precision of the intermediate + * multiplication results but provides only a single guard bit. There is no saturation + * on intermediate additions. Thus, if the accumulator overflows it wraps around and + * distorts the result. The input signals should be scaled down to avoid intermediate + * overflows. The input is thus scaled down by log2(numColsA) bits + * to avoid overflows, as a total of numColsA additions are performed internally. + * The 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result. + * + * + */ + +arm_status arm_mat_cmplx_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + q31_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pOut = pDst->pData; /* output data matrix pointer */ + q31_t *px; /* Temporary output data matrix pointer */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + q63_t sumReal1, sumImag1; /* accumulator */ + q31_t a0, b0, c0, d0; + q31_t a1, b1, c1, d1; + + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + uint16_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + 2 * i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + j = 0U; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sumReal1 = 0.0; + sumImag1 = 0.0; + + /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + pIn1 = pInA; + + /* Apply loop unrolling and compute 4 MACs simultaneously. */ + colCnt = numColsA >> 2; + + /* matrix multiplication */ + while (colCnt > 0U) + { + + /* Reading real part of complex matrix A */ + a0 = *pIn1; + + /* Reading real part of complex matrix B */ + c0 = *pIn2; + + /* Reading imaginary part of complex matrix A */ + b0 = *(pIn1 + 1U); + + /* Reading imaginary part of complex matrix B */ + d0 = *(pIn2 + 1U); + + /* Multiply and Accumlates */ + sumReal1 += (q63_t) a0 *c0; + sumImag1 += (q63_t) b0 *c0; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal1 -= (q63_t) b0 *d0; + sumImag1 += (q63_t) a0 *d0; + + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + + /* read real and imag values from pSrcA and pSrcB buffer */ + a1 = *pIn1; + c1 = *pIn2; + b1 = *(pIn1 + 1U); + d1 = *(pIn2 + 1U); + + /* Multiply and Accumlates */ + sumReal1 += (q63_t) a1 *c1; + sumImag1 += (q63_t) b1 *c1; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal1 -= (q63_t) b1 *d1; + sumImag1 += (q63_t) a1 *d1; + + a0 = *pIn1; + c0 = *pIn2; + + b0 = *(pIn1 + 1U); + d0 = *(pIn2 + 1U); + + /* Multiply and Accumlates */ + sumReal1 += (q63_t) a0 *c0; + sumImag1 += (q63_t) b0 *c0; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal1 -= (q63_t) b0 *d0; + sumImag1 += (q63_t) a0 *d0; + + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + + a1 = *pIn1; + c1 = *pIn2; + + b1 = *(pIn1 + 1U); + d1 = *(pIn2 + 1U); + + /* Multiply and Accumlates */ + sumReal1 += (q63_t) a1 *c1; + sumImag1 += (q63_t) b1 *c1; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal1 -= (q63_t) b1 *d1; + sumImag1 += (q63_t) a1 *d1; + + /* Decrement the loop count */ + colCnt--; + } + + /* If the columns of pSrcA is not a multiple of 4, compute any remaining MACs here. + ** No loop unrolling is used. */ + colCnt = numColsA % 0x4U; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + a1 = *pIn1; + c1 = *pIn2; + + b1 = *(pIn1 + 1U); + d1 = *(pIn2 + 1U); + + /* Multiply and Accumlates */ + sumReal1 += (q63_t) a1 *c1; + sumImag1 += (q63_t) b1 *c1; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal1 -= (q63_t) b1 *d1; + sumImag1 += (q63_t) a1 *d1; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Store the result in the destination buffer */ + *px++ = (q31_t) clip_q63_to_q31(sumReal1 >> 31); + *px++ = (q31_t) clip_q63_to_q31(sumImag1 >> 31); + + /* Update the pointer pIn2 to point to the starting address of the next column */ + j++; + pIn2 = pSrcB->pData + 2U * j; + + /* Decrement the column loop counter */ + col--; + + } while (col > 0U); + + /* Update the pointer pInA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + 2 * numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_f32.c new file mode 100644 index 0000000..34399c7 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_f32.c @@ -0,0 +1,76 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_init_f32.c + * Description: Floating-point matrix initialization + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixInit Matrix Initialization + * + * Initializes the underlying matrix data structure. + * The functions set the numRows, + * numCols, and pData fields + * of the matrix data structure. + */ + +/** + * @addtogroup MatrixInit + * @{ + */ + +/** + * @brief Floating-point matrix initialization. + * @param[in,out] *S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] *pData points to the matrix data array. + * @return none + */ + +void arm_mat_init_f32( + arm_matrix_instance_f32 * S, + uint16_t nRows, + uint16_t nColumns, + float32_t * pData) +{ + /* Assign Number of Rows */ + S->numRows = nRows; + + /* Assign Number of Columns */ + S->numCols = nColumns; + + /* Assign Data pointer */ + S->pData = pData; +} + +/** + * @} end of MatrixInit group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q15.c new file mode 100644 index 0000000..6be7387 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q15.c @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_init_q15.c + * Description: Q15 matrix initialization + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixInit + * @{ + */ + + /** + * @brief Q15 matrix initialization. + * @param[in,out] *S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] *pData points to the matrix data array. + * @return none + */ + +void arm_mat_init_q15( + arm_matrix_instance_q15 * S, + uint16_t nRows, + uint16_t nColumns, + q15_t * pData) +{ + /* Assign Number of Rows */ + S->numRows = nRows; + + /* Assign Number of Columns */ + S->numCols = nColumns; + + /* Assign Data pointer */ + S->pData = pData; +} + +/** + * @} end of MatrixInit group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q31.c new file mode 100644 index 0000000..c8a0839 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_init_q31.c @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_init_q31.c + * Description: Q31 matrix initialization + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixInit Matrix Initialization + * + */ + +/** + * @addtogroup MatrixInit + * @{ + */ + + /** + * @brief Q31 matrix initialization. + * @param[in,out] *S points to an instance of the floating-point matrix structure. + * @param[in] nRows number of rows in the matrix. + * @param[in] nColumns number of columns in the matrix. + * @param[in] *pData points to the matrix data array. + * @return none + */ + +void arm_mat_init_q31( + arm_matrix_instance_q31 * S, + uint16_t nRows, + uint16_t nColumns, + q31_t * pData) +{ + /* Assign Number of Rows */ + S->numRows = nRows; + + /* Assign Number of Columns */ + S->numCols = nColumns; + + /* Assign Data pointer */ + S->pData = pData; +} + +/** + * @} end of MatrixInit group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c new file mode 100644 index 0000000..c0f8fc4 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c @@ -0,0 +1,691 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_inverse_f32.c + * Description: Floating-point matrix inverse + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixInv Matrix Inverse + * + * Computes the inverse of a matrix. + * + * The inverse is defined only if the input matrix is square and non-singular (the determinant + * is non-zero). The function checks that the input and output matrices are square and of the + * same size. + * + * Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix + * inversion of floating-point matrices. + * + * \par Algorithm + * The Gauss-Jordan method is used to find the inverse. + * The algorithm performs a sequence of elementary row-operations until it + * reduces the input matrix to an identity matrix. Applying the same sequence + * of elementary row-operations to an identity matrix yields the inverse matrix. + * If the input matrix is singular, then the algorithm terminates and returns error status + * ARM_MATH_SINGULAR. + * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method" + */ + +/** + * @addtogroup MatrixInv + * @{ + */ + +/** + * @brief Floating-point matrix inverse. + * @param[in] *pSrc points to input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns + * ARM_MATH_SIZE_MISMATCH if the input matrix is not square or if the size + * of the output matrix does not match the size of the input matrix. + * If the input matrix is found to be singular (non-invertible), then the function returns + * ARM_MATH_SINGULAR. Otherwise, the function returns ARM_MATH_SUCCESS. + */ + +arm_status arm_mat_inverse_f32( + const arm_matrix_instance_f32 * pSrc, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn = pSrc->pData; /* input data matrix pointer */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + float32_t *pInT1, *pInT2; /* Temporary input data matrix pointer */ + float32_t *pOutT1, *pOutT2; /* Temporary output data matrix pointer */ + float32_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst; /* Temporary input and output data matrix pointer */ + uint32_t numRows = pSrc->numRows; /* Number of rows in the matrix */ + uint32_t numCols = pSrc->numCols; /* Number of Cols in the matrix */ + +#if defined (ARM_MATH_DSP) + float32_t maxC; /* maximum value in the column */ + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + float32_t Xchg, in = 0.0f, in1; /* Temporary input values */ + uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */ + arm_status status; /* status of matrix inverse */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols) + || (pSrc->numRows != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + + /*-------------------------------------------------------------------------------------------------------------- + * Matrix Inverse can be solved using elementary row operations. + * + * Gauss-Jordan Method: + * + * 1. First combine the identity matrix and the input matrix separated by a bar to form an + * augmented matrix as follows: + * _ _ _ _ + * | a11 a12 | 1 0 | | X11 X12 | + * | | | = | | + * |_ a21 a22 | 0 1 _| |_ X21 X21 _| + * + * 2. In our implementation, pDst Matrix is used as identity matrix. + * + * 3. Begin with the first row. Let i = 1. + * + * 4. Check to see if the pivot for column i is the greatest of the column. + * The pivot is the element of the main diagonal that is on the current row. + * For instance, if working with row i, then the pivot element is aii. + * If the pivot is not the most significant of the columns, exchange that row with a row + * below it that does contain the most significant value in column i. If the most + * significant value of the column is zero, then an inverse to that matrix does not exist. + * The most significant value of the column is the absolute maximum. + * + * 5. Divide every element of row i by the pivot. + * + * 6. For every row below and row i, replace that row with the sum of that row and + * a multiple of row i so that each new element in column i below row i is zero. + * + * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros + * for every element below and above the main diagonal. + * + * 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc). + * Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst). + *----------------------------------------------------------------------------------------------------------------*/ + + /* Working pointer for destination matrix */ + pOutT1 = pOut; + + /* Loop over the number of rows */ + rowCnt = numRows; + + /* Making the destination matrix as identity matrix */ + while (rowCnt > 0U) + { + /* Writing all zeroes in lower triangle of the destination matrix */ + j = numRows - rowCnt; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Writing all ones in the diagonal of the destination matrix */ + *pOutT1++ = 1.0f; + + /* Writing all zeroes in upper triangle of the destination matrix */ + j = rowCnt - 1U; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Decrement the loop counter */ + rowCnt--; + } + + /* Loop over the number of columns of the input matrix. + All the elements in each column are processed by the row operations */ + loopCnt = numCols; + + /* Index modifier to navigate through the columns */ + l = 0U; + + while (loopCnt > 0U) + { + /* Check if the pivot element is zero.. + * If it is zero then interchange the row with non zero row below. + * If there is no non zero element to replace in the rows below, + * then the matrix is Singular. */ + + /* Working pointer for the input matrix that points + * to the pivot element of the particular row */ + pInT1 = pIn + (l * numCols); + + /* Working pointer for the destination matrix that points + * to the pivot element of the particular row */ + pOutT1 = pOut + (l * numCols); + + /* Temporary variable to hold the pivot value */ + in = *pInT1; + + /* Grab the most significant value from column l */ + maxC = 0; + for (i = l; i < numRows; i++) + { + maxC = *pInT1 > 0 ? (*pInT1 > maxC ? *pInT1 : maxC) : (-*pInT1 > maxC ? -*pInT1 : maxC); + pInT1 += numCols; + } + + /* Update the status if the matrix is singular */ + if (maxC == 0.0f) + { + return ARM_MATH_SINGULAR; + } + + /* Restore pInT1 */ + pInT1 = pIn; + + /* Destination pointer modifier */ + k = 1U; + + /* Check if the pivot element is the most significant of the column */ + if ( (in > 0.0f ? in : -in) != maxC) + { + /* Loop over the number rows present below */ + i = numRows - (l + 1U); + + while (i > 0U) + { + /* Update the input and destination pointers */ + pInT2 = pInT1 + (numCols * l); + pOutT2 = pOutT1 + (numCols * k); + + /* Look for the most significant element to + * replace in the rows below */ + if ((*pInT2 > 0.0f ? *pInT2: -*pInT2) == maxC) + { + /* Loop over number of columns + * to the right of the pilot element */ + j = numCols - l; + + while (j > 0U) + { + /* Exchange the row elements of the input matrix */ + Xchg = *pInT2; + *pInT2++ = *pInT1; + *pInT1++ = Xchg; + + /* Decrement the loop counter */ + j--; + } + + /* Loop over number of columns of the destination matrix */ + j = numCols; + + while (j > 0U) + { + /* Exchange the row elements of the destination matrix */ + Xchg = *pOutT2; + *pOutT2++ = *pOutT1; + *pOutT1++ = Xchg; + + /* Decrement the loop counter */ + j--; + } + + /* Flag to indicate whether exchange is done or not */ + flag = 1U; + + /* Break after exchange is done */ + break; + } + + /* Update the destination pointer modifier */ + k++; + + /* Decrement the loop counter */ + i--; + } + } + + /* Update the status if the matrix is singular */ + if ((flag != 1U) && (in == 0.0f)) + { + return ARM_MATH_SINGULAR; + } + + /* Points to the pivot row of input and destination matrices */ + pPivotRowIn = pIn + (l * numCols); + pPivotRowDst = pOut + (l * numCols); + + /* Temporary pointers to the pivot row pointers */ + pInT1 = pPivotRowIn; + pInT2 = pPivotRowDst; + + /* Pivot element of the row */ + in = *pPivotRowIn; + + /* Loop over number of columns + * to the right of the pilot element */ + j = (numCols - l); + + while (j > 0U) + { + /* Divide each element of the row of the input matrix + * by the pivot element */ + in1 = *pInT1; + *pInT1++ = in1 / in; + + /* Decrement the loop counter */ + j--; + } + + /* Loop over number of columns of the destination matrix */ + j = numCols; + + while (j > 0U) + { + /* Divide each element of the row of the destination matrix + * by the pivot element */ + in1 = *pInT2; + *pInT2++ = in1 / in; + + /* Decrement the loop counter */ + j--; + } + + /* Replace the rows with the sum of that row and a multiple of row i + * so that each new element in column i above row i is zero.*/ + + /* Temporary pointers for input and destination matrices */ + pInT1 = pIn; + pInT2 = pOut; + + /* index used to check for pivot element */ + i = 0U; + + /* Loop over number of rows */ + /* to be replaced by the sum of that row and a multiple of row i */ + k = numRows; + + while (k > 0U) + { + /* Check for the pivot element */ + if (i == l) + { + /* If the processing element is the pivot element, + only the columns to the right are to be processed */ + pInT1 += numCols - l; + + pInT2 += numCols; + } + else + { + /* Element of the reference row */ + in = *pInT1; + + /* Working pointers for input and destination pivot rows */ + pPRT_in = pPivotRowIn; + pPRT_pDst = pPivotRowDst; + + /* Loop over the number of columns to the right of the pivot element, + to replace the elements in the input matrix */ + j = (numCols - l); + + while (j > 0U) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + in1 = *pInT1; + *pInT1++ = in1 - (in * *pPRT_in++); + + /* Decrement the loop counter */ + j--; + } + + /* Loop over the number of columns to + replace the elements in the destination matrix */ + j = numCols; + + while (j > 0U) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + in1 = *pInT2; + *pInT2++ = in1 - (in * *pPRT_pDst++); + + /* Decrement the loop counter */ + j--; + } + + } + + /* Increment the temporary input pointer */ + pInT1 = pInT1 + l; + + /* Decrement the loop counter */ + k--; + + /* Increment the pivot index */ + i++; + } + + /* Increment the input pointer */ + pIn++; + + /* Decrement the loop counter */ + loopCnt--; + + /* Increment the index modifier */ + l++; + } + + +#else + + /* Run the below code for Cortex-M0 */ + + float32_t Xchg, in = 0.0f; /* Temporary input values */ + uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */ + arm_status status; /* status of matrix inverse */ + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols) + || (pSrc->numRows != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { + + /*-------------------------------------------------------------------------------------------------------------- + * Matrix Inverse can be solved using elementary row operations. + * + * Gauss-Jordan Method: + * + * 1. First combine the identity matrix and the input matrix separated by a bar to form an + * augmented matrix as follows: + * _ _ _ _ _ _ _ _ + * | | a11 a12 | | | 1 0 | | | X11 X12 | + * | | | | | | | = | | + * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _| + * + * 2. In our implementation, pDst Matrix is used as identity matrix. + * + * 3. Begin with the first row. Let i = 1. + * + * 4. Check to see if the pivot for row i is zero. + * The pivot is the element of the main diagonal that is on the current row. + * For instance, if working with row i, then the pivot element is aii. + * If the pivot is zero, exchange that row with a row below it that does not + * contain a zero in column i. If this is not possible, then an inverse + * to that matrix does not exist. + * + * 5. Divide every element of row i by the pivot. + * + * 6. For every row below and row i, replace that row with the sum of that row and + * a multiple of row i so that each new element in column i below row i is zero. + * + * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros + * for every element below and above the main diagonal. + * + * 8. Now an identical matrix is formed to the left of the bar(input matrix, src). + * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst). + *----------------------------------------------------------------------------------------------------------------*/ + + /* Working pointer for destination matrix */ + pOutT1 = pOut; + + /* Loop over the number of rows */ + rowCnt = numRows; + + /* Making the destination matrix as identity matrix */ + while (rowCnt > 0U) + { + /* Writing all zeroes in lower triangle of the destination matrix */ + j = numRows - rowCnt; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Writing all ones in the diagonal of the destination matrix */ + *pOutT1++ = 1.0f; + + /* Writing all zeroes in upper triangle of the destination matrix */ + j = rowCnt - 1U; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Decrement the loop counter */ + rowCnt--; + } + + /* Loop over the number of columns of the input matrix. + All the elements in each column are processed by the row operations */ + loopCnt = numCols; + + /* Index modifier to navigate through the columns */ + l = 0U; + //for(loopCnt = 0U; loopCnt < numCols; loopCnt++) + while (loopCnt > 0U) + { + /* Check if the pivot element is zero.. + * If it is zero then interchange the row with non zero row below. + * If there is no non zero element to replace in the rows below, + * then the matrix is Singular. */ + + /* Working pointer for the input matrix that points + * to the pivot element of the particular row */ + pInT1 = pIn + (l * numCols); + + /* Working pointer for the destination matrix that points + * to the pivot element of the particular row */ + pOutT1 = pOut + (l * numCols); + + /* Temporary variable to hold the pivot value */ + in = *pInT1; + + /* Destination pointer modifier */ + k = 1U; + + /* Check if the pivot element is zero */ + if (*pInT1 == 0.0f) + { + /* Loop over the number rows present below */ + for (i = (l + 1U); i < numRows; i++) + { + /* Update the input and destination pointers */ + pInT2 = pInT1 + (numCols * l); + pOutT2 = pOutT1 + (numCols * k); + + /* Check if there is a non zero pivot element to + * replace in the rows below */ + if (*pInT2 != 0.0f) + { + /* Loop over number of columns + * to the right of the pilot element */ + for (j = 0U; j < (numCols - l); j++) + { + /* Exchange the row elements of the input matrix */ + Xchg = *pInT2; + *pInT2++ = *pInT1; + *pInT1++ = Xchg; + } + + for (j = 0U; j < numCols; j++) + { + Xchg = *pOutT2; + *pOutT2++ = *pOutT1; + *pOutT1++ = Xchg; + } + + /* Flag to indicate whether exchange is done or not */ + flag = 1U; + + /* Break after exchange is done */ + break; + } + + /* Update the destination pointer modifier */ + k++; + } + } + + /* Update the status if the matrix is singular */ + if ((flag != 1U) && (in == 0.0f)) + { + return ARM_MATH_SINGULAR; + } + + /* Points to the pivot row of input and destination matrices */ + pPivotRowIn = pIn + (l * numCols); + pPivotRowDst = pOut + (l * numCols); + + /* Temporary pointers to the pivot row pointers */ + pInT1 = pPivotRowIn; + pOutT1 = pPivotRowDst; + + /* Pivot element of the row */ + in = *(pIn + (l * numCols)); + + /* Loop over number of columns + * to the right of the pilot element */ + for (j = 0U; j < (numCols - l); j++) + { + /* Divide each element of the row of the input matrix + * by the pivot element */ + *pInT1 = *pInT1 / in; + pInT1++; + } + for (j = 0U; j < numCols; j++) + { + /* Divide each element of the row of the destination matrix + * by the pivot element */ + *pOutT1 = *pOutT1 / in; + pOutT1++; + } + + /* Replace the rows with the sum of that row and a multiple of row i + * so that each new element in column i above row i is zero.*/ + + /* Temporary pointers for input and destination matrices */ + pInT1 = pIn; + pOutT1 = pOut; + + for (i = 0U; i < numRows; i++) + { + /* Check for the pivot element */ + if (i == l) + { + /* If the processing element is the pivot element, + only the columns to the right are to be processed */ + pInT1 += numCols - l; + pOutT1 += numCols; + } + else + { + /* Element of the reference row */ + in = *pInT1; + + /* Working pointers for input and destination pivot rows */ + pPRT_in = pPivotRowIn; + pPRT_pDst = pPivotRowDst; + + /* Loop over the number of columns to the right of the pivot element, + to replace the elements in the input matrix */ + for (j = 0U; j < (numCols - l); j++) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + *pInT1 = *pInT1 - (in * *pPRT_in++); + pInT1++; + } + /* Loop over the number of columns to + replace the elements in the destination matrix */ + for (j = 0U; j < numCols; j++) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + *pOutT1 = *pOutT1 - (in * *pPRT_pDst++); + pOutT1++; + } + + } + /* Increment the temporary input pointer */ + pInT1 = pInT1 + l; + } + /* Increment the input pointer */ + pIn++; + + /* Decrement the loop counter */ + loopCnt--; + /* Increment the index modifier */ + l++; + } + + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + + if ((flag != 1U) && (in == 0.0f)) + { + pIn = pSrc->pData; + for (i = 0; i < numRows * numCols; i++) + { + if (pIn[i] != 0.0f) + break; + } + + if (i == numRows * numCols) + status = ARM_MATH_SINGULAR; + } + } + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixInv group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c new file mode 100644 index 0000000..441376b --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c @@ -0,0 +1,691 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_inverse_f64.c + * Description: Floating-point matrix inverse + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixInv Matrix Inverse + * + * Computes the inverse of a matrix. + * + * The inverse is defined only if the input matrix is square and non-singular (the determinant + * is non-zero). The function checks that the input and output matrices are square and of the + * same size. + * + * Matrix inversion is numerically sensitive and the CMSIS DSP library only supports matrix + * inversion of floating-point matrices. + * + * \par Algorithm + * The Gauss-Jordan method is used to find the inverse. + * The algorithm performs a sequence of elementary row-operations until it + * reduces the input matrix to an identity matrix. Applying the same sequence + * of elementary row-operations to an identity matrix yields the inverse matrix. + * If the input matrix is singular, then the algorithm terminates and returns error status + * ARM_MATH_SINGULAR. + * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method" + */ + +/** + * @addtogroup MatrixInv + * @{ + */ + +/** + * @brief Floating-point matrix inverse. + * @param[in] *pSrc points to input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns + * ARM_MATH_SIZE_MISMATCH if the input matrix is not square or if the size + * of the output matrix does not match the size of the input matrix. + * If the input matrix is found to be singular (non-invertible), then the function returns + * ARM_MATH_SINGULAR. Otherwise, the function returns ARM_MATH_SUCCESS. + */ + +arm_status arm_mat_inverse_f64( + const arm_matrix_instance_f64 * pSrc, + arm_matrix_instance_f64 * pDst) +{ + float64_t *pIn = pSrc->pData; /* input data matrix pointer */ + float64_t *pOut = pDst->pData; /* output data matrix pointer */ + float64_t *pInT1, *pInT2; /* Temporary input data matrix pointer */ + float64_t *pOutT1, *pOutT2; /* Temporary output data matrix pointer */ + float64_t *pPivotRowIn, *pPRT_in, *pPivotRowDst, *pPRT_pDst; /* Temporary input and output data matrix pointer */ + uint32_t numRows = pSrc->numRows; /* Number of rows in the matrix */ + uint32_t numCols = pSrc->numCols; /* Number of Cols in the matrix */ + +#if defined (ARM_MATH_DSP) + float64_t maxC; /* maximum value in the column */ + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + float64_t Xchg, in = 0.0f, in1; /* Temporary input values */ + uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */ + arm_status status; /* status of matrix inverse */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols) + || (pSrc->numRows != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + + /*-------------------------------------------------------------------------------------------------------------- + * Matrix Inverse can be solved using elementary row operations. + * + * Gauss-Jordan Method: + * + * 1. First combine the identity matrix and the input matrix separated by a bar to form an + * augmented matrix as follows: + * _ _ _ _ + * | a11 a12 | 1 0 | | X11 X12 | + * | | | = | | + * |_ a21 a22 | 0 1 _| |_ X21 X21 _| + * + * 2. In our implementation, pDst Matrix is used as identity matrix. + * + * 3. Begin with the first row. Let i = 1. + * + * 4. Check to see if the pivot for column i is the greatest of the column. + * The pivot is the element of the main diagonal that is on the current row. + * For instance, if working with row i, then the pivot element is aii. + * If the pivot is not the most significant of the columns, exchange that row with a row + * below it that does contain the most significant value in column i. If the most + * significant value of the column is zero, then an inverse to that matrix does not exist. + * The most significant value of the column is the absolute maximum. + * + * 5. Divide every element of row i by the pivot. + * + * 6. For every row below and row i, replace that row with the sum of that row and + * a multiple of row i so that each new element in column i below row i is zero. + * + * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros + * for every element below and above the main diagonal. + * + * 8. Now an identical matrix is formed to the left of the bar(input matrix, pSrc). + * Therefore, the matrix to the right of the bar is our solution(pDst matrix, pDst). + *----------------------------------------------------------------------------------------------------------------*/ + + /* Working pointer for destination matrix */ + pOutT1 = pOut; + + /* Loop over the number of rows */ + rowCnt = numRows; + + /* Making the destination matrix as identity matrix */ + while (rowCnt > 0U) + { + /* Writing all zeroes in lower triangle of the destination matrix */ + j = numRows - rowCnt; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Writing all ones in the diagonal of the destination matrix */ + *pOutT1++ = 1.0f; + + /* Writing all zeroes in upper triangle of the destination matrix */ + j = rowCnt - 1U; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Decrement the loop counter */ + rowCnt--; + } + + /* Loop over the number of columns of the input matrix. + All the elements in each column are processed by the row operations */ + loopCnt = numCols; + + /* Index modifier to navigate through the columns */ + l = 0U; + + while (loopCnt > 0U) + { + /* Check if the pivot element is zero.. + * If it is zero then interchange the row with non zero row below. + * If there is no non zero element to replace in the rows below, + * then the matrix is Singular. */ + + /* Working pointer for the input matrix that points + * to the pivot element of the particular row */ + pInT1 = pIn + (l * numCols); + + /* Working pointer for the destination matrix that points + * to the pivot element of the particular row */ + pOutT1 = pOut + (l * numCols); + + /* Temporary variable to hold the pivot value */ + in = *pInT1; + + /* Grab the most significant value from column l */ + maxC = 0; + for (i = l; i < numRows; i++) + { + maxC = *pInT1 > 0 ? (*pInT1 > maxC ? *pInT1 : maxC) : (-*pInT1 > maxC ? -*pInT1 : maxC); + pInT1 += numCols; + } + + /* Update the status if the matrix is singular */ + if (maxC == 0.0f) + { + return ARM_MATH_SINGULAR; + } + + /* Restore pInT1 */ + pInT1 = pIn; + + /* Destination pointer modifier */ + k = 1U; + + /* Check if the pivot element is the most significant of the column */ + if ( (in > 0.0f ? in : -in) != maxC) + { + /* Loop over the number rows present below */ + i = numRows - (l + 1U); + + while (i > 0U) + { + /* Update the input and destination pointers */ + pInT2 = pInT1 + (numCols * l); + pOutT2 = pOutT1 + (numCols * k); + + /* Look for the most significant element to + * replace in the rows below */ + if ((*pInT2 > 0.0f ? *pInT2: -*pInT2) == maxC) + { + /* Loop over number of columns + * to the right of the pilot element */ + j = numCols - l; + + while (j > 0U) + { + /* Exchange the row elements of the input matrix */ + Xchg = *pInT2; + *pInT2++ = *pInT1; + *pInT1++ = Xchg; + + /* Decrement the loop counter */ + j--; + } + + /* Loop over number of columns of the destination matrix */ + j = numCols; + + while (j > 0U) + { + /* Exchange the row elements of the destination matrix */ + Xchg = *pOutT2; + *pOutT2++ = *pOutT1; + *pOutT1++ = Xchg; + + /* Decrement the loop counter */ + j--; + } + + /* Flag to indicate whether exchange is done or not */ + flag = 1U; + + /* Break after exchange is done */ + break; + } + + /* Update the destination pointer modifier */ + k++; + + /* Decrement the loop counter */ + i--; + } + } + + /* Update the status if the matrix is singular */ + if ((flag != 1U) && (in == 0.0f)) + { + return ARM_MATH_SINGULAR; + } + + /* Points to the pivot row of input and destination matrices */ + pPivotRowIn = pIn + (l * numCols); + pPivotRowDst = pOut + (l * numCols); + + /* Temporary pointers to the pivot row pointers */ + pInT1 = pPivotRowIn; + pInT2 = pPivotRowDst; + + /* Pivot element of the row */ + in = *pPivotRowIn; + + /* Loop over number of columns + * to the right of the pilot element */ + j = (numCols - l); + + while (j > 0U) + { + /* Divide each element of the row of the input matrix + * by the pivot element */ + in1 = *pInT1; + *pInT1++ = in1 / in; + + /* Decrement the loop counter */ + j--; + } + + /* Loop over number of columns of the destination matrix */ + j = numCols; + + while (j > 0U) + { + /* Divide each element of the row of the destination matrix + * by the pivot element */ + in1 = *pInT2; + *pInT2++ = in1 / in; + + /* Decrement the loop counter */ + j--; + } + + /* Replace the rows with the sum of that row and a multiple of row i + * so that each new element in column i above row i is zero.*/ + + /* Temporary pointers for input and destination matrices */ + pInT1 = pIn; + pInT2 = pOut; + + /* index used to check for pivot element */ + i = 0U; + + /* Loop over number of rows */ + /* to be replaced by the sum of that row and a multiple of row i */ + k = numRows; + + while (k > 0U) + { + /* Check for the pivot element */ + if (i == l) + { + /* If the processing element is the pivot element, + only the columns to the right are to be processed */ + pInT1 += numCols - l; + + pInT2 += numCols; + } + else + { + /* Element of the reference row */ + in = *pInT1; + + /* Working pointers for input and destination pivot rows */ + pPRT_in = pPivotRowIn; + pPRT_pDst = pPivotRowDst; + + /* Loop over the number of columns to the right of the pivot element, + to replace the elements in the input matrix */ + j = (numCols - l); + + while (j > 0U) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + in1 = *pInT1; + *pInT1++ = in1 - (in * *pPRT_in++); + + /* Decrement the loop counter */ + j--; + } + + /* Loop over the number of columns to + replace the elements in the destination matrix */ + j = numCols; + + while (j > 0U) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + in1 = *pInT2; + *pInT2++ = in1 - (in * *pPRT_pDst++); + + /* Decrement the loop counter */ + j--; + } + + } + + /* Increment the temporary input pointer */ + pInT1 = pInT1 + l; + + /* Decrement the loop counter */ + k--; + + /* Increment the pivot index */ + i++; + } + + /* Increment the input pointer */ + pIn++; + + /* Decrement the loop counter */ + loopCnt--; + + /* Increment the index modifier */ + l++; + } + + +#else + + /* Run the below code for Cortex-M0 */ + + float64_t Xchg, in = 0.0f; /* Temporary input values */ + uint32_t i, rowCnt, flag = 0U, j, loopCnt, k, l; /* loop counters */ + arm_status status; /* status of matrix inverse */ + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols) + || (pSrc->numRows != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { + + /*-------------------------------------------------------------------------------------------------------------- + * Matrix Inverse can be solved using elementary row operations. + * + * Gauss-Jordan Method: + * + * 1. First combine the identity matrix and the input matrix separated by a bar to form an + * augmented matrix as follows: + * _ _ _ _ _ _ _ _ + * | | a11 a12 | | | 1 0 | | | X11 X12 | + * | | | | | | | = | | + * |_ |_ a21 a22 _| | |_0 1 _| _| |_ X21 X21 _| + * + * 2. In our implementation, pDst Matrix is used as identity matrix. + * + * 3. Begin with the first row. Let i = 1. + * + * 4. Check to see if the pivot for row i is zero. + * The pivot is the element of the main diagonal that is on the current row. + * For instance, if working with row i, then the pivot element is aii. + * If the pivot is zero, exchange that row with a row below it that does not + * contain a zero in column i. If this is not possible, then an inverse + * to that matrix does not exist. + * + * 5. Divide every element of row i by the pivot. + * + * 6. For every row below and row i, replace that row with the sum of that row and + * a multiple of row i so that each new element in column i below row i is zero. + * + * 7. Move to the next row and column and repeat steps 2 through 5 until you have zeros + * for every element below and above the main diagonal. + * + * 8. Now an identical matrix is formed to the left of the bar(input matrix, src). + * Therefore, the matrix to the right of the bar is our solution(dst matrix, dst). + *----------------------------------------------------------------------------------------------------------------*/ + + /* Working pointer for destination matrix */ + pOutT1 = pOut; + + /* Loop over the number of rows */ + rowCnt = numRows; + + /* Making the destination matrix as identity matrix */ + while (rowCnt > 0U) + { + /* Writing all zeroes in lower triangle of the destination matrix */ + j = numRows - rowCnt; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Writing all ones in the diagonal of the destination matrix */ + *pOutT1++ = 1.0f; + + /* Writing all zeroes in upper triangle of the destination matrix */ + j = rowCnt - 1U; + while (j > 0U) + { + *pOutT1++ = 0.0f; + j--; + } + + /* Decrement the loop counter */ + rowCnt--; + } + + /* Loop over the number of columns of the input matrix. + All the elements in each column are processed by the row operations */ + loopCnt = numCols; + + /* Index modifier to navigate through the columns */ + l = 0U; + //for(loopCnt = 0U; loopCnt < numCols; loopCnt++) + while (loopCnt > 0U) + { + /* Check if the pivot element is zero.. + * If it is zero then interchange the row with non zero row below. + * If there is no non zero element to replace in the rows below, + * then the matrix is Singular. */ + + /* Working pointer for the input matrix that points + * to the pivot element of the particular row */ + pInT1 = pIn + (l * numCols); + + /* Working pointer for the destination matrix that points + * to the pivot element of the particular row */ + pOutT1 = pOut + (l * numCols); + + /* Temporary variable to hold the pivot value */ + in = *pInT1; + + /* Destination pointer modifier */ + k = 1U; + + /* Check if the pivot element is zero */ + if (*pInT1 == 0.0f) + { + /* Loop over the number rows present below */ + for (i = (l + 1U); i < numRows; i++) + { + /* Update the input and destination pointers */ + pInT2 = pInT1 + (numCols * l); + pOutT2 = pOutT1 + (numCols * k); + + /* Check if there is a non zero pivot element to + * replace in the rows below */ + if (*pInT2 != 0.0f) + { + /* Loop over number of columns + * to the right of the pilot element */ + for (j = 0U; j < (numCols - l); j++) + { + /* Exchange the row elements of the input matrix */ + Xchg = *pInT2; + *pInT2++ = *pInT1; + *pInT1++ = Xchg; + } + + for (j = 0U; j < numCols; j++) + { + Xchg = *pOutT2; + *pOutT2++ = *pOutT1; + *pOutT1++ = Xchg; + } + + /* Flag to indicate whether exchange is done or not */ + flag = 1U; + + /* Break after exchange is done */ + break; + } + + /* Update the destination pointer modifier */ + k++; + } + } + + /* Update the status if the matrix is singular */ + if ((flag != 1U) && (in == 0.0f)) + { + return ARM_MATH_SINGULAR; + } + + /* Points to the pivot row of input and destination matrices */ + pPivotRowIn = pIn + (l * numCols); + pPivotRowDst = pOut + (l * numCols); + + /* Temporary pointers to the pivot row pointers */ + pInT1 = pPivotRowIn; + pOutT1 = pPivotRowDst; + + /* Pivot element of the row */ + in = *(pIn + (l * numCols)); + + /* Loop over number of columns + * to the right of the pilot element */ + for (j = 0U; j < (numCols - l); j++) + { + /* Divide each element of the row of the input matrix + * by the pivot element */ + *pInT1 = *pInT1 / in; + pInT1++; + } + for (j = 0U; j < numCols; j++) + { + /* Divide each element of the row of the destination matrix + * by the pivot element */ + *pOutT1 = *pOutT1 / in; + pOutT1++; + } + + /* Replace the rows with the sum of that row and a multiple of row i + * so that each new element in column i above row i is zero.*/ + + /* Temporary pointers for input and destination matrices */ + pInT1 = pIn; + pOutT1 = pOut; + + for (i = 0U; i < numRows; i++) + { + /* Check for the pivot element */ + if (i == l) + { + /* If the processing element is the pivot element, + only the columns to the right are to be processed */ + pInT1 += numCols - l; + pOutT1 += numCols; + } + else + { + /* Element of the reference row */ + in = *pInT1; + + /* Working pointers for input and destination pivot rows */ + pPRT_in = pPivotRowIn; + pPRT_pDst = pPivotRowDst; + + /* Loop over the number of columns to the right of the pivot element, + to replace the elements in the input matrix */ + for (j = 0U; j < (numCols - l); j++) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + *pInT1 = *pInT1 - (in * *pPRT_in++); + pInT1++; + } + /* Loop over the number of columns to + replace the elements in the destination matrix */ + for (j = 0U; j < numCols; j++) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + *pOutT1 = *pOutT1 - (in * *pPRT_pDst++); + pOutT1++; + } + + } + /* Increment the temporary input pointer */ + pInT1 = pInT1 + l; + } + /* Increment the input pointer */ + pIn++; + + /* Decrement the loop counter */ + loopCnt--; + /* Increment the index modifier */ + l++; + } + + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + + if ((flag != 1U) && (in == 0.0f)) + { + pIn = pSrc->pData; + for (i = 0; i < numRows * numCols; i++) + { + if (pIn[i] != 0.0f) + break; + } + + if (i == numRows * numCols) + status = ARM_MATH_SINGULAR; + } + } + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixInv group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c new file mode 100644 index 0000000..fa9f03f --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c @@ -0,0 +1,274 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_mult_f32.c + * Description: Floating-point matrix multiplication + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixMult Matrix Multiplication + * + * Multiplies two matrices. + * + * \image html MatrixMultiplication.gif "Multiplication of two 3 x 3 matrices" + + * Matrix multiplication is only defined if the number of columns of the + * first matrix equals the number of rows of the second matrix. + * Multiplying an M x N matrix with an N x P matrix results + * in an M x P matrix. + * When matrix size checking is enabled, the functions check: (1) that the inner dimensions of + * pSrcA and pSrcB are equal; and (2) that the size of the output + * matrix equals the outer dimensions of pSrcA and pSrcB. + */ + + +/** + * @addtogroup MatrixMult + * @{ + */ + +/** + * @brief Floating-point matrix multiplication. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + +arm_status arm_mat_mult_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + float32_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + float32_t *px; /* Temporary output data matrix pointer */ + float32_t sum; /* Accumulator */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + float32_t in1, in2, in3, in4; + uint16_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + j = 0U; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0.0f; + + /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + pIn1 = pInA; + + /* Apply loop unrolling and compute 4 MACs simultaneously. */ + colCnt = numColsA >> 2U; + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + in3 = *pIn2; + pIn2 += numColsB; + in1 = pIn1[0]; + in2 = pIn1[1]; + sum += in1 * in3; + in4 = *pIn2; + pIn2 += numColsB; + sum += in2 * in4; + + in3 = *pIn2; + pIn2 += numColsB; + in1 = pIn1[2]; + in2 = pIn1[3]; + sum += in1 * in3; + in4 = *pIn2; + pIn2 += numColsB; + sum += in2 * in4; + pIn1 += 4U; + + /* Decrement the loop count */ + colCnt--; + } + + /* If the columns of pSrcA is not a multiple of 4, compute any remaining MACs here. + ** No loop unrolling is used. */ + colCnt = numColsA % 0x4U; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + sum += *pIn1++ * (*pIn2); + pIn2 += numColsB; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Store the result in the destination buffer */ + *px++ = sum; + + /* Update the pointer pIn2 to point to the starting address of the next column */ + j++; + pIn2 = pSrcB->pData + j; + + /* Decrement the column loop counter */ + col--; + + } while (col > 0U); + +#else + + /* Run the below code for Cortex-M0 */ + + float32_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + uint16_t col, i = 0U, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pInA with each column in pInB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0.0f; + + /* Initialize the pointer pIn1 to point to the starting address of the row being processed */ + pIn1 = pInA; + + /* Matrix A columns number of MAC operations are to be performed */ + colCnt = numColsA; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + sum += *pIn1++ * (*pIn2); + pIn2 += numColsB; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Store the result in the destination buffer */ + *px++ = sum; + + /* Decrement the column loop counter */ + col--; + + /* Update the pointer pIn2 to point to the starting address of the next column */ + pIn2 = pInB + (numColsB - col); + + } while (col > 0U); + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Update the pointer pInA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c new file mode 100644 index 0000000..796df88 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c @@ -0,0 +1,525 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_mult_fast_q15.c + * Description: Q15 matrix multiplication (fast variant) + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixMult + * @{ + */ + + +/** + * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @param[in] *pState points to the array for storing intermediate results + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * + * \par + * The difference between the function arm_mat_mult_q15() and this fast variant is that + * the fast variant use a 32-bit rather than a 64-bit accumulator. + * The result of each 1.15 x 1.15 multiplication is truncated to + * 2.30 format. These intermediate results are accumulated in a 32-bit register in 2.30 + * format. Finally, the accumulator is saturated and converted to a 1.15 result. + * + * \par + * The fast version has the same overflow behavior as the standard version but provides + * less precision since it discards the low 16 bits of each multiplication result. + * In order to avoid overflows completely the input signals must be scaled down. + * Scale down one of the input matrices by log2(numColsA) bits to + * avoid overflows, as a total of numColsA additions are computed internally for each + * output element. + * + * \par + * See arm_mat_mult_q15() for a slower implementation of this function + * which uses 64-bit accumulation to provide higher precision. + */ + +arm_status arm_mat_mult_fast_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pState) +{ + q31_t sum; /* accumulator */ + q15_t *pSrcBT = pState; /* input data matrix pointer for transpose */ + q15_t *pInA = pSrcA->pData; /* input data matrix pointer A of Q15 type */ + q15_t *pInB = pSrcB->pData; /* input data matrix pointer B of Q15 type */ + q15_t *px; /* Temporary output data matrix pointer */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + uint16_t numRowsB = pSrcB->numRows; /* number of rows of input matrix A */ + uint32_t col, i = 0U, row = numRowsB, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifndef UNALIGNED_SUPPORT_DISABLE + + q31_t in; /* Temporary variable to hold the input value */ + q31_t inA1, inA2, inB1, inB2; + q31_t sum2, sum3, sum4; + q15_t *pInA2, *pInB2, *px2; + uint32_t j = 0; + +#else + + q15_t in; /* Temporary variable to hold the input value */ + q15_t inA1, inA2, inB1, inB2; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif + { + /* Matrix transpose */ + do + { + /* Apply loop unrolling and exchange the columns with row elements */ + col = numColsB >> 2; + + /* The pointer px is set to starting address of the column being processed */ + px = pSrcBT + i; + + /* 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 (col > 0U) + { +#ifndef UNALIGNED_SUPPORT_DISABLE + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + /* Unpack and store one element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) in; + +#else + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Unpack and store the second element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#else + + *px = (q15_t) in; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + /* Unpack and store one element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) in; + +#else + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Unpack and store the second element in the destination */ + +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#else + + *px = (q15_t) in; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + +#else + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Decrement the column loop counter */ + col--; + } + + /* If the columns of pSrcB is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + col = numColsB % 0x4U; + + while (col > 0U) + { + /* Read and store the input element in the destination */ + *px = *pInB++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Decrement the column loop counter */ + col--; + } + + i++; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* Reset the variables for the usage in the following multiplication process */ + row = numRowsA; + i = 0U; + px = pDst->pData; + +#ifndef UNALIGNED_SUPPORT_DISABLE + /* Process two rows from matrix A at a time and output two rows at a time */ + row = row >> 1; + px2 = px + numColsB; +#endif + + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + while (row > 0U) + { + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the transposed pSrcB data */ + pInB = pSrcBT; + +#ifndef UNALIGNED_SUPPORT_DISABLE + /* Process two (transposed) columns from matrix B at a time */ + col = col >> 1; + j = 0; +#endif + + /* column loop */ + while (col > 0U) + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Initiate the pointer pInA to point to the starting address of the column being processed */ + pInA = pSrcA->pData + i; + +#ifndef UNALIGNED_SUPPORT_DISABLE + sum2 = 0; + sum3 = 0; + sum4 = 0; + pInB = pSrcBT + j; + pInA2 = pInA + numColsA; + pInB2 = pInB + numRowsB; + + /* Read in two elements at once - alows dual MAC instruction */ + colCnt = numColsA >> 1; +#else + colCnt = numColsA >> 2; +#endif + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ +#ifndef UNALIGNED_SUPPORT_DISABLE + + inA1 = *__SIMD32(pInA)++; + inB1 = *__SIMD32(pInB)++; + inA2 = *__SIMD32(pInA2)++; + inB2 = *__SIMD32(pInB2)++; + + sum = __SMLAD(inA1, inB1, sum); + sum2 = __SMLAD(inA1, inB2, sum2); + sum3 = __SMLAD(inA2, inB1, sum3); + sum4 = __SMLAD(inA2, inB2, sum4); + +#else + + inA1 = *pInA; + inB1 = *pInB; + sum += inA1 * inB1; + + inA2 = pInA[1]; + inB2 = pInB[1]; + sum += inA2 * inB2; + + inA1 = pInA[2]; + inB1 = pInB[2]; + sum += inA1 * inB1; + + inA2 = pInA[3]; + inB2 = pInB[3]; + sum += inA2 * inB2; + + pInA += 4; + pInB += 4; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /* Decrement the loop counter */ + colCnt--; + } + + /* process odd column samples */ +#ifndef UNALIGNED_SUPPORT_DISABLE + if (numColsA & 1U) { + inA1 = *pInA++; + inB1 = *pInB++; + inA2 = *pInA2++; + inB2 = *pInB2++; + sum += inA1 * inB1; + sum2 += inA1 * inB2; + sum3 += inA2 * inB1; + sum4 += inA2 * inB2; + } +#else + colCnt = numColsA % 0x4U; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + sum += (q31_t) (*pInA++) * (*pInB++); + + colCnt--; + } +#endif + + /* Saturate and store the result in the destination buffer */ + *px++ = (q15_t) (sum >> 15); + +#ifndef UNALIGNED_SUPPORT_DISABLE + *px++ = (q15_t) (sum2 >> 15); + *px2++ = (q15_t) (sum3 >> 15); + *px2++ = (q15_t) (sum4 >> 15); + j += numRowsB * 2; +#endif + + /* Decrement the column loop counter */ + col--; + + } + + i = i + numColsA; + +#ifndef UNALIGNED_SUPPORT_DISABLE + i = i + numColsA; + px = px2 + (numColsB & 1U); + px2 = px + numColsB; +#endif + + /* Decrement the row loop counter */ + row--; + + } + + /* Compute any remaining odd row/column below */ + +#ifndef UNALIGNED_SUPPORT_DISABLE + + /* Compute remaining output column */ + if (numColsB & 1U) { + + /* Avoid redundant computation of last element */ + row = numRowsA & (~0x1); + + /* Point to remaining unfilled column in output matrix */ + px = pDst->pData+numColsB-1; + pInA = pSrcA->pData; + + /* row loop */ + while (row > 0) + { + + /* point to last column in matrix B */ + pInB = pSrcBT + numRowsB*(numColsB-1); + + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Compute 4 columns at once */ + colCnt = numColsA >> 2; + + /* matrix multiplication */ + while (colCnt > 0U) + { + inA1 = *__SIMD32(pInA)++; + inA2 = *__SIMD32(pInA)++; + inB1 = *__SIMD32(pInB)++; + inB2 = *__SIMD32(pInB)++; + + sum = __SMLAD(inA1, inB1, sum); + sum = __SMLAD(inA2, inB2, sum); + + /* Decrement the loop counter */ + colCnt--; + } + + colCnt = numColsA & 3U; + while (colCnt > 0U) { + sum += (q31_t) (*pInA++) * (*pInB++); + colCnt--; + } + + /* Store the result in the destination buffer */ + *px = (q15_t) (sum >> 15); + px += numColsB; + + /* Decrement the row loop counter */ + row--; + } + } + + /* Compute remaining output row */ + if (numRowsA & 1U) { + + /* point to last row in output matrix */ + px = pDst->pData+(numColsB)*(numRowsA-1); + + pInB = pSrcBT; + col = numColsB; + i = 0U; + + /* col loop */ + while (col > 0) + { + + /* point to last row in matrix A */ + pInA = pSrcA->pData + (numRowsA-1)*numColsA; + + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Compute 4 columns at once */ + colCnt = numColsA >> 2; + + /* matrix multiplication */ + while (colCnt > 0U) + { + inA1 = *__SIMD32(pInA)++; + inA2 = *__SIMD32(pInA)++; + inB1 = *__SIMD32(pInB)++; + inB2 = *__SIMD32(pInB)++; + + sum = __SMLAD(inA1, inB1, sum); + sum = __SMLAD(inA2, inB2, sum); + + /* Decrement the loop counter */ + colCnt--; + } + + colCnt = numColsA & 3U; + while (colCnt > 0U) { + sum += (q31_t) (*pInA++) * (*pInB++); + colCnt--; + } + + /* Store the result in the destination buffer */ + *px++ = (q15_t) (sum >> 15); + + /* Decrement the col loop counter */ + col--; + } + } + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c new file mode 100644 index 0000000..bff3177 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c @@ -0,0 +1,384 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_mult_fast_q31.c + * Description: Q31 matrix multiplication (fast variant) + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixMult + * @{ + */ + +/** + * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4 + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * + * \par + * The difference between the function arm_mat_mult_q31() and this fast variant is that + * the fast variant use a 32-bit rather than a 64-bit accumulator. + * The result of each 1.31 x 1.31 multiplication is truncated to + * 2.30 format. These intermediate results are accumulated in a 32-bit register in 2.30 + * format. Finally, the accumulator is saturated and converted to a 1.31 result. + * + * \par + * The fast version has the same overflow behavior as the standard version but provides + * less precision since it discards the low 32 bits of each multiplication result. + * In order to avoid overflows completely the input signals must be scaled down. + * Scale down one of the input matrices by log2(numColsA) bits to + * avoid overflows, as a total of numColsA additions are computed internally for each + * output element. + * + * \par + * See arm_mat_mult_q31() for a slower implementation of this function + * which uses 64-bit accumulation to provide higher precision. + */ + +arm_status arm_mat_mult_fast_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + q31_t *px; /* Temporary output data matrix pointer */ + q31_t sum; /* Accumulator */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + uint32_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + q31_t inA1, inB1; + +#if defined (ARM_MATH_DSP) + + q31_t sum2, sum3, sum4; + q31_t inA2, inB2; + q31_t *pInA2; + q31_t *px2; + +#endif + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + + px = pDst->pData; + +#if defined (ARM_MATH_DSP) + row = row >> 1; + px2 = px + numColsB; +#endif + + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + while (row > 0U) + { + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pInB = pSrcB->pData; + + j = 0U; + +#if defined (ARM_MATH_DSP) + col = col >> 1; +#endif + + /* column loop */ + while (col > 0U) + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Initiate data pointers */ + pInA = pSrcA->pData + i; + pInB = pSrcB->pData + j; + +#if defined (ARM_MATH_DSP) + sum2 = 0; + sum3 = 0; + sum4 = 0; + pInA2 = pInA + numColsA; + colCnt = numColsA; +#else + colCnt = numColsA >> 2; +#endif + + /* matrix multiplication */ + while (colCnt > 0U) + { + +#if defined (ARM_MATH_DSP) + inA1 = *pInA++; + inB1 = pInB[0]; + inA2 = *pInA2++; + inB2 = pInB[1]; + pInB += numColsB; + + sum = __SMMLA(inA1, inB1, sum); + sum2 = __SMMLA(inA1, inB2, sum2); + sum3 = __SMMLA(inA2, inB1, sum3); + sum4 = __SMMLA(inA2, inB2, sum4); +#else + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* Perform the multiply-accumulates */ + inB1 = *pInB; + pInB += numColsB; + inA1 = pInA[0]; + sum = __SMMLA(inA1, inB1, sum); + + inB1 = *pInB; + pInB += numColsB; + inA1 = pInA[1]; + sum = __SMMLA(inA1, inB1, sum); + + inB1 = *pInB; + pInB += numColsB; + inA1 = pInA[2]; + sum = __SMMLA(inA1, inB1, sum); + + inB1 = *pInB; + pInB += numColsB; + inA1 = pInA[3]; + sum = __SMMLA(inA1, inB1, sum); + + pInA += 4U; +#endif + + /* Decrement the loop counter */ + colCnt--; + } + +#ifdef ARM_MATH_CM0_FAMILY + /* If the columns of pSrcA is not a multiple of 4, compute any remaining output samples here. */ + colCnt = numColsA % 0x4U; + while (colCnt > 0U) + { + sum = __SMMLA(*pInA++, *pInB, sum); + pInB += numColsB; + colCnt--; + } + j++; +#endif + + /* Convert the result from 2.30 to 1.31 format and store in destination buffer */ + *px++ = sum << 1; + +#if defined (ARM_MATH_DSP) + *px++ = sum2 << 1; + *px2++ = sum3 << 1; + *px2++ = sum4 << 1; + j += 2; +#endif + + /* Decrement the column loop counter */ + col--; + + } + + i = i + numColsA; + +#if defined (ARM_MATH_DSP) + i = i + numColsA; + px = px2 + (numColsB & 1U); + px2 = px + numColsB; +#endif + + /* Decrement the row loop counter */ + row--; + + } + + /* Compute any remaining odd row/column below */ + +#if defined (ARM_MATH_DSP) + + /* Compute remaining output column */ + if (numColsB & 1U) { + + /* Avoid redundant computation of last element */ + row = numRowsA & (~0x1); + + /* Point to remaining unfilled column in output matrix */ + px = pDst->pData+numColsB-1; + pInA = pSrcA->pData; + + /* row loop */ + while (row > 0) + { + + /* point to last column in matrix B */ + pInB = pSrcB->pData + numColsB-1; + + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Compute 4 columns at once */ + colCnt = numColsA >> 2; + + /* matrix multiplication */ + while (colCnt > 0U) + { + inA1 = *pInA++; + inA2 = *pInA++; + inB1 = *pInB; + pInB += numColsB; + inB2 = *pInB; + pInB += numColsB; + sum = __SMMLA(inA1, inB1, sum); + sum = __SMMLA(inA2, inB2, sum); + + inA1 = *pInA++; + inA2 = *pInA++; + inB1 = *pInB; + pInB += numColsB; + inB2 = *pInB; + pInB += numColsB; + sum = __SMMLA(inA1, inB1, sum); + sum = __SMMLA(inA2, inB2, sum); + + /* Decrement the loop counter */ + colCnt--; + } + + colCnt = numColsA & 3U; + while (colCnt > 0U) { + sum = __SMMLA(*pInA++, *pInB, sum); + pInB += numColsB; + colCnt--; + } + + /* Convert the result from 2.30 to 1.31 format and store in destination buffer */ + *px = sum << 1; + px += numColsB; + + /* Decrement the row loop counter */ + row--; + } + } + + /* Compute remaining output row */ + if (numRowsA & 1U) { + + /* point to last row in output matrix */ + px = pDst->pData+(numColsB)*(numRowsA-1); + + col = numColsB; + i = 0U; + + /* col loop */ + while (col > 0) + { + + /* point to last row in matrix A */ + pInA = pSrcA->pData + (numRowsA-1)*numColsA; + pInB = pSrcB->pData + i; + + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Compute 4 columns at once */ + colCnt = numColsA >> 2; + + /* matrix multiplication */ + while (colCnt > 0U) + { + inA1 = *pInA++; + inA2 = *pInA++; + inB1 = *pInB; + pInB += numColsB; + inB2 = *pInB; + pInB += numColsB; + sum = __SMMLA(inA1, inB1, sum); + sum = __SMMLA(inA2, inB2, sum); + + inA1 = *pInA++; + inA2 = *pInA++; + inB1 = *pInB; + pInB += numColsB; + inB2 = *pInB; + pInB += numColsB; + sum = __SMMLA(inA1, inB1, sum); + sum = __SMMLA(inA2, inB2, sum); + + /* Decrement the loop counter */ + colCnt--; + } + + colCnt = numColsA & 3U; + while (colCnt > 0U) { + sum = __SMMLA(*pInA++, *pInB, sum); + pInB += numColsB; + colCnt--; + } + + /* Saturate and store the result in the destination buffer */ + *px++ = sum << 1; + i++; + + /* Decrement the col loop counter */ + col--; + } + } + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c new file mode 100644 index 0000000..abd55bd --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c @@ -0,0 +1,457 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_mult_q15.c + * Description: Q15 matrix multiplication + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixMult + * @{ + */ + + +/** + * @brief Q15 matrix multiplication + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @param[in] *pState points to the array for storing intermediate results (Unused) + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * + * \par + * The function is implemented using a 64-bit internal accumulator. The inputs to the + * multiplications are in 1.15 format and multiplications yield a 2.30 result. + * The 2.30 intermediate + * results are accumulated in a 64-bit accumulator in 34.30 format. This approach + * provides 33 guard bits and there is no risk of overflow. The 34.30 result is then + * truncated to 34.15 format by discarding the low 15 bits and then saturated to + * 1.15 format. + * + * \par + * Refer to arm_mat_mult_fast_q15() for a faster but less precise version of this function for Cortex-M3 and Cortex-M4. + * + */ + +arm_status arm_mat_mult_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst, + q15_t * pState) +{ + q63_t sum; /* accumulator */ + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + q15_t *pSrcBT = pState; /* input data matrix pointer for transpose */ + q15_t *pInA = pSrcA->pData; /* input data matrix pointer A of Q15 type */ + q15_t *pInB = pSrcB->pData; /* input data matrix pointer B of Q15 type */ + q15_t *px; /* Temporary output data matrix pointer */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + uint16_t numRowsB = pSrcB->numRows; /* number of rows of input matrix A */ + uint16_t col, i = 0U, row = numRowsB, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifndef UNALIGNED_SUPPORT_DISABLE + + q31_t in; /* Temporary variable to hold the input value */ + q31_t pSourceA1, pSourceB1, pSourceA2, pSourceB2; + +#else + + q15_t in; /* Temporary variable to hold the input value */ + q15_t inA1, inB1, inA2, inB2; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { + /* Matrix transpose */ + do + { + /* Apply loop unrolling and exchange the columns with row elements */ + col = numColsB >> 2; + + /* The pointer px is set to starting address of the column being processed */ + px = pSrcBT + i; + + /* 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 (col > 0U) + { +#ifndef UNALIGNED_SUPPORT_DISABLE + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + /* Unpack and store one element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) in; + +#else + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Unpack and store the second element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#else + + *px = (q15_t) in; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read two elements from the row */ + in = *__SIMD32(pInB)++; + + /* Unpack and store one element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) in; + +#else + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Unpack and store the second element in the destination */ + +#ifndef ARM_MATH_BIG_ENDIAN + + *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#else + + *px = (q15_t) in; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + +#else + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Read one element from the row */ + in = *pInB++; + + /* Store one element in the destination */ + *px = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /* Decrement the column loop counter */ + col--; + } + + /* If the columns of pSrcB is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + col = numColsB % 0x4U; + + while (col > 0U) + { + /* Read and store the input element in the destination */ + *px = *pInB++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += numRowsB; + + /* Decrement the column loop counter */ + col--; + } + + i++; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* Reset the variables for the usage in the following multiplication process */ + row = numRowsA; + i = 0U; + px = pDst->pData; + + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the transposed pSrcB data */ + pInB = pSrcBT; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Apply loop unrolling and compute 2 MACs simultaneously. */ + colCnt = numColsA >> 2; + + /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + pInA = pSrcA->pData + i; + + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ +#ifndef UNALIGNED_SUPPORT_DISABLE + + /* read real and imag values from pSrcA and pSrcB buffer */ + pSourceA1 = *__SIMD32(pInA)++; + pSourceB1 = *__SIMD32(pInB)++; + + pSourceA2 = *__SIMD32(pInA)++; + pSourceB2 = *__SIMD32(pInB)++; + + /* Multiply and Accumlates */ + sum = __SMLALD(pSourceA1, pSourceB1, sum); + sum = __SMLALD(pSourceA2, pSourceB2, sum); + +#else + /* read real and imag values from pSrcA and pSrcB buffer */ + inA1 = *pInA++; + inB1 = *pInB++; + inA2 = *pInA++; + /* Multiply and Accumlates */ + sum += inA1 * inB1; + inB2 = *pInB++; + + inA1 = *pInA++; + inB1 = *pInB++; + /* Multiply and Accumlates */ + sum += inA2 * inB2; + inA2 = *pInA++; + inB2 = *pInB++; + + /* Multiply and Accumlates */ + sum += inA1 * inB1; + sum += inA2 * inB2; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /* Decrement the loop counter */ + colCnt--; + } + + /* process remaining column samples */ + colCnt = numColsA & 3U; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + sum += *pInA++ * *pInB++; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Saturate and store the result in the destination buffer */ + *px = (q15_t) (__SSAT((sum >> 15), 16)); + px++; + + /* Decrement the column loop counter */ + col--; + + } while (col > 0U); + + i = i + numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + +#else + + /* Run the below code for Cortex-M0 */ + + q15_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + q15_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + q15_t *pInA = pSrcA->pData; /* input data matrix pointer A of Q15 type */ + q15_t *pInB = pSrcB->pData; /* input data matrix pointer B of Q15 type */ + q15_t *pOut = pDst->pData; /* output data matrix pointer */ + q15_t *px; /* Temporary output data matrix pointer */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t col, i = 0U, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Initiate the pointer pIn1 to point to the starting address of pSrcA */ + pIn1 = pInA; + + /* Matrix A columns number of MAC operations are to be performed */ + colCnt = numColsA; + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* Perform the multiply-accumulates */ + sum += (q31_t) * pIn1++ * *pIn2; + pIn2 += numColsB; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Convert the result from 34.30 to 1.15 format and store the saturated value in destination buffer */ + /* Saturate and store the result in the destination buffer */ + *px++ = (q15_t) __SSAT((sum >> 15), 16); + + /* Decrement the column loop counter */ + col--; + + /* Update the pointer pIn2 to point to the starting address of the next column */ + pIn2 = pInB + (numColsB - col); + + } while (col > 0U); + + /* Update the pointer pSrcA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + +#endif /* #if defined (ARM_MATH_DSP) */ + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c new file mode 100644 index 0000000..2ce3637 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c @@ -0,0 +1,282 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_mult_q31.c + * Description: Q31 matrix multiplication + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixMult + * @{ + */ + +/** + * @brief Q31 matrix multiplication + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * + * \par + * The function is implemented using an internal 64-bit accumulator. + * The accumulator has a 2.62 format and maintains full precision of the intermediate + * multiplication results but provides only a single guard bit. There is no saturation + * on intermediate additions. Thus, if the accumulator overflows it wraps around and + * distorts the result. The input signals should be scaled down to avoid intermediate + * overflows. The input is thus scaled down by log2(numColsA) bits + * to avoid overflows, as a total of numColsA additions are performed internally. + * The 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result. + * + * \par + * See arm_mat_mult_fast_q31() for a faster but less precise implementation of this function for Cortex-M3 and Cortex-M4. + * + */ + +arm_status arm_mat_mult_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + q31_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pOut = pDst->pData; /* output data matrix pointer */ + q31_t *px; /* Temporary output data matrix pointer */ + q63_t sum; /* Accumulator */ + uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */ + uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */ + uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */ + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + uint16_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + q31_t a0, a1, a2, a3, b0, b1, b2, b3; + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + j = 0U; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Initiate the pointer pIn1 to point to the starting address of pInA */ + pIn1 = pInA; + + /* Apply loop unrolling and compute 4 MACs simultaneously. */ + colCnt = numColsA >> 2; + + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* Perform the multiply-accumulates */ + b0 = *pIn2; + pIn2 += numColsB; + + a0 = *pIn1++; + a1 = *pIn1++; + + b1 = *pIn2; + pIn2 += numColsB; + b2 = *pIn2; + pIn2 += numColsB; + + sum += (q63_t) a0 *b0; + sum += (q63_t) a1 *b1; + + a2 = *pIn1++; + a3 = *pIn1++; + + b3 = *pIn2; + pIn2 += numColsB; + + sum += (q63_t) a2 *b2; + sum += (q63_t) a3 *b3; + + /* Decrement the loop counter */ + colCnt--; + } + + /* If the columns of pSrcA is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + colCnt = numColsA % 0x4U; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* Perform the multiply-accumulates */ + sum += (q63_t) * pIn1++ * *pIn2; + pIn2 += numColsB; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Convert the result from 2.62 to 1.31 format and store in destination buffer */ + *px++ = (q31_t) (sum >> 31); + + /* Update the pointer pIn2 to point to the starting address of the next column */ + j++; + pIn2 = (pSrcB->pData) + j; + + /* Decrement the column loop counter */ + col--; + + } while (col > 0U); + +#else + + /* Run the below code for Cortex-M0 */ + + q31_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + uint16_t col, i = 0U, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrcA->numCols != pSrcB->numRows) || + (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ + /* row loop */ + do + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + i; + + /* For every row wise process, the column loop counter is to be initiated */ + col = numColsB; + + /* For every row wise process, the pIn2 pointer is set + ** to the starting address of the pSrcB data */ + pIn2 = pSrcB->pData; + + /* column loop */ + do + { + /* Set the variable sum, that acts as accumulator, to zero */ + sum = 0; + + /* Initiate the pointer pIn1 to point to the starting address of pInA */ + pIn1 = pInA; + + /* Matrix A columns number of MAC operations are to be performed */ + colCnt = numColsA; + + /* matrix multiplication */ + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* Perform the multiply-accumulates */ + sum += (q63_t) * pIn1++ * *pIn2; + pIn2 += numColsB; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Convert the result from 2.62 to 1.31 format and store in destination buffer */ + *px++ = (q31_t) clip_q63_to_q31(sum >> 31); + + /* Decrement the column loop counter */ + col--; + + /* Update the pointer pIn2 to point to the starting address of the next column */ + pIn2 = pInB + (numColsB - col); + + } while (col > 0U); + +#endif + + /* Update the pointer pInA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + numColsA; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixMult group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c new file mode 100644 index 0000000..3e4f5f7 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c @@ -0,0 +1,169 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_scale_f32.c + * Description: Multiplies a floating-point matrix by a scalar + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixScale Matrix Scale + * + * Multiplies a matrix by a scalar. This is accomplished by multiplying each element in the + * matrix by the scalar. For example: + * \image html MatrixScale.gif "Matrix Scaling of a 3 x 3 matrix" + * + * The function checks to make sure that the input and output matrices are of the same size. + * + * In the fixed-point Q15 and Q31 functions, scale is represented by + * a fractional multiplication scaleFract and an arithmetic shift shift. + * The shift allows the gain of the scaling operation to exceed 1.0. + * The overall scale factor applied to the fixed-point data is + * 
+ *     scale = scaleFract * 2^shift.
+ *
+ */ + +/** + * @addtogroup MatrixScale + * @{ + */ + +/** + * @brief Floating-point matrix scaling. + * @param[in] *pSrc points to input matrix structure + * @param[in] scale scale factor to be applied + * @param[out] *pDst points to output matrix structure + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + * + */ + +arm_status arm_mat_scale_f32( + const arm_matrix_instance_f32 * pSrc, + float32_t scale, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn = pSrc->pData; /* input data matrix pointer */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix scaling */ + +#if defined (ARM_MATH_DSP) + + float32_t in1, in2, in3, in4; /* temporary variables */ + float32_t out1, out2, out3, out4; /* temporary variables */ + +#endif // #if defined (ARM_MATH_DSP) + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numRows) || (pSrc->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Loop Unrolling */ + blkCnt = numSamples >> 2; + + /* First part of the processing with loop unrolling. Compute 4 outputs at a time. + ** a second loop below computes the remaining 1 to 3 samples. */ + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) * scale */ + /* Scaling and results are stored in the destination buffer. */ + in1 = pIn[0]; + in2 = pIn[1]; + in3 = pIn[2]; + in4 = pIn[3]; + + out1 = in1 * scale; + out2 = in2 * scale; + out3 = in3 * scale; + out4 = in4 * scale; + + + pOut[0] = out1; + pOut[1] = out2; + pOut[2] = out3; + pOut[3] = out4; + + /* update pointers to process next sampels */ + pIn += 4U; + pOut += 4U; + + /* Decrement the numSamples loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) * scale */ + /* The results are stored in the destination buffer. */ + *pOut++ = (*pIn++) * scale; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixScale group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c new file mode 100644 index 0000000..4eff925 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c @@ -0,0 +1,171 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_scale_q15.c + * Description: Multiplies a Q15 matrix by a scalar + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixScale + * @{ + */ + +/** + * @brief Q15 matrix scaling. + * @param[in] *pSrc points to input matrix + * @param[in] scaleFract fractional portion of the scale factor + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * \par + * The input data *pSrc and scaleFract 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. + */ + +arm_status arm_mat_scale_q15( + const arm_matrix_instance_q15 * pSrc, + q15_t scaleFract, + int32_t shift, + arm_matrix_instance_q15 * pDst) +{ + q15_t *pIn = pSrc->pData; /* input data matrix pointer */ + q15_t *pOut = pDst->pData; /* output data matrix pointer */ + uint32_t numSamples; /* total number of elements in the matrix */ + int32_t totShift = 15 - shift; /* total shift to apply after scaling */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix scaling */ + +#if defined (ARM_MATH_DSP) + + q15_t in1, in2, in3, in4; + q31_t out1, out2, out3, out4; + q31_t inA1, inA2; + +#endif // #if defined (ARM_MATH_DSP) + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch */ + if ((pSrc->numRows != pDst->numRows) || (pSrc->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif // #ifdef ARM_MATH_MATRIX_CHECK + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + /* Loop Unrolling */ + blkCnt = numSamples >> 2; + + /* First part of the processing with loop unrolling. Compute 4 outputs at a time. + ** a second loop below computes the remaining 1 to 3 samples. */ + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) * k */ + /* Scale, saturate and then store the results in the destination buffer. */ + /* Reading 2 inputs from memory */ + inA1 = _SIMD32_OFFSET(pIn); + inA2 = _SIMD32_OFFSET(pIn + 2); + + /* C = A * scale */ + /* Scale the inputs and then store the 2 results in the destination buffer + * 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); + + out1 = out1 >> totShift; + inA1 = _SIMD32_OFFSET(pIn + 4); + out2 = out2 >> totShift; + inA2 = _SIMD32_OFFSET(pIn + 6); + out3 = out3 >> totShift; + out4 = out4 >> totShift; + + in1 = (q15_t) (__SSAT(out1, 16)); + in2 = (q15_t) (__SSAT(out2, 16)); + in3 = (q15_t) (__SSAT(out3, 16)); + in4 = (q15_t) (__SSAT(out4, 16)); + + _SIMD32_OFFSET(pOut) = __PKHBT(in2, in1, 16); + _SIMD32_OFFSET(pOut + 2) = __PKHBT(in4, in3, 16); + + /* update pointers to process next sampels */ + pIn += 4U; + pOut += 4U; + + + /* Decrement the numSamples loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) * k */ + /* Scale, saturate and then store the results in the destination buffer. */ + *pOut++ = + (q15_t) (__SSAT(((q31_t) (*pIn++) * scaleFract) >> totShift, 16)); + + /* Decrement the numSamples loop counter */ + blkCnt--; + } + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixScale group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c new file mode 100644 index 0000000..1b2b373 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c @@ -0,0 +1,191 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_scale_q31.c + * Description: Multiplies a Q31 matrix by a scalar + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixScale + * @{ + */ + +/** + * @brief Q31 matrix scaling. + * @param[in] *pSrc points to input matrix + * @param[in] scaleFract fractional portion of the scale factor + * @param[in] shift number of bits to shift the result by + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * @details + * Scaling and Overflow Behavior: + * \par + * The input data *pSrc and scaleFract 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. + */ + +arm_status arm_mat_scale_q31( + const arm_matrix_instance_q31 * pSrc, + q31_t scaleFract, + int32_t shift, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pIn = pSrc->pData; /* input data matrix pointer */ + q31_t *pOut = pDst->pData; /* output data matrix pointer */ + uint32_t numSamples; /* total number of elements in the matrix */ + int32_t totShift = shift + 1; /* shift to apply after scaling */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix scaling */ + q31_t in1, in2, out1; /* temporary variabels */ + +#if defined (ARM_MATH_DSP) + + q31_t in3, in4, out2, out3, out4; /* temporary variables */ + +#endif // #ifndef ARM_MAT_CM0 + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch */ + if ((pSrc->numRows != pDst->numRows) || (pSrc->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif // #ifdef ARM_MATH_MATRIX_CHECK + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Loop Unrolling */ + blkCnt = numSamples >> 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(m,n) = A(m,n) * k */ + /* Read values from input */ + in1 = *pIn; + in2 = *(pIn + 1); + in3 = *(pIn + 2); + in4 = *(pIn + 3); + + /* multiply input with scaler value */ + in1 = ((q63_t) in1 * scaleFract) >> 32; + in2 = ((q63_t) in2 * scaleFract) >> 32; + in3 = ((q63_t) in3 * scaleFract) >> 32; + in4 = ((q63_t) in4 * scaleFract) >> 32; + + /* apply shifting */ + out1 = in1 << totShift; + out2 = in2 << totShift; + + /* saturate the results. */ + if (in1 != (out1 >> totShift)) + out1 = 0x7FFFFFFF ^ (in1 >> 31); + + if (in2 != (out2 >> totShift)) + out2 = 0x7FFFFFFF ^ (in2 >> 31); + + out3 = in3 << totShift; + out4 = in4 << totShift; + + *pOut = out1; + *(pOut + 1) = out2; + + if (in3 != (out3 >> totShift)) + out3 = 0x7FFFFFFF ^ (in3 >> 31); + + if (in4 != (out4 >> totShift)) + out4 = 0x7FFFFFFF ^ (in4 >> 31); + + + *(pOut + 2) = out3; + *(pOut + 3) = out4; + + /* update pointers to process next sampels */ + pIn += 4U; + pOut += 4U; + + + /* Decrement the numSamples loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) * k */ + /* Scale, saturate and then store the results in the destination buffer. */ + in1 = *pIn++; + + in2 = ((q63_t) in1 * scaleFract) >> 32; + + out1 = in2 << totShift; + + if (in2 != (out1 >> totShift)) + out1 = 0x7FFFFFFF ^ (in2 >> 31); + + *pOut++ = out1; + + /* Decrement the numSamples loop counter */ + blkCnt--; + } + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixScale group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c new file mode 100644 index 0000000..42eaadb --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c @@ -0,0 +1,197 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_sub_f32.c + * Description: Floating-point matrix subtraction + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @defgroup MatrixSub Matrix Subtraction + * + * Subtract two matrices. + * \image html MatrixSubtraction.gif "Subraction of two 3 x 3 matrices" + * + * The functions check to make sure that + * pSrcA, pSrcB, and pDst have the same + * number of rows and columns. + */ + +/** + * @addtogroup MatrixSub + * @{ + */ + +/** + * @brief Floating-point matrix subtraction + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + +arm_status arm_mat_sub_f32( + const arm_matrix_instance_f32 * pSrcA, + const arm_matrix_instance_f32 * pSrcB, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + +#if defined (ARM_MATH_DSP) + + float32_t inA1, inA2, inB1, inB2, out1, out2; /* temporary variables */ + +#endif // #if defined (ARM_MATH_DSP) + + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix subtraction */ + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numRows != pSrcB->numRows) || + (pSrcA->numCols != pSrcB->numCols) || + (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Loop Unrolling */ + blkCnt = numSamples >> 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(m,n) = A(m,n) - B(m,n) */ + /* Subtract and then store the results in the destination buffer. */ + /* Read values from source A */ + inA1 = pIn1[0]; + + /* Read values from source B */ + inB1 = pIn2[0]; + + /* Read values from source A */ + inA2 = pIn1[1]; + + /* out = sourceA - sourceB */ + out1 = inA1 - inB1; + + /* Read values from source B */ + inB2 = pIn2[1]; + + /* Read values from source A */ + inA1 = pIn1[2]; + + /* out = sourceA - sourceB */ + out2 = inA2 - inB2; + + /* Read values from source B */ + inB1 = pIn2[2]; + + /* Store result in destination */ + pOut[0] = out1; + pOut[1] = out2; + + /* Read values from source A */ + inA2 = pIn1[3]; + + /* Read values from source B */ + inB2 = pIn2[3]; + + /* out = sourceA - sourceB */ + out1 = inA1 - inB1; + + + /* out = sourceA - sourceB */ + out2 = inA2 - inB2; + + /* Store result in destination */ + pOut[2] = out1; + + /* Store result in destination */ + pOut[3] = out2; + + + /* update pointers to process next sampels */ + pIn1 += 4U; + pIn2 += 4U; + pOut += 4U; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) - B(m,n) */ + /* Subtract and then store the results in the destination buffer. */ + *pOut++ = (*pIn1++) - (*pIn2++); + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixSub group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c new file mode 100644 index 0000000..07818dc --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_sub_q15.c + * Description: Q15 Matrix subtraction + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixSub + * @{ + */ + +/** + * @brief Q15 matrix subtraction. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * Scaling and Overflow Behavior: + * \par + * The function uses saturating arithmetic. + * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated. + */ + +arm_status arm_mat_sub_q15( + const arm_matrix_instance_q15 * pSrcA, + const arm_matrix_instance_q15 * pSrcB, + arm_matrix_instance_q15 * pDst) +{ + q15_t *pInA = pSrcA->pData; /* input data matrix pointer A */ + q15_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + q15_t *pOut = pDst->pData; /* output data matrix pointer */ + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix subtraction */ + + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrcA->numRows != pSrcB->numRows) || + (pSrcA->numCols != pSrcB->numCols) || + (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Apply loop unrolling */ + blkCnt = numSamples >> 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(m,n) = A(m,n) - B(m,n) */ + /* Subtract, Saturate and then store the results in the destination buffer. */ + *__SIMD32(pOut)++ = __QSUB16(*__SIMD32(pInA)++, *__SIMD32(pInB)++); + *__SIMD32(pOut)++ = __QSUB16(*__SIMD32(pInA)++, *__SIMD32(pInB)++); + + /* Decrement the loop counter */ + blkCnt--; + } + + /* If the blockSize is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) - B(m,n) */ + /* Subtract and then store the results in the destination buffer. */ + *pOut++ = (q15_t) __QSUB16(*pInA++, *pInB++); + + /* Decrement the loop counter */ + blkCnt--; + } + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) - B(m,n) */ + /* Subtract and then store the results in the destination buffer. */ + *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ - *pInB++), 16); + + /* Decrement the loop counter */ + blkCnt--; + } + +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixSub group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c new file mode 100644 index 0000000..ebfd09d --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_sub_q31.c + * Description: Q31 matrix subtraction + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixSub + * @{ + */ + +/** + * @brief Q31 matrix subtraction. + * @param[in] *pSrcA points to the first input matrix structure + * @param[in] *pSrcB points to the second input matrix structure + * @param[out] *pDst points to output matrix structure + * @return The function returns either + * ARM_MATH_SIZE_MISMATCH or ARM_MATH_SUCCESS based on the outcome of size checking. + * + * Scaling and Overflow Behavior: + * \par + * The function uses saturating arithmetic. + * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated. + */ + + +arm_status arm_mat_sub_q31( + const arm_matrix_instance_q31 * pSrcA, + const arm_matrix_instance_q31 * pSrcB, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */ + q31_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */ + q31_t *pOut = pDst->pData; /* output data matrix pointer */ + q31_t inA1, inB1; /* temporary variables */ + +#if defined (ARM_MATH_DSP) + + q31_t inA2, inB2; /* temporary variables */ + q31_t out1, out2; /* temporary variables */ + +#endif // #if defined (ARM_MATH_DSP) + + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix subtraction */ + + +#ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ + if ((pSrcA->numRows != pSrcB->numRows) || + (pSrcA->numCols != pSrcB->numCols) || + (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif + { + /* Total number of samples in the input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Loop Unrolling */ + blkCnt = numSamples >> 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(m,n) = A(m,n) - B(m,n) */ + /* Subtract, saturate and then store the results in the destination buffer. */ + /* Read values from source A */ + inA1 = pIn1[0]; + + /* Read values from source B */ + inB1 = pIn2[0]; + + /* Read values from source A */ + inA2 = pIn1[1]; + + /* Subtract and saturate */ + out1 = __QSUB(inA1, inB1); + + /* Read values from source B */ + inB2 = pIn2[1]; + + /* Read values from source A */ + inA1 = pIn1[2]; + + /* Subtract and saturate */ + out2 = __QSUB(inA2, inB2); + + /* Read values from source B */ + inB1 = pIn2[2]; + + /* Store result in destination */ + pOut[0] = out1; + pOut[1] = out2; + + /* Read values from source A */ + inA2 = pIn1[3]; + + /* Read values from source B */ + inB2 = pIn2[3]; + + /* Subtract and saturate */ + out1 = __QSUB(inA1, inB1); + + /* Subtract and saturate */ + out2 = __QSUB(inA2, inB2); + + /* Store result in destination */ + pOut[2] = out1; + pOut[3] = out2; + + /* update pointers to process next samples */ + pIn1 += 4U; + pIn2 += 4U; + pOut += 4U; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + + /* Initialize blkCnt with number of samples */ + blkCnt = numSamples; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) - B(m,n) */ + /* Subtract, saturate and then store the results in the destination buffer. */ + inA1 = *pIn1++; + inB1 = *pIn2++; + + inA1 = __QSUB(inA1, inB1); + + *pOut++ = inA1; + + /* Decrement the loop counter */ + blkCnt--; + } + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixSub group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c new file mode 100644 index 0000000..aaedb9d --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c @@ -0,0 +1,206 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_trans_f32.c + * Description: Floating-point matrix transpose + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 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. + */ + +/** + * @defgroup MatrixTrans Matrix Transpose + * + * Tranposes a matrix. + * Transposing an M x N matrix flips it around the center diagonal and results in an N x M matrix. + * \image html MatrixTranspose.gif "Transpose of a 3 x 3 matrix" + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixTrans + * @{ + */ + +/** + * @brief Floating-point matrix transpose. + * @param[in] *pSrc points to the input matrix + * @param[out] *pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + + +arm_status arm_mat_trans_f32( + const arm_matrix_instance_f32 * pSrc, + arm_matrix_instance_f32 * pDst) +{ + float32_t *pIn = pSrc->pData; /* input data matrix pointer */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ + float32_t *px; /* Temporary output data matrix pointer */ + uint16_t nRows = pSrc->numRows; /* number of rows */ + uint16_t nColumns = pSrc->numCols; /* number of columns */ + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + uint16_t blkCnt, i = 0U, row = nRows; /* loop counters */ + arm_status status; /* status of matrix transpose */ + + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Matrix transpose by exchanging the rows with columns */ + /* row loop */ + do + { + /* Loop Unrolling */ + blkCnt = nColumns >> 2; + + /* The pointer px is set to starting address of the column being processed */ + px = pOut + i; + + /* 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) /* column loop */ + { + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Decrement the column loop counter */ + blkCnt--; + } + + /* Perform matrix transpose for last 3 samples here. */ + blkCnt = nColumns % 0x4U; + + while (blkCnt > 0U) + { + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Decrement the column loop counter */ + blkCnt--; + } + +#else + + /* Run the below code for Cortex-M0 */ + + uint16_t col, i = 0U, row = nRows; /* loop counters */ + arm_status status; /* status of matrix transpose */ + + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Matrix transpose by exchanging the rows with columns */ + /* row loop */ + do + { + /* The pointer px is set to starting address of the column being processed */ + px = pOut + i; + + /* Initialize column loop counter */ + col = nColumns; + + while (col > 0U) + { + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Decrement the column loop counter */ + col--; + } + +#endif /* #if defined (ARM_MATH_DSP) */ + + i++; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); /* row loop end */ + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixTrans group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c new file mode 100644 index 0000000..817210c --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c @@ -0,0 +1,272 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_trans_q15.c + * Description: Q15 matrix transpose + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixTrans + * @{ + */ + +/* + * @brief Q15 matrix transpose. + * @param[in] *pSrc points to the input matrix + * @param[out] *pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + +arm_status arm_mat_trans_q15( + const arm_matrix_instance_q15 * pSrc, + arm_matrix_instance_q15 * pDst) +{ + q15_t *pSrcA = pSrc->pData; /* input data matrix pointer */ + q15_t *pOut = pDst->pData; /* output data matrix pointer */ + uint16_t nRows = pSrc->numRows; /* number of nRows */ + uint16_t nColumns = pSrc->numCols; /* number of nColumns */ + uint16_t col, row = nRows, i = 0U; /* row and column loop counters */ + arm_status status; /* status of matrix transpose */ + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ +#ifndef UNALIGNED_SUPPORT_DISABLE + + q31_t in; /* variable to hold temporary output */ + +#else + + q15_t in; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Matrix transpose by exchanging the rows with columns */ + /* row loop */ + do + { + + /* Apply loop unrolling and exchange the columns with row elements */ + col = nColumns >> 2U; + + /* The pointer pOut is set to starting address of the column being processed */ + pOut = pDst->pData + i; + + /* 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 (col > 0U) + { +#ifndef UNALIGNED_SUPPORT_DISABLE + + /* Read two elements from the row */ + in = *__SIMD32(pSrcA)++; + + /* Unpack and store one element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *pOut = (q15_t) in; + +#else + + *pOut = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer pOut to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Unpack and store the second element in the destination */ + +#ifndef ARM_MATH_BIG_ENDIAN + + *pOut = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#else + + *pOut = (q15_t) in; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer pOut to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Read two elements from the row */ +#ifndef ARM_MATH_BIG_ENDIAN + + in = *__SIMD32(pSrcA)++; + +#else + + in = *__SIMD32(pSrcA)++; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Unpack and store one element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *pOut = (q15_t) in; + +#else + + *pOut = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Update the pointer pOut to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Unpack and store the second element in the destination */ +#ifndef ARM_MATH_BIG_ENDIAN + + *pOut = (q15_t) ((in & (q31_t) 0xffff0000) >> 16); + +#else + + *pOut = (q15_t) in; + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + +#else + /* Read one element from the row */ + in = *pSrcA++; + + /* Store one element in the destination */ + *pOut = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Read one element from the row */ + in = *pSrcA++; + + /* Store one element in the destination */ + *pOut = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Read one element from the row */ + in = *pSrcA++; + + /* Store one element in the destination */ + *pOut = in; + + /* Update the pointer px to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Read one element from the row */ + in = *pSrcA++; + + /* Store one element in the destination */ + *pOut = in; + +#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ + + /* Update the pointer pOut to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Decrement the column loop counter */ + col--; + } + + /* Perform matrix transpose for last 3 samples here. */ + col = nColumns % 0x4U; + +#else + + /* Run the below code for Cortex-M0 */ + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Matrix transpose by exchanging the rows with columns */ + /* row loop */ + do + { + /* The pointer pOut is set to starting address of the column being processed */ + pOut = pDst->pData + i; + + /* Initialize column loop counter */ + col = nColumns; + +#endif /* #if defined (ARM_MATH_DSP) */ + + while (col > 0U) + { + /* Read and store the input element in the destination */ + *pOut = *pSrcA++; + + /* Update the pointer pOut to point to the next row of the transposed matrix */ + pOut += nRows; + + /* Decrement the column loop counter */ + col--; + } + + i++; + + /* Decrement the row loop counter */ + row--; + + } while (row > 0U); + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixTrans group + */ diff --git a/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c new file mode 100644 index 0000000..9f94938 --- /dev/null +++ b/Blink/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c @@ -0,0 +1,198 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_mat_trans_q31.c + * Description: Q31 matrix transpose + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_math.h" + +/** + * @ingroup groupMatrix + */ + +/** + * @addtogroup MatrixTrans + * @{ + */ + +/* + * @brief Q31 matrix transpose. + * @param[in] *pSrc points to the input matrix + * @param[out] *pDst points to the output matrix + * @return The function returns either ARM_MATH_SIZE_MISMATCH + * or ARM_MATH_SUCCESS based on the outcome of size checking. + */ + +arm_status arm_mat_trans_q31( + const arm_matrix_instance_q31 * pSrc, + arm_matrix_instance_q31 * pDst) +{ + q31_t *pIn = pSrc->pData; /* input data matrix pointer */ + q31_t *pOut = pDst->pData; /* output data matrix pointer */ + q31_t *px; /* Temporary output data matrix pointer */ + uint16_t nRows = pSrc->numRows; /* number of nRows */ + uint16_t nColumns = pSrc->numCols; /* number of nColumns */ + +#if defined (ARM_MATH_DSP) + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + uint16_t blkCnt, i = 0U, row = nRows; /* loop counters */ + arm_status status; /* status of matrix transpose */ + + +#ifdef ARM_MATH_MATRIX_CHECK + + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Matrix transpose by exchanging the rows with columns */ + /* row loop */ + do + { + /* Apply loop unrolling and exchange the columns with row elements */ + blkCnt = nColumns >> 2U; + + /* The pointer px is set to starting address of the column being processed */ + px = pOut + i; + + /* 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) + { + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Decrement the column loop counter */ + blkCnt--; + } + + /* Perform matrix transpose for last 3 samples here. */ + blkCnt = nColumns % 0x4U; + + while (blkCnt > 0U) + { + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Decrement the column loop counter */ + blkCnt--; + } + +#else + + /* Run the below code for Cortex-M0 */ + + uint16_t col, i = 0U, row = nRows; /* loop counters */ + arm_status status; /* status of matrix transpose */ + + +#ifdef ARM_MATH_MATRIX_CHECK + + /* Check for matrix mismatch condition */ + if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + { + /* Set status as ARM_MATH_SIZE_MISMATCH */ + status = ARM_MATH_SIZE_MISMATCH; + } + else +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + + { + /* Matrix transpose by exchanging the rows with columns */ + /* row loop */ + do + { + /* The pointer px is set to starting address of the column being processed */ + px = pOut + i; + + /* Initialize column loop counter */ + col = nColumns; + + while (col > 0U) + { + /* Read and store the input element in the destination */ + *px = *pIn++; + + /* Update the pointer px to point to the next row of the transposed matrix */ + px += nRows; + + /* Decrement the column loop counter */ + col--; + } + +#endif /* #if defined (ARM_MATH_DSP) */ + + i++; + + /* Decrement the row loop counter */ + row--; + + } + while (row > 0U); /* row loop end */ + + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} + +/** + * @} end of MatrixTrans group + */ -- cgit