Converted output gain and Zframe. Also disabled AGC for fixed-point until
[speexdsp.git] / libspeex / preprocess.c
1 /* Copyright (C) 2003 Epic Games (written by Jean-Marc Valin)
2    Copyright (C) 2004-2006 Epic Games 
3    
4    File: preprocess.c
5    Preprocessor with denoising based on the algorithm by Ephraim and Malah
6
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions are
9    met:
10
11    1. Redistributions of source code must retain the above copyright notice,
12    this list of conditions and the following disclaimer.
13
14    2. Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17
18    3. The name of the author may not be used to endorse or promote products
19    derived from this software without specific prior written permission.
20
21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31    POSSIBILITY OF SUCH DAMAGE.
32 */
33
34
35 /*
36    Recommended papers:
37    
38    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
39    short-time spectral amplitude estimator". IEEE Transactions on Acoustics, 
40    Speech and Signal Processing, vol. ASSP-32, no. 6, pp. 1109-1121, 1984.
41    
42    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
43    log-spectral amplitude estimator". IEEE Transactions on Acoustics, Speech and 
44    Signal Processing, vol. ASSP-33, no. 2, pp. 443-445, 1985.
45    
46    I. Cohen and B. Berdugo, "Speech enhancement for non-stationary noise environments".
47    Signal Processing, vol. 81, no. 2, pp. 2403-2418, 2001.
48
49    Stefan Gustafsson, Rainer Martin, Peter Jax, and Peter Vary. "A psychoacoustic 
50    approach to combined acoustic echo cancellation and noise reduction". IEEE 
51    Transactions on Speech and Audio Processing, 2002.
52    
53    J.-M. Valin, J. Rouat, and F. Michaud, "Microphone array post-filter for separation
54    of simultaneous non-stationary sources". In Proceedings IEEE International 
55    Conference on Acoustics, Speech, and Signal Processing, 2004.
56 */
57
58 #ifdef HAVE_CONFIG_H
59 #include "config.h"
60 #endif
61
62 #include <math.h>
63 #include "speex/speex_preprocess.h"
64 #include "speex/speex_echo.h"
65 #include "misc.h"
66 #include "fftwrap.h"
67 #include "filterbank.h"
68 #include "math_approx.h"
69
70 #ifndef M_PI
71 #define M_PI 3.14159263
72 #endif
73
74 #define LOUDNESS_EXP 2.5
75
76 #define NB_BANDS 24
77
78 #define SPEEX_PROB_START_DEFAULT       0.35f
79 #define SPEEX_PROB_CONTINUE_DEFAULT    0.20f
80 #define NOISE_SUPPRESS_DEFAULT       -25
81 #define ECHO_SUPPRESS_DEFAULT        -45
82 #define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
83
84 #ifndef NULL
85 #define NULL 0
86 #endif
87
88 #define SQR(x) ((x)*(x))
89 #define SQR16(x) (MULT16_16((x),(x)))
90 #define SQR16_Q15(x) (MULT16_16_Q15((x),(x)))
91
92 #define FLOOR(x) x = floor(x);
93
94 #ifdef FIXED_POINT
95 static inline spx_word16_t DIV32_16_Q8(spx_word32_t a, spx_word32_t b)
96 {
97    if (SHR32(a,7) >= b)
98    {
99       return 32767;
100    } else {
101       if (b>=QCONST32(1,23))
102       {
103          a = SHR32(a,8);
104          b = SHR32(b,8);
105       }
106       if (b>=QCONST32(1,19))
107       {
108          a = SHR32(a,4);
109          b = SHR32(b,4);
110       }
111       if (b>=QCONST32(1,15))
112       {
113          a = SHR32(a,4);
114          b = SHR32(b,4);
115       }
116       a = SHL32(a,8);
117       return PDIV32_16(a,b);
118    }
119    
120 }
121 static inline spx_word16_t DIV32_16_Q15(spx_word32_t a, spx_word32_t b)
122 {
123    if (SHR32(a,15) >= b)
124    {
125       return 32767;
126    } else {
127       if (b>=QCONST32(1,23))
128       {
129          a = SHR32(a,8);
130          b = SHR32(b,8);
131       }
132       if (b>=QCONST32(1,19))
133       {
134          a = SHR32(a,4);
135          b = SHR32(b,4);
136       }
137       if (b>=QCONST32(1,15))
138       {
139          a = SHR32(a,4);
140          b = SHR32(b,4);
141       }
142       a = SHL32(a,15);      
143       return DIV32_16(a,b);
144    }
145 }
146 #define SNR_SCALING 256.f
147 #define SNR_SCALING_1 0.0039062f
148 #define SNR_SHIFT 8
149
150 #define FRAC_SCALING 32767.f
151 #define FRAC_SCALING_1 3.0518e-05
152 #define FRAC_SHIFT 1
153
154 #define EXPIN_SCALING 2048.f
155 #define EXPIN_SCALING_1 0.00048828f
156 #define EXPIN_SHIFT 11
157 #define EXPOUT_SCALING_1 1.5259e-05
158
159 #define NOISE_SHIFT 7
160
161 #else
162
163 #define DIV32_16_Q8(a,b) ((a)/(b))
164 #define DIV32_16_Q15(a,b) ((a)/(b))
165 #define SNR_SCALING 1.f
166 #define SNR_SCALING_1 1.f
167 #define SNR_SHIFT 0
168 #define FRAC_SCALING 1.f
169 #define FRAC_SCALING_1 1.f
170 #define FRAC_SHIFT 0
171 #define NOISE_SHIFT 0
172
173 #define EXPIN_SCALING 1.f
174 #define EXPIN_SCALING_1 1.f
175 #define EXPOUT_SCALING_1 1.f
176
177 #endif
178
179 /** Speex pre-processor state. */
180 struct SpeexPreprocessState_ {
181    /* Basic info */
182    int    frame_size;        /**< Number of samples processed each time */
183    int    ps_size;           /**< Number of points in the power spectrum */
184    int    sampling_rate;     /**< Sampling rate of the input/output */
185    int    nbands;
186    FilterBank *bank;
187    
188    /* Parameters */
189    int    denoise_enabled;
190    int    agc_enabled;
191    float  agc_level;
192    int    vad_enabled;
193    int    dereverb_enabled;
194    float  reverb_decay;
195    float  reverb_level;
196    float  speech_prob_start;
197    float  speech_prob_continue;
198    int    noise_suppress;
199    int    echo_suppress;
200    int    echo_suppress_active;
201    SpeexEchoState *echo_state;
202    
203    /* DSP-related arrays */
204    spx_word16_t *frame;      /**< Processing frame (2*ps_size) */
205    spx_word16_t *ft;         /**< Processing frame in freq domain (2*ps_size) */
206    spx_word32_t *ps;         /**< Current power spectrum */
207    spx_word16_t *gain2;      /**< Adjusted gains */
208    spx_word16_t *gain_floor; /**< Minimum gain allowed */
209    spx_word16_t *window;     /**< Analysis/Synthesis window */
210    spx_word32_t *noise;      /**< Noise estimate */
211    spx_word32_t *reverb_estimate; /**< Estimate of reverb energy */
212    spx_word32_t *old_ps;     /**< Power spectrum for last frame */
213    spx_word16_t *gain;       /**< Ephraim Malah gain */
214    spx_word16_t *prior;      /**< A-priori SNR */
215    spx_word16_t *post;       /**< A-posteriori SNR */
216
217    spx_word32_t *S;          /**< Smoothed power spectrum */
218    spx_word32_t *Smin;       /**< See Cohen paper */
219    spx_word32_t *Stmp;       /**< See Cohen paper */
220    int *update_prob;       /**< Propability of speech presence for noise update */
221
222    spx_word16_t *zeta;       /**< Smoothed a priori SNR */
223
224    float *loudness_weight;   /**< Perceptual loudness curve */
225
226    spx_word32_t *echo_noise;
227    spx_word32_t *residual_echo;
228
229    /* Misc */
230    spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
231    spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
232
233    int    was_speech;
234    float  loudness;          /**< loudness estimate */
235    float  loudness2;         /**< loudness estimate */
236    int    nb_adapt;          /**< Number of frames used for adaptation so far */
237    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
238    int    min_count;         /**< Number of frames processed so far */
239    void  *fft_lookup;        /**< Lookup table for the FFT */
240 #ifdef FIXED_POINT
241    int    frame_shift;
242 #endif
243 };
244
245
246 static void conj_window(spx_word16_t *w, int len)
247 {
248    int i;
249    for (i=0;i<len;i++)
250    {
251       float tmp;
252       float x=4*((float)i)/len;
253       int inv=0;
254       if (x<1)
255       {
256       } else if (x<2)
257       {
258          x=2-x;
259          inv=1;
260       } else if (x<3)
261       {
262          x=x-2;
263          inv=1;
264       } else {
265          x=4-x;
266       }
267       x*=1.9979;
268       tmp=(.5-.5*cos(x))*(.5-.5*cos(x));
269       if (inv)
270          tmp=1-tmp;
271       w[i]=Q15_ONE*sqrt(tmp);
272    }
273 }
274
275 #ifdef FIXED_POINT
276 /* This function approximates the gain function 
277    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
278    which multiplied by xi/(1+xi) is the optimal gain
279    in the loudness domain ( sqrt[amplitude] )
280    Input in Q11 format, output in Q15
281 */
282 static inline spx_word32_t hypergeom_gain(spx_word32_t xx)
283 {
284    int ind;
285    spx_word16_t frac;
286    float x;
287    /* Q13 table */
288    static const spx_word16_t table[21] = {
289        6730,  8357,  9868, 11267, 12563, 13770, 14898,
290       15959, 16961, 17911, 18816, 19682, 20512, 21311,
291       22082, 22827, 23549, 24250, 24931, 25594, 26241};
292       x = EXPIN_SCALING_1*xx;
293       ind = SHR32(xx,10);
294       if (ind<0)
295          return FRAC_SCALING;
296       if (ind>19)
297          return FRAC_SCALING*(1+.1296/x);
298       frac = SHL32(xx-SHL32(ind,10),5);
299       return SHL32(DIV32_16(PSHR32(MULT16_16(Q15_ONE-frac,table[ind]) + MULT16_16(frac,table[ind+1]),7),(spx_sqrt(SHL32(xx,15)+6711))),7);
300 }
301
302 static inline spx_word16_t qcurve(spx_word16_t x)
303 {
304    x = MAX16(x, 1);
305    return DIV32_16(SHL32(EXTEND32(32767),9),ADD16(512,MULT16_16_Q15(QCONST16(.60f,15),DIV32_16(32767,x))));
306 }
307 #else
308 /* This function approximates the gain function 
309    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
310    which multiplied by xi/(1+xi) is the optimal gain
311    in the loudness domain ( sqrt[amplitude] )
312 */
313 static inline spx_word32_t hypergeom_gain(spx_word32_t xx)
314 {
315    int ind;
316    float integer, frac;
317    float x;
318    static const float table[21] = {
319       0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
320       1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
321       2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
322       x = EXPIN_SCALING_1*xx;
323       integer = floor(2*x);
324       ind = (int)integer;
325       if (ind<0)
326          return FRAC_SCALING;
327       if (ind>19)
328          return FRAC_SCALING*(1+.1296/x);
329       frac = 2*x-integer;
330       return FRAC_SCALING*((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
331 }
332
333 static inline spx_word16_t qcurve(spx_word16_t x)
334 {
335    return 1.f/(1.f+.15f/(SNR_SCALING_1*x));
336 }
337 #endif
338 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
339 {
340    int i;
341    int N, N3, N4, M;
342
343    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
344    st->frame_size = frame_size;
345
346    /* Round ps_size down to the nearest power of two */
347 #if 0
348    i=1;
349    st->ps_size = st->frame_size;
350    while(1)
351    {
352       if (st->ps_size & ~i)
353       {
354          st->ps_size &= ~i;
355          i<<=1;
356       } else {
357          break;
358       }
359    }
360    
361    
362    if (st->ps_size < 3*st->frame_size/4)
363       st->ps_size = st->ps_size * 3 / 2;
364 #else
365    st->ps_size = st->frame_size;
366 #endif
367
368    N = st->ps_size;
369    N3 = 2*N - st->frame_size;
370    N4 = st->frame_size - N3;
371    
372    st->sampling_rate = sampling_rate;
373    st->denoise_enabled = 1;
374    st->agc_enabled = 0;
375    st->agc_level = 8000;
376    st->vad_enabled = 0;
377    st->dereverb_enabled = 0;
378    st->reverb_decay = .0;
379    st->reverb_level = .0;
380    st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
381    st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
382    st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
383
384    st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
385    st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
386
387    st->echo_state = NULL;
388    
389    st->nbands = NB_BANDS;
390    M = st->nbands;
391    st->bank = filterbank_new(M, sampling_rate, N, 1);
392    
393    st->frame = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
394    st->window = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
395    st->ft = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
396    
397    st->ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
398    st->noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
399    st->echo_noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
400    st->residual_echo = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
401    st->reverb_estimate = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
402    st->old_ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
403    st->prior = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
404    st->post = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
405    st->gain = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
406    st->gain2 = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
407    st->gain_floor = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
408    st->zeta = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
409    
410    st->S = (spx_word32_t*)speex_alloc(N*sizeof(float));
411    st->Smin = (spx_word32_t*)speex_alloc(N*sizeof(float));
412    st->Stmp = (spx_word32_t*)speex_alloc(N*sizeof(float));
413    st->update_prob = (int*)speex_alloc(N*sizeof(float));
414    
415    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
416    st->inbuf = (spx_word16_t*)speex_alloc(N3*sizeof(float));
417    st->outbuf = (spx_word16_t*)speex_alloc(N3*sizeof(float));
418
419    conj_window(st->window, 2*N3);
420    for (i=2*N3;i<2*st->ps_size;i++)
421       st->window[i]=Q15_ONE;
422    
423    if (N4>0)
424    {
425       for (i=N3-1;i>=0;i--)
426       {
427          st->window[i+N3+N4]=st->window[i+N3];
428          st->window[i+N3]=1;
429       }
430    }
431    for (i=0;i<N+M;i++)
432    {
433       st->noise[i]=QCONST32(1.f,NOISE_SHIFT);
434       st->reverb_estimate[i]=0.;
435       st->old_ps[i]=1.;
436       st->gain[i]=Q15_ONE;
437       st->post[i]=Q15_ONE;
438       st->prior[i]=Q15_ONE;
439    }
440
441    for (i=0;i<N;i++)
442       st->update_prob[i] = 1;
443    for (i=0;i<N3;i++)
444    {
445       st->inbuf[i]=0;
446       st->outbuf[i]=0;
447    }
448
449    for (i=0;i<N;i++)
450    {
451       float ff=((float)i)*.5*sampling_rate/((float)N);
452       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
453       if (st->loudness_weight[i]<.01f)
454          st->loudness_weight[i]=.01f;
455       st->loudness_weight[i] *= st->loudness_weight[i];
456    }
457
458    st->was_speech = 0;
459    st->loudness = pow(6000,LOUDNESS_EXP);
460    st->loudness2 = 6000;
461    st->nb_loudness_adapt = 0;
462
463    st->fft_lookup = spx_fft_init(2*N);
464
465    st->nb_adapt=0;
466    st->min_count=0;
467    return st;
468 }
469
470 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
471 {
472    speex_free(st->frame);
473    speex_free(st->ft);
474    speex_free(st->ps);
475    speex_free(st->gain2);
476    speex_free(st->gain_floor);
477    speex_free(st->window);
478    speex_free(st->noise);
479    speex_free(st->reverb_estimate);
480    speex_free(st->old_ps);
481    speex_free(st->gain);
482    speex_free(st->prior);
483    speex_free(st->post);
484    speex_free(st->loudness_weight);
485    speex_free(st->echo_noise);
486    speex_free(st->residual_echo);
487
488    speex_free(st->S);
489    speex_free(st->Smin);
490    speex_free(st->Stmp);
491    speex_free(st->update_prob);
492    speex_free(st->zeta);
493
494    speex_free(st->inbuf);
495    speex_free(st->outbuf);
496
497    spx_fft_destroy(st->fft_lookup);
498    filterbank_destroy(st->bank);
499    speex_free(st);
500 }
501
502 /* FIXME: The AGC doesn't work yet with fixed-point*/
503 #ifndef FIXED_POINT
504 static void speex_compute_agc(SpeexPreprocessState *st)
505 {
506    int i;
507    int N = st->ps_size;
508    float scale=.5f/N;
509    float agc_gain;
510    int freq_start, freq_end;
511    float active_bands = 0;
512
513    freq_start = (int)(300.0f*2*N/st->sampling_rate);
514    freq_end   = (int)(2000.0f*2*N/st->sampling_rate);
515    for (i=freq_start;i<freq_end;i++)
516    {
517       if (st->S[i] > 20.f*st->Smin[i]+1000.f)
518          active_bands+=1;
519    }
520    active_bands /= (freq_end-freq_start+1);
521
522    if (active_bands > .2f)
523    {
524       float loudness=0.f;
525       float rate, rate2=.2f;
526       st->nb_loudness_adapt++;
527       rate=2.0f/(1+st->nb_loudness_adapt);
528       if (rate < .05f)
529          rate = .05f;
530       if (rate < .1f && pow(loudness, LOUDNESS_EXP) > st->loudness)
531          rate = .1f;
532       if (rate < .2f && pow(loudness, LOUDNESS_EXP) > 3.f*st->loudness)
533          rate = .2f;
534       if (rate < .4f && pow(loudness, LOUDNESS_EXP) > 10.f*st->loudness)
535          rate = .4f;
536
537       for (i=2;i<N;i++)
538       {
539          loudness += scale*st->ps[i] * FRAC_SCALING_1*FRAC_SCALING_1*st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
540       }
541       loudness=sqrt(loudness);
542       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
543         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
544       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
545       
546       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
547
548       loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
549
550       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
551    }
552    
553    agc_gain = st->agc_level/st->loudness2;
554    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
555    if (agc_gain>200)
556       agc_gain = 200;
557
558    for (i=0;i<N;i++)
559       st->gain2[i] *= agc_gain;
560    
561 }
562 #endif
563
564 static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
565 {
566    int i;
567    int N = st->ps_size;
568    int N3 = 2*N - st->frame_size;
569    int N4 = st->frame_size - N3;
570    spx_word32_t *ps=st->ps;
571
572    /* 'Build' input frame */
573    for (i=0;i<N3;i++)
574       st->frame[i]=st->inbuf[i];
575    for (i=0;i<st->frame_size;i++)
576       st->frame[N3+i]=x[i];
577    
578    /* Update inbuf */
579    for (i=0;i<N3;i++)
580       st->inbuf[i]=x[N4+i];
581
582    /* Windowing */
583    for (i=0;i<2*N;i++)
584       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
585
586 #ifdef FIXED_POINT
587    {
588       spx_word16_t max_val=0;
589       for (i=0;i<2*N;i++)
590          max_val = MAX16(max_val, ABS16(st->frame[i]));
591       st->frame_shift = 14-spx_ilog2(EXTEND32(max_val));
592       for (i=0;i<2*N;i++)
593          st->frame[i] = SHL16(st->frame[i], st->frame_shift);
594    }
595 #endif
596    
597    /* Perform FFT */
598    spx_fft(st->fft_lookup, st->frame, st->ft);
599          
600    /* Power spectrum */
601    /*FIXME: Set ps[0] properly */
602    ps[0]=1;
603    for (i=1;i<N;i++)
604       ps[i]=1+MULT16_16(st->ft[2*i-1],st->ft[2*i-1]) + MULT16_16(st->ft[2*i],st->ft[2*i]);
605    for (i=0;i<N;i++)
606       st->ps[i] = PSHR32(st->ps[i], 2*st->frame_shift);
607
608    filterbank_compute_bank32(st->bank, ps, ps+N);
609 }
610
611 static void update_noise_prob(SpeexPreprocessState *st)
612 {
613    int i;
614    int min_range;
615    int N = st->ps_size;
616
617    for (i=1;i<N-1;i++)
618       st->S[i] = 100.f+ .8f*st->S[i] + .05f*st->ps[i-1]+.1f*st->ps[i]+.05f*st->ps[i+1];
619    st->S[0] = 100.f+ .8f*st->S[0] + .2*st->ps[0];
620    st->S[N-1] = 100.f+ .8f*st->S[N-1] + .2*st->ps[N-1];
621    
622    if (st->nb_adapt==1)
623    {
624       for (i=0;i<N;i++)
625          st->Smin[i] = st->Stmp[i] = 0.f;
626    }
627
628    if (st->nb_adapt < 100)
629       min_range = 15;
630    else if (st->nb_adapt < 1000)
631       min_range = 50;
632    else if (st->nb_adapt < 10000)
633       min_range = 150;
634    else
635       min_range = 300;
636    if (st->min_count > min_range)
637    {
638       st->min_count = 0;
639       for (i=0;i<N;i++)
640       {
641          st->Smin[i] = MIN32(st->Stmp[i], st->S[i]);
642          st->Stmp[i] = st->S[i];
643       }
644    } else {
645       for (i=0;i<N;i++)
646       {
647          st->Smin[i] = MIN32(st->Smin[i], st->S[i]);
648          st->Stmp[i] = MIN32(st->Stmp[i], st->S[i]);      
649       }
650    }
651    for (i=0;i<N;i++)
652    {
653       if (st->S[i] > 2.5*st->Smin[i])
654          st->update_prob[i] = 1;
655       else
656          st->update_prob[i] = 0;
657       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
658       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
659    }
660
661 }
662
663 #define NOISE_OVERCOMPENS 1.
664
665 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
666
667 int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
668 {
669    return speex_preprocess_run(st, x);
670 }
671
672 int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
673 {
674    int i;
675    int M;
676    int N = st->ps_size;
677    int N3 = 2*N - st->frame_size;
678    int N4 = st->frame_size - N3;
679    spx_word32_t *ps=st->ps;
680    spx_word32_t Zframe;
681    spx_word16_t Pframe;
682    float beta, beta_1;
683    float echo_floor;
684    float noise_floor;
685    
686    st->nb_adapt++;
687    st->min_count++;
688    
689    beta =1.0f/st->nb_adapt;
690    if (beta < .03f)
691       beta=.03f;
692    beta_1 = 1.0f-beta;
693    M = st->nbands;
694    /* Deal with residual echo if provided */
695    if (st->echo_state)
696    {
697       speex_echo_get_residual(st->echo_state, st->residual_echo, N);
698       for (i=0;i<N;i++)
699          st->echo_noise[i] = MAX32(MULT16_32_Q15(QCONST16(.6f,15),st->echo_noise[i]), st->residual_echo[i]);
700       filterbank_compute_bank32(st->bank, st->echo_noise, st->echo_noise+N);
701    } else {
702       for (i=0;i<N+M;i++)
703          st->echo_noise[i] = 0;
704    }
705    preprocess_analysis(st, x);
706
707    update_noise_prob(st);
708
709    /* Noise estimation always updated for the 10 first frames */
710    /*if (st->nb_adapt<10)
711    {
712       for (i=1;i<N-1;i++)
713          st->update_prob[i] = 0;
714    }
715    */
716    
717    /* Update the noise estimate for the frequencies where it can be */
718    for (i=0;i<N;i++)
719    {
720       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i], NOISE_SHIFT))
721          st->noise[i] = beta_1*st->noise[i] + beta*NOISE_OVERCOMPENS*SHL32(st->ps[i],NOISE_SHIFT);
722    }
723    filterbank_compute_bank32(st->bank, st->noise, st->noise+N);
724
725    /* Special case for first frame */
726    if (st->nb_adapt==1)
727       for (i=0;i<N+M;i++)
728          st->old_ps[i] = ps[i];
729
730    /* Compute a posteriori SNR */
731    for (i=0;i<N+M;i++)
732    {
733       spx_word16_t gamma;
734       
735       /* Total noise estimate including residual echo and reverberation */
736       spx_word32_t tot_noise = 1.f+ PSHR32(st->noise[i],NOISE_SHIFT) + st->echo_noise[i] + st->reverb_estimate[i];
737       
738       /* A posteriori SNR = ps/noise - 1*/
739       st->post[i] = SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,SNR_SHIFT));
740       st->post[i]=MIN16(st->post[i], QCONST16(100.f,SNR_SHIFT));
741       
742       /* Computing update gamma = .1 + .9*(old/(old+noise))^2 */
743       gamma = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.89f,15),SQR16_Q15(DIV32_16_Q15(st->old_ps[i],ADD32(st->old_ps[i],tot_noise))));
744       
745       /* A priori SNR update = gamma*max(0,post) + (1-gamma)*old/noise */
746       st->prior[i] = EXTRACT16(PSHR32(ADD32(MULT16_16(gamma,MAX16(0,st->post[i])), MULT16_16(Q15_ONE-gamma,DIV32_16_Q8(st->old_ps[i],tot_noise))), 15));
747       st->prior[i]=MIN16(st->prior[i], QCONST16(100.f,SNR_SHIFT));
748    }
749
750    /*print_vec(st->post, N+M, "");*/
751
752    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
753    st->zeta[0] = .7f*st->zeta[0] + .3f*st->prior[0];
754    for (i=1;i<N-1;i++)
755       st->zeta[i] = .7f*st->zeta[i] + .3f*(.5f*st->prior[i]+.25f*st->prior[i+1]+.25f*st->prior[i-1]);
756    for (i=N-1;i<N+M;i++)
757       st->zeta[i] = .7f*st->zeta[i] + .3f*st->prior[i];
758
759    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
760    Zframe = 0;
761    for (i=N;i<N+M;i++)
762       Zframe = ADD32(Zframe, EXTEND32(st->zeta[i]));
763    Pframe = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.899f,15),qcurve(DIV32_16(Zframe,st->nbands)));
764    
765    noise_floor = exp(.2302585f*st->noise_suppress);
766    echo_floor = exp(.2302585f* (st->echo_suppress*(1-FRAC_SCALING_1*Pframe) + st->echo_suppress_active*FRAC_SCALING_1*Pframe));
767    
768    /* Compute Ephraim & Malah gain speech probability of presence for each critical band (Bark scale) 
769       Technically this is actually wrong because the EM gaim assumes a slightly different probability 
770       distribution */
771    for (i=N;i<N+M;i++)
772    {
773       /* See EM and Cohen papers*/
774       spx_word32_t theta;
775       /* Gain from hypergeometric function */
776       spx_word32_t MM;
777       /* Weiner filter gain */
778       spx_word16_t prior_ratio;
779       /* a priority probability of speech presence based on Bark sub-band alone */
780       spx_word16_t P1;
781       /* Speech absence a priori probability (considering sub-band and frame) */
782       spx_word16_t q;
783 #ifdef FIXED_POINT
784       spx_word16_t tmp;
785 #endif
786       /* Compute the gain floor based on different floors for the background noise and residual echo */
787       st->gain_floor[i] = FRAC_SCALING*sqrt(noise_floor*PSHR32(st->noise[i],NOISE_SHIFT) + echo_floor*st->echo_noise[i])/sqrt(1+PSHR32(st->noise[i],NOISE_SHIFT) + st->echo_noise[i]);
788       prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
789       theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
790
791       MM = hypergeom_gain(theta);
792       /* Gain with bound */
793       st->gain[i] = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
794       /* Save old Bark power spectrum */
795       st->old_ps[i] = MULT16_32_P15(QCONST16(.2f,15),st->old_ps[i]) + MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(st->gain[i])),ps[i]);
796
797       P1 = QCONST16(.199f,15)+MULT16_16_Q15(QCONST16(.8f,15),qcurve (st->zeta[i]));
798       q = Q15_ONE-MULT16_16_Q15(Pframe,P1);
799 #ifdef FIXED_POINT
800       theta = MIN32(theta, EXTEND32(32767));
801 /*Q8*/tmp = MULT16_16_Q15((SHL32(1,SNR_SHIFT)+st->prior[i]),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1))));
802       tmp = MIN16(QCONST16(3.,SNR_SHIFT), tmp); /* Prevent overflows in the next line*/
803 /*Q8*/tmp = PSHR(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8);
804       st->gain2[i]=DIV32_16(SHL(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
805 #else
806       st->gain2[i]=1/(1.f + (q/(1.f-q))*(1+st->prior[i])*exp(-theta));
807 #endif
808    }
809    /* Convert the EM gains and speech prob to linear frequency */
810    filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
811    filterbank_compute_psd16(st->bank,st->gain+N, st->gain);
812    
813    /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
814    if (1)
815    {
816       filterbank_compute_psd16(st->bank,st->gain_floor+N, st->gain_floor);
817    
818       /* Compute gain according to the Ephraim-Malah algorithm -- linear frequency */
819       for (i=0;i<N;i++)
820       {
821          spx_word32_t MM;
822          spx_word32_t theta;
823          spx_word16_t prior_ratio;
824          spx_word16_t tmp;
825          spx_word16_t p;
826          spx_word16_t g;
827          
828          /* Wiener filter gain */
829          prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
830          theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
831
832          /* Optimal estimator for loudness domain */
833          MM = hypergeom_gain(theta);
834          /* EM gain with bound */
835          g = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
836          /* Interpolated speech probability of presence */
837          p = st->gain2[i];
838                   
839          /* Constrain the gain to be close to the Bark scale gain */
840          if (MULT16_16_Q15(QCONST16(.333f,15),g) > st->gain[i])
841             g = MULT16_16(3,st->gain[i]);
842          st->gain[i] = g;
843          
844          /* Save old power spectrum */
845          st->old_ps[i] = MULT16_32_P15(QCONST16(.2f,15),st->old_ps[i]) + MULT16_32_P15(MULT16_16_P15(QCONST16(.8f,15),SQR16_Q15(st->gain[i])),ps[i]);
846          
847          /* Apply gain floor */
848          if (st->gain[i] < st->gain_floor[i])
849             st->gain[i] = st->gain_floor[i];
850
851          /* Exponential decay model for reverberation (unused) */
852          /*st->reverb_estimate[i] = st->reverb_decay*st->reverb_estimate[i] + st->reverb_decay*st->reverb_level*st->gain[i]*st->gain[i]*st->ps[i];*/
853          
854          /* Take into account speech probability of presence (loudness domain MMSE estimator) */
855          /* gain2 = [p*sqrt(gain)+(1-p)*sqrt(gain _floor) ]^2 */
856          tmp = MULT16_16_P15(p,spx_sqrt(SHL32(EXTEND32(st->gain[i]),15))) + MULT16_16_P15(SUB16(Q15_ONE,p),spx_sqrt(SHL32(EXTEND32(st->gain_floor[i]),15)));
857          st->gain2[i]=SQR16_Q15(tmp);
858
859          /* Use this if you want a log-domain MMSE estimator instead */
860          /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
861       }
862    } else {
863       for (i=N;i<N+M;i++)
864       {
865          float p = FRAC_SCALING_1*st->gain2[i];
866          if (st->gain[i] < st->gain_floor[i])
867             st->gain[i] = st->gain_floor[i];
868          st->gain2[i]=FRAC_SCALING*(p*sqrt(FRAC_SCALING_1*st->gain[i])+sqrt(FRAC_SCALING_1*st->gain_floor[i])*(1-p)) * (p*sqrt(FRAC_SCALING_1*st->gain[i])+sqrt(FRAC_SCALING_1*st->gain_floor[i])*(1-p));
869       }
870       filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
871       
872    }
873    
874    if (!st->denoise_enabled)
875    {
876       for (i=0;i<N+M;i++)
877          st->gain2[i]=FRAC_SCALING;
878    }
879    
880    /*FIXME: This *will* not work for fixed-point */
881 #ifndef FIXED_POINT
882    if (st->agc_enabled)
883       speex_compute_agc(st);
884 #endif
885    
886    /* Apply computed gain */
887    for (i=1;i<N;i++)
888    {
889       st->ft[2*i-1] = MULT16_16_P15(st->gain2[i],st->ft[2*i-1]);
890       st->ft[2*i] = MULT16_16_P15(st->gain2[i],st->ft[2*i]);
891    }
892    st->ft[0] = MULT16_16_P15(st->gain2[0],st->ft[0]);
893    st->ft[2*N-1] = MULT16_16_P15(st->gain2[N-1],st->ft[2*N-1]);
894
895    /* Inverse FFT with 1/N scaling */
896    spx_ifft(st->fft_lookup, st->ft, st->frame);
897    for (i=0;i<2*N;i++)
898       st->frame[i] = PSHR16(st->frame[i], st->frame_shift);
899
900    /*FIXME: This *will* not work for fixed-point */
901 #ifndef FIXED_POINT
902    if (st->agc_enabled)
903    {
904       float max_sample=0;
905       for (i=0;i<2*N;i++)
906          if (fabs(st->frame[i])>max_sample)
907             max_sample = fabs(st->frame[i]);
908       if (max_sample>28000.f)
909       {
910          float damp = 28000.f/max_sample;
911          for (i=0;i<2*N;i++)
912             st->frame[i] *= damp;
913       }
914    }
915 #endif
916    
917    for (i=0;i<2*N;i++)
918       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
919
920    /* Perform overlap and add */
921    for (i=0;i<N3;i++)
922       x[i] = st->outbuf[i] + st->frame[i];
923    for (i=0;i<N4;i++)
924       x[N3+i] = st->frame[N3+i];
925    
926    /* Update outbuf */
927    for (i=0;i<N3;i++)
928       st->outbuf[i] = st->frame[st->frame_size+i];
929
930    if (st->vad_enabled)
931    {
932       if (FRAC_SCALING_1*Pframe > st->speech_prob_start || (st->was_speech && FRAC_SCALING_1*Pframe > st->speech_prob_continue))
933       {
934          st->was_speech=1;
935          return 1;
936       } else
937       {
938          st->was_speech=0;
939          return 0;
940       }
941    } else {
942       return 1;
943    }
944 }
945
946 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
947 {
948    int i;
949    int N = st->ps_size;
950    int N3 = 2*N - st->frame_size;
951    int M;
952    spx_word32_t *ps=st->ps;
953
954    M = st->nbands;
955    st->min_count++;
956    
957    preprocess_analysis(st, x);
958
959    update_noise_prob(st);
960    
961    for (i=1;i<N-1;i++)
962    {
963       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i],NOISE_SHIFT))
964       {
965          st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
966       }
967    }
968
969    for (i=0;i<N3;i++)
970       st->outbuf[i] = MULT16_16_Q15(x[st->frame_size-N3+i],st->window[st->frame_size+i]);
971
972    /* Save old power spectrum */
973    for (i=0;i<N+M;i++)
974       st->old_ps[i] = ps[i];
975
976    for (i=1;i<N;i++)
977       st->reverb_estimate[i] *= st->reverb_decay;
978 }
979
980
981 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
982 {
983    int i;
984    SpeexPreprocessState *st;
985    st=(SpeexPreprocessState*)state;
986    switch(request)
987    {
988    case SPEEX_PREPROCESS_SET_DENOISE:
989       st->denoise_enabled = (*(int*)ptr);
990       break;
991    case SPEEX_PREPROCESS_GET_DENOISE:
992       (*(int*)ptr) = st->denoise_enabled;
993       break;
994
995    case SPEEX_PREPROCESS_SET_AGC:
996       st->agc_enabled = (*(int*)ptr);
997       break;
998    case SPEEX_PREPROCESS_GET_AGC:
999       (*(int*)ptr) = st->agc_enabled;
1000       break;
1001
1002    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
1003       st->agc_level = (*(float*)ptr);
1004       if (st->agc_level<1)
1005          st->agc_level=1;
1006       if (st->agc_level>32768)
1007          st->agc_level=32768;
1008       break;
1009    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
1010       (*(float*)ptr) = st->agc_level;
1011       break;
1012
1013    case SPEEX_PREPROCESS_SET_VAD:
1014       speex_warning("The VAD has been replaced by a hack pending a complete rewrite");
1015       st->vad_enabled = (*(spx_int32_t*)ptr);
1016       break;
1017    case SPEEX_PREPROCESS_GET_VAD:
1018       (*(spx_int32_t*)ptr) = st->vad_enabled;
1019       break;
1020    
1021    case SPEEX_PREPROCESS_SET_DEREVERB:
1022       st->dereverb_enabled = (*(int*)ptr);
1023       for (i=0;i<st->ps_size;i++)
1024          st->reverb_estimate[i]=0;
1025       break;
1026    case SPEEX_PREPROCESS_GET_DEREVERB:
1027       (*(int*)ptr) = st->dereverb_enabled;
1028       break;
1029
1030    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
1031       st->reverb_level = (*(float*)ptr);
1032       break;
1033    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
1034       (*(float*)ptr) = st->reverb_level;
1035       break;
1036    
1037    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
1038       st->reverb_decay = (*(float*)ptr);
1039       break;
1040    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
1041       (*(float*)ptr) = st->reverb_decay;
1042       break;
1043
1044    case SPEEX_PREPROCESS_SET_PROB_START:
1045       st->speech_prob_start = (*(int*)ptr) / 100.0;
1046       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
1047          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
1048       break;
1049    case SPEEX_PREPROCESS_GET_PROB_START:
1050       (*(int*)ptr) = st->speech_prob_start * 100;
1051       break;
1052
1053    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
1054       st->speech_prob_continue = (*(int*)ptr) / 100.0;
1055       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
1056          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
1057       break;
1058    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
1059       (*(int*)ptr) = st->speech_prob_continue * 100;
1060       break;
1061
1062    case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
1063       st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
1064       break;
1065    case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
1066       (*(spx_int32_t*)ptr) = st->noise_suppress;
1067       break;
1068    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
1069       st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
1070       break;
1071    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
1072       (*(spx_int32_t*)ptr) = st->echo_suppress;
1073       break;
1074    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
1075       st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
1076       break;
1077    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
1078       (*(spx_int32_t*)ptr) = st->echo_suppress_active;
1079       break;
1080    case SPEEX_PREPROCESS_SET_ECHO_STATE:
1081       st->echo_state = (SpeexEchoState*)ptr;
1082       break;
1083    case SPEEX_PREPROCESS_GET_ECHO_STATE:
1084       ptr = (void*)st->echo_state;
1085       break;
1086
1087    default:
1088       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
1089       return -1;
1090    }
1091    return 0;
1092 }