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 5.f
  75 #define AMP_SCALE .001f
  76 #define AMP_SCALE_1 1000.f
  77
  78 #define NB_BANDS 24
  79
  80 #define SPEECH_PROB_START_DEFAULT       QCONST16(0.35f,15)
  81 #define SPEECH_PROB_CONTINUE_DEFAULT    QCONST16(0.20f,15)
  82 #define NOISE_SUPPRESS_DEFAULT       -15
  83 #define ECHO_SUPPRESS_DEFAULT        -40
  84 #define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
  85
  86 #ifndef NULL
  87 #define NULL 0
  88 #endif
  89
  90 #define SQR(x) ((x)*(x))
  91 #define SQR16(x) (MULT16_16((x),(x)))
  92 #define SQR16_Q15(x) (MULT16_16_Q15((x),(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)-a;
 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    vad_enabled;
 191    int    dereverb_enabled;
 192    spx_word16_t  reverb_decay;
 193    spx_word16_t  reverb_level;
 194    spx_word16_t speech_prob_start;
 195    spx_word16_t speech_prob_continue;
 196    int    noise_suppress;
 197    int    echo_suppress;
 198    int    echo_suppress_active;
 199    SpeexEchoState *echo_state;
 200
 201    /* DSP-related arrays */
 202    spx_word16_t *frame;      /**< Processing frame (2*ps_size) */
 203    spx_word16_t *ft;         /**< Processing frame in freq domain (2*ps_size) */
 204    spx_word32_t *ps;         /**< Current power spectrum */
 205    spx_word16_t *gain2;      /**< Adjusted gains */
 206    spx_word16_t *gain_floor; /**< Minimum gain allowed */
 207    spx_word16_t *window;     /**< Analysis/Synthesis window */
 208    spx_word32_t *noise;      /**< Noise estimate */
 209    spx_word32_t *reverb_estimate; /**< Estimate of reverb energy */
 210    spx_word32_t *old_ps;     /**< Power spectrum for last frame */
 211    spx_word16_t *gain;       /**< Ephraim Malah gain */
 212    spx_word16_t *prior;      /**< A-priori SNR */
 213    spx_word16_t *post;       /**< A-posteriori SNR */
 214
 215    spx_word32_t *S;          /**< Smoothed power spectrum */
 216    spx_word32_t *Smin;       /**< See Cohen paper */
 217    spx_word32_t *Stmp;       /**< See Cohen paper */
 218    int *update_prob;       /**< Propability of speech presence for noise update */
 219
 220    spx_word16_t *zeta;       /**< Smoothed a priori SNR */
 221    spx_word32_t *echo_noise;
 222    spx_word32_t *residual_echo;
 223
 224    /* Misc */
 225    spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
 226    spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
 227
 228    /* AGC stuff, only for floating point for now */
 229 #ifndef FIXED_POINT
 230    int    agc_enabled;
 231    float  agc_level;
 232    float  loudness_accum;
 233    float *loudness_weight;   /**< Perceptual loudness curve */
 234    float  loudness;          /**< Loudness estimate */
 235    float  agc_gain;          /**< Current AGC gain */
 236    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
 237    float  max_gain;          /**< Maximum gain allowed */
 238    float  max_increase_step; /**< Maximum increase in gain from one frame to another */
 239    float  max_decrease_step; /**< Maximum decrease in gain from one frame to another */
 240    float  prev_loudness;     /**< Loudness of previous frame */
 241    float  init_max;          /**< Current gain limit during initialisation */
 242 #endif
 243    int    nb_adapt;          /**< Number of frames used for adaptation so far */
 244    int    was_speech;
 245    int    min_count;         /**< Number of frames processed so far */
 246    void  *fft_lookup;        /**< Lookup table for the FFT */
 247 #ifdef FIXED_POINT
 248    int    frame_shift;
 249 #endif
 250 };
 251
 252
 253 static void conj_window(spx_word16_t *w, int len)
 254 {
 255    int i;
 256    for (i=0;i<len;i++)
 257    {
 258       spx_word16_t tmp;
 259 #ifdef FIXED_POINT
 260       spx_word16_t x = DIV32_16(MULT16_16(32767,i),len);
 261 #else
 262       spx_word16_t x = DIV32_16(MULT16_16(QCONST16(4.f,13),i),len);
 263 #endif
 264       int inv=0;
 265       if (x<QCONST16(1.f,13))
 266       {
 267       } else if (x<QCONST16(2.f,13))
 268       {
 269          x=QCONST16(2.f,13)-x;
 270          inv=1;
 271       } else if (x<QCONST16(3.f,13))
 272       {
 273          x=x-QCONST16(2.f,13);
 274          inv=1;
 275       } else {
 276          x=QCONST16(2.f,13)-x+QCONST16(2.f,13); /* 4 - x */
 277       }
 278       x = MULT16_16_Q14(QCONST16(1.271903f,14), x);
 279       tmp = SQR16_Q15(QCONST16(.5f,15)-MULT16_16_P15(QCONST16(.5f,15),spx_cos_norm(QCONST32(x,2))));
 280       if (inv)
 281          tmp=SUB16(Q15_ONE,tmp);
 282       w[i]=spx_sqrt(SHL32(EXTEND32(tmp),15));
 283    }
 284 }
 285
 286
 287 #ifdef FIXED_POINT
 288 /* This function approximates the gain function
 289    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)
 290    which multiplied by xi/(1+xi) is the optimal gain
 291    in the loudness domain ( sqrt[amplitude] )
 292    Input in Q11 format, output in Q15
 293 */
 294 static inline spx_word32_t hypergeom_gain(spx_word32_t xx)
 295 {
 296    int ind;
 297    spx_word16_t frac;
 298    /* Q13 table */
 299    static const spx_word16_t table[21] = {
 300        6730,  8357,  9868, 11267, 12563, 13770, 14898,
 301       15959, 16961, 17911, 18816, 19682, 20512, 21311,
 302       22082, 22827, 23549, 24250, 24931, 25594, 26241};
 303       ind = SHR32(xx,10);
 304       if (ind<0)
 305          return Q15_ONE;
 306       if (ind>19)
 307          return ADD32(EXTEND32(Q15_ONE),EXTEND32(DIV32_16(QCONST32(.1296,23), SHR32(xx,EXPIN_SHIFT-SNR_SHIFT))));
 308       frac = SHL32(xx-SHL32(ind,10),5);
 309       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);
 310 }
 311
 312 static inline spx_word16_t qcurve(spx_word16_t x)
 313 {
 314    x = MAX16(x, 1);
 315    return DIV32_16(SHL32(EXTEND32(32767),9),ADD16(512,MULT16_16_Q15(QCONST16(.60f,15),DIV32_16(32767,x))));
 316 }
 317
 318 /* Compute the gain floor based on different floors for the background noise and residual echo */
 319 static void compute_gain_floor(int noise_suppress, int effective_echo_suppress, spx_word32_t *noise, spx_word32_t *echo, spx_word16_t *gain_floor, int len)
 320 {
 321    int i;
 322
 323    if (noise_suppress > effective_echo_suppress)
 324    {
 325       spx_word16_t noise_gain, gain_ratio;
 326       noise_gain = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(0.11513,11),noise_suppress)),1)));
 327       gain_ratio = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(.2302585f,11),effective_echo_suppress-noise_suppress)),1)));
 328
 329       /* gain_floor = sqrt [ (noise*noise_floor + echo*echo_floor) / (noise+echo) ] */
 330       for (i=0;i<len;i++)
 331          gain_floor[i] = MULT16_16_Q15(noise_gain,
 332                                        spx_sqrt(SHL32(EXTEND32(DIV32_16_Q15(PSHR32(noise[i],NOISE_SHIFT) + MULT16_32_Q15(gain_ratio,echo[i]),
 333                                              (1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]) )),15)));
 334    } else {
 335       spx_word16_t echo_gain, gain_ratio;
 336       echo_gain = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(0.11513,11),effective_echo_suppress)),1)));
 337       gain_ratio = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(.2302585f,11),noise_suppress-effective_echo_suppress)),1)));
 338
 339       /* gain_floor = sqrt [ (noise*noise_floor + echo*echo_floor) / (noise+echo) ] */
 340       for (i=0;i<len;i++)
 341          gain_floor[i] = MULT16_16_Q15(echo_gain,
 342                                        spx_sqrt(SHL32(EXTEND32(DIV32_16_Q15(MULT16_32_Q15(gain_ratio,PSHR32(noise[i],NOISE_SHIFT)) + echo[i],
 343                                              (1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]) )),15)));
 344    }
 345 }
 346
 347 #else
 348 /* This function approximates the gain function
 349    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)
 350    which multiplied by xi/(1+xi) is the optimal gain
 351    in the loudness domain ( sqrt[amplitude] )
 352 */
 353 static inline spx_word32_t hypergeom_gain(spx_word32_t xx)
 354 {
 355    int ind;
 356    float integer, frac;
 357    float x;
 358    static const float table[21] = {
 359       0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
 360       1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
 361       2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
 362       x = EXPIN_SCALING_1*xx;
 363       integer = floor(2*x);
 364       ind = (int)integer;
 365       if (ind<0)
 366          return FRAC_SCALING;
 367       if (ind>19)
 368          return FRAC_SCALING*(1+.1296/x);
 369       frac = 2*x-integer;
 370       return FRAC_SCALING*((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
 371 }
 372
 373 static inline spx_word16_t qcurve(spx_word16_t x)
 374 {
 375    return 1.f/(1.f+.15f/(SNR_SCALING_1*x));
 376 }
 377
 378 static void compute_gain_floor(int noise_suppress, int effective_echo_suppress, spx_word32_t *noise, spx_word32_t *echo, spx_word16_t *gain_floor, int len)
 379 {
 380    int i;
 381    float echo_floor;
 382    float noise_floor;
 383
 384    noise_floor = exp(.2302585f*noise_suppress);
 385    echo_floor = exp(.2302585f*effective_echo_suppress);
 386
 387    /* Compute the gain floor based on different floors for the background noise and residual echo */
 388    for (i=0;i<len;i++)
 389       gain_floor[i] = FRAC_SCALING*sqrt(noise_floor*PSHR32(noise[i],NOISE_SHIFT) + echo_floor*echo[i])/sqrt(1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]);
 390 }
 391
 392 #endif
 393 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
 394 {
 395    int i;
 396    int N, N3, N4, M;
 397
 398    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
 399    st->frame_size = frame_size;
 400
 401    /* Round ps_size down to the nearest power of two */
 402 #if 0
 403    i=1;
 404    st->ps_size = st->frame_size;
 405    while(1)
 406    {
 407       if (st->ps_size & ~i)
 408       {
 409          st->ps_size &= ~i;
 410          i<<=1;
 411       } else {
 412          break;
 413       }
 414    }
 415
 416
 417    if (st->ps_size < 3*st->frame_size/4)
 418       st->ps_size = st->ps_size * 3 / 2;
 419 #else
 420    st->ps_size = st->frame_size;
 421 #endif
 422
 423    N = st->ps_size;
 424    N3 = 2*N - st->frame_size;
 425    N4 = st->frame_size - N3;
 426
 427    st->sampling_rate = sampling_rate;
 428    st->denoise_enabled = 1;
 429    st->vad_enabled = 0;
 430    st->dereverb_enabled = 0;
 431    st->reverb_decay = 0;
 432    st->reverb_level = 0;
 433    st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
 434    st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
 435    st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
 436
 437    st->speech_prob_start = SPEECH_PROB_START_DEFAULT;
 438    st->speech_prob_continue = SPEECH_PROB_CONTINUE_DEFAULT;
 439
 440    st->echo_state = NULL;
 441
 442    st->nbands = NB_BANDS;
 443    M = st->nbands;
 444    st->bank = filterbank_new(M, sampling_rate, N, 1);
 445
 446    st->frame = (spx_word16_t*)speex_alloc(2*N*sizeof(spx_word16_t));
 447    st->window = (spx_word16_t*)speex_alloc(2*N*sizeof(spx_word16_t));
 448    st->ft = (spx_word16_t*)speex_alloc(2*N*sizeof(spx_word16_t));
 449
 450    st->ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
 451    st->noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
 452    st->echo_noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
 453    st->residual_echo = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
 454    st->reverb_estimate = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
 455    st->old_ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(spx_word32_t));
 456    st->prior = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
 457    st->post = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
 458    st->gain = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
 459    st->gain2 = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
 460    st->gain_floor = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
 461    st->zeta = (spx_word16_t*)speex_alloc((N+M)*sizeof(spx_word16_t));
 462
 463    st->S = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
 464    st->Smin = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
 465    st->Stmp = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
 466    st->update_prob = (int*)speex_alloc(N*sizeof(int));
 467
 468    st->inbuf = (spx_word16_t*)speex_alloc(N3*sizeof(spx_word16_t));
 469    st->outbuf = (spx_word16_t*)speex_alloc(N3*sizeof(spx_word16_t));
 470
 471    conj_window(st->window, 2*N3);
 472    for (i=2*N3;i<2*st->ps_size;i++)
 473       st->window[i]=Q15_ONE;
 474
 475    if (N4>0)
 476    {
 477       for (i=N3-1;i>=0;i--)
 478       {
 479          st->window[i+N3+N4]=st->window[i+N3];
 480          st->window[i+N3]=1;
 481       }
 482    }
 483    for (i=0;i<N+M;i++)
 484    {
 485       st->noise[i]=QCONST32(1.f,NOISE_SHIFT);
 486       st->reverb_estimate[i]=0;
 487       st->old_ps[i]=1;
 488       st->gain[i]=Q15_ONE;
 489       st->post[i]=SHL16(1, SNR_SHIFT);
 490       st->prior[i]=SHL16(1, SNR_SHIFT);
 491    }
 492
 493    for (i=0;i<N;i++)
 494       st->update_prob[i] = 1;
 495    for (i=0;i<N3;i++)
 496    {
 497       st->inbuf[i]=0;
 498       st->outbuf[i]=0;
 499    }
 500 #ifndef FIXED_POINT
 501    st->agc_enabled = 0;
 502    st->agc_level = 8000;
 503    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
 504    for (i=0;i<N;i++)
 505    {
 506       float ff=((float)i)*.5*sampling_rate/((float)N);
 507       /*st->loudness_weight[i] = .5f*(1.f/(1.f+ff/8000.f))+1.f*exp(-.5f*(ff-3800.f)*(ff-3800.f)/9e5f);*/
 508       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
 509       if (st->loudness_weight[i]<.01f)
 510          st->loudness_weight[i]=.01f;
 511       st->loudness_weight[i] *= st->loudness_weight[i];
 512    }
 513    /*st->loudness = pow(AMP_SCALE*st->agc_level,LOUDNESS_EXP);*/
 514    st->loudness = 1e-15;
 515    st->agc_gain = 1;
 516    st->nb_loudness_adapt = 0;
 517    st->max_gain = 30;
 518    st->max_increase_step = exp(0.11513f * 12.*st->frame_size / st->sampling_rate);
 519    st->max_decrease_step = exp(-0.11513f * 40.*st->frame_size / st->sampling_rate);
 520    st->prev_loudness = 1;
 521    st->init_max = 1;
 522 #endif
 523    st->was_speech = 0;
 524
 525    st->fft_lookup = spx_fft_init(2*N);
 526
 527    st->nb_adapt=0;
 528    st->min_count=0;
 529    return st;
 530 }
 531
 532 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
 533 {
 534    speex_free(st->frame);
 535    speex_free(st->ft);
 536    speex_free(st->ps);
 537    speex_free(st->gain2);
 538    speex_free(st->gain_floor);
 539    speex_free(st->window);
 540    speex_free(st->noise);
 541    speex_free(st->reverb_estimate);
 542    speex_free(st->old_ps);
 543    speex_free(st->gain);
 544    speex_free(st->prior);
 545    speex_free(st->post);
 546 #ifndef FIXED_POINT
 547    speex_free(st->loudness_weight);
 548 #endif
 549    speex_free(st->echo_noise);
 550    speex_free(st->residual_echo);
 551
 552    speex_free(st->S);
 553    speex_free(st->Smin);
 554    speex_free(st->Stmp);
 555    speex_free(st->update_prob);
 556    speex_free(st->zeta);
 557
 558    speex_free(st->inbuf);
 559    speex_free(st->outbuf);
 560
 561    spx_fft_destroy(st->fft_lookup);
 562    filterbank_destroy(st->bank);
 563    speex_free(st);
 564 }
 565
 566 /* FIXME: The AGC doesn't work yet with fixed-point*/
 567 #ifndef FIXED_POINT
 568 static void speex_compute_agc(SpeexPreprocessState *st, spx_word16_t Pframe, spx_word16_t *ft)
 569 {
 570    int i;
 571    int N = st->ps_size;
 572    float target_gain;
 573    float loudness=1.f;
 574    float rate;
 575
 576    for (i=2;i<N;i++)
 577    {
 578       loudness += 2.f*N*st->ps[i]* st->loudness_weight[i];
 579    }
 580    loudness=sqrt(loudness);
 581       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
 582    loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
 583    if (Pframe>.3f)
 584    {
 585       st->nb_loudness_adapt++;
 586       /*rate=2.0f*Pframe*Pframe/(1+st->nb_loudness_adapt);*/
 587       rate = .03*Pframe*Pframe;
 588       st->loudness = (1-rate)*st->loudness + (rate)*pow(AMP_SCALE*loudness, LOUDNESS_EXP);
 589       st->loudness_accum = (1-rate)*st->loudness_accum + rate;
 590       if (st->init_max < st->max_gain && st->nb_adapt > 20)
 591          st->init_max *= 1.f + .1f*Pframe*Pframe;
 592    }
 593    /*printf ("%f %f %f %f\n", Pframe, loudness, pow(st->loudness, 1.0f/LOUDNESS_EXP), st->loudness2);*/
 594
 595    target_gain = AMP_SCALE*st->agc_level*pow(st->loudness/(1e-4+st->loudness_accum), -1.0f/LOUDNESS_EXP);
 596
 597    if ((Pframe>.5  && st->nb_adapt > 20) || target_gain < st->agc_gain)
 598    {
 599       if (target_gain > st->max_increase_step*st->agc_gain)
 600          target_gain = st->max_increase_step*st->agc_gain;
 601       if (target_gain < st->max_decrease_step*st->agc_gain && loudness < 10*st->prev_loudness)
 602          target_gain = st->max_decrease_step*st->agc_gain;
 603       if (target_gain > st->max_gain)
 604          target_gain = st->max_gain;
 605       if (target_gain > st->init_max)
 606          target_gain = st->init_max;
 607
 608       st->agc_gain = target_gain;
 609    }
 610    /*fprintf (stderr, "%f %f %f\n", loudness, (float)AMP_SCALE_1*pow(st->loudness, 1.0f/LOUDNESS_EXP), st->agc_gain);*/
 611
 612    for (i=0;i<2*N;i++)
 613       ft[i] *= st->agc_gain;
 614    st->prev_loudness = loudness;
 615 }
 616 #endif
 617
 618 static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
 619 {
 620    int i;
 621    int N = st->ps_size;
 622    int N3 = 2*N - st->frame_size;
 623    int N4 = st->frame_size - N3;
 624    spx_word32_t *ps=st->ps;
 625
 626    /* 'Build' input frame */
 627    for (i=0;i<N3;i++)
 628       st->frame[i]=st->inbuf[i];
 629    for (i=0;i<st->frame_size;i++)
 630       st->frame[N3+i]=x[i];
 631
 632    /* Update inbuf */
 633    for (i=0;i<N3;i++)
 634       st->inbuf[i]=x[N4+i];
 635
 636    /* Windowing */
 637    for (i=0;i<2*N;i++)
 638       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
 639
 640 #ifdef FIXED_POINT
 641    {
 642       spx_word16_t max_val=0;
 643       for (i=0;i<2*N;i++)
 644          max_val = MAX16(max_val, ABS16(st->frame[i]));
 645       st->frame_shift = 14-spx_ilog2(EXTEND32(max_val));
 646       for (i=0;i<2*N;i++)
 647          st->frame[i] = SHL16(st->frame[i], st->frame_shift);
 648    }
 649 #endif
 650
 651    /* Perform FFT */
 652    spx_fft(st->fft_lookup, st->frame, st->ft);
 653
 654    /* Power spectrum */
 655    ps[0]=MULT16_16(st->ft[0],st->ft[0]);
 656    for (i=1;i<N;i++)
 657       ps[i]=MULT16_16(st->ft[2*i-1],st->ft[2*i-1]) + MULT16_16(st->ft[2*i],st->ft[2*i]);
 658    for (i=0;i<N;i++)
 659       st->ps[i] = PSHR32(st->ps[i], 2*st->frame_shift);
 660
 661    filterbank_compute_bank32(st->bank, ps, ps+N);
 662 }
 663
 664 static void update_noise_prob(SpeexPreprocessState *st)
 665 {
 666    int i;
 667    int min_range;
 668    int N = st->ps_size;
 669
 670    for (i=1;i<N-1;i++)
 671       st->S[i] =  MULT16_32_Q15(QCONST16(.8f,15),st->S[i]) + MULT16_32_Q15(QCONST16(.05f,15),st->ps[i-1])
 672                       + MULT16_32_Q15(QCONST16(.1f,15),st->ps[i]) + MULT16_32_Q15(QCONST16(.05f,15),st->ps[i+1]);
 673    st->S[0] =  MULT16_32_Q15(QCONST16(.8f,15),st->S[0]) + MULT16_32_Q15(QCONST16(.2f,15),st->ps[0]);
 674    st->S[N-1] =  MULT16_32_Q15(QCONST16(.8f,15),st->S[N-1]) + MULT16_32_Q15(QCONST16(.2f,15),st->ps[N-1]);
 675
 676    if (st->nb_adapt==1)
 677    {
 678       for (i=0;i<N;i++)
 679          st->Smin[i] = st->Stmp[i] = 0;
 680    }
 681
 682    if (st->nb_adapt < 100)
 683       min_range = 15;
 684    else if (st->nb_adapt < 1000)
 685       min_range = 50;
 686    else if (st->nb_adapt < 10000)
 687       min_range = 150;
 688    else
 689       min_range = 300;
 690    if (st->min_count > min_range)
 691    {
 692       st->min_count = 0;
 693       for (i=0;i<N;i++)
 694       {
 695          st->Smin[i] = MIN32(st->Stmp[i], st->S[i]);
 696          st->Stmp[i] = st->S[i];
 697       }
 698    } else {
 699       for (i=0;i<N;i++)
 700       {
 701          st->Smin[i] = MIN32(st->Smin[i], st->S[i]);
 702          st->Stmp[i] = MIN32(st->Stmp[i], st->S[i]);
 703       }
 704    }
 705    for (i=0;i<N;i++)
 706    {
 707       if (MULT16_32_Q15(QCONST16(.4f,15),st->S[i]) > ADD32(st->Smin[i],EXTEND32(20)))
 708          st->update_prob[i] = 1;
 709       else
 710          st->update_prob[i] = 0;
 711       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
 712       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
 713    }
 714
 715 }
 716
 717 #define NOISE_OVERCOMPENS 1.
 718
 719 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
 720
 721 int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
 722 {
 723    return speex_preprocess_run(st, x);
 724 }
 725
 726 int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
 727 {
 728    int i;
 729    int M;
 730    int N = st->ps_size;
 731    int N3 = 2*N - st->frame_size;
 732    int N4 = st->frame_size - N3;
 733    spx_word32_t *ps=st->ps;
 734    spx_word32_t Zframe;
 735    spx_word16_t Pframe;
 736    spx_word16_t beta, beta_1;
 737    spx_word16_t effective_echo_suppress;
 738
 739    st->nb_adapt++;
 740    st->min_count++;
 741
 742    beta = MAX16(QCONST16(.03,15),DIV32_16(Q15_ONE,st->nb_adapt));
 743    beta_1 = Q15_ONE-beta;
 744    M = st->nbands;
 745    /* Deal with residual echo if provided */
 746    if (st->echo_state)
 747    {
 748       speex_echo_get_residual(st->echo_state, st->residual_echo, N);
 749 #ifndef FIXED_POINT
 750       /* If there are NaNs or ridiculous values, it'll show up in the DC and we just reset everything to zero */
 751       if (!(st->residual_echo[0] >=0 && st->residual_echo[0]<N*1e9f))
 752       {
 753          for (i=0;i<N;i++)
 754             st->residual_echo[i] = 0;
 755       }
 756 #endif
 757       for (i=0;i<N;i++)
 758          st->echo_noise[i] = MAX32(MULT16_32_Q15(QCONST16(.6f,15),st->echo_noise[i]), st->residual_echo[i]);
 759       filterbank_compute_bank32(st->bank, st->echo_noise, st->echo_noise+N);
 760    } else {
 761       for (i=0;i<N+M;i++)
 762          st->echo_noise[i] = 0;
 763    }
 764    preprocess_analysis(st, x);
 765
 766    update_noise_prob(st);
 767
 768    /* Noise estimation always updated for the 10 first frames */
 769    /*if (st->nb_adapt<10)
 770    {
 771       for (i=1;i<N-1;i++)
 772          st->update_prob[i] = 0;
 773    }
 774    */
 775
 776    /* Update the noise estimate for the frequencies where it can be */
 777    for (i=0;i<N;i++)
 778    {
 779       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i], NOISE_SHIFT))
 780          st->noise[i] = MAX32(EXTEND32(0),MULT16_32_Q15(beta_1,st->noise[i]) + MULT16_32_Q15(beta,SHL32(st->ps[i],NOISE_SHIFT)));
 781    }
 782    filterbank_compute_bank32(st->bank, st->noise, st->noise+N);
 783
 784    /* Special case for first frame */
 785    if (st->nb_adapt==1)
 786       for (i=0;i<N+M;i++)
 787          st->old_ps[i] = ps[i];
 788
 789    /* Compute a posteriori SNR */
 790    for (i=0;i<N+M;i++)
 791    {
 792       spx_word16_t gamma;
 793
 794       /* Total noise estimate including residual echo and reverberation */
 795       spx_word32_t tot_noise = ADD32(ADD32(ADD32(EXTEND32(1), PSHR32(st->noise[i],NOISE_SHIFT)) , st->echo_noise[i]) , st->reverb_estimate[i]);
 796
 797       /* A posteriori SNR = ps/noise - 1*/
 798       st->post[i] = SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,SNR_SHIFT));
 799       st->post[i]=MIN16(st->post[i], QCONST16(100.f,SNR_SHIFT));
 800
 801       /* Computing update gamma = .1 + .9*(old/(old+noise))^2 */
 802       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))));
 803
 804       /* A priori SNR update = gamma*max(0,post) + (1-gamma)*old/noise */
 805       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));
 806       st->prior[i]=MIN16(st->prior[i], QCONST16(100.f,SNR_SHIFT));
 807    }
 808
 809    /*print_vec(st->post, N+M, "");*/
 810
 811    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
 812    st->zeta[0] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),st->zeta[0]), MULT16_16(QCONST16(.3f,15),st->prior[0])),15);
 813    for (i=1;i<N-1;i++)
 814       st->zeta[i] = PSHR32(ADD32(ADD32(ADD32(MULT16_16(QCONST16(.7f,15),st->zeta[i]), MULT16_16(QCONST16(.15f,15),st->prior[i])),
 815                            MULT16_16(QCONST16(.075f,15),st->prior[i-1])), MULT16_16(QCONST16(.075f,15),st->prior[i+1])),15);
 816    for (i=N-1;i<N+M;i++)
 817       st->zeta[i] = PSHR32(ADD32(MULT16_16(QCONST16(.7f,15),st->zeta[i]), MULT16_16(QCONST16(.3f,15),st->prior[i])),15);
 818
 819    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
 820    Zframe = 0;
 821    for (i=N;i<N+M;i++)
 822       Zframe = ADD32(Zframe, EXTEND32(st->zeta[i]));
 823    Pframe = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.899f,15),qcurve(DIV32_16(Zframe,st->nbands)));
 824
 825    effective_echo_suppress = EXTRACT16(PSHR32(ADD32(MULT16_16(SUB16(Q15_ONE,Pframe), st->echo_suppress), MULT16_16(Pframe, st->echo_suppress_active)),15));
 826
 827    compute_gain_floor(st->noise_suppress, effective_echo_suppress, st->noise+N, st->echo_noise+N, st->gain_floor+N, M);
 828
 829    /* Compute Ephraim & Malah gain speech probability of presence for each critical band (Bark scale)
 830       Technically this is actually wrong because the EM gaim assumes a slightly different probability
 831       distribution */
 832    for (i=N;i<N+M;i++)
 833    {
 834       /* See EM and Cohen papers*/
 835       spx_word32_t theta;
 836       /* Gain from hypergeometric function */
 837       spx_word32_t MM;
 838       /* Weiner filter gain */
 839       spx_word16_t prior_ratio;
 840       /* a priority probability of speech presence based on Bark sub-band alone */
 841       spx_word16_t P1;
 842       /* Speech absence a priori probability (considering sub-band and frame) */
 843       spx_word16_t q;
 844 #ifdef FIXED_POINT
 845       spx_word16_t tmp;
 846 #endif
 847
 848       prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
 849       theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
 850
 851       MM = hypergeom_gain(theta);
 852       /* Gain with bound */
 853       st->gain[i] = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
 854       /* Save old Bark power spectrum */
 855       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]);
 856
 857       P1 = QCONST16(.199f,15)+MULT16_16_Q15(QCONST16(.8f,15),qcurve (st->zeta[i]));
 858       q = Q15_ONE-MULT16_16_Q15(Pframe,P1);
 859 #ifdef FIXED_POINT
 860       theta = MIN32(theta, EXTEND32(32767));
 861 /*Q8*/tmp = MULT16_16_Q15((SHL32(1,SNR_SHIFT)+st->prior[i]),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1))));
 862       tmp = MIN16(QCONST16(3.,SNR_SHIFT), tmp); /* Prevent overflows in the next line*/
 863 /*Q8*/tmp = EXTRACT16(PSHR32(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8));
 864       st->gain2[i]=DIV32_16(SHL32(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
 865 #else
 866       st->gain2[i]=1/(1.f + (q/(1.f-q))*(1+st->prior[i])*exp(-theta));
 867 #endif
 868    }
 869    /* Convert the EM gains and speech prob to linear frequency */
 870    filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
 871    filterbank_compute_psd16(st->bank,st->gain+N, st->gain);
 872
 873    /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
 874    if (1)
 875    {
 876       filterbank_compute_psd16(st->bank,st->gain_floor+N, st->gain_floor);
 877
 878       /* Compute gain according to the Ephraim-Malah algorithm -- linear frequency */
 879       for (i=0;i<N;i++)
 880       {
 881          spx_word32_t MM;
 882          spx_word32_t theta;
 883          spx_word16_t prior_ratio;
 884          spx_word16_t tmp;
 885          spx_word16_t p;
 886          spx_word16_t g;
 887
 888          /* Wiener filter gain */
 889          prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
 890          theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
 891
 892          /* Optimal estimator for loudness domain */
 893          MM = hypergeom_gain(theta);
 894          /* EM gain with bound */
 895          g = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
 896          /* Interpolated speech probability of presence */
 897          p = st->gain2[i];
 898
 899          /* Constrain the gain to be close to the Bark scale gain */
 900          if (MULT16_16_Q15(QCONST16(.333f,15),g) > st->gain[i])
 901             g = MULT16_16(3,st->gain[i]);
 902          st->gain[i] = g;
 903
 904          /* Save old power spectrum */
 905          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]);
 906
 907          /* Apply gain floor */
 908          if (st->gain[i] < st->gain_floor[i])
 909             st->gain[i] = st->gain_floor[i];
 910
 911          /* Exponential decay model for reverberation (unused) */
 912          /*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];*/
 913
 914          /* Take into account speech probability of presence (loudness domain MMSE estimator) */
 915          /* gain2 = [p*sqrt(gain)+(1-p)*sqrt(gain _floor) ]^2 */
 916          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)));
 917          st->gain2[i]=SQR16_Q15(tmp);
 918
 919          /* Use this if you want a log-domain MMSE estimator instead */
 920          /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
 921       }
 922    } else {
 923       for (i=N;i<N+M;i++)
 924       {
 925          spx_word16_t tmp;
 926          spx_word16_t p = st->gain2[i];
 927          st->gain[i] = MAX16(st->gain[i], st->gain_floor[i]);
 928          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)));
 929          st->gain2[i]=SQR16_Q15(tmp);
 930       }
 931       filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
 932    }
 933
 934    /* If noise suppression is off, don't apply the gain (but then why call this in the first place!) */
 935    if (!st->denoise_enabled)
 936    {
 937       for (i=0;i<N+M;i++)
 938          st->gain2[i]=Q15_ONE;
 939    }
 940
 941    /* Apply computed gain */
 942    for (i=1;i<N;i++)
 943    {
 944       st->ft[2*i-1] = MULT16_16_P15(st->gain2[i],st->ft[2*i-1]);
 945       st->ft[2*i] = MULT16_16_P15(st->gain2[i],st->ft[2*i]);
 946    }
 947    st->ft[0] = MULT16_16_P15(st->gain2[0],st->ft[0]);
 948    st->ft[2*N-1] = MULT16_16_P15(st->gain2[N-1],st->ft[2*N-1]);
 949
 950    /*FIXME: This *will* not work for fixed-point */
 951 #ifndef FIXED_POINT
 952    if (st->agc_enabled)
 953       speex_compute_agc(st, Pframe, st->ft);
 954 #endif
 955
 956    /* Inverse FFT with 1/N scaling */
 957    spx_ifft(st->fft_lookup, st->ft, st->frame);
 958    /* Scale back to original (lower) amplitude */
 959    for (i=0;i<2*N;i++)
 960       st->frame[i] = PSHR16(st->frame[i], st->frame_shift);
 961
 962    /*FIXME: This *will* not work for fixed-point */
 963 #ifndef FIXED_POINT
 964    if (st->agc_enabled)
 965    {
 966       float max_sample=0;
 967       for (i=0;i<2*N;i++)
 968          if (fabs(st->frame[i])>max_sample)
 969             max_sample = fabs(st->frame[i]);
 970       if (max_sample>28000.f)
 971       {
 972          float damp = 28000.f/max_sample;
 973          for (i=0;i<2*N;i++)
 974             st->frame[i] *= damp;
 975       }
 976    }
 977 #endif
 978
 979    /* Synthesis window (for WOLA) */
 980    for (i=0;i<2*N;i++)
 981       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
 982
 983    /* Perform overlap and add */
 984    for (i=0;i<N3;i++)
 985       x[i] = st->outbuf[i] + st->frame[i];
 986    for (i=0;i<N4;i++)
 987       x[N3+i] = st->frame[N3+i];
 988
 989    /* Update outbuf */
 990    for (i=0;i<N3;i++)
 991       st->outbuf[i] = st->frame[st->frame_size+i];
 992
 993    /* FIXME: This VAD is a kludge */
 994    if (st->vad_enabled)
 995    {
 996       if (Pframe > st->speech_prob_start || (st->was_speech && Pframe > st->speech_prob_continue))
 997       {
 998          st->was_speech=1;
 999          return 1;
1000       } else
1001       {
1002          st->was_speech=0;
1003          return 0;
1004       }
1005    } else {
1006       return 1;
1007    }
1008 }
1009
1010 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x)
1011 {
1012    int i;
1013    int N = st->ps_size;
1014    int N3 = 2*N - st->frame_size;
1015    int M;
1016    spx_word32_t *ps=st->ps;
1017
1018    M = st->nbands;
1019    st->min_count++;
1020
1021    preprocess_analysis(st, x);
1022
1023    update_noise_prob(st);
1024
1025    for (i=1;i<N-1;i++)
1026    {
1027       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i],NOISE_SHIFT))
1028       {
1029          st->noise[i] = MULT16_32_Q15(QCONST16(.95f,15),st->noise[i]) + MULT16_32_Q15(QCONST16(.05f,15),SHL32(st->ps[i],NOISE_SHIFT));
1030       }
1031    }
1032
1033    for (i=0;i<N3;i++)
1034       st->outbuf[i] = MULT16_16_Q15(x[st->frame_size-N3+i],st->window[st->frame_size+i]);
1035
1036    /* Save old power spectrum */
1037    for (i=0;i<N+M;i++)
1038       st->old_ps[i] = ps[i];
1039
1040    for (i=0;i<N;i++)
1041       st->reverb_estimate[i] = MULT16_32_Q15(st->reverb_decay, st->reverb_estimate[i]);
1042 }
1043
1044
1045 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
1046 {
1047    int i;
1048    SpeexPreprocessState *st;
1049    st=(SpeexPreprocessState*)state;
1050    switch(request)
1051    {
1052    case SPEEX_PREPROCESS_SET_DENOISE:
1053       st->denoise_enabled = (*(spx_int32_t*)ptr);
1054       break;
1055    case SPEEX_PREPROCESS_GET_DENOISE:
1056       (*(spx_int32_t*)ptr) = st->denoise_enabled;
1057       break;
1058 #ifndef FIXED_POINT
1059    case SPEEX_PREPROCESS_SET_AGC:
1060       st->agc_enabled = (*(spx_int32_t*)ptr);
1061       break;
1062    case SPEEX_PREPROCESS_GET_AGC:
1063       (*(spx_int32_t*)ptr) = st->agc_enabled;
1064       break;
1065
1066    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
1067       st->agc_level = (*(float*)ptr);
1068       if (st->agc_level<1)
1069          st->agc_level=1;
1070       if (st->agc_level>32768)
1071          st->agc_level=32768;
1072       break;
1073    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
1074       (*(float*)ptr) = st->agc_level;
1075       break;
1076    case SPEEX_PREPROCESS_SET_AGC_INCREMENT:
1077       st->max_increase_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
1078       break;
1079    case SPEEX_PREPROCESS_GET_AGC_INCREMENT:
1080       (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_increase_step)*st->sampling_rate/st->frame_size);
1081       break;
1082    case SPEEX_PREPROCESS_SET_AGC_DECREMENT:
1083       st->max_decrease_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
1084       break;
1085    case SPEEX_PREPROCESS_GET_AGC_DECREMENT:
1086       (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_decrease_step)*st->sampling_rate/st->frame_size);
1087       break;
1088    case SPEEX_PREPROCESS_SET_AGC_MAX_GAIN:
1089       st->max_gain = exp(0.11513f * (*(spx_int32_t*)ptr));
1090       break;
1091    case SPEEX_PREPROCESS_GET_AGC_MAX_GAIN:
1092       (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_gain));
1093       break;
1094 #endif
1095    case SPEEX_PREPROCESS_SET_VAD:
1096       speex_warning("The VAD has been replaced by a hack pending a complete rewrite");
1097       st->vad_enabled = (*(spx_int32_t*)ptr);
1098       break;
1099    case SPEEX_PREPROCESS_GET_VAD:
1100       (*(spx_int32_t*)ptr) = st->vad_enabled;
1101       break;
1102
1103    case SPEEX_PREPROCESS_SET_DEREVERB:
1104       st->dereverb_enabled = (*(spx_int32_t*)ptr);
1105       for (i=0;i<st->ps_size;i++)
1106          st->reverb_estimate[i]=0;
1107       break;
1108    case SPEEX_PREPROCESS_GET_DEREVERB:
1109       (*(spx_int32_t*)ptr) = st->dereverb_enabled;
1110       break;
1111
1112    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
1113       st->reverb_level = (*(float*)ptr);
1114       break;
1115    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
1116       (*(float*)ptr) = st->reverb_level;
1117       break;
1118
1119    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
1120       st->reverb_decay = (*(float*)ptr);
1121       break;
1122    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
1123       (*(float*)ptr) = st->reverb_decay;
1124       break;
1125
1126    case SPEEX_PREPROCESS_SET_PROB_START:
1127       *(spx_int32_t*)ptr = MIN32(Q15_ONE,MAX32(0, *(spx_int32_t*)ptr));
1128       st->speech_prob_start = DIV32_16(MULT16_16(32767,*(spx_int32_t*)ptr), 100);
1129       break;
1130    case SPEEX_PREPROCESS_GET_PROB_START:
1131       (*(spx_int32_t*)ptr) = MULT16_16_Q15(st->speech_prob_start, 100);
1132       break;
1133
1134    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
1135       *(spx_int32_t*)ptr = MIN32(Q15_ONE,MAX32(0, *(spx_int32_t*)ptr));
1136       st->speech_prob_continue = DIV32_16(MULT16_16(32767,*(spx_int32_t*)ptr), 100);
1137       break;
1138    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
1139       (*(spx_int32_t*)ptr) = MULT16_16_Q15(st->speech_prob_continue, 100);
1140       break;
1141
1142    case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
1143       st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
1144       break;
1145    case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
1146       (*(spx_int32_t*)ptr) = st->noise_suppress;
1147       break;
1148    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
1149       st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
1150       break;
1151    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
1152       (*(spx_int32_t*)ptr) = st->echo_suppress;
1153       break;
1154    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
1155       st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
1156       break;
1157    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
1158       (*(spx_int32_t*)ptr) = st->echo_suppress_active;
1159       break;
1160    case SPEEX_PREPROCESS_SET_ECHO_STATE:
1161       st->echo_state = (SpeexEchoState*)ptr;
1162       break;
1163    case SPEEX_PREPROCESS_GET_ECHO_STATE:
1164       ptr = (void*)st->echo_state;
1165       break;
1166
1167    default:
1168       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
1169       return -1;
1170    }
1171    return 0;
1172 }