Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

Dependents:   Nucleo-Heart-Rate ejercicioVrms2 PROYECTOFINAL ejercicioVrms ... more

functions/MatrixFunctions/arm_mat_mult_q31.c@3:4098b9d3d571, 2018-06-21 (annotated)

Committer:
xorjoep
Date:
Thu Jun 21 11:56:27 2018 +0000
Revision:
3:4098b9d3d571
Parent:
1:24714b45cd1b 
headers is a folder not a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xorjoep 1:24714b45cd1b 1 /* ----------------------------------------------------------------------
xorjoep 1:24714b45cd1b 2 * Project: CMSIS DSP Library
xorjoep 1:24714b45cd1b 3 * Title: arm_mat_mult_q31.c
xorjoep 1:24714b45cd1b 4 * Description: Q31 matrix multiplication
xorjoep 1:24714b45cd1b 5 *
xorjoep 1:24714b45cd1b 6 * $Date: 27. January 2017
xorjoep 1:24714b45cd1b 7 * $Revision: V.1.5.1
xorjoep 1:24714b45cd1b 8 *
xorjoep 1:24714b45cd1b 9 * Target Processor: Cortex-M cores
xorjoep 1:24714b45cd1b 10 * -------------------------------------------------------------------- */
xorjoep 1:24714b45cd1b 11 /*
xorjoep 1:24714b45cd1b 12 * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
xorjoep 1:24714b45cd1b 13 *
xorjoep 1:24714b45cd1b 14 * SPDX-License-Identifier: Apache-2.0
xorjoep 1:24714b45cd1b 15 *
xorjoep 1:24714b45cd1b 16 * Licensed under the Apache License, Version 2.0 (the License); you may
xorjoep 1:24714b45cd1b 17 * not use this file except in compliance with the License.
xorjoep 1:24714b45cd1b 18 * You may obtain a copy of the License at
xorjoep 1:24714b45cd1b 19 *
xorjoep 1:24714b45cd1b 20 * www.apache.org/licenses/LICENSE-2.0
xorjoep 1:24714b45cd1b 21 *
xorjoep 1:24714b45cd1b 22 * Unless required by applicable law or agreed to in writing, software
xorjoep 1:24714b45cd1b 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
xorjoep 1:24714b45cd1b 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
xorjoep 1:24714b45cd1b 25 * See the License for the specific language governing permissions and
xorjoep 1:24714b45cd1b 26 * limitations under the License.
xorjoep 1:24714b45cd1b 27 */
xorjoep 1:24714b45cd1b 28
xorjoep 1:24714b45cd1b 29 #include "arm_math.h"
xorjoep 1:24714b45cd1b 30
xorjoep 1:24714b45cd1b 31 /**
xorjoep 1:24714b45cd1b 32 * @ingroup groupMatrix
xorjoep 1:24714b45cd1b 33 */
xorjoep 1:24714b45cd1b 34
xorjoep 1:24714b45cd1b 35 /**
xorjoep 1:24714b45cd1b 36 * @addtogroup MatrixMult
xorjoep 1:24714b45cd1b 37 * @{
xorjoep 1:24714b45cd1b 38 */
xorjoep 1:24714b45cd1b 39
xorjoep 1:24714b45cd1b 40 /**
xorjoep 1:24714b45cd1b 41 * @brief Q31 matrix multiplication
xorjoep 1:24714b45cd1b 42 * @param[in] *pSrcA points to the first input matrix structure
xorjoep 1:24714b45cd1b 43 * @param[in] *pSrcB points to the second input matrix structure
xorjoep 1:24714b45cd1b 44 * @param[out] *pDst points to output matrix structure
xorjoep 1:24714b45cd1b 45 * @return The function returns either
xorjoep 1:24714b45cd1b 46 * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
xorjoep 1:24714b45cd1b 47 *
xorjoep 1:24714b45cd1b 48 * @details
xorjoep 1:24714b45cd1b 49 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 50 *
xorjoep 1:24714b45cd1b 51 * \par
xorjoep 1:24714b45cd1b 52 * The function is implemented using an internal 64-bit accumulator.
xorjoep 1:24714b45cd1b 53 * The accumulator has a 2.62 format and maintains full precision of the intermediate
xorjoep 1:24714b45cd1b 54 * multiplication results but provides only a single guard bit. There is no saturation
xorjoep 1:24714b45cd1b 55 * on intermediate additions. Thus, if the accumulator overflows it wraps around and
xorjoep 1:24714b45cd1b 56 * distorts the result. The input signals should be scaled down to avoid intermediate
xorjoep 1:24714b45cd1b 57 * overflows. The input is thus scaled down by log2(numColsA) bits
xorjoep 1:24714b45cd1b 58 * to avoid overflows, as a total of numColsA additions are performed internally.
xorjoep 1:24714b45cd1b 59 * The 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
xorjoep 1:24714b45cd1b 60 *
xorjoep 1:24714b45cd1b 61 * \par
xorjoep 1:24714b45cd1b 62 * See <code>arm_mat_mult_fast_q31()</code> for a faster but less precise implementation of this function for Cortex-M3 and Cortex-M4.
xorjoep 1:24714b45cd1b 63 *
xorjoep 1:24714b45cd1b 64 */
xorjoep 1:24714b45cd1b 65
xorjoep 1:24714b45cd1b 66 arm_status arm_mat_mult_q31(
xorjoep 1:24714b45cd1b 67 const arm_matrix_instance_q31 * pSrcA,
xorjoep 1:24714b45cd1b 68 const arm_matrix_instance_q31 * pSrcB,
xorjoep 1:24714b45cd1b 69 arm_matrix_instance_q31 * pDst)
xorjoep 1:24714b45cd1b 70 {
xorjoep 1:24714b45cd1b 71 q31_t *pIn1 = pSrcA->pData; /* input data matrix pointer A */
xorjoep 1:24714b45cd1b 72 q31_t *pIn2 = pSrcB->pData; /* input data matrix pointer B */
xorjoep 1:24714b45cd1b 73 q31_t *pInA = pSrcA->pData; /* input data matrix pointer A */
xorjoep 1:24714b45cd1b 74 q31_t *pOut = pDst->pData; /* output data matrix pointer */
xorjoep 1:24714b45cd1b 75 q31_t *px; /* Temporary output data matrix pointer */
xorjoep 1:24714b45cd1b 76 q63_t sum; /* Accumulator */
xorjoep 1:24714b45cd1b 77 uint16_t numRowsA = pSrcA->numRows; /* number of rows of input matrix A */
xorjoep 1:24714b45cd1b 78 uint16_t numColsB = pSrcB->numCols; /* number of columns of input matrix B */
xorjoep 1:24714b45cd1b 79 uint16_t numColsA = pSrcA->numCols; /* number of columns of input matrix A */
xorjoep 1:24714b45cd1b 80
xorjoep 1:24714b45cd1b 81 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 82
xorjoep 1:24714b45cd1b 83 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 84
xorjoep 1:24714b45cd1b 85 uint16_t col, i = 0U, j, row = numRowsA, colCnt; /* loop counters */
xorjoep 1:24714b45cd1b 86 arm_status status; /* status of matrix multiplication */
xorjoep 1:24714b45cd1b 87 q31_t a0, a1, a2, a3, b0, b1, b2, b3;
xorjoep 1:24714b45cd1b 88
xorjoep 1:24714b45cd1b 89 #ifdef ARM_MATH_MATRIX_CHECK
xorjoep 1:24714b45cd1b 90
xorjoep 1:24714b45cd1b 91
xorjoep 1:24714b45cd1b 92 /* Check for matrix mismatch condition */
xorjoep 1:24714b45cd1b 93 if ((pSrcA->numCols != pSrcB->numRows) ||
xorjoep 1:24714b45cd1b 94 (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols))
xorjoep 1:24714b45cd1b 95 {
xorjoep 1:24714b45cd1b 96 /* Set status as ARM_MATH_SIZE_MISMATCH */
xorjoep 1:24714b45cd1b 97 status = ARM_MATH_SIZE_MISMATCH;
xorjoep 1:24714b45cd1b 98 }
xorjoep 1:24714b45cd1b 99 else
xorjoep 1:24714b45cd1b 100 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
xorjoep 1:24714b45cd1b 101
xorjoep 1:24714b45cd1b 102 {
xorjoep 1:24714b45cd1b 103 /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */
xorjoep 1:24714b45cd1b 104 /* row loop */
xorjoep 1:24714b45cd1b 105 do
xorjoep 1:24714b45cd1b 106 {
xorjoep 1:24714b45cd1b 107 /* Output pointer is set to starting address of the row being processed */
xorjoep 1:24714b45cd1b 108 px = pOut + i;
xorjoep 1:24714b45cd1b 109
xorjoep 1:24714b45cd1b 110 /* For every row wise process, the column loop counter is to be initiated */
xorjoep 1:24714b45cd1b 111 col = numColsB;
xorjoep 1:24714b45cd1b 112
xorjoep 1:24714b45cd1b 113 /* For every row wise process, the pIn2 pointer is set
xorjoep 1:24714b45cd1b 114 ** to the starting address of the pSrcB data */
xorjoep 1:24714b45cd1b 115 pIn2 = pSrcB->pData;
xorjoep 1:24714b45cd1b 116
xorjoep 1:24714b45cd1b 117 j = 0U;
xorjoep 1:24714b45cd1b 118
xorjoep 1:24714b45cd1b 119 /* column loop */
xorjoep 1:24714b45cd1b 120 do
xorjoep 1:24714b45cd1b 121 {
xorjoep 1:24714b45cd1b 122 /* Set the variable sum, that acts as accumulator, to zero */
xorjoep 1:24714b45cd1b 123 sum = 0;
xorjoep 1:24714b45cd1b 124
xorjoep 1:24714b45cd1b 125 /* Initiate the pointer pIn1 to point to the starting address of pInA */
xorjoep 1:24714b45cd1b 126 pIn1 = pInA;
xorjoep 1:24714b45cd1b 127
xorjoep 1:24714b45cd1b 128 /* Apply loop unrolling and compute 4 MACs simultaneously. */
xorjoep 1:24714b45cd1b 129 colCnt = numColsA >> 2;
xorjoep 1:24714b45cd1b 130
xorjoep 1:24714b45cd1b 131
xorjoep 1:24714b45cd1b 132 /* matrix multiplication */
xorjoep 1:24714b45cd1b 133 while (colCnt > 0U)
xorjoep 1:24714b45cd1b 134 {
xorjoep 1:24714b45cd1b 135 /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */
xorjoep 1:24714b45cd1b 136 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 137 b0 = *pIn2;
xorjoep 1:24714b45cd1b 138 pIn2 += numColsB;
xorjoep 1:24714b45cd1b 139
xorjoep 1:24714b45cd1b 140 a0 = *pIn1++;
xorjoep 1:24714b45cd1b 141 a1 = *pIn1++;
xorjoep 1:24714b45cd1b 142
xorjoep 1:24714b45cd1b 143 b1 = *pIn2;
xorjoep 1:24714b45cd1b 144 pIn2 += numColsB;
xorjoep 1:24714b45cd1b 145 b2 = *pIn2;
xorjoep 1:24714b45cd1b 146 pIn2 += numColsB;
xorjoep 1:24714b45cd1b 147
xorjoep 1:24714b45cd1b 148 sum += (q63_t) a0 *b0;
xorjoep 1:24714b45cd1b 149 sum += (q63_t) a1 *b1;
xorjoep 1:24714b45cd1b 150
xorjoep 1:24714b45cd1b 151 a2 = *pIn1++;
xorjoep 1:24714b45cd1b 152 a3 = *pIn1++;
xorjoep 1:24714b45cd1b 153
xorjoep 1:24714b45cd1b 154 b3 = *pIn2;
xorjoep 1:24714b45cd1b 155 pIn2 += numColsB;
xorjoep 1:24714b45cd1b 156
xorjoep 1:24714b45cd1b 157 sum += (q63_t) a2 *b2;
xorjoep 1:24714b45cd1b 158 sum += (q63_t) a3 *b3;
xorjoep 1:24714b45cd1b 159
xorjoep 1:24714b45cd1b 160 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 161 colCnt--;
xorjoep 1:24714b45cd1b 162 }
xorjoep 1:24714b45cd1b 163
xorjoep 1:24714b45cd1b 164 /* If the columns of pSrcA is not a multiple of 4, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 165 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 166 colCnt = numColsA % 0x4U;
xorjoep 1:24714b45cd1b 167
xorjoep 1:24714b45cd1b 168 while (colCnt > 0U)
xorjoep 1:24714b45cd1b 169 {
xorjoep 1:24714b45cd1b 170 /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */
xorjoep 1:24714b45cd1b 171 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 172 sum += (q63_t) * pIn1++ * *pIn2;
xorjoep 1:24714b45cd1b 173 pIn2 += numColsB;
xorjoep 1:24714b45cd1b 174
xorjoep 1:24714b45cd1b 175 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 176 colCnt--;
xorjoep 1:24714b45cd1b 177 }
xorjoep 1:24714b45cd1b 178
xorjoep 1:24714b45cd1b 179 /* Convert the result from 2.62 to 1.31 format and store in destination buffer */
xorjoep 1:24714b45cd1b 180 *px++ = (q31_t) (sum >> 31);
xorjoep 1:24714b45cd1b 181
xorjoep 1:24714b45cd1b 182 /* Update the pointer pIn2 to point to the starting address of the next column */
xorjoep 1:24714b45cd1b 183 j++;
xorjoep 1:24714b45cd1b 184 pIn2 = (pSrcB->pData) + j;
xorjoep 1:24714b45cd1b 185
xorjoep 1:24714b45cd1b 186 /* Decrement the column loop counter */
xorjoep 1:24714b45cd1b 187 col--;
xorjoep 1:24714b45cd1b 188
xorjoep 1:24714b45cd1b 189 } while (col > 0U);
xorjoep 1:24714b45cd1b 190
xorjoep 1:24714b45cd1b 191 #else
xorjoep 1:24714b45cd1b 192
xorjoep 1:24714b45cd1b 193 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 194
xorjoep 1:24714b45cd1b 195 q31_t *pInB = pSrcB->pData; /* input data matrix pointer B */
xorjoep 1:24714b45cd1b 196 uint16_t col, i = 0U, row = numRowsA, colCnt; /* loop counters */
xorjoep 1:24714b45cd1b 197 arm_status status; /* status of matrix multiplication */
xorjoep 1:24714b45cd1b 198
xorjoep 1:24714b45cd1b 199
xorjoep 1:24714b45cd1b 200 #ifdef ARM_MATH_MATRIX_CHECK
xorjoep 1:24714b45cd1b 201
xorjoep 1:24714b45cd1b 202 /* Check for matrix mismatch condition */
xorjoep 1:24714b45cd1b 203 if ((pSrcA->numCols != pSrcB->numRows) ||
xorjoep 1:24714b45cd1b 204 (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols))
xorjoep 1:24714b45cd1b 205 {
xorjoep 1:24714b45cd1b 206 /* Set status as ARM_MATH_SIZE_MISMATCH */
xorjoep 1:24714b45cd1b 207 status = ARM_MATH_SIZE_MISMATCH;
xorjoep 1:24714b45cd1b 208 }
xorjoep 1:24714b45cd1b 209 else
xorjoep 1:24714b45cd1b 210 #endif /* #ifdef ARM_MATH_MATRIX_CHECK */
xorjoep 1:24714b45cd1b 211
xorjoep 1:24714b45cd1b 212 {
xorjoep 1:24714b45cd1b 213 /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */
xorjoep 1:24714b45cd1b 214 /* row loop */
xorjoep 1:24714b45cd1b 215 do
xorjoep 1:24714b45cd1b 216 {
xorjoep 1:24714b45cd1b 217 /* Output pointer is set to starting address of the row being processed */
xorjoep 1:24714b45cd1b 218 px = pOut + i;
xorjoep 1:24714b45cd1b 219
xorjoep 1:24714b45cd1b 220 /* For every row wise process, the column loop counter is to be initiated */
xorjoep 1:24714b45cd1b 221 col = numColsB;
xorjoep 1:24714b45cd1b 222
xorjoep 1:24714b45cd1b 223 /* For every row wise process, the pIn2 pointer is set
xorjoep 1:24714b45cd1b 224 ** to the starting address of the pSrcB data */
xorjoep 1:24714b45cd1b 225 pIn2 = pSrcB->pData;
xorjoep 1:24714b45cd1b 226
xorjoep 1:24714b45cd1b 227 /* column loop */
xorjoep 1:24714b45cd1b 228 do
xorjoep 1:24714b45cd1b 229 {
xorjoep 1:24714b45cd1b 230 /* Set the variable sum, that acts as accumulator, to zero */
xorjoep 1:24714b45cd1b 231 sum = 0;
xorjoep 1:24714b45cd1b 232
xorjoep 1:24714b45cd1b 233 /* Initiate the pointer pIn1 to point to the starting address of pInA */
xorjoep 1:24714b45cd1b 234 pIn1 = pInA;
xorjoep 1:24714b45cd1b 235
xorjoep 1:24714b45cd1b 236 /* Matrix A columns number of MAC operations are to be performed */
xorjoep 1:24714b45cd1b 237 colCnt = numColsA;
xorjoep 1:24714b45cd1b 238
xorjoep 1:24714b45cd1b 239 /* matrix multiplication */
xorjoep 1:24714b45cd1b 240 while (colCnt > 0U)
xorjoep 1:24714b45cd1b 241 {
xorjoep 1:24714b45cd1b 242 /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */
xorjoep 1:24714b45cd1b 243 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 244 sum += (q63_t) * pIn1++ * *pIn2;
xorjoep 1:24714b45cd1b 245 pIn2 += numColsB;
xorjoep 1:24714b45cd1b 246
xorjoep 1:24714b45cd1b 247 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 248 colCnt--;
xorjoep 1:24714b45cd1b 249 }
xorjoep 1:24714b45cd1b 250
xorjoep 1:24714b45cd1b 251 /* Convert the result from 2.62 to 1.31 format and store in destination buffer */
xorjoep 1:24714b45cd1b 252 *px++ = (q31_t) clip_q63_to_q31(sum >> 31);
xorjoep 1:24714b45cd1b 253
xorjoep 1:24714b45cd1b 254 /* Decrement the column loop counter */
xorjoep 1:24714b45cd1b 255 col--;
xorjoep 1:24714b45cd1b 256
xorjoep 1:24714b45cd1b 257 /* Update the pointer pIn2 to point to the starting address of the next column */
xorjoep 1:24714b45cd1b 258 pIn2 = pInB + (numColsB - col);
xorjoep 1:24714b45cd1b 259
xorjoep 1:24714b45cd1b 260 } while (col > 0U);
xorjoep 1:24714b45cd1b 261
xorjoep 1:24714b45cd1b 262 #endif
xorjoep 1:24714b45cd1b 263
xorjoep 1:24714b45cd1b 264 /* Update the pointer pInA to point to the starting address of the next row */
xorjoep 1:24714b45cd1b 265 i = i + numColsB;
xorjoep 1:24714b45cd1b 266 pInA = pInA + numColsA;
xorjoep 1:24714b45cd1b 267
xorjoep 1:24714b45cd1b 268 /* Decrement the row loop counter */
xorjoep 1:24714b45cd1b 269 row--;
xorjoep 1:24714b45cd1b 270
xorjoep 1:24714b45cd1b 271 } while (row > 0U);
xorjoep 1:24714b45cd1b 272
xorjoep 1:24714b45cd1b 273 /* set status as ARM_MATH_SUCCESS */
xorjoep 1:24714b45cd1b 274 status = ARM_MATH_SUCCESS;
xorjoep 1:24714b45cd1b 275 }
xorjoep 1:24714b45cd1b 276 /* Return to application */
xorjoep 1:24714b45cd1b 277 return (status);
xorjoep 1:24714b45cd1b 278 }
xorjoep 1:24714b45cd1b 279
xorjoep 1:24714b45cd1b 280 /**
xorjoep 1:24714b45cd1b 281 * @} end of MatrixMult group
xorjoep 1:24714b45cd1b 282 */