mdf.c

mediastreamer2是开源的网络传输媒体流的库

   st->saturated = 0;
   st->screwed_up = 0;
   /* This is the default sampling rate */
   st->sampling_rate = 8000;
   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;
   st->beta_max = (.5f*st->frame_size)/st->sampling_rate;
#endif
   st->leak_estimate = 0;

   st->fft_table = spx_fft_init(N);
   
   st->e = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
   st->x = (spx_word16_t*)speex_alloc(K*N*sizeof(spx_word16_t));
   st->input = (spx_word16_t*)speex_alloc(C*st->frame_size*sizeof(spx_word16_t));
   st->y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
   st->last_y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
   st->Yf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
   st->Rf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
   st->Xf = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
   st->Yh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));
   st->Eh = (spx_word32_t*)speex_alloc((st->frame_size+1)*sizeof(spx_word32_t));

   st->X = (spx_word16_t*)speex_alloc(K*(M+1)*N*sizeof(spx_word16_t));
   st->Y = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
   st->E = (spx_word16_t*)speex_alloc(C*N*sizeof(spx_word16_t));
   st->W = (spx_word32_t*)speex_alloc(C*K*M*N*sizeof(spx_word32_t));
#ifdef TWO_PATH
   st->foreground = (spx_word16_t*)speex_alloc(M*N*C*K*sizeof(spx_word16_t));
#endif
   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*K*C;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(.8f,15), st->prop[i]),sum);
      }
   }
   
   st->memX = (spx_word16_t*)speex_alloc(K*sizeof(spx_word16_t));
   st->memD = (spx_word16_t*)speex_alloc(C*sizeof(spx_word16_t));
   st->memE = (spx_word16_t*)speex_alloc(C*sizeof(spx_word16_t));
   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 = (spx_mem_t*)speex_alloc(2*C*sizeof(spx_mem_t));
   st->adapted = 0;
   st->Pey = st->Pyy = FLOAT_ONE;
   
#ifdef TWO_PATH
   st->Davg1 = st->Davg2 = 0;
   st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
#endif
   
   st->play_buf = (spx_int16_t*)speex_alloc(K*(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 */
EXPORT void speex_echo_state_reset(SpeexEchoState *st)
{
   int i, M, N, C, K;
   st->cancel_count=0;
   st->screwed_up = 0;
   N = st->window_size;
   M = st->M;
   C=st->C;
   K=st->K;
   for (i=0;i<N*M;i++)
      st->W[i] = 0;
#ifdef TWO_PATH
   for (i=0;i<N*M;i++)
      st->foreground[i] = 0;
#endif
   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*C;i++)
   {
      st->E[i] = 0;
   }
   for (i=0;i<N*K;i++)
   {
      st->x[i] = 0;
   }
   for (i=0;i<2*C;i++)
      st->notch_mem[i] = 0;
   for (i=0;i<C;i++)
      st->memD[i]=st->memE[i]=0;
   for (i=0;i<K;i++)
      st->memX[i]=0;

   st->saturated = 0;
   st->adapted = 0;
   st->sum_adapt = 0;
   st->Pey = st->Pyy = FLOAT_ONE;
#ifdef TWO_PATH
   st->Davg1 = st->Davg2 = 0;
   st->Dvar1 = st->Dvar2 = FLOAT_ZERO;
#endif
   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 */
EXPORT 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);
#ifdef TWO_PATH
   speex_free(st->foreground);
#endif
   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->memX);
   speex_free(st->memD);
   speex_free(st->memE);
   speex_free(st->notch_mem);

   speex_free(st->play_buf);
   speex_free(st);
   
#ifdef DUMP_ECHO_CANCEL_DATA
   fclose(rFile);
   fclose(pFile);
   fclose(oFile);
   rFile = pFile = oFile = NULL;
#endif
}

EXPORT 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];
   }
}

EXPORT 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 (deprecated, last arg now ignored) */
EXPORT 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 */
EXPORT void speex_echo_cancellation(SpeexEchoState *st, const spx_int16_t *in, const spx_int16_t *far_end, spx_int16_t *out)
{
   int i,j, chan, speak;
   int N,M, C, K;
   spx_word32_t Syy,See,Sxx,Sdd, Sff;
#ifdef TWO_PATH
   spx_word32_t Dbf;
   int update_foreground;
#endif
   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;
   C = st->C;
   K = st->K;

   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

   for (chan = 0; chan < C; chan++)
   {
      /* Apply a notch filter to make sure DC doesn't end up causing problems */
      filter_dc_notch16(in+chan, st->notch_radius, st->input+chan*st->frame_size, st->frame_size, st->notch_mem+2*chan, C);
      /* Copy input data to buffer and apply pre-emphasis */
      /* Copy input data to buffer */
      for (i=0;i<st->frame_size;i++)
      {
         spx_word32_t tmp32;
         /* FIXME: This core has changed a bit, need to merge properly */
         tmp32 = SUB32(EXTEND32(st->input[chan*st->frame_size+i]), EXTEND32(MULT16_16_P15(st->preemph, st->memD[chan])));
#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[chan] = st->input[chan*st->frame_size+i];
         st->input[chan*st->frame_size+i] = EXTRACT16(tmp32);
      }
   }

   for (speak = 0; speak < K; speak++)
   {
      for (i=0;i<st->frame_size;i++)
      {
         spx_word32_t tmp32;
         st->x[speak*N+i] = st->x[speak*N+i+st->frame_size];
         tmp32 = SUB32(EXTEND32(far_end[i*K+speak]), EXTEND32(MULT16_16_P15(st->preemph, st->memX[speak])));
#ifdef FIXED_POINT
         /*FIXME: If saturation occurs here, we need to freeze adaptation for M 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[speak*N+i+st->frame_size] = EXTRACT16(tmp32);
         st->memX[speak] = far_end[i*K+speak];
      }
   }   
   
   for (speak = 0; speak < K; speak++)
   {
      /* 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*K+speak*N+i] = st->X[j*N*K+speak*N+i];
      }
      /* Convert x (echo input) to frequency domain */
      spx_fft(st->fft_table, st->x+speak*N, &st->X[speak*N]);
   }
   
   Sxx = 0;
   for (speak = 0; speak < K; speak++)
   {
      Sxx += mdf_inner_prod(st->x+speak*N+st->frame_size, st->x+speak*N+st->frame_size, st->frame_size);
      power_spectrum_accum(st->X+speak*N, st->Xf, N);
   }
   
   Sff = 0;  
   for (chan = 0; chan < C; chan++)
   {
#ifdef TWO_PATH
      /* Compute foreground filter */
      spectral_mul_accum16(st->X, st->foreground+chan*N*K*M, st->Y+chan*N, N, M*K);
      spx_ifft(st->fft_table, st->Y+chan*N, st->e+chan*N);
      for (i=0;i<st->frame_size;i++)
         st->e[chan*N+i] = SUB16(st->input[chan*st->frame_size+i], st->e[chan*N+i+st->frame_size]);
      Sff += mdf_inner_prod(st->e+chan*N, st->e+chan*N, st->frame_size);
#endif
   }
   
   /* Adjust proportional adaption rate */
   /* FIXME: Adjust that for C, K*/
   if (st->adapted)
      mdf_adjust_prop (st->W, N, M, C*K, st->prop);
   /* Compute weight gradient */
   if (st->saturated == 0)
   {
      for (chan = 0; chan < C; chan++)
      {
         for (speak = 0; speak < K; speak++)
         {
            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*K+speak*N], st->E+chan*N, st->PHI, N);
               for (i=0;i<N;i++)
                  st->W[chan*N*K*M + j*N*K + speak*N + i] += st->PHI[i];
            }
         }
      }
   } else {
      st->saturated--;
   }
   
   /* FIXME: MC conversion required */ 
   /* Update weight to prevent circular convolution (MDF / AUMDF) */
   for (chan = 0; chan < C; chan++)
   {
      for (speak = 0; speak < K; speak++)
      {
         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[chan*N*K*M + j*N*K + speak*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++)