123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207 |
- #include "arm_math.h"
- arm_status arm_mat_add_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst)
- {
- q31_t *pIn1 = pSrcA->pData;
- q31_t *pIn2 = pSrcB->pData;
- q31_t *pOut = pDst->pData;
- q31_t inA1, inB1;
- #ifndef ARM_MATH_CM0_FAMILY
- q31_t inA2, inB2;
- q31_t out1, out2;
- #endif
- uint32_t numSamples;
- uint32_t blkCnt;
- arm_status status;
- #ifdef ARM_MATH_MATRIX_CHECK
-
- if((pSrcA->numRows != pSrcB->numRows) ||
- (pSrcA->numCols != pSrcB->numCols) ||
- (pSrcA->numRows != pDst->numRows) || (pSrcA->numCols != pDst->numCols))
- {
-
- status = ARM_MATH_SIZE_MISMATCH;
- }
- else
- #endif
- {
-
- numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols;
- #ifndef ARM_MATH_CM0_FAMILY
-
-
- blkCnt = numSamples >> 2u;
-
- while(blkCnt > 0u)
- {
-
-
-
- inA1 = pIn1[0];
-
- inB1 = pIn2[0];
-
- inA2 = pIn1[1];
-
- out1 = __QADD(inA1, inB1);
-
- inB2 = pIn2[1];
-
- inA1 = pIn1[2];
-
- out2 = __QADD(inA2, inB2);
-
- inB1 = pIn2[2];
-
- pOut[0] = out1;
- pOut[1] = out2;
-
- inA2 = pIn1[3];
-
- inB2 = pIn2[3];
-
- out1 = __QADD(inA1, inB1);
- out2 = __QADD(inA2, inB2);
-
- pOut[2] = out1;
- pOut[3] = out2;
-
- pIn1 += 4u;
- pIn2 += 4u;
- pOut += 4u;
-
- blkCnt--;
- }
-
- blkCnt = numSamples % 0x4u;
- #else
-
-
- blkCnt = numSamples;
- #endif
- while(blkCnt > 0u)
- {
-
-
- inA1 = *pIn1++;
- inB1 = *pIn2++;
- inA1 = __QADD(inA1, inB1);
-
- blkCnt--;
- *pOut++ = inA1;
- }
-
- status = ARM_MATH_SUCCESS;
- }
-
- return (status);
- }
|