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