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