1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_power_q15.c
4 * Description: Sum of the squares of the elements of a Q15 vector
5 *
6 * $Date: 23 April 2021
7 * $Revision: V1.9.0
8 *
9 * Target Processor: Cortex-M and Cortex-A cores
10 * -------------------------------------------------------------------- */
11 /*
12 * Copyright (C) 2010-2021 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 "dsp/statistics_functions.h"
30
31 /**
32 @ingroup groupStats
33 */
34
35 /**
36 @addtogroup power
37 @{
38 */
39
40 /**
41 @brief Sum of the squares of the elements of a Q15 vector.
42 @param[in] pSrc points to the input vector
43 @param[in] blockSize number of samples in input vector
44 @param[out] pResult sum of the squares value returned here
45
46 @par Scaling and Overflow Behavior
47 The function is implemented using a 64-bit internal accumulator.
48 The input is represented in 1.15 format.
49 Intermediate multiplication yields a 2.30 format, and this
50 result is added without saturation to a 64-bit accumulator in 34.30 format.
51 With 33 guard bits in the accumulator, there is no risk of overflow, and the
52 full precision of the intermediate multiplication is preserved.
53 Finally, the return result is in 34.30 format.
54 */
55 #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
56
arm_power_q15(const q15_t * pSrc,uint32_t blockSize,q63_t * pResult)57 ARM_DSP_ATTRIBUTE void arm_power_q15(
58 const q15_t * pSrc,
59 uint32_t blockSize,
60 q63_t * pResult)
61 {
62 uint32_t blkCnt; /* loop counters */
63 q15x8_t vecSrc;
64 q63_t sum = 0LL;
65 q15_t in;
66
67 /* Compute 8 outputs at a time */
68 blkCnt = blockSize >> 3U;
69 while (blkCnt > 0U)
70 {
71 vecSrc = vldrhq_s16(pSrc);
72 /*
73 * sum lanes
74 */
75 sum = vmlaldavaq(sum, vecSrc, vecSrc);
76
77 blkCnt --;
78 pSrc += 8;
79 }
80
81 /*
82 * tail
83 */
84 blkCnt = blockSize & 0x7;
85 while (blkCnt > 0U)
86 {
87 /* C = A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1] */
88
89 /* Compute Power and store result in a temporary variable, sum. */
90 in = *pSrc++;
91 sum += ((q31_t) in * in);
92
93 /* Decrement loop counter */
94 blkCnt--;
95 }
96
97 *pResult = sum;
98 }
99 #else
arm_power_q15(const q15_t * pSrc,uint32_t blockSize,q63_t * pResult)100 ARM_DSP_ATTRIBUTE void arm_power_q15(
101 const q15_t * pSrc,
102 uint32_t blockSize,
103 q63_t * pResult)
104 {
105 uint32_t blkCnt; /* Loop counter */
106 q63_t sum = 0; /* Temporary result storage */
107 q15_t in; /* Temporary variable to store input value */
108
109 #if defined (ARM_MATH_LOOPUNROLL) && defined (ARM_MATH_DSP)
110 q31_t in32; /* Temporary variable to store packed input value */
111 #endif
112
113 #if defined (ARM_MATH_LOOPUNROLL)
114
115 /* Loop unrolling: Compute 4 outputs at a time */
116 blkCnt = blockSize >> 2U;
117
118 while (blkCnt > 0U)
119 {
120 /* C = A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1] */
121
122 /* Compute Power and store result in a temporary variable, sum. */
123 #if defined (ARM_MATH_DSP)
124 in32 = read_q15x2_ia (&pSrc);
125 sum = __SMLALD(in32, in32, sum);
126
127 in32 = read_q15x2_ia (&pSrc);
128 sum = __SMLALD(in32, in32, sum);
129 #else
130 in = *pSrc++;
131 sum += ((q31_t) in * in);
132
133 in = *pSrc++;
134 sum += ((q31_t) in * in);
135
136 in = *pSrc++;
137 sum += ((q31_t) in * in);
138
139 in = *pSrc++;
140 sum += ((q31_t) in * in);
141 #endif /* #if defined (ARM_MATH_DSP) */
142
143 /* Decrement loop counter */
144 blkCnt--;
145 }
146
147 /* Loop unrolling: Compute remaining outputs */
148 blkCnt = blockSize % 0x4U;
149
150 #else
151
152 /* Initialize blkCnt with number of samples */
153 blkCnt = blockSize;
154
155 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
156
157 while (blkCnt > 0U)
158 {
159 /* C = A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1] */
160
161 /* Compute Power and store result in a temporary variable, sum. */
162 in = *pSrc++;
163 sum += ((q31_t) in * in);
164
165 /* Decrement loop counter */
166 blkCnt--;
167 }
168
169 /* Store result in 34.30 format */
170 *pResult = sum;
171 }
172 #endif /* defined(ARM_MATH_MVEI) */
173
174 /**
175 @} end of power group
176 */
177
