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