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 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 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);
}
