decoder excitation now in 16-bit precision (was 32), which saves quite a bit
[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. Submitted to IEEE Transactions on Speech 
45    and Audio Processing, 2006.
46    
47    There is no explicit double-talk detection, but a continuous variation
48    in the learning rate based on residual echo, double-talk and background
49    noise.
50    
51    About the fixed-point version:
52    All the signals are represented with 16-bit words. The filter weights 
53    are represented with 32-bit words, but only the top 16 bits are used
54    in most cases. The lower 16 bits are completely unreliable (due to the
55    fact that the update is done only on the top bits), but help in the
56    adaptation -- probably by removing a "threshold effect" due to
57    quantization (rounding going to zero) when the gradient is small.
58    
59    Another kludge that seems to work good: when performing the weight
60    update, we only move half the way toward the "goal" this seems to
61    reduce the effect of quantization noise in the update phase. This
62    can be seen as applying a gradient descent on a "soft constraint"
63    instead of having a hard constraint.
64    
65 */
66
67 #ifdef HAVE_CONFIG_H
68 #include "config.h"
69 #endif
70
71 #include "misc.h"
72 #include "speex/speex_echo.h"
73 #include "fftwrap.h"
74 #include "pseudofloat.h"
75 #include "math_approx.h"
76
77 #ifndef M_PI
78 #define M_PI 3.14159265358979323846
79 #endif
80
81 #define min(a,b) ((a)<(b) ? (a) : (b))
82 #define max(a,b) ((a)>(b) ? (a) : (b))
83
84 #ifdef FIXED_POINT
85 #define WEIGHT_SHIFT 11
86 #define NORMALIZE_SCALEDOWN 5
87 #define NORMALIZE_SCALEUP 3
88 #else
89 #define WEIGHT_SHIFT 0
90 #endif
91
92 #ifdef FIXED_POINT
93 static const spx_float_t MIN_LEAK = {16777, -24};
94 #define TOP16(x) ((x)>>16)
95 #else
96 static const spx_float_t MIN_LEAK = .001f;
97 #define TOP16(x) (x)
98 #endif
99
100
101 /** Speex echo cancellation state. */
102 struct SpeexEchoState_ {
103    int frame_size;           /**< Number of samples processed each time */
104    int window_size;
105    int M;
106    int cancel_count;
107    int adapted;
108    spx_int32_t sampling_rate;
109    spx_word16_t spec_average;
110    spx_word16_t beta0;
111    spx_word16_t beta_max;
112    spx_word32_t sum_adapt;
113    spx_word16_t *e;
114    spx_word16_t *x;
115    spx_word16_t *X;
116    spx_word16_t *d;
117    spx_word16_t *y;
118    spx_word16_t *last_y;
119    spx_word32_t *Yps;
120    spx_word16_t *Y;
121    spx_word16_t *E;
122    spx_word32_t *PHI;
123    spx_word32_t *W;
124    spx_word32_t *power;
125    spx_float_t *power_1;
126    spx_word16_t *wtmp;
127 #ifdef FIXED_POINT
128    spx_word16_t *wtmp2;
129 #endif
130    spx_word32_t *Rf;
131    spx_word32_t *Yf;
132    spx_word32_t *Xf;
133    spx_word32_t *Eh;
134    spx_word32_t *Yh;
135    spx_float_t Pey;
136    spx_float_t Pyy;
137    spx_word16_t *window;
138    void *fft_table;
139    spx_word16_t memX, memD, memE;
140    spx_word16_t preemph;
141    spx_word16_t notch_radius;
142    spx_mem_t notch_mem[2];
143
144    /* NOTE: If you only use speex_echo_cancel() and want to save some memory, remove this */
145    spx_int16_t *play_buf;
146    int play_buf_pos;
147 };
148
149 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)
150 {
151    int i;
152    spx_word16_t den2;
153 #ifdef FIXED_POINT
154    den2 = MULT16_16_Q15(radius,radius) + MULT16_16_Q15(QCONST16(.7,15),MULT16_16_Q15(32767-radius,32767-radius));
155 #else
156    den2 = radius*radius + .7*(1-radius)*(1-radius);
157 #endif   
158    /*printf ("%d %d %d %d %d %d\n", num[0], num[1], num[2], den[0], den[1], den[2]);*/
159    for (i=0;i<len;i++)
160    {
161       spx_word16_t vin = in[i];
162       spx_word32_t vout = mem[0] + SHL32(EXTEND32(vin),15);
163 #ifdef FIXED_POINT
164       mem[0] = mem[1] + SHL32(SHL32(-EXTEND32(vin),15) + MULT16_32_Q15(radius,vout),1);
165 #else
166       mem[0] = mem[1] + 2*(-vin + radius*vout);
167 #endif
168       mem[1] = SHL32(EXTEND32(vin),15) - MULT16_32_Q15(den2,vout);
169       out[i] = SATURATE32(PSHR32(MULT16_32_Q15(radius,vout),15),32767);
170    }
171 }
172
173 static inline spx_word32_t mdf_inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
174 {
175    spx_word32_t sum=0;
176    len >>= 1;
177    while(len--)
178    {
179       spx_word32_t part=0;
180       part = MAC16_16(part,*x++,*y++);
181       part = MAC16_16(part,*x++,*y++);
182       /* HINT: If you had a 40-bit accumulator, you could shift only at the end */
183       sum = ADD32(sum,SHR32(part,6));
184    }
185    return sum;
186 }
187
188 /** Compute power spectrum of a half-complex (packed) vector */
189 static inline void power_spectrum(const spx_word16_t *X, spx_word32_t *ps, int N)
190 {
191    int i, j;
192    ps[0]=MULT16_16(X[0],X[0]);
193    for (i=1,j=1;i<N-1;i+=2,j++)
194    {
195       ps[j] =  MULT16_16(X[i],X[i]) + MULT16_16(X[i+1],X[i+1]);
196    }
197    ps[j]=MULT16_16(X[i],X[i]);
198 }
199
200 /** Compute cross-power spectrum of a half-complex (packed) vectors and add to acc */
201 #ifdef FIXED_POINT
202 static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M)
203 {
204    int i,j;
205    spx_word32_t tmp1=0,tmp2=0;
206    for (j=0;j<M;j++)
207    {
208       tmp1 = MAC16_16(tmp1, X[j*N],TOP16(Y[j*N]));
209    }
210    acc[0] = PSHR32(tmp1,WEIGHT_SHIFT);
211    for (i=1;i<N-1;i+=2)
212    {
213       tmp1 = tmp2 = 0;
214       for (j=0;j<M;j++)
215       {
216          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])));
217          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]));
218       }
219       acc[i] = PSHR32(tmp1,WEIGHT_SHIFT);
220       acc[i+1] = PSHR32(tmp2,WEIGHT_SHIFT);
221    }
222    tmp1 = tmp2 = 0;
223    for (j=0;j<M;j++)
224    {
225       tmp1 = MAC16_16(tmp1, X[(j+1)*N-1],TOP16(Y[(j+1)*N-1]));
226    }
227    acc[N-1] = PSHR32(tmp1,WEIGHT_SHIFT);
228 }
229 #else
230 static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M)
231 {
232    int i,j;
233    for (i=0;i<N;i++)
234       acc[i] = 0;
235    for (j=0;j<M;j++)
236    {
237       acc[0] += X[0]*Y[0];
238       for (i=1;i<N-1;i+=2)
239       {
240          acc[i] += (X[i]*Y[i] - X[i+1]*Y[i+1]);
241          acc[i+1] += (X[i+1]*Y[i] + X[i]*Y[i+1]);
242       }
243       acc[i] += X[i]*Y[i];
244       X += N;
245       Y += N;
246    }
247 }
248 #endif
249
250 /** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */
251 static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N)
252 {
253    int i, j;
254    prod[0] = FLOAT_MUL32(w[0],MULT16_16(X[0],Y[0]));
255    for (i=1,j=1;i<N-1;i+=2,j++)
256    {
257       prod[i] = FLOAT_MUL32(w[j],MAC16_16(MULT16_16(X[i],Y[i]), X[i+1],Y[i+1]));
258       prod[i+1] = FLOAT_MUL32(w[j],MAC16_16(MULT16_16(-X[i+1],Y[i]), X[i],Y[i+1]));
259    }
260    prod[i] = FLOAT_MUL32(w[j],MULT16_16(X[i],Y[i]));
261 }
262
263
264 /** Creates a new echo canceller state */
265 SpeexEchoState *speex_echo_state_init(int frame_size, int filter_length)
266 {
267    int i,N,M;
268    SpeexEchoState *st = (SpeexEchoState *)speex_alloc(sizeof(SpeexEchoState));
269
270    st->frame_size = frame_size;
271    st->window_size = 2*frame_size;
272    N = st->window_size;
273    M = st->M = (filter_length+st->frame_size-1)/frame_size;
274    st->cancel_count=0;
275    st->sum_adapt = 0;
276    /* FIXME: Make that an init option (new API call?) */
277    st->sampling_rate = 8000;
278    st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
279 #ifdef FIXED_POINT
280    st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
281    st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
282 #else
283    st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
284    st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
285 #endif
286
287    st->fft_table = spx_fft_init(N);
288    
289    st->e = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
290    st->x = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
291    st->d = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
292    st->y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
293    st->Yps = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
294    st->last_y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
295    st->Yf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
296    st->Rf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
297    st->Xf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
298    st->Yh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
299    st->Eh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
300
301    st->X = (spx_word16_t*)speex_alloc(M*N*sizeof(spx_word16_t));
302    st->Y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
303    st->E = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
304    st->W = (spx_word32_t*)speex_alloc(M*N*sizeof(spx_word32_t));
305    st->PHI = (spx_word32_t*)speex_alloc(M*N*sizeof(spx_word32_t));
306    st->power = (spx_word32_t*)speex_alloc((frame_size+1)*sizeof(spx_word32_t));
307    st->power_1 = (spx_float_t*)speex_alloc((frame_size+1)*sizeof(spx_float_t));
308    st->window = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
309    st->wtmp = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
310 #ifdef FIXED_POINT
311    st->wtmp2 = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
312    for (i=0;i<N>>1;i++)
313    {
314       st->window[i] = (16383-SHL16(spx_cos(DIV32_16(MULT16_16(25736,i<<1),N)),1));
315       st->window[N-i-1] = st->window[i];
316    }
317 #else
318    for (i=0;i<N;i++)
319       st->window[i] = .5-.5*cos(2*M_PI*i/N);
320 #endif
321    for (i=0;i<N*M;i++)
322    {
323       st->W[i] = st->PHI[i] = 0;
324    }
325    st->memX=st->memD=st->memE=0;
326    st->preemph = QCONST16(.9,15);
327    if (st->sampling_rate<12000)
328       st->notch_radius = QCONST16(.9, 15);
329    else if (st->sampling_rate<24000)
330       st->notch_radius = QCONST16(.982, 15);
331    else
332       st->notch_radius = QCONST16(.992, 15);
333
334    st->notch_mem[0] = st->notch_mem[1] = 0;
335    st->adapted = 0;
336    st->Pey = st->Pyy = FLOAT_ONE;
337    
338    st->play_buf = (spx_int16_t*)speex_alloc(2*st->frame_size*sizeof(spx_int16_t));
339    st->play_buf_pos = 0;
340
341    return st;
342 }
343
344 /** Resets echo canceller state */
345 void speex_echo_state_reset(SpeexEchoState *st)
346 {
347    int i, M, N;
348    st->cancel_count=0;
349    N = st->window_size;
350    M = st->M;
351    for (i=0;i<N*M;i++)
352    {
353       st->W[i] = 0;
354       st->X[i] = 0;
355    }
356    for (i=0;i<=st->frame_size;i++)
357       st->power[i] = 0;
358    
359    st->adapted = 0;
360    st->sum_adapt = 0;
361    st->Pey = st->Pyy = FLOAT_ONE;
362
363 }
364
365 /** Destroys an echo canceller state */
366 void speex_echo_state_destroy(SpeexEchoState *st)
367 {
368    spx_fft_destroy(st->fft_table);
369
370    speex_free(st->e);
371    speex_free(st->x);
372    speex_free(st->d);
373    speex_free(st->y);
374    speex_free(st->last_y);
375    speex_free(st->Yps);
376    speex_free(st->Yf);
377    speex_free(st->Rf);
378    speex_free(st->Xf);
379    speex_free(st->Yh);
380    speex_free(st->Eh);
381
382    speex_free(st->X);
383    speex_free(st->Y);
384    speex_free(st->E);
385    speex_free(st->W);
386    speex_free(st->PHI);
387    speex_free(st->power);
388    speex_free(st->power_1);
389    speex_free(st->window);
390    speex_free(st->wtmp);
391 #ifdef FIXED_POINT
392    speex_free(st->wtmp2);
393 #endif
394    speex_free(st);
395 }
396
397 void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out, spx_int32_t *Yout)
398 {
399    int i;
400    if (st->play_buf_pos>=st->frame_size)
401    {
402       speex_echo_cancel(st, rec, st->play_buf, out, Yout);
403       st->play_buf_pos -= st->frame_size;
404       for (i=0;i<st->frame_size;i++)
405          st->play_buf[i] = st->play_buf[i+st->frame_size];
406    } else {
407       speex_warning("no playback frame available");
408       if (st->play_buf_pos!=0)
409       {
410          speex_warning("internal playback buffer corruption?");
411          st->play_buf_pos = 0;
412       }
413       for (i=0;i<st->frame_size;i++)
414          out[i] = rec[i];
415    }
416 }
417
418 void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play)
419 {
420    if (st->play_buf_pos<=st->frame_size)
421    {
422       int i;
423       for (i=0;i<st->frame_size;i++)
424          st->play_buf[st->play_buf_pos+i] = play[i];
425       st->play_buf_pos += st->frame_size;
426    } else {
427       speex_warning("had to discard a playback frame");
428    }
429 }
430
431 /** Performs echo cancellation on a frame */
432 void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *ref, const spx_int16_t *echo, spx_int16_t *out, spx_int32_t *Yout)
433 {
434    int i,j;
435    int N,M;
436    spx_word32_t Syy,See;
437    spx_word16_t leak_estimate;
438    spx_word16_t ss, ss_1;
439    spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
440    spx_float_t alpha, alpha_1;
441    spx_word16_t RER;
442    spx_word32_t tmp32;
443    spx_word16_t M_1;
444    int saturated=0;
445    
446    N = st->window_size;
447    M = st->M;
448    st->cancel_count++;
449 #ifdef FIXED_POINT
450    ss=DIV32_16(11469,M);
451    ss_1 = SUB16(32767,ss);
452    M_1 = DIV32_16(32767,M);
453 #else
454    ss=.35/M;
455    ss_1 = 1-ss;
456    M_1 = 1.f/M;
457 #endif
458
459    filter_dc_notch16(ref, st->notch_radius, st->d, st->frame_size, st->notch_mem);
460    /* Copy input data to buffer */
461    for (i=0;i<st->frame_size;i++)
462    {
463       spx_word16_t tmp;
464       spx_word32_t tmp32;
465       st->x[i] = st->x[i+st->frame_size];
466       tmp32 = SUB32(EXTEND32(echo[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX)));
467 #ifdef FIXED_POINT
468       /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */
469       if (tmp32 > 32767)
470       {
471          tmp32 = 32767;
472          saturated = 1;
473       }      
474       if (tmp32 < -32767)
475       {
476          tmp32 = -32767;
477          saturated = 1;
478       }      
479 #endif
480       st->x[i+st->frame_size] = EXTRACT16(tmp32);
481       st->memX = echo[i];
482       
483       tmp = st->d[i];
484       st->d[i] = st->d[i+st->frame_size];
485       tmp32 = SUB32(EXTEND32(tmp), EXTEND32(MULT16_16_P15(st->preemph, st->memD)));
486 #ifdef FIXED_POINT
487       if (tmp32 > 32767)
488       {
489          tmp32 = 32767;
490          saturated = 1;
491       }      
492       if (tmp32 < -32767)
493       {
494          tmp32 = -32767;
495          saturated = 1;
496       }
497 #endif
498       st->d[i+st->frame_size] = tmp32;
499       st->memD = tmp;
500    }
501
502    /* Shift memory: this could be optimized eventually*/
503    for (i=0;i<N*(M-1);i++)
504       st->X[i]=st->X[i+N];
505
506    /* Convert x (echo input) to frequency domain */
507    spx_fft(st->fft_table, st->x, &st->X[(M-1)*N]);
508    
509    /* Compute filter response Y */
510    spectral_mul_accum(st->X, st->W, st->Y, N, M);
511    
512    spx_ifft(st->fft_table, st->Y, st->y);
513
514 #if 1
515    spectral_mul_accum(st->X, st->PHI, st->Y, N, M);   
516    spx_ifft(st->fft_table, st->Y, st->e);
517 #endif
518
519    /* Compute error signal (for the output with de-emphasis) */ 
520    for (i=0;i<st->frame_size;i++)
521    {
522       spx_word32_t tmp_out;
523 #if 1
524       spx_word16_t y = 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]);
525       tmp_out = SUB32(EXTEND32(st->d[i+st->frame_size]), EXTEND32(y));
526 #else
527       tmp_out = SUB32(EXTEND32(st->d[i+st->frame_size]), EXTEND32(st->y[i+st->frame_size]));
528 #endif
529
530       /* Saturation */
531       if (tmp_out>32767)
532          tmp_out = 32767;
533       else if (tmp_out<-32768)
534          tmp_out = -32768;
535       tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE)));
536       /* This is an arbitrary test for saturation */
537       if (ref[i] <= -32000 || ref[i] >= 32000)
538       {
539          tmp_out = 0;
540          saturated = 1;
541       }
542       out[i] = tmp_out;
543       st->memE = tmp_out;
544    }
545
546    /* Compute error signal (filter update version) */ 
547    for (i=0;i<st->frame_size;i++)
548    {
549       st->e[i] = 0;
550       st->e[i+st->frame_size] = st->d[i+st->frame_size] - st->y[i+st->frame_size];
551    }
552
553    /* Compute a bunch of correlations */
554    See = mdf_inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size);
555    See = ADD32(See, SHR32(EXTEND32(10000),6));
556    Syy = mdf_inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
557    
558    /* Convert error to frequency domain */
559    spx_fft(st->fft_table, st->e, st->E);
560    for (i=0;i<st->frame_size;i++)
561       st->y[i] = 0;
562    spx_fft(st->fft_table, st->y, st->Y);
563
564    /* Compute power spectrum of echo (X), error (E) and filter response (Y) */
565    power_spectrum(st->E, st->Rf, N);
566    power_spectrum(st->Y, st->Yf, N);
567    power_spectrum(&st->X[(M-1)*N], st->Xf, N);
568    
569    /* Smooth echo energy estimate over time */
570    for (j=0;j<=st->frame_size;j++)
571       st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]);
572    
573    /* Enable this to compute the power based only on the tail (would need to compute more 
574       efficiently to make this really useful */
575    if (0)
576    {
577       float scale2 = .5f/M;
578       for (j=0;j<=st->frame_size;j++)
579          st->power[j] = 0;
580       for (i=0;i<M;i++)
581       {
582          power_spectrum(&st->X[i*N], st->Xf, N);
583          for (j=0;j<=st->frame_size;j++)
584             st->power[j] += scale2*st->Xf[j];
585       }
586    }
587
588    /* Compute filtered spectra and (cross-)correlations */
589    for (j=st->frame_size;j>=0;j--)
590    {
591       spx_float_t Eh, Yh;
592       Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]);
593       Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]);
594       Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));
595       Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));
596 #ifdef FIXED_POINT
597       st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]);
598       st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]);
599 #else
600       st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j];
601       st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j];
602 #endif
603    }
604    
605    /* Compute correlation updatete rate */
606    tmp32 = MULT16_32_Q15(st->beta0,Syy);
607    if (tmp32 > MULT16_32_Q15(st->beta_max,See))
608       tmp32 = MULT16_32_Q15(st->beta_max,See);
609    alpha = FLOAT_DIV32(tmp32, See);
610    alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha);
611    /* Update correlations (recursive average) */
612    st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey));
613    st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy));
614    if (FLOAT_LT(st->Pyy, FLOAT_ONE))
615       st->Pyy = FLOAT_ONE;
616    /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */
617    if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy)))
618       st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy);
619    if (FLOAT_GT(st->Pey, st->Pyy))
620       st->Pey = st->Pyy;
621    /* leak_estimate is the linear regression result */
622    leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14));
623    /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */
624    if (leak_estimate > 16383)
625       leak_estimate = 32767;
626    else
627       leak_estimate = SHL16(leak_estimate,1);
628    /*printf ("%f\n", leak_estimate);*/
629    
630    /* Compute Residual to Error Ratio */
631 #ifdef FIXED_POINT
632    tmp32 = MULT16_32_Q15(leak_estimate,Syy);
633    tmp32 = ADD32(tmp32, SHL32(tmp32,1));
634    if (tmp32 > SHR32(See,1))
635       tmp32 = SHR32(See,1);
636    RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15));
637 #else
638    RER = 3.*MULT16_32_Q15(leak_estimate,Syy) / See;
639    if (RER > .5)
640       RER = .5;
641 #endif
642
643    /* We consider that the filter has had minimal adaptation if the following is true*/
644    if (!st->adapted && st->sum_adapt > QCONST32(1,15))
645    {
646       st->adapted = 1;
647    }
648
649    if (st->adapted)
650    {
651       for (i=0;i<=st->frame_size;i++)
652       {
653          spx_word32_t r, e;
654          /* Compute frequency-domain adaptation mask */
655          r = MULT16_32_Q15(leak_estimate,SHL32(st->Yf[i],3));
656          e = SHL32(st->Rf[i],3)+1;
657 #ifdef FIXED_POINT
658          if (r>SHR32(e,1))
659             r = SHR32(e,1);
660 #else
661          if (r>.5*e)
662             r = .5*e;
663 #endif
664          r = MULT16_32_Q15(QCONST16(.8,15),r) + MULT16_32_Q15(QCONST16(.2,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));
665          /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/
666          st->power_1[i] = FLOAT_SHL(FLOAT_DIV32_FLOAT(MULT16_32_Q15(M_1,r),FLOAT_MUL32U(e,st->power[i]+10)),WEIGHT_SHIFT+16);
667       }
668    } else {
669       spx_word32_t Sxx;
670       spx_word16_t adapt_rate=0;
671
672       Sxx = mdf_inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);
673       /* Temporary adaption rate if filter is not adapted correctly */
674
675       tmp32 = MULT16_32_Q15(QCONST16(.15f, 15), Sxx);
676 #ifdef FIXED_POINT
677       if (Sxx > SHR32(See,2))
678          Sxx = SHR32(See,2);
679 #else
680       if (Sxx > .25*See)
681          Sxx = .25*See;      
682 #endif
683       adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(MULT16_32_Q15(M_1,Sxx), See),15));
684       
685       for (i=0;i<=st->frame_size;i++)
686          st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1);
687
688
689       /* How much have we adapted so far? */
690       st->sum_adapt = ADD32(st->sum_adapt,adapt_rate);
691    }
692    /* Compute weight gradient */
693    for (j=0;j<M;j++)
694    {
695       weighted_spectral_mul_conj(st->power_1, &st->X[j*N], st->E, st->PHI+N*j, N);
696    }
697
698    if (!saturated)
699    {
700       /* Gradient descent */
701       for (i=0;i<M*N;i++)
702       {
703          st->W[i] += st->PHI[i];
704          /* Old value of W in PHI */
705          st->PHI[i] = st->W[i] - st->PHI[i];
706       }
707    }
708    
709    /* Update weight to prevent circular convolution (MDF / AUMDF) */
710    for (j=0;j<M;j++)
711    {
712       /* This is a variant of the Alternatively Updated MDF (AUMDF) */
713       /* Remove the "if" to make this an MDF filter */
714       if (j==M-1 || st->cancel_count%(M-1) == j)
715       {
716 #ifdef FIXED_POINT
717          for (i=0;i<N;i++)
718             st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16));
719          spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
720          for (i=0;i<st->frame_size;i++)
721          {
722             st->wtmp[i]=0;
723          }
724          for (i=st->frame_size;i<N;i++)
725          {
726             st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
727          }
728          spx_fft(st->fft_table, st->wtmp, st->wtmp2);
729          /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
730          for (i=0;i<N;i++)
731             st->W[j*N+i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
732 #else
733          spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
734          for (i=st->frame_size;i<N;i++)
735          {
736             st->wtmp[i]=0;
737          }
738          spx_fft(st->fft_table, st->wtmp, &st->W[j*N]);
739 #endif
740       }
741    }
742
743    /* Compute spectrum of estimated echo for use in an echo post-filter (if necessary)*/
744    if (Yout)
745    {
746       spx_word16_t leak2;
747       if (st->adapted)
748       {
749          /* If the filter is adapted, take the filtered echo */
750          for (i=0;i<st->frame_size;i++)
751             st->last_y[i] = st->last_y[st->frame_size+i];
752          for (i=0;i<st->frame_size;i++)
753             st->last_y[st->frame_size+i] = ref[i]-out[i];
754       } else {
755          /* If filter isn't adapted yet, all we can do is take the echo signal directly */
756          for (i=0;i<N;i++)
757             st->last_y[i] = st->x[i];
758       }
759       
760       /* Apply hanning window (should pre-compute it)*/
761       for (i=0;i<N;i++)
762          st->y[i] = MULT16_16_Q15(st->window[i],st->last_y[i]);
763       
764       /* Compute power spectrum of the echo */
765       spx_fft(st->fft_table, st->y, st->Y);
766       power_spectrum(st->Y, st->Yps, N);
767       
768 #ifdef FIXED_POINT
769       if (leak_estimate > 16383)
770          leak2 = 32767;
771       else
772          leak2 = SHL16(leak_estimate, 1);
773 #else
774       if (leak_estimate>.5)
775          leak2 = 1;
776       else
777          leak2 = 2*leak_estimate;
778 #endif
779       /* Estimate residual echo */
780       for (i=0;i<=st->frame_size;i++)
781          Yout[i] = MULT16_32_Q15(leak2,st->Yps[i]);
782    }
783 }
784
785
786 int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
787 {
788    switch(request)
789    {
790       
791       case SPEEX_ECHO_GET_FRAME_SIZE:
792          (*(int*)ptr) = st->frame_size;
793          break;
794       case SPEEX_ECHO_SET_SAMPLING_RATE:
795          st->sampling_rate = (*(int*)ptr);
796          st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
797 #ifdef FIXED_POINT
798          st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
799          st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
800 #else
801          st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
802          st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
803 #endif
804          if (st->sampling_rate<12000)
805             st->notch_radius = QCONST16(.9, 15);
806          else if (st->sampling_rate<24000)
807             st->notch_radius = QCONST16(.982, 15);
808          else
809             st->notch_radius = QCONST16(.992, 15);
810          break;
811       case SPEEX_ECHO_GET_SAMPLING_RATE:
812          (*(int*)ptr) = st->sampling_rate;
813          break;
814       default:
815          speex_warning_int("Unknown speex_echo_ctl request: ", request);
816          return -1;
817    }
818    return 0;
819 }