Index: pjproject/trunk/pjmedia/src/pjmedia-codec/speex/mdf.c
===================================================================
--- pjproject/trunk/pjmedia/src/pjmedia-codec/speex/mdf.c	(revision 523)
+++ pjproject/trunk/pjmedia/src/pjmedia-codec/speex/mdf.c	(revision 628)
@@ -91,5 +91,5 @@
 
 #ifdef FIXED_POINT
-static const spx_float_t MIN_LEAK = ((spx_float_t){16777, -24});
+static const spx_float_t MIN_LEAK = {16777, -24};
 #define TOP16(x) ((x)>>16)
 #else
@@ -141,7 +141,11 @@
    spx_word16_t notch_radius;
    spx_mem_t notch_mem[2];
+
+   /* NOTE: If you only use speex_echo_cancel() and want to save some memory, remove this */
+   spx_int16_t *play_buf;
+   int play_buf_pos;
 };
 
-static inline void filter_dc_notch16(spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem)
+static inline void filter_dc_notch16(const spx_int16_t *in, spx_word16_t radius, spx_word16_t *out, int len, spx_mem_t *mem)
 {
    int i;
@@ -167,13 +171,11 @@
 }
 
-static inline spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
+static inline spx_word32_t mdf_inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
 {
    spx_word32_t sum=0;
-   len >>= 2;
+   len >>= 1;
    while(len--)
    {
       spx_word32_t part=0;
-      part = MAC16_16(part,*x++,*y++);
-      part = MAC16_16(part,*x++,*y++);
       part = MAC16_16(part,*x++,*y++);
       part = MAC16_16(part,*x++,*y++);
@@ -185,5 +187,5 @@
 
 /** Compute power spectrum of a half-complex (packed) vector */
-static inline void power_spectrum(spx_word16_t *X, spx_word32_t *ps, int N)
+static inline void power_spectrum(const spx_word16_t *X, spx_word32_t *ps, int N)
 {
    int i, j;
@@ -198,5 +200,5 @@
 /** Compute cross-power spectrum of a half-complex (packed) vectors and add to acc */
 #ifdef FIXED_POINT
-static inline void spectral_mul_accum(spx_word16_t *X, spx_word32_t *Y, spx_word16_t *acc, int N, int M)
+static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M)
 {
    int i,j;
@@ -226,5 +228,5 @@
 }
 #else
-static inline void spectral_mul_accum(spx_word16_t *X, spx_word32_t *Y, spx_word16_t *acc, int N, int M)
+static inline void spectral_mul_accum(const spx_word16_t *X, const spx_word32_t *Y, spx_word16_t *acc, int N, int M)
 {
    int i,j;
@@ -247,5 +249,5 @@
 
 /** Compute weighted cross-power spectrum of a half-complex (packed) vector with conjugate */
-static inline void weighted_spectral_mul_conj(spx_float_t *w, spx_word16_t *X, spx_word16_t *Y, spx_word32_t *prod, int N)
+static inline void weighted_spectral_mul_conj(const spx_float_t *w, const spx_word16_t *X, const spx_word16_t *Y, spx_word32_t *prod, int N)
 {
    int i, j;
@@ -274,8 +276,8 @@
    /* FIXME: Make that an init option (new API call?) */
    st->sampling_rate = 8000;
-   st->spec_average = DIV32_16(SHL32(st->frame_size, 15), st->sampling_rate);
-#ifdef FIXED_POINT
-   st->beta0 = DIV32_16(SHL32(st->frame_size, 16), st->sampling_rate);
-   st->beta_max = DIV32_16(SHL32(st->frame_size, 14), st->sampling_rate);
+   st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
+#ifdef FIXED_POINT
+   st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
+   st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
 #else
    st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
@@ -333,4 +335,8 @@
    st->adapted = 0;
    st->Pey = st->Pyy = FLOAT_ONE;
+   
+   st->play_buf = (spx_int16_t*)speex_alloc(2*st->frame_size*sizeof(spx_int16_t));
+   st->play_buf_pos = 0;
+
    return st;
 }
@@ -386,10 +392,44 @@
    speex_free(st->wtmp2);
 #endif
+   speex_free(st->play_buf);
    speex_free(st);
 }
 
-extern int fixed_point;
+void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out, spx_int32_t *Yout)
+{
+   int i;
+   if (st->play_buf_pos>=st->frame_size)
+   {
+      speex_echo_cancel(st, rec, st->play_buf, out, Yout);
+      st->play_buf_pos -= st->frame_size;
+      for (i=0;i<st->frame_size;i++)
+         st->play_buf[i] = st->play_buf[i+st->frame_size];
+   } else {
+      speex_warning("no playback frame available");
+      if (st->play_buf_pos!=0)
+      {
+         speex_warning("internal playback buffer corruption?");
+         st->play_buf_pos = 0;
+      }
+      for (i=0;i<st->frame_size;i++)
+         out[i] = rec[i];
+   }
+}
+
+void speex_echo_playback(SpeexEchoState *st, const spx_int16_t *play)
+{
+   if (st->play_buf_pos<=st->frame_size)
+   {
+      int i;
+      for (i=0;i<st->frame_size;i++)
+         st->play_buf[st->play_buf_pos+i] = play[i];
+      st->play_buf_pos += st->frame_size;
+   } else {
+      speex_warning("had to discard a playback frame");
+   }
+}
+
 /** Performs echo cancellation on a frame */
-void speex_echo_cancel(SpeexEchoState *st, short *ref, short *echo, short *out, spx_int32_t *Yout)
+void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *ref, const spx_int16_t *echo, spx_int16_t *out, spx_int32_t *Yout)
 {
    int i,j;
@@ -403,4 +443,5 @@
    spx_word32_t tmp32;
    spx_word16_t M_1;
+   int saturated=0;
    
    N = st->window_size;
@@ -417,16 +458,44 @@
 #endif
 
-   filter_dc_notch16((spx_int16_t*)ref, st->notch_radius, st->d, st->frame_size, st->notch_mem);
+   filter_dc_notch16(ref, st->notch_radius, st->d, st->frame_size, st->notch_mem);
    /* Copy input data to buffer */
    for (i=0;i<st->frame_size;i++)
    {
       spx_word16_t tmp;
+      spx_word32_t tmp32;
       st->x[i] = st->x[i+st->frame_size];
-      st->x[i+st->frame_size] = SUB16(echo[i], MULT16_16_P15(st->preemph, st->memX));
+      tmp32 = SUB32(EXTEND32(echo[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX)));
+#ifdef FIXED_POINT
+      /*FIXME: If saturation occurs here, we need to freeze adaptation for M frames (not just one) */
+      if (tmp32 > 32767)
+      {
+         tmp32 = 32767;
+         saturated = 1;
+      }      
+      if (tmp32 < -32767)
+      {
+         tmp32 = -32767;
+         saturated = 1;
+      }      
+#endif
+      st->x[i+st->frame_size] = EXTRACT16(tmp32);
       st->memX = echo[i];
       
       tmp = st->d[i];
       st->d[i] = st->d[i+st->frame_size];
-      st->d[i+st->frame_size] = SUB16(tmp, MULT16_16_P15(st->preemph, st->memD));
+      tmp32 = SUB32(EXTEND32(tmp), EXTEND32(MULT16_16_P15(st->preemph, st->memD)));
+#ifdef FIXED_POINT
+      if (tmp32 > 32767)
+      {
+         tmp32 = 32767;
+         saturated = 1;
+      }      
+      if (tmp32 < -32767)
+      {
+         tmp32 = -32767;
+         saturated = 1;
+      }
+#endif
+      st->d[i+st->frame_size] = tmp32;
       st->memD = tmp;
    }
@@ -466,4 +535,10 @@
          tmp_out = -32768;
       tmp_out = ADD32(tmp_out, EXTEND32(MULT16_16_P15(st->preemph, st->memE)));
+      /* This is an arbitrary test for saturation */
+      if (ref[i] <= -32000 || ref[i] >= 32000)
+      {
+         tmp_out = 0;
+         saturated = 1;
+      }
       out[i] = tmp_out;
       st->memE = tmp_out;
@@ -478,7 +553,7 @@
 
    /* Compute a bunch of correlations */
-   See = inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size);
-   See = ADD32(See, SHR32(10000,6));
-   Syy = inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
+   See = mdf_inner_prod(st->e+st->frame_size, st->e+st->frame_size, st->frame_size);
+   See = ADD32(See, SHR32(EXTEND32(10000),6));
+   Syy = mdf_inner_prod(st->y+st->frame_size, st->y+st->frame_size, st->frame_size);
    
    /* Convert error to frequency domain */
@@ -545,6 +620,7 @@
    if (FLOAT_GT(st->Pey, st->Pyy))
       st->Pey = st->Pyy;
-   /* leak_estimate is the limear regression result */
+   /* leak_estimate is the linear regression result */
    leak_estimate = FLOAT_EXTRACT16(FLOAT_SHL(FLOAT_DIVU(st->Pey, st->Pyy),14));
+   /* This looks like a stupid bug, but it's right (because we convert from Q14 to Q15) */
    if (leak_estimate > 16383)
       leak_estimate = 32767;
@@ -595,5 +671,5 @@
       spx_word16_t adapt_rate=0;
 
-      Sxx = inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);
+      Sxx = mdf_inner_prod(st->x+st->frame_size, st->x+st->frame_size, st->frame_size);
       /* Temporary adaption rate if filter is not adapted correctly */
 
@@ -621,10 +697,13 @@
    }
 
-   /* Gradient descent */
-   for (i=0;i<M*N;i++)
-   {
-      st->W[i] += st->PHI[i];
-      /* Old value of W in PHI */
-      st->PHI[i] = st->W[i] - st->PHI[i];
+   if (!saturated)
+   {
+      /* Gradient descent */
+      for (i=0;i<M*N;i++)
+      {
+         st->W[i] += st->PHI[i];
+         /* Old value of W in PHI */
+         st->PHI[i] = st->W[i] - st->PHI[i];
+      }
    }
    
@@ -638,5 +717,5 @@
 #ifdef FIXED_POINT
          for (i=0;i<N;i++)
-            st->wtmp2[i] = PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16);
+            st->wtmp2[i] = EXTRACT16(PSHR32(st->W[j*N+i],NORMALIZE_SCALEDOWN+16));
          spx_ifft(st->fft_table, st->wtmp2, st->wtmp);
          for (i=0;i<st->frame_size;i++)
@@ -646,10 +725,10 @@
          for (i=st->frame_size;i<N;i++)
          {
-            st->wtmp[i]=SHL(st->wtmp[i],NORMALIZE_SCALEUP);
+            st->wtmp[i]=SHL16(st->wtmp[i],NORMALIZE_SCALEUP);
          }
          spx_fft(st->fft_table, st->wtmp, st->wtmp2);
          /* The "-1" in the shift is a sort of kludge that trades less efficient update speed for decrease noise */
          for (i=0;i<N;i++)
-            st->W[j*N+i] -= SHL32(st->wtmp2[i],16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
+            st->W[j*N+i] -= SHL32(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
 #else
          spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
@@ -716,8 +795,8 @@
       case SPEEX_ECHO_SET_SAMPLING_RATE:
          st->sampling_rate = (*(int*)ptr);
-         st->spec_average = DIV32_16(SHL32(st->frame_size, 15), st->sampling_rate);
-#ifdef FIXED_POINT
-         st->beta0 = DIV32_16(SHL32(st->frame_size, 16), st->sampling_rate);
-         st->beta_max = DIV32_16(SHL32(st->frame_size, 14), st->sampling_rate);
+         st->spec_average = DIV32_16(SHL32(EXTEND32(st->frame_size), 15), st->sampling_rate);
+#ifdef FIXED_POINT
+         st->beta0 = DIV32_16(SHL32(EXTEND32(st->frame_size), 16), st->sampling_rate);
+         st->beta_max = DIV32_16(SHL32(EXTEND32(st->frame_size), 14), st->sampling_rate);
 #else
          st->beta0 = (2.0f*st->frame_size)/st->sampling_rate;
