mdf.c

一个开源SIP协议栈

   st->X = (spx_word16_t*)speex_alloc((M+1)*N*sizeof(spx_word16_t));
   st->Y = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
   st->E = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
   st->W = (spx_word32_t*)speex_alloc(M*N*sizeof(spx_word32_t));
   st->PHI = (spx_word32_t*)speex_alloc(N*sizeof(spx_word32_t));
   st->power = (spx_word32_t*)speex_alloc((frame_size+1)*sizeof(spx_word32_t));
   st->power_1 = (spx_float_t*)speex_alloc((frame_size+1)*sizeof(spx_float_t));
   st->window = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
   st->prop = (spx_word16_t*)speex_alloc(M*sizeof(spx_word16_t));
   st->wtmp = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
#ifdef FIXED_POINT
   st->wtmp2 = (spx_word16_t*)speex_alloc(N*sizeof(spx_word16_t));
   for (i=0;i<N>>1;i++)
   {
      st->window[i] = (16383-SHL16(spx_cos(DIV32_16(MULT16_16(25736,i<<1),N)),1));
      st->window[N-i-1] = st->window[i];
   }
#else
   for (i=0;i<N;i++)
      st->window[i] = .5-.5*cos(2*M_PI*i/N);
#endif
   for (i=0;i<=st->frame_size;i++)
      st->power_1[i] = FLOAT_ONE;
   for (i=0;i<N*M;i++)
      st->W[i] = 0;
   {
      spx_word32_t sum = 0;
      /* Ratio of ~10 between adaptation rate of first and last block */
      spx_word16_t decay = SHR32(spx_exp(NEG16(DIV32_16(QCONST16(2.4,11),M))),1);
      st->prop[0] = QCONST16(.7, 15);
      sum = EXTEND32(st->prop[0]);
      for (i=1;i<M;i++)
      {
         st->prop[i] = MULT16_16_Q15(st->prop[i-1], decay);
         sum = ADD32(sum, EXTEND32(st->prop[i]));
      }
      for (i=M-1;i>=0;i--)
      {
         st->prop[i] = DIV32(MULT16_16(QCONST16(.8,15), st->prop[i]),sum);
      }
   }
   
   st->memX=st->memD=st->memE=0;
   st->preemph = QCONST16(.9,15);
   if (st->sampling_rate<12000)
      st->notch_radius = QCONST16(.9, 15);
   else if (st->sampling_rate<24000)
      st->notch_radius = QCONST16(.982, 15);
   else
      st->notch_radius = QCONST16(.992, 15);

   st->notch_mem[0] = st->notch_mem[1] = 0;
   st->adapted = 0;
   st->Pey = st->Pyy = FLOAT_ONE;
   
   st->play_buf = (spx_int16_t*)speex_alloc((PLAYBACK_DELAY+1)*st->frame_size*sizeof(spx_int16_t));
   st->play_buf_pos = PLAYBACK_DELAY*st->frame_size;
   st->play_buf_started = 0;
   
   return st;
}

/** Resets echo canceller state */
void speex_echo_state_reset(SpeexEchoState *st)
{
   int i, M, N;
   st->cancel_count=0;
   st->screwed_up = 0;
   N = st->window_size;
   M = st->M;
   for (i=0;i<N*M;i++)
      st->W[i] = 0;
   for (i=0;i<N*(M+1);i++)
      st->X[i] = 0;
   for (i=0;i<=st->frame_size;i++)
   {
      st->power[i] = 0;
      st->power_1[i] = FLOAT_ONE;
      st->Eh[i] = 0;
      st->Yh[i] = 0;
   }
   for (i=0;i<st->frame_size;i++)
   {
      st->last_y[i] = 0;
   }
   for (i=0;i<N;i++)
   {
      st->E[i] = 0;
      st->x[i] = 0;
   }
   st->notch_mem[0] = st->notch_mem[1] = 0;
   st->memX=st->memD=st->memE=0;

   st->saturated = 0;
   st->adapted = 0;
   st->sum_adapt = 0;
   st->Pey = st->Pyy = FLOAT_ONE;
   for (i=0;i<3*st->frame_size;i++)
      st->play_buf[i] = 0;
   st->play_buf_pos = PLAYBACK_DELAY*st->frame_size;
   st->play_buf_started = 0;

}

/** Destroys an echo canceller state */
void speex_echo_state_destroy(SpeexEchoState *st)
{
   spx_fft_destroy(st->fft_table);

   speex_free(st->e);
   speex_free(st->x);
   speex_free(st->input);
   speex_free(st->y);
   speex_free(st->last_y);
   speex_free(st->Yf);
   speex_free(st->Rf);
   speex_free(st->Xf);
   speex_free(st->Yh);
   speex_free(st->Eh);

   speex_free(st->X);
   speex_free(st->Y);
   speex_free(st->E);
   speex_free(st->W);
   speex_free(st->PHI);
   speex_free(st->power);
   speex_free(st->power_1);
   speex_free(st->window);
   speex_free(st->prop);
   speex_free(st->wtmp);
#ifdef FIXED_POINT
   speex_free(st->wtmp2);
#endif
   speex_free(st->play_buf);
   speex_free(st);
}

void speex_echo_capture(SpeexEchoState *st, const spx_int16_t *rec, spx_int16_t *out)
{
   int i;
   /*speex_warning_int("capture with fill level ", st->play_buf_pos/st->frame_size);*/
   st->play_buf_started = 1;
   if (st->play_buf_pos>=st->frame_size)
   {
      speex_echo_cancellation(st, rec, st->play_buf, out);
      st->play_buf_pos -= st->frame_size;
      for (i=0;i<st->play_buf_pos;i++)
         st->play_buf[i] = st->play_buf[i+st->frame_size];
   } else {
      speex_warning("No playback frame available (your application is buggy and/or got xruns)");
      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)
{
   /*speex_warning_int("playback with fill level ", st->play_buf_pos/st->frame_size);*/
   if (!st->play_buf_started)
   {
      speex_warning("discarded first playback frame");
      return;
   }
   if (st->play_buf_pos<=PLAYBACK_DELAY*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;
      if (st->play_buf_pos <= (PLAYBACK_DELAY-1)*st->frame_size)
      {
         speex_warning("Auto-filling the buffer (your application is buggy and/or got xruns)");
         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 (your application is buggy and/or got xruns)");
   }
}

/** Performs echo cancellation on a frame */
void speex_echo_cancel(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out, spx_int32_t *Yout)
{
   speex_echo_cancellation(st, in, far_end, out);
}

/** Performs echo cancellation on a frame (deprecated, last arg now ignored) */
void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
{
   int i,j;
   int N,M;
   spx_word32_t Syy,See,Sxx,Sdd;
   spx_word32_t Sey;
   spx_word16_t ss, ss_1;
   spx_float_t Pey = FLOAT_ONE, Pyy=FLOAT_ONE;
   spx_float_t alpha, alpha_1;
   spx_word16_t RER;
   spx_word32_t tmp32;
   
   N = st->window_size;
   M = st->M;
   st->cancel_count++;
#ifdef FIXED_POINT
   ss=DIV32_16(11469,M);
   ss_1 = SUB16(32767,ss);
#else
   ss=.35/M;
   ss_1 = 1-ss;
#endif

   filter_dc_notch16(in, st->notch_radius, st->input, st->frame_size, st->notch_mem);
   /* Copy input data to buffer and apply pre-emphasis */
   for (i=0;i<st->frame_size;i++)
   {
      spx_word32_t tmp32;
      st->x[i] = st->x[i+st->frame_size];
      tmp32 = SUB32(EXTEND32(far_end[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memX)));
#ifdef FIXED_POINT
      /* If saturation occurs here, we need to freeze adaptation for M+1 frames (not just one) */
      if (tmp32 > 32767)
      {
         tmp32 = 32767;
         st->saturated = M+1;
      }
      if (tmp32 < -32767)
      {
         tmp32 = -32767;
         st->saturated = M+1;
      }      
#endif
      st->x[i+st->frame_size] = EXTRACT16(tmp32);
      st->memX = far_end[i];
      
      tmp32 = SUB32(EXTEND32(st->input[i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD)));
#ifdef FIXED_POINT
      if (tmp32 > 32767)
      {
         tmp32 = 32767;
         if (st->saturated == 0)
            st->saturated = 1;
      }      
      if (tmp32 < -32767)
      {
         tmp32 = -32767;
         if (st->saturated == 0)
            st->saturated = 1;
      }
#endif
      st->memD = st->input[i];
      st->input[i] = tmp32;
   }

   /* Shift memory: this could be optimized eventually*/
   for (j=M-1;j>=0;j--)
   {
      for (i=0;i<N;i++)
         st->X[(j+1)*N+i] = st->X[j*N+i];
   }

   /* Convert x (far end) to frequency domain */
   spx_fft(st->fft_table, st->x, &st->X[0]);
   
#ifdef SMOOTH_BLOCKS
   spectral_mul_accum(st->X, st->W, st->Y, N, M);   
   spx_ifft(st->fft_table, st->Y, st->e);
#endif

   /* Compute weight gradient */
   if (st->saturated == 0)
   {
      for (j=M-1;j>=0;j--)
      {
         weighted_spectral_mul_conj(st->power_1, FLOAT_SHL(PSEUDOFLOAT(st->prop[j]),-15), &st->X[(j+1)*N], st->E, st->PHI, N);
         for (i=0;i<N;i++)
            st->W[j*N+i] = ADD32(st->W[j*N+i], st->PHI[i]);
         
      }
   } else {
      st->saturated--;
   }
   
   /* Update weight to prevent circular convolution (MDF / AUMDF) */
   for (j=0;j<M;j++)
   {
      /* This is a variant of the Alternatively Updated MDF (AUMDF) */
      /* Remove the "if" to make this an MDF filter */
      if (j==0 || st->cancel_count%(M-1) == j-1)
      {
#ifdef FIXED_POINT
         for (i=0;i<N;i++)
            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++)
         {
            st->wtmp[i]=0;
         }
         for (i=st->frame_size;i<N;i++)
         {
            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(EXTEND32(st->wtmp2[i]),16+NORMALIZE_SCALEDOWN-NORMALIZE_SCALEUP-1);
#else
         spx_ifft(st->fft_table, &st->W[j*N], st->wtmp);
         for (i=st->frame_size;i<N;i++)
         {
            st->wtmp[i]=0;
         }