2 * Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
4 * Use of this source code is governed by a BSD-style license
5 * that can be found in the LICENSE file in the root of the source
6 * tree. An additional intellectual property rights grant can be found
7 * in the file PATENTS. All contributing project authors may
8 * be found in the AUTHORS file in the root of the source tree.
12 * The core AEC algorithm, SSE2 version of speed-critical functions.
15 #include <emmintrin.h>
17 #include <string.h> // memset
19 #include "webrtc/modules/audio_processing/aec/aec_common.h"
20 #include "webrtc/modules/audio_processing/aec/aec_core_internal.h"
21 #include "webrtc/modules/audio_processing/aec/aec_rdft.h"
23 __inline static float MulRe(float aRe, float aIm, float bRe, float bIm) {
24 return aRe * bRe - aIm * bIm;
27 __inline static float MulIm(float aRe, float aIm, float bRe, float bIm) {
28 return aRe * bIm + aIm * bRe;
31 static void FilterFarSSE2(AecCore* aec, float yf[2][PART_LEN1]) {
33 const int num_partitions = aec->num_partitions;
34 for (i = 0; i < num_partitions; i++) {
36 int xPos = (i + aec->xfBufBlockPos) * PART_LEN1;
37 int pos = i * PART_LEN1;
39 if (i + aec->xfBufBlockPos >= num_partitions) {
40 xPos -= num_partitions * (PART_LEN1);
43 // vectorized code (four at once)
44 for (j = 0; j + 3 < PART_LEN1; j += 4) {
45 const __m128 xfBuf_re = _mm_loadu_ps(&aec->xfBuf[0][xPos + j]);
46 const __m128 xfBuf_im = _mm_loadu_ps(&aec->xfBuf[1][xPos + j]);
47 const __m128 wfBuf_re = _mm_loadu_ps(&aec->wfBuf[0][pos + j]);
48 const __m128 wfBuf_im = _mm_loadu_ps(&aec->wfBuf[1][pos + j]);
49 const __m128 yf_re = _mm_loadu_ps(&yf[0][j]);
50 const __m128 yf_im = _mm_loadu_ps(&yf[1][j]);
51 const __m128 a = _mm_mul_ps(xfBuf_re, wfBuf_re);
52 const __m128 b = _mm_mul_ps(xfBuf_im, wfBuf_im);
53 const __m128 c = _mm_mul_ps(xfBuf_re, wfBuf_im);
54 const __m128 d = _mm_mul_ps(xfBuf_im, wfBuf_re);
55 const __m128 e = _mm_sub_ps(a, b);
56 const __m128 f = _mm_add_ps(c, d);
57 const __m128 g = _mm_add_ps(yf_re, e);
58 const __m128 h = _mm_add_ps(yf_im, f);
59 _mm_storeu_ps(&yf[0][j], g);
60 _mm_storeu_ps(&yf[1][j], h);
62 // scalar code for the remaining items.
63 for (; j < PART_LEN1; j++) {
64 yf[0][j] += MulRe(aec->xfBuf[0][xPos + j],
65 aec->xfBuf[1][xPos + j],
66 aec->wfBuf[0][pos + j],
67 aec->wfBuf[1][pos + j]);
68 yf[1][j] += MulIm(aec->xfBuf[0][xPos + j],
69 aec->xfBuf[1][xPos + j],
70 aec->wfBuf[0][pos + j],
71 aec->wfBuf[1][pos + j]);
76 static void ScaleErrorSignalSSE2(AecCore* aec, float ef[2][PART_LEN1]) {
77 const __m128 k1e_10f = _mm_set1_ps(1e-10f);
78 const __m128 kMu = aec->extended_filter_enabled ? _mm_set1_ps(kExtendedMu)
79 : _mm_set1_ps(aec->normal_mu);
80 const __m128 kThresh = aec->extended_filter_enabled
81 ? _mm_set1_ps(kExtendedErrorThreshold)
82 : _mm_set1_ps(aec->normal_error_threshold);
85 // vectorized code (four at once)
86 for (i = 0; i + 3 < PART_LEN1; i += 4) {
87 const __m128 xPow = _mm_loadu_ps(&aec->xPow[i]);
88 const __m128 ef_re_base = _mm_loadu_ps(&ef[0][i]);
89 const __m128 ef_im_base = _mm_loadu_ps(&ef[1][i]);
91 const __m128 xPowPlus = _mm_add_ps(xPow, k1e_10f);
92 __m128 ef_re = _mm_div_ps(ef_re_base, xPowPlus);
93 __m128 ef_im = _mm_div_ps(ef_im_base, xPowPlus);
94 const __m128 ef_re2 = _mm_mul_ps(ef_re, ef_re);
95 const __m128 ef_im2 = _mm_mul_ps(ef_im, ef_im);
96 const __m128 ef_sum2 = _mm_add_ps(ef_re2, ef_im2);
97 const __m128 absEf = _mm_sqrt_ps(ef_sum2);
98 const __m128 bigger = _mm_cmpgt_ps(absEf, kThresh);
99 __m128 absEfPlus = _mm_add_ps(absEf, k1e_10f);
100 const __m128 absEfInv = _mm_div_ps(kThresh, absEfPlus);
101 __m128 ef_re_if = _mm_mul_ps(ef_re, absEfInv);
102 __m128 ef_im_if = _mm_mul_ps(ef_im, absEfInv);
103 ef_re_if = _mm_and_ps(bigger, ef_re_if);
104 ef_im_if = _mm_and_ps(bigger, ef_im_if);
105 ef_re = _mm_andnot_ps(bigger, ef_re);
106 ef_im = _mm_andnot_ps(bigger, ef_im);
107 ef_re = _mm_or_ps(ef_re, ef_re_if);
108 ef_im = _mm_or_ps(ef_im, ef_im_if);
109 ef_re = _mm_mul_ps(ef_re, kMu);
110 ef_im = _mm_mul_ps(ef_im, kMu);
112 _mm_storeu_ps(&ef[0][i], ef_re);
113 _mm_storeu_ps(&ef[1][i], ef_im);
115 // scalar code for the remaining items.
118 aec->extended_filter_enabled ? kExtendedMu : aec->normal_mu;
119 const float error_threshold = aec->extended_filter_enabled
120 ? kExtendedErrorThreshold
121 : aec->normal_error_threshold;
122 for (; i < (PART_LEN1); i++) {
124 ef[0][i] /= (aec->xPow[i] + 1e-10f);
125 ef[1][i] /= (aec->xPow[i] + 1e-10f);
126 abs_ef = sqrtf(ef[0][i] * ef[0][i] + ef[1][i] * ef[1][i]);
128 if (abs_ef > error_threshold) {
129 abs_ef = error_threshold / (abs_ef + 1e-10f);
141 static void FilterAdaptationSSE2(AecCore* aec,
143 float ef[2][PART_LEN1]) {
145 const int num_partitions = aec->num_partitions;
146 for (i = 0; i < num_partitions; i++) {
147 int xPos = (i + aec->xfBufBlockPos) * (PART_LEN1);
148 int pos = i * PART_LEN1;
150 if (i + aec->xfBufBlockPos >= num_partitions) {
151 xPos -= num_partitions * PART_LEN1;
154 // Process the whole array...
155 for (j = 0; j < PART_LEN; j += 4) {
156 // Load xfBuf and ef.
157 const __m128 xfBuf_re = _mm_loadu_ps(&aec->xfBuf[0][xPos + j]);
158 const __m128 xfBuf_im = _mm_loadu_ps(&aec->xfBuf[1][xPos + j]);
159 const __m128 ef_re = _mm_loadu_ps(&ef[0][j]);
160 const __m128 ef_im = _mm_loadu_ps(&ef[1][j]);
161 // Calculate the product of conjugate(xfBuf) by ef.
162 // re(conjugate(a) * b) = aRe * bRe + aIm * bIm
163 // im(conjugate(a) * b)= aRe * bIm - aIm * bRe
164 const __m128 a = _mm_mul_ps(xfBuf_re, ef_re);
165 const __m128 b = _mm_mul_ps(xfBuf_im, ef_im);
166 const __m128 c = _mm_mul_ps(xfBuf_re, ef_im);
167 const __m128 d = _mm_mul_ps(xfBuf_im, ef_re);
168 const __m128 e = _mm_add_ps(a, b);
169 const __m128 f = _mm_sub_ps(c, d);
170 // Interleave real and imaginary parts.
171 const __m128 g = _mm_unpacklo_ps(e, f);
172 const __m128 h = _mm_unpackhi_ps(e, f);
174 _mm_storeu_ps(&fft[2 * j + 0], g);
175 _mm_storeu_ps(&fft[2 * j + 4], h);
177 // ... and fixup the first imaginary entry.
178 fft[1] = MulRe(aec->xfBuf[0][xPos + PART_LEN],
179 -aec->xfBuf[1][xPos + PART_LEN],
183 aec_rdft_inverse_128(fft);
184 memset(fft + PART_LEN, 0, sizeof(float) * PART_LEN);
188 float scale = 2.0f / PART_LEN2;
189 const __m128 scale_ps = _mm_load_ps1(&scale);
190 for (j = 0; j < PART_LEN; j += 4) {
191 const __m128 fft_ps = _mm_loadu_ps(&fft[j]);
192 const __m128 fft_scale = _mm_mul_ps(fft_ps, scale_ps);
193 _mm_storeu_ps(&fft[j], fft_scale);
196 aec_rdft_forward_128(fft);
199 float wt1 = aec->wfBuf[1][pos];
200 aec->wfBuf[0][pos + PART_LEN] += fft[1];
201 for (j = 0; j < PART_LEN; j += 4) {
202 __m128 wtBuf_re = _mm_loadu_ps(&aec->wfBuf[0][pos + j]);
203 __m128 wtBuf_im = _mm_loadu_ps(&aec->wfBuf[1][pos + j]);
204 const __m128 fft0 = _mm_loadu_ps(&fft[2 * j + 0]);
205 const __m128 fft4 = _mm_loadu_ps(&fft[2 * j + 4]);
206 const __m128 fft_re =
207 _mm_shuffle_ps(fft0, fft4, _MM_SHUFFLE(2, 0, 2, 0));
208 const __m128 fft_im =
209 _mm_shuffle_ps(fft0, fft4, _MM_SHUFFLE(3, 1, 3, 1));
210 wtBuf_re = _mm_add_ps(wtBuf_re, fft_re);
211 wtBuf_im = _mm_add_ps(wtBuf_im, fft_im);
212 _mm_storeu_ps(&aec->wfBuf[0][pos + j], wtBuf_re);
213 _mm_storeu_ps(&aec->wfBuf[1][pos + j], wtBuf_im);
215 aec->wfBuf[1][pos] = wt1;
220 static __m128 mm_pow_ps(__m128 a, __m128 b) {
221 // a^b = exp2(b * log2(a))
222 // exp2(x) and log2(x) are calculated using polynomial approximations.
223 __m128 log2_a, b_log2_a, a_exp_b;
225 // Calculate log2(x), x = a.
227 // To calculate log2(x), we decompose x like this:
230 // y is in the [1.0, 2.0) range
232 // log2(x) = log2(y) + n
233 // n can be evaluated by playing with float representation.
234 // log2(y) in a small range can be approximated, this code uses an order
235 // five polynomial approximation. The coefficients have been
236 // estimated with the Remez algorithm and the resulting
237 // polynomial has a maximum relative error of 0.00086%.
240 // This is done by masking the exponent, shifting it into the top bit of
241 // the mantissa, putting eight into the biased exponent (to shift/
242 // compensate the fact that the exponent has been shifted in the top/
243 // fractional part and finally getting rid of the implicit leading one
244 // from the mantissa by substracting it out.
245 static const ALIGN16_BEG int float_exponent_mask[4] ALIGN16_END = {
246 0x7F800000, 0x7F800000, 0x7F800000, 0x7F800000};
247 static const ALIGN16_BEG int eight_biased_exponent[4] ALIGN16_END = {
248 0x43800000, 0x43800000, 0x43800000, 0x43800000};
249 static const ALIGN16_BEG int implicit_leading_one[4] ALIGN16_END = {
250 0x43BF8000, 0x43BF8000, 0x43BF8000, 0x43BF8000};
251 static const int shift_exponent_into_top_mantissa = 8;
252 const __m128 two_n = _mm_and_ps(a, *((__m128*)float_exponent_mask));
253 const __m128 n_1 = _mm_castsi128_ps(_mm_srli_epi32(
254 _mm_castps_si128(two_n), shift_exponent_into_top_mantissa));
255 const __m128 n_0 = _mm_or_ps(n_1, *((__m128*)eight_biased_exponent));
256 const __m128 n = _mm_sub_ps(n_0, *((__m128*)implicit_leading_one));
259 static const ALIGN16_BEG int mantissa_mask[4] ALIGN16_END = {
260 0x007FFFFF, 0x007FFFFF, 0x007FFFFF, 0x007FFFFF};
261 static const ALIGN16_BEG int zero_biased_exponent_is_one[4] ALIGN16_END = {
262 0x3F800000, 0x3F800000, 0x3F800000, 0x3F800000};
263 const __m128 mantissa = _mm_and_ps(a, *((__m128*)mantissa_mask));
265 _mm_or_ps(mantissa, *((__m128*)zero_biased_exponent_is_one));
267 // Approximate log2(y) ~= (y - 1) * pol5(y).
268 // pol5(y) = C5 * y^5 + C4 * y^4 + C3 * y^3 + C2 * y^2 + C1 * y + C0
269 static const ALIGN16_BEG float ALIGN16_END C5[4] = {
270 -3.4436006e-2f, -3.4436006e-2f, -3.4436006e-2f, -3.4436006e-2f};
271 static const ALIGN16_BEG float ALIGN16_END
272 C4[4] = {3.1821337e-1f, 3.1821337e-1f, 3.1821337e-1f, 3.1821337e-1f};
273 static const ALIGN16_BEG float ALIGN16_END
274 C3[4] = {-1.2315303f, -1.2315303f, -1.2315303f, -1.2315303f};
275 static const ALIGN16_BEG float ALIGN16_END
276 C2[4] = {2.5988452f, 2.5988452f, 2.5988452f, 2.5988452f};
277 static const ALIGN16_BEG float ALIGN16_END
278 C1[4] = {-3.3241990f, -3.3241990f, -3.3241990f, -3.3241990f};
279 static const ALIGN16_BEG float ALIGN16_END
280 C0[4] = {3.1157899f, 3.1157899f, 3.1157899f, 3.1157899f};
281 const __m128 pol5_y_0 = _mm_mul_ps(y, *((__m128*)C5));
282 const __m128 pol5_y_1 = _mm_add_ps(pol5_y_0, *((__m128*)C4));
283 const __m128 pol5_y_2 = _mm_mul_ps(pol5_y_1, y);
284 const __m128 pol5_y_3 = _mm_add_ps(pol5_y_2, *((__m128*)C3));
285 const __m128 pol5_y_4 = _mm_mul_ps(pol5_y_3, y);
286 const __m128 pol5_y_5 = _mm_add_ps(pol5_y_4, *((__m128*)C2));
287 const __m128 pol5_y_6 = _mm_mul_ps(pol5_y_5, y);
288 const __m128 pol5_y_7 = _mm_add_ps(pol5_y_6, *((__m128*)C1));
289 const __m128 pol5_y_8 = _mm_mul_ps(pol5_y_7, y);
290 const __m128 pol5_y = _mm_add_ps(pol5_y_8, *((__m128*)C0));
291 const __m128 y_minus_one =
292 _mm_sub_ps(y, *((__m128*)zero_biased_exponent_is_one));
293 const __m128 log2_y = _mm_mul_ps(y_minus_one, pol5_y);
296 log2_a = _mm_add_ps(n, log2_y);
300 b_log2_a = _mm_mul_ps(b, log2_a);
302 // Calculate exp2(x), x = b * log2(a).
304 // To calculate 2^x, we decompose x like this:
306 // n is an integer, the value of x - 0.5 rounded down, therefore
307 // y is in the [0.5, 1.5) range
310 // 2^n can be evaluated by playing with float representation.
311 // 2^y in a small range can be approximated, this code uses an order two
312 // polynomial approximation. The coefficients have been estimated
313 // with the Remez algorithm and the resulting polynomial has a
314 // maximum relative error of 0.17%.
316 // To avoid over/underflow, we reduce the range of input to ]-127, 129].
317 static const ALIGN16_BEG float max_input[4] ALIGN16_END = {129.f, 129.f,
319 static const ALIGN16_BEG float min_input[4] ALIGN16_END = {
320 -126.99999f, -126.99999f, -126.99999f, -126.99999f};
321 const __m128 x_min = _mm_min_ps(b_log2_a, *((__m128*)max_input));
322 const __m128 x_max = _mm_max_ps(x_min, *((__m128*)min_input));
324 static const ALIGN16_BEG float half[4] ALIGN16_END = {0.5f, 0.5f,
326 const __m128 x_minus_half = _mm_sub_ps(x_max, *((__m128*)half));
327 const __m128i x_minus_half_floor = _mm_cvtps_epi32(x_minus_half);
329 static const ALIGN16_BEG int float_exponent_bias[4] ALIGN16_END = {
331 static const int float_exponent_shift = 23;
332 const __m128i two_n_exponent =
333 _mm_add_epi32(x_minus_half_floor, *((__m128i*)float_exponent_bias));
335 _mm_castsi128_ps(_mm_slli_epi32(two_n_exponent, float_exponent_shift));
337 const __m128 y = _mm_sub_ps(x_max, _mm_cvtepi32_ps(x_minus_half_floor));
338 // Approximate 2^y ~= C2 * y^2 + C1 * y + C0.
339 static const ALIGN16_BEG float C2[4] ALIGN16_END = {
340 3.3718944e-1f, 3.3718944e-1f, 3.3718944e-1f, 3.3718944e-1f};
341 static const ALIGN16_BEG float C1[4] ALIGN16_END = {
342 6.5763628e-1f, 6.5763628e-1f, 6.5763628e-1f, 6.5763628e-1f};
343 static const ALIGN16_BEG float C0[4] ALIGN16_END = {1.0017247f, 1.0017247f,
344 1.0017247f, 1.0017247f};
345 const __m128 exp2_y_0 = _mm_mul_ps(y, *((__m128*)C2));
346 const __m128 exp2_y_1 = _mm_add_ps(exp2_y_0, *((__m128*)C1));
347 const __m128 exp2_y_2 = _mm_mul_ps(exp2_y_1, y);
348 const __m128 exp2_y = _mm_add_ps(exp2_y_2, *((__m128*)C0));
351 a_exp_b = _mm_mul_ps(exp2_y, two_n);
356 static void OverdriveAndSuppressSSE2(AecCore* aec,
357 float hNl[PART_LEN1],
359 float efw[2][PART_LEN1]) {
361 const __m128 vec_hNlFb = _mm_set1_ps(hNlFb);
362 const __m128 vec_one = _mm_set1_ps(1.0f);
363 const __m128 vec_minus_one = _mm_set1_ps(-1.0f);
364 const __m128 vec_overDriveSm = _mm_set1_ps(aec->overDriveSm);
365 // vectorized code (four at once)
366 for (i = 0; i + 3 < PART_LEN1; i += 4) {
368 __m128 vec_hNl = _mm_loadu_ps(&hNl[i]);
369 const __m128 vec_weightCurve = _mm_loadu_ps(&WebRtcAec_weightCurve[i]);
370 const __m128 bigger = _mm_cmpgt_ps(vec_hNl, vec_hNlFb);
371 const __m128 vec_weightCurve_hNlFb = _mm_mul_ps(vec_weightCurve, vec_hNlFb);
372 const __m128 vec_one_weightCurve = _mm_sub_ps(vec_one, vec_weightCurve);
373 const __m128 vec_one_weightCurve_hNl =
374 _mm_mul_ps(vec_one_weightCurve, vec_hNl);
375 const __m128 vec_if0 = _mm_andnot_ps(bigger, vec_hNl);
376 const __m128 vec_if1 = _mm_and_ps(
377 bigger, _mm_add_ps(vec_weightCurve_hNlFb, vec_one_weightCurve_hNl));
378 vec_hNl = _mm_or_ps(vec_if0, vec_if1);
381 const __m128 vec_overDriveCurve =
382 _mm_loadu_ps(&WebRtcAec_overDriveCurve[i]);
383 const __m128 vec_overDriveSm_overDriveCurve =
384 _mm_mul_ps(vec_overDriveSm, vec_overDriveCurve);
385 vec_hNl = mm_pow_ps(vec_hNl, vec_overDriveSm_overDriveCurve);
386 _mm_storeu_ps(&hNl[i], vec_hNl);
389 // Suppress error signal
391 __m128 vec_efw_re = _mm_loadu_ps(&efw[0][i]);
392 __m128 vec_efw_im = _mm_loadu_ps(&efw[1][i]);
393 vec_efw_re = _mm_mul_ps(vec_efw_re, vec_hNl);
394 vec_efw_im = _mm_mul_ps(vec_efw_im, vec_hNl);
396 // Ooura fft returns incorrect sign on imaginary component. It matters
397 // here because we are making an additive change with comfort noise.
398 vec_efw_im = _mm_mul_ps(vec_efw_im, vec_minus_one);
399 _mm_storeu_ps(&efw[0][i], vec_efw_re);
400 _mm_storeu_ps(&efw[1][i], vec_efw_im);
403 // scalar code for the remaining items.
404 for (; i < PART_LEN1; i++) {
406 if (hNl[i] > hNlFb) {
407 hNl[i] = WebRtcAec_weightCurve[i] * hNlFb +
408 (1 - WebRtcAec_weightCurve[i]) * hNl[i];
410 hNl[i] = powf(hNl[i], aec->overDriveSm * WebRtcAec_overDriveCurve[i]);
412 // Suppress error signal
416 // Ooura fft returns incorrect sign on imaginary component. It matters
417 // here because we are making an additive change with comfort noise.
422 void WebRtcAec_InitAec_SSE2(void) {
423 WebRtcAec_FilterFar = FilterFarSSE2;
424 WebRtcAec_ScaleErrorSignal = ScaleErrorSignalSSE2;
425 WebRtcAec_FilterAdaptation = FilterAdaptationSSE2;
426 WebRtcAec_OverdriveAndSuppress = OverdriveAndSuppressSSE2;