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, which is presented with time-aligned signals. 13 */ 14 15 #include <math.h> 16 #include <stdlib.h> 17 #include <string.h> 18 19 #include "aec_core.h" 20 #include "aec_rdft.h" 21 #include "ring_buffer.h" 22 #include "system_wrappers/interface/cpu_features_wrapper.h" 23 24 // Noise suppression 25 static const int converged = 250; 26 27 // Metrics 28 static const int subCountLen = 4; 29 static const int countLen = 50; 30 31 // Quantities to control H band scaling for SWB input 32 static const int flagHbandCn = 1; // flag for adding comfort noise in H band 33 static const float cnScaleHband = (float)0.4; // scale for comfort noise in H band 34 // Initial bin for averaging nlp gain in low band 35 static const int freqAvgIc = PART_LEN / 2; 36 37 /* Matlab code to produce table: 38 win = sqrt(hanning(63)); win = [0 ; win(1:32)]; 39 fprintf(1, '\t%.14f, %.14f, %.14f,\n', win); 40 */ 41 /* 42 static const float sqrtHanning[33] = { 43 0.00000000000000, 0.04906767432742, 0.09801714032956, 44 0.14673047445536, 0.19509032201613, 0.24298017990326, 45 0.29028467725446, 0.33688985339222, 0.38268343236509, 46 0.42755509343028, 0.47139673682600, 0.51410274419322, 47 0.55557023301960, 0.59569930449243, 0.63439328416365, 48 0.67155895484702, 0.70710678118655, 0.74095112535496, 49 0.77301045336274, 0.80320753148064, 0.83146961230255, 50 0.85772861000027, 0.88192126434835, 0.90398929312344, 51 0.92387953251129, 0.94154406518302, 0.95694033573221, 52 0.97003125319454, 0.98078528040323, 0.98917650996478, 53 0.99518472667220, 0.99879545620517, 1.00000000000000 54 }; 55 */ 56 57 static const float sqrtHanning[65] = { 58 0.00000000000000f, 0.02454122852291f, 0.04906767432742f, 59 0.07356456359967f, 0.09801714032956f, 0.12241067519922f, 60 0.14673047445536f, 0.17096188876030f, 0.19509032201613f, 61 0.21910124015687f, 0.24298017990326f, 0.26671275747490f, 62 0.29028467725446f, 0.31368174039889f, 0.33688985339222f, 63 0.35989503653499f, 0.38268343236509f, 0.40524131400499f, 64 0.42755509343028f, 0.44961132965461f, 0.47139673682600f, 65 0.49289819222978f, 0.51410274419322f, 0.53499761988710f, 66 0.55557023301960f, 0.57580819141785f, 0.59569930449243f, 67 0.61523159058063f, 0.63439328416365f, 0.65317284295378f, 68 0.67155895484702f, 0.68954054473707f, 0.70710678118655f, 69 0.72424708295147f, 0.74095112535496f, 0.75720884650648f, 70 0.77301045336274f, 0.78834642762661f, 0.80320753148064f, 71 0.81758481315158f, 0.83146961230255f, 0.84485356524971f, 72 0.85772861000027f, 0.87008699110871f, 0.88192126434835f, 73 0.89322430119552f, 0.90398929312344f, 0.91420975570353f, 74 0.92387953251129f, 0.93299279883474f, 0.94154406518302f, 75 0.94952818059304f, 0.95694033573221f, 0.96377606579544f, 76 0.97003125319454f, 0.97570213003853f, 0.98078528040323f, 77 0.98527764238894f, 0.98917650996478f, 0.99247953459871f, 78 0.99518472667220f, 0.99729045667869f, 0.99879545620517f, 79 0.99969881869620f, 1.00000000000000f 80 }; 81 82 /* Matlab code to produce table: 83 weightCurve = [0 ; 0.3 * sqrt(linspace(0,1,64))' + 0.1]; 84 fprintf(1, '\t%.4f, %.4f, %.4f, %.4f, %.4f, %.4f,\n', weightCurve); 85 */ 86 const float WebRtcAec_weightCurve[65] = { 87 0.0000f, 0.1000f, 0.1378f, 0.1535f, 0.1655f, 0.1756f, 88 0.1845f, 0.1926f, 0.2000f, 0.2069f, 0.2134f, 0.2195f, 89 0.2254f, 0.2309f, 0.2363f, 0.2414f, 0.2464f, 0.2512f, 90 0.2558f, 0.2604f, 0.2648f, 0.2690f, 0.2732f, 0.2773f, 91 0.2813f, 0.2852f, 0.2890f, 0.2927f, 0.2964f, 0.3000f, 92 0.3035f, 0.3070f, 0.3104f, 0.3138f, 0.3171f, 0.3204f, 93 0.3236f, 0.3268f, 0.3299f, 0.3330f, 0.3360f, 0.3390f, 94 0.3420f, 0.3449f, 0.3478f, 0.3507f, 0.3535f, 0.3563f, 95 0.3591f, 0.3619f, 0.3646f, 0.3673f, 0.3699f, 0.3726f, 96 0.3752f, 0.3777f, 0.3803f, 0.3828f, 0.3854f, 0.3878f, 97 0.3903f, 0.3928f, 0.3952f, 0.3976f, 0.4000f 98 }; 99 100 /* Matlab code to produce table: 101 overDriveCurve = [sqrt(linspace(0,1,65))' + 1]; 102 fprintf(1, '\t%.4f, %.4f, %.4f, %.4f, %.4f, %.4f,\n', overDriveCurve); 103 */ 104 const float WebRtcAec_overDriveCurve[65] = { 105 1.0000f, 1.1250f, 1.1768f, 1.2165f, 1.2500f, 1.2795f, 106 1.3062f, 1.3307f, 1.3536f, 1.3750f, 1.3953f, 1.4146f, 107 1.4330f, 1.4507f, 1.4677f, 1.4841f, 1.5000f, 1.5154f, 108 1.5303f, 1.5449f, 1.5590f, 1.5728f, 1.5863f, 1.5995f, 109 1.6124f, 1.6250f, 1.6374f, 1.6495f, 1.6614f, 1.6731f, 110 1.6847f, 1.6960f, 1.7071f, 1.7181f, 1.7289f, 1.7395f, 111 1.7500f, 1.7603f, 1.7706f, 1.7806f, 1.7906f, 1.8004f, 112 1.8101f, 1.8197f, 1.8292f, 1.8385f, 1.8478f, 1.8570f, 113 1.8660f, 1.8750f, 1.8839f, 1.8927f, 1.9014f, 1.9100f, 114 1.9186f, 1.9270f, 1.9354f, 1.9437f, 1.9520f, 1.9601f, 115 1.9682f, 1.9763f, 1.9843f, 1.9922f, 2.0000f 116 }; 117 118 // "Private" function prototypes. 119 static void ProcessBlock(aec_t *aec, const short *farend, 120 const short *nearend, const short *nearendH, 121 short *out, short *outH); 122 123 static void BufferFar(aec_t *aec, const short *farend, int farLen); 124 static void FetchFar(aec_t *aec, short *farend, int farLen, int knownDelay); 125 126 static void NonLinearProcessing(aec_t *aec, short *output, short *outputH); 127 128 static void GetHighbandGain(const float *lambda, float *nlpGainHband); 129 130 // Comfort_noise also computes noise for H band returned in comfortNoiseHband 131 static void ComfortNoise(aec_t *aec, float efw[2][PART_LEN1], 132 complex_t *comfortNoiseHband, 133 const float *noisePow, const float *lambda); 134 135 static void WebRtcAec_InitLevel(power_level_t *level); 136 static void WebRtcAec_InitStats(stats_t *stats); 137 static void UpdateLevel(power_level_t *level, const short *in); 138 static void UpdateMetrics(aec_t *aec); 139 140 __inline static float MulRe(float aRe, float aIm, float bRe, float bIm) 141 { 142 return aRe * bRe - aIm * bIm; 143 } 144 145 __inline static float MulIm(float aRe, float aIm, float bRe, float bIm) 146 { 147 return aRe * bIm + aIm * bRe; 148 } 149 150 static int CmpFloat(const void *a, const void *b) 151 { 152 const float *da = (const float *)a; 153 const float *db = (const float *)b; 154 155 return (*da > *db) - (*da < *db); 156 } 157 158 int WebRtcAec_CreateAec(aec_t **aecInst) 159 { 160 aec_t *aec = malloc(sizeof(aec_t)); 161 *aecInst = aec; 162 if (aec == NULL) { 163 return -1; 164 } 165 166 if (WebRtcApm_CreateBuffer(&aec->farFrBuf, FRAME_LEN + PART_LEN) == -1) { 167 WebRtcAec_FreeAec(aec); 168 aec = NULL; 169 return -1; 170 } 171 172 if (WebRtcApm_CreateBuffer(&aec->nearFrBuf, FRAME_LEN + PART_LEN) == -1) { 173 WebRtcAec_FreeAec(aec); 174 aec = NULL; 175 return -1; 176 } 177 178 if (WebRtcApm_CreateBuffer(&aec->outFrBuf, FRAME_LEN + PART_LEN) == -1) { 179 WebRtcAec_FreeAec(aec); 180 aec = NULL; 181 return -1; 182 } 183 184 if (WebRtcApm_CreateBuffer(&aec->nearFrBufH, FRAME_LEN + PART_LEN) == -1) { 185 WebRtcAec_FreeAec(aec); 186 aec = NULL; 187 return -1; 188 } 189 190 if (WebRtcApm_CreateBuffer(&aec->outFrBufH, FRAME_LEN + PART_LEN) == -1) { 191 WebRtcAec_FreeAec(aec); 192 aec = NULL; 193 return -1; 194 } 195 196 return 0; 197 } 198 199 int WebRtcAec_FreeAec(aec_t *aec) 200 { 201 if (aec == NULL) { 202 return -1; 203 } 204 205 WebRtcApm_FreeBuffer(aec->farFrBuf); 206 WebRtcApm_FreeBuffer(aec->nearFrBuf); 207 WebRtcApm_FreeBuffer(aec->outFrBuf); 208 209 WebRtcApm_FreeBuffer(aec->nearFrBufH); 210 WebRtcApm_FreeBuffer(aec->outFrBufH); 211 212 free(aec); 213 return 0; 214 } 215 216 static void FilterFar(aec_t *aec, float yf[2][PART_LEN1]) 217 { 218 int i; 219 for (i = 0; i < NR_PART; i++) { 220 int j; 221 int xPos = (i + aec->xfBufBlockPos) * PART_LEN1; 222 int pos = i * PART_LEN1; 223 // Check for wrap 224 if (i + aec->xfBufBlockPos >= NR_PART) { 225 xPos -= NR_PART*(PART_LEN1); 226 } 227 228 for (j = 0; j < PART_LEN1; j++) { 229 yf[0][j] += MulRe(aec->xfBuf[0][xPos + j], aec->xfBuf[1][xPos + j], 230 aec->wfBuf[0][ pos + j], aec->wfBuf[1][ pos + j]); 231 yf[1][j] += MulIm(aec->xfBuf[0][xPos + j], aec->xfBuf[1][xPos + j], 232 aec->wfBuf[0][ pos + j], aec->wfBuf[1][ pos + j]); 233 } 234 } 235 } 236 237 static void ScaleErrorSignal(aec_t *aec, float ef[2][PART_LEN1]) 238 { 239 int i; 240 float absEf; 241 for (i = 0; i < (PART_LEN1); i++) { 242 ef[0][i] /= (aec->xPow[i] + 1e-10f); 243 ef[1][i] /= (aec->xPow[i] + 1e-10f); 244 absEf = sqrtf(ef[0][i] * ef[0][i] + ef[1][i] * ef[1][i]); 245 246 if (absEf > aec->errThresh) { 247 absEf = aec->errThresh / (absEf + 1e-10f); 248 ef[0][i] *= absEf; 249 ef[1][i] *= absEf; 250 } 251 252 // Stepsize factor 253 ef[0][i] *= aec->mu; 254 ef[1][i] *= aec->mu; 255 } 256 } 257 258 static void FilterAdaptation(aec_t *aec, float *fft, float ef[2][PART_LEN1]) { 259 int i, j; 260 for (i = 0; i < NR_PART; i++) { 261 int xPos = (i + aec->xfBufBlockPos)*(PART_LEN1); 262 int pos; 263 // Check for wrap 264 if (i + aec->xfBufBlockPos >= NR_PART) { 265 xPos -= NR_PART * PART_LEN1; 266 } 267 268 pos = i * PART_LEN1; 269 270 #ifdef UNCONSTR 271 for (j = 0; j < PART_LEN1; j++) { 272 aec->wfBuf[pos + j][0] += MulRe(aec->xfBuf[xPos + j][0], 273 -aec->xfBuf[xPos + j][1], 274 ef[j][0], ef[j][1]); 275 aec->wfBuf[pos + j][1] += MulIm(aec->xfBuf[xPos + j][0], 276 -aec->xfBuf[xPos + j][1], 277 ef[j][0], ef[j][1]); 278 } 279 #else 280 for (j = 0; j < PART_LEN; j++) { 281 282 fft[2 * j] = MulRe(aec->xfBuf[0][xPos + j], 283 -aec->xfBuf[1][xPos + j], 284 ef[0][j], ef[1][j]); 285 fft[2 * j + 1] = MulIm(aec->xfBuf[0][xPos + j], 286 -aec->xfBuf[1][xPos + j], 287 ef[0][j], ef[1][j]); 288 } 289 fft[1] = MulRe(aec->xfBuf[0][xPos + PART_LEN], 290 -aec->xfBuf[1][xPos + PART_LEN], 291 ef[0][PART_LEN], ef[1][PART_LEN]); 292 293 aec_rdft_inverse_128(fft); 294 memset(fft + PART_LEN, 0, sizeof(float) * PART_LEN); 295 296 // fft scaling 297 { 298 float scale = 2.0f / PART_LEN2; 299 for (j = 0; j < PART_LEN; j++) { 300 fft[j] *= scale; 301 } 302 } 303 aec_rdft_forward_128(fft); 304 305 aec->wfBuf[0][pos] += fft[0]; 306 aec->wfBuf[0][pos + PART_LEN] += fft[1]; 307 308 for (j = 1; j < PART_LEN; j++) { 309 aec->wfBuf[0][pos + j] += fft[2 * j]; 310 aec->wfBuf[1][pos + j] += fft[2 * j + 1]; 311 } 312 #endif // UNCONSTR 313 } 314 } 315 316 static void OverdriveAndSuppress(aec_t *aec, float hNl[PART_LEN1], 317 const float hNlFb, 318 float efw[2][PART_LEN1]) { 319 int i; 320 for (i = 0; i < PART_LEN1; i++) { 321 // Weight subbands 322 if (hNl[i] > hNlFb) { 323 hNl[i] = WebRtcAec_weightCurve[i] * hNlFb + 324 (1 - WebRtcAec_weightCurve[i]) * hNl[i]; 325 } 326 hNl[i] = powf(hNl[i], aec->overDriveSm * WebRtcAec_overDriveCurve[i]); 327 328 // Suppress error signal 329 efw[0][i] *= hNl[i]; 330 efw[1][i] *= hNl[i]; 331 332 // Ooura fft returns incorrect sign on imaginary component. It matters here 333 // because we are making an additive change with comfort noise. 334 efw[1][i] *= -1; 335 } 336 } 337 338 WebRtcAec_FilterFar_t WebRtcAec_FilterFar; 339 WebRtcAec_ScaleErrorSignal_t WebRtcAec_ScaleErrorSignal; 340 WebRtcAec_FilterAdaptation_t WebRtcAec_FilterAdaptation; 341 WebRtcAec_OverdriveAndSuppress_t WebRtcAec_OverdriveAndSuppress; 342 343 int WebRtcAec_InitAec(aec_t *aec, int sampFreq) 344 { 345 int i; 346 347 aec->sampFreq = sampFreq; 348 349 if (sampFreq == 8000) { 350 aec->mu = 0.6f; 351 aec->errThresh = 2e-6f; 352 } 353 else { 354 aec->mu = 0.5f; 355 aec->errThresh = 1.5e-6f; 356 } 357 358 if (WebRtcApm_InitBuffer(aec->farFrBuf) == -1) { 359 return -1; 360 } 361 362 if (WebRtcApm_InitBuffer(aec->nearFrBuf) == -1) { 363 return -1; 364 } 365 366 if (WebRtcApm_InitBuffer(aec->outFrBuf) == -1) { 367 return -1; 368 } 369 370 if (WebRtcApm_InitBuffer(aec->nearFrBufH) == -1) { 371 return -1; 372 } 373 374 if (WebRtcApm_InitBuffer(aec->outFrBufH) == -1) { 375 return -1; 376 } 377 378 // Default target suppression level 379 aec->targetSupp = -11.5; 380 aec->minOverDrive = 2.0; 381 382 // Sampling frequency multiplier 383 // SWB is processed as 160 frame size 384 if (aec->sampFreq == 32000) { 385 aec->mult = (short)aec->sampFreq / 16000; 386 } 387 else { 388 aec->mult = (short)aec->sampFreq / 8000; 389 } 390 391 aec->farBufWritePos = 0; 392 aec->farBufReadPos = 0; 393 394 aec->inSamples = 0; 395 aec->outSamples = 0; 396 aec->knownDelay = 0; 397 398 // Initialize buffers 399 memset(aec->farBuf, 0, sizeof(aec->farBuf)); 400 memset(aec->xBuf, 0, sizeof(aec->xBuf)); 401 memset(aec->dBuf, 0, sizeof(aec->dBuf)); 402 memset(aec->eBuf, 0, sizeof(aec->eBuf)); 403 // For H band 404 memset(aec->dBufH, 0, sizeof(aec->dBufH)); 405 406 memset(aec->xPow, 0, sizeof(aec->xPow)); 407 memset(aec->dPow, 0, sizeof(aec->dPow)); 408 memset(aec->dInitMinPow, 0, sizeof(aec->dInitMinPow)); 409 aec->noisePow = aec->dInitMinPow; 410 aec->noiseEstCtr = 0; 411 412 // Initial comfort noise power 413 for (i = 0; i < PART_LEN1; i++) { 414 aec->dMinPow[i] = 1.0e6f; 415 } 416 417 // Holds the last block written to 418 aec->xfBufBlockPos = 0; 419 // TODO: Investigate need for these initializations. Deleting them doesn't 420 // change the output at all and yields 0.4% overall speedup. 421 memset(aec->xfBuf, 0, sizeof(complex_t) * NR_PART * PART_LEN1); 422 memset(aec->wfBuf, 0, sizeof(complex_t) * NR_PART * PART_LEN1); 423 memset(aec->sde, 0, sizeof(complex_t) * PART_LEN1); 424 memset(aec->sxd, 0, sizeof(complex_t) * PART_LEN1); 425 memset(aec->xfwBuf, 0, sizeof(complex_t) * NR_PART * PART_LEN1); 426 memset(aec->se, 0, sizeof(float) * PART_LEN1); 427 428 // To prevent numerical instability in the first block. 429 for (i = 0; i < PART_LEN1; i++) { 430 aec->sd[i] = 1; 431 } 432 for (i = 0; i < PART_LEN1; i++) { 433 aec->sx[i] = 1; 434 } 435 436 memset(aec->hNs, 0, sizeof(aec->hNs)); 437 memset(aec->outBuf, 0, sizeof(float) * PART_LEN); 438 439 aec->hNlFbMin = 1; 440 aec->hNlFbLocalMin = 1; 441 aec->hNlXdAvgMin = 1; 442 aec->hNlNewMin = 0; 443 aec->hNlMinCtr = 0; 444 aec->overDrive = 2; 445 aec->overDriveSm = 2; 446 aec->delayIdx = 0; 447 aec->stNearState = 0; 448 aec->echoState = 0; 449 aec->divergeState = 0; 450 451 aec->seed = 777; 452 aec->delayEstCtr = 0; 453 454 // Features on by default (G.167) 455 #ifdef G167 456 aec->adaptToggle = 1; 457 aec->nlpToggle = 1; 458 aec->cnToggle = 1; 459 #endif 460 461 // Metrics disabled by default 462 aec->metricsMode = 0; 463 WebRtcAec_InitMetrics(aec); 464 465 // Assembly optimization 466 WebRtcAec_FilterFar = FilterFar; 467 WebRtcAec_ScaleErrorSignal = ScaleErrorSignal; 468 WebRtcAec_FilterAdaptation = FilterAdaptation; 469 WebRtcAec_OverdriveAndSuppress = OverdriveAndSuppress; 470 if (WebRtc_GetCPUInfo(kSSE2)) { 471 #if defined(__SSE2__) 472 WebRtcAec_InitAec_SSE2(); 473 #endif 474 } 475 aec_rdft_init(); 476 477 return 0; 478 } 479 480 void WebRtcAec_InitMetrics(aec_t *aec) 481 { 482 aec->stateCounter = 0; 483 WebRtcAec_InitLevel(&aec->farlevel); 484 WebRtcAec_InitLevel(&aec->nearlevel); 485 WebRtcAec_InitLevel(&aec->linoutlevel); 486 WebRtcAec_InitLevel(&aec->nlpoutlevel); 487 488 WebRtcAec_InitStats(&aec->erl); 489 WebRtcAec_InitStats(&aec->erle); 490 WebRtcAec_InitStats(&aec->aNlp); 491 WebRtcAec_InitStats(&aec->rerl); 492 } 493 494 495 void WebRtcAec_ProcessFrame(aec_t *aec, const short *farend, 496 const short *nearend, const short *nearendH, 497 short *out, short *outH, 498 int knownDelay) 499 { 500 short farBl[PART_LEN], nearBl[PART_LEN], outBl[PART_LEN]; 501 short farFr[FRAME_LEN]; 502 // For H band 503 short nearBlH[PART_LEN], outBlH[PART_LEN]; 504 505 int size = 0; 506 507 // initialize: only used for SWB 508 memset(nearBlH, 0, sizeof(nearBlH)); 509 memset(outBlH, 0, sizeof(outBlH)); 510 511 // Buffer the current frame. 512 // Fetch an older one corresponding to the delay. 513 BufferFar(aec, farend, FRAME_LEN); 514 FetchFar(aec, farFr, FRAME_LEN, knownDelay); 515 516 // Buffer the synchronized far and near frames, 517 // to pass the smaller blocks individually. 518 WebRtcApm_WriteBuffer(aec->farFrBuf, farFr, FRAME_LEN); 519 WebRtcApm_WriteBuffer(aec->nearFrBuf, nearend, FRAME_LEN); 520 // For H band 521 if (aec->sampFreq == 32000) { 522 WebRtcApm_WriteBuffer(aec->nearFrBufH, nearendH, FRAME_LEN); 523 } 524 525 // Process as many blocks as possible. 526 while (WebRtcApm_get_buffer_size(aec->farFrBuf) >= PART_LEN) { 527 528 WebRtcApm_ReadBuffer(aec->farFrBuf, farBl, PART_LEN); 529 WebRtcApm_ReadBuffer(aec->nearFrBuf, nearBl, PART_LEN); 530 531 // For H band 532 if (aec->sampFreq == 32000) { 533 WebRtcApm_ReadBuffer(aec->nearFrBufH, nearBlH, PART_LEN); 534 } 535 536 ProcessBlock(aec, farBl, nearBl, nearBlH, outBl, outBlH); 537 538 WebRtcApm_WriteBuffer(aec->outFrBuf, outBl, PART_LEN); 539 // For H band 540 if (aec->sampFreq == 32000) { 541 WebRtcApm_WriteBuffer(aec->outFrBufH, outBlH, PART_LEN); 542 } 543 } 544 545 // Stuff the out buffer if we have less than a frame to output. 546 // This should only happen for the first frame. 547 size = WebRtcApm_get_buffer_size(aec->outFrBuf); 548 if (size < FRAME_LEN) { 549 WebRtcApm_StuffBuffer(aec->outFrBuf, FRAME_LEN - size); 550 if (aec->sampFreq == 32000) { 551 WebRtcApm_StuffBuffer(aec->outFrBufH, FRAME_LEN - size); 552 } 553 } 554 555 // Obtain an output frame. 556 WebRtcApm_ReadBuffer(aec->outFrBuf, out, FRAME_LEN); 557 // For H band 558 if (aec->sampFreq == 32000) { 559 WebRtcApm_ReadBuffer(aec->outFrBufH, outH, FRAME_LEN); 560 } 561 } 562 563 static void ProcessBlock(aec_t *aec, const short *farend, 564 const short *nearend, const short *nearendH, 565 short *output, short *outputH) 566 { 567 int i; 568 float d[PART_LEN], y[PART_LEN], e[PART_LEN], dH[PART_LEN]; 569 short eInt16[PART_LEN]; 570 float scale; 571 572 float fft[PART_LEN2]; 573 float xf[2][PART_LEN1], yf[2][PART_LEN1], ef[2][PART_LEN1]; 574 complex_t df[PART_LEN1]; 575 576 const float gPow[2] = {0.9f, 0.1f}; 577 578 // Noise estimate constants. 579 const int noiseInitBlocks = 500 * aec->mult; 580 const float step = 0.1f; 581 const float ramp = 1.0002f; 582 const float gInitNoise[2] = {0.999f, 0.001f}; 583 584 #ifdef AEC_DEBUG 585 fwrite(farend, sizeof(short), PART_LEN, aec->farFile); 586 fwrite(nearend, sizeof(short), PART_LEN, aec->nearFile); 587 #endif 588 589 memset(dH, 0, sizeof(dH)); 590 591 // ---------- Ooura fft ---------- 592 // Concatenate old and new farend blocks. 593 for (i = 0; i < PART_LEN; i++) { 594 aec->xBuf[i + PART_LEN] = (float)farend[i]; 595 d[i] = (float)nearend[i]; 596 } 597 598 if (aec->sampFreq == 32000) { 599 for (i = 0; i < PART_LEN; i++) { 600 dH[i] = (float)nearendH[i]; 601 } 602 } 603 604 605 memcpy(fft, aec->xBuf, sizeof(float) * PART_LEN2); 606 memcpy(aec->dBuf + PART_LEN, d, sizeof(float) * PART_LEN); 607 // For H band 608 if (aec->sampFreq == 32000) { 609 memcpy(aec->dBufH + PART_LEN, dH, sizeof(float) * PART_LEN); 610 } 611 612 aec_rdft_forward_128(fft); 613 614 // Far fft 615 xf[1][0] = 0; 616 xf[1][PART_LEN] = 0; 617 xf[0][0] = fft[0]; 618 xf[0][PART_LEN] = fft[1]; 619 620 for (i = 1; i < PART_LEN; i++) { 621 xf[0][i] = fft[2 * i]; 622 xf[1][i] = fft[2 * i + 1]; 623 } 624 625 // Near fft 626 memcpy(fft, aec->dBuf, sizeof(float) * PART_LEN2); 627 aec_rdft_forward_128(fft); 628 df[0][1] = 0; 629 df[PART_LEN][1] = 0; 630 df[0][0] = fft[0]; 631 df[PART_LEN][0] = fft[1]; 632 633 for (i = 1; i < PART_LEN; i++) { 634 df[i][0] = fft[2 * i]; 635 df[i][1] = fft[2 * i + 1]; 636 } 637 638 // Power smoothing 639 for (i = 0; i < PART_LEN1; i++) { 640 aec->xPow[i] = gPow[0] * aec->xPow[i] + gPow[1] * NR_PART * 641 (xf[0][i] * xf[0][i] + xf[1][i] * xf[1][i]); 642 aec->dPow[i] = gPow[0] * aec->dPow[i] + gPow[1] * 643 (df[i][0] * df[i][0] + df[i][1] * df[i][1]); 644 } 645 646 // Estimate noise power. Wait until dPow is more stable. 647 if (aec->noiseEstCtr > 50) { 648 for (i = 0; i < PART_LEN1; i++) { 649 if (aec->dPow[i] < aec->dMinPow[i]) { 650 aec->dMinPow[i] = (aec->dPow[i] + step * (aec->dMinPow[i] - 651 aec->dPow[i])) * ramp; 652 } 653 else { 654 aec->dMinPow[i] *= ramp; 655 } 656 } 657 } 658 659 // Smooth increasing noise power from zero at the start, 660 // to avoid a sudden burst of comfort noise. 661 if (aec->noiseEstCtr < noiseInitBlocks) { 662 aec->noiseEstCtr++; 663 for (i = 0; i < PART_LEN1; i++) { 664 if (aec->dMinPow[i] > aec->dInitMinPow[i]) { 665 aec->dInitMinPow[i] = gInitNoise[0] * aec->dInitMinPow[i] + 666 gInitNoise[1] * aec->dMinPow[i]; 667 } 668 else { 669 aec->dInitMinPow[i] = aec->dMinPow[i]; 670 } 671 } 672 aec->noisePow = aec->dInitMinPow; 673 } 674 else { 675 aec->noisePow = aec->dMinPow; 676 } 677 678 679 // Update the xfBuf block position. 680 aec->xfBufBlockPos--; 681 if (aec->xfBufBlockPos == -1) { 682 aec->xfBufBlockPos = NR_PART - 1; 683 } 684 685 // Buffer xf 686 memcpy(aec->xfBuf[0] + aec->xfBufBlockPos * PART_LEN1, xf[0], 687 sizeof(float) * PART_LEN1); 688 memcpy(aec->xfBuf[1] + aec->xfBufBlockPos * PART_LEN1, xf[1], 689 sizeof(float) * PART_LEN1); 690 691 memset(yf[0], 0, sizeof(float) * (PART_LEN1 * 2)); 692 693 // Filter far 694 WebRtcAec_FilterFar(aec, yf); 695 696 // Inverse fft to obtain echo estimate and error. 697 fft[0] = yf[0][0]; 698 fft[1] = yf[0][PART_LEN]; 699 for (i = 1; i < PART_LEN; i++) { 700 fft[2 * i] = yf[0][i]; 701 fft[2 * i + 1] = yf[1][i]; 702 } 703 aec_rdft_inverse_128(fft); 704 705 scale = 2.0f / PART_LEN2; 706 for (i = 0; i < PART_LEN; i++) { 707 y[i] = fft[PART_LEN + i] * scale; // fft scaling 708 } 709 710 for (i = 0; i < PART_LEN; i++) { 711 e[i] = d[i] - y[i]; 712 } 713 714 // Error fft 715 memcpy(aec->eBuf + PART_LEN, e, sizeof(float) * PART_LEN); 716 memset(fft, 0, sizeof(float) * PART_LEN); 717 memcpy(fft + PART_LEN, e, sizeof(float) * PART_LEN); 718 aec_rdft_forward_128(fft); 719 720 ef[1][0] = 0; 721 ef[1][PART_LEN] = 0; 722 ef[0][0] = fft[0]; 723 ef[0][PART_LEN] = fft[1]; 724 for (i = 1; i < PART_LEN; i++) { 725 ef[0][i] = fft[2 * i]; 726 ef[1][i] = fft[2 * i + 1]; 727 } 728 729 // Scale error signal inversely with far power. 730 WebRtcAec_ScaleErrorSignal(aec, ef); 731 #ifdef G167 732 if (aec->adaptToggle) { 733 #endif 734 // Filter adaptation 735 WebRtcAec_FilterAdaptation(aec, fft, ef); 736 #ifdef G167 737 } 738 #endif 739 740 NonLinearProcessing(aec, output, outputH); 741 742 #if defined(AEC_DEBUG) || defined(G167) 743 for (i = 0; i < PART_LEN; i++) { 744 eInt16[i] = (short)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, e[i], 745 WEBRTC_SPL_WORD16_MIN); 746 } 747 #endif 748 #ifdef G167 749 if (aec->nlpToggle == 0) { 750 memcpy(output, eInt16, sizeof(eInt16)); 751 } 752 #endif 753 754 if (aec->metricsMode == 1) { 755 for (i = 0; i < PART_LEN; i++) { 756 eInt16[i] = (short)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, e[i], 757 WEBRTC_SPL_WORD16_MIN); 758 } 759 760 // Update power levels and echo metrics 761 UpdateLevel(&aec->farlevel, farend); 762 UpdateLevel(&aec->nearlevel, nearend); 763 UpdateLevel(&aec->linoutlevel, eInt16); 764 UpdateLevel(&aec->nlpoutlevel, output); 765 UpdateMetrics(aec); 766 } 767 768 #ifdef AEC_DEBUG 769 fwrite(eInt16, sizeof(short), PART_LEN, aec->outLpFile); 770 fwrite(output, sizeof(short), PART_LEN, aec->outFile); 771 #endif 772 } 773 774 static void NonLinearProcessing(aec_t *aec, short *output, short *outputH) 775 { 776 float efw[2][PART_LEN1], dfw[2][PART_LEN1]; 777 complex_t xfw[PART_LEN1]; 778 complex_t comfortNoiseHband[PART_LEN1]; 779 float fft[PART_LEN2]; 780 float scale, dtmp; 781 float nlpGainHband; 782 int i, j, pos; 783 784 // Coherence and non-linear filter 785 float cohde[PART_LEN1], cohxd[PART_LEN1]; 786 float hNlDeAvg, hNlXdAvg; 787 float hNl[PART_LEN1]; 788 float hNlPref[PREF_BAND_SIZE]; 789 float hNlFb = 0, hNlFbLow = 0; 790 const float prefBandQuant = 0.75f, prefBandQuantLow = 0.5f; 791 const int prefBandSize = PREF_BAND_SIZE / aec->mult; 792 const int minPrefBand = 4 / aec->mult; 793 794 // Near and error power sums 795 float sdSum = 0, seSum = 0; 796 797 // Power estimate smoothing coefficients 798 const float gCoh[2][2] = {{0.9f, 0.1f}, {0.93f, 0.07f}}; 799 const float *ptrGCoh = gCoh[aec->mult - 1]; 800 801 // Filter energey 802 float wfEnMax = 0, wfEn = 0; 803 const int delayEstInterval = 10 * aec->mult; 804 805 aec->delayEstCtr++; 806 if (aec->delayEstCtr == delayEstInterval) { 807 aec->delayEstCtr = 0; 808 } 809 810 // initialize comfort noise for H band 811 memset(comfortNoiseHband, 0, sizeof(comfortNoiseHband)); 812 nlpGainHband = (float)0.0; 813 dtmp = (float)0.0; 814 815 // Measure energy in each filter partition to determine delay. 816 // TODO: Spread by computing one partition per block? 817 if (aec->delayEstCtr == 0) { 818 wfEnMax = 0; 819 aec->delayIdx = 0; 820 for (i = 0; i < NR_PART; i++) { 821 pos = i * PART_LEN1; 822 wfEn = 0; 823 for (j = 0; j < PART_LEN1; j++) { 824 wfEn += aec->wfBuf[0][pos + j] * aec->wfBuf[0][pos + j] + 825 aec->wfBuf[1][pos + j] * aec->wfBuf[1][pos + j]; 826 } 827 828 if (wfEn > wfEnMax) { 829 wfEnMax = wfEn; 830 aec->delayIdx = i; 831 } 832 } 833 } 834 835 // NLP 836 // Windowed far fft 837 for (i = 0; i < PART_LEN; i++) { 838 fft[i] = aec->xBuf[i] * sqrtHanning[i]; 839 fft[PART_LEN + i] = aec->xBuf[PART_LEN + i] * sqrtHanning[PART_LEN - i]; 840 } 841 aec_rdft_forward_128(fft); 842 843 xfw[0][1] = 0; 844 xfw[PART_LEN][1] = 0; 845 xfw[0][0] = fft[0]; 846 xfw[PART_LEN][0] = fft[1]; 847 for (i = 1; i < PART_LEN; i++) { 848 xfw[i][0] = fft[2 * i]; 849 xfw[i][1] = fft[2 * i + 1]; 850 } 851 852 // Buffer far. 853 memcpy(aec->xfwBuf, xfw, sizeof(xfw)); 854 855 // Use delayed far. 856 memcpy(xfw, aec->xfwBuf + aec->delayIdx * PART_LEN1, sizeof(xfw)); 857 858 // Windowed near fft 859 for (i = 0; i < PART_LEN; i++) { 860 fft[i] = aec->dBuf[i] * sqrtHanning[i]; 861 fft[PART_LEN + i] = aec->dBuf[PART_LEN + i] * sqrtHanning[PART_LEN - i]; 862 } 863 aec_rdft_forward_128(fft); 864 865 dfw[1][0] = 0; 866 dfw[1][PART_LEN] = 0; 867 dfw[0][0] = fft[0]; 868 dfw[0][PART_LEN] = fft[1]; 869 for (i = 1; i < PART_LEN; i++) { 870 dfw[0][i] = fft[2 * i]; 871 dfw[1][i] = fft[2 * i + 1]; 872 } 873 874 // Windowed error fft 875 for (i = 0; i < PART_LEN; i++) { 876 fft[i] = aec->eBuf[i] * sqrtHanning[i]; 877 fft[PART_LEN + i] = aec->eBuf[PART_LEN + i] * sqrtHanning[PART_LEN - i]; 878 } 879 aec_rdft_forward_128(fft); 880 efw[1][0] = 0; 881 efw[1][PART_LEN] = 0; 882 efw[0][0] = fft[0]; 883 efw[0][PART_LEN] = fft[1]; 884 for (i = 1; i < PART_LEN; i++) { 885 efw[0][i] = fft[2 * i]; 886 efw[1][i] = fft[2 * i + 1]; 887 } 888 889 // Smoothed PSD 890 for (i = 0; i < PART_LEN1; i++) { 891 aec->sd[i] = ptrGCoh[0] * aec->sd[i] + ptrGCoh[1] * 892 (dfw[0][i] * dfw[0][i] + dfw[1][i] * dfw[1][i]); 893 aec->se[i] = ptrGCoh[0] * aec->se[i] + ptrGCoh[1] * 894 (efw[0][i] * efw[0][i] + efw[1][i] * efw[1][i]); 895 // We threshold here to protect against the ill-effects of a zero farend. 896 // The threshold is not arbitrarily chosen, but balances protection and 897 // adverse interaction with the algorithm's tuning. 898 // TODO: investigate further why this is so sensitive. 899 aec->sx[i] = ptrGCoh[0] * aec->sx[i] + ptrGCoh[1] * 900 WEBRTC_SPL_MAX(xfw[i][0] * xfw[i][0] + xfw[i][1] * xfw[i][1], 15); 901 902 aec->sde[i][0] = ptrGCoh[0] * aec->sde[i][0] + ptrGCoh[1] * 903 (dfw[0][i] * efw[0][i] + dfw[1][i] * efw[1][i]); 904 aec->sde[i][1] = ptrGCoh[0] * aec->sde[i][1] + ptrGCoh[1] * 905 (dfw[0][i] * efw[1][i] - dfw[1][i] * efw[0][i]); 906 907 aec->sxd[i][0] = ptrGCoh[0] * aec->sxd[i][0] + ptrGCoh[1] * 908 (dfw[0][i] * xfw[i][0] + dfw[1][i] * xfw[i][1]); 909 aec->sxd[i][1] = ptrGCoh[0] * aec->sxd[i][1] + ptrGCoh[1] * 910 (dfw[0][i] * xfw[i][1] - dfw[1][i] * xfw[i][0]); 911 912 sdSum += aec->sd[i]; 913 seSum += aec->se[i]; 914 } 915 916 // Divergent filter safeguard. 917 if (aec->divergeState == 0) { 918 if (seSum > sdSum) { 919 aec->divergeState = 1; 920 } 921 } 922 else { 923 if (seSum * 1.05f < sdSum) { 924 aec->divergeState = 0; 925 } 926 } 927 928 if (aec->divergeState == 1) { 929 memcpy(efw, dfw, sizeof(efw)); 930 } 931 932 // Reset if error is significantly larger than nearend (13 dB). 933 if (seSum > (19.95f * sdSum)) { 934 memset(aec->wfBuf, 0, sizeof(aec->wfBuf)); 935 } 936 937 // Subband coherence 938 for (i = 0; i < PART_LEN1; i++) { 939 cohde[i] = (aec->sde[i][0] * aec->sde[i][0] + aec->sde[i][1] * aec->sde[i][1]) / 940 (aec->sd[i] * aec->se[i] + 1e-10f); 941 cohxd[i] = (aec->sxd[i][0] * aec->sxd[i][0] + aec->sxd[i][1] * aec->sxd[i][1]) / 942 (aec->sx[i] * aec->sd[i] + 1e-10f); 943 } 944 945 hNlXdAvg = 0; 946 for (i = minPrefBand; i < prefBandSize + minPrefBand; i++) { 947 hNlXdAvg += cohxd[i]; 948 } 949 hNlXdAvg /= prefBandSize; 950 hNlXdAvg = 1 - hNlXdAvg; 951 952 hNlDeAvg = 0; 953 for (i = minPrefBand; i < prefBandSize + minPrefBand; i++) { 954 hNlDeAvg += cohde[i]; 955 } 956 hNlDeAvg /= prefBandSize; 957 958 if (hNlXdAvg < 0.75f && hNlXdAvg < aec->hNlXdAvgMin) { 959 aec->hNlXdAvgMin = hNlXdAvg; 960 } 961 962 if (hNlDeAvg > 0.98f && hNlXdAvg > 0.9f) { 963 aec->stNearState = 1; 964 } 965 else if (hNlDeAvg < 0.95f || hNlXdAvg < 0.8f) { 966 aec->stNearState = 0; 967 } 968 969 if (aec->hNlXdAvgMin == 1) { 970 aec->echoState = 0; 971 aec->overDrive = aec->minOverDrive; 972 973 if (aec->stNearState == 1) { 974 memcpy(hNl, cohde, sizeof(hNl)); 975 hNlFb = hNlDeAvg; 976 hNlFbLow = hNlDeAvg; 977 } 978 else { 979 for (i = 0; i < PART_LEN1; i++) { 980 hNl[i] = 1 - cohxd[i]; 981 } 982 hNlFb = hNlXdAvg; 983 hNlFbLow = hNlXdAvg; 984 } 985 } 986 else { 987 988 if (aec->stNearState == 1) { 989 aec->echoState = 0; 990 memcpy(hNl, cohde, sizeof(hNl)); 991 hNlFb = hNlDeAvg; 992 hNlFbLow = hNlDeAvg; 993 } 994 else { 995 aec->echoState = 1; 996 for (i = 0; i < PART_LEN1; i++) { 997 hNl[i] = WEBRTC_SPL_MIN(cohde[i], 1 - cohxd[i]); 998 } 999 1000 // Select an order statistic from the preferred bands. 1001 // TODO: Using quicksort now, but a selection algorithm may be preferred. 1002 memcpy(hNlPref, &hNl[minPrefBand], sizeof(float) * prefBandSize); 1003 qsort(hNlPref, prefBandSize, sizeof(float), CmpFloat); 1004 hNlFb = hNlPref[(int)floor(prefBandQuant * (prefBandSize - 1))]; 1005 hNlFbLow = hNlPref[(int)floor(prefBandQuantLow * (prefBandSize - 1))]; 1006 } 1007 } 1008 1009 // Track the local filter minimum to determine suppression overdrive. 1010 if (hNlFbLow < 0.6f && hNlFbLow < aec->hNlFbLocalMin) { 1011 aec->hNlFbLocalMin = hNlFbLow; 1012 aec->hNlFbMin = hNlFbLow; 1013 aec->hNlNewMin = 1; 1014 aec->hNlMinCtr = 0; 1015 } 1016 aec->hNlFbLocalMin = WEBRTC_SPL_MIN(aec->hNlFbLocalMin + 0.0008f / aec->mult, 1); 1017 aec->hNlXdAvgMin = WEBRTC_SPL_MIN(aec->hNlXdAvgMin + 0.0006f / aec->mult, 1); 1018 1019 if (aec->hNlNewMin == 1) { 1020 aec->hNlMinCtr++; 1021 } 1022 if (aec->hNlMinCtr == 2) { 1023 aec->hNlNewMin = 0; 1024 aec->hNlMinCtr = 0; 1025 aec->overDrive = WEBRTC_SPL_MAX(aec->targetSupp / 1026 ((float)log(aec->hNlFbMin + 1e-10f) + 1e-10f), aec->minOverDrive); 1027 } 1028 1029 // Smooth the overdrive. 1030 if (aec->overDrive < aec->overDriveSm) { 1031 aec->overDriveSm = 0.99f * aec->overDriveSm + 0.01f * aec->overDrive; 1032 } 1033 else { 1034 aec->overDriveSm = 0.9f * aec->overDriveSm + 0.1f * aec->overDrive; 1035 } 1036 1037 WebRtcAec_OverdriveAndSuppress(aec, hNl, hNlFb, efw); 1038 1039 #ifdef G167 1040 if (aec->cnToggle) { 1041 ComfortNoise(aec, efw, comfortNoiseHband, aec->noisePow, hNl); 1042 } 1043 #else 1044 // Add comfort noise. 1045 ComfortNoise(aec, efw, comfortNoiseHband, aec->noisePow, hNl); 1046 #endif 1047 1048 // Inverse error fft. 1049 fft[0] = efw[0][0]; 1050 fft[1] = efw[0][PART_LEN]; 1051 for (i = 1; i < PART_LEN; i++) { 1052 fft[2*i] = efw[0][i]; 1053 // Sign change required by Ooura fft. 1054 fft[2*i + 1] = -efw[1][i]; 1055 } 1056 aec_rdft_inverse_128(fft); 1057 1058 // Overlap and add to obtain output. 1059 scale = 2.0f / PART_LEN2; 1060 for (i = 0; i < PART_LEN; i++) { 1061 fft[i] *= scale; // fft scaling 1062 fft[i] = fft[i]*sqrtHanning[i] + aec->outBuf[i]; 1063 1064 // Saturation protection 1065 output[i] = (short)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, fft[i], 1066 WEBRTC_SPL_WORD16_MIN); 1067 1068 fft[PART_LEN + i] *= scale; // fft scaling 1069 aec->outBuf[i] = fft[PART_LEN + i] * sqrtHanning[PART_LEN - i]; 1070 } 1071 1072 // For H band 1073 if (aec->sampFreq == 32000) { 1074 1075 // H band gain 1076 // average nlp over low band: average over second half of freq spectrum 1077 // (4->8khz) 1078 GetHighbandGain(hNl, &nlpGainHband); 1079 1080 // Inverse comfort_noise 1081 if (flagHbandCn == 1) { 1082 fft[0] = comfortNoiseHband[0][0]; 1083 fft[1] = comfortNoiseHband[PART_LEN][0]; 1084 for (i = 1; i < PART_LEN; i++) { 1085 fft[2*i] = comfortNoiseHband[i][0]; 1086 fft[2*i + 1] = comfortNoiseHband[i][1]; 1087 } 1088 aec_rdft_inverse_128(fft); 1089 scale = 2.0f / PART_LEN2; 1090 } 1091 1092 // compute gain factor 1093 for (i = 0; i < PART_LEN; i++) { 1094 dtmp = (float)aec->dBufH[i]; 1095 dtmp = (float)dtmp * nlpGainHband; // for variable gain 1096 1097 // add some comfort noise where Hband is attenuated 1098 if (flagHbandCn == 1) { 1099 fft[i] *= scale; // fft scaling 1100 dtmp += cnScaleHband * fft[i]; 1101 } 1102 1103 // Saturation protection 1104 outputH[i] = (short)WEBRTC_SPL_SAT(WEBRTC_SPL_WORD16_MAX, dtmp, 1105 WEBRTC_SPL_WORD16_MIN); 1106 } 1107 } 1108 1109 // Copy the current block to the old position. 1110 memcpy(aec->xBuf, aec->xBuf + PART_LEN, sizeof(float) * PART_LEN); 1111 memcpy(aec->dBuf, aec->dBuf + PART_LEN, sizeof(float) * PART_LEN); 1112 memcpy(aec->eBuf, aec->eBuf + PART_LEN, sizeof(float) * PART_LEN); 1113 1114 // Copy the current block to the old position for H band 1115 if (aec->sampFreq == 32000) { 1116 memcpy(aec->dBufH, aec->dBufH + PART_LEN, sizeof(float) * PART_LEN); 1117 } 1118 1119 memmove(aec->xfwBuf + PART_LEN1, aec->xfwBuf, sizeof(aec->xfwBuf) - 1120 sizeof(complex_t) * PART_LEN1); 1121 } 1122 1123 static void GetHighbandGain(const float *lambda, float *nlpGainHband) 1124 { 1125 int i; 1126 1127 nlpGainHband[0] = (float)0.0; 1128 for (i = freqAvgIc; i < PART_LEN1 - 1; i++) { 1129 nlpGainHband[0] += lambda[i]; 1130 } 1131 nlpGainHband[0] /= (float)(PART_LEN1 - 1 - freqAvgIc); 1132 } 1133 1134 static void ComfortNoise(aec_t *aec, float efw[2][PART_LEN1], 1135 complex_t *comfortNoiseHband, const float *noisePow, const float *lambda) 1136 { 1137 int i, num; 1138 float rand[PART_LEN]; 1139 float noise, noiseAvg, tmp, tmpAvg; 1140 WebRtc_Word16 randW16[PART_LEN]; 1141 complex_t u[PART_LEN1]; 1142 1143 const float pi2 = 6.28318530717959f; 1144 1145 // Generate a uniform random array on [0 1] 1146 WebRtcSpl_RandUArray(randW16, PART_LEN, &aec->seed); 1147 for (i = 0; i < PART_LEN; i++) { 1148 rand[i] = ((float)randW16[i]) / 32768; 1149 } 1150 1151 // Reject LF noise 1152 u[0][0] = 0; 1153 u[0][1] = 0; 1154 for (i = 1; i < PART_LEN1; i++) { 1155 tmp = pi2 * rand[i - 1]; 1156 1157 noise = sqrtf(noisePow[i]); 1158 u[i][0] = noise * (float)cos(tmp); 1159 u[i][1] = -noise * (float)sin(tmp); 1160 } 1161 u[PART_LEN][1] = 0; 1162 1163 for (i = 0; i < PART_LEN1; i++) { 1164 // This is the proper weighting to match the background noise power 1165 tmp = sqrtf(WEBRTC_SPL_MAX(1 - lambda[i] * lambda[i], 0)); 1166 //tmp = 1 - lambda[i]; 1167 efw[0][i] += tmp * u[i][0]; 1168 efw[1][i] += tmp * u[i][1]; 1169 } 1170 1171 // For H band comfort noise 1172 // TODO: don't compute noise and "tmp" twice. Use the previous results. 1173 noiseAvg = 0.0; 1174 tmpAvg = 0.0; 1175 num = 0; 1176 if (aec->sampFreq == 32000 && flagHbandCn == 1) { 1177 1178 // average noise scale 1179 // average over second half of freq spectrum (i.e., 4->8khz) 1180 // TODO: we shouldn't need num. We know how many elements we're summing. 1181 for (i = PART_LEN1 >> 1; i < PART_LEN1; i++) { 1182 num++; 1183 noiseAvg += sqrtf(noisePow[i]); 1184 } 1185 noiseAvg /= (float)num; 1186 1187 // average nlp scale 1188 // average over second half of freq spectrum (i.e., 4->8khz) 1189 // TODO: we shouldn't need num. We know how many elements we're summing. 1190 num = 0; 1191 for (i = PART_LEN1 >> 1; i < PART_LEN1; i++) { 1192 num++; 1193 tmpAvg += sqrtf(WEBRTC_SPL_MAX(1 - lambda[i] * lambda[i], 0)); 1194 } 1195 tmpAvg /= (float)num; 1196 1197 // Use average noise for H band 1198 // TODO: we should probably have a new random vector here. 1199 // Reject LF noise 1200 u[0][0] = 0; 1201 u[0][1] = 0; 1202 for (i = 1; i < PART_LEN1; i++) { 1203 tmp = pi2 * rand[i - 1]; 1204 1205 // Use average noise for H band 1206 u[i][0] = noiseAvg * (float)cos(tmp); 1207 u[i][1] = -noiseAvg * (float)sin(tmp); 1208 } 1209 u[PART_LEN][1] = 0; 1210 1211 for (i = 0; i < PART_LEN1; i++) { 1212 // Use average NLP weight for H band 1213 comfortNoiseHband[i][0] = tmpAvg * u[i][0]; 1214 comfortNoiseHband[i][1] = tmpAvg * u[i][1]; 1215 } 1216 } 1217 } 1218 1219 // Buffer the farend to account for knownDelay 1220 static void BufferFar(aec_t *aec, const short *farend, int farLen) 1221 { 1222 int writeLen = farLen, writePos = 0; 1223 1224 // Check if the write position must be wrapped. 1225 while (aec->farBufWritePos + writeLen > FAR_BUF_LEN) { 1226 1227 // Write to remaining buffer space before wrapping. 1228 writeLen = FAR_BUF_LEN - aec->farBufWritePos; 1229 memcpy(aec->farBuf + aec->farBufWritePos, farend + writePos, 1230 sizeof(short) * writeLen); 1231 aec->farBufWritePos = 0; 1232 writePos = writeLen; 1233 writeLen = farLen - writeLen; 1234 } 1235 1236 memcpy(aec->farBuf + aec->farBufWritePos, farend + writePos, 1237 sizeof(short) * writeLen); 1238 aec->farBufWritePos += writeLen; 1239 } 1240 1241 static void FetchFar(aec_t *aec, short *farend, int farLen, int knownDelay) 1242 { 1243 int readLen = farLen, readPos = 0, delayChange = knownDelay - aec->knownDelay; 1244 1245 aec->farBufReadPos -= delayChange; 1246 1247 // Check if delay forces a read position wrap. 1248 while(aec->farBufReadPos < 0) { 1249 aec->farBufReadPos += FAR_BUF_LEN; 1250 } 1251 while(aec->farBufReadPos > FAR_BUF_LEN - 1) { 1252 aec->farBufReadPos -= FAR_BUF_LEN; 1253 } 1254 1255 aec->knownDelay = knownDelay; 1256 1257 // Check if read position must be wrapped. 1258 while (aec->farBufReadPos + readLen > FAR_BUF_LEN) { 1259 1260 // Read from remaining buffer space before wrapping. 1261 readLen = FAR_BUF_LEN - aec->farBufReadPos; 1262 memcpy(farend + readPos, aec->farBuf + aec->farBufReadPos, 1263 sizeof(short) * readLen); 1264 aec->farBufReadPos = 0; 1265 readPos = readLen; 1266 readLen = farLen - readLen; 1267 } 1268 memcpy(farend + readPos, aec->farBuf + aec->farBufReadPos, 1269 sizeof(short) * readLen); 1270 aec->farBufReadPos += readLen; 1271 } 1272 1273 static void WebRtcAec_InitLevel(power_level_t *level) 1274 { 1275 const float bigFloat = 1E17f; 1276 1277 level->averagelevel = 0; 1278 level->framelevel = 0; 1279 level->minlevel = bigFloat; 1280 level->frsum = 0; 1281 level->sfrsum = 0; 1282 level->frcounter = 0; 1283 level->sfrcounter = 0; 1284 } 1285 1286 static void WebRtcAec_InitStats(stats_t *stats) 1287 { 1288 stats->instant = offsetLevel; 1289 stats->average = offsetLevel; 1290 stats->max = offsetLevel; 1291 stats->min = offsetLevel * (-1); 1292 stats->sum = 0; 1293 stats->hisum = 0; 1294 stats->himean = offsetLevel; 1295 stats->counter = 0; 1296 stats->hicounter = 0; 1297 } 1298 1299 static void UpdateLevel(power_level_t *level, const short *in) 1300 { 1301 int k; 1302 1303 for (k = 0; k < PART_LEN; k++) { 1304 level->sfrsum += in[k] * in[k]; 1305 } 1306 level->sfrcounter++; 1307 1308 if (level->sfrcounter > subCountLen) { 1309 level->framelevel = level->sfrsum / (subCountLen * PART_LEN); 1310 level->sfrsum = 0; 1311 level->sfrcounter = 0; 1312 1313 if (level->framelevel > 0) { 1314 if (level->framelevel < level->minlevel) { 1315 level->minlevel = level->framelevel; // New minimum 1316 } else { 1317 level->minlevel *= (1 + 0.001f); // Small increase 1318 } 1319 } 1320 level->frcounter++; 1321 level->frsum += level->framelevel; 1322 1323 if (level->frcounter > countLen) { 1324 level->averagelevel = level->frsum / countLen; 1325 level->frsum = 0; 1326 level->frcounter = 0; 1327 } 1328 1329 } 1330 } 1331 1332 static void UpdateMetrics(aec_t *aec) 1333 { 1334 float dtmp, dtmp2, dtmp3; 1335 1336 const float actThresholdNoisy = 8.0f; 1337 const float actThresholdClean = 40.0f; 1338 const float safety = 0.99995f; 1339 const float noisyPower = 300000.0f; 1340 1341 float actThreshold; 1342 float echo, suppressedEcho; 1343 1344 if (aec->echoState) { // Check if echo is likely present 1345 aec->stateCounter++; 1346 } 1347 1348 if (aec->farlevel.frcounter == countLen) { 1349 1350 if (aec->farlevel.minlevel < noisyPower) { 1351 actThreshold = actThresholdClean; 1352 } 1353 else { 1354 actThreshold = actThresholdNoisy; 1355 } 1356 1357 if ((aec->stateCounter > (0.5f * countLen * subCountLen)) 1358 && (aec->farlevel.sfrcounter == 0) 1359 1360 // Estimate in active far-end segments only 1361 && (aec->farlevel.averagelevel > (actThreshold * aec->farlevel.minlevel)) 1362 ) { 1363 1364 // Subtract noise power 1365 echo = aec->nearlevel.averagelevel - safety * aec->nearlevel.minlevel; 1366 1367 // ERL 1368 dtmp = 10 * (float)log10(aec->farlevel.averagelevel / 1369 aec->nearlevel.averagelevel + 1e-10f); 1370 dtmp2 = 10 * (float)log10(aec->farlevel.averagelevel / echo + 1e-10f); 1371 1372 aec->erl.instant = dtmp; 1373 if (dtmp > aec->erl.max) { 1374 aec->erl.max = dtmp; 1375 } 1376 1377 if (dtmp < aec->erl.min) { 1378 aec->erl.min = dtmp; 1379 } 1380 1381 aec->erl.counter++; 1382 aec->erl.sum += dtmp; 1383 aec->erl.average = aec->erl.sum / aec->erl.counter; 1384 1385 // Upper mean 1386 if (dtmp > aec->erl.average) { 1387 aec->erl.hicounter++; 1388 aec->erl.hisum += dtmp; 1389 aec->erl.himean = aec->erl.hisum / aec->erl.hicounter; 1390 } 1391 1392 // A_NLP 1393 dtmp = 10 * (float)log10(aec->nearlevel.averagelevel / 1394 aec->linoutlevel.averagelevel + 1e-10f); 1395 1396 // subtract noise power 1397 suppressedEcho = aec->linoutlevel.averagelevel - safety * aec->linoutlevel.minlevel; 1398 1399 dtmp2 = 10 * (float)log10(echo / suppressedEcho + 1e-10f); 1400 dtmp3 = 10 * (float)log10(aec->nearlevel.averagelevel / suppressedEcho + 1e-10f); 1401 1402 aec->aNlp.instant = dtmp2; 1403 if (dtmp > aec->aNlp.max) { 1404 aec->aNlp.max = dtmp; 1405 } 1406 1407 if (dtmp < aec->aNlp.min) { 1408 aec->aNlp.min = dtmp; 1409 } 1410 1411 aec->aNlp.counter++; 1412 aec->aNlp.sum += dtmp; 1413 aec->aNlp.average = aec->aNlp.sum / aec->aNlp.counter; 1414 1415 // Upper mean 1416 if (dtmp > aec->aNlp.average) { 1417 aec->aNlp.hicounter++; 1418 aec->aNlp.hisum += dtmp; 1419 aec->aNlp.himean = aec->aNlp.hisum / aec->aNlp.hicounter; 1420 } 1421 1422 // ERLE 1423 1424 // subtract noise power 1425 suppressedEcho = aec->nlpoutlevel.averagelevel - safety * aec->nlpoutlevel.minlevel; 1426 1427 dtmp = 10 * (float)log10(aec->nearlevel.averagelevel / 1428 aec->nlpoutlevel.averagelevel + 1e-10f); 1429 dtmp2 = 10 * (float)log10(echo / suppressedEcho + 1e-10f); 1430 1431 dtmp = dtmp2; 1432 aec->erle.instant = dtmp; 1433 if (dtmp > aec->erle.max) { 1434 aec->erle.max = dtmp; 1435 } 1436 1437 if (dtmp < aec->erle.min) { 1438 aec->erle.min = dtmp; 1439 } 1440 1441 aec->erle.counter++; 1442 aec->erle.sum += dtmp; 1443 aec->erle.average = aec->erle.sum / aec->erle.counter; 1444 1445 // Upper mean 1446 if (dtmp > aec->erle.average) { 1447 aec->erle.hicounter++; 1448 aec->erle.hisum += dtmp; 1449 aec->erle.himean = aec->erle.hisum / aec->erle.hicounter; 1450 } 1451 } 1452 1453 aec->stateCounter = 0; 1454 } 1455 } 1456 1457
