src/third_party/webrtc/modules/audio_processing/aecm/aecm_core.c

   1 /*
   2  *  Copyright (c) 2012 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 #include "webrtc/modules/audio_processing/aecm/aecm_core.h"
  12
  13 #include <assert.h>
  14 #include <stddef.h>
  15 #include <stdlib.h>
  16
  17 #include "webrtc/common_audio/signal_processing/include/real_fft.h"
  18 #include "webrtc/modules/audio_processing/aecm/include/echo_control_mobile.h"
  19 #include "webrtc/modules/audio_processing/utility/delay_estimator_wrapper.h"
  20 #include "webrtc/modules/audio_processing/utility/ring_buffer.h"
  21 #include "webrtc/system_wrappers/interface/compile_assert_c.h"
  22 #include "webrtc/system_wrappers/interface/cpu_features_wrapper.h"
  23 #include "webrtc/typedefs.h"
  24
  25 #ifdef AEC_DEBUG
  26 FILE *dfile;
  27 FILE *testfile;
  28 #endif
  29
  30 const int16_t WebRtcAecm_kCosTable[] = {
  31     8192,  8190,  8187,  8180,  8172,  8160,  8147,  8130,  8112,
  32     8091,  8067,  8041,  8012,  7982,  7948,  7912,  7874,  7834,
  33     7791,  7745,  7697,  7647,  7595,  7540,  7483,  7424,  7362,
  34     7299,  7233,  7164,  7094,  7021,  6947,  6870,  6791,  6710,
  35     6627,  6542,  6455,  6366,  6275,  6182,  6087,  5991,  5892,
  36     5792,  5690,  5586,  5481,  5374,  5265,  5155,  5043,  4930,
  37     4815,  4698,  4580,  4461,  4341,  4219,  4096,  3971,  3845,
  38     3719,  3591,  3462,  3331,  3200,  3068,  2935,  2801,  2667,
  39     2531,  2395,  2258,  2120,  1981,  1842,  1703,  1563,  1422,
  40     1281,  1140,   998,   856,   713,   571,   428,   285,   142,
  41        0,  -142,  -285,  -428,  -571,  -713,  -856,  -998, -1140,
  42    -1281, -1422, -1563, -1703, -1842, -1981, -2120, -2258, -2395,
  43    -2531, -2667, -2801, -2935, -3068, -3200, -3331, -3462, -3591,
  44    -3719, -3845, -3971, -4095, -4219, -4341, -4461, -4580, -4698,
  45    -4815, -4930, -5043, -5155, -5265, -5374, -5481, -5586, -5690,
  46    -5792, -5892, -5991, -6087, -6182, -6275, -6366, -6455, -6542,
  47    -6627, -6710, -6791, -6870, -6947, -7021, -7094, -7164, -7233,
  48    -7299, -7362, -7424, -7483, -7540, -7595, -7647, -7697, -7745,
  49    -7791, -7834, -7874, -7912, -7948, -7982, -8012, -8041, -8067,
  50    -8091, -8112, -8130, -8147, -8160, -8172, -8180, -8187, -8190,
  51    -8191, -8190, -8187, -8180, -8172, -8160, -8147, -8130, -8112,
  52    -8091, -8067, -8041, -8012, -7982, -7948, -7912, -7874, -7834,
  53    -7791, -7745, -7697, -7647, -7595, -7540, -7483, -7424, -7362,
  54    -7299, -7233, -7164, -7094, -7021, -6947, -6870, -6791, -6710,
  55    -6627, -6542, -6455, -6366, -6275, -6182, -6087, -5991, -5892,
  56    -5792, -5690, -5586, -5481, -5374, -5265, -5155, -5043, -4930,
  57    -4815, -4698, -4580, -4461, -4341, -4219, -4096, -3971, -3845,
  58    -3719, -3591, -3462, -3331, -3200, -3068, -2935, -2801, -2667,
  59    -2531, -2395, -2258, -2120, -1981, -1842, -1703, -1563, -1422,
  60    -1281, -1140,  -998,  -856,  -713,  -571,  -428,  -285,  -142,
  61        0,   142,   285,   428,   571,   713,   856,   998,  1140,
  62     1281,  1422,  1563,  1703,  1842,  1981,  2120,  2258,  2395,
  63     2531,  2667,  2801,  2935,  3068,  3200,  3331,  3462,  3591,
  64     3719,  3845,  3971,  4095,  4219,  4341,  4461,  4580,  4698,
  65     4815,  4930,  5043,  5155,  5265,  5374,  5481,  5586,  5690,
  66     5792,  5892,  5991,  6087,  6182,  6275,  6366,  6455,  6542,
  67     6627,  6710,  6791,  6870,  6947,  7021,  7094,  7164,  7233,
  68     7299,  7362,  7424,  7483,  7540,  7595,  7647,  7697,  7745,
  69     7791,  7834,  7874,  7912,  7948,  7982,  8012,  8041,  8067,
  70     8091,  8112,  8130,  8147,  8160,  8172,  8180,  8187,  8190
  71 };
  72
  73 const int16_t WebRtcAecm_kSinTable[] = {
  74        0,    142,    285,    428,    571,    713,    856,    998,
  75     1140,   1281,   1422,   1563,   1703,   1842,   1981,   2120,
  76     2258,   2395,   2531,   2667,   2801,   2935,   3068,   3200,
  77     3331,   3462,   3591,   3719,   3845,   3971,   4095,   4219,
  78     4341,   4461,   4580,   4698,   4815,   4930,   5043,   5155,
  79     5265,   5374,   5481,   5586,   5690,   5792,   5892,   5991,
  80     6087,   6182,   6275,   6366,   6455,   6542,   6627,   6710,
  81     6791,   6870,   6947,   7021,   7094,   7164,   7233,   7299,
  82     7362,   7424,   7483,   7540,   7595,   7647,   7697,   7745,
  83     7791,   7834,   7874,   7912,   7948,   7982,   8012,   8041,
  84     8067,   8091,   8112,   8130,   8147,   8160,   8172,   8180,
  85     8187,   8190,   8191,   8190,   8187,   8180,   8172,   8160,
  86     8147,   8130,   8112,   8091,   8067,   8041,   8012,   7982,
  87     7948,   7912,   7874,   7834,   7791,   7745,   7697,   7647,
  88     7595,   7540,   7483,   7424,   7362,   7299,   7233,   7164,
  89     7094,   7021,   6947,   6870,   6791,   6710,   6627,   6542,
  90     6455,   6366,   6275,   6182,   6087,   5991,   5892,   5792,
  91     5690,   5586,   5481,   5374,   5265,   5155,   5043,   4930,
  92     4815,   4698,   4580,   4461,   4341,   4219,   4096,   3971,
  93     3845,   3719,   3591,   3462,   3331,   3200,   3068,   2935,
  94     2801,   2667,   2531,   2395,   2258,   2120,   1981,   1842,
  95     1703,   1563,   1422,   1281,   1140,    998,    856,    713,
  96      571,    428,    285,    142,      0,   -142,   -285,   -428,
  97     -571,   -713,   -856,   -998,  -1140,  -1281,  -1422,  -1563,
  98    -1703,  -1842,  -1981,  -2120,  -2258,  -2395,  -2531,  -2667,
  99    -2801,  -2935,  -3068,  -3200,  -3331,  -3462,  -3591,  -3719,
 100    -3845,  -3971,  -4095,  -4219,  -4341,  -4461,  -4580,  -4698,
 101    -4815,  -4930,  -5043,  -5155,  -5265,  -5374,  -5481,  -5586,
 102    -5690,  -5792,  -5892,  -5991,  -6087,  -6182,  -6275,  -6366,
 103    -6455,  -6542,  -6627,  -6710,  -6791,  -6870,  -6947,  -7021,
 104    -7094,  -7164,  -7233,  -7299,  -7362,  -7424,  -7483,  -7540,
 105    -7595,  -7647,  -7697,  -7745,  -7791,  -7834,  -7874,  -7912,
 106    -7948,  -7982,  -8012,  -8041,  -8067,  -8091,  -8112,  -8130,
 107    -8147,  -8160,  -8172,  -8180,  -8187,  -8190,  -8191,  -8190,
 108    -8187,  -8180,  -8172,  -8160,  -8147,  -8130,  -8112,  -8091,
 109    -8067,  -8041,  -8012,  -7982,  -7948,  -7912,  -7874,  -7834,
 110    -7791,  -7745,  -7697,  -7647,  -7595,  -7540,  -7483,  -7424,
 111    -7362,  -7299,  -7233,  -7164,  -7094,  -7021,  -6947,  -6870,
 112    -6791,  -6710,  -6627,  -6542,  -6455,  -6366,  -6275,  -6182,
 113    -6087,  -5991,  -5892,  -5792,  -5690,  -5586,  -5481,  -5374,
 114    -5265,  -5155,  -5043,  -4930,  -4815,  -4698,  -4580,  -4461,
 115    -4341,  -4219,  -4096,  -3971,  -3845,  -3719,  -3591,  -3462,
 116    -3331,  -3200,  -3068,  -2935,  -2801,  -2667,  -2531,  -2395,
 117    -2258,  -2120,  -1981,  -1842,  -1703,  -1563,  -1422,  -1281,
 118    -1140,   -998,   -856,   -713,   -571,   -428,   -285,   -142
 119 };
 120
 121 // Initialization table for echo channel in 8 kHz
 122 static const int16_t kChannelStored8kHz[PART_LEN1] = {
 123     2040,   1815,   1590,   1498,   1405,   1395,   1385,   1418,
 124     1451,   1506,   1562,   1644,   1726,   1804,   1882,   1918,
 125     1953,   1982,   2010,   2025,   2040,   2034,   2027,   2021,
 126     2014,   1997,   1980,   1925,   1869,   1800,   1732,   1683,
 127     1635,   1604,   1572,   1545,   1517,   1481,   1444,   1405,
 128     1367,   1331,   1294,   1270,   1245,   1239,   1233,   1247,
 129     1260,   1282,   1303,   1338,   1373,   1407,   1441,   1470,
 130     1499,   1524,   1549,   1565,   1582,   1601,   1621,   1649,
 131     1676
 132 };
 133
 134 // Initialization table for echo channel in 16 kHz
 135 static const int16_t kChannelStored16kHz[PART_LEN1] = {
 136     2040,   1590,   1405,   1385,   1451,   1562,   1726,   1882,
 137     1953,   2010,   2040,   2027,   2014,   1980,   1869,   1732,
 138     1635,   1572,   1517,   1444,   1367,   1294,   1245,   1233,
 139     1260,   1303,   1373,   1441,   1499,   1549,   1582,   1621,
 140     1676,   1741,   1802,   1861,   1921,   1983,   2040,   2102,
 141     2170,   2265,   2375,   2515,   2651,   2781,   2922,   3075,
 142     3253,   3471,   3738,   3976,   4151,   4258,   4308,   4288,
 143     4270,   4253,   4237,   4179,   4086,   3947,   3757,   3484,
 144     3153
 145 };
 146
 147 // Moves the pointer to the next entry and inserts |far_spectrum| and
 148 // corresponding Q-domain in its buffer.
 149 //
 150 // Inputs:
 151 //      - self          : Pointer to the delay estimation instance
 152 //      - far_spectrum  : Pointer to the far end spectrum
 153 //      - far_q         : Q-domain of far end spectrum
 154 //
 155 void WebRtcAecm_UpdateFarHistory(AecmCore_t* self,
 156                                  uint16_t* far_spectrum,
 157                                  int far_q) {
 158   // Get new buffer position
 159   self->far_history_pos++;
 160   if (self->far_history_pos >= MAX_DELAY) {
 161     self->far_history_pos = 0;
 162   }
 163   // Update Q-domain buffer
 164   self->far_q_domains[self->far_history_pos] = far_q;
 165   // Update far end spectrum buffer
 166   memcpy(&(self->far_history[self->far_history_pos * PART_LEN1]),
 167          far_spectrum,
 168          sizeof(uint16_t) * PART_LEN1);
 169 }
 170
 171 // Returns a pointer to the far end spectrum aligned to current near end
 172 // spectrum. The function WebRtc_DelayEstimatorProcessFix(...) should have been
 173 // called before AlignedFarend(...). Otherwise, you get the pointer to the
 174 // previous frame. The memory is only valid until the next call of
 175 // WebRtc_DelayEstimatorProcessFix(...).
 176 //
 177 // Inputs:
 178 //      - self              : Pointer to the AECM instance.
 179 //      - delay             : Current delay estimate.
 180 //
 181 // Output:
 182 //      - far_q             : The Q-domain of the aligned far end spectrum
 183 //
 184 // Return value:
 185 //      - far_spectrum      : Pointer to the aligned far end spectrum
 186 //                            NULL - Error
 187 //
 188 const uint16_t* WebRtcAecm_AlignedFarend(AecmCore_t* self,
 189                                          int* far_q,
 190                                          int delay) {
 191   int buffer_position = 0;
 192   assert(self != NULL);
 193   buffer_position = self->far_history_pos - delay;
 194
 195   // Check buffer position
 196   if (buffer_position < 0) {
 197     buffer_position += MAX_DELAY;
 198   }
 199   // Get Q-domain
 200   *far_q = self->far_q_domains[buffer_position];
 201   // Return far end spectrum
 202   return &(self->far_history[buffer_position * PART_LEN1]);
 203 }
 204
 205 // Declare function pointers.
 206 CalcLinearEnergies WebRtcAecm_CalcLinearEnergies;
 207 StoreAdaptiveChannel WebRtcAecm_StoreAdaptiveChannel;
 208 ResetAdaptiveChannel WebRtcAecm_ResetAdaptiveChannel;
 209
 210 int WebRtcAecm_CreateCore(AecmCore_t **aecmInst)
 211 {
 212     AecmCore_t *aecm = malloc(sizeof(AecmCore_t));
 213     *aecmInst = aecm;
 214     if (aecm == NULL)
 215     {
 216         return -1;
 217     }
 218
 219     aecm->farFrameBuf = WebRtc_CreateBuffer(FRAME_LEN + PART_LEN,
 220                                             sizeof(int16_t));
 221     if (!aecm->farFrameBuf)
 222     {
 223         WebRtcAecm_FreeCore(aecm);
 224         aecm = NULL;
 225         return -1;
 226     }
 227
 228     aecm->nearNoisyFrameBuf = WebRtc_CreateBuffer(FRAME_LEN + PART_LEN,
 229                                                   sizeof(int16_t));
 230     if (!aecm->nearNoisyFrameBuf)
 231     {
 232         WebRtcAecm_FreeCore(aecm);
 233         aecm = NULL;
 234         return -1;
 235     }
 236
 237     aecm->nearCleanFrameBuf = WebRtc_CreateBuffer(FRAME_LEN + PART_LEN,
 238                                                   sizeof(int16_t));
 239     if (!aecm->nearCleanFrameBuf)
 240     {
 241         WebRtcAecm_FreeCore(aecm);
 242         aecm = NULL;
 243         return -1;
 244     }
 245
 246     aecm->outFrameBuf = WebRtc_CreateBuffer(FRAME_LEN + PART_LEN,
 247                                             sizeof(int16_t));
 248     if (!aecm->outFrameBuf)
 249     {
 250         WebRtcAecm_FreeCore(aecm);
 251         aecm = NULL;
 252         return -1;
 253     }
 254
 255     aecm->delay_estimator_farend = WebRtc_CreateDelayEstimatorFarend(PART_LEN1,
 256                                                                      MAX_DELAY);
 257     if (aecm->delay_estimator_farend == NULL) {
 258       WebRtcAecm_FreeCore(aecm);
 259       aecm = NULL;
 260       return -1;
 261     }
 262     aecm->delay_estimator =
 263         WebRtc_CreateDelayEstimator(aecm->delay_estimator_farend, 0);
 264     if (aecm->delay_estimator == NULL) {
 265       WebRtcAecm_FreeCore(aecm);
 266       aecm = NULL;
 267       return -1;
 268     }
 269     // TODO(bjornv): Explicitly disable robust delay validation until no
 270     // performance regression has been established.  Then remove the line.
 271     WebRtc_enable_robust_validation(aecm->delay_estimator, 0);
 272
 273     aecm->real_fft = WebRtcSpl_CreateRealFFT(PART_LEN_SHIFT);
 274     if (aecm->real_fft == NULL) {
 275       WebRtcAecm_FreeCore(aecm);
 276       aecm = NULL;
 277       return -1;
 278     }
 279
 280     // Init some aecm pointers. 16 and 32 byte alignment is only necessary
 281     // for Neon code currently.
 282     aecm->xBuf = (int16_t*) (((uintptr_t)aecm->xBuf_buf + 31) & ~ 31);
 283     aecm->dBufClean = (int16_t*) (((uintptr_t)aecm->dBufClean_buf + 31) & ~ 31);
 284     aecm->dBufNoisy = (int16_t*) (((uintptr_t)aecm->dBufNoisy_buf + 31) & ~ 31);
 285     aecm->outBuf = (int16_t*) (((uintptr_t)aecm->outBuf_buf + 15) & ~ 15);
 286     aecm->channelStored = (int16_t*) (((uintptr_t)
 287                                              aecm->channelStored_buf + 15) & ~ 15);
 288     aecm->channelAdapt16 = (int16_t*) (((uintptr_t)
 289                                               aecm->channelAdapt16_buf + 15) & ~ 15);
 290     aecm->channelAdapt32 = (int32_t*) (((uintptr_t)
 291                                               aecm->channelAdapt32_buf + 31) & ~ 31);
 292
 293     return 0;
 294 }
 295
 296 void WebRtcAecm_InitEchoPathCore(AecmCore_t* aecm, const int16_t* echo_path)
 297 {
 298     int i = 0;
 299
 300     // Reset the stored channel
 301     memcpy(aecm->channelStored, echo_path, sizeof(int16_t) * PART_LEN1);
 302     // Reset the adapted channels
 303     memcpy(aecm->channelAdapt16, echo_path, sizeof(int16_t) * PART_LEN1);
 304     for (i = 0; i < PART_LEN1; i++)
 305     {
 306         aecm->channelAdapt32[i] = WEBRTC_SPL_LSHIFT_W32(
 307             (int32_t)(aecm->channelAdapt16[i]), 16);
 308     }
 309
 310     // Reset channel storing variables
 311     aecm->mseAdaptOld = 1000;
 312     aecm->mseStoredOld = 1000;
 313     aecm->mseThreshold = WEBRTC_SPL_WORD32_MAX;
 314     aecm->mseChannelCount = 0;
 315 }
 316
 317 static void CalcLinearEnergiesC(AecmCore_t* aecm,
 318                                 const uint16_t* far_spectrum,
 319                                 int32_t* echo_est,
 320                                 uint32_t* far_energy,
 321                                 uint32_t* echo_energy_adapt,
 322                                 uint32_t* echo_energy_stored)
 323 {
 324     int i;
 325
 326     // Get energy for the delayed far end signal and estimated
 327     // echo using both stored and adapted channels.
 328     for (i = 0; i < PART_LEN1; i++)
 329     {
 330         echo_est[i] = WEBRTC_SPL_MUL_16_U16(aecm->channelStored[i],
 331                                            far_spectrum[i]);
 332         (*far_energy) += (uint32_t)(far_spectrum[i]);
 333         (*echo_energy_adapt) += WEBRTC_SPL_UMUL_16_16(aecm->channelAdapt16[i],
 334                                           far_spectrum[i]);
 335         (*echo_energy_stored) += (uint32_t)echo_est[i];
 336     }
 337 }
 338
 339 static void StoreAdaptiveChannelC(AecmCore_t* aecm,
 340                                   const uint16_t* far_spectrum,
 341                                   int32_t* echo_est)
 342 {
 343     int i;
 344
 345     // During startup we store the channel every block.
 346     memcpy(aecm->channelStored, aecm->channelAdapt16, sizeof(int16_t) * PART_LEN1);
 347     // Recalculate echo estimate
 348     for (i = 0; i < PART_LEN; i += 4)
 349     {
 350         echo_est[i] = WEBRTC_SPL_MUL_16_U16(aecm->channelStored[i],
 351                                            far_spectrum[i]);
 352         echo_est[i + 1] = WEBRTC_SPL_MUL_16_U16(aecm->channelStored[i + 1],
 353                                            far_spectrum[i + 1]);
 354         echo_est[i + 2] = WEBRTC_SPL_MUL_16_U16(aecm->channelStored[i + 2],
 355                                            far_spectrum[i + 2]);
 356         echo_est[i + 3] = WEBRTC_SPL_MUL_16_U16(aecm->channelStored[i + 3],
 357                                            far_spectrum[i + 3]);
 358     }
 359     echo_est[i] = WEBRTC_SPL_MUL_16_U16(aecm->channelStored[i],
 360                                        far_spectrum[i]);
 361 }
 362
 363 static void ResetAdaptiveChannelC(AecmCore_t* aecm)
 364 {
 365     int i;
 366
 367     // The stored channel has a significantly lower MSE than the adaptive one for
 368     // two consecutive calculations. Reset the adaptive channel.
 369     memcpy(aecm->channelAdapt16, aecm->channelStored,
 370            sizeof(int16_t) * PART_LEN1);
 371     // Restore the W32 channel
 372     for (i = 0; i < PART_LEN; i += 4)
 373     {
 374         aecm->channelAdapt32[i] = WEBRTC_SPL_LSHIFT_W32(
 375                 (int32_t)aecm->channelStored[i], 16);
 376         aecm->channelAdapt32[i + 1] = WEBRTC_SPL_LSHIFT_W32(
 377                 (int32_t)aecm->channelStored[i + 1], 16);
 378         aecm->channelAdapt32[i + 2] = WEBRTC_SPL_LSHIFT_W32(
 379                 (int32_t)aecm->channelStored[i + 2], 16);
 380         aecm->channelAdapt32[i + 3] = WEBRTC_SPL_LSHIFT_W32(
 381                 (int32_t)aecm->channelStored[i + 3], 16);
 382     }
 383     aecm->channelAdapt32[i] = WEBRTC_SPL_LSHIFT_W32((int32_t)aecm->channelStored[i], 16);
 384 }
 385
 386 // Initialize function pointers for ARM Neon platform.
 387 #if (defined WEBRTC_DETECT_ARM_NEON || defined WEBRTC_ARCH_ARM_NEON)
 388 static void WebRtcAecm_InitNeon(void)
 389 {
 390   WebRtcAecm_StoreAdaptiveChannel = WebRtcAecm_StoreAdaptiveChannelNeon;
 391   WebRtcAecm_ResetAdaptiveChannel = WebRtcAecm_ResetAdaptiveChannelNeon;
 392   WebRtcAecm_CalcLinearEnergies = WebRtcAecm_CalcLinearEnergiesNeon;
 393 }
 394 #endif
 395
 396 // Initialize function pointers for MIPS platform.
 397 #if defined(MIPS32_LE)
 398 static void WebRtcAecm_InitMips(void)
 399 {
 400 #if defined(MIPS_DSP_R1_LE)
 401   WebRtcAecm_StoreAdaptiveChannel = WebRtcAecm_StoreAdaptiveChannel_mips;
 402   WebRtcAecm_ResetAdaptiveChannel = WebRtcAecm_ResetAdaptiveChannel_mips;
 403 #endif
 404   WebRtcAecm_CalcLinearEnergies = WebRtcAecm_CalcLinearEnergies_mips;
 405 }
 406 #endif
 407
 408 // WebRtcAecm_InitCore(...)
 409 //
 410 // This function initializes the AECM instant created with WebRtcAecm_CreateCore(...)
 411 // Input:
 412 //      - aecm            : Pointer to the Echo Suppression instance
 413 //      - samplingFreq   : Sampling Frequency
 414 //
 415 // Output:
 416 //      - aecm            : Initialized instance
 417 //
 418 // Return value         :  0 - Ok
 419 //                        -1 - Error
 420 //
 421 int WebRtcAecm_InitCore(AecmCore_t * const aecm, int samplingFreq)
 422 {
 423     int i = 0;
 424     int32_t tmp32 = PART_LEN1 * PART_LEN1;
 425     int16_t tmp16 = PART_LEN1;
 426
 427     if (samplingFreq != 8000 && samplingFreq != 16000)
 428     {
 429         samplingFreq = 8000;
 430         return -1;
 431     }
 432     // sanity check of sampling frequency
 433     aecm->mult = (int16_t)samplingFreq / 8000;
 434
 435     aecm->farBufWritePos = 0;
 436     aecm->farBufReadPos = 0;
 437     aecm->knownDelay = 0;
 438     aecm->lastKnownDelay = 0;
 439
 440     WebRtc_InitBuffer(aecm->farFrameBuf);
 441     WebRtc_InitBuffer(aecm->nearNoisyFrameBuf);
 442     WebRtc_InitBuffer(aecm->nearCleanFrameBuf);
 443     WebRtc_InitBuffer(aecm->outFrameBuf);
 444
 445     memset(aecm->xBuf_buf, 0, sizeof(aecm->xBuf_buf));
 446     memset(aecm->dBufClean_buf, 0, sizeof(aecm->dBufClean_buf));
 447     memset(aecm->dBufNoisy_buf, 0, sizeof(aecm->dBufNoisy_buf));
 448     memset(aecm->outBuf_buf, 0, sizeof(aecm->outBuf_buf));
 449
 450     aecm->seed = 666;
 451     aecm->totCount = 0;
 452
 453     if (WebRtc_InitDelayEstimatorFarend(aecm->delay_estimator_farend) != 0) {
 454       return -1;
 455     }
 456     if (WebRtc_InitDelayEstimator(aecm->delay_estimator) != 0) {
 457       return -1;
 458     }
 459     // Set far end histories to zero
 460     memset(aecm->far_history, 0, sizeof(uint16_t) * PART_LEN1 * MAX_DELAY);
 461     memset(aecm->far_q_domains, 0, sizeof(int) * MAX_DELAY);
 462     aecm->far_history_pos = MAX_DELAY;
 463
 464     aecm->nlpFlag = 1;
 465     aecm->fixedDelay = -1;
 466
 467     aecm->dfaCleanQDomain = 0;
 468     aecm->dfaCleanQDomainOld = 0;
 469     aecm->dfaNoisyQDomain = 0;
 470     aecm->dfaNoisyQDomainOld = 0;
 471
 472     memset(aecm->nearLogEnergy, 0, sizeof(aecm->nearLogEnergy));
 473     aecm->farLogEnergy = 0;
 474     memset(aecm->echoAdaptLogEnergy, 0, sizeof(aecm->echoAdaptLogEnergy));
 475     memset(aecm->echoStoredLogEnergy, 0, sizeof(aecm->echoStoredLogEnergy));
 476
 477     // Initialize the echo channels with a stored shape.
 478     if (samplingFreq == 8000)
 479     {
 480         WebRtcAecm_InitEchoPathCore(aecm, kChannelStored8kHz);
 481     }
 482     else
 483     {
 484         WebRtcAecm_InitEchoPathCore(aecm, kChannelStored16kHz);
 485     }
 486
 487     memset(aecm->echoFilt, 0, sizeof(aecm->echoFilt));
 488     memset(aecm->nearFilt, 0, sizeof(aecm->nearFilt));
 489     aecm->noiseEstCtr = 0;
 490
 491     aecm->cngMode = AecmTrue;
 492
 493     memset(aecm->noiseEstTooLowCtr, 0, sizeof(aecm->noiseEstTooLowCtr));
 494     memset(aecm->noiseEstTooHighCtr, 0, sizeof(aecm->noiseEstTooHighCtr));
 495     // Shape the initial noise level to an approximate pink noise.
 496     for (i = 0; i < (PART_LEN1 >> 1) - 1; i++)
 497     {
 498         aecm->noiseEst[i] = (tmp32 << 8);
 499         tmp16--;
 500         tmp32 -= (int32_t)((tmp16 << 1) + 1);
 501     }
 502     for (; i < PART_LEN1; i++)
 503     {
 504         aecm->noiseEst[i] = (tmp32 << 8);
 505     }
 506
 507     aecm->farEnergyMin = WEBRTC_SPL_WORD16_MAX;
 508     aecm->farEnergyMax = WEBRTC_SPL_WORD16_MIN;
 509     aecm->farEnergyMaxMin = 0;
 510     aecm->farEnergyVAD = FAR_ENERGY_MIN; // This prevents false speech detection at the
 511                                          // beginning.
 512     aecm->farEnergyMSE = 0;
 513     aecm->currentVADValue = 0;
 514     aecm->vadUpdateCount = 0;
 515     aecm->firstVAD = 1;
 516
 517     aecm->startupState = 0;
 518     aecm->supGain = SUPGAIN_DEFAULT;
 519     aecm->supGainOld = SUPGAIN_DEFAULT;
 520
 521     aecm->supGainErrParamA = SUPGAIN_ERROR_PARAM_A;
 522     aecm->supGainErrParamD = SUPGAIN_ERROR_PARAM_D;
 523     aecm->supGainErrParamDiffAB = SUPGAIN_ERROR_PARAM_A - SUPGAIN_ERROR_PARAM_B;
 524     aecm->supGainErrParamDiffBD = SUPGAIN_ERROR_PARAM_B - SUPGAIN_ERROR_PARAM_D;
 525
 526     // Assert a preprocessor definition at compile-time. It's an assumption
 527     // used in assembly code, so check the assembly files before any change.
 528     COMPILE_ASSERT(PART_LEN % 16 == 0);
 529
 530     // Initialize function pointers.
 531     WebRtcAecm_CalcLinearEnergies = CalcLinearEnergiesC;
 532     WebRtcAecm_StoreAdaptiveChannel = StoreAdaptiveChannelC;
 533     WebRtcAecm_ResetAdaptiveChannel = ResetAdaptiveChannelC;
 534
 535 #ifdef WEBRTC_DETECT_ARM_NEON
 536     uint64_t features = WebRtc_GetCPUFeaturesARM();
 537     if ((features & kCPUFeatureNEON) != 0)
 538     {
 539       WebRtcAecm_InitNeon();
 540     }
 541 #elif defined(WEBRTC_ARCH_ARM_NEON)
 542     WebRtcAecm_InitNeon();
 543 #endif
 544
 545 #if defined(MIPS32_LE)
 546     WebRtcAecm_InitMips();
 547 #endif
 548     return 0;
 549 }
 550
 551 // TODO(bjornv): This function is currently not used. Add support for these
 552 // parameters from a higher level
 553 int WebRtcAecm_Control(AecmCore_t *aecm, int delay, int nlpFlag)
 554 {
 555     aecm->nlpFlag = nlpFlag;
 556     aecm->fixedDelay = delay;
 557
 558     return 0;
 559 }
 560
 561 int WebRtcAecm_FreeCore(AecmCore_t *aecm)
 562 {
 563     if (aecm == NULL)
 564     {
 565         return -1;
 566     }
 567
 568     WebRtc_FreeBuffer(aecm->farFrameBuf);
 569     WebRtc_FreeBuffer(aecm->nearNoisyFrameBuf);
 570     WebRtc_FreeBuffer(aecm->nearCleanFrameBuf);
 571     WebRtc_FreeBuffer(aecm->outFrameBuf);
 572
 573     WebRtc_FreeDelayEstimator(aecm->delay_estimator);
 574     WebRtc_FreeDelayEstimatorFarend(aecm->delay_estimator_farend);
 575     WebRtcSpl_FreeRealFFT(aecm->real_fft);
 576
 577     free(aecm);
 578
 579     return 0;
 580 }
 581
 582 int WebRtcAecm_ProcessFrame(AecmCore_t * aecm,
 583                             const int16_t * farend,
 584                             const int16_t * nearendNoisy,
 585                             const int16_t * nearendClean,
 586                             int16_t * out)
 587 {
 588     int16_t outBlock_buf[PART_LEN + 8]; // Align buffer to 8-byte boundary.
 589     int16_t* outBlock = (int16_t*) (((uintptr_t) outBlock_buf + 15) & ~ 15);
 590
 591     int16_t farFrame[FRAME_LEN];
 592     const int16_t* out_ptr = NULL;
 593     int size = 0;
 594
 595     // Buffer the current frame.
 596     // Fetch an older one corresponding to the delay.
 597     WebRtcAecm_BufferFarFrame(aecm, farend, FRAME_LEN);
 598     WebRtcAecm_FetchFarFrame(aecm, farFrame, FRAME_LEN, aecm->knownDelay);
 599
 600     // Buffer the synchronized far and near frames,
 601     // to pass the smaller blocks individually.
 602     WebRtc_WriteBuffer(aecm->farFrameBuf, farFrame, FRAME_LEN);
 603     WebRtc_WriteBuffer(aecm->nearNoisyFrameBuf, nearendNoisy, FRAME_LEN);
 604     if (nearendClean != NULL)
 605     {
 606         WebRtc_WriteBuffer(aecm->nearCleanFrameBuf, nearendClean, FRAME_LEN);
 607     }
 608
 609     // Process as many blocks as possible.
 610     while (WebRtc_available_read(aecm->farFrameBuf) >= PART_LEN)
 611     {
 612         int16_t far_block[PART_LEN];
 613         const int16_t* far_block_ptr = NULL;
 614         int16_t near_noisy_block[PART_LEN];
 615         const int16_t* near_noisy_block_ptr = NULL;
 616
 617         WebRtc_ReadBuffer(aecm->farFrameBuf, (void**) &far_block_ptr, far_block,
 618                           PART_LEN);
 619         WebRtc_ReadBuffer(aecm->nearNoisyFrameBuf,
 620                           (void**) &near_noisy_block_ptr,
 621                           near_noisy_block,
 622                           PART_LEN);
 623         if (nearendClean != NULL)
 624         {
 625             int16_t near_clean_block[PART_LEN];
 626             const int16_t* near_clean_block_ptr = NULL;
 627
 628             WebRtc_ReadBuffer(aecm->nearCleanFrameBuf,
 629                               (void**) &near_clean_block_ptr,
 630                               near_clean_block,
 631                               PART_LEN);
 632             if (WebRtcAecm_ProcessBlock(aecm,
 633                                         far_block_ptr,
 634                                         near_noisy_block_ptr,
 635                                         near_clean_block_ptr,
 636                                         outBlock) == -1)
 637             {
 638                 return -1;
 639             }
 640         } else
 641         {
 642             if (WebRtcAecm_ProcessBlock(aecm,
 643                                         far_block_ptr,
 644                                         near_noisy_block_ptr,
 645                                         NULL,
 646                                         outBlock) == -1)
 647             {
 648                 return -1;
 649             }
 650         }
 651
 652         WebRtc_WriteBuffer(aecm->outFrameBuf, outBlock, PART_LEN);
 653     }
 654
 655     // Stuff the out buffer if we have less than a frame to output.
 656     // This should only happen for the first frame.
 657     size = (int) WebRtc_available_read(aecm->outFrameBuf);
 658     if (size < FRAME_LEN)
 659     {
 660         WebRtc_MoveReadPtr(aecm->outFrameBuf, size - FRAME_LEN);
 661     }
 662
 663     // Obtain an output frame.
 664     WebRtc_ReadBuffer(aecm->outFrameBuf, (void**) &out_ptr, out, FRAME_LEN);
 665     if (out_ptr != out) {
 666       // ReadBuffer() hasn't copied to |out| in this case.
 667       memcpy(out, out_ptr, FRAME_LEN * sizeof(int16_t));
 668     }
 669
 670     return 0;
 671 }
 672
 673 // WebRtcAecm_AsymFilt(...)
 674 //
 675 // Performs asymmetric filtering.
 676 //
 677 // Inputs:
 678 //      - filtOld       : Previous filtered value.
 679 //      - inVal         : New input value.
 680 //      - stepSizePos   : Step size when we have a positive contribution.
 681 //      - stepSizeNeg   : Step size when we have a negative contribution.
 682 //
 683 // Output:
 684 //
 685 // Return: - Filtered value.
 686 //
 687 int16_t WebRtcAecm_AsymFilt(const int16_t filtOld, const int16_t inVal,
 688                             const int16_t stepSizePos,
 689                             const int16_t stepSizeNeg)
 690 {
 691     int16_t retVal;
 692
 693     if ((filtOld == WEBRTC_SPL_WORD16_MAX) | (filtOld == WEBRTC_SPL_WORD16_MIN))
 694     {
 695         return inVal;
 696     }
 697     retVal = filtOld;
 698     if (filtOld > inVal)
 699     {
 700         retVal -= WEBRTC_SPL_RSHIFT_W16(filtOld - inVal, stepSizeNeg);
 701     } else
 702     {
 703         retVal += WEBRTC_SPL_RSHIFT_W16(inVal - filtOld, stepSizePos);
 704     }
 705
 706     return retVal;
 707 }
 708
 709 // WebRtcAecm_CalcEnergies(...)
 710 //
 711 // This function calculates the log of energies for nearend, farend and estimated
 712 // echoes. There is also an update of energy decision levels, i.e. internal VAD.
 713 //
 714 //
 715 // @param  aecm         [i/o]   Handle of the AECM instance.
 716 // @param  far_spectrum [in]    Pointer to farend spectrum.
 717 // @param  far_q        [in]    Q-domain of farend spectrum.
 718 // @param  nearEner     [in]    Near end energy for current block in
 719 //                              Q(aecm->dfaQDomain).
 720 // @param  echoEst      [out]   Estimated echo in Q(xfa_q+RESOLUTION_CHANNEL16).
 721 //
 722 void WebRtcAecm_CalcEnergies(AecmCore_t * aecm,
 723                              const uint16_t* far_spectrum,
 724                              const int16_t far_q,
 725                              const uint32_t nearEner,
 726                              int32_t * echoEst)
 727 {
 728     // Local variables
 729     uint32_t tmpAdapt = 0;
 730     uint32_t tmpStored = 0;
 731     uint32_t tmpFar = 0;
 732
 733     int i;
 734
 735     int16_t zeros, frac;
 736     int16_t tmp16;
 737     int16_t increase_max_shifts = 4;
 738     int16_t decrease_max_shifts = 11;
 739     int16_t increase_min_shifts = 11;
 740     int16_t decrease_min_shifts = 3;
 741     int16_t kLogLowValue = WEBRTC_SPL_LSHIFT_W16(PART_LEN_SHIFT, 7);
 742
 743     // Get log of near end energy and store in buffer
 744
 745     // Shift buffer
 746     memmove(aecm->nearLogEnergy + 1, aecm->nearLogEnergy,
 747             sizeof(int16_t) * (MAX_BUF_LEN - 1));
 748
 749     // Logarithm of integrated magnitude spectrum (nearEner)
 750     tmp16 = kLogLowValue;
 751     if (nearEner)
 752     {
 753         zeros = WebRtcSpl_NormU32(nearEner);
 754         frac = (int16_t)WEBRTC_SPL_RSHIFT_U32(
 755                               (WEBRTC_SPL_LSHIFT_U32(nearEner, zeros) & 0x7FFFFFFF),
 756                               23);
 757         // log2 in Q8
 758         tmp16 += WEBRTC_SPL_LSHIFT_W16((31 - zeros), 8) + frac;
 759         tmp16 -= WEBRTC_SPL_LSHIFT_W16(aecm->dfaNoisyQDomain, 8);
 760     }
 761     aecm->nearLogEnergy[0] = tmp16;
 762     // END: Get log of near end energy
 763
 764     WebRtcAecm_CalcLinearEnergies(aecm, far_spectrum, echoEst, &tmpFar, &tmpAdapt, &tmpStored);
 765
 766     // Shift buffers
 767     memmove(aecm->echoAdaptLogEnergy + 1, aecm->echoAdaptLogEnergy,
 768             sizeof(int16_t) * (MAX_BUF_LEN - 1));
 769     memmove(aecm->echoStoredLogEnergy + 1, aecm->echoStoredLogEnergy,
 770             sizeof(int16_t) * (MAX_BUF_LEN - 1));
 771
 772     // Logarithm of delayed far end energy
 773     tmp16 = kLogLowValue;
 774     if (tmpFar)
 775     {
 776         zeros = WebRtcSpl_NormU32(tmpFar);
 777         frac = (int16_t)WEBRTC_SPL_RSHIFT_U32((WEBRTC_SPL_LSHIFT_U32(tmpFar, zeros)
 778                         & 0x7FFFFFFF), 23);
 779         // log2 in Q8
 780         tmp16 += WEBRTC_SPL_LSHIFT_W16((31 - zeros), 8) + frac;
 781         tmp16 -= WEBRTC_SPL_LSHIFT_W16(far_q, 8);
 782     }
 783     aecm->farLogEnergy = tmp16;
 784
 785     // Logarithm of estimated echo energy through adapted channel
 786     tmp16 = kLogLowValue;
 787     if (tmpAdapt)
 788     {
 789         zeros = WebRtcSpl_NormU32(tmpAdapt);
 790         frac = (int16_t)WEBRTC_SPL_RSHIFT_U32((WEBRTC_SPL_LSHIFT_U32(tmpAdapt, zeros)
 791                         & 0x7FFFFFFF), 23);
 792         //log2 in Q8
 793         tmp16 += WEBRTC_SPL_LSHIFT_W16((31 - zeros), 8) + frac;
 794         tmp16 -= WEBRTC_SPL_LSHIFT_W16(RESOLUTION_CHANNEL16 + far_q, 8);
 795     }
 796     aecm->echoAdaptLogEnergy[0] = tmp16;
 797
 798     // Logarithm of estimated echo energy through stored channel
 799     tmp16 = kLogLowValue;
 800     if (tmpStored)
 801     {
 802         zeros = WebRtcSpl_NormU32(tmpStored);
 803         frac = (int16_t)WEBRTC_SPL_RSHIFT_U32((WEBRTC_SPL_LSHIFT_U32(tmpStored, zeros)
 804                         & 0x7FFFFFFF), 23);
 805         //log2 in Q8
 806         tmp16 += WEBRTC_SPL_LSHIFT_W16((31 - zeros), 8) + frac;
 807         tmp16 -= WEBRTC_SPL_LSHIFT_W16(RESOLUTION_CHANNEL16 + far_q, 8);
 808     }
 809     aecm->echoStoredLogEnergy[0] = tmp16;
 810
 811     // Update farend energy levels (min, max, vad, mse)
 812     if (aecm->farLogEnergy > FAR_ENERGY_MIN)
 813     {
 814         if (aecm->startupState == 0)
 815         {
 816             increase_max_shifts = 2;
 817             decrease_min_shifts = 2;
 818             increase_min_shifts = 8;
 819         }
 820
 821         aecm->farEnergyMin = WebRtcAecm_AsymFilt(aecm->farEnergyMin, aecm->farLogEnergy,
 822                                                  increase_min_shifts, decrease_min_shifts);
 823         aecm->farEnergyMax = WebRtcAecm_AsymFilt(aecm->farEnergyMax, aecm->farLogEnergy,
 824                                                  increase_max_shifts, decrease_max_shifts);
 825         aecm->farEnergyMaxMin = (aecm->farEnergyMax - aecm->farEnergyMin);
 826
 827         // Dynamic VAD region size
 828         tmp16 = 2560 - aecm->farEnergyMin;
 829         if (tmp16 > 0)
 830         {
 831             tmp16 = (int16_t)WEBRTC_SPL_MUL_16_16_RSFT(tmp16, FAR_ENERGY_VAD_REGION, 9);
 832         } else
 833         {
 834             tmp16 = 0;
 835         }
 836         tmp16 += FAR_ENERGY_VAD_REGION;
 837
 838         if ((aecm->startupState == 0) | (aecm->vadUpdateCount > 1024))
 839         {
 840             // In startup phase or VAD update halted
 841             aecm->farEnergyVAD = aecm->farEnergyMin + tmp16;
 842         } else
 843         {
 844             if (aecm->farEnergyVAD > aecm->farLogEnergy)
 845             {
 846                 aecm->farEnergyVAD += WEBRTC_SPL_RSHIFT_W16(aecm->farLogEnergy +
 847                                                             tmp16 -
 848                                                             aecm->farEnergyVAD,
 849                                                             6);
 850                 aecm->vadUpdateCount = 0;
 851             } else
 852             {
 853                 aecm->vadUpdateCount++;
 854             }
 855         }
 856         // Put MSE threshold higher than VAD
 857         aecm->farEnergyMSE = aecm->farEnergyVAD + (1 << 8);
 858     }
 859
 860     // Update VAD variables
 861     if (aecm->farLogEnergy > aecm->farEnergyVAD)
 862     {
 863         if ((aecm->startupState == 0) | (aecm->farEnergyMaxMin > FAR_ENERGY_DIFF))
 864         {
 865             // We are in startup or have significant dynamics in input speech level
 866             aecm->currentVADValue = 1;
 867         }
 868     } else
 869     {
 870         aecm->currentVADValue = 0;
 871     }
 872     if ((aecm->currentVADValue) && (aecm->firstVAD))
 873     {
 874         aecm->firstVAD = 0;
 875         if (aecm->echoAdaptLogEnergy[0] > aecm->nearLogEnergy[0])
 876         {
 877             // The estimated echo has higher energy than the near end signal.
 878             // This means that the initialization was too aggressive. Scale
 879             // down by a factor 8
 880             for (i = 0; i < PART_LEN1; i++)
 881             {
 882                 aecm->channelAdapt16[i] >>= 3;
 883             }
 884             // Compensate the adapted echo energy level accordingly.
 885             aecm->echoAdaptLogEnergy[0] -= (3 << 8);
 886             aecm->firstVAD = 1;
 887         }
 888     }
 889 }
 890
 891 // WebRtcAecm_CalcStepSize(...)
 892 //
 893 // This function calculates the step size used in channel estimation
 894 //
 895 //
 896 // @param  aecm  [in]    Handle of the AECM instance.
 897 // @param  mu    [out]   (Return value) Stepsize in log2(), i.e. number of shifts.
 898 //
 899 //
 900 int16_t WebRtcAecm_CalcStepSize(AecmCore_t * const aecm)
 901 {
 902
 903     int32_t tmp32;
 904     int16_t tmp16;
 905     int16_t mu = MU_MAX;
 906
 907     // Here we calculate the step size mu used in the
 908     // following NLMS based Channel estimation algorithm
 909     if (!aecm->currentVADValue)
 910     {
 911         // Far end energy level too low, no channel update
 912         mu = 0;
 913     } else if (aecm->startupState > 0)
 914     {
 915         if (aecm->farEnergyMin >= aecm->farEnergyMax)
 916         {
 917             mu = MU_MIN;
 918         } else
 919         {
 920             tmp16 = (aecm->farLogEnergy - aecm->farEnergyMin);
 921             tmp32 = WEBRTC_SPL_MUL_16_16(tmp16, MU_DIFF);
 922             tmp32 = WebRtcSpl_DivW32W16(tmp32, aecm->farEnergyMaxMin);
 923             mu = MU_MIN - 1 - (int16_t)(tmp32);
 924             // The -1 is an alternative to rounding. This way we get a larger
 925             // stepsize, so we in some sense compensate for truncation in NLMS
 926         }
 927         if (mu < MU_MAX)
 928         {
 929             mu = MU_MAX; // Equivalent with maximum step size of 2^-MU_MAX
 930         }
 931     }
 932
 933     return mu;
 934 }
 935
 936 // WebRtcAecm_UpdateChannel(...)
 937 //
 938 // This function performs channel estimation. NLMS and decision on channel storage.
 939 //
 940 //
 941 // @param  aecm         [i/o]   Handle of the AECM instance.
 942 // @param  far_spectrum [in]    Absolute value of the farend signal in Q(far_q)
 943 // @param  far_q        [in]    Q-domain of the farend signal
 944 // @param  dfa          [in]    Absolute value of the nearend signal (Q[aecm->dfaQDomain])
 945 // @param  mu           [in]    NLMS step size.
 946 // @param  echoEst      [i/o]   Estimated echo in Q(far_q+RESOLUTION_CHANNEL16).
 947 //
 948 void WebRtcAecm_UpdateChannel(AecmCore_t * aecm,
 949                               const uint16_t* far_spectrum,
 950                               const int16_t far_q,
 951                               const uint16_t * const dfa,
 952                               const int16_t mu,
 953                               int32_t * echoEst)
 954 {
 955
 956     uint32_t tmpU32no1, tmpU32no2;
 957     int32_t tmp32no1, tmp32no2;
 958     int32_t mseStored;
 959     int32_t mseAdapt;
 960
 961     int i;
 962
 963     int16_t zerosFar, zerosNum, zerosCh, zerosDfa;
 964     int16_t shiftChFar, shiftNum, shift2ResChan;
 965     int16_t tmp16no1;
 966     int16_t xfaQ, dfaQ;
 967
 968     // This is the channel estimation algorithm. It is base on NLMS but has a variable step
 969     // length, which was calculated above.
 970     if (mu)
 971     {
 972         for (i = 0; i < PART_LEN1; i++)
 973         {
 974             // Determine norm of channel and farend to make sure we don't get overflow in
 975             // multiplication
 976             zerosCh = WebRtcSpl_NormU32(aecm->channelAdapt32[i]);
 977             zerosFar = WebRtcSpl_NormU32((uint32_t)far_spectrum[i]);
 978             if (zerosCh + zerosFar > 31)
 979             {
 980                 // Multiplication is safe
 981                 tmpU32no1 = WEBRTC_SPL_UMUL_32_16(aecm->channelAdapt32[i],
 982                         far_spectrum[i]);
 983                 shiftChFar = 0;
 984             } else
 985             {
 986                 // We need to shift down before multiplication
 987                 shiftChFar = 32 - zerosCh - zerosFar;
 988                 tmpU32no1 = WEBRTC_SPL_UMUL_32_16(
 989                     WEBRTC_SPL_RSHIFT_W32(aecm->channelAdapt32[i], shiftChFar),
 990                     far_spectrum[i]);
 991             }
 992             // Determine Q-domain of numerator
 993             zerosNum = WebRtcSpl_NormU32(tmpU32no1);
 994             if (dfa[i])
 995             {
 996                 zerosDfa = WebRtcSpl_NormU32((uint32_t)dfa[i]);
 997             } else
 998             {
 999                 zerosDfa = 32;
1000             }
1001             tmp16no1 = zerosDfa - 2 + aecm->dfaNoisyQDomain -
1002                 RESOLUTION_CHANNEL32 - far_q + shiftChFar;
1003             if (zerosNum > tmp16no1 + 1)
1004             {
1005                 xfaQ = tmp16no1;
1006                 dfaQ = zerosDfa - 2;
1007             } else
1008             {
1009                 xfaQ = zerosNum - 2;
1010                 dfaQ = RESOLUTION_CHANNEL32 + far_q - aecm->dfaNoisyQDomain -
1011                     shiftChFar + xfaQ;
1012             }
1013             // Add in the same Q-domain
1014             tmpU32no1 = WEBRTC_SPL_SHIFT_W32(tmpU32no1, xfaQ);
1015             tmpU32no2 = WEBRTC_SPL_SHIFT_W32((uint32_t)dfa[i], dfaQ);
1016             tmp32no1 = (int32_t)tmpU32no2 - (int32_t)tmpU32no1;
1017             zerosNum = WebRtcSpl_NormW32(tmp32no1);
1018             if ((tmp32no1) && (far_spectrum[i] > (CHANNEL_VAD << far_q)))
1019             {
1020                 //
1021                 // Update is needed
1022                 //
1023                 // This is what we would like to compute
1024                 //
1025                 // tmp32no1 = dfa[i] - (aecm->channelAdapt[i] * far_spectrum[i])
1026                 // tmp32norm = (i + 1)
1027                 // aecm->channelAdapt[i] += (2^mu) * tmp32no1
1028                 //                        / (tmp32norm * far_spectrum[i])
1029                 //
1030
1031                 // Make sure we don't get overflow in multiplication.
1032                 if (zerosNum + zerosFar > 31)
1033                 {
1034                     if (tmp32no1 > 0)
1035                     {
1036                         tmp32no2 = (int32_t)WEBRTC_SPL_UMUL_32_16(tmp32no1,
1037                                                                         far_spectrum[i]);
1038                     } else
1039                     {
1040                         tmp32no2 = -(int32_t)WEBRTC_SPL_UMUL_32_16(-tmp32no1,
1041                                                                          far_spectrum[i]);
1042                     }
1043                     shiftNum = 0;
1044                 } else
1045                 {
1046                     shiftNum = 32 - (zerosNum + zerosFar);
1047                     if (tmp32no1 > 0)
1048                     {
1049                         tmp32no2 = (int32_t)WEBRTC_SPL_UMUL_32_16(
1050                                 WEBRTC_SPL_RSHIFT_W32(tmp32no1, shiftNum),
1051                                 far_spectrum[i]);
1052                     } else
1053                     {
1054                         tmp32no2 = -(int32_t)WEBRTC_SPL_UMUL_32_16(
1055                                 WEBRTC_SPL_RSHIFT_W32(-tmp32no1, shiftNum),
1056                                 far_spectrum[i]);
1057                     }
1058                 }
1059                 // Normalize with respect to frequency bin
1060                 tmp32no2 = WebRtcSpl_DivW32W16(tmp32no2, i + 1);
1061                 // Make sure we are in the right Q-domain
1062                 shift2ResChan = shiftNum + shiftChFar - xfaQ - mu - ((30 - zerosFar) << 1);
1063                 if (WebRtcSpl_NormW32(tmp32no2) < shift2ResChan)
1064                 {
1065                     tmp32no2 = WEBRTC_SPL_WORD32_MAX;
1066                 } else
1067                 {
1068                     tmp32no2 = WEBRTC_SPL_SHIFT_W32(tmp32no2, shift2ResChan);
1069                 }
1070                 aecm->channelAdapt32[i] = WEBRTC_SPL_ADD_SAT_W32(aecm->channelAdapt32[i],
1071                         tmp32no2);
1072                 if (aecm->channelAdapt32[i] < 0)
1073                 {
1074                     // We can never have negative channel gain
1075                     aecm->channelAdapt32[i] = 0;
1076                 }
1077                 aecm->channelAdapt16[i]
1078                         = (int16_t)WEBRTC_SPL_RSHIFT_W32(aecm->channelAdapt32[i], 16);
1079             }
1080         }
1081     }
1082     // END: Adaptive channel update
1083
1084     // Determine if we should store or restore the channel
1085     if ((aecm->startupState == 0) & (aecm->currentVADValue))
1086     {
1087         // During startup we store the channel every block,
1088         // and we recalculate echo estimate
1089         WebRtcAecm_StoreAdaptiveChannel(aecm, far_spectrum, echoEst);
1090     } else
1091     {
1092         if (aecm->farLogEnergy < aecm->farEnergyMSE)
1093         {
1094             aecm->mseChannelCount = 0;
1095         } else
1096         {
1097             aecm->mseChannelCount++;
1098         }
1099         // Enough data for validation. Store channel if we can.
1100         if (aecm->mseChannelCount >= (MIN_MSE_COUNT + 10))
1101         {
1102             // We have enough data.
1103             // Calculate MSE of "Adapt" and "Stored" versions.
1104             // It is actually not MSE, but average absolute error.
1105             mseStored = 0;
1106             mseAdapt = 0;
1107             for (i = 0; i < MIN_MSE_COUNT; i++)
1108             {
1109                 tmp32no1 = ((int32_t)aecm->echoStoredLogEnergy[i]
1110                         - (int32_t)aecm->nearLogEnergy[i]);
1111                 tmp32no2 = WEBRTC_SPL_ABS_W32(tmp32no1);
1112                 mseStored += tmp32no2;
1113
1114                 tmp32no1 = ((int32_t)aecm->echoAdaptLogEnergy[i]
1115                         - (int32_t)aecm->nearLogEnergy[i]);
1116                 tmp32no2 = WEBRTC_SPL_ABS_W32(tmp32no1);
1117                 mseAdapt += tmp32no2;
1118             }
1119             if (((mseStored << MSE_RESOLUTION) < (MIN_MSE_DIFF * mseAdapt))
1120                     & ((aecm->mseStoredOld << MSE_RESOLUTION) < (MIN_MSE_DIFF
1121                             * aecm->mseAdaptOld)))
1122             {
1123                 // The stored channel has a significantly lower MSE than the adaptive one for
1124                 // two consecutive calculations. Reset the adaptive channel.
1125                 WebRtcAecm_ResetAdaptiveChannel(aecm);
1126             } else if (((MIN_MSE_DIFF * mseStored) > (mseAdapt << MSE_RESOLUTION)) & (mseAdapt
1127                     < aecm->mseThreshold) & (aecm->mseAdaptOld < aecm->mseThreshold))
1128             {
1129                 // The adaptive channel has a significantly lower MSE than the stored one.
1130                 // The MSE for the adaptive channel has also been low for two consecutive
1131                 // calculations. Store the adaptive channel.
1132                 WebRtcAecm_StoreAdaptiveChannel(aecm, far_spectrum, echoEst);
1133
1134                 // Update threshold
1135                 if (aecm->mseThreshold == WEBRTC_SPL_WORD32_MAX)
1136                 {
1137                     aecm->mseThreshold = (mseAdapt + aecm->mseAdaptOld);
1138                 } else
1139                 {
1140                     aecm->mseThreshold += WEBRTC_SPL_MUL_16_16_RSFT(mseAdapt
1141                             - WEBRTC_SPL_MUL_16_16_RSFT(aecm->mseThreshold, 5, 3), 205, 8);
1142                 }
1143
1144             }
1145
1146             // Reset counter
1147             aecm->mseChannelCount = 0;
1148
1149             // Store the MSE values.
1150             aecm->mseStoredOld = mseStored;
1151             aecm->mseAdaptOld = mseAdapt;
1152         }
1153     }
1154     // END: Determine if we should store or reset channel estimate.
1155 }
1156
1157 // CalcSuppressionGain(...)
1158 //
1159 // This function calculates the suppression gain that is used in the Wiener filter.
1160 //
1161 //
1162 // @param  aecm     [i/n]   Handle of the AECM instance.
1163 // @param  supGain  [out]   (Return value) Suppression gain with which to scale the noise
1164 //                          level (Q14).
1165 //
1166 //
1167 int16_t WebRtcAecm_CalcSuppressionGain(AecmCore_t * const aecm)
1168 {
1169     int32_t tmp32no1;
1170
1171     int16_t supGain = SUPGAIN_DEFAULT;
1172     int16_t tmp16no1;
1173     int16_t dE = 0;
1174
1175     // Determine suppression gain used in the Wiener filter. The gain is based on a mix of far
1176     // end energy and echo estimation error.
1177     // Adjust for the far end signal level. A low signal level indicates no far end signal,
1178     // hence we set the suppression gain to 0
1179     if (!aecm->currentVADValue)
1180     {
1181         supGain = 0;
1182     } else
1183     {
1184         // Adjust for possible double talk. If we have large variations in estimation error we
1185         // likely have double talk (or poor channel).
1186         tmp16no1 = (aecm->nearLogEnergy[0] - aecm->echoStoredLogEnergy[0] - ENERGY_DEV_OFFSET);
1187         dE = WEBRTC_SPL_ABS_W16(tmp16no1);
1188
1189         if (dE < ENERGY_DEV_TOL)
1190         {
1191             // Likely no double talk. The better estimation, the more we can suppress signal.
1192             // Update counters
1193             if (dE < SUPGAIN_EPC_DT)
1194             {
1195                 tmp32no1 = WEBRTC_SPL_MUL_16_16(aecm->supGainErrParamDiffAB, dE);
1196                 tmp32no1 += (SUPGAIN_EPC_DT >> 1);
1197                 tmp16no1 = (int16_t)WebRtcSpl_DivW32W16(tmp32no1, SUPGAIN_EPC_DT);
1198                 supGain = aecm->supGainErrParamA - tmp16no1;
1199             } else
1200             {
1201                 tmp32no1 = WEBRTC_SPL_MUL_16_16(aecm->supGainErrParamDiffBD,
1202                                                 (ENERGY_DEV_TOL - dE));
1203                 tmp32no1 += ((ENERGY_DEV_TOL - SUPGAIN_EPC_DT) >> 1);
1204                 tmp16no1 = (int16_t)WebRtcSpl_DivW32W16(tmp32no1, (ENERGY_DEV_TOL
1205                         - SUPGAIN_EPC_DT));
1206                 supGain = aecm->supGainErrParamD + tmp16no1;
1207             }
1208         } else
1209         {
1210             // Likely in double talk. Use default value
1211             supGain = aecm->supGainErrParamD;
1212         }
1213     }
1214
1215     if (supGain > aecm->supGainOld)
1216     {
1217         tmp16no1 = supGain;
1218     } else
1219     {
1220         tmp16no1 = aecm->supGainOld;
1221     }
1222     aecm->supGainOld = supGain;
1223     if (tmp16no1 < aecm->supGain)
1224     {
1225         aecm->supGain += (int16_t)((tmp16no1 - aecm->supGain) >> 4);
1226     } else
1227     {
1228         aecm->supGain += (int16_t)((tmp16no1 - aecm->supGain) >> 4);
1229     }
1230
1231     // END: Update suppression gain
1232
1233     return aecm->supGain;
1234 }
1235
1236 void WebRtcAecm_BufferFarFrame(AecmCore_t* const aecm,
1237                                const int16_t* const farend,
1238                                const int farLen)
1239 {
1240     int writeLen = farLen, writePos = 0;
1241
1242     // Check if the write position must be wrapped
1243     while (aecm->farBufWritePos + writeLen > FAR_BUF_LEN)
1244     {
1245         // Write to remaining buffer space before wrapping
1246         writeLen = FAR_BUF_LEN - aecm->farBufWritePos;
1247         memcpy(aecm->farBuf + aecm->farBufWritePos, farend + writePos,
1248                sizeof(int16_t) * writeLen);
1249         aecm->farBufWritePos = 0;
1250         writePos = writeLen;
1251         writeLen = farLen - writeLen;
1252     }
1253
1254     memcpy(aecm->farBuf + aecm->farBufWritePos, farend + writePos,
1255            sizeof(int16_t) * writeLen);
1256     aecm->farBufWritePos += writeLen;
1257 }
1258
1259 void WebRtcAecm_FetchFarFrame(AecmCore_t * const aecm, int16_t * const farend,
1260                               const int farLen, const int knownDelay)
1261 {
1262     int readLen = farLen;
1263     int readPos = 0;
1264     int delayChange = knownDelay - aecm->lastKnownDelay;
1265
1266     aecm->farBufReadPos -= delayChange;
1267
1268     // Check if delay forces a read position wrap
1269     while (aecm->farBufReadPos < 0)
1270     {
1271         aecm->farBufReadPos += FAR_BUF_LEN;
1272     }
1273     while (aecm->farBufReadPos > FAR_BUF_LEN - 1)
1274     {
1275         aecm->farBufReadPos -= FAR_BUF_LEN;
1276     }
1277
1278     aecm->lastKnownDelay = knownDelay;
1279
1280     // Check if read position must be wrapped
1281     while (aecm->farBufReadPos + readLen > FAR_BUF_LEN)
1282     {
1283
1284         // Read from remaining buffer space before wrapping
1285         readLen = FAR_BUF_LEN - aecm->farBufReadPos;
1286         memcpy(farend + readPos, aecm->farBuf + aecm->farBufReadPos,
1287                sizeof(int16_t) * readLen);
1288         aecm->farBufReadPos = 0;
1289         readPos = readLen;
1290         readLen = farLen - readLen;
1291     }
1292     memcpy(farend + readPos, aecm->farBuf + aecm->farBufReadPos,
1293            sizeof(int16_t) * readLen);
1294     aecm->farBufReadPos += readLen;
1295 }