1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_power_q31.c
4 * Description: Sum of the squares of the elements of a Q31 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 Q31 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.31 format.
50 Intermediate multiplication yields a 2.62 format, and this
51 result is truncated to 2.48 format by discarding the lower 14 bits.
52 The 2.48 result is then added without saturation to a 64-bit accumulator in 16.48 format.
53 With 15 guard bits in the accumulator, there is no risk of overflow, and the
54 full precision of the intermediate multiplication is preserved.
55 Finally, the return result is in 16.48 format.
56 */
57 #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
arm_power_q31(const q31_t * pSrc,uint32_t blockSize,q63_t * pResult)58 void arm_power_q31(
59 const q31_t * pSrc,
60 uint32_t blockSize,
61 q63_t * pResult)
62 {
63 uint32_t blkCnt; /* loop counters */
64 q31x4_t vecSrc;
65 q63_t sum = 0LL;
66 q31_t in;
67
68 /* Compute 4 outputs at a time */
69 blkCnt = blockSize >> 2U;
70 while (blkCnt > 0U)
71 {
72 vecSrc = vldrwq_s32(pSrc);
73 /*
74 * sum lanes
75 */
76 sum = vrmlaldavhaq(sum, vecSrc, vecSrc);
77
78 blkCnt --;
79 pSrc += 4;
80 }
81
82 /*
83 * tail
84 */
85 blkCnt = blockSize & 0x3;
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 += ((q63_t) in * in) >> 8;
93
94 /* Decrement loop counter */
95 blkCnt--;
96 }
97
98 *pResult = asrl(sum, 6);
99 }
100 #else
arm_power_q31(const q31_t * pSrc,uint32_t blockSize,q63_t * pResult)101 void arm_power_q31(
102 const q31_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 q31_t in; /* Temporary variable to store input value */
109
110 #if defined (ARM_MATH_LOOPUNROLL)
111
112 /* Loop unrolling: Compute 4 outputs at a time */
113 blkCnt = blockSize >> 2U;
114
115 while (blkCnt > 0U)
116 {
117 /* C = A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1] */
118
119 /* Compute Power then shift intermediate results by 14 bits to maintain 16.48 format and store result in a temporary variable sum, providing 15 guard bits. */
120 in = *pSrc++;
121 sum += ((q63_t) in * in) >> 14U;
122
123 in = *pSrc++;
124 sum += ((q63_t) in * in) >> 14U;
125
126 in = *pSrc++;
127 sum += ((q63_t) in * in) >> 14U;
128
129 in = *pSrc++;
130 sum += ((q63_t) in * in) >> 14U;
131
132 /* Decrement loop counter */
133 blkCnt--;
134 }
135
136 /* Loop unrolling: Compute remaining outputs */
137 blkCnt = blockSize % 0x4U;
138
139 #else
140
141 /* Initialize blkCnt with number of samples */
142 blkCnt = blockSize;
143
144 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
145
146 while (blkCnt > 0U)
147 {
148 /* C = A[0] * A[0] + A[1] * A[1] + ... + A[blockSize-1] * A[blockSize-1] */
149
150 /* Compute Power and store result in a temporary variable, sum. */
151 in = *pSrc++;
152 sum += ((q63_t) in * in) >> 14U;
153
154 /* Decrement loop counter */
155 blkCnt--;
156 }
157
158 /* Store results in 16.48 format */
159 *pResult = sum;
160 }
161 #endif /* defined(ARM_MATH_MVEI) */
162
163 /**
164 @} end of power group
165 */
166
