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