1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_cmplx_mag_q31.c
4 * Description: Q31 complex magnitude
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/complex_math_functions.h"
30
31 /**
32 @ingroup groupCmplxMath
33 */
34
35 /**
36 @addtogroup cmplx_mag
37 @{
38 */
39
40 /**
41 @brief Q31 complex magnitude.
42 @param[in] pSrc points to input vector
43 @param[out] pDst points to output vector
44 @param[in] numSamples number of samples in each vector
45
46 @par Scaling and Overflow Behavior
47 The function implements 1.31 by 1.31 multiplications and finally output is converted into 2.30 format.
48 Input down scaling is not required.
49 */
50
51 #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
52
53 #include "arm_helium_utils.h"
54
arm_cmplx_mag_q31(const q31_t * pSrc,q31_t * pDst,uint32_t numSamples)55 ARM_DSP_ATTRIBUTE void arm_cmplx_mag_q31(
56 const q31_t * pSrc,
57 q31_t * pDst,
58 uint32_t numSamples)
59 {
60 int32_t blockSize = numSamples; /* loop counters */
61 uint32_t blkCnt; /* loop counters */
62
63 q31x4x2_t vecSrc;
64 q31x4_t sum;
65
66 q31_t real, imag; /* Temporary input variables */
67 q31_t acc0, acc1; /* Accumulators */
68
69 /* Compute 4 complex samples at a time */
70 blkCnt = blockSize >> 2;
71 while (blkCnt > 0U)
72 {
73 vecSrc = vld2q(pSrc);
74
75 sum = vqaddq(vmulhq(vecSrc.val[0], vecSrc.val[0]),
76 vmulhq(vecSrc.val[1], vecSrc.val[1]));
77
78 sum = vshrq(sum, 1);
79
80 /*
81
82 This function is using a table. There are compilations flags to avoid
83 including this table (and in this case, arm_cmplx_maq_q31 must not
84 be built and linked.)
85
86 */
87 sum = FAST_VSQRT_Q31(sum);
88
89 vst1q(pDst, sum);
90
91 /*
92 * Decrement the blockSize loop counter
93 */
94 blkCnt--;
95 pSrc += 8;
96 pDst += 4;
97 }
98
99 /*
100 * tail
101 */
102 blkCnt = blockSize & 3;
103 while (blkCnt > 0U)
104 {
105 /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
106
107 real = *pSrc++;
108 imag = *pSrc++;
109 acc0 = (q31_t) (((q63_t) real * real) >> 33);
110 acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
111
112 /* store result in 2.30 format in destination buffer. */
113 arm_sqrt_q31(acc0 + acc1, pDst++);
114
115 /* Decrement loop counter */
116 blkCnt--;
117 }
118 }
119
120 #else
arm_cmplx_mag_q31(const q31_t * pSrc,q31_t * pDst,uint32_t numSamples)121 ARM_DSP_ATTRIBUTE void arm_cmplx_mag_q31(
122 const q31_t * pSrc,
123 q31_t * pDst,
124 uint32_t numSamples)
125 {
126 uint32_t blkCnt; /* Loop counter */
127 q31_t real, imag; /* Temporary input variables */
128 q31_t acc0, acc1; /* Accumulators */
129
130 #if defined (ARM_MATH_LOOPUNROLL)
131
132 /* Loop unrolling: Compute 4 outputs at a time */
133 blkCnt = numSamples >> 2U;
134
135 while (blkCnt > 0U)
136 {
137 /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
138
139 real = *pSrc++;
140 imag = *pSrc++;
141 acc0 = (q31_t) (((q63_t) real * real) >> 33);
142 acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
143
144 /* store result in 2.30 format in destination buffer. */
145 arm_sqrt_q31(acc0 + acc1, pDst++);
146
147 real = *pSrc++;
148 imag = *pSrc++;
149 acc0 = (q31_t) (((q63_t) real * real) >> 33);
150 acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
151 arm_sqrt_q31(acc0 + acc1, pDst++);
152
153 real = *pSrc++;
154 imag = *pSrc++;
155 acc0 = (q31_t) (((q63_t) real * real) >> 33);
156 acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
157 arm_sqrt_q31(acc0 + acc1, pDst++);
158
159 real = *pSrc++;
160 imag = *pSrc++;
161 acc0 = (q31_t) (((q63_t) real * real) >> 33);
162 acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
163 arm_sqrt_q31(acc0 + acc1, pDst++);
164
165 /* Decrement loop counter */
166 blkCnt--;
167 }
168
169 /* Loop unrolling: Compute remaining outputs */
170 blkCnt = numSamples % 0x4U;
171
172 #else
173
174 /* Initialize blkCnt with number of samples */
175 blkCnt = numSamples;
176
177 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
178
179 while (blkCnt > 0U)
180 {
181 /* C[0] = sqrt(A[0] * A[0] + A[1] * A[1]) */
182
183 real = *pSrc++;
184 imag = *pSrc++;
185 acc0 = (q31_t) (((q63_t) real * real) >> 33);
186 acc1 = (q31_t) (((q63_t) imag * imag) >> 33);
187
188 /* store result in 2.30 format in destination buffer. */
189 arm_sqrt_q31(acc0 + acc1, pDst++);
190
191 /* Decrement loop counter */
192 blkCnt--;
193 }
194
195 }
196 #endif /* defined(ARM_MATH_MVEI) */
197
198 /**
199 @} end of cmplx_mag group
200 */
201
