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.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 stepsize;
 111   float ratio, M;
 112
 113   // fast near-end and far-end average
 114   a->dfast += ALPHAFAST * (fabsf(d) - a->dfast);
 115   a->xfast += ALPHAFAST * (fabsf(x) - a->xfast);
 116
 117   // slow near-end and far-end average
 118   a->dslow += ALPHASLOW * (fabsf(d) - a->dslow);
 119   a->xslow += ALPHASLOW * (fabsf(x) - a->xslow);
 120
 121   if (a->xfast < M70dB_PCM) {
 122     return 0.0;   // no Spk signal
 123   }
 124
 125   if (a->dfast < M70dB_PCM) {
 126     return 0.0;   // no Mic signal
 127   }
 128
 129   // ratio of NFRs
 130   ratio = (a->dfast * a->xslow) / (a->dslow * a->xfast);
 131
 132   // begrenzte lineare Kennlinie
 133   M = (STEPY2 - STEPY1) / (STEPX2 - STEPX1);
 134   if (ratio < STEPX1) {
 135     stepsize = STEPY1;
 136   } else if (ratio > STEPX2) {
 137     stepsize = STEPY2;
 138   } else {
 139     // Punktrichtungsform einer Geraden
 140     stepsize = M * (ratio - STEPX1) + STEPY1;
 141   }
 142
 143   return stepsize;
 144 }
 145
 146
 147 static void AEC_leaky(AEC *a)
 148 // The xfast signal is used to charge the hangover timer to Thold.
 149 // When hangover expires (no Spk signal for some time) the vector w
 150 // is erased. This is my implementation of Leaky NLMS.
 151 {
 152   if (a->xfast >= M70dB_PCM) {
 153     // vector w is valid for hangover Thold time
 154     a->hangover = Thold;
 155   } else {
 156     if (a->hangover > 1) {
 157       --(a->hangover);
 158     } else if (1 == a->hangover) {
 159       --(a->hangover);
 160       // My Leaky NLMS is to erase vector w when hangover expires
 161       memset(a->w, 0, sizeof(a->w));
 162     }
 163   }
 164 }
 165
 166
 167 #if 0
 168 void AEC::openwdisplay() {
 169   // open TCP connection to program wdisplay.tcl
 170   fdwdisplay = socket_async("127.0.0.1", 50999);
 171 };
 172 #endif
 173
 174
 175 static REAL AEC_nlms_pw(AEC *a, REAL d, REAL x_, float stepsize)
 176 {
 177   REAL e;
 178   REAL ef;
 179   a->x[a->j] = x_;
 180   a->xf[a->j] = IIR1_highpass(a->Fx, x_);     // pre-whitening of x
 181
 182   // calculate error value
 183   // (mic signal - estimated mic signal from spk signal)
 184   e = d;
 185   if (a->hangover > 0) {
 186     e -= a->dotp(a->w, a->x + a->j);
 187   }
 188   ef = IIR1_highpass(a->Fe, e);     // pre-whitening of e
 189
 190   // optimize: iterative dotp(xf, xf)
 191   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]);
 192
 193   if (stepsize > 0.0) {
 194     // calculate variable step size
 195     REAL mikro_ef = stepsize * ef / a->dotp_xf_xf;
 196
 197 #ifdef DISABLE_ORC
 198     // update tap weights (filter learning)
 199     int i;
 200     for (i = 0; i < NLMS_LEN; i += 2) {
 201       // optimize: partial loop unrolling
 202       a->w[i] += mikro_ef * a->xf[i + a->j];
 203       a->w[i + 1] += mikro_ef * a->xf[i + a->j + 1];
 204     }
 205 #else
 206     update_tap_weights(a->w, &a->xf[a->j], mikro_ef, NLMS_LEN);
 207 #endif
 208   }
 209
 210   if (--(a->j) < 0) {
 211     // optimize: decrease number of memory copies
 212     a->j = NLMS_EXT;
 213     memmove(a->x + a->j + 1, a->x, (NLMS_LEN - 1) * sizeof(REAL));
 214     memmove(a->xf + a->j + 1, a->xf, (NLMS_LEN - 1) * sizeof(REAL));
 215   }
 216
 217   // Saturation
 218   if (e > MAXPCM) {
 219     return MAXPCM;
 220   } else if (e < -MAXPCM) {
 221     return -MAXPCM;
 222   } else {
 223     return e;
 224   }
 225 }
 226
 227
 228 int AEC_doAEC(AEC *a, int d_, int x_)
 229 {
 230   REAL d = (REAL) d_;
 231   REAL x = (REAL) x_;
 232
 233   // Mic Highpass Filter - to remove DC
 234   d = IIR_HP_highpass(a->acMic, d);
 235
 236   // Mic Highpass Filter - cut-off below 300Hz
 237   d = FIR_HP_300Hz_highpass(a->cutoff, d);
 238
 239   // Amplify, for e.g. Soundcards with -6dB max. volume
 240   d *= a->gain;
 241
 242   // Spk Highpass Filter - to remove DC
 243   x = IIR_HP_highpass(a->acSpk, x);
 244
 245   // Double Talk Detector
 246   a->stepsize = AEC_dtd(a, d, x);
 247
 248   // Leaky (ageing of vector w)
 249   AEC_leaky(a);
 250
 251   // Acoustic Echo Cancellation
 252   d = AEC_nlms_pw(a, d, x, a->stepsize);
 253
 254 #if 0
 255   if (fdwdisplay >= 0) {
 256     if (++dumpcnt >= (WIDEB*RATE/10)) {
 257       // wdisplay creates 10 dumps per seconds = large CPU load!
 258       dumpcnt = 0;
 259       write(fdwdisplay, ws, DUMP_LEN*sizeof(float));
 260       // we don't check return value. This is not production quality!!!
 261       memset(ws, 0, sizeof(ws));
 262     } else {
 263       int i;
 264       for (i = 0; i < DUMP_LEN; i += 2) {
 265         // optimize: partial loop unrolling
 266         ws[i] += w[i];
 267         ws[i + 1] += w[i + 1];
 268       }
 269     }
 270   }
 271 #endif
 272
 273   return (int) d;
 274 }