Fix for fixed-point preprocessor bug reported by Peter Rowling
[speexdsp.git] / libspeex / preprocess.c
index 8679ee7..bbdb19f 100644 (file)
 #define M_PI 3.14159263
 #endif
 
-#define LOUDNESS_EXP 2.5
-
+#define LOUDNESS_EXP 5.f
+#define AMP_SCALE .001f
+#define AMP_SCALE_1 1000.f
+      
 #define NB_BANDS 24
 
 #define SPEECH_PROB_START_DEFAULT       QCONST16(0.35f,15)
 #define SPEECH_PROB_CONTINUE_DEFAULT    QCONST16(0.20f,15)
-#define NOISE_SUPPRESS_DEFAULT       -25
-#define ECHO_SUPPRESS_DEFAULT        -45
+#define NOISE_SUPPRESS_DEFAULT       -15
+#define ECHO_SUPPRESS_DEFAULT        -40
 #define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
 
 #ifndef NULL
@@ -185,12 +187,10 @@ struct SpeexPreprocessState_ {
    
    /* Parameters */
    int    denoise_enabled;
-   int    agc_enabled;
-   float  agc_level;
    int    vad_enabled;
    int    dereverb_enabled;
-   float  reverb_decay;
-   float  reverb_level;
+   spx_word16_t  reverb_decay;
+   spx_word16_t  reverb_level;
    spx_word16_t speech_prob_start;
    spx_word16_t speech_prob_continue;
    int    noise_suppress;
@@ -215,12 +215,9 @@ struct SpeexPreprocessState_ {
    spx_word32_t *S;          /**< Smoothed power spectrum */
    spx_word32_t *Smin;       /**< See Cohen paper */
    spx_word32_t *Stmp;       /**< See Cohen paper */
-   int *update_prob;       /**< Propability of speech presence for noise update */
+   int *update_prob;         /**< Probability of speech presence for noise update */
 
    spx_word16_t *zeta;       /**< Smoothed a priori SNR */
-
-   float *loudness_weight;   /**< Perceptual loudness curve */
-
    spx_word32_t *echo_noise;
    spx_word32_t *residual_echo;
 
@@ -228,11 +225,23 @@ struct SpeexPreprocessState_ {
    spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
    spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
 
-   int    was_speech;
-   float  loudness;          /**< loudness estimate */
-   float  loudness2;         /**< loudness estimate */
-   int    nb_adapt;          /**< Number of frames used for adaptation so far */
+   /* AGC stuff, only for floating point for now */
+#ifndef FIXED_POINT
+   int    agc_enabled;
+   float  agc_level;
+   float  loudness_accum;
+   float *loudness_weight;   /**< Perceptual loudness curve */
+   float  loudness;          /**< Loudness estimate */
+   float  agc_gain;          /**< Current AGC gain */
    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
+   float  max_gain;          /**< Maximum gain allowed */
+   float  max_increase_step; /**< Maximum increase in gain from one frame to another */
+   float  max_decrease_step; /**< Maximum decrease in gain from one frame to another */
+   float  prev_loudness;     /**< Loudness of previous frame */
+   float  init_max;          /**< Current gain limit during initialisation */
+#endif
+   int    nb_adapt;          /**< Number of frames used for adaptation so far */
+   int    was_speech;
    int    min_count;         /**< Number of frames processed so far */
    void  *fft_lookup;        /**< Lookup table for the FFT */
 #ifdef FIXED_POINT
@@ -246,30 +255,35 @@ static void conj_window(spx_word16_t *w, int len)
    int i;
    for (i=0;i<len;i++)
    {
-      float tmp;
-      float x=4*((float)i)/len;
+      spx_word16_t tmp;
+#ifdef FIXED_POINT
+      spx_word16_t x = DIV32_16(MULT16_16(32767,i),len);
+#else      
+      spx_word16_t x = DIV32_16(MULT16_16(QCONST16(4.f,13),i),len);
+#endif
       int inv=0;
-      if (x<1)
+      if (x<QCONST16(1.f,13))
       {
-      } else if (x<2)
+      } else if (x<QCONST16(2.f,13))
       {
-         x=2-x;
+         x=QCONST16(2.f,13)-x;
          inv=1;
-      } else if (x<3)
+      } else if (x<QCONST16(3.f,13))
       {
-         x=x-2;
+         x=x-QCONST16(2.f,13);
          inv=1;
       } else {
-         x=4-x;
+         x=QCONST16(2.f,13)-x+QCONST16(2.f,13); /* 4 - x */
       }
-      x*=1.9979;
-      tmp=(.5-.5*cos(x))*(.5-.5*cos(x));
+      x = MULT16_16_Q14(QCONST16(1.271903f,14), x);
+      tmp = SQR16_Q15(QCONST16(.5f,15)-MULT16_16_P15(QCONST16(.5f,15),spx_cos_norm(QCONST32(x,2))));
       if (inv)
-         tmp=1-tmp;
-      w[i]=Q15_ONE*sqrt(tmp);
+         tmp=SUB16(Q15_ONE,tmp);
+      w[i]=spx_sqrt(SHL32(EXTEND32(tmp),15));
    }
 }
 
+      
 #ifdef FIXED_POINT
 /* This function approximates the gain function 
    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
@@ -300,6 +314,36 @@ static inline spx_word16_t qcurve(spx_word16_t x)
    x = MAX16(x, 1);
    return DIV32_16(SHL32(EXTEND32(32767),9),ADD16(512,MULT16_16_Q15(QCONST16(.60f,15),DIV32_16(32767,x))));
 }
+
+/* Compute the gain floor based on different floors for the background noise and residual echo */
+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)
+{
+   int i;
+   
+   if (noise_suppress > effective_echo_suppress)
+   {
+      spx_word16_t noise_gain, gain_ratio;
+      noise_gain = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(0.11513,11),noise_suppress)),1)));
+      gain_ratio = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(.2302585f,11),effective_echo_suppress-noise_suppress)),1)));
+
+      /* gain_floor = sqrt [ (noise*noise_floor + echo*echo_floor) / (noise+echo) ] */
+      for (i=0;i<len;i++)
+         gain_floor[i] = MULT16_16_Q15(noise_gain,
+                                       spx_sqrt(SHL32(EXTEND32(DIV32_16_Q15(PSHR32(noise[i],NOISE_SHIFT) + MULT16_32_Q15(gain_ratio,echo[i]),
+                                             (1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]) )),15)));
+   } else {
+      spx_word16_t echo_gain, gain_ratio;
+      echo_gain = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(0.11513,11),effective_echo_suppress)),1)));
+      gain_ratio = EXTRACT16(MIN32(Q15_ONE,SHR32(spx_exp(MULT16_16(QCONST16(.2302585f,11),noise_suppress-effective_echo_suppress)),1)));
+
+      /* gain_floor = sqrt [ (noise*noise_floor + echo*echo_floor) / (noise+echo) ] */
+      for (i=0;i<len;i++)
+         gain_floor[i] = MULT16_16_Q15(echo_gain,
+                                       spx_sqrt(SHL32(EXTEND32(DIV32_16_Q15(MULT16_32_Q15(gain_ratio,PSHR32(noise[i],NOISE_SHIFT)) + echo[i],
+                                             (1+PSHR32(noise[i],NOISE_SHIFT) + echo[i]) )),15)));
+   }
+}
+
 #else
 /* This function approximates the gain function 
    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
@@ -330,6 +374,21 @@ static inline spx_word16_t qcurve(spx_word16_t x)
 {
    return 1.f/(1.f+.15f/(SNR_SCALING_1*x));
 }
+
+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)
+{
+   int i;
+   float echo_floor;
+   float noise_floor;
+
+   noise_floor = exp(.2302585f*noise_suppress);
+   echo_floor = exp(.2302585f*effective_echo_suppress);
+
+   /* Compute the gain floor based on different floors for the background noise and residual echo */
+   for (i=0;i<len;i++)
+      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]);
+}
+
 #endif
 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
 {
@@ -367,12 +426,10 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
    
    st->sampling_rate = sampling_rate;
    st->denoise_enabled = 1;
-   st->agc_enabled = 0;
-   st->agc_level = 8000;
    st->vad_enabled = 0;
    st->dereverb_enabled = 0;
-   st->reverb_decay = .0;
-   st->reverb_level = .0;
+   st->reverb_decay = 0;
+   st->reverb_level = 0;
    st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
    st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
    st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
@@ -408,7 +465,6 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
    st->Stmp = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
    st->update_prob = (int*)speex_alloc(N*sizeof(int));
    
-   st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
    st->inbuf = (spx_word16_t*)speex_alloc(N3*sizeof(spx_word16_t));
    st->outbuf = (spx_word16_t*)speex_alloc(N3*sizeof(spx_word16_t));
 
@@ -427,11 +483,11 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
    for (i=0;i<N+M;i++)
    {
       st->noise[i]=QCONST32(1.f,NOISE_SHIFT);
-      st->reverb_estimate[i]=0.;
-      st->old_ps[i]=1.;
+      st->reverb_estimate[i]=0;
+      st->old_ps[i]=1;
       st->gain[i]=Q15_ONE;
-      st->post[i]=Q15_ONE;
-      st->prior[i]=Q15_ONE;
+      st->post[i]=SHL16(1, SNR_SHIFT);
+      st->prior[i]=SHL16(1, SNR_SHIFT);
    }
 
    for (i=0;i<N;i++)
@@ -441,20 +497,30 @@ SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_r
       st->inbuf[i]=0;
       st->outbuf[i]=0;
    }
-
+#ifndef FIXED_POINT
+   st->agc_enabled = 0;
+   st->agc_level = 8000;
+   st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
    for (i=0;i<N;i++)
    {
       float ff=((float)i)*.5*sampling_rate/((float)N);
+      /*st->loudness_weight[i] = .5f*(1.f/(1.f+ff/8000.f))+1.f*exp(-.5f*(ff-3800.f)*(ff-3800.f)/9e5f);*/
       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
       if (st->loudness_weight[i]<.01f)
          st->loudness_weight[i]=.01f;
       st->loudness_weight[i] *= st->loudness_weight[i];
    }
-
-   st->was_speech = 0;
-   st->loudness = pow(6000,LOUDNESS_EXP);
-   st->loudness2 = 6000;
+   /*st->loudness = pow(AMP_SCALE*st->agc_level,LOUDNESS_EXP);*/
+   st->loudness = 1e-15;
+   st->agc_gain = 1;
    st->nb_loudness_adapt = 0;
+   st->max_gain = 30;
+   st->max_increase_step = exp(0.11513f * 12.*st->frame_size / st->sampling_rate);
+   st->max_decrease_step = exp(-0.11513f * 40.*st->frame_size / st->sampling_rate);
+   st->prev_loudness = 1;
+   st->init_max = 1;
+#endif
+   st->was_speech = 0;
 
    st->fft_lookup = spx_fft_init(2*N);
 
@@ -477,7 +543,9 @@ void speex_preprocess_state_destroy(SpeexPreprocessState *st)
    speex_free(st->gain);
    speex_free(st->prior);
    speex_free(st->post);
+#ifndef FIXED_POINT
    speex_free(st->loudness_weight);
+#endif
    speex_free(st->echo_noise);
    speex_free(st->residual_echo);
 
@@ -497,63 +565,53 @@ void speex_preprocess_state_destroy(SpeexPreprocessState *st)
 
 /* FIXME: The AGC doesn't work yet with fixed-point*/
 #ifndef FIXED_POINT
-static void speex_compute_agc(SpeexPreprocessState *st)
+static void speex_compute_agc(SpeexPreprocessState *st, spx_word16_t Pframe, spx_word16_t *ft)
 {
    int i;
    int N = st->ps_size;
-   float scale=.5f/N;
-   float agc_gain;
-   int freq_start, freq_end;
-   float active_bands = 0;
-
-   freq_start = (int)(300.0f*2*N/st->sampling_rate);
-   freq_end   = (int)(2000.0f*2*N/st->sampling_rate);
-   for (i=freq_start;i<freq_end;i++)
+   float target_gain;
+   float loudness=1.f;
+   float rate;
+   
+   for (i=2;i<N;i++)
    {
-      if (st->S[i] > 20.f*st->Smin[i]+1000.f)
-         active_bands+=1;
+      loudness += 2.f*N*st->ps[i]* st->loudness_weight[i];
    }
-   active_bands /= (freq_end-freq_start+1);
-
-   if (active_bands > .2f)
+   loudness=sqrt(loudness);
+      /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
+   loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
+   if (Pframe>.3f)
    {
-      float loudness=0.f;
-      float rate, rate2=.2f;
       st->nb_loudness_adapt++;
-      rate=2.0f/(1+st->nb_loudness_adapt);
-      if (rate < .05f)
-         rate = .05f;
-      if (rate < .1f && pow(loudness, LOUDNESS_EXP) > st->loudness)
-         rate = .1f;
-      if (rate < .2f && pow(loudness, LOUDNESS_EXP) > 3.f*st->loudness)
-         rate = .2f;
-      if (rate < .4f && pow(loudness, LOUDNESS_EXP) > 10.f*st->loudness)
-         rate = .4f;
-
-      for (i=2;i<N;i++)
-      {
-         loudness += scale*st->ps[i] * FRAC_SCALING_1*FRAC_SCALING_1*st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
-      }
-      loudness=sqrt(loudness);
-      /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
-        loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
-      st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
-      
-      st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
-
-      loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
-
-      /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
+      /*rate=2.0f*Pframe*Pframe/(1+st->nb_loudness_adapt);*/
+      rate = .03*Pframe*Pframe;
+      st->loudness = (1-rate)*st->loudness + (rate)*pow(AMP_SCALE*loudness, LOUDNESS_EXP);
+      st->loudness_accum = (1-rate)*st->loudness_accum + rate;
+      if (st->init_max < st->max_gain && st->nb_adapt > 20)
+         st->init_max *= 1.f + .1f*Pframe*Pframe;
    }
+   /*printf ("%f %f %f %f\n", Pframe, loudness, pow(st->loudness, 1.0f/LOUDNESS_EXP), st->loudness2);*/
    
-   agc_gain = st->agc_level/st->loudness2;
-   /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
-   if (agc_gain>200)
-      agc_gain = 200;
+   target_gain = AMP_SCALE*st->agc_level*pow(st->loudness/(1e-4+st->loudness_accum), -1.0f/LOUDNESS_EXP);
 
-   for (i=0;i<N;i++)
-      st->gain2[i] *= agc_gain;
+   if ((Pframe>.5  && st->nb_adapt > 20) || target_gain < st->agc_gain)
+   {
+      if (target_gain > st->max_increase_step*st->agc_gain)
+         target_gain = st->max_increase_step*st->agc_gain;
+      if (target_gain < st->max_decrease_step*st->agc_gain && loudness < 10*st->prev_loudness)
+         target_gain = st->max_decrease_step*st->agc_gain;
+      if (target_gain > st->max_gain)
+         target_gain = st->max_gain;
+      if (target_gain > st->init_max)
+         target_gain = st->init_max;
    
+      st->agc_gain = target_gain;
+   }
+   /*fprintf (stderr, "%f %f %f\n", loudness, (float)AMP_SCALE_1*pow(st->loudness, 1.0f/LOUDNESS_EXP), st->agc_gain);*/
+      
+   for (i=0;i<2*N;i++)
+      ft[i] *= st->agc_gain;
+   st->prev_loudness = loudness;
 }
 #endif
 
@@ -594,7 +652,6 @@ static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
    spx_fft(st->fft_lookup, st->frame, st->ft);
          
    /* Power spectrum */
-   /*FIXME: Set ps[0] properly */
    ps[0]=MULT16_16(st->ft[0],st->ft[0]);
    for (i=1;i<N;i++)
       ps[i]=MULT16_16(st->ft[2*i-1],st->ft[2*i-1]) + MULT16_16(st->ft[2*i],st->ft[2*i]);
@@ -677,10 +734,11 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
    spx_word32_t Zframe;
    spx_word16_t Pframe;
    spx_word16_t beta, beta_1;
-   float echo_floor;
-   float noise_floor;
+   spx_word16_t effective_echo_suppress;
    
    st->nb_adapt++;
+   if (st->nb_adapt>20000)
+      st->nb_adapt = 20000;
    st->min_count++;
    
    beta = MAX16(QCONST16(.03,15),DIV32_16(Q15_ONE,st->nb_adapt));
@@ -690,6 +748,14 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
    if (st->echo_state)
    {
       speex_echo_get_residual(st->echo_state, st->residual_echo, N);
+#ifndef FIXED_POINT
+      /* If there are NaNs or ridiculous values, it'll show up in the DC and we just reset everything to zero */
+      if (!(st->residual_echo[0] >=0 && st->residual_echo[0]<N*1e9f))
+      {
+         for (i=0;i<N;i++)
+            st->residual_echo[i] = 0;
+      }
+#endif
       for (i=0;i<N;i++)
          st->echo_noise[i] = MAX32(MULT16_32_Q15(QCONST16(.6f,15),st->echo_noise[i]), st->residual_echo[i]);
       filterbank_compute_bank32(st->bank, st->echo_noise, st->echo_noise+N);
@@ -713,7 +779,7 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
    for (i=0;i<N;i++)
    {
       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i], NOISE_SHIFT))
-         st->noise[i] = MULT16_32_Q15(beta_1,st->noise[i]) + MULT16_32_Q15(beta,SHL32(st->ps[i],NOISE_SHIFT));
+         st->noise[i] = MAX32(EXTEND32(0),MULT16_32_Q15(beta_1,st->noise[i]) + MULT16_32_Q15(beta,SHL32(st->ps[i],NOISE_SHIFT)));
    }
    filterbank_compute_bank32(st->bank, st->noise, st->noise+N);
 
@@ -758,9 +824,10 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
       Zframe = ADD32(Zframe, EXTEND32(st->zeta[i]));
    Pframe = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.899f,15),qcurve(DIV32_16(Zframe,st->nbands)));
    
-   noise_floor = exp(.2302585f*st->noise_suppress);
-   echo_floor = exp(.2302585f* (st->echo_suppress*(1-FRAC_SCALING_1*Pframe) + st->echo_suppress_active*FRAC_SCALING_1*Pframe));
+   effective_echo_suppress = EXTRACT16(PSHR32(ADD32(MULT16_16(SUB16(Q15_ONE,Pframe), st->echo_suppress), MULT16_16(Pframe, st->echo_suppress_active)),15));
    
+   compute_gain_floor(st->noise_suppress, effective_echo_suppress, st->noise+N, st->echo_noise+N, st->gain_floor+N, M);
+         
    /* Compute Ephraim & Malah gain speech probability of presence for each critical band (Bark scale) 
       Technically this is actually wrong because the EM gaim assumes a slightly different probability 
       distribution */
@@ -779,8 +846,7 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
 #ifdef FIXED_POINT
       spx_word16_t tmp;
 #endif
-      /* Compute the gain floor based on different floors for the background noise and residual echo */
-      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]);
+      
       prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
       theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
 
@@ -796,8 +862,8 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
       theta = MIN32(theta, EXTEND32(32767));
 /*Q8*/tmp = MULT16_16_Q15((SHL32(1,SNR_SHIFT)+st->prior[i]),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1))));
       tmp = MIN16(QCONST16(3.,SNR_SHIFT), tmp); /* Prevent overflows in the next line*/
-/*Q8*/tmp = PSHR(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8);
-      st->gain2[i]=DIV32_16(SHL(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
+/*Q8*/tmp = EXTRACT16(PSHR32(MULT16_16(PDIV32_16(SHL32(EXTEND32(q),8),(Q15_ONE-q)),tmp),8));
+      st->gain2[i]=DIV32_16(SHL32(EXTEND32(32767),SNR_SHIFT), ADD16(256,tmp));
 #else
       st->gain2[i]=1/(1.f + (q/(1.f-q))*(1+st->prior[i])*exp(-theta));
 #endif
@@ -873,13 +939,7 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
       for (i=0;i<N+M;i++)
          st->gain2[i]=Q15_ONE;
    }
-   
-   /*FIXME: This *will* not work for fixed-point */
-#ifndef FIXED_POINT
-   if (st->agc_enabled)
-      speex_compute_agc(st);
-#endif
-   
+      
    /* Apply computed gain */
    for (i=1;i<N;i++)
    {
@@ -889,6 +949,12 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
    st->ft[0] = MULT16_16_P15(st->gain2[0],st->ft[0]);
    st->ft[2*N-1] = MULT16_16_P15(st->gain2[N-1],st->ft[2*N-1]);
    
+   /*FIXME: This *will* not work for fixed-point */
+#ifndef FIXED_POINT
+   if (st->agc_enabled)
+      speex_compute_agc(st, Pframe, st->ft);
+#endif
+
    /* Inverse FFT with 1/N scaling */
    spx_ifft(st->fft_lookup, st->ft, st->frame);
    /* Scale back to original (lower) amplitude */
@@ -943,7 +1009,7 @@ int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
    }
 }
 
-void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
+void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x)
 {
    int i;
    int N = st->ps_size;
@@ -962,7 +1028,7 @@ void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x,
    {
       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i],NOISE_SHIFT))
       {
-         st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
+         st->noise[i] = MULT16_32_Q15(QCONST16(.95f,15),st->noise[i]) + MULT16_32_Q15(QCONST16(.05f,15),SHL32(st->ps[i],NOISE_SHIFT));
       }
    }
 
@@ -973,8 +1039,8 @@ void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x,
    for (i=0;i<N+M;i++)
       st->old_ps[i] = ps[i];
 
-   for (i=1;i<N;i++)
-      st->reverb_estimate[i] *= st->reverb_decay;
+   for (i=0;i<N;i++)
+      st->reverb_estimate[i] = MULT16_32_Q15(st->reverb_decay, st->reverb_estimate[i]);
 }
 
 
@@ -991,7 +1057,7 @@ int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
    case SPEEX_PREPROCESS_GET_DENOISE:
       (*(spx_int32_t*)ptr) = st->denoise_enabled;
       break;
-
+#ifndef FIXED_POINT
    case SPEEX_PREPROCESS_SET_AGC:
       st->agc_enabled = (*(spx_int32_t*)ptr);
       break;
@@ -1009,7 +1075,25 @@ int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
       (*(float*)ptr) = st->agc_level;
       break;
-
+   case SPEEX_PREPROCESS_SET_AGC_INCREMENT:
+      st->max_increase_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
+      break;
+   case SPEEX_PREPROCESS_GET_AGC_INCREMENT:
+      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_increase_step)*st->sampling_rate/st->frame_size);
+      break;
+   case SPEEX_PREPROCESS_SET_AGC_DECREMENT:
+      st->max_decrease_step = exp(0.11513f * (*(spx_int32_t*)ptr)*st->frame_size / st->sampling_rate);
+      break;
+   case SPEEX_PREPROCESS_GET_AGC_DECREMENT:
+      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_decrease_step)*st->sampling_rate/st->frame_size);
+      break;
+   case SPEEX_PREPROCESS_SET_AGC_MAX_GAIN:
+      st->max_gain = exp(0.11513f * (*(spx_int32_t*)ptr));
+      break;
+   case SPEEX_PREPROCESS_GET_AGC_MAX_GAIN:
+      (*(spx_int32_t*)ptr) = floor(.5+8.6858*log(st->max_gain));
+      break;
+#endif
    case SPEEX_PREPROCESS_SET_VAD:
       speex_warning("The VAD has been replaced by a hack pending a complete rewrite");
       st->vad_enabled = (*(spx_int32_t*)ptr);