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

Rev	Author	Line No.	Line
2	mjames	1	/* ----------------------------------------------------------------------
		2	* Project: CMSIS DSP Library
		3	* Title: arm_mat_scale_q31.c
		4	* Description: Multiplies a Q31 matrix by a scalar
		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 MatrixScale
		37	* @{
		38	*/
		39
		40	/**
		41	* @brief Q31 matrix scaling.
		42	* @param[in] *pSrc points to input matrix
		43	* @param[in] scaleFract fractional portion of the scale factor
		44	* @param[in] shift number of bits to shift the result by
		45	* @param[out] *pDst points to output matrix structure
		46	* @return The function returns either
		47	* <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
		48	*
		49	* @details
		50	* <b>Scaling and Overflow Behavior:</b>
		51	* \par
		52	* The input data <code>*pSrc</code> and <code>scaleFract</code> are in 1.31 format.
		53	* These are multiplied to yield a 2.62 intermediate result and this is shifted with saturation to 1.31 format.
		54	*/
		55
		56	arm_status arm_mat_scale_q31(
		57	const arm_matrix_instance_q31 * pSrc,
		58	q31_t scaleFract,
		59	int32_t shift,
		60	arm_matrix_instance_q31 * pDst)
		61	{
		62	q31_t pIn = pSrc->pData; / input data matrix pointer */
		63	q31_t pOut = pDst->pData; / output data matrix pointer */
		64	uint32_t numSamples; /* total number of elements in the matrix */
		65	int32_t totShift = shift + 1; /* shift to apply after scaling */
		66	uint32_t blkCnt; /* loop counters */
		67	arm_status status; /* status of matrix scaling */
		68	q31_t in1, in2, out1; /* temporary variabels */
		69
		70	#if defined (ARM_MATH_DSP)
		71
		72	q31_t in3, in4, out2, out3, out4; /* temporary variables */
		73
		74	#endif // #ifndef ARM_MAT_CM0
		75
		76	#ifdef ARM_MATH_MATRIX_CHECK
		77	/* Check for matrix mismatch */
		78	if ((pSrc->numRows != pDst->numRows) \|\| (pSrc->numCols != pDst->numCols))
		79	{
		80	/* Set status as ARM_MATH_SIZE_MISMATCH */
		81	status = ARM_MATH_SIZE_MISMATCH;
		82	}
		83	else
		84	#endif // #ifdef ARM_MATH_MATRIX_CHECK
		85	{
		86	/* Total number of samples in the input matrix */
		87	numSamples = (uint32_t) pSrc->numRows * pSrc->numCols;
		88
		89	#if defined (ARM_MATH_DSP)
		90
		91	/* Run the below code for Cortex-M4 and Cortex-M3 */
		92
		93	/* Loop Unrolling */
		94	blkCnt = numSamples >> 2U;
		95
		96	/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
		97	** a second loop below computes the remaining 1 to 3 samples. */
		98	while (blkCnt > 0U)
		99	{
		100	/* C(m,n) = A(m,n) * k */
		101	/* Read values from input */
		102	in1 = *pIn;
		103	in2 = *(pIn + 1);
		104	in3 = *(pIn + 2);
		105	in4 = *(pIn + 3);
		106
		107	/* multiply input with scaler value */
		108	in1 = ((q63_t) in1 * scaleFract) >> 32;
		109	in2 = ((q63_t) in2 * scaleFract) >> 32;
		110	in3 = ((q63_t) in3 * scaleFract) >> 32;
		111	in4 = ((q63_t) in4 * scaleFract) >> 32;
		112
		113	/* apply shifting */
		114	out1 = in1 << totShift;
		115	out2 = in2 << totShift;
		116
		117	/* saturate the results. */
		118	if (in1 != (out1 >> totShift))
		119	out1 = 0x7FFFFFFF ^ (in1 >> 31);
		120
		121	if (in2 != (out2 >> totShift))
		122	out2 = 0x7FFFFFFF ^ (in2 >> 31);
		123
		124	out3 = in3 << totShift;
		125	out4 = in4 << totShift;
		126
		127	*pOut = out1;
		128	*(pOut + 1) = out2;
		129
		130	if (in3 != (out3 >> totShift))
		131	out3 = 0x7FFFFFFF ^ (in3 >> 31);
		132
		133	if (in4 != (out4 >> totShift))
		134	out4 = 0x7FFFFFFF ^ (in4 >> 31);
		135
		136
		137	*(pOut + 2) = out3;
		138	*(pOut + 3) = out4;
		139
		140	/* update pointers to process next sampels */
		141	pIn += 4U;
		142	pOut += 4U;
		143
		144
		145	/* Decrement the numSamples loop counter */
		146	blkCnt--;
		147	}
		148
		149	/* If the numSamples is not a multiple of 4, compute any remaining output samples here.
		150	** No loop unrolling is used. */
		151	blkCnt = numSamples % 0x4U;
		152
		153	#else
		154
		155	/* Run the below code for Cortex-M0 */
		156
		157	/* Initialize blkCnt with number of samples */
		158	blkCnt = numSamples;
		159
		160	#endif /* #if defined (ARM_MATH_DSP) */
		161
		162	while (blkCnt > 0U)
		163	{
		164	/* C(m,n) = A(m,n) * k */
		165	/* Scale, saturate and then store the results in the destination buffer. */
		166	in1 = *pIn++;
		167
		168	in2 = ((q63_t) in1 * scaleFract) >> 32;
		169
		170	out1 = in2 << totShift;
		171
		172	if (in2 != (out1 >> totShift))
		173	out1 = 0x7FFFFFFF ^ (in2 >> 31);
		174
		175	*pOut++ = out1;
		176
		177	/* Decrement the numSamples loop counter */
		178	blkCnt--;
		179	}
		180
		181	/* Set status as ARM_MATH_SUCCESS */
		182	status = ARM_MATH_SUCCESS;
		183	}
		184
		185	/* Return to application */
		186	return (status);
		187	}
		188
		189	/**
		190	* @} end of MatrixScale group
		191	*/

Subversion Repositories dashGPS

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