initial commitHEADmaster

1 files changed, 196 insertions, 0 deletions

diff --git a/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c b/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c
new file mode 100644
index 0000000..88c8791
--- /dev/null
+++ b/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_q31.c
@@ -0,0 +1,196 @@
+/* ----------------------------------------------------------------------

+ * Project:      CMSIS DSP Library

+ * Title:        arm_mat_mult_q31.c

+ * Description:  Q31 matrix multiplication

+ *

+ * $Date:        18. March 2019

+ * $Revision:    V1.6.0

+ *

+ * Target Processor: Cortex-M cores

+ * -------------------------------------------------------------------- */

+/*

+ * Copyright (C) 2010-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_math.h"

+

+/**

+  @ingroup groupMatrix

+ */

+

+/**

+  @addtogroup MatrixMult

+  @{

+ */

+

+/**

+  @brief         Q31 matrix multiplication.

+  @param[in]     pSrcA      points to the first input matrix structure

+  @param[in]     pSrcB      points to the second input matrix structure

+  @param[out]    pDst       points to output matrix structure

+  @return        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)

+{

+  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 */

+  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)    )

+  {

+    /* 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 row being processed */

+      px = pOut + i;

+

+      /* For every row wise process, column loop counter is to be initiated */

+      col = numColsB;

+

+      /* For every row wise process, pIn2 pointer is set to starting address of pSrcB data */

+      pIn2 = pSrcB->pData;

+

+      /* column loop */

+      do

+      {

+        /* Set the variable sum, that acts as accumulator, to zero */

+        sum = 0;

+

+        /* Initialize pointer pIn1 to point to starting address of column being processed */

+        pIn1 = pInA;

+

+#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 += (q63_t) *pIn1++ * *pIn2;

+          pIn2 += numColsB;

+

+          sum += (q63_t) *pIn1++ * *pIn2;

+          pIn2 += numColsB;

+

+          sum += (q63_t) *pIn1++ * *pIn2;

+          pIn2 += numColsB;

+

+          sum += (q63_t) *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) */

+

+          /* Perform the multiply-accumulates */

+          sum += (q63_t) *pIn1++ * *pIn2;

+          pIn2 += numColsB;

+

+          /* Decrement loop counter */

+          colCnt--;

+        }

+

+        /* Convert result from 2.62 to 1.31 format and store in destination buffer */

+        *px++ = (q31_t) (sum >> 31);

+

+        /* Decrement column loop counter */

+        col--;

+

+        /* Update pointer pIn2 to point to starting address of next column */

+        pIn2 = pInB + (numColsB - col);

+

+      } while (col > 0U);

+

+      /* Update pointer pInA to point to starting address of next row */

+      i = i + numColsB;

+      pInA = pInA + numColsA;

+

+      /* Decrement row loop counter */

+      row--;

+

+    } while (row > 0U);

+

+    /* Set status as ARM_MATH_SUCCESS */

+    status = ARM_MATH_SUCCESS;

+  }

+

+  /* Return to application */

+  return (status);

+}

+

+/**

+  @} end of MatrixMult group

+ */