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