diff options
Diffstat (limited to 'DSP/Source/MatrixFunctions')
27 files changed, 3236 insertions, 2534 deletions
diff --git a/DSP/Source/MatrixFunctions/CMakeLists.txt b/DSP/Source/MatrixFunctions/CMakeLists.txt new file mode 100644 index 0000000..d48d6b1 --- /dev/null +++ b/DSP/Source/MatrixFunctions/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required (VERSION 3.6) + +project(CMSISDSPMatrix) + + +file(GLOB SRC "./*_*.c") + +add_library(CMSISDSPMatrix STATIC ${SRC}) + +configdsp(CMSISDSPMatrix ..) + +### Includes +target_include_directories(CMSISDSPMatrix PUBLIC "${DSP}/../../Include") + + + diff --git a/DSP/Source/MatrixFunctions/MatrixFunctions.c b/DSP/Source/MatrixFunctions/MatrixFunctions.c new file mode 100644 index 0000000..da721fe --- /dev/null +++ b/DSP/Source/MatrixFunctions/MatrixFunctions.c @@ -0,0 +1,53 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: MatrixFunctions.c + * Description: Combination of all matrix function source files. + * + * $Date: 18. March 2019 + * $Revision: V1.0.0 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2019 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "arm_mat_add_f32.c" +#include "arm_mat_add_q15.c" +#include "arm_mat_add_q31.c" +#include "arm_mat_cmplx_mult_f32.c" +#include "arm_mat_cmplx_mult_q15.c" +#include "arm_mat_cmplx_mult_q31.c" +#include "arm_mat_init_f32.c" +#include "arm_mat_init_q15.c" +#include "arm_mat_init_q31.c" +#include "arm_mat_inverse_f32.c" +#include "arm_mat_inverse_f64.c" +#include "arm_mat_mult_f32.c" +#include "arm_mat_mult_fast_q15.c" +#include "arm_mat_mult_fast_q31.c" +#include "arm_mat_mult_q15.c" +#include "arm_mat_mult_q31.c" +#include "arm_mat_scale_f32.c" +#include "arm_mat_scale_q15.c" +#include "arm_mat_scale_q31.c" +#include "arm_mat_sub_f32.c" +#include "arm_mat_sub_q15.c" +#include "arm_mat_sub_q31.c" +#include "arm_mat_trans_f32.c" +#include "arm_mat_trans_q15.c" +#include "arm_mat_trans_q31.c" diff --git a/DSP/Source/MatrixFunctions/arm_mat_add_f32.c b/DSP/Source/MatrixFunctions/arm_mat_add_f32.c index 4a54049..8e1246c 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_add_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_add_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_add_f32.c * Description: Floating-point matrix addition * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,35 +29,43 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @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 - * <code>pSrcA</code>, <code>pSrcB</code>, and <code>pDst</code> have the same - * number of rows and columns. + @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 + <code>pSrcA</code>, <code>pSrcB</code>, and <code>pDst</code> have the same + number of rows and columns. */ /** - * @addtogroup MatrixAdd - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. + @brief Floating-point matrix addition. + @param[in] pSrcA points to first input matrix structure + @param[in] pSrcB points to second input matrix structure + @param[out] pDst points to output matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed */ +#if defined(ARM_MATH_NEON) +/* +Neon version is assuming the matrix is small enough. +So no blocking is used for taking into account cache effects. +For big matrix, there exist better libraries for Neon. + +*/ arm_status arm_mat_add_f32( const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, @@ -67,12 +75,8 @@ arm_status arm_mat_add_f32( 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 */ @@ -89,108 +93,140 @@ arm_status arm_mat_add_f32( else #endif { + float32x4_t vec1; + float32x4_t vec2; + float32x4_t res; /* 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. + /* 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]; + vec1 = vld1q_f32(pIn1); + vec2 = vld1q_f32(pIn2); + res = vaddq_f32(vec1, vec2); + vst1q_f32(pOut, res); - /* Read values from source B */ - inB1 = pIn2[0]; + /* update pointers to process next samples */ + pIn1 += 4U; + pIn2 += 4U; + pOut += 4U; + /* Decrement the loop counter */ + blkCnt--; + } - /* Read values from source A */ - inA2 = pIn1[1]; + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; - /* out = sourceA + sourceB */ - out1 = inA1 + inB1; + 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++); - /* Read values from source B */ - inB2 = pIn2[1]; + /* Decrement the loop counter */ + blkCnt--; + } - /* Read values from source A */ - inA1 = pIn1[2]; + /* set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } - /* out = sourceA + sourceB */ - out2 = inA2 + inB2; + /* Return to application */ + return (status); +} +#else +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 *pInA = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ - /* Read values from source B */ - inB1 = pIn2[2]; + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix addition */ - /* Store result in destination */ - pOut[0] = out1; - pOut[1] = out2; +#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 input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; - /* Read values from source A */ - inA2 = pIn1[3]; +#if defined (ARM_MATH_LOOPUNROLL) - /* Read values from source B */ - inB2 = pIn2[3]; + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; - /* out = sourceA + sourceB */ - out1 = inA1 + inB1; + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) + B(m,n) */ - /* out = sourceA + sourceB */ - out2 = inA2 + inB2; + /* Add and store result in destination buffer. */ + *pOut++ = *pInA++ + *pInB++; - /* Store result in destination */ - pOut[2] = out1; + *pOut++ = *pInA++ + *pInB++; - /* Store result in destination */ - pOut[3] = out2; + *pOut++ = *pInA++ + *pInB++; + *pOut++ = *pInA++ + *pInB++; - /* update pointers to process next sampels */ - pIn1 += 4U; - pIn2 += 4U; - pOut += 4U; - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } - /* If the numSamples is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 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 */ + /* Add and store result in destination buffer. */ + *pOut++ = *pInA++ + *pInB++; + + /* Decrement loop counter */ blkCnt--; } - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; - } /* Return to application */ return (status); } +#endif /* #if defined(ARM_MATH_NEON) */ /** - * @} end of MatrixAdd group + @} end of MatrixAdd group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_add_q15.c b/DSP/Source/MatrixFunctions/arm_mat_add_q15.c index 896e60c..2aaf849 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_add_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_add_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_add_q15.c * Description: Q15 matrix addition * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,116 +29,114 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixAdd - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * <b>Scaling and Overflow Behavior:</b> - * \par - * The function uses saturating arithmetic. - * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated. + @brief Q15 matrix addition. + @param[in] pSrcA points to first input matrix structure + @param[in] pSrcB points to second input matrix structure + @param[out] pDst points to output matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated. */ arm_status arm_mat_add_q15( const arm_matrix_instance_q15 * pSrcA, const arm_matrix_instance_q15 * pSrcB, - arm_matrix_instance_q15 * pDst) + 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 */ + 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 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)) + (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); +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ -#if defined (ARM_MATH_DSP) + { + /* Total number of samples in input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; - /* Run the below code for Cortex-M4 and Cortex-M3 */ +#if defined (ARM_MATH_LOOPUNROLL) - /* Loop unrolling */ - blkCnt = (uint32_t) numSamples >> 2U; + /* Loop unrolling: Compute 4 outputs at a time */ + 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. */ - *__SIMD32(pOut)++ = __QADD16(*__SIMD32(pInA)++, *__SIMD32(pInB)++); - *__SIMD32(pOut)++ = __QADD16(*__SIMD32(pInA)++, *__SIMD32(pInB)++); - /* Decrement the loop counter */ - blkCnt--; - } + /* Add, saturate and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + write_q15x2_ia (&pOut, __QADD16(read_q15x2_ia (&pInA), read_q15x2_ia (&pInB))); - /* 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; + write_q15x2_ia (&pOut, __QADD16(read_q15x2_ia (&pInA), read_q15x2_ia (&pInB))); +#else + *pOut++ = (q15_t) __SSAT(((q31_t) *pInA++ + *pInB++), 16); - /* q15 pointers of input and output are initialized */ + *pOut++ = (q15_t) __SSAT(((q31_t) *pInA++ + *pInB++), 16); - 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++); + *pOut++ = (q15_t) __SSAT(((q31_t) *pInA++ + *pInB++), 16); + + *pOut++ = (q15_t) __SSAT(((q31_t) *pInA++ + *pInB++), 16); +#endif - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } -#else + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; - /* Run the below code for Cortex-M0 */ +#else /* Initialize blkCnt with number of samples */ - blkCnt = (uint32_t) numSamples; + blkCnt = numSamples; +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ - /* 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 */ + /* Add, saturate and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + *pOut++ = (q15_t) __QADD16(*pInA++, *pInB++); +#else + *pOut++ = (q15_t) __SSAT(((q31_t) *pInA++ + *pInB++), 16); +#endif + + /* Decrement loop counter */ blkCnt--; } -#endif /* #if defined (ARM_MATH_DSP) */ - - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -147,5 +145,5 @@ arm_status arm_mat_add_q15( } /** - * @} end of MatrixAdd group + @} end of MatrixAdd group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_add_q31.c b/DSP/Source/MatrixFunctions/arm_mat_add_q31.c index f230ad2..6194809 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_add_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_add_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_add_q31.c * Description: Q31 matrix addition * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,160 +29,104 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixAdd - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * <b>Scaling and Overflow Behavior:</b> - * \par - * The function uses saturating arithmetic. - * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated. + @brief Q31 matrix addition. + @param[in] pSrcA points to first input matrix structure + @param[in] pSrcB points to second input matrix structure + @param[out] pDst points to output matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated. */ arm_status arm_mat_add_q31( const arm_matrix_instance_q31 * pSrcA, const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst) + 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 *pInB = 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 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)) + (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 + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { - /* Total number of samples in the input matrix */ + /* Total number of samples in input matrix */ numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; -#if defined (ARM_MATH_DSP) - - /* Run the below code for Cortex-M4 and Cortex-M3 */ +#if defined (ARM_MATH_LOOPUNROLL) - /* Loop Unrolling */ + /* Loop unrolling: Compute 4 outputs at a time */ 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); + /* Add, saturate and store result in destination buffer. */ + *pOut++ = __QADD(*pInA++, *pInB++); - /* Read values from source B */ - inB2 = pIn2[1]; + *pOut++ = __QADD(*pInA++, *pInB++); - /* Read values from source A */ - inA1 = pIn1[2]; + *pOut++ = __QADD(*pInA++, *pInB++); - /* Add and saturate */ - out2 = __QADD(inA2, inB2); + *pOut++ = __QADD(*pInA++, *pInB++); - /* 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 */ + /* Decrement loop counter */ blkCnt--; } - /* If the numSamples is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 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); + /* Add, saturate and store result in destination buffer. */ + *pOut++ = __QADD(*pInA++, *pInB++); - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; - - *pOut++ = inA1; - } - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -191,5 +135,5 @@ arm_status arm_mat_add_q31( } /** - * @} end of MatrixAdd group + @} end of MatrixAdd group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c b/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c index bb8341e..8e2af31 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_cmplx_mult_f32.c * Description: Floating-point matrix multiplication * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,36 +29,38 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @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 <code>M x N</code> matrix with an <code>N x P</code> matrix results - * in an <code>M x P</code> matrix. - * When matrix size checking is enabled, the functions check: (1) that the inner dimensions of - * <code>pSrcA</code> and <code>pSrcB</code> are equal; and (2) that the size of the output - * matrix equals the outer dimensions of <code>pSrcA</code> and <code>pSrcB</code>. + @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 <code>M x N</code> matrix with an <code>N x P</code> matrix results + in an <code>M x P</code> matrix. + @par + When matrix size checking is enabled, the functions check: + - that the inner dimensions of <code>pSrcA</code> and <code>pSrcB</code> are equal; + - that the size of the output matrix equals the outer dimensions of <code>pSrcA</code> and <code>pSrcB</code>. */ /** - * @addtogroup CmplxMatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. + @brief Floating-point Complex matrix multiplication. + @param[in] pSrcA points to first input complex matrix structure + @param[in] pSrcB points to second input complex matrix structure + @param[out] pDst points to output complex matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed */ - +#if defined(ARM_MATH_NEON) arm_status arm_mat_cmplx_mult_f32( const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, @@ -74,14 +76,20 @@ arm_status arm_mat_cmplx_mult_f32( 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 a1, a1B,b1, b1B, c1, d1; float32_t sumReal2, sumImag2; /* accumulator */ - /* Run the below code for Cortex-M4 and Cortex-M3 */ + float32x4x2_t a0V, a1V; + float32x4_t accR0,accI0, accR1,accI1,tempR, tempI; + float32x2_t accum = vdup_n_f32(0); + float32_t *pIn1B = pSrcA->pData; - uint16_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + uint16_t col, i = 0U, j, rowCnt, row = numRowsA, colCnt; /* loop counters */ arm_status status; /* status of matrix multiplication */ + float32_t sumReal1B, sumImag1B; + float32_t sumReal2B, sumImag2B; + float32_t *pxB; #ifdef ARM_MATH_MATRIX_CHECK @@ -99,11 +107,15 @@ arm_status arm_mat_cmplx_mult_f32( { /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ - /* row loop */ - do + + rowCnt = row >> 1; + + /* Row loop */ + while (rowCnt > 0U) { /* Output pointer is set to starting address of the row being processed */ px = pOut + 2 * i; + pxB = px + 2 * numColsB; /* For every row wise process, the column loop counter is to be initiated */ col = numColsB; @@ -114,107 +126,231 @@ arm_status arm_mat_cmplx_mult_f32( j = 0U; - /* column loop */ - do + /* Column loop */ + while (col > 0U) { /* Set the variable sum, that acts as accumulator, to zero */ sumReal1 = 0.0f; sumImag1 = 0.0f; + sumReal1B = 0.0f; + sumImag1B = 0.0f; sumReal2 = 0.0f; sumImag2 = 0.0f; + sumReal2B = 0.0f; + sumImag2B = 0.0f; /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ pIn1 = pInA; + pIn1B = pIn1 + 2*numColsA; - /* Apply loop unrolling and compute 4 MACs simultaneously. */ + accR0 = vdupq_n_f32(0.0); + accI0 = vdupq_n_f32(0.0); + accR1 = vdupq_n_f32(0.0); + accI1 = vdupq_n_f32(0.0); + + /* Compute 4 MACs simultaneously. */ colCnt = numColsA >> 2; - /* matrix multiplication */ + /* Matrix multiplication */ while (colCnt > 0U) { - /* Reading real part of complex matrix A */ - a0 = *pIn1; + a0V = vld2q_f32(pIn1); // load & separate real/imag pSrcA (de-interleave 2) + a1V = vld2q_f32(pIn1B); // load & separate real/imag pSrcA (de-interleave 2) - /* Reading real part of complex matrix B */ - c0 = *pIn2; + pIn1 += 8; + pIn1B += 8; - /* Reading imaginary part of complex matrix A */ - b0 = *(pIn1 + 1U); + tempR[0] = *pIn2; + tempI[0] = *(pIn2 + 1U); + pIn2 += 2 * numColsB; - /* Reading imaginary part of complex matrix B */ - d0 = *(pIn2 + 1U); + tempR[1] = *pIn2; + tempI[1] = *(pIn2 + 1U); + pIn2 += 2 * numColsB; - sumReal1 += a0 * c0; - sumImag1 += b0 * c0; + tempR[2] = *pIn2; + tempI[2] = *(pIn2 + 1U); + pIn2 += 2 * numColsB; - pIn1 += 2U; + tempR[3] = *pIn2; + tempI[3] = *(pIn2 + 1U); pIn2 += 2 * numColsB; - sumReal2 -= b0 * d0; - sumImag2 += a0 * d0; + accR0 = vmlaq_f32(accR0,a0V.val[0],tempR); + accR0 = vmlsq_f32(accR0,a0V.val[1],tempI); + + accI0 = vmlaq_f32(accI0,a0V.val[1],tempR); + accI0 = vmlaq_f32(accI0,a0V.val[0],tempI); + + accR1 = vmlaq_f32(accR1,a1V.val[0],tempR); + accR1 = vmlsq_f32(accR1,a1V.val[1],tempI); + + accI1 = vmlaq_f32(accI1,a1V.val[1],tempR); + accI1 = vmlaq_f32(accI1,a1V.val[0],tempI); + + /* Decrement the loop count */ + colCnt--; + } + + accum = vpadd_f32(vget_low_f32(accR0), vget_high_f32(accR0)); + sumReal1 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(accI0), vget_high_f32(accI0)); + sumImag1 += accum[0] + accum[1]; - /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + accum = vpadd_f32(vget_low_f32(accR1), vget_high_f32(accR1)); + sumReal1B += accum[0] + accum[1]; + accum = vpadd_f32(vget_low_f32(accI1), vget_high_f32(accI1)); + sumImag1B += accum[0] + accum[1]; + + /* If the columns of pSrcA is not a multiple of 4, compute any remaining MACs here. + ** No loop unrolling is used. */ + colCnt = numColsA & 3; + + 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; + a1B = *pIn1B; + c1 = *pIn2; b1 = *(pIn1 + 1U); + b1B = *(pIn1B + 1U); + d1 = *(pIn2 + 1U); sumReal1 += a1 * c1; sumImag1 += b1 * c1; + sumReal1B += a1B * c1; + sumImag1B += b1B * c1; + pIn1 += 2U; + pIn1B += 2U; pIn2 += 2 * numColsB; sumReal2 -= b1 * d1; sumImag2 += a1 * d1; - a0 = *pIn1; - c0 = *pIn2; + sumReal2B -= b1B * d1; + sumImag2B += a1B * d1; - b0 = *(pIn1 + 1U); - d0 = *(pIn2 + 1U); + /* Decrement the loop counter */ + colCnt--; + } - sumReal1 += a0 * c0; - sumImag1 += b0 * c0; + sumReal1 += sumReal2; + sumImag1 += sumImag2; - pIn1 += 2U; - pIn2 += 2 * numColsB; + sumReal1B += sumReal2B; + sumImag1B += sumImag2B; - sumReal2 -= b0 * d0; - sumImag2 += a0 * d0; + /* Store the result in the destination buffer */ + *px++ = sumReal1; + *px++ = sumImag1; + *pxB++ = sumReal1B; + *pxB++ = sumImag1B; - /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* Update the pointer pIn2 to point to the starting address of the next column */ + j++; + pIn2 = pSrcB->pData + 2U * j; - a1 = *pIn1; - c1 = *pIn2; + /* Decrement the column loop counter */ + col--; + } - b1 = *(pIn1 + 1U); - d1 = *(pIn2 + 1U); + /* Update the pointer pInA to point to the starting address of the next 2 row */ + i = i + 2*numColsB; + pInA = pInA + 4 * numColsA; - sumReal1 += a1 * c1; - sumImag1 += b1 * c1; + /* Decrement the row loop counter */ + rowCnt--; + } - pIn1 += 2U; + rowCnt = row & 1; + while (rowCnt > 0U) + { + /* 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 */ + while (col > 0U) + { + /* 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; + + accR0 = vdupq_n_f32(0.0); + accI0 = vdupq_n_f32(0.0); + + /* Compute 4 MACs simultaneously. */ + colCnt = numColsA >> 2; + + /* Matrix multiplication */ + while (colCnt > 0U) + { + /* Reading real part of complex matrix A */ + a0V = vld2q_f32(pIn1); // load & separate real/imag pSrcA (de-interleave 2) + pIn1 += 8; + + tempR[0] = *pIn2; + tempI[0] = *(pIn2 + 1U); pIn2 += 2 * numColsB; - sumReal2 -= b1 * d1; - sumImag2 += a1 * d1; + tempR[1] = *pIn2; + tempI[1] = *(pIn2 + 1U); + pIn2 += 2 * numColsB; + + tempR[2] = *pIn2; + tempI[2] = *(pIn2 + 1U); + pIn2 += 2 * numColsB; + + tempR[3] = *pIn2; + tempI[3] = *(pIn2 + 1U); + pIn2 += 2 * numColsB; + + accR0 = vmlaq_f32(accR0,a0V.val[0],tempR); + accR0 = vmlsq_f32(accR0,a0V.val[1],tempI); + + accI0 = vmlaq_f32(accI0,a0V.val[1],tempR); + accI0 = vmlaq_f32(accI0,a0V.val[0],tempI); /* Decrement the loop count */ colCnt--; } + accum = vpadd_f32(vget_low_f32(accR0), vget_high_f32(accR0)); + sumReal1 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(accI0), vget_high_f32(accI0)); + sumImag1 += accum[0] + accum[1]; + /* If the columns of pSrcA is not a multiple of 4, compute any remaining MACs here. ** No loop unrolling is used. */ - colCnt = numColsA % 0x4U; + colCnt = numColsA & 3; while (colCnt > 0U) { - /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* 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; @@ -248,13 +384,234 @@ arm_status arm_mat_cmplx_mult_f32( /* 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 */ + rowCnt--; + + } + + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} +#else +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 sumReal, sumImag; /* Accumulator */ + float32_t a1, b1, c1, d1; + uint32_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ + arm_status status; /* status of matrix multiplication */ + +#if defined (ARM_MATH_LOOPUNROLL) + float32_t a0, b0, c0, d0; +#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 */ + + { + /* 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 */ + sumReal = 0.0f; + sumImag = 0.0f; + + /* Initiate pointer pIn1 to point to starting address of column being processed */ + pIn1 = pInA; + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Apply loop unrolling and compute 4 MACs simultaneously. */ + colCnt = numColsA >> 2U; + + /* 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 */ + sumReal += a0 * c0; + sumImag += b0 * c0; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal -= b0 * d0; + sumImag += 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 */ + sumReal += a1 * c1; + sumImag += b1 * c1; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal -= b1 * d1; + sumImag += a1 * d1; + + a0 = *(pIn1 ); + c0 = *(pIn2 ); + b0 = *(pIn1 + 1U); + d0 = *(pIn2 + 1U); + + /* Multiply and Accumlates */ + sumReal += a0 * c0; + sumImag += b0 * c0; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal -= b0 * d0; + sumImag += 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 */ + sumReal += a1 * c1; + sumImag += b1 * c1; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal -= b1 * d1; + sumImag += a1 * d1; + + /* Decrement 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; + +#else + + /* Initialize blkCnt with number of samples */ + colCnt = numColsA; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + 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 */ + sumReal += a1 * c1; + sumImag += b1 * c1; + + /* update pointers */ + pIn1 += 2U; + pIn2 += 2 * numColsB; + + /* Multiply and Accumlates */ + sumReal -= b1 * d1; + sumImag += a1 * d1; + + /* Decrement loop counter */ + colCnt--; + } + + /* Store result in destination buffer */ + *px++ = sumReal; + *px++ = sumImag; + + /* Update pointer pIn2 to point to starting address of next column */ + j++; + pIn2 = pSrcB->pData + 2U * j; + + /* Decrement column loop counter */ + col--; + + } while (col > 0U); + + /* Update pointer pInA to point to starting address of next row */ + i = i + numColsB; + pInA = pInA + 2 * numColsA; + + /* Decrement row loop counter */ row--; } while (row > 0U); @@ -267,6 +624,8 @@ arm_status arm_mat_cmplx_mult_f32( return (status); } +#endif /* #if defined(ARM_MATH_NEON) */ + /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c b/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c index 5dee79c..4c5a45b 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q15.c @@ -3,13 +3,13 @@ * Title: arm_cmplx_mat_mult_q15.c * Description: Q15 complex matrix multiplication * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,141 +29,115 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup CmplxMatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> 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 - * <b>Scaling and Overflow Behavior:</b> - * - * \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 <code>arm_mat_mult_fast_q15()</code> for a faster but less precise version of this function. - * + @brief Q15 Complex matrix multiplication. + @param[in] pSrcA points to first input complex matrix structure + @param[in] pSrcB points to second input complex matrix structure + @param[out] pDst points to output complex matrix structure + @param[in] pScratch points to an array for storing intermediate results + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Conditions for optimum performance + Input, output and state buffers should be aligned by 32-bit + + @par Scaling and Overflow Behavior + The function is implemented using an internal 64-bit 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. */ - - - 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) + 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; + 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 */ + q63_t sumReal, sumImag; /* accumulator */ + uint32_t col, i = 0U, row = numRowsB, colCnt; /* Loop counters */ + arm_status status; /* Status of matrix multiplication */ + +#if defined (ARM_MATH_DSP) + q31_t prod1, prod2; + q31_t pSourceA, pSourceB; #else - q31_t in; /* Temporary variable to hold the input value */ - q31_t prod1, prod2; - q31_t pSourceA, pSourceB; -#endif + q15_t a, b, c, d; +#endif /* #if defined (ARM_MATH_DSP) */ #ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ if ((pSrcA->numCols != pSrcB->numRows) || - (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + (pSrcA->numRows != pDst->numRows) || + (pSrcB->numCols != pDst->numCols) ) { /* Set status as ARM_MATH_SIZE_MISMATCH */ status = ARM_MATH_SIZE_MISMATCH; } else -#endif + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { /* Matrix transpose */ do { + /* The pointer px is set to starting address of column being processed */ + px = pSrcBT + i; + +#if defined (ARM_MATH_LOOPUNROLL) + /* 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. */ + 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 */ + /* Read two elements from row */ + write_q15x2 (px, read_q15x2_ia (&pInB)); + + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB * 2; - /* Read two elements from the row */ - in = *pInB++; - *px = in; - in = *pInB++; - px[1] = in; + /* Read two elements from row */ + write_q15x2 (px, read_q15x2_ia (&pInB)); - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB * 2; - /* Read two elements from the row */ - in = *pInB++; - *px = in; - in = *pInB++; - px[1] = in; + /* Read two elements from row */ + write_q15x2 (px, read_q15x2_ia (&pInB)); - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB * 2; - /* Read two elements from the row */ - in = *pInB++; - *px = in; - in = *pInB++; - px[1] = in; + /* Read two elements from row */ + write_q15x2 (px, read_q15x2_ia (&pInB)); - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB * 2; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } @@ -171,79 +145,33 @@ arm_status arm_mat_cmplx_mult_q15( ** 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--; - } + /* Initialize blkCnt with number of samples */ + col = numColsB; - /* 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; +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ while (col > 0U) { - /* Read two elements from the row */ - in = *__SIMD32(pInB)++; - - *__SIMD32(px) = in; -#endif + /* Read two elements from row */ + write_q15x2 (px, read_q15x2_ia (&pInB)); - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB * 2; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } i = i + 2U; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); - /* Reset the variables for the usage in the following multiplication process */ + /* Reset variables for usage in following multiplication process */ row = numRowsA; i = 0U; px = pDst->pData; @@ -252,33 +180,61 @@ arm_status arm_mat_cmplx_mult_q15( /* row loop */ do { - /* For every row wise process, the column loop counter is to be initiated */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of transposed pSrcB data */ pInB = pSrcBT; /* column loop */ do { - /* Set the variable sum, that acts as accumulator, to zero */ + /* Set 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 */ + /* Initiate pointer pInA to point to starting address of column being processed */ pInA = pSrcA->pData + i * 2; + /* Apply loop unrolling and compute 2 MACs simultaneously. */ + colCnt = numColsA >> 1U; /* 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) */ + /* c(m,n) = a(1,1) * b(1,1) + a(1,2) * b(2,1) + .... + a(m,p) * b(p,n) */ + +#if defined (ARM_MATH_DSP) -#ifdef UNALIGNED_SUPPORT_DISABLE + /* read real and imag values from pSrcA and pSrcB buffer */ + pSourceA = read_q15x2_ia ((q15_t **) &pInA); + pSourceB = read_q15x2_ia ((q15_t **) &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 = read_q15x2_ia ((q15_t **) &pInA); + pSourceB = read_q15x2_ia ((q15_t **) &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; + +#else /* #if defined (ARM_MATH_DSP) */ /* read real and imag values from pSrcA buffer */ a = *pInA; @@ -304,30 +260,28 @@ arm_status arm_mat_cmplx_mult_q15( 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; + 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; +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Decrement 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) */ + +#if defined (ARM_MATH_DSP) /* read real and imag values from pSrcA and pSrcB buffer */ - pSourceA = *__SIMD32(pInA)++; - pSourceB = *__SIMD32(pInB)++; + pSourceA = read_q15x2_ia ((q15_t **) &pInA); + pSourceB = read_q15x2_ia ((q15_t **) &pInB); /* Multiply and Accumlates */ #ifdef ARM_MATH_BIG_ENDIAN @@ -339,18 +293,7 @@ arm_status arm_mat_cmplx_mult_q15( 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 +#else /* #if defined (ARM_MATH_DSP) */ /* read real and imag values from pSrcA and pSrcB buffer */ a = *pInA++; @@ -359,48 +302,32 @@ arm_status arm_mat_cmplx_mult_q15( 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; + sumReal += (q31_t) a * c; + sumImag += (q31_t) a * d; + sumReal -= (q31_t) b * d; + sumImag += (q31_t) b * c; -#endif /* #ifdef UNALIGNED_SUPPORT_DISABLE */ +#endif /* #if defined (ARM_MATH_DSP) */ } - /* Saturate and store the result in the destination buffer */ - + /* Saturate and store result in destination buffer */ *px++ = (q15_t) (__SSAT(sumReal >> 15, 16)); *px++ = (q15_t) (__SSAT(sumImag >> 15, 16)); - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } while (col > 0U); i = i + numColsA; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -409,5 +336,5 @@ arm_status arm_mat_cmplx_mult_q15( } /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c b/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c index 65cbb66..7b458f9 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_cmplx_mult_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_cmplx_mult_q31.c * Description: Floating-point matrix multiplication * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,74 +29,69 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup CmplxMatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * - * \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. - * - * + @brief Q31 Complex matrix multiplication. + @param[in] pSrcA points to first input complex matrix structure + @param[in] pSrcB points to second input complex matrix structure + @param[out] pDst points to output complex matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function is implemented using an internal 64-bit accumulator. + The accumulator has a 2.62 format and maintains full precision of the intermediate + multiplication results but provides only a single guard bit. There is no saturation + on intermediate additions. Thus, if the accumulator overflows it wraps around and + distorts the result. The input signals should be scaled down to avoid intermediate + overflows. 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) + 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 *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; + 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 sumReal, sumImag; /* Accumulator */ 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 */ + uint32_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */ arm_status status; /* status of matrix multiplication */ -#ifdef ARM_MATH_MATRIX_CHECK +#if defined (ARM_MATH_LOOPUNROLL) + q31_t a0, b0, c0, d0; +#endif +#ifdef ARM_MATH_MATRIX_CHECK /* Check for matrix mismatch condition */ if ((pSrcA->numCols != pSrcB->numRows) || - (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + (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 */ + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */ @@ -119,16 +114,18 @@ arm_status arm_mat_cmplx_mult_q31( do { /* Set the variable sum, that acts as accumulator, to zero */ - sumReal1 = 0.0; - sumImag1 = 0.0; + sumReal = 0.0; + sumImag = 0.0; - /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + /* Initiate pointer pIn1 to point to starting address of column being processed */ pIn1 = pInA; +#if defined (ARM_MATH_LOOPUNROLL) + /* Apply loop unrolling and compute 4 MACs simultaneously. */ - colCnt = numColsA >> 2; + colCnt = numColsA >> 2U; - /* matrix multiplication */ + /* matrix multiplication */ while (colCnt > 0U) { @@ -145,76 +142,74 @@ arm_status arm_mat_cmplx_mult_q31( d0 = *(pIn2 + 1U); /* Multiply and Accumlates */ - sumReal1 += (q63_t) a0 *c0; - sumImag1 += (q63_t) b0 *c0; + sumReal += (q63_t) a0 * c0; + sumImag += (q63_t) b0 * c0; /* update pointers */ pIn1 += 2U; pIn2 += 2 * numColsB; /* Multiply and Accumlates */ - sumReal1 -= (q63_t) b0 *d0; - sumImag1 += (q63_t) a0 *d0; + sumReal -= (q63_t) b0 * d0; + sumImag += (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) */ + /* 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; + a1 = *(pIn1 ); + c1 = *(pIn2 ); b1 = *(pIn1 + 1U); d1 = *(pIn2 + 1U); /* Multiply and Accumlates */ - sumReal1 += (q63_t) a1 *c1; - sumImag1 += (q63_t) b1 *c1; + sumReal += (q63_t) a1 * c1; + sumImag += (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; + sumReal -= (q63_t) b1 * d1; + sumImag += (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; + sumReal += (q63_t) a0 * c0; + sumImag += (q63_t) b0 * c0; /* update pointers */ pIn1 += 2U; pIn2 += 2 * numColsB; /* Multiply and Accumlates */ - sumReal1 -= (q63_t) b0 *d0; - sumImag1 += (q63_t) a0 *d0; + sumReal -= (q63_t) b0 * d0; + sumImag += (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; + /* 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; + sumReal += (q63_t) a1 * c1; + sumImag += (q63_t) b1 * c1; /* update pointers */ pIn1 += 2U; pIn2 += 2 * numColsB; /* Multiply and Accumlates */ - sumReal1 -= (q63_t) b1 *d1; - sumImag1 += (q63_t) a1 *d1; + sumReal -= (q63_t) b1 * d1; + sumImag += (q63_t) a1 * d1; - /* Decrement the loop count */ + /* Decrement loop count */ colCnt--; } @@ -222,49 +217,55 @@ arm_status arm_mat_cmplx_mult_q31( ** No loop unrolling is used. */ colCnt = numColsA % 0x4U; +#else + + /* Initialize blkCnt with number of samples */ + colCnt = numColsA; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + 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; - + /* 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; + sumReal += (q63_t) a1 * c1; + sumImag += (q63_t) b1 * c1; /* update pointers */ pIn1 += 2U; pIn2 += 2 * numColsB; /* Multiply and Accumlates */ - sumReal1 -= (q63_t) b1 *d1; - sumImag1 += (q63_t) a1 *d1; + sumReal -= (q63_t) b1 * d1; + sumImag += (q63_t) a1 * d1; - /* Decrement the loop counter */ + /* Decrement 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); + /* Store result in destination buffer */ + *px++ = (q31_t) clip_q63_to_q31(sumReal >> 31); + *px++ = (q31_t) clip_q63_to_q31(sumImag >> 31); - /* Update the pointer pIn2 to point to the starting address of the next column */ + /* Update pointer pIn2 to point to starting address of next column */ j++; pIn2 = pSrcB->pData + 2U * j; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } while (col > 0U); - /* Update the pointer pInA to point to the starting address of the next row */ + /* Update pointer pInA to point to starting address of next row */ i = i + numColsB; pInA = pInA + 2 * numColsA; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); @@ -278,5 +279,5 @@ arm_status arm_mat_cmplx_mult_q31( } /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_init_f32.c b/DSP/Source/MatrixFunctions/arm_mat_init_f32.c index 783f7be..ce02a25 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_init_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_init_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_init_f32.c * Description: Floating-point matrix initialization * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,31 +29,31 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @defgroup MatrixInit Matrix Initialization - * - * Initializes the underlying matrix data structure. - * The functions set the <code>numRows</code>, - * <code>numCols</code>, and <code>pData</code> fields - * of the matrix data structure. + @defgroup MatrixInit Matrix Initialization + + Initializes the underlying matrix data structure. + The functions set the <code>numRows</code>, + <code>numCols</code>, and <code>pData</code> fields + of the matrix data structure. */ /** - * @addtogroup MatrixInit - * @{ + @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 - */ + @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, @@ -72,5 +72,5 @@ void arm_mat_init_f32( } /** - * @} end of MatrixInit group + @} end of MatrixInit group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_init_q15.c b/DSP/Source/MatrixFunctions/arm_mat_init_q15.c index 08da19f..0275503 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_init_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_init_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_init_q15.c * Description: Q15 matrix initialization * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,22 +29,22 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixInit - * @{ + @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 - */ +/** + @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, @@ -63,5 +63,5 @@ void arm_mat_init_q15( } /** - * @} end of MatrixInit group + @} end of MatrixInit group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_init_q31.c b/DSP/Source/MatrixFunctions/arm_mat_init_q31.c index 22e6f6d..d5c5722 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_init_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_init_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_init_q31.c * Description: Q31 matrix initialization * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,27 +29,27 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @defgroup MatrixInit Matrix Initialization - * + @defgroup MatrixInit Matrix Initialization + */ /** - * @addtogroup MatrixInit - * @{ + @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 - */ +/** + @brief Q31 matrix initialization. + @param[in,out] S points to an instance of the Q31 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, @@ -68,5 +68,5 @@ void arm_mat_init_q31( } /** - * @} end of MatrixInit group + @} end of MatrixInit group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c b/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c index b82373a..d602b98 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_inverse_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_inverse_f32.c * Description: Floating-point matrix inverse * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,47 +29,45 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @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 - * <code>ARM_MATH_SINGULAR</code>. - * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method" + @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 + <code>ARM_MATH_SINGULAR</code>. + \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method" */ /** - * @addtogroup MatrixInv - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> 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 - * <code>ARM_MATH_SINGULAR</code>. Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>. + @brief Floating-point matrix inverse. + @param[in] pSrc points to input matrix structure + @param[out] pDst points to output matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + - \ref ARM_MATH_SINGULAR : Input matrix is found to be singular (non-invertible) */ - +#if defined(ARM_MATH_NEON) arm_status arm_mat_inverse_f32( const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst) @@ -82,18 +80,17 @@ arm_status arm_mat_inverse_f32( 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 */ + float32x4_t vec1; + float32x4_t vec2; + float32x4_t tmpV; #ifdef ARM_MATH_MATRIX_CHECK - /* Check for matrix mismatch condition */ if ((pSrc->numRows != pSrc->numCols) || (pDst->numRows != pDst->numCols) || (pSrc->numRows != pDst->numRows)) @@ -105,42 +102,41 @@ arm_status arm_mat_inverse_f32( #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). - *----------------------------------------------------------------------------------------------------------------*/ + /*-------------------------------------------------------------------------------------------------------------- + * 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; @@ -164,6 +160,7 @@ arm_status arm_mat_inverse_f32( /* Writing all zeroes in upper triangle of the destination matrix */ j = rowCnt - 1U; + while (j > 0U) { *pOutT1++ = 0.0f; @@ -201,6 +198,7 @@ arm_status arm_mat_inverse_f32( /* 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); @@ -213,7 +211,7 @@ arm_status arm_mat_inverse_f32( return ARM_MATH_SINGULAR; } - /* Restore pInT1 */ + /* Restore pInT1 */ pInT1 = pIn; /* Destination pointer modifier */ @@ -295,10 +293,28 @@ arm_status arm_mat_inverse_f32( /* Pivot element of the row */ in = *pPivotRowIn; + tmpV = vdupq_n_f32(1.0/in); /* Loop over number of columns * to the right of the pilot element */ - j = (numCols - l); + j = (numCols - l) >> 2; + + while (j > 0U) + { + /* Divide each element of the row of the input matrix + * by the pivot element */ + vec1 = vld1q_f32(pInT1); + + vec1 = vmulq_f32(vec1, tmpV); + vst1q_f32(pInT1, vec1); + pInT1 += 4; + + /* Decrement the loop counter */ + j--; + } + + /* Tail */ + j = (numCols - l) & 3; while (j > 0U) { @@ -312,7 +328,24 @@ arm_status arm_mat_inverse_f32( } /* Loop over number of columns of the destination matrix */ - j = numCols; + j = numCols >> 2; + + while (j > 0U) + { + /* Divide each element of the row of the destination matrix + * by the pivot element */ + vec1 = vld1q_f32(pInT2); + + vec1 = vmulq_f32(vec1, tmpV); + vst1q_f32(pInT2, vec1); + pInT2 += 4; + + /* Decrement the loop counter */ + j--; + } + + /* Tail */ + j = numCols & 3; while (j > 0U) { @@ -354,6 +387,7 @@ arm_status arm_mat_inverse_f32( { /* Element of the reference row */ in = *pInT1; + tmpV = vdupq_n_f32(in); /* Working pointers for input and destination pivot rows */ pPRT_in = pPivotRowIn; @@ -361,7 +395,25 @@ arm_status arm_mat_inverse_f32( /* Loop over the number of columns to the right of the pivot element, to replace the elements in the input matrix */ - j = (numCols - l); + j = (numCols - l) >> 2; + + while (j > 0U) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + vec1 = vld1q_f32(pInT1); + vec2 = vld1q_f32(pPRT_in); + vec1 = vmlsq_f32(vec1, tmpV, vec2); + vst1q_f32(pInT1, vec1); + pPRT_in += 4; + pInT1 += 4; + + /* Decrement the loop counter */ + j--; + } + + /* Tail */ + j = (numCols - l) & 3; while (j > 0U) { @@ -376,7 +428,25 @@ arm_status arm_mat_inverse_f32( /* Loop over the number of columns to replace the elements in the destination matrix */ - j = numCols; + j = numCols >> 2; + + while (j > 0U) + { + /* Replace the element by the sum of that row + and a multiple of the reference row */ + vec1 = vld1q_f32(pInT2); + vec2 = vld1q_f32(pPRT_pDst); + vec1 = vmlsq_f32(vec1, tmpV, vec2); + vst1q_f32(pInT2, vec1); + pPRT_pDst += 4; + pInT2 += 4; + + /* Decrement the loop counter */ + j--; + } + + /* Tail */ + j = numCols & 3; while (j > 0U) { @@ -411,62 +481,96 @@ arm_status arm_mat_inverse_f32( l++; } + /* 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); +} #else +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 */ - /* Run the below code for Cortex-M0 */ +#if defined (ARM_MATH_DSP) + float32_t maxC; /* maximum value in the column */ - float32_t Xchg, in = 0.0f; /* Temporary input values */ + 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)) + 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 */ + +#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). - *----------------------------------------------------------------------------------------------------------------*/ + * 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; @@ -496,7 +600,334 @@ arm_status arm_mat_inverse_f32( j--; } + /* Decrement 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 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 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 loop counter */ + j--; + } + + } + + /* Increment temporary input pointer */ + pInT1 = pInT1 + l; + + /* Decrement loop counter */ + k--; + + /* Increment pivot index */ + i++; + } + + /* Increment the input pointer */ + pIn++; + /* Decrement the loop counter */ + loopCnt--; + + /* Increment the index modifier */ + l++; + } + + +#else + + 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 loop counter */ rowCnt--; } @@ -506,7 +937,7 @@ arm_status arm_mat_inverse_f32( /* 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.. @@ -640,6 +1071,7 @@ arm_status arm_mat_inverse_f32( *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++) @@ -651,19 +1083,21 @@ arm_status arm_mat_inverse_f32( } } - /* Increment the temporary input pointer */ + + /* Increment 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 */ @@ -682,10 +1116,12 @@ arm_status arm_mat_inverse_f32( status = ARM_MATH_SINGULAR; } } + /* Return to application */ return (status); } +#endif /* #if defined(ARM_MATH_NEON) */ /** - * @} end of MatrixInv group + @} end of MatrixInv group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c b/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c index 54e5982..4607e07 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c +++ b/DSP/Source/MatrixFunctions/arm_mat_inverse_f64.c @@ -3,13 +3,13 @@ * Title: arm_mat_inverse_f64.c * Description: Floating-point matrix inverse * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,50 +29,28 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @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 - * <code>ARM_MATH_SINGULAR</code>. - * \image html MatrixInverse.gif "Matrix Inverse of a 3 x 3 matrix using Gauss-Jordan Method" - */ /** - * @addtogroup MatrixInv - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> 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 - * <code>ARM_MATH_SINGULAR</code>. Otherwise, the function returns <code>ARM_MATH_SUCCESS</code>. + @brief Floating-point (64 bit) matrix inverse. + @param[in] pSrc points to input matrix structure + @param[out] pDst points to output matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + - \ref ARM_MATH_SINGULAR : Input matrix is found to be singular (non-invertible) */ arm_status arm_mat_inverse_f64( const arm_matrix_instance_f64 * pSrc, - arm_matrix_instance_f64 * pDst) + arm_matrix_instance_f64 * pDst) { float64_t *pIn = pSrc->pData; /* input data matrix pointer */ float64_t *pOut = pDst->pData; /* output data matrix pointer */ @@ -85,62 +63,61 @@ arm_status arm_mat_inverse_f64( #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 */ + float64_t Xchg, in = 0.0, 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)) + 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 */ + +#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). - *----------------------------------------------------------------------------------------------------------------*/ + * 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; @@ -155,22 +132,22 @@ arm_status arm_mat_inverse_f64( j = numRows - rowCnt; while (j > 0U) { - *pOutT1++ = 0.0f; + *pOutT1++ = 0.0; j--; } /* Writing all ones in the diagonal of the destination matrix */ - *pOutT1++ = 1.0f; + *pOutT1++ = 1.0; /* Writing all zeroes in upper triangle of the destination matrix */ j = rowCnt - 1U; while (j > 0U) { - *pOutT1++ = 0.0f; + *pOutT1++ = 0.0; j--; } - /* Decrement the loop counter */ + /* Decrement loop counter */ rowCnt--; } @@ -208,7 +185,7 @@ arm_status arm_mat_inverse_f64( } /* Update the status if the matrix is singular */ - if (maxC == 0.0f) + if (maxC == 0.0) { return ARM_MATH_SINGULAR; } @@ -220,7 +197,7 @@ arm_status arm_mat_inverse_f64( k = 1U; /* Check if the pivot element is the most significant of the column */ - if ( (in > 0.0f ? in : -in) != maxC) + if ( (in > 0.0 ? in : -in) != maxC) { /* Loop over the number rows present below */ i = numRows - (l + 1U); @@ -233,7 +210,7 @@ arm_status arm_mat_inverse_f64( /* Look for the most significant element to * replace in the rows below */ - if ((*pInT2 > 0.0f ? *pInT2: -*pInT2) == maxC) + if ((*pInT2 > 0.0 ? *pInT2: -*pInT2) == maxC) { /* Loop over number of columns * to the right of the pilot element */ @@ -260,7 +237,7 @@ arm_status arm_mat_inverse_f64( *pOutT2++ = *pOutT1; *pOutT1++ = Xchg; - /* Decrement the loop counter */ + /* Decrement loop counter */ j--; } @@ -274,13 +251,13 @@ arm_status arm_mat_inverse_f64( /* Update the destination pointer modifier */ k++; - /* Decrement the loop counter */ + /* Decrement loop counter */ i--; } } /* Update the status if the matrix is singular */ - if ((flag != 1U) && (in == 0.0f)) + if ((flag != 1U) && (in == 0.0)) { return ARM_MATH_SINGULAR; } @@ -385,19 +362,19 @@ arm_status arm_mat_inverse_f64( in1 = *pInT2; *pInT2++ = in1 - (in * *pPRT_pDst++); - /* Decrement the loop counter */ + /* Decrement loop counter */ j--; } } - /* Increment the temporary input pointer */ + /* Increment temporary input pointer */ pInT1 = pInT1 + l; - /* Decrement the loop counter */ + /* Decrement loop counter */ k--; - /* Increment the pivot index */ + /* Increment pivot index */ i++; } @@ -414,59 +391,60 @@ arm_status arm_mat_inverse_f64( #else - /* Run the below code for Cortex-M0 */ - - float64_t Xchg, in = 0.0f; /* Temporary input values */ + float64_t Xchg, in = 0.0; /* 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)) + 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 */ + +#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). - *----------------------------------------------------------------------------------------------------------------*/ + * 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; @@ -481,22 +459,22 @@ arm_status arm_mat_inverse_f64( j = numRows - rowCnt; while (j > 0U) { - *pOutT1++ = 0.0f; + *pOutT1++ = 0.0; j--; } /* Writing all ones in the diagonal of the destination matrix */ - *pOutT1++ = 1.0f; + *pOutT1++ = 1.0; /* Writing all zeroes in upper triangle of the destination matrix */ j = rowCnt - 1U; while (j > 0U) { - *pOutT1++ = 0.0f; + *pOutT1++ = 0.0; j--; } - /* Decrement the loop counter */ + /* Decrement loop counter */ rowCnt--; } @@ -506,7 +484,7 @@ arm_status arm_mat_inverse_f64( /* 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.. @@ -529,7 +507,7 @@ arm_status arm_mat_inverse_f64( k = 1U; /* Check if the pivot element is zero */ - if (*pInT1 == 0.0f) + if (*pInT1 == 0.0) { /* Loop over the number rows present below */ for (i = (l + 1U); i < numRows; i++) @@ -540,7 +518,7 @@ arm_status arm_mat_inverse_f64( /* Check if there is a non zero pivot element to * replace in the rows below */ - if (*pInT2 != 0.0f) + if (*pInT2 != 0.0) { /* Loop over number of columns * to the right of the pilot element */ @@ -572,7 +550,7 @@ arm_status arm_mat_inverse_f64( } /* Update the status if the matrix is singular */ - if ((flag != 1U) && (in == 0.0f)) + if ((flag != 1U) && (in == 0.0)) { return ARM_MATH_SINGULAR; } @@ -640,6 +618,7 @@ arm_status arm_mat_inverse_f64( *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++) @@ -651,30 +630,32 @@ arm_status arm_mat_inverse_f64( } } - /* Increment the temporary input pointer */ + + /* Increment 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)) + if ((flag != 1U) && (in == 0.0)) { pIn = pSrc->pData; for (i = 0; i < numRows * numCols; i++) { - if (pIn[i] != 0.0f) + if (pIn[i] != 0.0) break; } @@ -682,10 +663,11 @@ arm_status arm_mat_inverse_f64( status = ARM_MATH_SINGULAR; } } + /* Return to application */ return (status); } /** - * @} end of MatrixInv group + @} end of MatrixInv group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c b/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c index a038f2f..ffddf99 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_mult_f32.c * Description: Floating-point matrix multiplication * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -62,6 +62,9 @@ * @return The function returns either * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. */ +#if defined(ARM_MATH_NEON) + +#define GROUPOFROWS 8 arm_status arm_mat_mult_f32( const arm_matrix_instance_f32 * pSrcA, @@ -78,32 +81,225 @@ arm_status arm_mat_mult_f32( 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 */ + uint16_t col, i = 0U, j, row = numRowsA, rowCnt, colCnt; /* loop counters */ arm_status status; /* status of matrix multiplication */ -#ifdef ARM_MATH_MATRIX_CHECK + float32x4_t a0V, a1V, a2V, a3V, a4V, a5V, a6V, a7V; + float32x4_t acc0,acc1,acc2,acc3,acc4,acc5,acc6,acc7,temp; + float32x2_t accum = vdup_n_f32(0); + float32_t *pIn1B = pSrcA->pData; + float32_t *pIn1C = pSrcA->pData; + float32_t *pIn1D = pSrcA->pData; + float32_t *pIn1E = pSrcA->pData; + float32_t *pIn1F = pSrcA->pData; + float32_t *pIn1G = pSrcA->pData; + float32_t *pIn1H = pSrcA->pData; + float32_t *pxB,*pxC, *pxD, *pxE, *pxF, *pxG, *pxH; /* Temporary output data matrix pointer */ + float32_t sum0,sum1, sum2,sum3, sum4, sum5 , sum6, sum7; + +#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 + /* Row loop */ + rowCnt = row >> 3; + + while(rowCnt > 0) + { + /* Output pointer is set to starting address of the row being processed */ + px = pOut + GROUPOFROWS*i; + pxB = px + numColsB; + pxC = px + 2*numColsB; + pxD = px + 3*numColsB; + pxE = px + 4*numColsB; + pxF = px + 5*numColsB; + pxG = px + 6*numColsB; + pxH = px + 7*numColsB; + + /* 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 */ + sum0 = 0.0f; + sum1 = 0.0f; + sum2 = 0.0f; + sum3 = 0.0f; + sum4 = 0.0f; + sum5 = 0.0f; + sum6 = 0.0f; + sum7 = 0.0f; + + /* Initiate the pointer pIn1 to point to the starting address of the column being processed */ + pIn1 = pInA; + pIn1B = pIn1 + numColsA; + pIn1C = pIn1 + 2*numColsA; + pIn1D = pIn1 + 3*numColsA; + pIn1E = pIn1 + 4*numColsA; + pIn1F = pIn1 + 5*numColsA; + pIn1G = pIn1 + 6*numColsA; + pIn1H = pIn1 + 7*numColsA; + + acc0 = vdupq_n_f32(0.0); + acc1 = vdupq_n_f32(0.0); + acc2 = vdupq_n_f32(0.0); + acc3 = vdupq_n_f32(0.0); + acc4 = vdupq_n_f32(0.0); + acc5 = vdupq_n_f32(0.0); + acc6 = vdupq_n_f32(0.0); + acc7 = vdupq_n_f32(0.0); + + /* 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) */ + a0V = vld1q_f32(pIn1); + a1V = vld1q_f32(pIn1B); + a2V = vld1q_f32(pIn1C); + a3V = vld1q_f32(pIn1D); + a4V = vld1q_f32(pIn1E); + a5V = vld1q_f32(pIn1F); + a6V = vld1q_f32(pIn1G); + a7V = vld1q_f32(pIn1H); + + pIn1 += 4; + pIn1B += 4; + pIn1C += 4; + pIn1D += 4; + pIn1E += 4; + pIn1F += 4; + pIn1G += 4; + pIn1H += 4; + + temp[0] = *pIn2; + pIn2 += numColsB; + temp[1] = *pIn2; + pIn2 += numColsB; + temp[2] = *pIn2; + pIn2 += numColsB; + temp[3] = *pIn2; + pIn2 += numColsB; + + acc0 = vmlaq_f32(acc0,a0V,temp); + acc1 = vmlaq_f32(acc1,a1V,temp); + acc2 = vmlaq_f32(acc2,a2V,temp); + acc3 = vmlaq_f32(acc3,a3V,temp); + acc4 = vmlaq_f32(acc4,a4V,temp); + acc5 = vmlaq_f32(acc5,a5V,temp); + acc6 = vmlaq_f32(acc6,a6V,temp); + acc7 = vmlaq_f32(acc7,a7V,temp); + + /* Decrement the loop count */ + colCnt--; + } + + accum = vpadd_f32(vget_low_f32(acc0), vget_high_f32(acc0)); + sum0 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc1), vget_high_f32(acc1)); + sum1 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc2), vget_high_f32(acc2)); + sum2 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc3), vget_high_f32(acc3)); + sum3 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc4), vget_high_f32(acc4)); + sum4 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc5), vget_high_f32(acc5)); + sum5 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc6), vget_high_f32(acc6)); + sum6 += accum[0] + accum[1]; + + accum = vpadd_f32(vget_low_f32(acc7), vget_high_f32(acc7)); + sum7 += accum[0] + accum[1]; + + /* If the columns of pSrcA is not a multiple of 4, compute any remaining MACs here. + ** No loop unrolling is used. */ + colCnt = numColsA & 3; + + while (colCnt > 0U) + { + /* c(m,n) = a(1,1)*b(1,1) + a(1,2)*b(2,1) + ... + a(m,p)*b(p,n) */ + sum0 += *pIn1++ * (*pIn2); + sum1 += *pIn1B++ * (*pIn2); + sum2 += *pIn1C++ * (*pIn2); + sum3 += *pIn1D++ * (*pIn2); + sum4 += *pIn1E++ * (*pIn2); + sum5 += *pIn1F++ * (*pIn2); + sum6 += *pIn1G++ * (*pIn2); + sum7 += *pIn1H++ * (*pIn2); + pIn2 += numColsB; + + /* Decrement the loop counter */ + colCnt--; + } + + /* Store the result in the destination buffer */ + *px++ = sum0; + *pxB++ = sum1; + *pxC++ = sum2; + *pxD++ = sum3; + *pxE++ = sum4; + *pxF++ = sum5; + *pxG++ = sum6; + *pxH++ = sum7; + + /* 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); + + /* Update the pointer pInA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + GROUPOFROWS*numColsA; + + /* Decrement the row loop counter */ + rowCnt--; + } + + /* + + i was the index of a group of rows computed by previous loop. + Now i is the index of a row since below code is computing row per row + and no more group of row per group of rows. + + */ + + i = GROUPOFROWS*i; + rowCnt = row & 7; + + while(rowCnt > 0) { /* Output pointer is set to starting address of the row being processed */ px = pOut + i; @@ -117,7 +313,7 @@ arm_status arm_mat_mult_f32( j = 0U; - /* column loop */ + /* Column loop */ do { /* Set the variable sum, that acts as accumulator, to zero */ @@ -126,43 +322,43 @@ arm_status arm_mat_mult_f32( /* 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. */ + acc0 = vdupq_n_f32(0.0); + + /* Compute 4 MACs simultaneously. */ colCnt = numColsA >> 2U; - /* matrix multiplication */ + /* 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; + /* c(m,n) = a(1,1)*b(1,1) + a(1,2)*b(2,1) + ... + a(m,p)*b(p,n) */ + a0V = vld1q_f32(pIn1); // load & separate real/imag pSrcA (de-interleave 2) + pIn1 += 4; + + temp[0] = *pIn2; pIn2 += numColsB; - in1 = pIn1[0]; - in2 = pIn1[1]; - sum += in1 * in3; - in4 = *pIn2; + temp[1] = *pIn2; pIn2 += numColsB; - sum += in2 * in4; - - in3 = *pIn2; + temp[2] = *pIn2; pIn2 += numColsB; - in1 = pIn1[2]; - in2 = pIn1[3]; - sum += in1 * in3; - in4 = *pIn2; + temp[3] = *pIn2; pIn2 += numColsB; - sum += in2 * in4; - pIn1 += 4U; + + acc0 = vmlaq_f32(acc0,a0V,temp); /* Decrement the loop count */ colCnt--; } + accum = vpadd_f32(vget_low_f32(acc0), vget_high_f32(acc0)); + sum += accum[0] + accum[1]; + /* 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) */ + /* 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; @@ -182,40 +378,67 @@ arm_status arm_mat_mult_f32( } while (col > 0U); -#else - /* Run the below code for Cortex-M0 */ + /* Update the pointer pInA to point to the starting address of the next row */ + i = i + numColsB; + pInA = pInA + numColsA; - 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 */ + /* Decrement the row loop counter */ + rowCnt--; + + } + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + + /* Return to application */ + return (status); +} +#else +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 *pInB = pSrcB->pData; /* Input data matrix pointer B */ + 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 */ + uint32_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)) + (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 */ + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { - /* The following loop performs the dot-product of each row in pInA with each column in pInB */ + /* 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 */ + /* Output pointer is set to starting address of row being processed */ px = pOut + i; - /* For every row wise process, the column loop counter is to be initiated */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of pSrcB data */ pIn2 = pSrcB->pData; /* column loop */ @@ -224,43 +447,78 @@ arm_status arm_mat_mult_f32( /* 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 */ + /* Initialize pointer pIn1 to point to starting address of column being processed */ pIn1 = pInA; - /* Matrix A columns number of MAC operations are to be performed */ +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 MACs at a time. */ + 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) */ + + /* Perform the multiply-accumulates */ + sum += *pIn1++ * *pIn2; + pIn2 += numColsB; + + sum += *pIn1++ * *pIn2; + pIn2 += numColsB; + + sum += *pIn1++ * *pIn2; + pIn2 += numColsB; + + sum += *pIn1++ * *pIn2; + pIn2 += numColsB; + + /* Decrement loop counter */ + colCnt--; + } + + /* Loop unrolling: Compute remaining MACs */ + colCnt = numColsA % 0x4U; + +#else + + /* Initialize cntCnt with number of columns */ colCnt = numColsA; +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + 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); + /* 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 += *pIn1++ * *pIn2; pIn2 += numColsB; - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } - /* Store the result in the destination buffer */ + /* Store result in destination buffer */ *px++ = sum; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; - /* Update the pointer pIn2 to point to the starting address of the next column */ + /* Update pointer pIn2 to point to starting address of 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 */ + /* Update pointer pInA to point to starting address of next row */ i = i + numColsB; pInA = pInA + numColsA; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -269,6 +527,8 @@ arm_status arm_mat_mult_f32( return (status); } +#endif /* #if defined(ARM_MATH_NEON) */ + /** * @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c b/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c index 8d720c7..670ace1 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_mult_fast_q15.c * Description: Q15 matrix multiplication (fast variant) * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,206 +29,165 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * - * \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 <code>arm_mat_mult_q15()</code> for a slower implementation of this function - * which uses 64-bit accumulation to provide higher precision. + @brief Q15 matrix multiplication (fast variant). + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The difference between the function \ref 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. + @remark + Refer to \ref 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) + 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; - + 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 */ + +#if defined (ARM_MATH_DSP) + q31_t in; /* Temporary variable to hold the input value */ + q31_t inA1, inB1, inA2, 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 */ + q15_t in; /* Temporary variable to hold the input value */ + q15_t inA1, inB1, inA2, inB2; +#endif /* #if defined (ARM_MATH_DSP) */ #ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ if ((pSrcA->numCols != pSrcB->numRows) || - (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + (pSrcA->numRows != pDst->numRows) || + (pSrcB->numCols != pDst->numCols) ) { /* Set status as ARM_MATH_SIZE_MISMATCH */ status = ARM_MATH_SIZE_MISMATCH; } else -#endif + +#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 */ + /* The pointer px is set to starting address of column being processed */ px = pSrcBT + i; + /* Apply loop unrolling and exchange columns with row elements */ + col = numColsB >> 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 (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 +#if defined (ARM_MATH_DSP) - *px = (q15_t) in; + /* Read two elements from row */ + in = read_q15x2_ia ((q15_t **) &pInB); + /* Unpack and store one element in 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 */ -#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB; - /* Unpack and store the second element in the destination */ + /* Unpack and store second element in 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 */ -#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB; - /* Read two elements from the row */ - in = *__SIMD32(pInB)++; - - /* Unpack and store one element in the destination */ + in = read_q15x2_ia ((q15_t **) &pInB); #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 */ +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 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 */ + px += numRowsB; -#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - -#else +#else /* #if defined (ARM_MATH_DSP) */ - /* Read one element from the row */ + /* Read one element from row */ in = *pInB++; - /* Store one element in the destination */ + /* Store one element in destination */ *px = in; - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of 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 */ +#endif /* #if defined (ARM_MATH_DSP) */ + + /* Decrement column loop counter */ col--; } @@ -238,31 +197,31 @@ arm_status arm_mat_mult_fast_q15( while (col > 0U) { - /* Read and store the input element in the destination */ + /* Read and store input element in destination */ *px = *pInB++; - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } i++; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); - /* Reset the variables for the usage in the following multiplication process */ + /* Reset variables for usage in following multiplication process */ row = numRowsA; i = 0U; px = pDst->pData; -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) /* Process two rows from matrix A at a time and output two rows at a time */ - row = row >> 1; + row = row >> 1U; px2 = px + numColsB; #endif @@ -270,29 +229,28 @@ arm_status arm_mat_mult_fast_q15( /* row loop */ while (row > 0U) { - /* For every row wise process, the column loop counter is to be initiated */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of transposed pSrcB data */ pInB = pSrcBT; -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) /* Process two (transposed) columns from matrix B at a time */ - col = col >> 1; + col = col >> 1U; j = 0; #endif /* column loop */ while (col > 0U) { - /* Set the variable sum, that acts as accumulator, to zero */ + /* Set 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 */ + /* Initiate pointer pInA to point to starting address of column being processed */ pInA = pSrcA->pData + i; -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) sum2 = 0; sum3 = 0; sum4 = 0; @@ -301,56 +259,55 @@ arm_status arm_mat_mult_fast_q15( pInB2 = pInB + numRowsB; /* Read in two elements at once - alows dual MAC instruction */ - colCnt = numColsA >> 1; + colCnt = numColsA >> 1U; #else - colCnt = numColsA >> 2; + colCnt = numColsA >> 2U; #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 + /* c(m,n) = a(1,1) * b(1,1) + a(1,2) * b(2,1) + .... + a(m,p) * b(p,n) */ - inA1 = *__SIMD32(pInA)++; - inB1 = *__SIMD32(pInB)++; - inA2 = *__SIMD32(pInA2)++; - inB2 = *__SIMD32(pInB2)++; +#if defined (ARM_MATH_DSP) + /* read real and imag values from pSrcA and pSrcB buffer */ + inA1 = read_q15x2_ia ((q15_t **) &pInA); + inB1 = read_q15x2_ia ((q15_t **) &pInB); + inA2 = read_q15x2_ia ((q15_t **) &pInA2); + inB2 = read_q15x2_ia ((q15_t **) &pInB2); + + /* Multiply and Accumlates */ 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; + /* read real and imag values from pSrcA and pSrcB buffer */ + inA1 = *pInA++; + inB1 = *pInB++; + /* Multiply and Accumlates */ sum += inA1 * inB1; - inA2 = pInA[1]; - inB2 = pInB[1]; + inA2 = *pInA++; + inB2 = *pInB++; sum += inA2 * inB2; - inA1 = pInA[2]; - inB1 = pInB[2]; + inA1 = *pInA++; + inB1 = *pInB++; sum += inA1 * inB1; - inA2 = pInA[3]; - inB2 = pInB[3]; + inA2 = *pInA++; + inB2 = *pInB++; sum += inA2 * inB2; +#endif /* #if defined (ARM_MATH_DSP) */ - pInA += 4; - pInB += 4; - -#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ - - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } /* process odd column samples */ -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) if (numColsA & 1U) { inA1 = *pInA++; inB1 = *pInB++; @@ -366,44 +323,45 @@ arm_status arm_mat_mult_fast_q15( 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++); + /* 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++; + /* Decrement loop counter */ colCnt--; } -#endif +#endif /* #if defined (ARM_MATH_DSP) */ - /* Saturate and store the result in the destination buffer */ + /* Saturate and store result in destination buffer */ *px++ = (q15_t) (sum >> 15); -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) *px++ = (q15_t) (sum2 >> 15); *px2++ = (q15_t) (sum3 >> 15); *px2++ = (q15_t) (sum4 >> 15); j += numRowsB * 2; #endif - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } i = i + numColsA; -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) i = i + numColsA; px = px2 + (numColsB & 1U); px2 = px + numColsB; #endif - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } /* Compute any remaining odd row/column below */ -#ifndef UNALIGNED_SUPPORT_DISABLE +#if defined (ARM_MATH_DSP) /* Compute remaining output column */ if (numColsB & 1U) { @@ -412,7 +370,7 @@ arm_status arm_mat_mult_fast_q15( row = numRowsA & (~0x1); /* Point to remaining unfilled column in output matrix */ - px = pDst->pData+numColsB-1; + px = pDst->pData + numColsB-1; pInA = pSrcA->pData; /* row loop */ @@ -420,26 +378,26 @@ arm_status arm_mat_mult_fast_q15( { /* point to last column in matrix B */ - pInB = pSrcBT + numRowsB*(numColsB-1); + pInB = pSrcBT + numRowsB * (numColsB-1); - /* Set the variable sum, that acts as accumulator, to zero */ + /* Set variable sum, that acts as accumulator, to zero */ sum = 0; /* Compute 4 columns at once */ - colCnt = numColsA >> 2; + colCnt = numColsA >> 2U; /* matrix multiplication */ while (colCnt > 0U) { - inA1 = *__SIMD32(pInA)++; - inA2 = *__SIMD32(pInA)++; - inB1 = *__SIMD32(pInB)++; - inB2 = *__SIMD32(pInB)++; + inA1 = read_q15x2_ia ((q15_t **) &pInA); + inA2 = read_q15x2_ia ((q15_t **) &pInA); + inB1 = read_q15x2_ia ((q15_t **) &pInB); + inB2 = read_q15x2_ia ((q15_t **) &pInB); sum = __SMLAD(inA1, inB1, sum); sum = __SMLAD(inA2, inB2, sum); - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } @@ -449,11 +407,11 @@ arm_status arm_mat_mult_fast_q15( colCnt--; } - /* Store the result in the destination buffer */ - *px = (q15_t) (sum >> 15); + /* Store result in destination buffer */ + *px = (q15_t) (sum >> 15); px += numColsB; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } } @@ -462,7 +420,7 @@ arm_status arm_mat_mult_fast_q15( if (numRowsA & 1U) { /* point to last row in output matrix */ - px = pDst->pData+(numColsB)*(numRowsA-1); + px = pDst->pData + (numColsB) * (numRowsA-1); pInB = pSrcBT; col = numColsB; @@ -471,48 +429,48 @@ arm_status arm_mat_mult_fast_q15( /* col loop */ while (col > 0) { - /* point to last row in matrix A */ - pInA = pSrcA->pData + (numRowsA-1)*numColsA; + pInA = pSrcA->pData + (numRowsA-1) * numColsA; - /* Set the variable sum, that acts as accumulator, to zero */ + /* Set variable sum, that acts as accumulator, to zero */ sum = 0; /* Compute 4 columns at once */ - colCnt = numColsA >> 2; + colCnt = numColsA >> 2U; /* matrix multiplication */ while (colCnt > 0U) { - inA1 = *__SIMD32(pInA)++; - inA2 = *__SIMD32(pInA)++; - inB1 = *__SIMD32(pInB)++; - inB2 = *__SIMD32(pInB)++; + inA1 = read_q15x2_ia ((q15_t **) &pInA); + inA2 = read_q15x2_ia ((q15_t **) &pInA); + inB1 = read_q15x2_ia ((q15_t **) &pInB); + inB2 = read_q15x2_ia ((q15_t **) &pInB); sum = __SMLAD(inA1, inB1, sum); sum = __SMLAD(inA2, inB2, sum); - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } - colCnt = numColsA & 3U; + colCnt = numColsA % 4U; while (colCnt > 0U) { sum += (q31_t) (*pInA++) * (*pInB++); + colCnt--; } - /* Store the result in the destination buffer */ - *px++ = (q15_t) (sum >> 15); + /* Store result in destination buffer */ + *px++ = (q15_t) (sum >> 15); - /* Decrement the col loop counter */ + /* Decrement column loop counter */ col--; } } -#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ +#endif /* #if defined (ARM_MATH_DSP) */ - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -521,5 +479,5 @@ arm_status arm_mat_mult_fast_q15( } /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c b/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c index 78b33ef..011959a 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_mult_fast_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_mult_fast_q31.c * Description: Q31 matrix multiplication (fast variant) * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,226 +29,166 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * - * \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 <code>arm_mat_mult_q31()</code> for a slower implementation of this function - * which uses 64-bit accumulation to provide higher precision. + @brief Q31 matrix multiplication (fast variant). + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The difference between the function \ref 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. + @remark + Refer to \ref 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) + 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 *pInA = pSrcA->pData; /* Input data matrix pointer A */ + q31_t *pInB = pSrcB->pData; /* Input data matrix pointer B */ q31_t *pInA2; + q31_t *px; /* Temporary output data matrix pointer */ q31_t *px2; + q31_t sum1, sum2, sum3, sum4; /* Accumulator */ + q31_t inA1, inA2, inB1, inB2; + 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 */ -#endif #ifdef ARM_MATH_MATRIX_CHECK /* Check for matrix mismatch condition */ if ((pSrcA->numCols != pSrcB->numRows) || - (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + (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 */ - { +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { px = pDst->pData; -#if defined (ARM_MATH_DSP) - row = row >> 1; + row = row >> 1U; 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 */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of pSrcB data */ pInB = pSrcB->pData; j = 0U; -#if defined (ARM_MATH_DSP) - col = col >> 1; -#endif + col = col >> 1U; /* 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) + sum1 = 0; sum2 = 0; sum3 = 0; sum4 = 0; + + /* Initiate data pointers */ + pInA = pSrcA->pData + i; + pInB = pSrcB->pData + j; pInA2 = pInA + numColsA; + colCnt = numColsA; -#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) */ -#if defined (ARM_MATH_DSP) inA1 = *pInA++; inB1 = pInB[0]; inA2 = *pInA2++; inB2 = pInB[1]; pInB += numColsB; - sum = __SMMLA(inA1, inB1, sum); +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(inA1, inB1, sum1); 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; + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) inA1 * inB1)) >> 32); + sum2 = (q31_t) ((((q63_t) sum2 << 32) + ((q63_t) inA1 * inB2)) >> 32); + sum3 = (q31_t) ((((q63_t) sum3 << 32) + ((q63_t) inA2 * inB1)) >> 32); + sum4 = (q31_t) ((((q63_t) sum4 << 32) + ((q63_t) inA2 * inB2)) >> 32); #endif - /* Decrement the loop counter */ + /* Decrement 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++ = sum1 << 1; *px++ = sum2 << 1; *px2++ = sum3 << 1; *px2++ = sum4 << 1; + j += 2; -#endif - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; - } - i = i + numColsA; - -#if defined (ARM_MATH_DSP) - i = i + numColsA; - px = px2 + (numColsB & 1U); - px2 = px + numColsB; -#endif + i = i + (numColsA << 1U); + px = px2 + (numColsB & 1U); + px2 = px + numColsB; - /* Decrement the row loop counter */ + /* Decrement 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); + row = numRowsA & (~1U); /* Point to remaining unfilled column in output matrix */ - px = pDst->pData+numColsB-1; + px = pDst->pData + numColsB-1; pInA = pSrcA->pData; /* row loop */ @@ -258,49 +198,75 @@ arm_status arm_mat_mult_fast_q31( /* point to last column in matrix B */ pInB = pSrcB->pData + numColsB-1; - /* Set the variable sum, that acts as accumulator, to zero */ - sum = 0; + /* Set variable sum1, that acts as accumulator, to zero */ + sum1 = 0; - /* Compute 4 columns at once */ - colCnt = numColsA >> 2; +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 columns at a time. */ + colCnt = numColsA >> 2U; /* matrix multiplication */ while (colCnt > 0U) { - inA1 = *pInA++; - inA2 = *pInA++; - inB1 = *pInB; +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(*pInA++, *pInB, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) *pInA++ * *pInB)) >> 32); +#endif pInB += numColsB; - inB2 = *pInB; + +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(*pInA++, *pInB, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) *pInA++ * *pInB)) >> 32); +#endif pInB += numColsB; - sum = __SMMLA(inA1, inB1, sum); - sum = __SMMLA(inA2, inB2, sum); - inA1 = *pInA++; - inA2 = *pInA++; - inB1 = *pInB; +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(*pInA++, *pInB, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) *pInA++ * *pInB)) >> 32); +#endif pInB += numColsB; - inB2 = *pInB; + +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(*pInA++, *pInB, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) *pInA++ * *pInB)) >> 32); +#endif pInB += numColsB; - sum = __SMMLA(inA1, inB1, sum); - sum = __SMMLA(inA2, inB2, sum); - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } - colCnt = numColsA & 3U; + /* Loop unrolling: Compute remaining column */ + colCnt = numColsA % 4U; + +#else + + /* Initialize colCnt with number of columns */ + colCnt = numColsA; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + while (colCnt > 0U) { - sum = __SMMLA(*pInA++, *pInB, sum); +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(*pInA++, *pInB, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) *pInA++ * *pInB)) >> 32); +#endif pInB += numColsB; + colCnt--; } /* Convert the result from 2.30 to 1.31 format and store in destination buffer */ - *px = sum << 1; + *px = sum1 << 1; px += numColsB; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } } @@ -309,7 +275,7 @@ arm_status arm_mat_mult_fast_q31( if (numRowsA & 1U) { /* point to last row in output matrix */ - px = pDst->pData+(numColsB)*(numRowsA-1); + px = pDst->pData + (numColsB) * (numRowsA-1); col = numColsB; i = 0U; @@ -319,14 +285,16 @@ arm_status arm_mat_mult_fast_q31( { /* point to last row in matrix A */ - pInA = pSrcA->pData + (numRowsA-1)*numColsA; + pInA = pSrcA->pData + (numRowsA-1) * numColsA; pInB = pSrcB->pData + i; - /* Set the variable sum, that acts as accumulator, to zero */ - sum = 0; + /* Set variable sum1, that acts as accumulator, to zero */ + sum1 = 0; - /* Compute 4 columns at once */ - colCnt = numColsA >> 2; +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 columns at a time. */ + colCnt = numColsA >> 2U; /* matrix multiplication */ while (colCnt > 0U) @@ -337,8 +305,13 @@ arm_status arm_mat_mult_fast_q31( pInB += numColsB; inB2 = *pInB; pInB += numColsB; - sum = __SMMLA(inA1, inB1, sum); - sum = __SMMLA(inA2, inB2, sum); +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(inA1, inB1, sum1); + sum1 = __SMMLA(inA2, inB2, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) inA1 * inB1)) >> 32); + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) inA2 * inB2)) >> 32); +#endif inA1 = *pInA++; inA2 = *pInA++; @@ -346,32 +319,49 @@ arm_status arm_mat_mult_fast_q31( pInB += numColsB; inB2 = *pInB; pInB += numColsB; - sum = __SMMLA(inA1, inB1, sum); - sum = __SMMLA(inA2, inB2, sum); +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(inA1, inB1, sum1); + sum1 = __SMMLA(inA2, inB2, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) inA1 * inB1)) >> 32); + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) inA2 * inB2)) >> 32); +#endif - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } - colCnt = numColsA & 3U; + /* Loop unrolling: Compute remaining column */ + colCnt = numColsA % 4U; + +#else + + /* Initialize colCnt with number of columns */ + colCnt = numColsA; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + while (colCnt > 0U) { - sum = __SMMLA(*pInA++, *pInB, sum); +#if defined (ARM_MATH_DSP) + sum1 = __SMMLA(*pInA++, *pInB, sum1); +#else + sum1 = (q31_t) ((((q63_t) sum1 << 32) + ((q63_t) *pInA++ * *pInB)) >> 32); +#endif pInB += numColsB; + colCnt--; } /* Saturate and store the result in the destination buffer */ - *px++ = sum << 1; + *px++ = sum1 << 1; i++; - /* Decrement the col loop counter */ + /* Decrement col loop counter */ col--; } } -#endif /* #if defined (ARM_MATH_DSP) */ - - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -380,5 +370,5 @@ arm_status arm_mat_mult_fast_q31( } /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c b/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c index 3244f47..1d2b69c 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_mult_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_mult_q15.c * Description: Q15 matrix multiplication * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,206 +29,129 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * - * \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 <code>arm_mat_mult_fast_q15()</code> for a faster but less precise version of this function for Cortex-M3 and Cortex-M4. - * + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function is implemented using an internal 64-bit 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 \ref arm_mat_mult_fast_q15() for a faster but less precise version of this function. */ 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) + 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 */ + q63_t sum; /* Accumulator */ + +#if defined (ARM_MATH_DSP) /* != CM0 */ + + 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 */ + + q31_t in; /* Temporary variable to hold the input value */ + q31_t inA1, inB1, inA2, inB2; #ifdef ARM_MATH_MATRIX_CHECK + /* Check for matrix mismatch condition */ if ((pSrcA->numCols != pSrcB->numRows) || - (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols)) + (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 */ + +#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 */ + /* The pointer px is set to starting address of column being processed */ px = pSrcBT + i; + /* Apply loop unrolling and exchange columns with row elements */ + col = numColsB >> 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 (col > 0U) { -#ifndef UNALIGNED_SUPPORT_DISABLE + /* Read two elements from row */ + in = read_q15x2_ia ((q15_t **) &pInB); - /* Read two elements from the row */ - in = *__SIMD32(pInB)++; - - /* Unpack and store one element in the destination */ + /* Unpack and store one element in 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 */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB; - /* Unpack and store the second element in the destination */ + /* Unpack and store second element in 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 */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB; - /* Read two elements from the row */ - in = *__SIMD32(pInB)++; + /* Read two elements from row */ + in = read_q15x2_ia ((q15_t **) &pInB); - /* Unpack and store one element in the destination */ + /* Unpack and store one element in 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 */ + /* Decrement column loop counter */ col--; } @@ -238,24 +161,24 @@ arm_status arm_mat_mult_q15( while (col > 0U) { - /* Read and store the input element in the destination */ + /* Read and store input element in destination */ *px = *pInB++; - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += numRowsB; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } i++; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); - /* Reset the variables for the usage in the following multiplication process */ + /* Reset variables for usage in following multiplication process */ row = numRowsA; i = 0U; px = pDst->pData; @@ -264,123 +187,98 @@ arm_status arm_mat_mult_q15( /* row loop */ do { - /* For every row wise process, the column loop counter is to be initiated */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of transposed pSrcB data */ pInB = pSrcBT; /* column loop */ do { - /* Set the variable sum, that acts as accumulator, to zero */ + /* Set 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 */ + /* Initiate pointer pInA to point to starting address of column being processed */ pInA = pSrcA->pData + i; + /* Apply loop unrolling and compute 2 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) */ -#ifndef UNALIGNED_SUPPORT_DISABLE + /* 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 */ - pSourceA1 = *__SIMD32(pInA)++; - pSourceB1 = *__SIMD32(pInB)++; - - pSourceA2 = *__SIMD32(pInA)++; - pSourceB2 = *__SIMD32(pInB)++; + inA1 = read_q15x2_ia ((q15_t **) &pInA); + inB1 = read_q15x2_ia ((q15_t **) &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++; + inA2 = read_q15x2_ia ((q15_t **) &pInA); + inB2 = read_q15x2_ia ((q15_t **) &pInB); - inA1 = *pInA++; - inB1 = *pInB++; /* Multiply and Accumlates */ - sum += inA2 * inB2; - inA2 = *pInA++; - inB2 = *pInB++; + sum = __SMLALD(inA1, inB1, sum); + sum = __SMLALD(inA2, inB2, sum); - /* Multiply and Accumlates */ - sum += inA1 * inB1; - sum += inA2 * inB2; - -#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */ - - /* Decrement the loop counter */ + /* Decrement loop counter */ colCnt--; } /* process remaining column samples */ - colCnt = numColsA & 3U; + 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) */ + /* 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 */ + /* Decrement loop counter */ colCnt--; } - /* Saturate and store the result in the destination buffer */ + /* Saturate and store result in destination buffer */ *px = (q15_t) (__SSAT((sum >> 15), 16)); px++; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } while (col > 0U); i = i + numColsA; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); -#else - - /* Run the below code for Cortex-M0 */ +#else /* #if defined (ARM_MATH_DSP) */ - 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 */ + 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 */ + uint32_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)) + (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 */ { @@ -391,11 +289,10 @@ arm_status arm_mat_mult_q15( /* 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 */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of pSrcB data */ pIn2 = pSrcB->pData; /* column loop */ @@ -404,7 +301,7 @@ arm_status arm_mat_mult_q15( /* Set the variable sum, that acts as accumulator, to zero */ sum = 0; - /* Initiate the pointer pIn1 to point to the starting address of pSrcA */ + /* Initiate pointer pIn1 to point to starting address of pSrcA */ pIn1 = pInA; /* Matrix A columns number of MAC operations are to be performed */ @@ -413,38 +310,41 @@ arm_status arm_mat_mult_q15( /* 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 */ + /* c(m,n) = a(1,1) * b(1,1) + a(1,2) * b(2,1) + .... + a(m,p) * b(p,n) */ + + /* Perform multiply-accumulates */ sum += (q31_t) * pIn1++ * *pIn2; pIn2 += numColsB; - /* Decrement the loop counter */ + /* Decrement 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 */ + /* Convert result from 34.30 to 1.15 format and store saturated value in destination buffer */ + + /* Saturate and store result in destination buffer */ *px++ = (q15_t) __SSAT((sum >> 15), 16); - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; - /* Update the pointer pIn2 to point to the starting address of the next column */ + /* Update pointer pIn2 to point to starting address of next column */ pIn2 = pInB + (numColsB - col); } while (col > 0U); - /* Update the pointer pSrcA to point to the starting address of the next row */ + /* Update pointer pSrcA to point to starting address of next row */ i = i + numColsB; pInA = pInA + numColsA; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); #endif /* #if defined (ARM_MATH_DSP) */ - /* set status as ARM_MATH_SUCCESS */ + + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -453,5 +353,5 @@ arm_status arm_mat_mult_q15( } /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c b/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c index 9bd2b97..161e723 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_mult_q31.c * Description: Q31 matrix multiplication * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,254 +29,168 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixMult - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * - * \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 <code>arm_mat_mult_fast_q31()</code> for a faster but less precise implementation of this function for Cortex-M3 and Cortex-M4. - * + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function is implemented using an internal 64-bit accumulator. + The accumulator has a 2.62 format and maintains full precision of the intermediate + multiplication results but provides only a single guard bit. There is no saturation + on intermediate additions. Thus, if the accumulator overflows it wraps around and + distorts the result. The input signals should be scaled down to avoid intermediate + overflows. 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. + @remark + Refer to \ref arm_mat_mult_fast_q31() for a faster but less precise implementation of this function. */ arm_status arm_mat_mult_q31( const arm_matrix_instance_q31 * pSrcA, const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst) + 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 *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 *pInB = pSrcB->pData; /* Input data matrix pointer B */ + 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; + 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, 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)) + (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 */ + +#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 */ + /* Output pointer is set to starting address of row being processed */ px = pOut + i; - /* For every row wise process, the column loop counter is to be initiated */ + /* For every row wise process, 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 */ + /* For every row wise process, pIn2 pointer is set to starting address of 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 */ + /* Initialize pointer pIn1 to point to starting address of column being processed */ pIn1 = pInA; - /* Apply loop unrolling and compute 4 MACs simultaneously. */ - colCnt = numColsA >> 2; +#if defined (ARM_MATH_LOOPUNROLL) + /* Loop unrolling: Compute 4 MACs at a time. */ + 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) */ + /* 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; + sum += (q63_t) *pIn1++ * *pIn2; pIn2 += numColsB; - a0 = *pIn1++; - a1 = *pIn1++; - - b1 = *pIn2; + sum += (q63_t) *pIn1++ * *pIn2; pIn2 += numColsB; - b2 = *pIn2; - pIn2 += numColsB; - - sum += (q63_t) a0 *b0; - sum += (q63_t) a1 *b1; - a2 = *pIn1++; - a3 = *pIn1++; - - b3 = *pIn2; + sum += (q63_t) *pIn1++ * *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; + sum += (q63_t) *pIn1++ * *pIn2; pIn2 += numColsB; - /* Decrement the loop counter */ + /* Decrement 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); + /* Loop unrolling: Compute remaining MACs */ + colCnt = numColsA % 0x4U; #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 */ + /* Initialize cntCnt with number of columns */ colCnt = numColsA; - /* matrix multiplication */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + while (colCnt > 0U) { - /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */ + /* 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; + sum += (q63_t) *pIn1++ * *pIn2; pIn2 += numColsB; - /* Decrement the loop counter */ + /* Decrement 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); + /* Convert result from 2.62 to 1.31 format and store in destination buffer */ + *px++ = (q31_t) (sum >> 31); - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; - /* Update the pointer pIn2 to point to the starting address of the next column */ + /* Update pointer pIn2 to point to starting address of next column */ pIn2 = pInB + (numColsB - col); } while (col > 0U); -#endif - - /* Update the pointer pInA to point to the starting address of the next row */ + /* Update pointer pInA to point to starting address of next row */ i = i + numColsB; pInA = pInA + numColsA; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; } while (row > 0U); - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } + /* Return to application */ return (status); } /** - * @} end of MatrixMult group + @} end of MatrixMult group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c b/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c index dbc385a..a0097b1 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_scale_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_scale_f32.c * Description: Multiplies a floating-point matrix by a scalar * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,42 +29,42 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @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, <code>scale</code> is represented by - * a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>. - * The shift allows the gain of the scaling operation to exceed 1.0. - * The overall scale factor applied to the fixed-point data is - * <pre> - * scale = scaleFract * 2^shift. - * </pre> + @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, <code>scale</code> is represented by + a fractional multiplication <code>scaleFract</code> and an arithmetic shift <code>shift</code>. + The shift allows the gain of the scaling operation to exceed 1.0. + The overall scale factor applied to the fixed-point data is + <pre> + scale = scaleFract * 2^shift. + </pre> */ /** - * @addtogroup MatrixScale - * @{ + @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 <code>ARM_MATH_SIZE_MISMATCH</code> - * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * + @brief Floating-point matrix scaling. + @param[in] pSrc points to input matrix + @param[in] scale scale factor to be applied + @param[out] pDst points to output matrix structure + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed */ - +#if defined(ARM_MATH_NEON_EXPERIMENTAL) arm_status arm_mat_scale_f32( const arm_matrix_instance_f32 * pSrc, float32_t scale, @@ -76,12 +76,10 @@ arm_status arm_mat_scale_f32( 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 */ @@ -93,37 +91,23 @@ arm_status arm_mat_scale_f32( else #endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { + float32x4_t vec1; + float32x4_t res; + /* 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. + /* 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; + vec1 = vld1q_f32(pIn); + res = vmulq_f32(vec1, vdupq_n_f32(scale)); + vst1q_f32(pOut, res); /* update pointers to process next sampels */ pIn += 4U; @@ -137,22 +121,89 @@ arm_status arm_mat_scale_f32( ** No loop unrolling is used. */ blkCnt = numSamples % 0x4U; + 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); +} #else +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 */ - /* Run the below code for Cortex-M0 */ +#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 input matrix */ + numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; + +#if defined (ARM_MATH_LOOPUNROLL) + + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) * scale */ + + /* Scale and store result in destination buffer. */ + *pOut++ = (*pIn++) * scale; + *pOut++ = (*pIn++) * scale; + *pOut++ = (*pIn++) * scale; + *pOut++ = (*pIn++) * scale; + + /* Decrement loop counter */ + blkCnt--; + } + + /* Loop unrolling: Compute remaining outputs */ + blkCnt = numSamples % 0x4U; + +#else /* Initialize blkCnt with number of samples */ blkCnt = numSamples; -#endif /* #if defined (ARM_MATH_DSP) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ while (blkCnt > 0U) { /* C(m,n) = A(m,n) * scale */ - /* The results are stored in the destination buffer. */ + + /* Scale and store result in destination buffer. */ *pOut++ = (*pIn++) * scale; - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } @@ -163,7 +214,8 @@ arm_status arm_mat_scale_f32( /* Return to application */ return (status); } +#endif /* #if defined(ARM_MATH_NEON) */ /** - * @} end of MatrixScale group + @} end of MatrixScale group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c b/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c index af664ca..9b75d4e 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_scale_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_scale_q15.c * Description: Multiplies a Q15 matrix by a scalar * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,135 +29,134 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixScale - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * \par - * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format. - * These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format. + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.15 format. + These are multiplied to yield a 2.30 intermediate result and this is shifted with saturation to 1.15 format. */ 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 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) + 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 */ + uint32_t blkCnt; /* Loop counter */ + arm_status status; /* Status of matrix scaling */ + int32_t kShift = 15 - shift; /* Total shift to apply after scaling */ + +#if defined (ARM_MATH_LOOPUNROLL) && defined (ARM_MATH_DSP) + q31_t inA1, inA2; + q31_t out1, out2, out3, out4; /* Temporary output variables */ + q15_t in1, in2, in3, in4; /* Temporary input variables */ +#endif #ifdef ARM_MATH_MATRIX_CHECK - /* Check for matrix mismatch */ - if ((pSrc->numRows != pDst->numRows) || (pSrc->numCols != pDst->numCols)) + + /* 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 + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { - /* Total number of samples in the input matrix */ + /* Total number of samples in input matrix */ numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; -#if defined (ARM_MATH_DSP) +#if defined (ARM_MATH_LOOPUNROLL) - /* Run the below code for Cortex-M4 and Cortex-M3 */ - /* Loop Unrolling */ - blkCnt = numSamples >> 2; + /* Loop unrolling: Compute 4 outputs at a time */ + 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 */ - /* 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 +#if defined (ARM_MATH_DSP) + /* read 2 times 2 samples at a time from source */ + inA1 = read_q15x2_ia ((q15_t **) &pIn); + inA2 = read_q15x2_ia ((q15_t **) &pIn); + + /* Scale inputs and store result in temporary variables * in single cycle by packing the outputs */ out1 = (q31_t) ((q15_t) (inA1 >> 16) * scaleFract); - out2 = (q31_t) ((q15_t) inA1 * scaleFract); + out2 = (q31_t) ((q15_t) (inA1 ) * scaleFract); out3 = (q31_t) ((q15_t) (inA2 >> 16) * scaleFract); - out4 = (q31_t) ((q15_t) inA2 * 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; + /* apply shifting */ + out1 = out1 >> kShift; + out2 = out2 >> kShift; + out3 = out3 >> kShift; + out4 = out4 >> kShift; + /* saturate the output */ in1 = (q15_t) (__SSAT(out1, 16)); in2 = (q15_t) (__SSAT(out2, 16)); in3 = (q15_t) (__SSAT(out3, 16)); in4 = (q15_t) (__SSAT(out4, 16)); - _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; + /* store result to destination */ + write_q15x2_ia (&pOut, __PKHBT(in2, in1, 16)); + write_q15x2_ia (&pOut, __PKHBT(in4, in3, 16)); +#else + *pOut++ = (q15_t) (__SSAT(((q31_t) (*pIn++) * scaleFract) >> kShift, 16)); + *pOut++ = (q15_t) (__SSAT(((q31_t) (*pIn++) * scaleFract) >> kShift, 16)); + *pOut++ = (q15_t) (__SSAT(((q31_t) (*pIn++) * scaleFract) >> kShift, 16)); + *pOut++ = (q15_t) (__SSAT(((q31_t) (*pIn++) * scaleFract) >> kShift, 16)); +#endif - /* Decrement the numSamples loop counter */ + /* Decrement loop counter */ blkCnt--; } - /* If the numSamples is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 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 */ + /* Scale, saturate and store result in destination buffer. */ + *pOut++ = (q15_t) (__SSAT(((q31_t) (*pIn++) * scaleFract) >> kShift, 16)); + + /* Decrement loop counter */ blkCnt--; } + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -167,5 +166,5 @@ arm_status arm_mat_scale_q15( } /** - * @} end of MatrixScale group + @} end of MatrixScale group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c b/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c index d190cf1..929b17f 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_scale_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_scale_q31.c * Description: Multiplies a Q31 matrix by a scalar * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,152 +29,125 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixScale - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * @details - * <b>Scaling and Overflow Behavior:</b> - * \par - * The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format. - * These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format. + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format. + These are multiplied to yield a 2.62 intermediate result which 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 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 + 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 */ + uint32_t blkCnt; /* Loop counter */ + arm_status status; /* Status of matrix scaling */ + int32_t kShift = shift + 1; /* Shift to apply after scaling */ + q31_t in, out; /* Temporary variabels */ #ifdef ARM_MATH_MATRIX_CHECK - /* Check for matrix mismatch */ - if ((pSrc->numRows != pDst->numRows) || (pSrc->numCols != pDst->numCols)) + + /* 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 + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { - /* Total number of samples in the input matrix */ + /* Total number of samples in input matrix */ numSamples = (uint32_t) pSrc->numRows * pSrc->numCols; -#if defined (ARM_MATH_DSP) - - /* Run the below code for Cortex-M4 and Cortex-M3 */ +#if defined (ARM_MATH_LOOPUNROLL) - /* Loop Unrolling */ + /* Loop unrolling: Compute 4 outputs at a time */ 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 */ + /* Scale, saturate and store result in destination buffer. */ + in = *pIn++; /* read four inputs from source */ + in = ((q63_t) in * scaleFract) >> 32; /* multiply input with scaler value */ + out = in << kShift; /* apply shifting */ + if (in != (out >> kShift)) /* saturate the results. */ + out = 0x7FFFFFFF ^ (in >> 31); + *pOut++ = out; /* Store result destination */ + + in = *pIn++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pOut++ = out; + + in = *pIn++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pOut++ = out; + + in = *pIn++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pOut++ = out; + + /* Decrement loop counter */ blkCnt--; } - /* If the numSamples is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 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; + /* Scale, saturate and store result in destination buffer. */ + in = *pIn++; + in = ((q63_t) in * scaleFract) >> 32; + out = in << kShift; + if (in != (out >> kShift)) + out = 0x7FFFFFFF ^ (in >> 31); + *pOut++ = out; - /* Decrement the numSamples loop counter */ + /* Decrement loop counter */ blkCnt--; } @@ -187,5 +160,5 @@ arm_status arm_mat_scale_q31( } /** - * @} end of MatrixScale group + @} end of MatrixScale group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c b/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c index 7c0b54e..cb57647 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_sub_f32.c @@ -3,13 +3,13 @@ * Title: arm_mat_sub_f32.c * Description: Floating-point matrix subtraction * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,34 +29,36 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @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 - * <code>pSrcA</code>, <code>pSrcB</code>, and <code>pDst</code> have the same - * number of rows and columns. + @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 + <code>pSrcA</code>, <code>pSrcB</code>, and <code>pDst</code> have the same + number of rows and columns. */ /** - * @addtogroup MatrixSub - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed */ +#if defined(ARM_MATH_NEON) arm_status arm_mat_sub_f32( const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, @@ -66,11 +68,9 @@ arm_status arm_mat_sub_f32( 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 */ @@ -88,99 +88,128 @@ arm_status arm_mat_sub_f32( else #endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { + float32x4_t vec1; + float32x4_t vec2; + float32x4_t res; + /* 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. + /* 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]; + vec1 = vld1q_f32(pIn1); + vec2 = vld1q_f32(pIn2); + res = vsubq_f32(vec1, vec2); + vst1q_f32(pOut, res); - /* Read values from source B */ - inB1 = pIn2[0]; + /* Update pointers to process next samples */ + pIn1 += 4U; + pIn2 += 4U; + pOut += 4U; - /* Read values from source A */ - inA2 = pIn1[1]; + /* Decrement the loop counter */ + blkCnt--; + } - /* out = sourceA - sourceB */ - out1 = inA1 - inB1; + /* If the numSamples is not a multiple of 4, compute any remaining output samples here. + ** No loop unrolling is used. */ + blkCnt = numSamples % 0x4U; - /* Read values from source B */ - inB2 = pIn2[1]; - /* Read values from source A */ - inA1 = pIn1[2]; + 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++); - /* out = sourceA - sourceB */ - out2 = inA2 - inB2; + /* Decrement the loop counter */ + blkCnt--; + } - /* Read values from source B */ - inB1 = pIn2[2]; + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } - /* Store result in destination */ - pOut[0] = out1; - pOut[1] = out2; + /* Return to application */ + return (status); +} +#else +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 *pInA = pSrcA->pData; /* input data matrix pointer A */ + float32_t *pInB = pSrcB->pData; /* input data matrix pointer B */ + float32_t *pOut = pDst->pData; /* output data matrix pointer */ - /* Read values from source A */ - inA2 = pIn1[3]; + uint32_t numSamples; /* total number of elements in the matrix */ + uint32_t blkCnt; /* loop counters */ + arm_status status; /* status of matrix subtraction */ - /* Read values from source B */ - inB2 = pIn2[3]; +#ifdef ARM_MATH_MATRIX_CHECK - /* out = sourceA - sourceB */ - out1 = inA1 - inB1; + /* 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 */ - /* out = sourceA - sourceB */ - out2 = inA2 - inB2; + { + /* Total number of samples in input matrix */ + numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; - /* Store result in destination */ - pOut[2] = out1; +#if defined (ARM_MATH_LOOPUNROLL) - /* Store result in destination */ - pOut[3] = out2; + /* Loop unrolling: Compute 4 outputs at a time */ + blkCnt = numSamples >> 2U; + while (blkCnt > 0U) + { + /* C(m,n) = A(m,n) - B(m,n) */ - /* update pointers to process next sampels */ - pIn1 += 4U; - pIn2 += 4U; - pOut += 4U; + /* Subtract and store result in destination buffer. */ + *pOut++ = (*pInA++) - (*pInB++); + *pOut++ = (*pInA++) - (*pInB++); + *pOut++ = (*pInA++) - (*pInB++); + *pOut++ = (*pInA++) - (*pInB++); - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } - /* If the numSamples is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 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 */ + /* Subtract and store result in destination buffer. */ + *pOut++ = (*pInA++) - (*pInB++); + + /* Decrement loop counter */ blkCnt--; } @@ -191,7 +220,7 @@ arm_status arm_mat_sub_f32( /* Return to application */ return (status); } - +#endif /* #if defined(ARM_MATH_NEON) */ /** - * @} end of MatrixSub group + @} end of MatrixSub group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c b/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c index 28e659f..5d5e5d0 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_sub_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_sub_q15.c * Description: Q15 Matrix subtraction * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,112 +29,108 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixSub - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * <b>Scaling and Overflow Behavior:</b> - * \par - * The function uses saturating arithmetic. - * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated. + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated. */ arm_status arm_mat_sub_q15( const arm_matrix_instance_q15 * pSrcA, const arm_matrix_instance_q15 * pSrcB, - arm_matrix_instance_q15 * pDst) + 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 */ + 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)) + (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 */ +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { - /* Total number of samples in the input matrix */ + /* Total number of samples in input matrix */ numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; -#if defined (ARM_MATH_DSP) - - /* Run the below code for Cortex-M4 and Cortex-M3 */ +#if defined (ARM_MATH_LOOPUNROLL) - /* Apply loop unrolling */ + /* Loop unrolling: Compute 4 outputs at a time */ 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 */ + /* Subtract, Saturate and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + write_q15x2_ia (&pOut, __QSUB16(read_q15x2_ia ((q15_t **) &pInA), read_q15x2_ia ((q15_t **) &pInB))); + write_q15x2_ia (&pOut, __QSUB16(read_q15x2_ia ((q15_t **) &pInA), read_q15x2_ia ((q15_t **) &pInB))); +#else + *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ - *pInB++), 16); + *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ - *pInB++), 16); + *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ - *pInB++), 16); + *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ - *pInB++), 16); +#endif + + /* Decrement loop counter */ blkCnt--; } - /* If the blockSize is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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; +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + while (blkCnt > 0U) { /* C(m,n) = A(m,n) - B(m,n) */ - /* Subtract and then store the results in the destination buffer. */ + + /* Subtract and store result in destination buffer. */ +#if defined (ARM_MATH_DSP) + *pOut++ = (q15_t) __QSUB16(*pInA++, *pInB++); +#else *pOut++ = (q15_t) __SSAT(((q31_t) * pInA++ - *pInB++), 16); +#endif - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } -#endif /* #if defined (ARM_MATH_DSP) */ - /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -144,5 +140,5 @@ arm_status arm_mat_sub_q15( } /** - * @} end of MatrixSub group + @} end of MatrixSub group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c b/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c index 3bf5508..40d1bef 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_sub_q31.c * Description: Q31 matrix subtraction * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,157 +29,100 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixSub - * @{ + @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 - * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - * - * <b>Scaling and Overflow Behavior:</b> - * \par - * The function uses saturating arithmetic. - * Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated. + @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 execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + + @par Scaling and Overflow Behavior + The function uses saturating arithmetic. + Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated. */ - arm_status arm_mat_sub_q31( const arm_matrix_instance_q31 * pSrcA, const arm_matrix_instance_q31 * pSrcB, - arm_matrix_instance_q31 * pDst) + 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 *pInB = 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 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 */ + + /* Check for matrix mismatch condition */ if ((pSrcA->numRows != pSrcB->numRows) || - (pSrcA->numCols != pSrcB->numCols) || - (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols)) + (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 + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ + { - /* Total number of samples in the input matrix */ + /* Total number of samples in input matrix */ numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols; -#if defined (ARM_MATH_DSP) - - /* Run the below code for Cortex-M4 and Cortex-M3 */ +#if defined (ARM_MATH_LOOPUNROLL) - /* Loop Unrolling */ + /* Loop unrolling: Compute 4 outputs at a time */ 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, saturate and then store the results in the destination buffer. */ + *pOut++ = __QSUB(*pInA++, *pInB++); - /* Subtract and saturate */ - out2 = __QSUB(inA2, inB2); + *pOut++ = __QSUB(*pInA++, *pInB++); - /* Store result in destination */ - pOut[2] = out1; - pOut[3] = out2; + *pOut++ = __QSUB(*pInA++, *pInB++); - /* update pointers to process next samples */ - pIn1 += 4U; - pIn2 += 4U; - pOut += 4U; + *pOut++ = __QSUB(*pInA++, *pInB++); - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } - /* If the numSamples is not a multiple of 4, compute any remaining output samples here. - ** No loop unrolling is used. */ + /* Loop unrolling: Compute remaining outputs */ 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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 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; + /* Subtract, saturate and store result in destination buffer. */ + *pOut++ = __QSUB(*pInA++, *pInB++); - /* Decrement the loop counter */ + /* Decrement loop counter */ blkCnt--; } @@ -192,5 +135,5 @@ arm_status arm_mat_sub_q31( } /** - * @} end of MatrixSub group + @} end of MatrixSub group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c b/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c index 84165ce..71748bf 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c +++ b/DSP/Source/MatrixFunctions/arm_mat_trans_f32.c @@ -3,8 +3,8 @@ * Title: arm_mat_trans_f32.c * Description: Floating-point matrix transpose * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ @@ -26,33 +26,36 @@ * limitations under the License. */ +#include "arm_math.h" + /** - * @defgroup MatrixTrans Matrix Transpose - * - * Tranposes a matrix. - * Transposing an <code>M x N</code> matrix flips it around the center diagonal and results in an <code>N x M</code> matrix. - * \image html MatrixTranspose.gif "Transpose of a 3 x 3 matrix" + @ingroup groupMatrix */ -#include "arm_math.h" - /** - * @ingroup groupMatrix + @defgroup MatrixTrans Matrix Transpose + + Tranposes a matrix. + + Transposing an <code>M x N</code> matrix flips it around the center diagonal and results in an <code>N x M</code> matrix. + \image html MatrixTranspose.gif "Transpose of a 3 x 3 matrix" */ /** - * @addtogroup MatrixTrans - * @{ + @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 <code>ARM_MATH_SIZE_MISMATCH</code> - * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. - */ + @brief Floating-point matrix transpose. + @param[in] pSrc points to input matrix + @param[out] pDst points to output matrix + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed + */ +#if defined(ARM_MATH_NEON) arm_status arm_mat_trans_f32( const arm_matrix_instance_f32 * pSrc, @@ -64,17 +67,11 @@ arm_status arm_mat_trans_f32( 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 */ + uint16_t blkCnt, rowCnt, 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)) { @@ -86,41 +83,44 @@ arm_status arm_mat_trans_f32( { /* Matrix transpose by exchanging the rows with columns */ - /* row loop */ - do + /* Row loop */ + rowCnt = row >> 2; + while (rowCnt > 0U) { - /* Loop Unrolling */ + float32x4_t row0V,row1V,row2V,row3V; + float32x4x2_t ra0,ra1,rb0,rb1; + 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. + /* Compute 4 outputs at a time. ** a second loop below computes the remaining 1 to 3 samples. */ - while (blkCnt > 0U) /* column loop */ + while (blkCnt > 0U) /* Column loop */ { - /* Read and store the input element in the destination */ - *px = *pIn++; + row0V = vld1q_f32(pIn); + row1V = vld1q_f32(pIn + 1 * nColumns); + row2V = vld1q_f32(pIn + 2 * nColumns); + row3V = vld1q_f32(pIn + 3 * nColumns); + pIn += 4; - /* Update the pointer px to point to the next row of the transposed matrix */ - px += nRows; + ra0 = vzipq_f32(row0V,row2V); + ra1 = vzipq_f32(row1V,row3V); - /* Read and store the input element in the destination */ - *px = *pIn++; + rb0 = vzipq_f32(ra0.val[0],ra1.val[0]); + rb1 = vzipq_f32(ra0.val[1],ra1.val[1]); - /* Update the pointer px to point to the next row of the transposed matrix */ + vst1q_f32(px,rb0.val[0]); 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 */ + vst1q_f32(px,rb0.val[1]); px += nRows; - /* Read and store the input element in the destination */ - *px = *pIn++; + vst1q_f32(px,rb1.val[0]); + px += nRows; - /* Update the pointer px to point to the next row of the transposed matrix */ + vst1q_f32(px,rb1.val[1]); px += nRows; /* Decrement the column loop counter */ @@ -133,6 +133,36 @@ arm_status arm_mat_trans_f32( while (blkCnt > 0U) { /* Read and store the input element in the destination */ + *px++ = *pIn; + *px++ = *(pIn + 1 * nColumns); + *px++ = *(pIn + 2 * nColumns); + *px++ = *(pIn + 3 * nColumns); + + px += (nRows - 4); + pIn++; + + /* Decrement the column loop counter */ + blkCnt--; + } + + i += 4; + pIn += 3 * nColumns; + + /* Decrement the row loop counter */ + rowCnt--; + + } /* Row loop end */ + + rowCnt = row & 3; + while (rowCnt > 0U) + { + blkCnt = nColumns ; + /* The pointer px is set to starting address of the column being processed */ + px = pOut + i; + + 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 */ @@ -141,57 +171,104 @@ arm_status arm_mat_trans_f32( /* Decrement the column loop counter */ blkCnt--; } + i++; + rowCnt -- ; + } -#else - - /* Run the below code for Cortex-M0 */ - - uint16_t col, i = 0U, row = nRows; /* loop counters */ - arm_status status; /* status of matrix transpose */ + /* Set status as ARM_MATH_SUCCESS */ + status = ARM_MATH_SUCCESS; + } + /* Return to application */ + return (status); +} +#else +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 nCols = pSrc->numCols; /* number of columns */ + uint32_t col, row = nRows, i = 0U; /* 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)) + 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 */ + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { /* Matrix transpose by exchanging the rows with columns */ - /* row loop */ + /* row loop */ do { - /* The pointer px is set to starting address of the column being processed */ + /* Pointer px is set to starting address of column being processed */ px = pOut + i; - /* Initialize column loop counter */ - col = nColumns; +#if defined (ARM_MATH_LOOPUNROLL) - while (col > 0U) + /* Loop unrolling: Compute 4 outputs at a time */ + col = nCols >> 2U; + + while (col > 0U) /* column loop */ { - /* Read and store the input element in the destination */ + /* Read and store input element in destination */ *px = *pIn++; + /* Update pointer px to point to next row of transposed matrix */ + px += nRows; - /* Update the pointer px to point to the next row of the transposed matrix */ + *px = *pIn++; px += nRows; - /* Decrement the column loop counter */ + *px = *pIn++; + px += nRows; + + *px = *pIn++; + px += nRows; + + /* Decrement column loop counter */ col--; } -#endif /* #if defined (ARM_MATH_DSP) */ + /* Loop unrolling: Compute remaining outputs */ + col = nCols % 0x4U; + +#else + + /* Initialize col with number of samples */ + col = nCols; + +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ + + while (col > 0U) + { + /* Read and store input element in destination */ + *px = *pIn++; + + /* Update pointer px to point to next row of transposed matrix */ + px += nRows; + + /* Decrement column loop counter */ + col--; + } i++; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; - } while (row > 0U); /* row loop end */ + } while (row > 0U); /* row loop end */ /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; @@ -200,6 +277,7 @@ arm_status arm_mat_trans_f32( /* Return to application */ return (status); } +#endif /* #if defined(ARM_MATH_NEON) */ /** * @} end of MatrixTrans group diff --git a/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c b/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c index 6ba0904..707e0d6 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c +++ b/DSP/Source/MatrixFunctions/arm_mat_trans_q15.c @@ -3,13 +3,13 @@ * Title: arm_mat_trans_q15.c * Description: Q15 matrix transpose * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,244 +29,154 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixTrans - * @{ + @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 <code>ARM_MATH_SIZE_MISMATCH</code> - * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. +/** + @brief Q15 matrix transpose. + @param[in] pSrc points to input matrix + @param[out] pDst points to output matrix + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed */ arm_status arm_mat_trans_q15( const arm_matrix_instance_q15 * pSrc, - arm_matrix_instance_q15 * pDst) + 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) + q15_t *pIn = pSrc->pData; /* input data matrix pointer */ + q15_t *pOut = pDst->pData; /* output data matrix pointer */ + uint16_t nRows = pSrc->numRows; /* number of rows */ + uint16_t nCols = pSrc->numCols; /* number of columns */ + uint32_t col, row = nRows, i = 0U; /* Loop counters */ + arm_status status; /* status of matrix transpose */ - /* 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 */ +#if defined (ARM_MATH_LOOPUNROLL) + q31_t in; /* variable to hold temporary output */ +#endif #ifdef ARM_MATH_MATRIX_CHECK - /* Check for matrix mismatch condition */ - if ((pSrc->numRows != pDst->numCols) || (pSrc->numCols != pDst->numRows)) + 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 */ + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { /* Matrix transpose by exchanging the rows with columns */ - /* row loop */ + /* row loop */ do { + /* Pointer pOut is set to starting address of column being processed */ + pOut = pDst->pData + i; - /* Apply loop unrolling and exchange the columns with row elements */ - col = nColumns >> 2U; +#if defined (ARM_MATH_LOOPUNROLL) - /* The pointer pOut is set to starting address of the column being processed */ - pOut = pDst->pData + i; + /* Loop unrolling: Compute 4 outputs at a time */ + col = nCols >> 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 (col > 0U) + while (col > 0U) /* column loop */ { -#ifndef UNALIGNED_SUPPORT_DISABLE - - /* Read two elements from the row */ - in = *__SIMD32(pSrcA)++; + /* Read two elements from row */ + in = read_q15x2_ia ((q15_t **) &pIn); - /* Unpack and store one element in the destination */ + /* Unpack and store one element in 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 */ -#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - - /* Update the pointer pOut to point to the next row of the transposed matrix */ + /* Update pointer pOut to point to next row of transposed matrix */ pOut += nRows; - /* Unpack and store the second element in the destination */ - + /* Unpack and store second element in 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 */ -#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - - /* Update the pointer pOut to point to the next row of the transposed matrix */ + /* Update pointer pOut to point to next row of transposed matrix */ pOut += nRows; - /* Read two elements from the row */ -#ifndef ARM_MATH_BIG_ENDIAN - - in = *__SIMD32(pSrcA)++; - -#else + /* Read two elements from row */ + in = read_q15x2_ia ((q15_t **) &pIn); - in = *__SIMD32(pSrcA)++; - -#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - - /* Unpack and store one element in the destination */ + /* Unpack and store one element in 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 */ +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ - /* Update the pointer pOut to point to the next row of the transposed matrix */ + /* Update pointer pOut to point to next row of transposed matrix */ pOut += nRows; - /* Unpack and store the second element in the destination */ + /* Unpack and store second element in 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 */ -#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 */ + /* Update pointer pOut to point to next row of transposed matrix */ pOut += nRows; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } - /* Perform matrix transpose for last 3 samples here. */ - col = nColumns % 0x4U; + /* Loop unrolling: Compute remaining outputs */ + col = nCols % 0x4U; #else - /* Run the below code for Cortex-M0 */ + /* Initialize col with number of samples */ + col = nCols; -#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) */ +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ while (col > 0U) { - /* Read and store the input element in the destination */ - *pOut = *pSrcA++; + /* Read and store input element in destination */ + *pOut = *pIn++; - /* Update the pointer pOut to point to the next row of the transposed matrix */ + /* Update pointer pOut to point to next row of transposed matrix */ pOut += nRows; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } i++; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; - } while (row > 0U); + } while (row > 0U); /* row loop end */ - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } + /* Return to application */ return (status); } /** - * @} end of MatrixTrans group + @} end of MatrixTrans group */ diff --git a/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c b/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c index 6f698e0..5d0b5e2 100644 --- a/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c +++ b/DSP/Source/MatrixFunctions/arm_mat_trans_q31.c @@ -3,13 +3,13 @@ * Title: arm_mat_trans_q31.c * Description: Q31 matrix transpose * - * $Date: 27. January 2017 - * $Revision: V.1.5.1 + * $Date: 18. March 2019 + * $Revision: V1.6.0 * * Target Processor: Cortex-M cores * -------------------------------------------------------------------- */ /* - * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. * * SPDX-License-Identifier: Apache-2.0 * @@ -29,163 +29,111 @@ #include "arm_math.h" /** - * @ingroup groupMatrix + @ingroup groupMatrix */ /** - * @addtogroup MatrixTrans - * @{ + @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 <code>ARM_MATH_SIZE_MISMATCH</code> - * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking. +/** + @brief Q31 matrix transpose. + @param[in] pSrc points to input matrix + @param[out] pDst points to output matrix + @return execution status + - \ref ARM_MATH_SUCCESS : Operation successful + - \ref ARM_MATH_SIZE_MISMATCH : Matrix size check failed */ arm_status arm_mat_trans_q31( const arm_matrix_instance_q31 * pSrc, - arm_matrix_instance_q31 * pDst) + 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 *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 */ + uint16_t nRows = pSrc->numRows; /* number of rows */ + uint16_t nCols = pSrc->numCols; /* number of columns */ + uint32_t col, row = nRows, i = 0U; /* 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)) + 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 */ + +#endif /* #ifdef ARM_MATH_MATRIX_CHECK */ { /* Matrix transpose by exchanging the rows with columns */ - /* row loop */ + /* 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 */ + /* Pointer px is set to starting address of 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++; +#if defined (ARM_MATH_LOOPUNROLL) - /* Update the pointer px to point to the next row of the transposed matrix */ - px += nRows; + /* Loop unrolling: Compute 4 outputs at a time */ + col = nCols >> 2U; - /* Read and store the input element in the destination */ + while (col > 0U) /* column loop */ + { + /* Read and store input element in destination */ *px = *pIn++; - - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of 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--; + /* Decrement column loop counter */ + col--; } -#else - - /* Run the below code for Cortex-M0 */ - - uint16_t col, i = 0U, row = nRows; /* loop counters */ - arm_status status; /* status of matrix transpose */ - + /* Loop unrolling: Compute remaining outputs */ + col = nCols % 0x4U; -#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 */ +#else - { - /* 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 col with number of samples */ + col = nCols; - /* Initialize column loop counter */ - col = nColumns; +#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ while (col > 0U) { - /* Read and store the input element in the destination */ + /* Read and store input element in destination */ *px = *pIn++; - /* Update the pointer px to point to the next row of the transposed matrix */ + /* Update pointer px to point to next row of transposed matrix */ px += nRows; - /* Decrement the column loop counter */ + /* Decrement column loop counter */ col--; } -#endif /* #if defined (ARM_MATH_DSP) */ - i++; - /* Decrement the row loop counter */ + /* Decrement row loop counter */ row--; - } - while (row > 0U); /* row loop end */ + } while (row > 0U); /* row loop end */ - /* set status as ARM_MATH_SUCCESS */ + /* Set status as ARM_MATH_SUCCESS */ status = ARM_MATH_SUCCESS; } @@ -194,5 +142,5 @@ arm_status arm_mat_trans_q31( } /** - * @} end of MatrixTrans group + @} end of MatrixTrans group */ |