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