WebSVN – CharLCD – Blame – /trunk/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.c

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_mat_add_q31.c
		4	* Description: Q31 matrix addition
		5	*
		6	* $Date: 27. January 2017
		7	* $Revision: V.1.5.1
		8	*
		9	* Target Processor: Cortex-M cores
		10	* -------------------------------------------------------------------- */
		11	/*
		12	* Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
		13	*
		14	* SPDX-License-Identifier: Apache-2.0
		15	*
		16	* Licensed under the Apache License, Version 2.0 (the License); you may
		17	* not use this file except in compliance with the License.
		18	* You may obtain a copy of the License at
		19	*
		20	* www.apache.org/licenses/LICENSE-2.0
		21	*
		22	* Unless required by applicable law or agreed to in writing, software
		23	* distributed under the License is distributed on an AS IS BASIS, WITHOUT
		24	* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
		25	* See the License for the specific language governing permissions and
		26	* limitations under the License.
		27	*/
		28
		29	#include "arm_math.h"
		30
		31	/**
		32	* @ingroup groupMatrix
		33	*/
		34
		35	/**
		36	* @addtogroup MatrixAdd
		37	* @{
		38	*/
		39
		40	/**
		41	* @brief Q31 matrix addition.
		42	* @param[in] *pSrcA points to the first input matrix structure
		43	* @param[in] *pSrcB points to the second input matrix structure
		44	* @param[out] *pDst points to output matrix structure
		45	* @return The function returns either
		46	* <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
		47	*
		48	* <b>Scaling and Overflow Behavior:</b>
		49	* \par
		50	* The function uses saturating arithmetic.
		51	* Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] will be saturated.
		52	*/
		53
		54	arm_status arm_mat_add_q31(
		55	const arm_matrix_instance_q31 * pSrcA,
		56	const arm_matrix_instance_q31 * pSrcB,
		57	arm_matrix_instance_q31 * pDst)
		58	{
		59	q31_t pIn1 = pSrcA->pData; / input data matrix pointer A */
		60	q31_t pIn2 = pSrcB->pData; / input data matrix pointer B */
		61	q31_t pOut = pDst->pData; / output data matrix pointer */
		62	q31_t inA1, inB1; /* temporary variables */
		63
		64	#if defined (ARM_MATH_DSP)
		65
		66	q31_t inA2, inB2; /* temporary variables */
		67	q31_t out1, out2; /* temporary variables */
		68
		69	#endif // #if defined (ARM_MATH_DSP)
		70
		71	uint32_t numSamples; /* total number of elements in the matrix */
		72	uint32_t blkCnt; /* loop counters */
		73	arm_status status; /* status of matrix addition */
		74
		75	#ifdef ARM_MATH_MATRIX_CHECK
		76	/* Check for matrix mismatch condition */
		77	if ((pSrcA->numRows != pSrcB->numRows) \|\|
		78	(pSrcA->numCols != pSrcB->numCols) \|\|
		79	(pSrcA->numRows != pDst->numRows) \|\| (pSrcA->numCols != pDst->numCols))
		80	{
		81	/* Set status as ARM_MATH_SIZE_MISMATCH */
		82	status = ARM_MATH_SIZE_MISMATCH;
		83	}
		84	else
		85	#endif
		86	{
		87	/* Total number of samples in the input matrix */
		88	numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols;
		89
		90	#if defined (ARM_MATH_DSP)
		91
		92	/* Run the below code for Cortex-M4 and Cortex-M3 */
		93
		94	/* Loop Unrolling */
		95	blkCnt = numSamples >> 2U;
		96
		97
		98	/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
		99	** a second loop below computes the remaining 1 to 3 samples. */
		100	while (blkCnt > 0U)
		101	{
		102	/* C(m,n) = A(m,n) + B(m,n) */
		103	/* Add, saturate and then store the results in the destination buffer. */
		104	/* Read values from source A */
		105	inA1 = pIn1[0];
		106
		107	/* Read values from source B */
		108	inB1 = pIn2[0];
		109
		110	/* Read values from source A */
		111	inA2 = pIn1[1];
		112
		113	/* Add and saturate */
		114	out1 = __QADD(inA1, inB1);
		115
		116	/* Read values from source B */
		117	inB2 = pIn2[1];
		118
		119	/* Read values from source A */
		120	inA1 = pIn1[2];
		121
		122	/* Add and saturate */
		123	out2 = __QADD(inA2, inB2);
		124
		125	/* Read values from source B */
		126	inB1 = pIn2[2];
		127
		128	/* Store result in destination */
		129	pOut[0] = out1;
		130	pOut[1] = out2;
		131
		132	/* Read values from source A */
		133	inA2 = pIn1[3];
		134
		135	/* Read values from source B */
		136	inB2 = pIn2[3];
		137
		138	/* Add and saturate */
		139	out1 = __QADD(inA1, inB1);
		140	out2 = __QADD(inA2, inB2);
		141
		142	/* Store result in destination */
		143	pOut[2] = out1;
		144	pOut[3] = out2;
		145
		146	/* update pointers to process next sampels */
		147	pIn1 += 4U;
		148	pIn2 += 4U;
		149	pOut += 4U;
		150
		151	/* Decrement the loop counter */
		152	blkCnt--;
		153	}
		154
		155	/* If the numSamples is not a multiple of 4, compute any remaining output samples here.
		156	** No loop unrolling is used. */
		157	blkCnt = numSamples % 0x4U;
		158
		159	#else
		160
		161	/* Run the below code for Cortex-M0 */
		162
		163	/* Initialize blkCnt with number of samples */
		164	blkCnt = numSamples;
		165
		166
		167	#endif /* #if defined (ARM_MATH_DSP) */
		168
		169	while (blkCnt > 0U)
		170	{
		171	/* C(m,n) = A(m,n) + B(m,n) */
		172	/* Add, saturate and then store the results in the destination buffer. */
		173	inA1 = *pIn1++;
		174	inB1 = *pIn2++;
		175
		176	inA1 = __QADD(inA1, inB1);
		177
		178	/* Decrement the loop counter */
		179	blkCnt--;
		180
		181	*pOut++ = inA1;
		182
		183	}
		184
		185	/* set status as ARM_MATH_SUCCESS */
		186	status = ARM_MATH_SUCCESS;
		187	}
		188
		189	/* Return to application */
		190	return (status);
		191	}
		192
		193	/**
		194	* @} end of MatrixAdd group
		195	*/

Subversion Repositories CharLCD

(root)/trunk/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_add_q31.c – Rev 2