sb_celp.c

一个开源SIP协议栈

      st->old_qlsp[i] = st->qlsp[i];

   st->first=0;

   return 1;
}





void *sb_decoder_init(const SpeexMode *m)
{
   spx_int32_t tmp;
   SBDecState *st;
   const SpeexSBMode *mode;
   st = (SBDecState*)speex_alloc(sizeof(SBDecState));
   if (!st)
      return NULL;
#if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
   st->stack = NULL;
#else
   st->stack = (char*)speex_alloc_scratch(SB_DEC_STACK);
#endif
   st->mode = m;
   mode=(const SpeexSBMode*)m->mode;

   st->encode_submode = 1;




   st->st_low = speex_decoder_init(mode->nb_mode);
   st->full_frame_size = 2*mode->frameSize;
   st->frame_size = mode->frameSize;
   st->subframeSize = mode->subframeSize;
   st->nbSubframes = mode->frameSize/mode->subframeSize;
   st->lpcSize=mode->lpcSize;
   speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
   st->sampling_rate*=2;
   tmp=1;
   speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);

   st->submodes=mode->submodes;
   st->submodeID=mode->defaultSubmode;

   st->first=1;


   st->x0d = (spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
   st->x1d = (spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
   st->high = (spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
   st->y0 = (spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));
   st->y1 = (spx_sig_t*)speex_alloc((st->full_frame_size)*sizeof(spx_sig_t));

   st->g0_mem = (spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
   st->g1_mem = (spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));

   st->exc = (spx_sig_t*)speex_alloc((st->frame_size)*sizeof(spx_sig_t));
   st->excBuf = (spx_sig_t*)speex_alloc((st->subframeSize)*sizeof(spx_sig_t));

   st->qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->old_qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->interp_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
   st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));

   st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
   st->mem_sp = (spx_mem_t*)speex_alloc((2*st->lpcSize)*sizeof(spx_mem_t));
   
   st->low_innov = (spx_word32_t*)speex_alloc((st->frame_size)*sizeof(spx_word32_t));
   speex_decoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, st->low_innov);
   st->innov_save = NULL;


   st->lpc_enh_enabled=0;
   st->seed = 1000;

#ifdef ENABLE_VALGRIND
   VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
#endif
   return st;
}

void sb_decoder_destroy(void *state)
{
   SBDecState *st;
   st = (SBDecState*)state;
   speex_decoder_destroy(st->st_low);
#if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
   speex_free_scratch(st->stack);
#endif

   speex_free(st->x0d);
   speex_free(st->x1d);
   speex_free(st->high);
   speex_free(st->y0);
   speex_free(st->y1);
   speex_free(st->g0_mem);
   speex_free(st->g1_mem);
   speex_free(st->exc);
   speex_free(st->excBuf);
   speex_free(st->qlsp);
   speex_free(st->old_qlsp);
   speex_free(st->interp_qlsp);
   speex_free(st->interp_qlpc);
   speex_free(st->pi_gain);
   speex_free(st->low_innov);
   speex_free(st->mem_sp);

   speex_free(state);
}

static void sb_decode_lost(SBDecState *st, spx_word16_t *out, int dtx, char *stack)
{
   int i;
   int saved_modeid=0;

   if (dtx)
   {
      saved_modeid=st->submodeID;
      st->submodeID=1;
   } else {
      bw_lpc(GAMMA_SCALING*0.99, st->interp_qlpc, st->interp_qlpc, st->lpcSize);
   }

   st->first=1;
   
   
   /* Final signal synthesis from excitation */
   if (!dtx)
   {
      spx_word16_t low_ener;
      low_ener = .9*compute_rms(st->exc, st->frame_size);
      for (i=0;i<st->frame_size;i++)
         st->exc[i] = speex_rand(low_ener, &st->seed);
   }

   for (i=0;i<st->frame_size;i++)
      st->high[i]=st->exc[i];

   iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
            st->mem_sp);
   
   
   /* Reconstruct the original */
   fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
   fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);

   mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);

   if (dtx)
   {
      st->submodeID=saved_modeid;
   }

   return;
}

int sb_decode(void *state, SpeexBits *bits, void *vout)
{
   int i, sub;
   SBDecState *st;
   int wideband;
   int ret;
   char *stack;
   VARDECL(spx_word32_t *low_pi_gain);
   VARDECL(spx_word16_t *low_exc);
   VARDECL(spx_coef_t *ak);
   spx_int32_t dtx;
   const SpeexSBMode *mode;
   spx_word16_t *out = (spx_word16_t*)vout;
   
   st = (SBDecState*)state;
   stack=st->stack;
   mode = (const SpeexSBMode*)(st->mode->mode);

   {
      VARDECL(spx_word16_t *low);
      ALLOC(low, st->frame_size, spx_word16_t);
      
      /* Decode the low-band */
      ret = speex_decode_native(st->st_low, bits, low);
      
      for (i=0;i<st->frame_size;i++)
         st->x0d[i] = SHL((spx_sig_t)low[i], SIG_SHIFT);
   }

   speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);

   /* If error decoding the narrowband part, propagate error */
   if (ret!=0)
   {
      return ret;
   }

   if (!bits)
   {
      sb_decode_lost(st, out, dtx, stack);
      return 0;
   }

   if (st->encode_submode)
   {

      /*Check "wideband bit"*/
      if (speex_bits_remaining(bits)>0)
         wideband = speex_bits_peek(bits);
      else
         wideband = 0;
      if (wideband)
      {
         /*Regular wideband frame, read the submode*/
         wideband = speex_bits_unpack_unsigned(bits, 1);
         st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
      } else
      {
         /*Was a narrowband frame, set "null submode"*/
         st->submodeID = 0;
      }
      if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
      {
         speex_warning("Invalid mode encountered: corrupted stream?");
         return -2;
      }
   }

   /* If null mode (no transmission), just set a couple things to zero*/
   if (st->submodes[st->submodeID] == NULL)
   {
      if (dtx)
      {
         sb_decode_lost(st, out, 1, stack);
         return 0;
      }

      for (i=0;i<st->frame_size;i++)
         st->exc[i]=VERY_SMALL;

      st->first=1;

      /* Final signal synthesis from excitation */
      iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);

      fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
      fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);

      mix_and_saturate(st->y0, st->y1, out, st->full_frame_size);

      return 0;

   }

   for (i=0;i<st->frame_size;i++)
      st->exc[i]=0;

   ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
   ALLOC(low_exc, st->frame_size, spx_word16_t);
   speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
   speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);

   SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
   
   if (st->first)
   {
      for (i=0;i<st->lpcSize;i++)
         st->old_qlsp[i] = st->qlsp[i];
   }
   
   ALLOC(ak, st->lpcSize, spx_coef_t);

   for (sub=0;sub<st->nbSubframes;sub++)
   {
      spx_sig_t *exc, *sp, *innov_save=NULL;
      spx_word16_t filter_ratio;
      spx_word16_t el=0;
      int offset;
      spx_word32_t rl=0,rh=0;
      
      offset = st->subframeSize*sub;
      sp=st->high+offset;
      exc=st->exc+offset;
      /* Pointer for saving innovation */
      if (st->innov_save)
      {
         innov_save = st->innov_save+2*offset;
         for (i=0;i<2*st->subframeSize;i++)
            innov_save[i]=0;
      }
      
      /* LSP interpolation */
      lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);

      lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);

      /* LSP to LPC */
      lsp_to_lpc(st->interp_qlsp, ak, st->lpcSize, stack);

      /* Calculate reponse ratio between the low and high filter in the middle
         of the band (4000 Hz) */
      
         st->pi_gain[sub]=LPC_SCALING;
         rh = LPC_SCALING;
         for (i=0;i<st->lpcSize;i+=2)
         {
            rh += st->interp_qlpc[i+1] - st->interp_qlpc[i];
            st->pi_gain[sub] += st->interp_qlpc[i] + st->interp_qlpc[i+1];
         }

         rl = low_pi_gain[sub];
#ifdef FIXED_POINT
         filter_ratio=PDIV32_16(SHL(rl+82,2),SHR(82+rh,5));
#else
         filter_ratio=(rl+.01)/(rh+.01);
#endif
      
      for (i=0;i<st->subframeSize;i++)
         exc[i]=0;
      if (!SUBMODE(innovation_unquant))
      {
         float g;
         int quant;

         quant = speex_bits_unpack_unsigned(bits, 5);
         g= exp(((float)quant-10)/8.0);
         
#ifdef FIXED_POINT
         g /= filter_ratio/128.;
#else
         g /= filter_ratio;
#endif
         /* High-band excitation using the low-band excitation and a gain */
         
#if 0
         for (i=0;i<st->subframeSize;i++)
            exc[i]=mode->folding_gain*g*st->low_innov[offset+i];
#else
         {
            float tmp=1;
            /*static tmp1=0,tmp2=0;
            static int seed=1;
            el = compute_rms(low_innov+offset, st->subframeSize);*/
            for (i=0;i<st->subframeSize;i++)
            {
               float e=tmp*g*mode->folding_gain*st->low_innov[offset+i];
               tmp *= -1;
               exc[i] = e;
               /*float r = speex_rand(g*el,&seed);
               exc[i] = .5*(r+tmp2 + e-tmp1);
               tmp1 = e;
               tmp2 = r;*/               
            }
            
         }
         
#endif    
      } else {
         spx_word16_t gc;
         spx_word32_t scale;
         int qgc = speex_bits_unpack_unsigned(bits, 4);

         el = compute_rms16(low_exc+offset, st->subframeSize);

#ifdef FIXED_POINT
         gc = MULT16_32_Q15(28626,gc_quant_bound[qgc]);
#else
         gc = exp((1/3.7)*qgc-0.15556);
#endif

         if (st->subframeSize==80)
            gc *= 1.4142;

         scale = SHL(MULT16_16(PDIV32_16(SHL(gc,SIG_SHIFT-6),filter_ratio),(1+el)),6);

         SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
                                     bits, stack, &st->seed);

         signal_mul(exc,exc,scale,st->subframeSize);

         if (SUBMODE(double_codebook)) {
            char *tmp_stack=stack;
            VARDECL(spx_sig_t *innov2);
            ALLOC(innov2, st->subframeSize, spx_sig_t);
            for (i=0;i<st->subframeSize;i++)
               innov2[i]=0;
            SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
                                        bits, stack, &st->seed);
            for (i=0;i<st->subframeSize;i++)
               innov2[i]*=scale/(float)SIG_SCALING*(1/2.5);
            for (i=0;i<st->subframeSize;i++)
               exc[i] = ADD32(exc[i],innov2[i]);
            stack = tmp_stack;
         }

      }
      
      if (st->innov_save)
      {
         for (i=0;i<st->subframeSize;i++)
            innov_save[2*i]=exc[i];
      }
      
      for (i=0;i<st->subframeSize;i++)
         sp[i]=st->excBuf[i];
      iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
               st->mem_sp);
      for (i=0;i<st->subframeSize;i++)
         st->excBuf[i]=exc[i];
      for (i=0;i<st->lpcSize;i++)
         st->interp_qlpc[i] = ak[i];

   }

   fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);