16-bit clean shift in lsp_to_lpc()
[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->play_buf);
395    speex_free(st);
396 }
397
398 void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out, spx_int32_t *Yout)
399 {
400    int i;
401    if (st->play_buf_pos>=st->frame_size)
402    {
403       speex_echo_cancel(st, rec, st->play_buf, out, Yout);
404       st->play_buf_pos -= st->frame_size;
405       for (i=0;i<st->frame_size;i++)
406          st->play_buf[i] = st->play_buf[i+st->frame_size];
407    } else {
408       speex_warning("no playback frame available");
409       if (st->play_buf_pos!=0)
410       {
411          speex_warning("internal playback buffer corruption?");
412          st->play_buf_pos = 0;
413       }
414       for (i=0;i<st->frame_size;i++)
415          out[i] = rec[i];
416    }
417 }
418
419 void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play)
420 {
421    if (st->play_buf_pos<=st->frame_size)
422    {
423       int i;
424       for (i=0;i<st->frame_size;i++)
425          st->play_buf[st->play_buf_pos+i] = play[i];
426       st->play_buf_pos += st->frame_size;
427    } else {
428       speex_warning("had to discard a playback frame");
429    }
430 }
431
432 /** Performs echo cancellation on a frame */
433 void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *ref, const spx_int16_t *echo, spx_int16_t *out, spx_int32_t *Yout)
434 {
435    int i,j;
436    int N,M;
437    spx_word32_t Syy,See;
438    spx_word16_t leak_estimate;
439    spx_word16_t ss, ss_1;
440    spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
441    spx_float_t alpha, alpha_1;
442    spx_word16_t RER;
443    spx_word32_t tmp32;
444    spx_word16_t M_1;
445    int saturated=0;
446    
447    N = st->window_size;
448    M = st->M;
449    st->cancel_count++;
450 #ifdef FIXED_POINT
451    ss=DIV32_16(11469,M);
452    ss_1 = SUB16(32767,ss);
453    M_1 = DIV32_16(32767,M);
454 #else
455    ss=.35/M;
456    ss_1 = 1-ss;
457    M_1 = 1.f/M;
458 #endif
459
460    filter_dc_notch16(ref, st->notch_radius, st->d, st->frame_size, st->notch_mem);
461    /* Copy input data to buffer */
462    for (i=0;i<st->frame_size;i++)
463    {
464       spx_word16_t tmp;
465       spx_word32_t tmp32;
466       st->x[i] = st->x[i+st->frame_size];
467       tmp32 = SUB32(EXTEND32(echo[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX)));
468 #ifdef FIXED_POINT
469       /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */
470       if (tmp32 > 32767)
471       {
472          tmp32 = 32767;
473          saturated = 1;
474       }      
475       if (tmp32 < -32767)
476       {
477          tmp32 = -32767;
478          saturated = 1;
479       }      
480 #endif
481       st->x[i+st->frame_size] = EXTRACT16(tmp32);
482       st->memX = echo[i];
483       
484       tmp = st->d[i];
485       st->d[i] = st->d[i+st->frame_size];
486       tmp32 = SUB32(EXTEND32(tmp), EXTEND32(MULT16_16_P15(st->preemph, st->memD)));
487 #ifdef FIXED_POINT
488       if (tmp32 > 32767)
489       {
490          tmp32 = 32767;
491          saturated = 1;
492       }      
493       if (tmp32 < -32767)
494       {
495          tmp32 = -32767;
496          saturated = 1;
497       }
498 #endif
499       st->d[i+st->frame_size] = tmp32;
500       st->memD = tmp;
501    }
502
503    /* Shift memory: this could be optimized eventually*/
504    for (i=0;i<N*(M-1);i++)
505       st->X[i]=st->X[i+N];
506
507    /* Convert x (echo input) to frequency domain */
508    spx_fft(st->fft_table, st->x, &st->X[(M-1)*N]);
509    
510    /* Compute filter response Y */
511    spectral_mul_accum(st->X, st->W, st->Y, N, M);
512    
513    spx_ifft(st->fft_table, st->Y, st->y);
514
515 #if 1
516    spectral_mul_accum(st->X, st->PHI, st->Y, N, M);   
517    spx_ifft(st->fft_table, st->Y, st->e);
518 #endif
519
520    /* Compute error signal (for the output with de-emphasis) */ 
521    for (i=0;i<st->frame_size;i++)
522    {
523       spx_word32_t tmp_out;
524 #if 1
525       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]);
526       tmp_out = SUB32(EXTEND32(st->d[i+st->frame_size]), EXTEND32(y));
527 #else
528       tmp_out = SUB32(EXTEND32(st->d[i+st->frame_size]), EXTEND32(st->y[i+st->frame_size]));
529 #endif
530
531       /* Saturation */
532       if (tmp_out>32767)
533          tmp_out = 32767;
534       else if (tmp_out<-32768)
535          tmp_out = -32768;
536       tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE)));
537       /* This is an arbitrary test for saturation */
538       if (ref[i] <= -32000 || ref[i] >= 32000)
539       {
540          tmp_out = 0;
541          saturated = 1;
542       }
543       out[i] = tmp_out;
544       st->memE = tmp_out;
545    }
546
547    /* Compute error signal (filter update version) */ 
548    for (i=0;i<st->frame_size;i++)
549    {
550       st->e[i] = 0;
551       st->e[i+st->frame_size] = st->d[i+st->frame_size] - st->y[i+st->frame_size];
552    }
553
554    /* Compute a bunch of correlations */
555    See = mdf_inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size);
556    See = ADD32(See, SHR32(EXTEND32(10000),6));
557    Syy = mdf_inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
558    
559    /* Convert error to frequency domain */
560    spx_fft(st->fft_table, st->e, st->E);
561    for (i=0;i<st->frame_size;i++)
562       st->y[i] = 0;
563    spx_fft(st->fft_table, st->y, st->Y);
564
565    /* Compute power spectrum of echo (X), error (E) and filter response (Y) */
566    power_spectrum(st->E, st->Rf, N);
567    power_spectrum(st->Y, st->Yf, N);
568    power_spectrum(&st->X[(M-1)*N], st->Xf, N);
569    
570    /* Smooth echo energy estimate over time */
571    for (j=0;j<=st->frame_size;j++)
572       st->power[j] = MULT16_32_Q15(ss_1,st->power[j]) + 1 + MULT16_32_Q15(ss,st->Xf[j]);
573    
574    /* Enable this to compute the power based only on the tail (would need to compute more 
575       efficiently to make this really useful */
576    if (0)
577    {
578       float scale2 = .5f/M;
579       for (j=0;j<=st->frame_size;j++)
580          st->power[j] = 0;
581       for (i=0;i<M;i++)
582       {
583          power_spectrum(&st->X[i*N], st->Xf, N);
584          for (j=0;j<=st->frame_size;j++)
585             st->power[j] += scale2*st->Xf[j];
586       }
587    }
588
589    /* Compute filtered spectra and (cross-)correlations */
590    for (j=st->frame_size;j>=0;j--)
591    {
592       spx_float_t Eh, Yh;
593       Eh = PSEUDOFLOAT(st->Rf[j] - st->Eh[j]);
594       Yh = PSEUDOFLOAT(st->Yf[j] - st->Yh[j]);
595       Pey = FLOAT_ADD(Pey,FLOAT_MULT(Eh,Yh));
596       Pyy = FLOAT_ADD(Pyy,FLOAT_MULT(Yh,Yh));
597 #ifdef FIXED_POINT
598       st->Eh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Eh[j]), st->spec_average, st->Rf[j]);
599       st->Yh[j] = MAC16_32_Q15(MULT16_32_Q15(SUB16(32767,st->spec_average),st->Yh[j]), st->spec_average, st->Yf[j]);
600 #else
601       st->Eh[j] = (1-st->spec_average)*st->Eh[j] + st->spec_average*st->Rf[j];
602       st->Yh[j] = (1-st->spec_average)*st->Yh[j] + st->spec_average*st->Yf[j];
603 #endif
604    }
605    
606    /* Compute correlation updatete rate */
607    tmp32 = MULT16_32_Q15(st->beta0,Syy);
608    if (tmp32 > MULT16_32_Q15(st->beta_max,See))
609       tmp32 = MULT16_32_Q15(st->beta_max,See);
610    alpha = FLOAT_DIV32(tmp32, See);
611    alpha_1 = FLOAT_SUB(FLOAT_ONE, alpha);
612    /* Update correlations (recursive average) */
613    st->Pey = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pey) , FLOAT_MULT(alpha,Pey));
614    st->Pyy = FLOAT_ADD(FLOAT_MULT(alpha_1,st->Pyy) , FLOAT_MULT(alpha,Pyy));
615    if (FLOAT_LT(st->Pyy, FLOAT_ONE))
616       st->Pyy = FLOAT_ONE;
617    /* We don't really hope to get better than 33 dB (MIN_LEAK-3dB) attenuation anyway */
618    if (FLOAT_LT(st->Pey, FLOAT_MULT(MIN_LEAK,st->Pyy)))
619       st->Pey = FLOAT_MULT(MIN_LEAK,st->Pyy);
620    if (FLOAT_GT(st->Pey, st->Pyy))
621       st->Pey = st->Pyy;
622    /* leak_estimate is the linear regression result */
623    leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14));
624    /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */
625    if (leak_estimate > 16383)
626       leak_estimate = 32767;
627    else
628       leak_estimate = SHL16(leak_estimate,1);
629    /*printf ("%f\n", leak_estimate);*/
630    
631    /* Compute Residual to Error Ratio */
632 #ifdef FIXED_POINT
633    tmp32 = MULT16_32_Q15(leak_estimate,Syy);
634    tmp32 = ADD32(tmp32, SHL32(tmp32,1));
635    if (tmp32 > SHR32(See,1))
636       tmp32 = SHR32(See,1);
637    RER = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(tmp32,See),15));
638 #else
639    RER = 3.*MULT16_32_Q15(leak_estimate,Syy) / See;
640    if (RER > .5)
641       RER = .5;
642 #endif
643
644    /* We consider that the filter has had minimal adaptation if the following is true*/
645    if (!st->adapted && st->sum_adapt > QCONST32(1,15))
646    {
647       st->adapted = 1;
648    }
649
650    if (st->adapted)
651    {
652       for (i=0;i<=st->frame_size;i++)
653       {
654          spx_word32_t r, e;
655          /* Compute frequency-domain adaptation mask */
656          r = MULT16_32_Q15(leak_estimate,SHL32(st->Yf[i],3));
657          e = SHL32(st->Rf[i],3)+1;
658 #ifdef FIXED_POINT
659          if (r>SHR32(e,1))
660             r = SHR32(e,1);
661 #else
662          if (r>.5*e)
663             r = .5*e;
664 #endif
665          r = MULT16_32_Q15(QCONST16(.8,15),r) + MULT16_32_Q15(QCONST16(.2,15),(spx_word32_t)(MULT16_32_Q15(RER,e)));
666          /*st->power_1[i] = adapt_rate*r/(e*(1+st->power[i]));*/
667          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);
668       }
669    } else {
670       spx_word32_t Sxx;
671       spx_word16_t adapt_rate=0;
672
673       Sxx = mdf_inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);
674       /* Temporary adaption rate if filter is not adapted correctly */
675
676       tmp32 = MULT16_32_Q15(QCONST16(.15f, 15), Sxx);
677 #ifdef FIXED_POINT
678       if (Sxx > SHR32(See,2))
679          Sxx = SHR32(See,2);
680 #else
681       if (Sxx > .25*See)
682          Sxx = .25*See;      
683 #endif
684       adapt_rate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIV32(MULT16_32_Q15(M_1,Sxx), See),15));
685       
686       for (i=0;i<=st->frame_size;i++)
687          st->power_1[i] = FLOAT_SHL(FLOAT_DIV32(EXTEND32(adapt_rate),ADD32(st->power[i],10)),WEIGHT_SHIFT+1);
688
689
690       /* How much have we adapted so far? */
691       st->sum_adapt = ADD32(st->sum_adapt,adapt_rate);
692    }
693    /* Compute weight gradient */
694    for (j=0;j<M;j++)
695    {
696       weighted_spectral_mul_conj(st->power_1, &st->X[j*N], st->E, st->PHI+N*j, N);
697    }
698
699    if (!saturated)
700    {
701       /* Gradient descent */
702       for (i=0;i<M*N;i++)
703       {
704          st->W[i] += st->PHI[i];
705          /* Old value of W in PHI */
706          st->PHI[i] = st->W[i] - st->PHI[i];
707       }
708    }
709    
710    /* Update weight to prevent circular convolution (MDF / AUMDF) */
711    for (j=0;j<M;j++)
712    {
713       /* This is a variant of the Alternatively Updated MDF (AUMDF) */
714       /* Remove the "if" to make this an MDF filter */
715       if (j==M-1 || st->cancel_count%(M-1) == j)
716       {
717 #ifdef FIXED_POINT
718          for (i=0;i<N;i++)
719             st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16));
720          spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
721          for (i=0;i<st->frame_size;i++)
722          {
723             st->wtmp[i]=0;
724          }
725          for (i=st->frame_size;i<N;i++)
726          {
727             st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
728          }
729          spx_fft(st->fft_table, st->wtmp, st->wtmp2);
730          /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
731          for (i=0;i<N;i++)
732             st->W[j*N+i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
733 #else
734          spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
735          for (i=st->frame_size;i<N;i++)
736          {
737             st->wtmp[i]=0;
738          }
739          spx_fft(st->fft_table, st->wtmp, &st->W[j*N]);
740 #endif
741       }
742    }
743
744    /* Compute spectrum of estimated echo for use in an echo post-filter (if necessary)*/
745    if (Yout)
746    {
747       spx_word16_t leak2;
748       if (st->adapted)
749       {
750          /* If the filter is adapted, take the filtered echo */
751          for (i=0;i<st->frame_size;i++)
752             st->last_y[i] = st->last_y[st->frame_size+i];
753          for (i=0;i<st->frame_size;i++)
754             st->last_y[st->frame_size+i] = ref[i]-out[i];
755       } else {
756          /* If filter isn't adapted yet, all we can do is take the echo signal directly */
757          for (i=0;i<N;i++)
758             st->last_y[i] = st->x[i];
759       }
760       
761       /* Apply hanning window (should pre-compute it)*/
762       for (i=0;i<N;i++)
763          st->y[i] = MULT16_16_Q15(st->window[i],st->last_y[i]);
764       
765       /* Compute power spectrum of the echo */
766       spx_fft(st->fft_table, st->y, st->Y);
767       power_spectrum(st->Y, st->Yps, N);
768       
769 #ifdef FIXED_POINT
770       if (leak_estimate > 16383)
771          leak2 = 32767;
772       else
773          leak2 = SHL16(leak_estimate, 1);
774 #else
775       if (leak_estimate>.5)
776          leak2 = 1;
777       else
778          leak2 = 2*leak_estimate;
779 #endif
780       /* Estimate residual echo */
781       for (i=0;i<=st->frame_size;i++)
782          Yout[i] = MULT16_32_Q15(leak2,st->Yps[i]);
783    }
784 }
785
786
787 int speex_echo_ctl(SpeexEchoState *st, int request, void *ptr)
788 {
789    switch(request)
790    {
791       
792       case SPEEX_ECHO_GET_FRAME_SIZE:
793          (*(int*)ptr) = st->frame_size;
794          break;
795       case SPEEX_ECHO_SET_SAMPLING_RATE:
796          st->sampling_rate = (*(int*)ptr);
797          st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
798 #ifdef FIXED_POINT
799          st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
800          st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
801 #else
802          st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
803          st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
804 #endif
805          if (st->sampling_rate<12000)
806             st->notch_radius = QCONST16(.9, 15);
807          else if (st->sampling_rate<24000)
808             st->notch_radius = QCONST16(.982, 15);
809          else
810             st->notch_radius = QCONST16(.992, 15);
811          break;
812       case SPEEX_ECHO_GET_SAMPLING_RATE:
813          (*(int*)ptr) = st->sampling_rate;
814          break;
815       default:
816          speex_warning_int("Unknown speex_echo_ctl request: ", request);
817          return -1;
818    }
819    return 0;
820 }