src/modules/echo-cancel/adrian-aec.c

   1 /* aec.cpp
   2  *
   3  * Copyright (C) DFS Deutsche Flugsicherung (2004, 2005).
   4  * All Rights Reserved.
   5  *
   6  * Acoustic Echo Cancellation NLMS-pw algorithm
   7  *
   8  * Version 0.3 filter created with www.dsptutor.freeuk.com
   9  * Version 0.3.1 Allow change of stability parameter delta
  10  * Version 0.4 Leaky Normalized LMS - pre whitening algorithm
  11  */
  12
  13 #ifndef _GNU_SOURCE
  14 #define _GNU_SOURCE
  15 #endif
  16
  17 #include <math.h>
  18 #include <string.h>
  19 #include <stdint.h>
  20
  21 #include <pulse/xmalloc.h>
  22
  23 #include "adrian-aec.h"
  24
  25 #ifndef DISABLE_ORC
  26 #include "adrian-aec-orc-gen.h"
  27 #endif
  28
  29 #ifdef __SSE__
  30 #include <xmmintrin.h>
  31 #endif
  32
  33 /* Vector Dot Product */
  34 static REAL dotp(REAL a[], REAL b[])
  35 {
  36   REAL sum0 = 0.0, sum1 = 0.0;
  37   int j;
  38
  39   for (j = 0; j < NLMS_LEN; j += 2) {
  40     // optimize: partial loop unrolling
  41     sum0 += a[j] * b[j];
  42     sum1 += a[j + 1] * b[j + 1];
  43   }
  44   return sum0 + sum1;
  45 }
  46
  47 static REAL dotp_sse(REAL a[], REAL b[])
  48 {
  49 #ifdef __SSE__
  50   /* This is taken from speex's inner product implementation */
  51   int j;
  52   REAL sum;
  53   __m128 acc = _mm_setzero_ps();
  54
  55   for (j=0;j<NLMS_LEN;j+=8)
  56   {
  57     acc = _mm_add_ps(acc, _mm_mul_ps(_mm_load_ps(a+j), _mm_loadu_ps(b+j)));
  58     acc = _mm_add_ps(acc, _mm_mul_ps(_mm_load_ps(a+j+4), _mm_loadu_ps(b+j+4)));
  59   }
  60   acc = _mm_add_ps(acc, _mm_movehl_ps(acc, acc));
  61   acc = _mm_add_ss(acc, _mm_shuffle_ps(acc, acc, 0x55));
  62   _mm_store_ss(&sum, acc);
  63
  64   return sum;
  65 #else
  66   return dotp(a, b);
  67 #endif
  68 }
  69
  70
  71 AEC* AEC_init(int RATE, int have_vector)
  72 {
  73   AEC *a = pa_xnew(AEC, 1);
  74   a->hangover = 0;
  75   memset(a->x, 0, sizeof(a->x));
  76   memset(a->xf, 0, sizeof(a->xf));
  77   memset(a->w_arr, 0, sizeof(a->w_arr));
  78   a->j = NLMS_EXT;
  79   a->delta = 0.0f;
  80   AEC_setambient(a, NoiseFloor);
  81   a->dfast = a->dslow = M75dB_PCM;
  82   a->xfast = a->xslow = M80dB_PCM;
  83   a->gain = 1.0f;
  84   a->Fx = IIR1_init(2000.0f/RATE);
  85   a->Fe = IIR1_init(2000.0f/RATE);
  86   a->cutoff = FIR_HP_300Hz_init();
  87   a->acMic = IIR_HP_init();
  88   a->acSpk = IIR_HP_init();
  89
  90   a->aes_y2 = M0dB;
  91
  92   a->fdwdisplay = -1;
  93   a->dumpcnt = 0;
  94   memset(a->ws, 0, sizeof(a->ws));
  95
  96   if (have_vector) {
  97       /* Get a 16-byte aligned location */
  98       a->w = (REAL *) (((uintptr_t) a->w_arr) + (((uintptr_t) a->w_arr) % 16));
  99       a->dotp = dotp_sse;
 100   } else {
 101       /* We don't care about alignment, just use the array as-is */
 102       a->w = a->w_arr;
 103       a->dotp = dotp;
 104   }
 105
 106   return a;
 107 }
 108
 109 void AEC_done(AEC *a) {
 110     pa_assert(a);
 111
 112     pa_xfree(a->Fx);
 113     pa_xfree(a->Fe);
 114     pa_xfree(a->acMic);
 115     pa_xfree(a->acSpk);
 116     pa_xfree(a->cutoff);
 117     pa_xfree(a);
 118 }
 119
 120 // Adrian soft decision DTD
 121 // (Dual Average Near-End to Far-End signal Ratio DTD)
 122 // This algorithm uses exponential smoothing with differnt
 123 // ageing parameters to get fast and slow near-end and far-end
 124 // signal averages. The ratio of NFRs term
 125 // (dfast / xfast) / (dslow / xslow) is used to compute the stepsize
 126 // A ratio value of 2.5 is mapped to stepsize 0, a ratio of 0 is
 127 // mapped to 1.0 with a limited linear function.
 128 static float AEC_dtd(AEC *a, REAL d, REAL x)
 129 {
 130   float ratio, stepsize;
 131
 132   // fast near-end and far-end average
 133   a->dfast += ALPHAFAST * (fabsf(d) - a->dfast);
 134   a->xfast += ALPHAFAST * (fabsf(x) - a->xfast);
 135
 136   // slow near-end and far-end average
 137   a->dslow += ALPHASLOW * (fabsf(d) - a->dslow);
 138   a->xslow += ALPHASLOW * (fabsf(x) - a->xslow);
 139
 140   if (a->xfast < M70dB_PCM) {
 141     return 0.0;   // no Spk signal
 142   }
 143
 144   if (a->dfast < M70dB_PCM) {
 145     return 0.0;   // no Mic signal
 146   }
 147
 148   // ratio of NFRs
 149   ratio = (a->dfast * a->xslow) / (a->dslow * a->xfast);
 150
 151   // Linear interpolation with clamping at the limits
 152   if (ratio < STEPX1)
 153     stepsize = STEPY1;
 154   else if (ratio > STEPX2)
 155     stepsize = STEPY2;
 156   else
 157     stepsize = STEPY1 + (STEPY2 - STEPY1) * (ratio - STEPX1) / (STEPX2 - STEPX1);
 158
 159   return stepsize;
 160 }
 161
 162
 163 static void AEC_leaky(AEC *a)
 164 // The xfast signal is used to charge the hangover timer to Thold.
 165 // When hangover expires (no Spk signal for some time) the vector w
 166 // is erased. This is my implementation of Leaky NLMS.
 167 {
 168   if (a->xfast >= M70dB_PCM) {
 169     // vector w is valid for hangover Thold time
 170     a->hangover = Thold;
 171   } else {
 172     if (a->hangover > 1) {
 173       --(a->hangover);
 174     } else if (1 == a->hangover) {
 175       --(a->hangover);
 176       // My Leaky NLMS is to erase vector w when hangover expires
 177       memset(a->w_arr, 0, sizeof(a->w_arr));
 178     }
 179   }
 180 }
 181
 182
 183 #if 0
 184 void AEC::openwdisplay() {
 185   // open TCP connection to program wdisplay.tcl
 186   fdwdisplay = socket_async("127.0.0.1", 50999);
 187 };
 188 #endif
 189
 190
 191 static REAL AEC_nlms_pw(AEC *a, REAL d, REAL x_, float stepsize)
 192 {
 193   REAL e;
 194   REAL ef;
 195   a->x[a->j] = x_;
 196   a->xf[a->j] = IIR1_highpass(a->Fx, x_);     // pre-whitening of x
 197
 198   // calculate error value
 199   // (mic signal - estimated mic signal from spk signal)
 200   e = d;
 201   if (a->hangover > 0) {
 202     e -= a->dotp(a->w, a->x + a->j);
 203   }
 204   ef = IIR1_highpass(a->Fe, e);     // pre-whitening of e
 205
 206   // optimize: iterative dotp(xf, xf)
 207   a->dotp_xf_xf += (a->xf[a->j] * a->xf[a->j] - a->xf[a->j + NLMS_LEN - 1] * a->xf[a->j + NLMS_LEN - 1]);
 208
 209   if (stepsize > 0.0) {
 210     // calculate variable step size
 211     REAL mikro_ef = stepsize * ef / a->dotp_xf_xf;
 212
 213 #ifdef DISABLE_ORC
 214     // update tap weights (filter learning)
 215     int i;
 216     for (i = 0; i < NLMS_LEN; i += 2) {
 217       // optimize: partial loop unrolling
 218       a->w[i] += mikro_ef * a->xf[i + a->j];
 219       a->w[i + 1] += mikro_ef * a->xf[i + a->j + 1];
 220     }
 221 #else
 222     update_tap_weights(a->w, &a->xf[a->j], mikro_ef, NLMS_LEN);
 223 #endif
 224   }
 225
 226   if (--(a->j) < 0) {
 227     // optimize: decrease number of memory copies
 228     a->j = NLMS_EXT;
 229     memmove(a->x + a->j + 1, a->x, (NLMS_LEN - 1) * sizeof(REAL));
 230     memmove(a->xf + a->j + 1, a->xf, (NLMS_LEN - 1) * sizeof(REAL));
 231   }
 232
 233   // Saturation
 234   if (e > MAXPCM) {
 235     return MAXPCM;
 236   } else if (e < -MAXPCM) {
 237     return -MAXPCM;
 238   } else {
 239     return e;
 240   }
 241 }
 242
 243
 244 int AEC_doAEC(AEC *a, int d_, int x_)
 245 {
 246   REAL d = (REAL) d_;
 247   REAL x = (REAL) x_;
 248
 249   // Mic Highpass Filter - to remove DC
 250   d = IIR_HP_highpass(a->acMic, d);
 251
 252   // Mic Highpass Filter - cut-off below 300Hz
 253   d = FIR_HP_300Hz_highpass(a->cutoff, d);
 254
 255   // Amplify, for e.g. Soundcards with -6dB max. volume
 256   d *= a->gain;
 257
 258   // Spk Highpass Filter - to remove DC
 259   x = IIR_HP_highpass(a->acSpk, x);
 260
 261   // Double Talk Detector
 262   a->stepsize = AEC_dtd(a, d, x);
 263
 264   // Leaky (ageing of vector w)
 265   AEC_leaky(a);
 266
 267   // Acoustic Echo Cancellation
 268   d = AEC_nlms_pw(a, d, x, a->stepsize);
 269
 270 #if 0
 271   if (fdwdisplay >= 0) {
 272     if (++dumpcnt >= (WIDEB*RATE/10)) {
 273       // wdisplay creates 10 dumps per seconds = large CPU load!
 274       dumpcnt = 0;
 275       write(fdwdisplay, ws, DUMP_LEN*sizeof(float));
 276       // we don't check return value. This is not production quality!!!
 277       memset(ws, 0, sizeof(ws));
 278     } else {
 279       int i;
 280       for (i = 0; i < DUMP_LEN; i += 2) {
 281         // optimize: partial loop unrolling
 282         ws[i] += w[i];
 283         ws[i + 1] += w[i + 1];
 284       }
 285     }
 286   }
 287 #endif
 288
 289   return (int) d;
 290 }