Audacity 3.2.0
SimdComplexConversions_sse2.h
Go to the documentation of this file.
1/* SPDX-License-Identifier: zlib */
2/*
3 * This code is cleaned up and modernized version of https://github.com/to-miz/sse_mathfun_extension
4 */
5
6#pragma once
7
8#include <emmintrin.h>
9#include <xmmintrin.h>
10
11#include <array>
12#include <complex>
13#include <type_traits>
14#include <memory>
15#include <utility>
16
18{
19// Before C++20 there is no standard and correct way to implement bit_cast.
20// It was verified that MSVC generates the correct code for the current use
21// cases
22namespace details
23{
24template <class To, class From>
25std::enable_if_t<
26 sizeof(To) == sizeof(From) && std::is_trivially_copyable_v<From> &&
27 std::is_trivially_copyable_v<To>,
28 To>
29// constexpr support needs compiler magic
30bit_cast(const From& src) noexcept
31{
32 static_assert(
33 std::is_trivially_constructible_v<To>,
34 "This implementation additionally requires "
35 "destination type to be trivially constructible");
36
37 To dst;
38 std::memcpy(&dst, &src, sizeof(To));
39 return dst;
40}
41
42constexpr float PIF = 3.141592653589793238f;
43constexpr float PIO2F = 1.5707963267948966192f;
44
45constexpr float cephes_PIF = 3.141592653589793238f;
46constexpr float cephes_PIO2F = 1.5707963267948966192f;
47constexpr float cephes_PIO4F = 0.7853981633974483096f;
48constexpr float cephes_FOPI = 1.27323954473516f; // 4 / M_PI
49constexpr float minus_cephes_DP1 = -0.78515625f;
50constexpr float minus_cephes_DP2 = -2.4187564849853515625e-4f;
51constexpr float minus_cephes_DP3 = -3.77489497744594108e-8f;
52constexpr float sincof_p0 = -1.9515295891e-4f;
53constexpr float sincof_p1 = 8.3321608736e-3f;
54constexpr float sincof_p2 = -1.6666654611e-1f;
55constexpr float coscof_p0 = 2.443315711809948e-005f;
56constexpr float coscof_p1 = -1.388731625493765e-003f;
57constexpr float coscof_p2 = 4.166664568298827e-002f;
58
59constexpr float atancof_p0 = 8.05374449538e-2f;
60constexpr float atancof_p1 = 1.38776856032e-1f;
61constexpr float atancof_p2 = 1.99777106478e-1f;
62constexpr float atancof_p3 = 3.33329491539e-1f;
63
64static const float sign_mask = bit_cast<float>(0x80000000);
65static const float inv_sign_mask = bit_cast<float>(~0x80000000);
66} // namespace details
67
68inline __m128 atan_ps(__m128 x)
69{
70 using namespace details;
71
72 __m128 sign_bit, y;
73
74 sign_bit = x;
75 /* take the absolute value */
76 x = _mm_and_ps(x, _mm_set1_ps(inv_sign_mask));
77 /* extract the sign bit (upper one) */
78 sign_bit = _mm_and_ps(sign_bit, _mm_set1_ps(sign_mask));
79
80 /* range reduction, init x and y depending on range */
81 /* x > 2.414213562373095 */
82 __m128 cmp0 = _mm_cmpgt_ps(x, _mm_set1_ps(2.414213562373095f));
83 /* x > 0.4142135623730950 */
84 __m128 cmp1 = _mm_cmpgt_ps(x, _mm_set1_ps(0.4142135623730950f));
85
86 /* x > 0.4142135623730950 && !( x > 2.414213562373095 ) */
87 __m128 cmp2 = _mm_andnot_ps(cmp0, cmp1);
88
89 /* -( 1.0/x ) */
90 __m128 y0 = _mm_and_ps(cmp0, _mm_set1_ps(cephes_PIO2F));
91 __m128 x0 = _mm_div_ps(_mm_set1_ps(1.0f), x);
92 x0 = _mm_xor_ps(x0, _mm_set1_ps(sign_mask));
93
94 __m128 y1 = _mm_and_ps(cmp2, _mm_set1_ps(cephes_PIO4F));
95 /* (x-1.0)/(x+1.0) */
96 __m128 x1_o = _mm_sub_ps(x, _mm_set1_ps(1.0f));
97 __m128 x1_u = _mm_add_ps(x, _mm_set1_ps(1.0f));
98 __m128 x1 = _mm_div_ps(x1_o, x1_u);
99
100 __m128 x2 = _mm_and_ps(cmp2, x1);
101 x0 = _mm_and_ps(cmp0, x0);
102 x2 = _mm_or_ps(x2, x0);
103 cmp1 = _mm_or_ps(cmp0, cmp2);
104 x2 = _mm_and_ps(cmp1, x2);
105 x = _mm_andnot_ps(cmp1, x);
106 x = _mm_or_ps(x2, x);
107
108 y = _mm_or_ps(y0, y1);
109
110 __m128 zz = _mm_mul_ps(x, x);
111 __m128 acc = _mm_set1_ps(atancof_p0);
112 acc = _mm_mul_ps(acc, zz);
113 acc = _mm_sub_ps(acc, _mm_set1_ps(atancof_p1));
114 acc = _mm_mul_ps(acc, zz);
115 acc = _mm_add_ps(acc, _mm_set1_ps(atancof_p2));
116 acc = _mm_mul_ps(acc, zz);
117 acc = _mm_sub_ps(acc, _mm_set1_ps(atancof_p3));
118 acc = _mm_mul_ps(acc, zz);
119 acc = _mm_mul_ps(acc, x);
120 acc = _mm_add_ps(acc, x);
121 y = _mm_add_ps(y, acc);
122
123 /* update the sign */
124 y = _mm_xor_ps(y, sign_bit);
125
126 return y;
127}
128
129inline __m128 atan2_ps(__m128 y, __m128 x)
130{
131 using namespace details;
132
133 __m128 zero = _mm_setzero_ps();
134 __m128 x_eq_0 = _mm_cmpeq_ps(x, zero);
135 __m128 x_gt_0 = _mm_cmpgt_ps(x, zero);
136 __m128 x_le_0 = _mm_cmple_ps(x, zero);
137 __m128 y_eq_0 = _mm_cmpeq_ps(y, zero);
138 __m128 x_lt_0 = _mm_cmplt_ps(x, zero);
139 __m128 y_lt_0 = _mm_cmplt_ps(y, zero);
140
141 __m128 zero_mask = _mm_and_ps(x_eq_0, y_eq_0);
142 __m128 zero_mask_other_case = _mm_and_ps(y_eq_0, x_gt_0);
143 zero_mask = _mm_or_ps(zero_mask, zero_mask_other_case);
144
145 __m128 pio2_mask = _mm_andnot_ps(y_eq_0, x_eq_0);
146 __m128 pio2_mask_sign = _mm_and_ps(y_lt_0, _mm_set1_ps(sign_mask));
147 __m128 pio2_result = _mm_set1_ps(cephes_PIO2F);
148 pio2_result = _mm_xor_ps(pio2_result, pio2_mask_sign);
149 pio2_result = _mm_and_ps(pio2_mask, pio2_result);
150
151 __m128 pi_mask = _mm_and_ps(y_eq_0, x_lt_0);
152 __m128 pi = _mm_set1_ps(cephes_PIF);
153 __m128 pi_result = _mm_and_ps(pi_mask, pi);
154
155 __m128 swap_sign_mask_offset = _mm_and_ps(x_lt_0, y_lt_0);
156 swap_sign_mask_offset =
157 _mm_and_ps(swap_sign_mask_offset, _mm_set1_ps(sign_mask));
158
159 __m128 offset0 = _mm_setzero_ps();
160 __m128 offset1 = _mm_set1_ps(cephes_PIF);
161 offset1 = _mm_xor_ps(offset1, swap_sign_mask_offset);
162
163 __m128 offset = _mm_andnot_ps(x_lt_0, offset0);
164 offset = _mm_and_ps(x_lt_0, offset1);
165
166 __m128 arg = _mm_div_ps(y, x);
167 __m128 atan_result = atan_ps(arg);
168 atan_result = _mm_add_ps(atan_result, offset);
169
170 /* select between zero_result, pio2_result and atan_result */
171
172 __m128 result = _mm_andnot_ps(zero_mask, pio2_result);
173 atan_result = _mm_andnot_ps(zero_mask, atan_result);
174 atan_result = _mm_andnot_ps(pio2_mask, atan_result);
175 result = _mm_or_ps(result, atan_result);
176 result = _mm_or_ps(result, pi_result);
177
178 return result;
179}
180
181inline std::pair<__m128, __m128> sincos_ps(__m128 x)
182{
183 using namespace details;
184 __m128 xmm1, xmm2, xmm3 = _mm_setzero_ps(), sign_bit_sin, y;
185 __m128i emm0, emm2, emm4;
186
187 sign_bit_sin = x;
188 /* take the absolute value */
189 x = _mm_and_ps(x, _mm_set1_ps(inv_sign_mask));
190 /* extract the sign bit (upper one) */
191 sign_bit_sin = _mm_and_ps(sign_bit_sin, _mm_set1_ps(sign_mask));
192
193 /* scale by 4/Pi */
194 y = _mm_mul_ps(x, _mm_set1_ps(cephes_FOPI));
195
196 /* store the integer part of y in emm2 */
197 emm2 = _mm_cvttps_epi32(y);
198
199 /* j=(j+1) & (~1) (see the cephes sources) */
200 emm2 = _mm_add_epi32(emm2, _mm_set1_epi32(1));
201 emm2 = _mm_and_si128(emm2, _mm_set1_epi32(~1));
202 y = _mm_cvtepi32_ps(emm2);
203
204 emm4 = emm2;
205
206 /* get the swap sign flag for the sine */
207 emm0 = _mm_and_si128(emm2, _mm_set1_epi32(4));
208 emm0 = _mm_slli_epi32(emm0, 29);
209 __m128 swap_sign_bit_sin = _mm_castsi128_ps(emm0);
210
211 /* get the polynom selection mask for the sine*/
212 emm2 = _mm_and_si128(emm2, _mm_set1_epi32(2));
213 emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
214 __m128 poly_mask = _mm_castsi128_ps(emm2);
215
216 /* The magic pass: "Extended precision modular arithmetic"
217 x = ((x - y * DP1) - y * DP2) - y * DP3; */
218 xmm1 = _mm_set1_ps(minus_cephes_DP1);
219 xmm2 = _mm_set1_ps(minus_cephes_DP2);
220 xmm3 = _mm_set1_ps(minus_cephes_DP3);
221 xmm1 = _mm_mul_ps(y, xmm1);
222 xmm2 = _mm_mul_ps(y, xmm2);
223 xmm3 = _mm_mul_ps(y, xmm3);
224 x = _mm_add_ps(x, xmm1);
225 x = _mm_add_ps(x, xmm2);
226 x = _mm_add_ps(x, xmm3);
227
228 emm4 = _mm_sub_epi32(emm4, _mm_set1_epi32(2));
229 emm4 = _mm_andnot_si128(emm4, _mm_set1_epi32(4));
230 emm4 = _mm_slli_epi32(emm4, 29);
231 __m128 sign_bit_cos = _mm_castsi128_ps(emm4);
232
233 sign_bit_sin = _mm_xor_ps(sign_bit_sin, swap_sign_bit_sin);
234
235 /* Evaluate the first polynom (0 <= x <= Pi/4) */
236 __m128 z = _mm_mul_ps(x, x);
237 y = _mm_set1_ps(coscof_p0);
238
239 y = _mm_mul_ps(y, z);
240 y = _mm_add_ps(y, _mm_set1_ps(coscof_p1));
241 y = _mm_mul_ps(y, z);
242 y = _mm_add_ps(y, _mm_set1_ps(coscof_p2));
243 y = _mm_mul_ps(y, z);
244 y = _mm_mul_ps(y, z);
245 __m128 tmp = _mm_mul_ps(z, _mm_set1_ps(0.5f));
246 y = _mm_sub_ps(y, tmp);
247 y = _mm_add_ps(y, _mm_set1_ps(1));
248
249 /* Evaluate the second polynom (Pi/4 <= x <= 0) */
250
251 __m128 y2 = _mm_set1_ps(sincof_p0);
252 y2 = _mm_mul_ps(y2, z);
253 y2 = _mm_add_ps(y2, _mm_set1_ps(sincof_p1));
254 y2 = _mm_mul_ps(y2, z);
255 y2 = _mm_add_ps(y2, _mm_set1_ps(sincof_p2));
256 y2 = _mm_mul_ps(y2, z);
257 y2 = _mm_mul_ps(y2, x);
258 y2 = _mm_add_ps(y2, x);
259
260 /* select the correct result from the two polynoms */
261 xmm3 = poly_mask;
262 __m128 ysin2 = _mm_and_ps(xmm3, y2);
263 __m128 ysin1 = _mm_andnot_ps(xmm3, y);
264 y2 = _mm_sub_ps(y2, ysin2);
265 y = _mm_sub_ps(y, ysin1);
266
267 xmm1 = _mm_add_ps(ysin1, ysin2);
268 xmm2 = _mm_add_ps(y, y2);
269
270 /* update the sign */
271 return std::make_pair(
272 _mm_xor_ps(xmm1, sign_bit_sin), _mm_xor_ps(xmm2, sign_bit_cos));
273}
274
275inline float atan2_ss(float y, float x)
276{
277 return _mm_cvtss_f32(atan2_ps(_mm_set_ss(y), _mm_set_ss(x)));
278}
279
280inline std::pair<float, float> sincos_ss(float angle)
281{
282 auto res = sincos_ps(_mm_set_ss(angle));
283 return std::make_pair(_mm_cvtss_f32(res.first), _mm_cvtss_f32(res.second));
284}
285
286inline __m128 norm(__m128 x, __m128 y)
287{
288 return _mm_add_ps(_mm_mul_ps(x, x), _mm_mul_ps(y, y));
289}
290
291inline float sqrt_ss(float x)
292{
293 __m128 sse_value = _mm_set_ss(x);
294 sse_value = _mm_sqrt_ss(sse_value);
295 return _mm_cvtss_f32(sse_value);
296}
297
298template <typename fnc>
300 const std::complex<float>* input, float* output, int n, const fnc& f)
301{
302 for (int i = 0; i <= n - 4; i += 4)
303 {
304 // Safe according to C++ standard
305 auto p1 = _mm_load_ps(reinterpret_cast<const float*>(input + i));
306 auto p2 = _mm_load_ps(reinterpret_cast<const float*>(input + i + 2));
307
308 // p1 = {real(c1), imag(c1), real(c2), imag(c2)}
309 // p2 = {real(c3), imag(c3), real(c4), imag(c4)}
310
311 auto rp = _mm_shuffle_ps(p1, p2, _MM_SHUFFLE(2, 0, 2, 0));
312 auto ip = _mm_shuffle_ps(p1, p2, _MM_SHUFFLE(3, 1, 3, 1));
313
314 __m128 out;
315 f(rp, ip, out);
316
317 _mm_store_ps(output + i, out);
318 }
319 // deal with last partial packet
320 for (int i = n & (~3); i < n; ++i)
321 {
322 __m128 out;
323 f(_mm_set_ss(real(input[i])), _mm_set_ss(imag(input[i])), out);
324 output[i] = _mm_cvtss_f32(out);
325 }
326}
327
329 const float* oldPhase, const float* newPhase, std::complex<float>* output,
330 int n)
331{
332 for (int i = 0; i <= n - 4; i += 4)
333 {
334 auto [sin, cos] = sincos_ps(
335 _mm_sub_ps(
336 _mm_load_ps(newPhase + i),
337 _mm_load_ps(oldPhase + i)));
338
339
340 // Safe according to C++ standard
341 auto p1 = _mm_load_ps(reinterpret_cast<float*>(output + i));
342 auto p2 = _mm_load_ps(reinterpret_cast<float*>(output + i + 2));
343
344 // p1 = {real(c1), imag(c1), real(c2), imag(c2)}
345 // p2 = {real(c3), imag(c3), real(c4), imag(c4)}
346
347 auto rp = _mm_shuffle_ps(p1, p2, _MM_SHUFFLE(2, 0, 2, 0));
348 auto ip = _mm_shuffle_ps(p1, p2, _MM_SHUFFLE(3, 1, 3, 1));
349
350
351 // We need to calculate (rp, ip) * (cos, sin) -> (rp*cos - ip*sin, rp*sin + ip*cos)
352
353 auto out_rp = _mm_sub_ps(_mm_mul_ps(rp, cos), _mm_mul_ps(ip, sin));
354 auto out_ip = _mm_add_ps(_mm_mul_ps(rp, sin), _mm_mul_ps(ip, cos));
355
356 p1 = _mm_unpacklo_ps(out_rp, out_ip);
357 p2 = _mm_unpackhi_ps(out_rp, out_ip);
358
359 _mm_store_ps(reinterpret_cast<float*>(output + i), p1);
360 _mm_store_ps(reinterpret_cast<float*>(output + i + 2), p2);
361 }
362 // deal with last partial packet
363 for (int i = n & (~3); i < n; ++i)
364 {
365 auto theta = newPhase[i] - oldPhase[i];
366 output[i] *= std::complex<float>(cosf(theta), sinf(theta));
367 }
368}
369
370} // namespace simd_complex_conversions
std::enable_if_t< sizeof(To)==sizeof(From) &&std::is_trivially_copyable_v< From > &&std::is_trivially_copyable_v< To >, To > bit_cast(const From &src) noexcept
__m128 atan2_ps(__m128 y, __m128 x)
std::pair< __m128, __m128 > sincos_ps(__m128 x)
__m128 norm(__m128 x, __m128 y)
void perform_parallel_simd_aligned(const std::complex< float > *input, float *output, int n, const fnc &f)
std::pair< float, float > sincos_ss(float angle)
void rotate_parallel_simd_aligned(const float *oldPhase, const float *newPhase, std::complex< float > *output, int n)