254527240b2588482f90ee78c920148f92dee6a7
[speexdsp.git] / libspeex / mdf.c
1 /* Copyright (C) 2003-2006 Jean-Marc Valin
2
3    File: mdf.c
4    Echo canceller based on the MDF algorithm (see below)
5
6    Redistribution and use in source and binary forms, with or without
7    modification, are permitted provided that the following conditions are
8    met:
9
10    1. Redistributions of source code must retain the above copyright notice,
11    this list of conditions and the following disclaimer.
12
13    2. Redistributions in binary form must reproduce the above copyright
14    notice, this list of conditions and the following disclaimer in the
15    documentation and/or other materials provided with the distribution.
16
17    3. The name of the author may not be used to endorse or promote products
18    derived from this software without specific prior written permission.
19
20    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
21    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
22    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
24    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
28    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30    POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 /*
34    The echo canceller is based on the MDF algorithm described in:
35
36    J. S. Soo, K. K. Pang Multidelay block frequency adaptive filter, 
37    IEEE Trans. Acoust. Speech Signal Process., Vol. ASSP-38, No. 2, 
38    February 1990.
39    
40    We use the Alternatively Updated MDF (AUMDF) variant. Robustness to 
41    double-talk is achieved using a variable learning rate as described in:
42    
43    Valin, J.-M., On Adjusting the Learning Rate in Frequency Domain Echo 
44    Cancellation With Double-Talk. IEEE Transactions on Audio,
45    Speech and Language Processing, Vol. 15, No. 3, pp. 1030-1034, 2007.
46    http://people.xiph.org/~jm/papers/valin_taslp2006.pdf
47    
48    There is no explicit double-talk detection, but a continuous variation
49    in the learning rate based on residual echo, double-talk and background
50    noise.
51    
52    About the fixed-point version:
53    All the signals are represented with 16-bit words. The filter weights 
54    are represented with 32-bit words, but only the top 16 bits are used
55    in most cases. The lower 16 bits are completely unreliable (due to the
56    fact that the update is done only on the top bits), but help in the
57    adaptation -- probably by removing a "threshold effect" due to
58    quantization (rounding going to zero) when the gradient is small.
59    
60    Another kludge that seems to work good: when performing the weight
61    update, we only move half the way toward the "goal" this seems to
62    reduce the effect of quantization noise in the update phase. This
63    can be seen as applying a gradient descent on a "soft constraint"
64    instead of having a hard constraint.
65    
66 */
67
68 #ifdef HAVE_CONFIG_H
69 #include "config.h"
70 #endif
71
72 #include "misc.h"
73 #include "speex/speex_echo.h"
74 #include "fftwrap.h"
75 #include "pseudofloat.h"
76 #include "math_approx.h"
77
78 #ifndef M_PI
79 #define M_PI 3.14159265358979323846
80 #endif
81
82 #ifdef FIXED_POINT
83 #define WEIGHT_SHIFT 11
84 #define NORMALIZE_SCALEDOWN 5
85 #define NORMALIZE_SCALEUP 3
86 #else
87 #define WEIGHT_SHIFT 0
88 #endif
89
90 /* If enabled, the AEC will use a foreground filter and a background filter to be more robust to double-talk
91    and difficult signals in general. The cost is an extra FFT and a matrix-vector multiply */
92 #define TWO_PATH
93
94 #ifdef FIXED_POINT
95 static const spx_float_t MIN_LEAK = {20972, -22};
96
97 /* Constants for the two-path filter */
98 static const spx_float_t VAR1_SMOOTH = {23593, -16};
99 static const spx_float_t VAR2_SMOOTH = {23675, -15};
100 static const spx_float_t VAR1_UPDATE = {16384, -15};
101 static const spx_float_t VAR2_UPDATE = {16384, -16};
102 static const spx_float_t VAR_BACKTRACK = {16384, -12};
103 #define TOP16(x) ((x)>>16)
104
105 #else
106
107 static const spx_float_t MIN_LEAK = .005f;
108
109 /* Constants for the two-path filter */
110 static const spx_float_t VAR1_SMOOTH = .36f;
111 static const spx_float_t VAR2_SMOOTH = .7225f;
112 static const spx_float_t VAR1_UPDATE = .5f;
113 static const spx_float_t VAR2_UPDATE = .25f;
114 static const spx_float_t VAR_BACKTRACK = 4.f;
115 #define TOP16(x) (x)
116 #endif
117
118
119 #define PLAYBACK_DELAY 2
120
121 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
122
123
124 /** Speex echo cancellation state. */
125 struct SpeexEchoState_ {
126    int frame_size;           /**< Number of samples processed each time */
127    int window_size;
128    int M;
129    int cancel_count;
130    int adapted;
131    int saturated;
132    int screwed_up;
133    spx_int32_t sampling_rate;
134    spx_word16_t spec_average;
135    spx_word16_t beta0;
136    spx_word16_t beta_max;
137    spx_word32_t sum_adapt;
138    spx_word16_t leak_estimate;
139    
140    spx_word16_t *e;      /* scratch */
141    spx_word16_t *x;      /* Far-end input buffer (2N) */
142    spx_word16_t *X;      /* Far-end buffer (M+1 frames) in frequency domain */
143    spx_word16_t *input;  /* scratch */
144    spx_word16_t *y;      /* scratch */
145    spx_word16_t *last_y;
146    spx_word16_t *Y;      /* scratch */
147    spx_word16_t *E;
148    spx_word32_t *PHI;    /* scratch */
149    spx_word32_t *W;      /* (Background) filter weights */
150 #ifdef TWO_PATH
151    spx_word16_t *foreground; /* Foreground filter weights */
152    spx_word32_t  Davg1;  /* 1st recursive average of the residual power difference */
153    spx_word32_t  Davg2;  /* 2nd recursive average of the residual power difference */
154    spx_float_t   Dvar1;  /* Estimated variance of 1st estimator */
155    spx_float_t   Dvar2;  /* Estimated variance of 2nd estimator */
156 #endif
157    spx_word32_t *power;  /* Power of the far-end signal */
158    spx_float_t  *power_1;/* Inverse power of far-end */
159    spx_word16_t *wtmp;   /* scratch */
160 #ifdef FIXED_POINT
161    spx_word16_t *wtmp2;  /* scratch */
162 #endif
163    spx_word32_t *Rf;     /* scratch */
164    spx_word32_t *Yf;     /* scratch */
165    spx_word32_t *Xf;     /* scratch */
166    spx_word32_t *Eh;
167    spx_word32_t *Yh;
168    spx_float_t   Pey;
169    spx_float_t   Pyy;
170    spx_word16_t *window;
171    spx_word16_t *prop;
172    void *fft_table;
173    spx_word16_t memX, memD, memE;
174    spx_word16_t preemph;
175    spx_word16_t notch_radius;
176    spx_mem_t notch_mem[2];
177
178    /* NOTE: If you only use speex_echo_cancel() and want to save some memory, remove this */
179    spx_int16_t *play_buf;
180    int play_buf_pos;
181    int play_buf_started;
182 };
183
184 static inline void filter_dc_notch16(const spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem)
185 {
186    int i;
187    spx_word16_t den2;
188 #ifdef FIXED_POINT
189    den2 = MULT16_16_Q15(radius,radius) + MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q15(32767-radius,32767-radius));
190 #else
191    den2 = radius*radius + .7*(1-radius)*(1-radius);
192 #endif   
193    /*printf ("%d %d %d %d %d %d\n", num[0], num[1], num[2], den[0], den[1], den[2]);*/
194    for (i=0;i<len;i++)
195    {
196       spx_word16_t vin = in[i];
197       spx_word32_t vout = mem[0] + SHL32(EXTEND32(vin),15);
198 #ifdef FIXED_POINT
199       mem[0] = mem[1] + SHL32(SHL32(-EXTEND32(vin),15) + MULT16_32_Q15(radius,vout),1);
200 #else
201       mem[0] = mem[1] + 2*(-vin + radius*vout);
202 #endif
203       mem[1] = SHL32(EXTEND32(vin),15) - MULT16_32_Q15(den2,vout);
204       out[i] = SATURATE32(PSHR32(MULT16_32_Q15(radius,vout),15),32767);
205    }
206 }
207
208 /* This inner product is slightly different from the codec version because of fixed-point */
209 static inline spx_word32_t mdf_inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
210 {
211    spx_word32_t sum=0;
212    len >>= 1;
213    while(len--)
214    {
215       spx_word32_t part=0;
216       part = MAC16_16(part,*x++,*y++);
217       part = MAC16_16(part,*x++,*y++);
218       /* HINT: If you had a 40-bit accumulator, you could shift only at the end */
219       sum = ADD32(sum,SHR32(part,6));
220    }
221    return sum;
222 }
223
224 /** Compute power spectrum of a half-complex (packed) vector */
225 static inline void power_spectrum(const spx_word16_t *X, spx_word32_t *ps, int N)
226 {
227    int i, j;
228    ps[0]=MULT16_16(X[0],X[0]);
229    for (i=1,j=1;i<N-1;i+=2,j++)
230    {
231       ps[j] =  MULT16_16(X[i],X[i]) + MULT16_16(X[i+1],X[i+1]);
232    }
233    ps[j]=MULT16_16(X[i],X[i]);
234 }
235
236 /** Compute cross-power spectrum of a half-complex (packed) vectors and add to acc */
237 #ifdef FIXED_POINT
238 static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M)
239 {
240    int i,j;
241    spx_word32_t tmp1=0,tmp2=0;
242    for (j=0;j<M;j++)
243    {
244       tmp1 = MAC16_16(tmp1, X[j*N],TOP16(Y[j*N]));
245    }
246    acc[0] = PSHR32(tmp1,WEIGHT_SHIFT);
247    for (i=1;i<N-1;i+=2)
248    {
249       tmp1 = tmp2 = 0;
250       for (j=0;j<M;j++)
251       {
252          tmp1 = SUB32(MAC16_16(tmp1, X[j*N+i],TOP16(Y[j*N+i])), MULT16_16(X[j*N+i+1],TOP16(Y[j*N+i+1])));
253          tmp2 = MAC16_16(MAC16_16(tmp2, X[j*N+i+1],TOP16(Y[j*N+i])), X[j*N+i], TOP16(Y[j*N+i+1]));
254       }
255       acc[i] = PSHR32(tmp1,WEIGHT_SHIFT);
256       acc[i+1] = PSHR32(tmp2,WEIGHT_SHIFT);
257    }
258    tmp1 = tmp2 = 0;
259    for (j=0;j<M;j++)
260    {
261       tmp1 = MAC16_16(tmp1, X[(j+1)*N-1],TOP16(Y[(j+1)*N-1]));
262    }
263    acc[N-1] = PSHR32(tmp1,WEIGHT_SHIFT);
264 }
265 static inline void spectral_mul_accum16(const spx_word16_t *X, const spx_word16_t *Y, spx_word16_t *acc, int N, int M)
266 {
267    int i,j;
268    spx_word32_t tmp1=0,tmp2=0;
269    for (j=0;j<M;j++)
270    {
271       tmp1 = MAC16_16(tmp1, X[j*N],Y[j*N]);
272    }
273    acc[0] = PSHR32(tmp1,WEIGHT_SHIFT);
274    for (i=1;i<N-1;i+=2)
275    {
276       tmp1 = tmp2 = 0;
277       for (j=0;j<M;j++)
278       {
279          tmp1 = SUB32(MAC16_16(tmp1, X[j*N+i],Y[j*N+i]), MULT16_16(X[j*N+i+1],Y[j*N+i+1]));
280          tmp2 = MAC16_16(MAC16_16(tmp2, X[j*N+i+1],Y[j*N+i]), X[j*N+i], Y[j*N+i+1]);
281       }
282       acc[i] = PSHR32(tmp1,WEIGHT_SHIFT);
283       acc[i+1] = PSHR32(tmp2,WEIGHT_SHIFT);
284    }
285    tmp1 = tmp2 = 0;
286    for (j=0;j<M;j++)
287    {
288       tmp1 = MAC16_16(tmp1, X[(j+1)*N-1],Y[(j+1)*N-1]);
289    }
290    acc[N-1] = PSHR32(tmp1,WEIGHT_SHIFT);
291 }
292
293 #else
294 static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M)
295 {
296    int i,j;
297    for (i=0;i<N;i++)
298       acc[i] = 0;
299    for (j=0;j<M;j++)
300    {
301       acc[0] += X[0]*Y[0];
302       for (i=1;i<N-1;i+=2)
303       {
304          acc[i] += (X[i]*Y[i] - X[i+1]*Y[i+1]);
305          acc[i+1] += (X[i+1]*Y[i] + X[i]*Y[i+1]);
306       }
307       acc[i] += X[i]*Y[i];
308       X += N;
309       Y += N;
310    }
311 }
312 #define spectral_mul_accum16 spectral_mul_accum
313 #endif
314
315 /** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */
316 static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_float_t p, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N)
317 {
318    int i, j;
319    spx_float_t W;
320    W = FLOAT_AMULT(p, w[0]);
321    prod[0] = FLOAT_MUL32(W,MULT16_16(X[0],Y[0]));
322    for (i=1,j=1;i<N-1;i+=2,j++)
323    {
324       W = FLOAT_AMULT(p, w[j]);
325       prod[i] = FLOAT_MUL32(W,MAC16_16(MULT16_16(X[i],Y[i]), X[i+1],Y[i+1]));
326       prod[i+1] = FLOAT_MUL32(W,MAC16_16(MULT16_16(-X[i+1],Y[i]), X[i],Y[i+1]));
327    }
328    W = FLOAT_AMULT(p, w[j]);
329    prod[i] = FLOAT_MUL32(W,MULT16_16(X[i],Y[i]));
330 }
331
332 static inline void mdf_adjust_prop(const spx_word32_t *W, int N, int M, spx_word16_t *prop)
333 {
334    int i, j;
335    spx_word16_t max_sum = 1;
336    spx_word32_t prop_sum = 1;
337    for (i=0;i<M;i++)
338    {
339       spx_word32_t tmp = 1;
340       for (j=0;j<N;j++)
341          tmp += MULT16_16(EXTRACT16(SHR32(W[i*N+j],18)), EXTRACT16(SHR32(W[i*N+j],18)));
342 #ifdef FIXED_POINT
343       /* Just a security in case an overflow were to occur */
344       tmp = MIN32(ABS32(tmp), 536870912);
345 #endif
346       prop[i] = spx_sqrt(tmp);
347       if (prop[i] > max_sum)
348          max_sum = prop[i];
349    }
350    for (i=0;i<M;i++)
351    {
352       prop[i] += MULT16_16_Q15(QCONST16(.1f,15),max_sum);
353       prop_sum += EXTEND32(prop[i]);
354    }
355    for (i=0;i<M;i++)
356    {
357       prop[i] = DIV32(MULT16_16(QCONST16(.99f,15), prop[i]),prop_sum);
358       /*printf ("%f ", prop[i]);*/
359    }
360    /*printf ("\n");*/
361 }
362
363 #ifdef DUMP_ECHO_CANCEL_DATA
364 #include <stdio.h>
365 static FILE *rFile=NULL, *pFile=NULL, *oFile=NULL;
366
367 static void dump_audio(const spx_int16_t *rec, const spx_int16_t *play, const spx_int16_t *out, int len)
368 {
369    if (!(rFile && pFile && oFile))
370    {
371       speex_fatal("Dump files not open");
372    }
373    fwrite(rec, sizeof(spx_int16_t), len, rFile);
374    fwrite(play, sizeof(spx_int16_t), len, pFile);
375    fwrite(out, sizeof(spx_int16_t), len, oFile);
376 }
377 #endif
378
379 /** Creates a new echo canceller state */
380 SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length)
381 {
382    int i,N,M;
383    SpeexEchoState *st = (SpeexEchoState *)speex_alloc(sizeof(SpeexEchoState));
384
385 #ifdef DUMP_ECHO_CANCEL_DATA
386    if (rFile || pFile || oFile)
387       speex_fatal("Opening dump files twice");
388    rFile = fopen("aec_rec.sw", "wb");
389    pFile = fopen("aec_play.sw", "wb");
390    oFile = fopen("aec_out.sw", "wb");
391 #endif
392    
393    st->frame_size = frame_size;
394    st->window_size = 2*frame_size;
395    N = st->window_size;
396    M = st->M = (filter_length+st->frame_size-1)/frame_size;
397    st->cancel_count=0;
398    st->sum_adapt = 0;
399    st->saturated = 0;
400    st->screwed_up = 0;
401    /* This is the default sampling rate */
402    st->sampling_rate = 8000;
403    st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
404 #ifdef FIXED_POINT
405    st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
406    st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
407 #else
408    st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
409    st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
410 #endif
411    st->leak_estimate = 0;
412
413    st->fft_table = spx_fft_init(N);
414    
415    st->e = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
416    st->x = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
417    st->input = (spx_word16_t*)speex_alloc(st->frame_size*sizeof(spx_word16_t));
418    st->y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
419    st->last_y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
420    st->Yf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
421    st->Rf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
422    st->Xf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
423    st->Yh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
424    st->Eh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
425
426    st->X = (spx_word16_t*)speex_alloc((M+1)*N*sizeof(spx_word16_t));
427    st->Y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
428    st->E = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
429    st->W = (spx_word32_t*)speex_alloc(M*N*sizeof(spx_word32_t));
430 #ifdef TWO_PATH
431    st->foreground = (spx_word16_t*)speex_alloc(M*N*sizeof(spx_word16_t));
432 #endif
433    st->PHI = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
434    st->power = (spx_word32_t*)speex_alloc((frame_size+1)*sizeof(spx_word32_t));
435    st->power_1 = (spx_float_t*)speex_alloc((frame_size+1)*sizeof(spx_float_t));
436    st->window = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
437    st->prop = (spx_word16_t*)speex_alloc(M*sizeof(spx_word16_t));
438    st->wtmp = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
439 #ifdef FIXED_POINT
440    st->wtmp2 = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
441    for (i=0;i<N>>1;i++)
442    {
443       st->window[i] = (16383-SHL16(spx_cos(DIV32_16(MULT16_16(25736,i<<1),N)),1));
444       st->window[N-i-1] = st->window[i];
445    }
446 #else
447    for (i=0;i<N;i++)
448       st->window[i] = .5-.5*cos(2*M_PI*i/N);
449 #endif
450    for (i=0;i<=st->frame_size;i++)
451       st->power_1[i] = FLOAT_ONE;
452    for (i=0;i<N*M;i++)
453       st->W[i] = 0;
454    {
455       spx_word32_t sum = 0;
456       /* Ratio of ~10 between adaptation rate of first and last block */
457       spx_word16_t decay = SHR32(spx_exp(NEG16(DIV32_16(QCONST16(2.4,11),M))),1);
458       st->prop[0] = QCONST16(.7, 15);
459       sum = EXTEND32(st->prop[0]);
460       for (i=1;i<M;i++)
461       {
462          st->prop[i] = MULT16_16_Q15(st->prop[i-1], decay);
463          sum = ADD32(sum, EXTEND32(st->prop[i]));
464       }
465       for (i=M-1;i>=0;i--)
466       {
467          st->prop[i] = DIV32(MULT16_16(QCONST16(.8,15), st->prop[i]),sum);
468       }
469    }
470    
471    st->memX=st->memD=st->memE=0;
472    st->preemph = QCONST16(.9,15);
473    if (st->sampling_rate<12000)
474       st->notch_radius = QCONST16(.9, 15);
475    else if (st->sampling_rate<24000)
476       st->notch_radius = QCONST16(.982, 15);
477    else
478       st->notch_radius = QCONST16(.992, 15);
479
480    st->notch_mem[0] = st->notch_mem[1] = 0;
481    st->adapted = 0;
482    st->Pey = st->Pyy = FLOAT_ONE;
483    
484 #ifdef TWO_PATH
485    st->Davg1 = st->Davg2 = 0;
486    st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
487 #endif
488    
489    st->play_buf = (spx_int16_t*)speex_alloc((PLAYBACK_DELAY+1)*st->frame_size*sizeof(spx_int16_t));
490    st->play_buf_pos = PLAYBACK_DELAY*st->frame_size;
491    st->play_buf_started = 0;
492    
493    return st;
494 }
495
496 /** Resets echo canceller state */
497 void speex_echo_state_reset(SpeexEchoState *st)
498 {
499    int i, M, N;
500    st->cancel_count=0;
501    st->screwed_up = 0;
502    N = st->window_size;
503    M = st->M;
504    for (i=0;i<N*M;i++)
505       st->W[i] = 0;
506 #ifdef TWO_PATH
507    for (i=0;i<N*M;i++)
508       st->foreground[i] = 0;
509 #endif
510    for (i=0;i<N*(M+1);i++)
511       st->X[i] = 0;
512    for (i=0;i<=st->frame_size;i++)
513    {
514       st->power[i] = 0;
515       st->power_1[i] = FLOAT_ONE;
516       st->Eh[i] = 0;
517       st->Yh[i] = 0;
518    }
519    for (i=0;i<st->frame_size;i++)
520    {
521       st->last_y[i] = 0;
522    }
523    for (i=0;i<N;i++)
524    {
525       st->E[i] = 0;
526       st->x[i] = 0;
527    }
528    st->notch_mem[0] = st->notch_mem[1] = 0;
529    st->memX=st->memD=st->memE=0;
530
531    st->saturated = 0;
532    st->adapted = 0;
533    st->sum_adapt = 0;
534    st->Pey = st->Pyy = FLOAT_ONE;
535 #ifdef TWO_PATH
536    st->Davg1 = st->Davg2 = 0;
537    st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
538 #endif
539    for (i=0;i<3*st->frame_size;i++)
540       st->play_buf[i] = 0;
541    st->play_buf_pos = PLAYBACK_DELAY*st->frame_size;
542    st->play_buf_started = 0;
543
544 }
545
546 /** Destroys an echo canceller state */
547 void speex_echo_state_destroy(SpeexEchoState *st)
548 {
549    spx_fft_destroy(st->fft_table);
550
551    speex_free(st->e);
552    speex_free(st->x);
553    speex_free(st->input);
554    speex_free(st->y);
555    speex_free(st->last_y);
556    speex_free(st->Yf);
557    speex_free(st->Rf);
558    speex_free(st->Xf);
559    speex_free(st->Yh);
560    speex_free(st->Eh);
561
562    speex_free(st->X);
563    speex_free(st->Y);
564    speex_free(st->E);
565    speex_free(st->W);
566 #ifdef TWO_PATH
567    speex_free(st->foreground);
568 #endif
569    speex_free(st->PHI);
570    speex_free(st->power);
571    speex_free(st->power_1);
572    speex_free(st->window);
573    speex_free(st->prop);
574    speex_free(st->wtmp);
575 #ifdef FIXED_POINT
576    speex_free(st->wtmp2);
577 #endif
578    speex_free(st->play_buf);
579    speex_free(st);
580    
581 #ifdef DUMP_ECHO_CANCEL_DATA
582    fclose(rFile);
583    fclose(pFile);
584    fclose(oFile);
585    rFile = pFile = oFile = NULL;
586 #endif
587 }
588
589 void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out)
590 {
591    int i;
592    /*speex_warning_int("capture with fill level ", st->play_buf_pos/st->frame_size);*/
593    st->play_buf_started = 1;
594    if (st->play_buf_pos>=st->frame_size)
595    {
596       speex_echo_cancellation(st, rec, st->play_buf, out);
597       st->play_buf_pos -= st->frame_size;
598       for (i=0;i<st->play_buf_pos;i++)
599          st->play_buf[i] = st->play_buf[i+st->frame_size];
600    } else {
601       speex_warning("No playback frame available (your application is buggy and/or got xruns)");
602       if (st->play_buf_pos!=0)
603       {
604          speex_warning("internal playback buffer corruption?");
605          st->play_buf_pos = 0;
606       }
607       for (i=0;i<st->frame_size;i++)
608          out[i] = rec[i];
609    }
610 }
611
612 void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play)
613 {
614    /*speex_warning_int("playback with fill level ", st->play_buf_pos/st->frame_size);*/
615    if (!st->play_buf_started)
616    {
617       speex_warning("discarded first playback frame");
618       return;
619    }
620    if (st->play_buf_pos<=PLAYBACK_DELAY*st->frame_size)
621    {
622       int i;
623       for (i=0;i<st->frame_size;i++)
624          st->play_buf[st->play_buf_pos+i] = play[i];
625       st->play_buf_pos += st->frame_size;
626       if (st->play_buf_pos <= (PLAYBACK_DELAY-1)*st->frame_size)
627       {
628          speex_warning("Auto-filling the buffer (your application is buggy and/or got xruns)");
629          for (i=0;i<st->frame_size;i++)
630             st->play_buf[st->play_buf_pos+i] = play[i];
631          st->play_buf_pos += st->frame_size;
632       }
633    } else {
634       speex_warning("Had to discard a playback frame (your application is buggy and/or got xruns)");
635    }
636 }
637
638 /** Performs echo cancellation on a frame */
639 void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out, spx_int32_t *Yout)
640 {
641    speex_echo_cancellation(st, in, far_end, out);
642 }
643
644 /** Performs echo cancellation on a frame (deprecated, last arg now ignored) */
645 void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
646 {
647    int i,j;
648    int N,M;
649    spx_word32_t Syy,See,Sxx,Sdd, Sff;
650 #ifdef TWO_PATH
651    spx_word32_t Dbf;
652    int update_foreground;
653 #endif
654    spx_word32_t Sey;
655    spx_word16_t ss, ss_1;
656    spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
657    spx_float_t alpha, alpha_1;
658    spx_word16_t RER;
659    spx_word32_t tmp32;
660    
661    N = st->window_size;
662    M = st->M;
663    st->cancel_count++;
664 #ifdef FIXED_POINT
665    ss=DIV32_16(11469,M);
666    ss_1 = SUB16(32767,ss);
667 #else
668    ss=.35/M;
669    ss_1 = 1-ss;
670 #endif
671
672    /* Apply a notch filter to make sure DC doesn't end up causing problems */
673    filter_dc_notch16(in, st->notch_radius, st->input, st->frame_size, st->notch_mem);
674    /* Copy input data to buffer and apply pre-emphasis */
675    for (i=0;i<st->frame_size;i++)
676    {
677       spx_word32_t tmp32;
678       tmp32 = SUB32(EXTEND32(far_end[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX)));
679 #ifdef FIXED_POINT
680       /* If saturation occurs here, we need to freeze adaptation for M+1 frames (not just one) */
681       if (tmp32 > 32767)
682       {
683          tmp32 = 32767;
684          st->saturated = M+1;
685       }
686       if (tmp32 < -32767)
687       {
688          tmp32 = -32767;
689          st->saturated = M+1;
690       }      
691 #endif
692       st->x[i+st->frame_size] = EXTRACT16(tmp32);
693       st->memX = far_end[i];
694       
695       tmp32 = SUB32(EXTEND32(st->input[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD)));
696 #ifdef FIXED_POINT
697       if (tmp32 > 32767)
698       {
699          tmp32 = 32767;
700          if (st->saturated == 0)
701             st->saturated = 1;
702       }      
703       if (tmp32 < -32767)
704       {
705          tmp32 = -32767;
706          if (st->saturated == 0)
707             st->saturated = 1;
708       }
709 #endif
710       st->memD = st->input[i];
711       st->input[i] = tmp32;
712    }
713
714    /* Shift memory: this could be optimized eventually*/
715    for (j=M-1;j>=0;j--)
716    {
717       for (i=0;i<N;i++)
718          st->X[(j+1)*N+i] = st->X[j*N+i];
719    }
720
721    /* Convert x (far end) to frequency domain */
722    spx_fft(st->fft_table, st->x, &st->X[0]);
723    for (i=0;i<N;i++)
724       st->last_y[i] = st->x[i];
725    Sxx = mdf_inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);
726    for (i=0;i<st->frame_size;i++)
727       st->x[i] = st->x[i+st->frame_size];
728    /* From here on, the top part of x is used as scratch space */
729    
730 #ifdef TWO_PATH
731    /* Compute foreground filter */
732    spectral_mul_accum16(st->X, st->foreground, st->Y, N, M);   
733    spx_ifft(st->fft_table, st->Y, st->e);
734    for (i=0;i<st->frame_size;i++)
735       st->e[i] = SUB16(st->input[i], st->e[i+st->frame_size]);
736    Sff = mdf_inner_prod(st->e, st->e, st->frame_size);
737 #endif
738    
739    /* Adjust proportional adaption rate */
740    mdf_adjust_prop (st->W, N, M, st->prop);
741    /* Compute weight gradient */
742    if (st->saturated == 0)
743    {
744       for (j=M-1;j>=0;j--)
745       {
746          weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N], st->E, st->PHI, N);
747          for (i=0;i<N;i++)
748             st->W[j*N+i] = ADD32(st->W[j*N+i], st->PHI[i]);
749          
750       }
751    } else {
752       st->saturated--;
753    }
754    
755    /* Update weight to prevent circular convolution (MDF / AUMDF) */
756    for (j=0;j<M;j++)
757    {
758       /* This is a variant of the Alternatively Updated MDF (AUMDF) */
759       /* Remove the "if" to make this an MDF filter */
760       if (j==0 || st->cancel_count%(M-1) == j-1)
761       {
762 #ifdef FIXED_POINT
763          for (i=0;i<N;i++)
764             st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16));
765          spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
766          for (i=0;i<st->frame_size;i++)
767          {
768             st->wtmp[i]=0;
769          }
770          for (i=st->frame_size;i<N;i++)
771          {
772             st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
773          }
774          spx_fft(st->fft_table, st->wtmp, st->wtmp2);
775          /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
776          for (i=0;i<N;i++)
777             st->W[j*N+i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
778 #else
779          spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
780          for (i=st->frame_size;i<N;i++)
781          {
782             st->wtmp[i]=0;
783          }
784          spx_fft(st->fft_table, st->wtmp, &st->W[j*N]);
785 #endif
786       }
787    }
788
789    /* Compute filter response Y */
790    spectral_mul_accum(st->X, st->W, st->Y, N, M);
791    spx_ifft(st->fft_table, st->Y, st->y);
792
793 #ifdef TWO_PATH
794    /* Difference in response, this is used to estimate the variance of our residual power estimate */
795    for (i=0;i<st->frame_size;i++)
796       st->e[i] = SUB16(st->e[i+st->frame_size], st->y[i+st->frame_size]);
797    Dbf = 10+mdf_inner_prod(st->e, st->e, st->frame_size);
798 #endif
799
800    for (i=0;i<st->frame_size;i++)
801       st->e[i] = SUB16(st->input[i], st->y[i+st->frame_size]);
802    See = mdf_inner_prod(st->e, st->e, st->frame_size);
803 #ifndef TWO_PATH
804    Sff = See;
805 #endif
806
807 #ifdef TWO_PATH
808    /* Logic for updating the foreground filter */
809    
810    /* For two time windows, compute the mean of the energy difference, as well as the variance */
811    st->Davg1 = ADD32(MULT16_32_Q15(QCONST16(.6f,15),st->Davg1), MULT16_32_Q15(QCONST16(.4f,15),SUB32(Sff,See)));
812    st->Davg2 = ADD32(MULT16_32_Q15(QCONST16(.85f,15),st->Davg2), MULT16_32_Q15(QCONST16(.15f,15),SUB32(Sff,See)));
813    st->Dvar1 = FLOAT_ADD(FLOAT_MULT(VAR1_SMOOTH, st->Dvar1), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.4f,15),Sff), MULT16_32_Q15(QCONST16(.4f,15),Dbf)));
814    st->Dvar2 = FLOAT_ADD(FLOAT_MULT(VAR2_SMOOTH, st->Dvar2), FLOAT_MUL32U(MULT16_32_Q15(QCONST16(.15f,15),Sff), MULT16_32_Q15(QCONST16(.15f,15),Dbf)));
815    
816    /* Equivalent float code:
817    st->Davg1 = .6*st->Davg1 + .4*(Sff-See);
818    st->Davg2 = .85*st->Davg2 + .15*(Sff-See);
819    st->Dvar1 = .36*st->Dvar1 + .16*Sff*Dbf;
820    st->Dvar2 = .7225*st->Dvar2 + .0225*Sff*Dbf;
821    */
822    
823    update_foreground = 0;
824    /* Check if we have a statistically significant reduction in the residual echo */
825    /* Note that this is *not* Gaussian, so we need to be careful about the longer tail */
826    if (FLOAT_GT(FLOAT_MUL32U(SUB32(Sff,See),ABS32(SUB32(Sff,See))), FLOAT_MUL32U(Sff,Dbf)))
827       update_foreground = 1;
828    else if (FLOAT_GT(FLOAT_MUL32U(st->Davg1, ABS32(st->Davg1)), FLOAT_MULT(VAR1_UPDATE,(st->Dvar1))))
829       update_foreground = 1;
830    else if (FLOAT_GT(FLOAT_MUL32U(st->Davg2, ABS32(st->Davg2)), FLOAT_MULT(VAR2_UPDATE,(st->Dvar2))))
831       update_foreground = 1;
832    
833    /* Do we update? */
834    if (update_foreground)
835    {
836       st->Davg1 = st->Davg2 = 0;
837       st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
838       /* Copy background filter to foreground filter */
839       for (i=0;i<N*M;i++)
840          st->foreground[i] = EXTRACT16(PSHR32(st->W[i],16));
841       /* Apply a smooth transition so as to not introduce blocking artifacts */
842       for (i=0;i<st->frame_size;i++)
843          st->e[i+st->frame_size] = MULT16_16_Q15(st->window[i+st->frame_size],st->e[i+st->frame_size]) + MULT16_16_Q15(st->window[i],st->y[i+st->frame_size]);
844    } else {
845       int reset_background=0;
846       /* Otherwise, check if the background filter is significantly worse */
847       if (FLOAT_GT(FLOAT_MUL32U(NEG32(SUB32(Sff,See)),ABS32(SUB32(Sff,See))), FLOAT_MULT(VAR_BACKTRACK,FLOAT_MUL32U(Sff,Dbf))))
848          reset_background = 1;
849       if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg1), ABS32(st->Davg1)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar1)))
850          reset_background = 1;
851       if (FLOAT_GT(FLOAT_MUL32U(NEG32(st->Davg2), ABS32(st->Davg2)), FLOAT_MULT(VAR_BACKTRACK,st->Dvar2)))
852          reset_background = 1;
853       if (reset_background)
854       {
855          /* Copy foreground filter to background filter */
856          for (i=0;i<N*M;i++)
857             st->W[i] = SHL32(EXTEND32(st->foreground[i]),16);
858          /* We also need to copy the output so as to get correct adaptation */
859          for (i=0;i<st->frame_size;i++)
860             st->y[i+st->frame_size] = st->e[i+st->frame_size];
861          for (i=0;i<st->frame_size;i++)
862             st->e[i] = SUB16(st->input[i], st->y[i+st->frame_size]);
863          See = Sff;
864          st->Davg1 = st->Davg2 = 0;
865          st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
866       }
867    }
868 #endif
869
870    /* Compute error signal (for the output with de-emphasis) */ 
871    for (i=0;i<st->frame_size;i++)
872    {
873       spx_word32_t tmp_out;
874 #ifdef TWO_PATH
875       tmp_out = SUB32(EXTEND32(st->input[i]), EXTEND32(st->e[i+st->frame_size]));
876 #else
877       tmp_out = SUB32(EXTEND32(st->input[i]), EXTEND32(st->y[i+st->frame_size]));
878 #endif
879       /* Saturation */
880       if (tmp_out>32767)
881          tmp_out = 32767;
882       else if (tmp_out<-32768)
883          tmp_out = -32768;
884       tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE)));
885       /* This is an arbitrary test for saturation in the microphone signal */
886       if (in[i] <= -32000 || in[i] >= 32000)
887       {
888          tmp_out = 0;
889          if (st->saturated == 0)
890             st->saturated = 1;
891       }
892       out[i] = (spx_int16_t)tmp_out;
893       st->memE = tmp_out;
894    }
895    
896 #ifdef DUMP_ECHO_CANCEL_DATA
897    dump_audio(in, far_end, out, st->frame_size);
898 #endif
899    
900    /* Compute error signal (filter update version) */ 
901    for (i=0;i<st->frame_size;i++)
902    {
903       st->e[i+st->frame_size] = st->e[i];
904       st->e[i] = 0;
905    }
906
907    /* Compute a bunch of correlations */
908    Sey = mdf_inner_prod(st->e+st->frame_size, st->y+st->frame_size, st->frame_size);
909    Syy = mdf_inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
910    Sdd = mdf_inner_prod(st->input, st->input, st->frame_size);
911    
912    /*printf ("%f %f %f %f\n", Sff, See, Syy, Sdd, st->update_cond);*/
913    
914    /* Do some sanity check */
915    if (!(Syy>=0 && Sxx>=0 && See >= 0)
916 #ifndef FIXED_POINT
917        || !(Sff < N*1e9 && Syy < N*1e9 && Sxx < N*1e9)
918 #endif
919       )
920    {
921       /* Things have gone really bad */
922       st->screwed_up += 50;
923       for (i=0;i<st->frame_size;i++)
924          out[i] = 0;
925    } else if (SHR32(Sff, 2) > ADD32(Sdd, SHR32(MULT16_16(N, 10000),6)))
926    {
927       /* AEC seems to add lots of echo instead of removing it, let's see if it will improve */
928       st->screwed_up++;
929    } else {
930       /* Everything's fine */
931       st->screwed_up=0;
932    }
933    if (st->screwed_up>=50)
934    {
935       speex_warning("The echo canceller started acting funny and got slapped (reset). It swears it will behave now.");
936       speex_echo_state_reset(st);
937       return;
938    }
939
940    /* Add a small noise floor to make sure not to have problems when dividing */
941    See = MAX32(See, SHR32(MULT16_16(N, 100),6));
942
943    /* Convert error to frequency domain */
944    spx_fft(st->fft_table, st->e, st->E);
945    for (i=0;i<st->frame_size;i++)
946       st->y[i] = 0;
947    spx_fft(st->fft_table, st->y, st->Y);
948
949    /* Compute power spectrum of far end (X), error (E) and filter response (Y) */
950    power_spectrum(st->E, st->Rf, N);
951    power_spectrum(st->Y, st->Yf, N);
952    power_spectrum(st->X, st->Xf, N);
953    
954    /* Smooth far end energy estimate over time */
955    for (j=0;j<=st->frame_size;j++)
956       st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]);
957    
958    /* Enable this to compute the power based only on the tail (would need to compute more 
959       efficiently to make this really useful */
960    if (0)
961    {
962       float scale2 = .5f/M;
963       for (j=0;j<=st->frame_size;j++)
964          st->power[j] = 100;
965       for (i=0;i<M;i++)
966       {
967          power_spectrum(&st->X[i*N], st->Xf, N);
968          for (j=0;j<=st->frame_size;j++)
969             st->power[j] += scale2*st->Xf[j];
970       }
971    }
972
973    /* Compute filtered spectra and (cross-)correlations */
974    for (j=st->frame_size;j>=0;j--)
975    {
976       spx_float_t Eh, Yh;
977       Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]);
978       Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]);
979       Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));
980       Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));
981 #ifdef FIXED_POINT
982       st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]);
983       st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]);
984 #else
985       st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j];
986       st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j];
987 #endif
988    }
989    
990    Pyy = FLOAT_SQRT(Pyy);
991    Pey = FLOAT_DIVU(Pey,Pyy);
992
993    /* Compute correlation updatete rate */
994    tmp32 = MULT16_32_Q15(st->beta0,Syy);
995    if (tmp32 > MULT16_32_Q15(st->beta_max,See))
996       tmp32 = MULT16_32_Q15(st->beta_max,See);
997    alpha = FLOAT_DIV32(tmp32, See);
998    alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha);
999    /* Update correlations (recursive average) */
1000    st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey));
1001    st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy));
1002    if (FLOAT_LT(st->Pyy, FLOAT_ONE))
1003       st->Pyy = FLOAT_ONE;
1004    /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */
1005    if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy)))
1006       st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy);
1007    if (FLOAT_GT(st->Pey, st->Pyy))
1008       st->Pey = st->Pyy;
1009    /* leak_estimate is the linear regression result */
1010    st->leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14));
1011    /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */
1012    if (st->leak_estimate > 16383)
1013       st->leak_estimate = 32767;
1014    else
1015       st->leak_estimate = SHL16(st->leak_estimate,1);
1016    /*printf ("%f\n", st->leak_estimate);*/
1017    
1018    /* Compute Residual to Error Ratio */
1019 #ifdef FIXED_POINT
1020    tmp32 = MULT16_32_Q15(st->leak_estimate,Syy);
1021    tmp32 = ADD32(SHR32(Sxx,13), ADD32(tmp32, SHL32(tmp32,1)));
1022    /* Check for y in e (lower bound on RER) */
1023    {
1024       spx_float_t bound = PSEUDOFLOAT(Sey);
1025       bound = FLOAT_DIVU(FLOAT_MULT(bound, bound), PSEUDOFLOAT(ADD32(1,Syy)));
1026       if (FLOAT_GT(bound, PSEUDOFLOAT(See)))
1027          tmp32 = See;
1028       else if (tmp32 < FLOAT_EXTRACT32(bound))
1029          tmp32 = FLOAT_EXTRACT32(bound);
1030    }
1031    if (tmp32 > SHR32(See,1))
1032       tmp32 = SHR32(See,1);
1033    RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15));
1034 #else
1035    RER = (.0001*Sxx + 3.*MULT16_32_Q15(st->leak_estimate,Syy)) / See;
1036    /* Check for y in e (lower bound on RER) */
1037    if (RER < Sey*Sey/(1+See*Syy))
1038       RER = Sey*Sey/(1+See*Syy);
1039    if (RER > .5)
1040       RER = .5;
1041 #endif
1042
1043    /* We consider that the filter has had minimal adaptation if the following is true*/
1044    if (!st->adapted && st->sum_adapt > QCONST32(M,15) && MULT16_32_Q15(st->leak_estimate,Syy) > MULT16_32_Q15(QCONST16(.03f,15),Syy))
1045    {
1046       st->adapted = 1;
1047    }
1048
1049    if (st->adapted)
1050    {
1051       /* Normal learning rate calculation once we're past the minimal adaptation phase */
1052       for (i=0;i<=st->frame_size;i++)
1053       {
1054          spx_word32_t r, e;
1055          /* Compute frequency-domain adaptation mask */
1056          r = MULT16_32_Q15(st->leak_estimate,SHL32(st->Yf[i],3));
1057          e = SHL32(st->Rf[i],3)+1;
1058 #ifdef FIXED_POINT
1059          if (r>SHR32(e,1))
1060             r = SHR32(e,1);
1061 #else
1062          if (r>.5*e)
1063             r = .5*e;
1064 #endif
1065          r = MULT16_32_Q15(QCONST16(.7,15),r) + MULT16_32_Q15(QCONST16(.3,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));
1066          /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/
1067          st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(r,FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16);
1068       }
1069    } else {
1070       /* Temporary adaption rate if filter is not yet adapted enough */
1071       spx_word16_t adapt_rate=0;
1072
1073       if (Sxx > SHR32(MULT16_16(N, 1000),6)) 
1074       {
1075          tmp32 = MULT16_32_Q15(QCONST16(.25f, 15), Sxx);
1076 #ifdef FIXED_POINT
1077          if (tmp32 > SHR32(See,2))
1078             tmp32 = SHR32(See,2);
1079 #else
1080          if (tmp32 > .25*See)
1081             tmp32 = .25*See;
1082 #endif
1083          adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32, See),15));
1084       }
1085       for (i=0;i<=st->frame_size;i++)
1086          st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1);
1087
1088
1089       /* How much have we adapted so far? */
1090       st->sum_adapt = ADD32(st->sum_adapt,adapt_rate);
1091    }
1092
1093    /* Save residual echo so it can be used by the nonlinear processor */
1094    if (st->adapted)
1095    {
1096       /* If the filter is adapted, take the filtered echo */
1097       for (i=0;i<st->frame_size;i++)
1098          st->last_y[i] = st->last_y[st->frame_size+i];
1099       for (i=0;i<st->frame_size;i++)
1100          st->last_y[st->frame_size+i] = in[i]-out[i];
1101    } else {
1102       /* If filter isn't adapted yet, all we can do is take the far end signal directly */
1103       /* moved earlier: for (i=0;i<N;i++)
1104       st->last_y[i] = st->x[i];*/
1105    }
1106
1107 }
1108
1109 /* Compute spectrum of estimated echo for use in an echo post-filter */
1110 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *residual_echo, int len)
1111 {
1112    int i;
1113    spx_word16_t leak2;
1114    int N;
1115    
1116    N = st->window_size;
1117
1118    /* Apply hanning window (should pre-compute it)*/
1119    for (i=0;i<N;i++)
1120       st->y[i] = MULT16_16_Q15(st->window[i],st->last_y[i]);
1121       
1122    /* Compute power spectrum of the echo */
1123    spx_fft(st->fft_table, st->y, st->Y);
1124    power_spectrum(st->Y, residual_echo, N);
1125       
1126 #ifdef FIXED_POINT
1127    if (st->leak_estimate > 16383)
1128       leak2 = 32767;
1129    else
1130       leak2 = SHL16(st->leak_estimate, 1);
1131 #else
1132    if (st->leak_estimate>.5)
1133       leak2 = 1;
1134    else
1135       leak2 = 2*st->leak_estimate;
1136 #endif
1137    /* Estimate residual echo */
1138    for (i=0;i<=st->frame_size;i++)
1139       residual_echo[i] = (spx_int32_t)MULT16_32_Q15(leak2,residual_echo[i]);
1140    
1141 }
1142
1143 int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
1144 {
1145    switch(request)
1146    {
1147       
1148       case SPEEX_ECHO_GET_FRAME_SIZE:
1149          (*(int*)ptr) = st->frame_size;
1150          break;
1151       case SPEEX_ECHO_SET_SAMPLING_RATE:
1152          st->sampling_rate = (*(int*)ptr);
1153          st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
1154 #ifdef FIXED_POINT
1155          st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
1156          st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
1157 #else
1158          st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
1159          st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
1160 #endif
1161          if (st->sampling_rate<12000)
1162             st->notch_radius = QCONST16(.9, 15);
1163          else if (st->sampling_rate<24000)
1164             st->notch_radius = QCONST16(.982, 15);
1165          else
1166             st->notch_radius = QCONST16(.992, 15);
1167          break;
1168       case SPEEX_ECHO_GET_SAMPLING_RATE:
1169          (*(int*)ptr) = st->sampling_rate;
1170          break;
1171       default:
1172          speex_warning_int("Unknown speex_echo_ctl request: ", request);
1173          return -1;
1174    }
1175    return 0;
1176 }