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 /* digital_agc.c 12 * 13 */ 14 15 #include <string.h> 16 #ifdef AGC_DEBUG 17 #include <stdio.h> 18 #endif 19 #include "digital_agc.h" 20 #include "gain_control.h" 21 22 // To generate the gaintable, copy&paste the following lines to a Matlab window: 23 // MaxGain = 6; MinGain = 0; CompRatio = 3; Knee = 1; 24 // zeros = 0:31; lvl = 2.^(1-zeros); 25 // A = -10*log10(lvl) * (CompRatio - 1) / CompRatio; 26 // B = MaxGain - MinGain; 27 // gains = round(2^16*10.^(0.05 * (MinGain + B * ( log(exp(-Knee*A)+exp(-Knee*B)) - log(1+exp(-Knee*B)) ) / log(1/(1+exp(Knee*B)))))); 28 // fprintf(1, '\t%i, %i, %i, %i,\n', gains); 29 // % Matlab code for plotting the gain and input/output level characteristic (copy/paste the following 3 lines): 30 // in = 10*log10(lvl); out = 20*log10(gains/65536); 31 // subplot(121); plot(in, out); axis([-30, 0, -5, 20]); grid on; xlabel('Input (dB)'); ylabel('Gain (dB)'); 32 // subplot(122); plot(in, in+out); axis([-30, 0, -30, 5]); grid on; xlabel('Input (dB)'); ylabel('Output (dB)'); 33 // zoom on; 34 35 // Generator table for y=log2(1+e^x) in Q8. 36 static const WebRtc_UWord16 kGenFuncTable[128] = { 37 256, 485, 786, 1126, 1484, 1849, 2217, 2586, 38 2955, 3324, 3693, 4063, 4432, 4801, 5171, 5540, 39 5909, 6279, 6648, 7017, 7387, 7756, 8125, 8495, 40 8864, 9233, 9603, 9972, 10341, 10711, 11080, 11449, 41 11819, 12188, 12557, 12927, 13296, 13665, 14035, 14404, 42 14773, 15143, 15512, 15881, 16251, 16620, 16989, 17359, 43 17728, 18097, 18466, 18836, 19205, 19574, 19944, 20313, 44 20682, 21052, 21421, 21790, 22160, 22529, 22898, 23268, 45 23637, 24006, 24376, 24745, 25114, 25484, 25853, 26222, 46 26592, 26961, 27330, 27700, 28069, 28438, 28808, 29177, 47 29546, 29916, 30285, 30654, 31024, 31393, 31762, 32132, 48 32501, 32870, 33240, 33609, 33978, 34348, 34717, 35086, 49 35456, 35825, 36194, 36564, 36933, 37302, 37672, 38041, 50 38410, 38780, 39149, 39518, 39888, 40257, 40626, 40996, 51 41365, 41734, 42104, 42473, 42842, 43212, 43581, 43950, 52 44320, 44689, 45058, 45428, 45797, 46166, 46536, 46905 53 }; 54 55 static const WebRtc_Word16 kAvgDecayTime = 250; // frames; < 3000 56 57 WebRtc_Word32 WebRtcAgc_CalculateGainTable(WebRtc_Word32 *gainTable, // Q16 58 WebRtc_Word16 digCompGaindB, // Q0 59 WebRtc_Word16 targetLevelDbfs,// Q0 60 WebRtc_UWord8 limiterEnable, 61 WebRtc_Word16 analogTarget) // Q0 62 { 63 // This function generates the compressor gain table used in the fixed digital part. 64 WebRtc_UWord32 tmpU32no1, tmpU32no2, absInLevel, logApprox; 65 WebRtc_Word32 inLevel, limiterLvl; 66 WebRtc_Word32 tmp32, tmp32no1, tmp32no2, numFIX, den, y32; 67 const WebRtc_UWord16 kLog10 = 54426; // log2(10) in Q14 68 const WebRtc_UWord16 kLog10_2 = 49321; // 10*log10(2) in Q14 69 const WebRtc_UWord16 kLogE_1 = 23637; // log2(e) in Q14 70 WebRtc_UWord16 constMaxGain; 71 WebRtc_UWord16 tmpU16, intPart, fracPart; 72 const WebRtc_Word16 kCompRatio = 3; 73 const WebRtc_Word16 kSoftLimiterLeft = 1; 74 WebRtc_Word16 limiterOffset = 0; // Limiter offset 75 WebRtc_Word16 limiterIdx, limiterLvlX; 76 WebRtc_Word16 constLinApprox, zeroGainLvl, maxGain, diffGain; 77 WebRtc_Word16 i, tmp16, tmp16no1; 78 int zeros, zerosScale; 79 80 // Constants 81 // kLogE_1 = 23637; // log2(e) in Q14 82 // kLog10 = 54426; // log2(10) in Q14 83 // kLog10_2 = 49321; // 10*log10(2) in Q14 84 85 // Calculate maximum digital gain and zero gain level 86 tmp32no1 = WEBRTC_SPL_MUL_16_16(digCompGaindB - analogTarget, kCompRatio - 1); 87 tmp16no1 = analogTarget - targetLevelDbfs; 88 tmp16no1 += WebRtcSpl_DivW32W16ResW16(tmp32no1 + (kCompRatio >> 1), kCompRatio); 89 maxGain = WEBRTC_SPL_MAX(tmp16no1, (analogTarget - targetLevelDbfs)); 90 tmp32no1 = WEBRTC_SPL_MUL_16_16(maxGain, kCompRatio); 91 zeroGainLvl = digCompGaindB; 92 zeroGainLvl -= WebRtcSpl_DivW32W16ResW16(tmp32no1 + ((kCompRatio - 1) >> 1), 93 kCompRatio - 1); 94 if ((digCompGaindB <= analogTarget) && (limiterEnable)) 95 { 96 zeroGainLvl += (analogTarget - digCompGaindB + kSoftLimiterLeft); 97 limiterOffset = 0; 98 } 99 100 // Calculate the difference between maximum gain and gain at 0dB0v: 101 // diffGain = maxGain + (compRatio-1)*zeroGainLvl/compRatio 102 // = (compRatio-1)*digCompGaindB/compRatio 103 tmp32no1 = WEBRTC_SPL_MUL_16_16(digCompGaindB, kCompRatio - 1); 104 diffGain = WebRtcSpl_DivW32W16ResW16(tmp32no1 + (kCompRatio >> 1), kCompRatio); 105 if (diffGain < 0) 106 { 107 return -1; 108 } 109 110 // Calculate the limiter level and index: 111 // limiterLvlX = analogTarget - limiterOffset 112 // limiterLvl = targetLevelDbfs + limiterOffset/compRatio 113 limiterLvlX = analogTarget - limiterOffset; 114 limiterIdx = 2 115 + WebRtcSpl_DivW32W16ResW16(WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)limiterLvlX, 13), 116 WEBRTC_SPL_RSHIFT_U16(kLog10_2, 1)); 117 tmp16no1 = WebRtcSpl_DivW32W16ResW16(limiterOffset + (kCompRatio >> 1), kCompRatio); 118 limiterLvl = targetLevelDbfs + tmp16no1; 119 120 // Calculate (through table lookup): 121 // constMaxGain = log2(1+2^(log2(e)*diffGain)); (in Q8) 122 constMaxGain = kGenFuncTable[diffGain]; // in Q8 123 124 // Calculate a parameter used to approximate the fractional part of 2^x with a 125 // piecewise linear function in Q14: 126 // constLinApprox = round(3/2*(4*(3-2*sqrt(2))/(log(2)^2)-0.5)*2^14); 127 constLinApprox = 22817; // in Q14 128 129 // Calculate a denominator used in the exponential part to convert from dB to linear scale: 130 // den = 20*constMaxGain (in Q8) 131 den = WEBRTC_SPL_MUL_16_U16(20, constMaxGain); // in Q8 132 133 for (i = 0; i < 32; i++) 134 { 135 // Calculate scaled input level (compressor): 136 // inLevel = fix((-constLog10_2*(compRatio-1)*(1-i)+fix(compRatio/2))/compRatio) 137 tmp16 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16(kCompRatio - 1, i - 1); // Q0 138 tmp32 = WEBRTC_SPL_MUL_16_U16(tmp16, kLog10_2) + 1; // Q14 139 inLevel = WebRtcSpl_DivW32W16(tmp32, kCompRatio); // Q14 140 141 // Calculate diffGain-inLevel, to map using the genFuncTable 142 inLevel = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)diffGain, 14) - inLevel; // Q14 143 144 // Make calculations on abs(inLevel) and compensate for the sign afterwards. 145 absInLevel = (WebRtc_UWord32)WEBRTC_SPL_ABS_W32(inLevel); // Q14 146 147 // LUT with interpolation 148 intPart = (WebRtc_UWord16)WEBRTC_SPL_RSHIFT_U32(absInLevel, 14); 149 fracPart = (WebRtc_UWord16)(absInLevel & 0x00003FFF); // extract the fractional part 150 tmpU16 = kGenFuncTable[intPart + 1] - kGenFuncTable[intPart]; // Q8 151 tmpU32no1 = WEBRTC_SPL_UMUL_16_16(tmpU16, fracPart); // Q22 152 tmpU32no1 += WEBRTC_SPL_LSHIFT_U32((WebRtc_UWord32)kGenFuncTable[intPart], 14); // Q22 153 logApprox = WEBRTC_SPL_RSHIFT_U32(tmpU32no1, 8); // Q14 154 // Compensate for negative exponent using the relation: 155 // log2(1 + 2^-x) = log2(1 + 2^x) - x 156 if (inLevel < 0) 157 { 158 zeros = WebRtcSpl_NormU32(absInLevel); 159 zerosScale = 0; 160 if (zeros < 15) 161 { 162 // Not enough space for multiplication 163 tmpU32no2 = WEBRTC_SPL_RSHIFT_U32(absInLevel, 15 - zeros); // Q(zeros-1) 164 tmpU32no2 = WEBRTC_SPL_UMUL_32_16(tmpU32no2, kLogE_1); // Q(zeros+13) 165 if (zeros < 9) 166 { 167 tmpU32no1 = WEBRTC_SPL_RSHIFT_U32(tmpU32no1, 9 - zeros); // Q(zeros+13) 168 zerosScale = 9 - zeros; 169 } else 170 { 171 tmpU32no2 = WEBRTC_SPL_RSHIFT_U32(tmpU32no2, zeros - 9); // Q22 172 } 173 } else 174 { 175 tmpU32no2 = WEBRTC_SPL_UMUL_32_16(absInLevel, kLogE_1); // Q28 176 tmpU32no2 = WEBRTC_SPL_RSHIFT_U32(tmpU32no2, 6); // Q22 177 } 178 logApprox = 0; 179 if (tmpU32no2 < tmpU32no1) 180 { 181 logApprox = WEBRTC_SPL_RSHIFT_U32(tmpU32no1 - tmpU32no2, 8 - zerosScale); //Q14 182 } 183 } 184 numFIX = WEBRTC_SPL_LSHIFT_W32(WEBRTC_SPL_MUL_16_U16(maxGain, constMaxGain), 6); // Q14 185 numFIX -= WEBRTC_SPL_MUL_32_16((WebRtc_Word32)logApprox, diffGain); // Q14 186 187 // Calculate ratio 188 // Shift numFIX as much as possible 189 zeros = WebRtcSpl_NormW32(numFIX); 190 numFIX = WEBRTC_SPL_LSHIFT_W32(numFIX, zeros); // Q(14+zeros) 191 192 // Shift den so we end up in Qy1 193 tmp32no1 = WEBRTC_SPL_SHIFT_W32(den, zeros - 8); // Q(zeros) 194 if (numFIX < 0) 195 { 196 numFIX -= WEBRTC_SPL_RSHIFT_W32(tmp32no1, 1); 197 } else 198 { 199 numFIX += WEBRTC_SPL_RSHIFT_W32(tmp32no1, 1); 200 } 201 y32 = WEBRTC_SPL_DIV(numFIX, tmp32no1); // in Q14 202 if (limiterEnable && (i < limiterIdx)) 203 { 204 tmp32 = WEBRTC_SPL_MUL_16_U16(i - 1, kLog10_2); // Q14 205 tmp32 -= WEBRTC_SPL_LSHIFT_W32(limiterLvl, 14); // Q14 206 y32 = WebRtcSpl_DivW32W16(tmp32 + 10, 20); 207 } 208 if (y32 > 39000) 209 { 210 tmp32 = WEBRTC_SPL_MUL(y32 >> 1, kLog10) + 4096; // in Q27 211 tmp32 = WEBRTC_SPL_RSHIFT_W32(tmp32, 13); // in Q14 212 } else 213 { 214 tmp32 = WEBRTC_SPL_MUL(y32, kLog10) + 8192; // in Q28 215 tmp32 = WEBRTC_SPL_RSHIFT_W32(tmp32, 14); // in Q14 216 } 217 tmp32 += WEBRTC_SPL_LSHIFT_W32(16, 14); // in Q14 (Make sure final output is in Q16) 218 219 // Calculate power 220 if (tmp32 > 0) 221 { 222 intPart = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 14); 223 fracPart = (WebRtc_UWord16)(tmp32 & 0x00003FFF); // in Q14 224 if (WEBRTC_SPL_RSHIFT_W32(fracPart, 13)) 225 { 226 tmp16 = WEBRTC_SPL_LSHIFT_W16(2, 14) - constLinApprox; 227 tmp32no2 = WEBRTC_SPL_LSHIFT_W32(1, 14) - fracPart; 228 tmp32no2 = WEBRTC_SPL_MUL_32_16(tmp32no2, tmp16); 229 tmp32no2 = WEBRTC_SPL_RSHIFT_W32(tmp32no2, 13); 230 tmp32no2 = WEBRTC_SPL_LSHIFT_W32(1, 14) - tmp32no2; 231 } else 232 { 233 tmp16 = constLinApprox - WEBRTC_SPL_LSHIFT_W16(1, 14); 234 tmp32no2 = WEBRTC_SPL_MUL_32_16(fracPart, tmp16); 235 tmp32no2 = WEBRTC_SPL_RSHIFT_W32(tmp32no2, 13); 236 } 237 fracPart = (WebRtc_UWord16)tmp32no2; 238 gainTable[i] = WEBRTC_SPL_LSHIFT_W32(1, intPart) 239 + WEBRTC_SPL_SHIFT_W32(fracPart, intPart - 14); 240 } else 241 { 242 gainTable[i] = 0; 243 } 244 } 245 246 return 0; 247 } 248 249 WebRtc_Word32 WebRtcAgc_InitDigital(DigitalAgc_t *stt, WebRtc_Word16 agcMode) 250 { 251 252 if (agcMode == kAgcModeFixedDigital) 253 { 254 // start at minimum to find correct gain faster 255 stt->capacitorSlow = 0; 256 } else 257 { 258 // start out with 0 dB gain 259 stt->capacitorSlow = 134217728; // (WebRtc_Word32)(0.125f * 32768.0f * 32768.0f); 260 } 261 stt->capacitorFast = 0; 262 stt->gain = 65536; 263 stt->gatePrevious = 0; 264 stt->agcMode = agcMode; 265 #ifdef AGC_DEBUG 266 stt->frameCounter = 0; 267 #endif 268 269 // initialize VADs 270 WebRtcAgc_InitVad(&stt->vadNearend); 271 WebRtcAgc_InitVad(&stt->vadFarend); 272 273 return 0; 274 } 275 276 WebRtc_Word32 WebRtcAgc_AddFarendToDigital(DigitalAgc_t *stt, const WebRtc_Word16 *in_far, 277 WebRtc_Word16 nrSamples) 278 { 279 // Check for valid pointer 280 if (&stt->vadFarend == NULL) 281 { 282 return -1; 283 } 284 285 // VAD for far end 286 WebRtcAgc_ProcessVad(&stt->vadFarend, in_far, nrSamples); 287 288 return 0; 289 } 290 291 WebRtc_Word32 WebRtcAgc_ProcessDigital(DigitalAgc_t *stt, const WebRtc_Word16 *in_near, 292 const WebRtc_Word16 *in_near_H, WebRtc_Word16 *out, 293 WebRtc_Word16 *out_H, WebRtc_UWord32 FS, 294 WebRtc_Word16 lowlevelSignal) 295 { 296 // array for gains (one value per ms, incl start & end) 297 WebRtc_Word32 gains[11]; 298 299 WebRtc_Word32 out_tmp, tmp32; 300 WebRtc_Word32 env[10]; 301 WebRtc_Word32 nrg, max_nrg; 302 WebRtc_Word32 cur_level; 303 WebRtc_Word32 gain32, delta; 304 WebRtc_Word16 logratio; 305 WebRtc_Word16 lower_thr, upper_thr; 306 WebRtc_Word16 zeros, zeros_fast, frac; 307 WebRtc_Word16 decay; 308 WebRtc_Word16 gate, gain_adj; 309 WebRtc_Word16 k, n; 310 WebRtc_Word16 L, L2; // samples/subframe 311 312 // determine number of samples per ms 313 if (FS == 8000) 314 { 315 L = 8; 316 L2 = 3; 317 } else if (FS == 16000) 318 { 319 L = 16; 320 L2 = 4; 321 } else if (FS == 32000) 322 { 323 L = 16; 324 L2 = 4; 325 } else 326 { 327 return -1; 328 } 329 330 memcpy(out, in_near, 10 * L * sizeof(WebRtc_Word16)); 331 if (FS == 32000) 332 { 333 memcpy(out_H, in_near_H, 10 * L * sizeof(WebRtc_Word16)); 334 } 335 // VAD for near end 336 logratio = WebRtcAgc_ProcessVad(&stt->vadNearend, out, L * 10); 337 338 // Account for far end VAD 339 if (stt->vadFarend.counter > 10) 340 { 341 tmp32 = WEBRTC_SPL_MUL_16_16(3, logratio); 342 logratio = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32 - stt->vadFarend.logRatio, 2); 343 } 344 345 // Determine decay factor depending on VAD 346 // upper_thr = 1.0f; 347 // lower_thr = 0.25f; 348 upper_thr = 1024; // Q10 349 lower_thr = 0; // Q10 350 if (logratio > upper_thr) 351 { 352 // decay = -2^17 / DecayTime; -> -65 353 decay = -65; 354 } else if (logratio < lower_thr) 355 { 356 decay = 0; 357 } else 358 { 359 // decay = (WebRtc_Word16)(((lower_thr - logratio) 360 // * (2^27/(DecayTime*(upper_thr-lower_thr)))) >> 10); 361 // SUBSTITUTED: 2^27/(DecayTime*(upper_thr-lower_thr)) -> 65 362 tmp32 = WEBRTC_SPL_MUL_16_16((lower_thr - logratio), 65); 363 decay = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 10); 364 } 365 366 // adjust decay factor for long silence (detected as low standard deviation) 367 // This is only done in the adaptive modes 368 if (stt->agcMode != kAgcModeFixedDigital) 369 { 370 if (stt->vadNearend.stdLongTerm < 4000) 371 { 372 decay = 0; 373 } else if (stt->vadNearend.stdLongTerm < 8096) 374 { 375 // decay = (WebRtc_Word16)(((stt->vadNearend.stdLongTerm - 4000) * decay) >> 12); 376 tmp32 = WEBRTC_SPL_MUL_16_16((stt->vadNearend.stdLongTerm - 4000), decay); 377 decay = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 12); 378 } 379 380 if (lowlevelSignal != 0) 381 { 382 decay = 0; 383 } 384 } 385 #ifdef AGC_DEBUG 386 stt->frameCounter++; 387 fprintf(stt->logFile, "%5.2f\t%d\t%d\t%d\t", (float)(stt->frameCounter) / 100, logratio, decay, stt->vadNearend.stdLongTerm); 388 #endif 389 // Find max amplitude per sub frame 390 // iterate over sub frames 391 for (k = 0; k < 10; k++) 392 { 393 // iterate over samples 394 max_nrg = 0; 395 for (n = 0; n < L; n++) 396 { 397 nrg = WEBRTC_SPL_MUL_16_16(out[k * L + n], out[k * L + n]); 398 if (nrg > max_nrg) 399 { 400 max_nrg = nrg; 401 } 402 } 403 env[k] = max_nrg; 404 } 405 406 // Calculate gain per sub frame 407 gains[0] = stt->gain; 408 for (k = 0; k < 10; k++) 409 { 410 // Fast envelope follower 411 // decay time = -131000 / -1000 = 131 (ms) 412 stt->capacitorFast = AGC_SCALEDIFF32(-1000, stt->capacitorFast, stt->capacitorFast); 413 if (env[k] > stt->capacitorFast) 414 { 415 stt->capacitorFast = env[k]; 416 } 417 // Slow envelope follower 418 if (env[k] > stt->capacitorSlow) 419 { 420 // increase capacitorSlow 421 stt->capacitorSlow 422 = AGC_SCALEDIFF32(500, (env[k] - stt->capacitorSlow), stt->capacitorSlow); 423 } else 424 { 425 // decrease capacitorSlow 426 stt->capacitorSlow 427 = AGC_SCALEDIFF32(decay, stt->capacitorSlow, stt->capacitorSlow); 428 } 429 430 // use maximum of both capacitors as current level 431 if (stt->capacitorFast > stt->capacitorSlow) 432 { 433 cur_level = stt->capacitorFast; 434 } else 435 { 436 cur_level = stt->capacitorSlow; 437 } 438 // Translate signal level into gain, using a piecewise linear approximation 439 // find number of leading zeros 440 zeros = WebRtcSpl_NormU32((WebRtc_UWord32)cur_level); 441 if (cur_level == 0) 442 { 443 zeros = 31; 444 } 445 tmp32 = (WEBRTC_SPL_LSHIFT_W32(cur_level, zeros) & 0x7FFFFFFF); 446 frac = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 19); // Q12 447 tmp32 = WEBRTC_SPL_MUL((stt->gainTable[zeros-1] - stt->gainTable[zeros]), frac); 448 gains[k + 1] = stt->gainTable[zeros] + WEBRTC_SPL_RSHIFT_W32(tmp32, 12); 449 #ifdef AGC_DEBUG 450 if (k == 0) 451 { 452 fprintf(stt->logFile, "%d\t%d\t%d\t%d\t%d\n", env[0], cur_level, stt->capacitorFast, stt->capacitorSlow, zeros); 453 } 454 #endif 455 } 456 457 // Gate processing (lower gain during absence of speech) 458 zeros = WEBRTC_SPL_LSHIFT_W16(zeros, 9) - WEBRTC_SPL_RSHIFT_W16(frac, 3); 459 // find number of leading zeros 460 zeros_fast = WebRtcSpl_NormU32((WebRtc_UWord32)stt->capacitorFast); 461 if (stt->capacitorFast == 0) 462 { 463 zeros_fast = 31; 464 } 465 tmp32 = (WEBRTC_SPL_LSHIFT_W32(stt->capacitorFast, zeros_fast) & 0x7FFFFFFF); 466 zeros_fast = WEBRTC_SPL_LSHIFT_W16(zeros_fast, 9); 467 zeros_fast -= (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 22); 468 469 gate = 1000 + zeros_fast - zeros - stt->vadNearend.stdShortTerm; 470 471 if (gate < 0) 472 { 473 stt->gatePrevious = 0; 474 } else 475 { 476 tmp32 = WEBRTC_SPL_MUL_16_16(stt->gatePrevious, 7); 477 gate = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32((WebRtc_Word32)gate + tmp32, 3); 478 stt->gatePrevious = gate; 479 } 480 // gate < 0 -> no gate 481 // gate > 2500 -> max gate 482 if (gate > 0) 483 { 484 if (gate < 2500) 485 { 486 gain_adj = WEBRTC_SPL_RSHIFT_W16(2500 - gate, 5); 487 } else 488 { 489 gain_adj = 0; 490 } 491 for (k = 0; k < 10; k++) 492 { 493 if ((gains[k + 1] - stt->gainTable[0]) > 8388608) 494 { 495 // To prevent wraparound 496 tmp32 = WEBRTC_SPL_RSHIFT_W32((gains[k+1] - stt->gainTable[0]), 8); 497 tmp32 = WEBRTC_SPL_MUL(tmp32, (178 + gain_adj)); 498 } else 499 { 500 tmp32 = WEBRTC_SPL_MUL((gains[k+1] - stt->gainTable[0]), (178 + gain_adj)); 501 tmp32 = WEBRTC_SPL_RSHIFT_W32(tmp32, 8); 502 } 503 gains[k + 1] = stt->gainTable[0] + tmp32; 504 } 505 } 506 507 // Limit gain to avoid overload distortion 508 for (k = 0; k < 10; k++) 509 { 510 // To prevent wrap around 511 zeros = 10; 512 if (gains[k + 1] > 47453132) 513 { 514 zeros = 16 - WebRtcSpl_NormW32(gains[k + 1]); 515 } 516 gain32 = WEBRTC_SPL_RSHIFT_W32(gains[k+1], zeros) + 1; 517 gain32 = WEBRTC_SPL_MUL(gain32, gain32); 518 // check for overflow 519 while (AGC_MUL32(WEBRTC_SPL_RSHIFT_W32(env[k], 12) + 1, gain32) 520 > WEBRTC_SPL_SHIFT_W32((WebRtc_Word32)32767, 2 * (1 - zeros + 10))) 521 { 522 // multiply by 253/256 ==> -0.1 dB 523 if (gains[k + 1] > 8388607) 524 { 525 // Prevent wrap around 526 gains[k + 1] = WEBRTC_SPL_MUL(WEBRTC_SPL_RSHIFT_W32(gains[k+1], 8), 253); 527 } else 528 { 529 gains[k + 1] = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL(gains[k+1], 253), 8); 530 } 531 gain32 = WEBRTC_SPL_RSHIFT_W32(gains[k+1], zeros) + 1; 532 gain32 = WEBRTC_SPL_MUL(gain32, gain32); 533 } 534 } 535 // gain reductions should be done 1 ms earlier than gain increases 536 for (k = 1; k < 10; k++) 537 { 538 if (gains[k] > gains[k + 1]) 539 { 540 gains[k] = gains[k + 1]; 541 } 542 } 543 // save start gain for next frame 544 stt->gain = gains[10]; 545 546 // Apply gain 547 // handle first sub frame separately 548 delta = WEBRTC_SPL_LSHIFT_W32(gains[1] - gains[0], (4 - L2)); 549 gain32 = WEBRTC_SPL_LSHIFT_W32(gains[0], 4); 550 // iterate over samples 551 for (n = 0; n < L; n++) 552 { 553 // For lower band 554 tmp32 = WEBRTC_SPL_MUL((WebRtc_Word32)out[n], WEBRTC_SPL_RSHIFT_W32(gain32 + 127, 7)); 555 out_tmp = WEBRTC_SPL_RSHIFT_W32(tmp32 , 16); 556 if (out_tmp > 4095) 557 { 558 out[n] = (WebRtc_Word16)32767; 559 } else if (out_tmp < -4096) 560 { 561 out[n] = (WebRtc_Word16)-32768; 562 } else 563 { 564 tmp32 = WEBRTC_SPL_MUL((WebRtc_Word32)out[n], WEBRTC_SPL_RSHIFT_W32(gain32, 4)); 565 out[n] = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32 , 16); 566 } 567 // For higher band 568 if (FS == 32000) 569 { 570 tmp32 = WEBRTC_SPL_MUL((WebRtc_Word32)out_H[n], 571 WEBRTC_SPL_RSHIFT_W32(gain32 + 127, 7)); 572 out_tmp = WEBRTC_SPL_RSHIFT_W32(tmp32 , 16); 573 if (out_tmp > 4095) 574 { 575 out_H[n] = (WebRtc_Word16)32767; 576 } else if (out_tmp < -4096) 577 { 578 out_H[n] = (WebRtc_Word16)-32768; 579 } else 580 { 581 tmp32 = WEBRTC_SPL_MUL((WebRtc_Word32)out_H[n], 582 WEBRTC_SPL_RSHIFT_W32(gain32, 4)); 583 out_H[n] = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32 , 16); 584 } 585 } 586 // 587 588 gain32 += delta; 589 } 590 // iterate over subframes 591 for (k = 1; k < 10; k++) 592 { 593 delta = WEBRTC_SPL_LSHIFT_W32(gains[k+1] - gains[k], (4 - L2)); 594 gain32 = WEBRTC_SPL_LSHIFT_W32(gains[k], 4); 595 // iterate over samples 596 for (n = 0; n < L; n++) 597 { 598 // For lower band 599 tmp32 = WEBRTC_SPL_MUL((WebRtc_Word32)out[k * L + n], 600 WEBRTC_SPL_RSHIFT_W32(gain32, 4)); 601 out[k * L + n] = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32 , 16); 602 // For higher band 603 if (FS == 32000) 604 { 605 tmp32 = WEBRTC_SPL_MUL((WebRtc_Word32)out_H[k * L + n], 606 WEBRTC_SPL_RSHIFT_W32(gain32, 4)); 607 out_H[k * L + n] = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32 , 16); 608 } 609 gain32 += delta; 610 } 611 } 612 613 return 0; 614 } 615 616 void WebRtcAgc_InitVad(AgcVad_t *state) 617 { 618 WebRtc_Word16 k; 619 620 state->HPstate = 0; // state of high pass filter 621 state->logRatio = 0; // log( P(active) / P(inactive) ) 622 // average input level (Q10) 623 state->meanLongTerm = WEBRTC_SPL_LSHIFT_W16(15, 10); 624 625 // variance of input level (Q8) 626 state->varianceLongTerm = WEBRTC_SPL_LSHIFT_W32(500, 8); 627 628 state->stdLongTerm = 0; // standard deviation of input level in dB 629 // short-term average input level (Q10) 630 state->meanShortTerm = WEBRTC_SPL_LSHIFT_W16(15, 10); 631 632 // short-term variance of input level (Q8) 633 state->varianceShortTerm = WEBRTC_SPL_LSHIFT_W32(500, 8); 634 635 state->stdShortTerm = 0; // short-term standard deviation of input level in dB 636 state->counter = 3; // counts updates 637 for (k = 0; k < 8; k++) 638 { 639 // downsampling filter 640 state->downState[k] = 0; 641 } 642 } 643 644 WebRtc_Word16 WebRtcAgc_ProcessVad(AgcVad_t *state, // (i) VAD state 645 const WebRtc_Word16 *in, // (i) Speech signal 646 WebRtc_Word16 nrSamples) // (i) number of samples 647 { 648 WebRtc_Word32 out, nrg, tmp32, tmp32b; 649 WebRtc_UWord16 tmpU16; 650 WebRtc_Word16 k, subfr, tmp16; 651 WebRtc_Word16 buf1[8]; 652 WebRtc_Word16 buf2[4]; 653 WebRtc_Word16 HPstate; 654 WebRtc_Word16 zeros, dB; 655 WebRtc_Word16 *buf1_ptr; 656 657 // process in 10 sub frames of 1 ms (to save on memory) 658 nrg = 0; 659 buf1_ptr = &buf1[0]; 660 HPstate = state->HPstate; 661 for (subfr = 0; subfr < 10; subfr++) 662 { 663 // downsample to 4 kHz 664 if (nrSamples == 160) 665 { 666 for (k = 0; k < 8; k++) 667 { 668 tmp32 = (WebRtc_Word32)in[2 * k] + (WebRtc_Word32)in[2 * k + 1]; 669 tmp32 = WEBRTC_SPL_RSHIFT_W32(tmp32, 1); 670 buf1[k] = (WebRtc_Word16)tmp32; 671 } 672 in += 16; 673 674 WebRtcSpl_DownsampleBy2(buf1, 8, buf2, state->downState); 675 } else 676 { 677 WebRtcSpl_DownsampleBy2(in, 8, buf2, state->downState); 678 in += 8; 679 } 680 681 // high pass filter and compute energy 682 for (k = 0; k < 4; k++) 683 { 684 out = buf2[k] + HPstate; 685 tmp32 = WEBRTC_SPL_MUL(600, out); 686 HPstate = (WebRtc_Word16)(WEBRTC_SPL_RSHIFT_W32(tmp32, 10) - buf2[k]); 687 tmp32 = WEBRTC_SPL_MUL(out, out); 688 nrg += WEBRTC_SPL_RSHIFT_W32(tmp32, 6); 689 } 690 } 691 state->HPstate = HPstate; 692 693 // find number of leading zeros 694 if (!(0xFFFF0000 & nrg)) 695 { 696 zeros = 16; 697 } else 698 { 699 zeros = 0; 700 } 701 if (!(0xFF000000 & (nrg << zeros))) 702 { 703 zeros += 8; 704 } 705 if (!(0xF0000000 & (nrg << zeros))) 706 { 707 zeros += 4; 708 } 709 if (!(0xC0000000 & (nrg << zeros))) 710 { 711 zeros += 2; 712 } 713 if (!(0x80000000 & (nrg << zeros))) 714 { 715 zeros += 1; 716 } 717 718 // energy level (range {-32..30}) (Q10) 719 dB = WEBRTC_SPL_LSHIFT_W16(15 - zeros, 11); 720 721 // Update statistics 722 723 if (state->counter < kAvgDecayTime) 724 { 725 // decay time = AvgDecTime * 10 ms 726 state->counter++; 727 } 728 729 // update short-term estimate of mean energy level (Q10) 730 tmp32 = (WEBRTC_SPL_MUL_16_16(state->meanShortTerm, 15) + (WebRtc_Word32)dB); 731 state->meanShortTerm = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 4); 732 733 // update short-term estimate of variance in energy level (Q8) 734 tmp32 = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL_16_16(dB, dB), 12); 735 tmp32 += WEBRTC_SPL_MUL(state->varianceShortTerm, 15); 736 state->varianceShortTerm = WEBRTC_SPL_RSHIFT_W32(tmp32, 4); 737 738 // update short-term estimate of standard deviation in energy level (Q10) 739 tmp32 = WEBRTC_SPL_MUL_16_16(state->meanShortTerm, state->meanShortTerm); 740 tmp32 = WEBRTC_SPL_LSHIFT_W32(state->varianceShortTerm, 12) - tmp32; 741 state->stdShortTerm = (WebRtc_Word16)WebRtcSpl_Sqrt(tmp32); 742 743 // update long-term estimate of mean energy level (Q10) 744 tmp32 = WEBRTC_SPL_MUL_16_16(state->meanLongTerm, state->counter) + (WebRtc_Word32)dB; 745 state->meanLongTerm = WebRtcSpl_DivW32W16ResW16(tmp32, 746 WEBRTC_SPL_ADD_SAT_W16(state->counter, 1)); 747 748 // update long-term estimate of variance in energy level (Q8) 749 tmp32 = WEBRTC_SPL_RSHIFT_W32(WEBRTC_SPL_MUL_16_16(dB, dB), 12); 750 tmp32 += WEBRTC_SPL_MUL(state->varianceLongTerm, state->counter); 751 state->varianceLongTerm = WebRtcSpl_DivW32W16(tmp32, 752 WEBRTC_SPL_ADD_SAT_W16(state->counter, 1)); 753 754 // update long-term estimate of standard deviation in energy level (Q10) 755 tmp32 = WEBRTC_SPL_MUL_16_16(state->meanLongTerm, state->meanLongTerm); 756 tmp32 = WEBRTC_SPL_LSHIFT_W32(state->varianceLongTerm, 12) - tmp32; 757 state->stdLongTerm = (WebRtc_Word16)WebRtcSpl_Sqrt(tmp32); 758 759 // update voice activity measure (Q10) 760 tmp16 = WEBRTC_SPL_LSHIFT_W16(3, 12); 761 tmp32 = WEBRTC_SPL_MUL_16_16(tmp16, (dB - state->meanLongTerm)); 762 tmp32 = WebRtcSpl_DivW32W16(tmp32, state->stdLongTerm); 763 tmpU16 = WEBRTC_SPL_LSHIFT_U16((WebRtc_UWord16)13, 12); 764 tmp32b = WEBRTC_SPL_MUL_16_U16(state->logRatio, tmpU16); 765 tmp32 += WEBRTC_SPL_RSHIFT_W32(tmp32b, 10); 766 767 state->logRatio = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmp32, 6); 768 769 // limit 770 if (state->logRatio > 2048) 771 { 772 state->logRatio = 2048; 773 } 774 if (state->logRatio < -2048) 775 { 776 state->logRatio = -2048; 777 } 778 779 return state->logRatio; // Q10 780 } 781
