static void FilterAdaptation(aec_t *aec, float *fft, float ef[2][PART_LEN1]) { int i, j; for (i = 0; i < NR_PART; i++) { int xPos = (i + aec->xfBufBlockPos)*(PART_LEN1); int pos; // Check for wrap if (i + aec->xfBufBlockPos >= NR_PART) { xPos -= NR_PART * PART_LEN1; } pos = i * PART_LEN1; for (j = 0; j < PART_LEN; j++) { fft[2 * j] = MulRe(aec->xfBuf[0][xPos + j], -aec->xfBuf[1][xPos + j], ef[0][j], ef[1][j]); fft[2 * j + 1] = MulIm(aec->xfBuf[0][xPos + j], -aec->xfBuf[1][xPos + j], ef[0][j], ef[1][j]); } fft[1] = MulRe(aec->xfBuf[0][xPos + PART_LEN], -aec->xfBuf[1][xPos + PART_LEN], ef[0][PART_LEN], ef[1][PART_LEN]); aec_rdft_inverse_128(fft); memset(fft + PART_LEN, 0, sizeof(float) * PART_LEN); // fft scaling { float scale = 2.0f / PART_LEN2; for (j = 0; j < PART_LEN; j++) { fft[j] *= scale; } } aec_rdft_forward_128(fft); aec->wfBuf[0][pos] += fft[0]; aec->wfBuf[0][pos + PART_LEN] += fft[1]; for (j = 1; j < PART_LEN; j++) { aec->wfBuf[0][pos + j] += fft[2 * j]; aec->wfBuf[1][pos + j] += fft[2 * j + 1]; } } }
static void TimeToFrequency(float time_data[PART_LEN2], float freq_data[2][PART_LEN1], int window) { int i = 0; // TODO(bjornv): Should we have a different function/wrapper for windowed FFT? if (window) { for (i = 0; i < PART_LEN; i++) { time_data[i] *= sqrtHanning[i]; time_data[PART_LEN + i] *= sqrtHanning[PART_LEN - i]; } } aec_rdft_forward_128(time_data); // Reorder. freq_data[1][0] = 0; freq_data[1][PART_LEN] = 0; freq_data[0][0] = time_data[0]; freq_data[0][PART_LEN] = time_data[1]; for (i = 1; i < PART_LEN; i++) { freq_data[0][i] = time_data[2 * i]; freq_data[1][i] = time_data[2 * i + 1]; } }
static void FilterAdaptationSSE2(aec_t *aec, float *fft, float ef[2][PART_LEN1]) { int i, j; for (i = 0; i < NR_PART; i++) { int xPos = (i + aec->xfBufBlockPos)*(PART_LEN1); int pos = i * PART_LEN1; // Check for wrap if (i + aec->xfBufBlockPos >= NR_PART) { xPos -= NR_PART * PART_LEN1; } #ifdef UNCONSTR for (j = 0; j < PART_LEN1; j++) { aec->wfBuf[pos + j][0] += MulRe(aec->xfBuf[xPos + j][0], -aec->xfBuf[xPos + j][1], ef[j][0], ef[j][1]); aec->wfBuf[pos + j][1] += MulIm(aec->xfBuf[xPos + j][0], -aec->xfBuf[xPos + j][1], ef[j][0], ef[j][1]); } #else // Process the whole array... for (j = 0; j < PART_LEN; j+= 4) { // Load xfBuf and ef. const __m128 xfBuf_re = _mm_loadu_ps(&aec->xfBuf[0][xPos + j]); const __m128 xfBuf_im = _mm_loadu_ps(&aec->xfBuf[1][xPos + j]); const __m128 ef_re = _mm_loadu_ps(&ef[0][j]); const __m128 ef_im = _mm_loadu_ps(&ef[1][j]); // Calculate the product of conjugate(xfBuf) by ef. // re(conjugate(a) * b) = aRe * bRe + aIm * bIm // im(conjugate(a) * b)= aRe * bIm - aIm * bRe const __m128 a = _mm_mul_ps(xfBuf_re, ef_re); const __m128 b = _mm_mul_ps(xfBuf_im, ef_im); const __m128 c = _mm_mul_ps(xfBuf_re, ef_im); const __m128 d = _mm_mul_ps(xfBuf_im, ef_re); const __m128 e = _mm_add_ps(a, b); const __m128 f = _mm_sub_ps(c, d); // Interleave real and imaginary parts. const __m128 g = _mm_unpacklo_ps(e, f); const __m128 h = _mm_unpackhi_ps(e, f); // Store _mm_storeu_ps(&fft[2*j + 0], g); _mm_storeu_ps(&fft[2*j + 4], h); } // ... and fixup the first imaginary entry. fft[1] = MulRe(aec->xfBuf[0][xPos + PART_LEN], -aec->xfBuf[1][xPos + PART_LEN], ef[0][PART_LEN], ef[1][PART_LEN]); aec_rdft_inverse_128(fft); memset(fft + PART_LEN, 0, sizeof(float)*PART_LEN); // fft scaling { float scale = 2.0f / PART_LEN2; const __m128 scale_ps = _mm_load_ps1(&scale); for (j = 0; j < PART_LEN; j+=4) { const __m128 fft_ps = _mm_loadu_ps(&fft[j]); const __m128 fft_scale = _mm_mul_ps(fft_ps, scale_ps); _mm_storeu_ps(&fft[j], fft_scale); } } aec_rdft_forward_128(fft); { float wt1 = aec->wfBuf[1][pos]; aec->wfBuf[0][pos + PART_LEN] += fft[1]; for (j = 0; j < PART_LEN; j+= 4) { __m128 wtBuf_re = _mm_loadu_ps(&aec->wfBuf[0][pos + j]); __m128 wtBuf_im = _mm_loadu_ps(&aec->wfBuf[1][pos + j]); const __m128 fft0 = _mm_loadu_ps(&fft[2 * j + 0]); const __m128 fft4 = _mm_loadu_ps(&fft[2 * j + 4]); const __m128 fft_re = _mm_shuffle_ps(fft0, fft4, _MM_SHUFFLE(2, 0, 2 ,0)); const __m128 fft_im = _mm_shuffle_ps(fft0, fft4, _MM_SHUFFLE(3, 1, 3 ,1)); wtBuf_re = _mm_add_ps(wtBuf_re, fft_re); wtBuf_im = _mm_add_ps(wtBuf_im, fft_im); _mm_storeu_ps(&aec->wfBuf[0][pos + j], wtBuf_re); _mm_storeu_ps(&aec->wfBuf[1][pos + j], wtBuf_im); } aec->wfBuf[1][pos] = wt1; } #endif // UNCONSTR } }
static void FilterAdaptationSSE2( int num_partitions, int x_fft_buf_block_pos, float x_fft_buf[2][kExtendedNumPartitions * PART_LEN1], float e_fft[2][PART_LEN1], float h_fft_buf[2][kExtendedNumPartitions * PART_LEN1]) { float fft[PART_LEN2]; int i, j; for (i = 0; i < num_partitions; i++) { int xPos = (i + x_fft_buf_block_pos) * (PART_LEN1); int pos = i * PART_LEN1; // Check for wrap if (i + x_fft_buf_block_pos >= num_partitions) { xPos -= num_partitions * PART_LEN1; } // Process the whole array... for (j = 0; j < PART_LEN; j += 4) { // Load x_fft_buf and e_fft. const __m128 x_fft_buf_re = _mm_loadu_ps(&x_fft_buf[0][xPos + j]); const __m128 x_fft_buf_im = _mm_loadu_ps(&x_fft_buf[1][xPos + j]); const __m128 e_fft_re = _mm_loadu_ps(&e_fft[0][j]); const __m128 e_fft_im = _mm_loadu_ps(&e_fft[1][j]); // Calculate the product of conjugate(x_fft_buf) by e_fft. // re(conjugate(a) * b) = aRe * bRe + aIm * bIm // im(conjugate(a) * b)= aRe * bIm - aIm * bRe const __m128 a = _mm_mul_ps(x_fft_buf_re, e_fft_re); const __m128 b = _mm_mul_ps(x_fft_buf_im, e_fft_im); const __m128 c = _mm_mul_ps(x_fft_buf_re, e_fft_im); const __m128 d = _mm_mul_ps(x_fft_buf_im, e_fft_re); const __m128 e = _mm_add_ps(a, b); const __m128 f = _mm_sub_ps(c, d); // Interleave real and imaginary parts. const __m128 g = _mm_unpacklo_ps(e, f); const __m128 h = _mm_unpackhi_ps(e, f); // Store _mm_storeu_ps(&fft[2 * j + 0], g); _mm_storeu_ps(&fft[2 * j + 4], h); } // ... and fixup the first imaginary entry. fft[1] = MulRe(x_fft_buf[0][xPos + PART_LEN], -x_fft_buf[1][xPos + PART_LEN], e_fft[0][PART_LEN], e_fft[1][PART_LEN]); aec_rdft_inverse_128(fft); memset(fft + PART_LEN, 0, sizeof(float) * PART_LEN); // fft scaling { float scale = 2.0f / PART_LEN2; const __m128 scale_ps = _mm_load_ps1(&scale); for (j = 0; j < PART_LEN; j += 4) { const __m128 fft_ps = _mm_loadu_ps(&fft[j]); const __m128 fft_scale = _mm_mul_ps(fft_ps, scale_ps); _mm_storeu_ps(&fft[j], fft_scale); } } aec_rdft_forward_128(fft); { float wt1 = h_fft_buf[1][pos]; h_fft_buf[0][pos + PART_LEN] += fft[1]; for (j = 0; j < PART_LEN; j += 4) { __m128 wtBuf_re = _mm_loadu_ps(&h_fft_buf[0][pos + j]); __m128 wtBuf_im = _mm_loadu_ps(&h_fft_buf[1][pos + j]); const __m128 fft0 = _mm_loadu_ps(&fft[2 * j + 0]); const __m128 fft4 = _mm_loadu_ps(&fft[2 * j + 4]); const __m128 fft_re = _mm_shuffle_ps(fft0, fft4, _MM_SHUFFLE(2, 0, 2, 0)); const __m128 fft_im = _mm_shuffle_ps(fft0, fft4, _MM_SHUFFLE(3, 1, 3, 1)); wtBuf_re = _mm_add_ps(wtBuf_re, fft_re); wtBuf_im = _mm_add_ps(wtBuf_im, fft_im); _mm_storeu_ps(&h_fft_buf[0][pos + j], wtBuf_re); _mm_storeu_ps(&h_fft_buf[1][pos + j], wtBuf_im); } h_fft_buf[1][pos] = wt1; } } }
static void SubbandCoherenceSSE2(AecCore* aec, float efw[2][PART_LEN1], float xfw[2][PART_LEN1], float* fft, float* cohde, float* cohxd) { float dfw[2][PART_LEN1]; int i; if (aec->delayEstCtr == 0) aec->delayIdx = PartitionDelay(aec); // Use delayed far. memcpy(xfw, aec->xfwBuf + aec->delayIdx * PART_LEN1, sizeof(xfw[0][0]) * 2 * PART_LEN1); // Windowed near fft WindowData(fft, aec->dBuf); aec_rdft_forward_128(fft); StoreAsComplex(fft, dfw); // Windowed error fft WindowData(fft, aec->eBuf); aec_rdft_forward_128(fft); StoreAsComplex(fft, efw); SmoothedPSD(aec, efw, dfw, xfw); { const __m128 vec_1eminus10 = _mm_set1_ps(1e-10f); // Subband coherence for (i = 0; i + 3 < PART_LEN1; i += 4) { const __m128 vec_sd = _mm_loadu_ps(&aec->sd[i]); const __m128 vec_se = _mm_loadu_ps(&aec->se[i]); const __m128 vec_sx = _mm_loadu_ps(&aec->sx[i]); const __m128 vec_sdse = _mm_add_ps(vec_1eminus10, _mm_mul_ps(vec_sd, vec_se)); const __m128 vec_sdsx = _mm_add_ps(vec_1eminus10, _mm_mul_ps(vec_sd, vec_sx)); const __m128 vec_sde_3210 = _mm_loadu_ps(&aec->sde[i][0]); const __m128 vec_sde_7654 = _mm_loadu_ps(&aec->sde[i + 2][0]); const __m128 vec_sxd_3210 = _mm_loadu_ps(&aec->sxd[i][0]); const __m128 vec_sxd_7654 = _mm_loadu_ps(&aec->sxd[i + 2][0]); const __m128 vec_sde_0 = _mm_shuffle_ps(vec_sde_3210, vec_sde_7654, _MM_SHUFFLE(2, 0, 2, 0)); const __m128 vec_sde_1 = _mm_shuffle_ps(vec_sde_3210, vec_sde_7654, _MM_SHUFFLE(3, 1, 3, 1)); const __m128 vec_sxd_0 = _mm_shuffle_ps(vec_sxd_3210, vec_sxd_7654, _MM_SHUFFLE(2, 0, 2, 0)); const __m128 vec_sxd_1 = _mm_shuffle_ps(vec_sxd_3210, vec_sxd_7654, _MM_SHUFFLE(3, 1, 3, 1)); __m128 vec_cohde = _mm_mul_ps(vec_sde_0, vec_sde_0); __m128 vec_cohxd = _mm_mul_ps(vec_sxd_0, vec_sxd_0); vec_cohde = _mm_add_ps(vec_cohde, _mm_mul_ps(vec_sde_1, vec_sde_1)); vec_cohde = _mm_div_ps(vec_cohde, vec_sdse); vec_cohxd = _mm_add_ps(vec_cohxd, _mm_mul_ps(vec_sxd_1, vec_sxd_1)); vec_cohxd = _mm_div_ps(vec_cohxd, vec_sdsx); _mm_storeu_ps(&cohde[i], vec_cohde); _mm_storeu_ps(&cohxd[i], vec_cohxd); } // scalar code for the remaining items. for (; i < PART_LEN1; i++) { cohde[i] = (aec->sde[i][0] * aec->sde[i][0] + aec->sde[i][1] * aec->sde[i][1]) / (aec->sd[i] * aec->se[i] + 1e-10f); cohxd[i] = (aec->sxd[i][0] * aec->sxd[i][0] + aec->sxd[i][1] * aec->sxd[i][1]) / (aec->sx[i] * aec->sd[i] + 1e-10f); } } }
static void FilterAdaptationNEON( int num_partitions, int x_fft_buf_block_pos, float x_fft_buf[2][kExtendedNumPartitions * PART_LEN1], float e_fft[2][PART_LEN1], float h_fft_buf[2][kExtendedNumPartitions * PART_LEN1]) { float fft[PART_LEN2]; int i; for (i = 0; i < num_partitions; i++) { int xPos = (i + x_fft_buf_block_pos) * PART_LEN1; int pos = i * PART_LEN1; int j; // Check for wrap if (i + x_fft_buf_block_pos >= num_partitions) { xPos -= num_partitions * PART_LEN1; } // Process the whole array... for (j = 0; j < PART_LEN; j += 4) { // Load x_fft_buf and e_fft. const float32x4_t x_fft_buf_re = vld1q_f32(&x_fft_buf[0][xPos + j]); const float32x4_t x_fft_buf_im = vld1q_f32(&x_fft_buf[1][xPos + j]); const float32x4_t e_fft_re = vld1q_f32(&e_fft[0][j]); const float32x4_t e_fft_im = vld1q_f32(&e_fft[1][j]); // Calculate the product of conjugate(x_fft_buf) by e_fft. // re(conjugate(a) * b) = aRe * bRe + aIm * bIm // im(conjugate(a) * b)= aRe * bIm - aIm * bRe const float32x4_t a = vmulq_f32(x_fft_buf_re, e_fft_re); const float32x4_t e = vmlaq_f32(a, x_fft_buf_im, e_fft_im); const float32x4_t c = vmulq_f32(x_fft_buf_re, e_fft_im); const float32x4_t f = vmlsq_f32(c, x_fft_buf_im, e_fft_re); // Interleave real and imaginary parts. const float32x4x2_t g_n_h = vzipq_f32(e, f); // Store vst1q_f32(&fft[2 * j + 0], g_n_h.val[0]); vst1q_f32(&fft[2 * j + 4], g_n_h.val[1]); } // ... and fixup the first imaginary entry. fft[1] = MulRe(x_fft_buf[0][xPos + PART_LEN], -x_fft_buf[1][xPos + PART_LEN], e_fft[0][PART_LEN], e_fft[1][PART_LEN]); aec_rdft_inverse_128(fft); memset(fft + PART_LEN, 0, sizeof(float) * PART_LEN); // fft scaling { const float scale = 2.0f / PART_LEN2; const float32x4_t scale_ps = vmovq_n_f32(scale); for (j = 0; j < PART_LEN; j += 4) { const float32x4_t fft_ps = vld1q_f32(&fft[j]); const float32x4_t fft_scale = vmulq_f32(fft_ps, scale_ps); vst1q_f32(&fft[j], fft_scale); } } aec_rdft_forward_128(fft); { const float wt1 = h_fft_buf[1][pos]; h_fft_buf[0][pos + PART_LEN] += fft[1]; for (j = 0; j < PART_LEN; j += 4) { float32x4_t wtBuf_re = vld1q_f32(&h_fft_buf[0][pos + j]); float32x4_t wtBuf_im = vld1q_f32(&h_fft_buf[1][pos + j]); const float32x4_t fft0 = vld1q_f32(&fft[2 * j + 0]); const float32x4_t fft4 = vld1q_f32(&fft[2 * j + 4]); const float32x4x2_t fft_re_im = vuzpq_f32(fft0, fft4); wtBuf_re = vaddq_f32(wtBuf_re, fft_re_im.val[0]); wtBuf_im = vaddq_f32(wtBuf_im, fft_re_im.val[1]); vst1q_f32(&h_fft_buf[0][pos + j], wtBuf_re); vst1q_f32(&h_fft_buf[1][pos + j], wtBuf_im); } h_fft_buf[1][pos] = wt1; } } }
static void NonLinearProcessing(aec_t *aec, short *output, short *outputH) { float efw[2][PART_LEN1], dfw[2][PART_LEN1], xfw[2][PART_LEN1]; complex_t comfortNoiseHband[PART_LEN1]; float fft[PART_LEN2]; float scale, dtmp; float nlpGainHband; int i, j, pos; // Coherence and non-linear filter float cohde[PART_LEN1], cohxd[PART_LEN1]; float hNlDeAvg, hNlXdAvg; float hNl[PART_LEN1]; float hNlPref[PREF_BAND_SIZE]; float hNlFb = 0, hNlFbLow = 0; const float prefBandQuant = 0.75f, prefBandQuantLow = 0.5f; const int prefBandSize = PREF_BAND_SIZE / aec->mult; const int minPrefBand = 4 / aec->mult; // Near and error power sums float sdSum = 0, seSum = 0; // Power estimate smoothing coefficients const float gCoh[2][2] = {{0.9f, 0.1f}, {0.93f, 0.07f}}; const float *ptrGCoh = gCoh[aec->mult - 1]; // Filter energy float wfEnMax = 0, wfEn = 0; const int delayEstInterval = 10 * aec->mult; float* xfw_ptr = NULL; aec->delayEstCtr++; if (aec->delayEstCtr == delayEstInterval) { aec->delayEstCtr = 0; } // initialize comfort noise for H band memset(comfortNoiseHband, 0, sizeof(comfortNoiseHband)); nlpGainHband = (float)0.0; dtmp = (float)0.0; // Measure energy in each filter partition to determine delay. // TODO: Spread by computing one partition per block? if (aec->delayEstCtr == 0) { wfEnMax = 0; aec->delayIdx = 0; for (i = 0; i < NR_PART; i++) { pos = i * PART_LEN1; wfEn = 0; for (j = 0; j < PART_LEN1; j++) { wfEn += aec->wfBuf[0][pos + j] * aec->wfBuf[0][pos + j] + aec->wfBuf[1][pos + j] * aec->wfBuf[1][pos + j]; } if (wfEn > wfEnMax) { wfEnMax = wfEn; aec->delayIdx = i; } } } // We should always have at least one element stored in |far_buf|. assert(WebRtc_available_read(aec->far_buf_windowed) > 0); // NLP WebRtc_ReadBuffer(aec->far_buf_windowed, (void**) &xfw_ptr, &xfw[0][0], 1); // TODO(bjornv): Investigate if we can reuse |far_buf_windowed| instead of // |xfwBuf|. // Buffer far. memcpy(aec->xfwBuf, xfw_ptr, sizeof(float) * 2 * PART_LEN1); // Use delayed far. memcpy(xfw, aec->xfwBuf + aec->delayIdx * PART_LEN1, sizeof(xfw)); // Windowed near fft for (i = 0; i < PART_LEN; i++) { fft[i] = aec->dBuf[i] * sqrtHanning[i]; fft[PART_LEN + i] = aec->dBuf[PART_LEN + i] * sqrtHanning[PART_LEN - i]; } aec_rdft_forward_128(fft); dfw[1][0] = 0; dfw[1][PART_LEN] = 0; dfw[0][0] = fft[0]; dfw[0][PART_LEN] = fft[1]; for (i = 1; i < PART_LEN; i++) { dfw[0][i] = fft[2 * i]; dfw[1][i] = fft[2 * i + 1]; } // Windowed error fft for (i = 0; i < PART_LEN; i++) { fft[i] = aec->eBuf[i] * sqrtHanning[i]; fft[PART_LEN + i] = aec->eBuf[PART_LEN + i] * sqrtHanning[PART_LEN - i]; } aec_rdft_forward_128(fft); efw[1][0] = 0; efw[1][PART_LEN] = 0; efw[0][0] = fft[0]; efw[0][PART_LEN] = fft[1]; for (i = 1; i < PART_LEN; i++) { efw[0][i] = fft[2 * i]; efw[1][i] = fft[2 * i + 1]; } // Smoothed PSD for (i = 0; i < PART_LEN1; i++) { aec->sd[i] = ptrGCoh[0] * aec->sd[i] + ptrGCoh[1] * (dfw[0][i] * dfw[0][i] + dfw[1][i] * dfw[1][i]); aec->se[i] = ptrGCoh[0] * aec->se[i] + ptrGCoh[1] * (efw[0][i] * efw[0][i] + efw[1][i] * efw[1][i]); // We threshold here to protect against the ill-effects of a zero farend. // The threshold is not arbitrarily chosen, but balances protection and // adverse interaction with the algorithm's tuning. // TODO: investigate further why this is so sensitive. aec->sx[i] = ptrGCoh[0] * aec->sx[i] + ptrGCoh[1] * WEBRTC_SPL_MAX(xfw[0][i] * xfw[0][i] + xfw[1][i] * xfw[1][i], 15); aec->sde[i][0] = ptrGCoh[0] * aec->sde[i][0] + ptrGCoh[1] * (dfw[0][i] * efw[0][i] + dfw[1][i] * efw[1][i]); aec->sde[i][1] = ptrGCoh[0] * aec->sde[i][1] + ptrGCoh[1] * (dfw[0][i] * efw[1][i] - dfw[1][i] * efw[0][i]); aec->sxd[i][0] = ptrGCoh[0] * aec->sxd[i][0] + ptrGCoh[1] * (dfw[0][i] * xfw[0][i] + dfw[1][i] * xfw[1][i]); aec->sxd[i][1] = ptrGCoh[0] * aec->sxd[i][1] + ptrGCoh[1] * (dfw[0][i] * xfw[1][i] - dfw[1][i] * xfw[0][i]); sdSum += aec->sd[i]; seSum += aec->se[i]; } // Divergent filter safeguard. if (aec->divergeState == 0) { if (seSum > sdSum) { aec->divergeState = 1; } } else { if (seSum * 1.05f < sdSum) { aec->divergeState = 0; } } if (aec->divergeState == 1) { memcpy(efw, dfw, sizeof(efw)); } // Reset if error is significantly larger than nearend (13 dB). if (seSum > (19.95f * sdSum)) { memset(aec->wfBuf, 0, sizeof(aec->wfBuf)); } // Subband coherence for (i = 0; i < PART_LEN1; i++) { cohde[i] = (aec->sde[i][0] * aec->sde[i][0] + aec->sde[i][1] * aec->sde[i][1]) / (aec->sd[i] * aec->se[i] + 1e-10f); cohxd[i] = (aec->sxd[i][0] * aec->sxd[i][0] + aec->sxd[i][1] * aec->sxd[i][1]) / (aec->sx[i] * aec->sd[i] + 1e-10f); } hNlXdAvg = 0; for (i = minPrefBand; i < prefBandSize + minPrefBand; i++) { hNlXdAvg += cohxd[i]; } hNlXdAvg /= prefBandSize; hNlXdAvg = 1 - hNlXdAvg; hNlDeAvg = 0; for (i = minPrefBand; i < prefBandSize + minPrefBand; i++) { hNlDeAvg += cohde[i]; } hNlDeAvg /= prefBandSize; if (hNlXdAvg < 0.75f && hNlXdAvg < aec->hNlXdAvgMin) { aec->hNlXdAvgMin = hNlXdAvg; } if (hNlDeAvg > 0.98f && hNlXdAvg > 0.9f) { aec->stNearState = 1; } else if (hNlDeAvg < 0.95f || hNlXdAvg < 0.8f) { aec->stNearState = 0; } if (aec->hNlXdAvgMin == 1) { aec->echoState = 0; aec->overDrive = aec->minOverDrive; if (aec->stNearState == 1) { memcpy(hNl, cohde, sizeof(hNl)); hNlFb = hNlDeAvg; hNlFbLow = hNlDeAvg; } else { for (i = 0; i < PART_LEN1; i++) { hNl[i] = 1 - cohxd[i]; } hNlFb = hNlXdAvg; hNlFbLow = hNlXdAvg; } } else { if (aec->stNearState == 1) { aec->echoState = 0; memcpy(hNl, cohde, sizeof(hNl)); hNlFb = hNlDeAvg; hNlFbLow = hNlDeAvg; } else { aec->echoState = 1; for (i = 0; i < PART_LEN1; i++) { hNl[i] = WEBRTC_SPL_MIN(cohde[i], 1 - cohxd[i]); } // Select an order statistic from the preferred bands. // TODO: Using quicksort now, but a selection algorithm may be preferred. memcpy(hNlPref, &hNl[minPrefBand], sizeof(float) * prefBandSize); qsort(hNlPref, prefBandSize, sizeof(float), CmpFloat); hNlFb = hNlPref[(int)floor(prefBandQuant * (prefBandSize - 1))]; hNlFbLow = hNlPref[(int)floor(prefBandQuantLow * (prefBandSize - 1))]; } } // Track the local filter minimum to determine suppression overdrive. if (hNlFbLow < 0.6f && hNlFbLow < aec->hNlFbLocalMin) { aec->hNlFbLocalMin = hNlFbLow; aec->hNlFbMin = hNlFbLow; aec->hNlNewMin = 1; aec->hNlMinCtr = 0; } aec->hNlFbLocalMin = WEBRTC_SPL_MIN(aec->hNlFbLocalMin + 0.0008f / aec->mult, 1); aec->hNlXdAvgMin = WEBRTC_SPL_MIN(aec->hNlXdAvgMin + 0.0006f / aec->mult, 1); if (aec->hNlNewMin == 1) { aec->hNlMinCtr++; } if (aec->hNlMinCtr == 2) { aec->hNlNewMin = 0; aec->hNlMinCtr = 0; aec->overDrive = WEBRTC_SPL_MAX(aec->targetSupp / ((float)log(aec->hNlFbMin + 1e-10f) + 1e-10f), aec->minOverDrive); } // Smooth the overdrive. if (aec->overDrive < aec->overDriveSm) { aec->overDriveSm = 0.99f * aec->overDriveSm + 0.01f * aec->overDrive; } else { aec->overDriveSm = 0.9f * aec->overDriveSm + 0.1f * aec->overDrive; } WebRtcAec_OverdriveAndSuppress(aec, hNl, hNlFb, efw); // Add comfort noise. ComfortNoise(aec, efw, comfortNoiseHband, aec->noisePow, hNl); // TODO(bjornv): Investigate how to take the windowing below into account if // needed. if (aec->metricsMode == 1) { // Note that we have a scaling by two in the time domain |eBuf|. // In addition the time domain signal is windowed before transformation, // losing half the energy on the average. We take care of the first // scaling only in UpdateMetrics(). UpdateLevel(&aec->nlpoutlevel, efw); } // Inverse error fft. fft[0] = efw[0][0]; fft[1] = efw[0][PART_LEN]; for (i = 1; i < PART_LEN; i++) { fft[2*i] = efw[0][i]; // Sign change required by Ooura fft. fft[2*i + 1] = -efw[1][i]; } aec_rdft_inverse_128(fft); // Overlap and add to obtain output. scale = 2.0f / PART_LEN2; for (i = 0; i < PART_LEN; i++) { fft[i] *= scale; // fft scaling fft[i] = fft[i]*sqrtHanning[i] + aec->outBuf[i]; // Saturation protection output[i] = (short)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, fft[i], WEBRTC_SPL_WORD16_MIN); fft[PART_LEN + i] *= scale; // fft scaling aec->outBuf[i] = fft[PART_LEN + i] * sqrtHanning[PART_LEN - i]; } // For H band if (aec->sampFreq == 32000) { // H band gain // average nlp over low band: average over second half of freq spectrum // (4->8khz) GetHighbandGain(hNl, &nlpGainHband); // Inverse comfort_noise if (flagHbandCn == 1) { fft[0] = comfortNoiseHband[0][0]; fft[1] = comfortNoiseHband[PART_LEN][0]; for (i = 1; i < PART_LEN; i++) { fft[2*i] = comfortNoiseHband[i][0]; fft[2*i + 1] = comfortNoiseHband[i][1]; } aec_rdft_inverse_128(fft); scale = 2.0f / PART_LEN2; } // compute gain factor for (i = 0; i < PART_LEN; i++) { dtmp = (float)aec->dBufH[i]; dtmp = (float)dtmp * nlpGainHband; // for variable gain // add some comfort noise where Hband is attenuated if (flagHbandCn == 1) { fft[i] *= scale; // fft scaling dtmp += cnScaleHband * fft[i]; } // Saturation protection outputH[i] = (short)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, dtmp, WEBRTC_SPL_WORD16_MIN); } } // Copy the current block to the old position. memcpy(aec->dBuf, aec->dBuf + PART_LEN, sizeof(float) * PART_LEN); memcpy(aec->eBuf, aec->eBuf + PART_LEN, sizeof(float) * PART_LEN); // Copy the current block to the old position for H band if (aec->sampFreq == 32000) { memcpy(aec->dBufH, aec->dBufH + PART_LEN, sizeof(float) * PART_LEN); } memmove(aec->xfwBuf + PART_LEN1, aec->xfwBuf, sizeof(aec->xfwBuf) - sizeof(complex_t) * PART_LEN1); }
static void ProcessBlock(aec_t* aec) { int i; float d[PART_LEN], y[PART_LEN], e[PART_LEN], dH[PART_LEN]; float scale; float fft[PART_LEN2]; float xf[2][PART_LEN1], yf[2][PART_LEN1], ef[2][PART_LEN1]; float df[2][PART_LEN1]; float far_spectrum = 0.0f; float near_spectrum = 0.0f; float abs_far_spectrum[PART_LEN1]; float abs_near_spectrum[PART_LEN1]; const float gPow[2] = {0.9f, 0.1f}; // Noise estimate constants. const int noiseInitBlocks = 500 * aec->mult; const float step = 0.1f; const float ramp = 1.0002f; const float gInitNoise[2] = {0.999f, 0.001f}; int16_t nearend[PART_LEN]; int16_t* nearend_ptr = NULL; int16_t output[PART_LEN]; int16_t outputH[PART_LEN]; float* xf_ptr = NULL; memset(dH, 0, sizeof(dH)); if (aec->sampFreq == 32000) { // Get the upper band first so we can reuse |nearend|. WebRtc_ReadBuffer(aec->nearFrBufH, (void**) &nearend_ptr, nearend, PART_LEN); for (i = 0; i < PART_LEN; i++) { dH[i] = (float) (nearend_ptr[i]); } memcpy(aec->dBufH + PART_LEN, dH, sizeof(float) * PART_LEN); } WebRtc_ReadBuffer(aec->nearFrBuf, (void**) &nearend_ptr, nearend, PART_LEN); // ---------- Ooura fft ---------- // Concatenate old and new nearend blocks. for (i = 0; i < PART_LEN; i++) { d[i] = (float) (nearend_ptr[i]); } memcpy(aec->dBuf + PART_LEN, d, sizeof(float) * PART_LEN); #ifdef WEBRTC_AEC_DEBUG_DUMP { int16_t farend[PART_LEN]; int16_t* farend_ptr = NULL; WebRtc_ReadBuffer(aec->far_time_buf, (void**) &farend_ptr, farend, 1); (void)fwrite(farend_ptr, sizeof(int16_t), PART_LEN, aec->farFile); (void)fwrite(nearend_ptr, sizeof(int16_t), PART_LEN, aec->nearFile); } #endif // We should always have at least one element stored in |far_buf|. assert(WebRtc_available_read(aec->far_buf) > 0); WebRtc_ReadBuffer(aec->far_buf, (void**) &xf_ptr, &xf[0][0], 1); // Near fft memcpy(fft, aec->dBuf, sizeof(float) * PART_LEN2); TimeToFrequency(fft, df, 0); // Power smoothing for (i = 0; i < PART_LEN1; i++) { far_spectrum = (xf_ptr[i] * xf_ptr[i]) + (xf_ptr[PART_LEN1 + i] * xf_ptr[PART_LEN1 + i]); aec->xPow[i] = gPow[0] * aec->xPow[i] + gPow[1] * NR_PART * far_spectrum; // Calculate absolute spectra abs_far_spectrum[i] = sqrtf(far_spectrum); near_spectrum = df[0][i] * df[0][i] + df[1][i] * df[1][i]; aec->dPow[i] = gPow[0] * aec->dPow[i] + gPow[1] * near_spectrum; // Calculate absolute spectra abs_near_spectrum[i] = sqrtf(near_spectrum); } // Estimate noise power. Wait until dPow is more stable. if (aec->noiseEstCtr > 50) { for (i = 0; i < PART_LEN1; i++) { if (aec->dPow[i] < aec->dMinPow[i]) { aec->dMinPow[i] = (aec->dPow[i] + step * (aec->dMinPow[i] - aec->dPow[i])) * ramp; } else { aec->dMinPow[i] *= ramp; } } } // Smooth increasing noise power from zero at the start, // to avoid a sudden burst of comfort noise. if (aec->noiseEstCtr < noiseInitBlocks) { aec->noiseEstCtr++; for (i = 0; i < PART_LEN1; i++) { if (aec->dMinPow[i] > aec->dInitMinPow[i]) { aec->dInitMinPow[i] = gInitNoise[0] * aec->dInitMinPow[i] + gInitNoise[1] * aec->dMinPow[i]; } else { aec->dInitMinPow[i] = aec->dMinPow[i]; } } aec->noisePow = aec->dInitMinPow; } else { aec->noisePow = aec->dMinPow; } // Block wise delay estimation used for logging if (aec->delay_logging_enabled) { int delay_estimate = 0; // Estimate the delay delay_estimate = WebRtc_DelayEstimatorProcessFloat(aec->delay_estimator, abs_far_spectrum, abs_near_spectrum, PART_LEN1); if (delay_estimate >= 0) { // Update delay estimate buffer. aec->delay_histogram[delay_estimate]++; } } // Update the xfBuf block position. aec->xfBufBlockPos--; if (aec->xfBufBlockPos == -1) { aec->xfBufBlockPos = NR_PART - 1; } // Buffer xf memcpy(aec->xfBuf[0] + aec->xfBufBlockPos * PART_LEN1, xf_ptr, sizeof(float) * PART_LEN1); memcpy(aec->xfBuf[1] + aec->xfBufBlockPos * PART_LEN1, &xf_ptr[PART_LEN1], sizeof(float) * PART_LEN1); memset(yf, 0, sizeof(yf)); // Filter far WebRtcAec_FilterFar(aec, yf); // Inverse fft to obtain echo estimate and error. fft[0] = yf[0][0]; fft[1] = yf[0][PART_LEN]; for (i = 1; i < PART_LEN; i++) { fft[2 * i] = yf[0][i]; fft[2 * i + 1] = yf[1][i]; } aec_rdft_inverse_128(fft); scale = 2.0f / PART_LEN2; for (i = 0; i < PART_LEN; i++) { y[i] = fft[PART_LEN + i] * scale; // fft scaling } for (i = 0; i < PART_LEN; i++) { e[i] = d[i] - y[i]; } // Error fft memcpy(aec->eBuf + PART_LEN, e, sizeof(float) * PART_LEN); memset(fft, 0, sizeof(float) * PART_LEN); memcpy(fft + PART_LEN, e, sizeof(float) * PART_LEN); // TODO(bjornv): Change to use TimeToFrequency(). aec_rdft_forward_128(fft); ef[1][0] = 0; ef[1][PART_LEN] = 0; ef[0][0] = fft[0]; ef[0][PART_LEN] = fft[1]; for (i = 1; i < PART_LEN; i++) { ef[0][i] = fft[2 * i]; ef[1][i] = fft[2 * i + 1]; } if (aec->metricsMode == 1) { // Note that the first PART_LEN samples in fft (before transformation) are // zero. Hence, the scaling by two in UpdateLevel() should not be // performed. That scaling is taken care of in UpdateMetrics() instead. UpdateLevel(&aec->linoutlevel, ef); } // Scale error signal inversely with far power. WebRtcAec_ScaleErrorSignal(aec, ef); WebRtcAec_FilterAdaptation(aec, fft, ef); NonLinearProcessing(aec, output, outputH); if (aec->metricsMode == 1) { // Update power levels and echo metrics UpdateLevel(&aec->farlevel, (float (*)[PART_LEN1]) xf_ptr); UpdateLevel(&aec->nearlevel, df); UpdateMetrics(aec); } // Store the output block. WebRtc_WriteBuffer(aec->outFrBuf, output, PART_LEN); // For H band if (aec->sampFreq == 32000) { WebRtc_WriteBuffer(aec->outFrBufH, outputH, PART_LEN); } #ifdef WEBRTC_AEC_DEBUG_DUMP { int16_t eInt16[PART_LEN]; for (i = 0; i < PART_LEN; i++) { eInt16[i] = (int16_t)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, e[i], WEBRTC_SPL_WORD16_MIN); } (void)fwrite(eInt16, sizeof(int16_t), PART_LEN, aec->outLinearFile); (void)fwrite(output, sizeof(int16_t), PART_LEN, aec->outFile); } #endif }
void ProcessBlock_background(aec_t *aec, const short *farend, const short *nearend, short *output, short vadState,short *farFrameAtEchoDelay) { int i; float d[PART_LEN], y[PART_LEN], e[PART_LEN], dH[PART_LEN]; short eInt16[PART_LEN]; float scale; float fft[PART_LEN2],fft_re[PART_LEN2],fft_im[PART_LEN2]; float xf[2][PART_LEN1], yf[2][PART_LEN1], ef[2][PART_LEN1]; complex_t df[PART_LEN1]; float far_spectrum = 0.0f; float near_spectrum = 0.0f; float abs_far_spectrum[PART_LEN1]; float abs_near_spectrum[PART_LEN1]; const float gPow[2] = {0.9f, 0.1f}; // Noise estimate constants. const int noiseInitBlocks = 500 * aec->mult; const float step = 0.1f; const float ramp = 1.0002f; const float gInitNoise[2] = {0.999f, 0.001f}; memset(dH, 0, sizeof(dH)); // ---------- Ooura fft ---------- // Concatenate old and new farend blocks. for (i = 0; i < PART_LEN; i++) { aec->xBuf_background[i + PART_LEN] = (float)farend[i]; d[i] = (float)nearend[i]; } memcpy(fft, aec->xBuf_background, sizeof(float) * PART_LEN2); memcpy(aec->dBuf_background + PART_LEN, d, sizeof(float) * PART_LEN); aec_rdft_forward_128(fft); // Far fft xf[1][0] = 0; xf[1][PART_LEN] = 0; xf[0][0] = fft[0]; xf[0][PART_LEN] = 0;//fft[1]; for (i = 1; i < PART_LEN; i++) { xf[0][i] = fft[2 * i]; xf[1][i] = fft[2 * i + 1]; } // Near fft memcpy(fft, aec->dBuf_background, sizeof(float) * PART_LEN2); aec_rdft_forward_128(fft); df[0][1] = 0; df[PART_LEN][1] = 0; df[0][0] = fft[0]; df[PART_LEN][0] = fft[1]; for (i = 1; i < PART_LEN; i++) { df[i][0] = fft[2 * i]; df[i][1] = fft[2 * i + 1]; } // Power smoothing for (i = 0; i < PART_LEN1; i++) { far_spectrum = xf[0][i] * xf[0][i] + xf[1][i] * xf[1][i]; aec->xPow[i] = gPow[0] * aec->xPow[i] + gPow[1] * NR_PART * far_spectrum; // Calculate absolute spectra abs_far_spectrum[i] = sqrtf(far_spectrum); near_spectrum = df[i][0] * df[i][0] + df[i][1] * df[i][1]; aec->dPow[i] = gPow[0] * aec->dPow[i] + gPow[1] * near_spectrum; // Calculate absolute spectra abs_near_spectrum[i] = sqrtf(near_spectrum); } // Estimate noise power. Wait until dPow is more stable. if (aec->noiseEstCtr > 50) { for (i = 0; i < PART_LEN1; i++) { if (aec->dPow[i] < aec->dMinPow[i]) { aec->dMinPow[i] = (aec->dPow[i] + step * (aec->dMinPow[i] - aec->dPow[i])) * ramp; } else { aec->dMinPow[i] *= ramp; } } } // Smooth increasing noise power from zero at the start, // to avoid a sudden burst of comfort noise. if (aec->noiseEstCtr < noiseInitBlocks) { aec->noiseEstCtr++; for (i = 0; i < PART_LEN1; i++) { if (aec->dMinPow[i] > aec->dInitMinPow[i]) { aec->dInitMinPow[i] = gInitNoise[0] * aec->dInitMinPow[i] + gInitNoise[1] * aec->dMinPow[i]; } else { aec->dInitMinPow[i] = aec->dMinPow[i]; } } aec->noisePow = aec->dInitMinPow; } else { aec->noisePow = aec->dMinPow; } // Update the xfBuf block position. aec->xfBufBlockPos_background--; if (aec->xfBufBlockPos_background == -1) { aec->xfBufBlockPos_background = NR_PART - 1; } // Buffer xf memcpy(aec->xfBuf_background[0] + aec->xfBufBlockPos_background * PART_LEN1, xf[0], sizeof(float) * PART_LEN1); memcpy(aec->xfBuf_background[1] + aec->xfBufBlockPos_background * PART_LEN1, xf[1], sizeof(float) * PART_LEN1); memset(yf[0], 0, sizeof(float) * (PART_LEN1 * 2)); // Filter far FilterFar_background(aec, yf); // Inverse fft to obtain echo estimate and error. fft[0] = yf[0][0]; fft[1] = yf[0][PART_LEN]; for (i = 1; i < PART_LEN; i++) { fft[2 * i] = yf[0][i]; fft[2 * i + 1] = yf[1][i]; } aec_rdft_inverse_128(fft); scale = 2.0f / PART_LEN2; for (i = 0; i < PART_LEN; i++) { y[i] = fft[PART_LEN + i] * scale; // fft scaling } for (i = 0; i < PART_LEN; i++) { e[i] = d[i] - y[i]; } // Error fft memcpy(aec->eBuf_background + PART_LEN, e, sizeof(float) * PART_LEN); memset(fft, 0, sizeof(float) * PART_LEN); memcpy(fft + PART_LEN, e, sizeof(float) * PART_LEN); aec_rdft_forward_128(fft); ef[1][0] = 0; ef[1][PART_LEN] = 0; ef[0][0] = fft[0]; ef[0][PART_LEN] = fft[1]; for (i = 1; i < PART_LEN; i++) { ef[0][i] = fft[2 * i]; ef[1][i] = fft[2 * i + 1]; } // Scale error signal inversely with far power. WebRtcAec_ScaleErrorSignal(aec, ef); if(aec->framePowerAtProbableEchoDelay_shortTerm>1000000) FilterAdaptation_background(aec, fft, ef); aec->background_lt_filteredop_power=(float)(aec->background_lt_filteredop_power*.95+0.05*frame_power(aec->eBuf_background,PART_LEN2)); for (i = 0; i < PART_LEN; i++) { output[i] = (int16_t)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, aec->eBuf_background[i], WEBRTC_SPL_WORD16_MIN); } memcpy(aec->xBuf_background, aec->xBuf_background + PART_LEN, sizeof(float) * PART_LEN); memcpy(aec->dBuf_background, aec->dBuf_background + PART_LEN, sizeof(float) * PART_LEN); memcpy(aec->eBuf_background, aec->eBuf_background + PART_LEN, sizeof(float) * PART_LEN); }
static void SubbandCoherenceNEON(AecCore* aec, float efw[2][PART_LEN1], float xfw[2][PART_LEN1], float* fft, float* cohde, float* cohxd) { float dfw[2][PART_LEN1]; int i; if (aec->delayEstCtr == 0) aec->delayIdx = PartitionDelay(aec); // Use delayed far. memcpy(xfw, aec->xfwBuf + aec->delayIdx * PART_LEN1, sizeof(xfw[0][0]) * 2 * PART_LEN1); // Windowed near fft WindowData(fft, aec->dBuf); aec_rdft_forward_128(fft); StoreAsComplex(fft, dfw); // Windowed error fft WindowData(fft, aec->eBuf); aec_rdft_forward_128(fft); StoreAsComplex(fft, efw); SmoothedPSD(aec, efw, dfw, xfw); { const float32x4_t vec_1eminus10 = vdupq_n_f32(1e-10f); // Subband coherence for (i = 0; i + 3 < PART_LEN1; i += 4) { const float32x4_t vec_sd = vld1q_f32(&aec->sd[i]); const float32x4_t vec_se = vld1q_f32(&aec->se[i]); const float32x4_t vec_sx = vld1q_f32(&aec->sx[i]); const float32x4_t vec_sdse = vmlaq_f32(vec_1eminus10, vec_sd, vec_se); const float32x4_t vec_sdsx = vmlaq_f32(vec_1eminus10, vec_sd, vec_sx); float32x4x2_t vec_sde = vld2q_f32(&aec->sde[i][0]); float32x4x2_t vec_sxd = vld2q_f32(&aec->sxd[i][0]); float32x4_t vec_cohde = vmulq_f32(vec_sde.val[0], vec_sde.val[0]); float32x4_t vec_cohxd = vmulq_f32(vec_sxd.val[0], vec_sxd.val[0]); vec_cohde = vmlaq_f32(vec_cohde, vec_sde.val[1], vec_sde.val[1]); vec_cohde = vdivq_f32(vec_cohde, vec_sdse); vec_cohxd = vmlaq_f32(vec_cohxd, vec_sxd.val[1], vec_sxd.val[1]); vec_cohxd = vdivq_f32(vec_cohxd, vec_sdsx); vst1q_f32(&cohde[i], vec_cohde); vst1q_f32(&cohxd[i], vec_cohxd); } } // scalar code for the remaining items. for (; i < PART_LEN1; i++) { cohde[i] = (aec->sde[i][0] * aec->sde[i][0] + aec->sde[i][1] * aec->sde[i][1]) / (aec->sd[i] * aec->se[i] + 1e-10f); cohxd[i] = (aec->sxd[i][0] * aec->sxd[i][0] + aec->sxd[i][1] * aec->sxd[i][1]) / (aec->sx[i] * aec->sd[i] + 1e-10f); } }