WebSVN – dashGPS – Blame – /branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_mat_sub_q31.c
		4	* Description: Q31 matrix subtraction
		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 MatrixSub
		37	* @{
		38	*/
		39
		40	/**
		41	* @brief Q31 matrix subtraction.
		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
		55	arm_status arm_mat_sub_q31(
		56	const arm_matrix_instance_q31 * pSrcA,
		57	const arm_matrix_instance_q31 * pSrcB,
		58	arm_matrix_instance_q31 * pDst)
		59	{
		60	q31_t pIn1 = pSrcA->pData; / input data matrix pointer A */
		61	q31_t pIn2 = pSrcB->pData; / input data matrix pointer B */
		62	q31_t pOut = pDst->pData; / output data matrix pointer */
		63	q31_t inA1, inB1; /* temporary variables */
		64
		65	#if defined (ARM_MATH_DSP)
		66
		67	q31_t inA2, inB2; /* temporary variables */
		68	q31_t out1, out2; /* temporary variables */
		69
		70	#endif // #if defined (ARM_MATH_DSP)
		71
		72	uint32_t numSamples; /* total number of elements in the matrix */
		73	uint32_t blkCnt; /* loop counters */
		74	arm_status status; /* status of matrix subtraction */
		75
		76
		77	#ifdef ARM_MATH_MATRIX_CHECK
		78	/* Check for matrix mismatch condition */
		79	if ((pSrcA->numRows != pSrcB->numRows) \|\|
		80	(pSrcA->numCols != pSrcB->numCols) \|\|
		81	(pSrcA->numRows != pDst->numRows) \|\| (pSrcA->numCols != pDst->numCols))
		82	{
		83	/* Set status as ARM_MATH_SIZE_MISMATCH */
		84	status = ARM_MATH_SIZE_MISMATCH;
		85	}
		86	else
		87	#endif
		88	{
		89	/* Total number of samples in the input matrix */
		90	numSamples = (uint32_t) pSrcA->numRows * pSrcA->numCols;
		91
		92	#if defined (ARM_MATH_DSP)
		93
		94	/* Run the below code for Cortex-M4 and Cortex-M3 */
		95
		96	/* Loop Unrolling */
		97	blkCnt = numSamples >> 2U;
		98
		99	/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
		100	** a second loop below computes the remaining 1 to 3 samples. */
		101	while (blkCnt > 0U)
		102	{
		103	/* C(m,n) = A(m,n) - B(m,n) */
		104	/* Subtract, saturate and then store the results in the destination buffer. */
		105	/* Read values from source A */
		106	inA1 = pIn1[0];
		107
		108	/* Read values from source B */
		109	inB1 = pIn2[0];
		110
		111	/* Read values from source A */
		112	inA2 = pIn1[1];
		113
		114	/* Subtract and saturate */
		115	out1 = __QSUB(inA1, inB1);
		116
		117	/* Read values from source B */
		118	inB2 = pIn2[1];
		119
		120	/* Read values from source A */
		121	inA1 = pIn1[2];
		122
		123	/* Subtract and saturate */
		124	out2 = __QSUB(inA2, inB2);
		125
		126	/* Read values from source B */
		127	inB1 = pIn2[2];
		128
		129	/* Store result in destination */
		130	pOut[0] = out1;
		131	pOut[1] = out2;
		132
		133	/* Read values from source A */
		134	inA2 = pIn1[3];
		135
		136	/* Read values from source B */
		137	inB2 = pIn2[3];
		138
		139	/* Subtract and saturate */
		140	out1 = __QSUB(inA1, inB1);
		141
		142	/* Subtract and saturate */
		143	out2 = __QSUB(inA2, inB2);
		144
		145	/* Store result in destination */
		146	pOut[2] = out1;
		147	pOut[3] = out2;
		148
		149	/* update pointers to process next samples */
		150	pIn1 += 4U;
		151	pIn2 += 4U;
		152	pOut += 4U;
		153
		154	/* Decrement the loop counter */
		155	blkCnt--;
		156	}
		157
		158	/* If the numSamples is not a multiple of 4, compute any remaining output samples here.
		159	** No loop unrolling is used. */
		160	blkCnt = numSamples % 0x4U;
		161
		162	#else
		163
		164	/* Run the below code for Cortex-M0 */
		165
		166	/* Initialize blkCnt with number of samples */
		167	blkCnt = numSamples;
		168
		169	#endif /* #if defined (ARM_MATH_DSP) */
		170
		171	while (blkCnt > 0U)
		172	{
		173	/* C(m,n) = A(m,n) - B(m,n) */
		174	/* Subtract, saturate and then store the results in the destination buffer. */
		175	inA1 = *pIn1++;
		176	inB1 = *pIn2++;
		177
		178	inA1 = __QSUB(inA1, inB1);
		179
		180	*pOut++ = inA1;
		181
		182	/* Decrement the loop counter */
		183	blkCnt--;
		184	}
		185
		186	/* Set status as ARM_MATH_SUCCESS */
		187	status = ARM_MATH_SUCCESS;
		188	}
		189
		190	/* Return to application */
		191	return (status);
		192	}
		193
		194	/**
		195	* @} end of MatrixSub group
		196	*/

Subversion Repositories dashGPS

(root)/branches/dashGPS-bmp/Drivers/CMSIS/DSP/Source/MatrixFunctions/arm_mat_sub_q31.c – Rev 2