sb_celp.cpp

KphoneSI (kpsi) is a SIP (Session Initiation Protocol) user agent for Linux, with which you can in

      float *exc, *sp, *res, *target, *sw, tmp, filter_ratio;
      int offset;
      float rl, rh, eh=0, el=0;
      int fold;

      offset = st->subframeSize*sub;
      sp=st->high+offset;
      exc=st->exc+offset;
      res=st->res+offset;
      target=st->target+offset;
      sw=st->sw+offset;
      
      /* LSP interpolation (quantized and unquantized) */
      tmp = (1.0 + sub)/st->nbSubframes;
      for (i=0;i<st->lpcSize;i++)
         st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
      for (i=0;i<st->lpcSize;i++)
         st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
      
      lsp_enforce_margin(st->interp_lsp, st->lpcSize, .05);
      lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .05);

      /* Compute interpolated LPCs (quantized and unquantized) */
      for (i=0;i<st->lpcSize;i++)
         st->interp_lsp[i] = cos(st->interp_lsp[i]);
      for (i=0;i<st->lpcSize;i++)
         st->interp_qlsp[i] = cos(st->interp_qlsp[i]);

      lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
      lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);

      bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
      bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);

      /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
         filters */
      rl=rh=0;
      tmp=1;
      st->pi_gain[sub]=0;
      for (i=0;i<=st->lpcSize;i++)
      {
         rh += tmp*st->interp_qlpc[i];
         tmp = -tmp;
         st->pi_gain[sub]+=st->interp_qlpc[i];
      }
      rl = low_pi_gain[sub];
      rl=1/(fabs(rl)+.01);
      rh=1/(fabs(rh)+.01);
      /* Compute ratio, will help predict the gain */
      filter_ratio=fabs(.01+rh)/(.01+fabs(rl));

      fold = filter_ratio<5;
      /*printf ("filter_ratio %f\n", filter_ratio);*/
      fold=0;

      /* Compute "real excitation" */
      fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
      /* Compute energy of low-band and high-band excitation */
      for (i=0;i<st->subframeSize;i++)
         eh+=sqr(exc[i]);

      if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
         float g;
         /*speex_bits_pack(bits, 1, 1);*/
         for (i=0;i<st->subframeSize;i++)
            el+=sqr(low_innov[offset+i]);

         /* Gain to use if we want to use the low-band excitation for high-band */
         g=eh/(.01+el);
         g=sqrt(g);

         g *= filter_ratio;
         /*print_vec(&g, 1, "gain factor");*/
         /* Gain quantization */
         {
            int quant = (int) floor(.5 + 10 + 8.0 * log((g+.0001)));
            /*speex_warning_int("tata", quant);*/
            if (quant<0)
               quant=0;
            if (quant>31)
               quant=31;
            speex_bits_pack(bits, quant, 5);
            g= .1*exp(quant/9.4);
         }
         /*printf ("folding gain: %f\n", g);*/
         g /= filter_ratio;

      } else {
         float gc, scale, scale_1;

         for (i=0;i<st->subframeSize;i++)
            el+=sqr(low_exc[offset+i]);
         /*speex_bits_pack(bits, 0, 1);*/

         gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
         {
            int qgc = (int)floor(.5+3.7*(log(gc)+2));
            if (qgc<0)
               qgc=0;
            if (qgc>15)
               qgc=15;
            speex_bits_pack(bits, qgc, 4);
            gc = exp((1/3.7)*qgc-2);
         }

         scale = gc*sqrt(1+el)/filter_ratio;
         scale_1 = 1/scale;

         for (i=0;i<st->subframeSize;i++)
            exc[i]=0;
         exc[0]=1;
         syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
         
         /* Reset excitation */
         for (i=0;i<st->subframeSize;i++)
            exc[i]=0;
         
         /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
         for (i=0;i<st->lpcSize;i++)
            mem[i]=st->mem_sp[i];
         iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);

         for (i=0;i<st->lpcSize;i++)
            mem[i]=st->mem_sw[i];
         filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);

         /* Compute weighted signal */
         for (i=0;i<st->lpcSize;i++)
            mem[i]=st->mem_sw[i];
         filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);

         /* Compute target signal */
         for (i=0;i<st->subframeSize;i++)
            target[i]=sw[i]-res[i];

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


         for (i=0;i<st->subframeSize;i++)
            target[i]*=scale_1;
         
         /* Reset excitation */
         for (i=0;i<st->subframeSize;i++)
            innov[i]=0;

         /*print_vec(target, st->subframeSize, "\ntarget");*/
         SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
                                   SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
                                   innov, syn_resp, bits, stack, (st->complexity+1)>>1);
         /*print_vec(target, st->subframeSize, "after");*/

         for (i=0;i<st->subframeSize;i++)
            exc[i] += innov[i]*scale;

         if (SUBMODE(double_codebook)) {
            char *tmp_stack=stack;
            float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
            for (i=0;i<st->subframeSize;i++)
               innov2[i]=0;
            for (i=0;i<st->subframeSize;i++)
               target[i]*=2.5;
            SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
                                      SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
                                      innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
            for (i=0;i<st->subframeSize;i++)
               innov2[i]*=scale*(1/2.5);
            for (i=0;i<st->subframeSize;i++)
               exc[i] += innov2[i];
         }

      }

         /*Keep the previous memory*/
         for (i=0;i<st->lpcSize;i++)
            mem[i]=st->mem_sp[i];
         /* Final signal synthesis from excitation */
         iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
         
         /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
         filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
   }


#ifndef RELEASE

   /* 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);

   for (i=0;i<st->full_frame_size;i++)
      in[i]=2*(st->y0[i]-st->y1[i]);
#endif
   for (i=0;i<st->lpcSize;i++)
      st->old_lsp[i] = st->lsp[i];
   for (i=0;i<st->lpcSize;i++)
      st->old_qlsp[i] = st->qlsp[i];

   st->first=0;

   return 1;
}





void *sb_decoder_init(SpeexMode *m)
{
   SBDecState *st;
   SpeexSBMode *mode;
   st = (SBDecState*)speex_alloc(sizeof(SBDecState)+6000*sizeof(float));
   st->mode = m;
   mode=(SpeexSBMode*)m->mode;

   st->stack = ((char*)st) + sizeof(SBDecState);



   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=8;
   speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
   st->sampling_rate*=2;

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

   st->first=1;


   st->x0d=PUSH(st->stack, st->frame_size, float);
   st->x1d=PUSH(st->stack, st->frame_size, float);
   st->high=PUSH(st->stack, st->full_frame_size, float);
   st->y0=PUSH(st->stack, st->full_frame_size, float);
   st->y1=PUSH(st->stack, st->full_frame_size, float);

   st->h0_mem=PUSH(st->stack, QMF_ORDER, float);
   st->h1_mem=PUSH(st->stack, QMF_ORDER, float);
   st->g0_mem=PUSH(st->stack, QMF_ORDER, float);
   st->g1_mem=PUSH(st->stack, QMF_ORDER, float);

   st->exc=PUSH(st->stack, st->frame_size, float);

   st->qlsp = PUSH(st->stack, st->lpcSize, float);
   st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
   st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
   st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, float);

   st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
   st->mem_sp = PUSH(st->stack, 2*st->lpcSize, float);
   
   st->lpc_enh_enabled=0;

   return st;
}

void sb_decoder_destroy(void *state)
{
   SBDecState *st;
   st = (SBDecState*)state;
   speex_decoder_destroy(st->st_low);

   speex_free(state);
}

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

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

   st->first=1;
   
   awk1=PUSH(stack, st->lpcSize+1, float);
   awk2=PUSH(stack, st->lpcSize+1, float);
   awk3=PUSH(stack, st->lpcSize+1, float);
   
   if (st->lpc_enh_enabled)
   {
      float r=.9;
      
      float k1,k2,k3;
      if (st->submodes[st->submodeID] != NULL)
      {
         k1=SUBMODE(lpc_enh_k1);
         k2=SUBMODE(lpc_enh_k2);
      } else {
         k1=k2=.7;
      }
      k3=(1-(1-r*k1)/(1-r*k2))/r;
      k3=k1-k2;
      if (!st->lpc_enh_enabled)
      {
         k1=k2;
         k3=0;
      }
      bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
      bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
      bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
      /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
   }
   
   
   /* Final signal synthesis from excitation */
   if (!dtx)
   {
      for (i=0;i<st->frame_size;i++)
         st->exc[i] *= .9;
   }

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

   if (st->lpc_enh_enabled)
   {
      /* Use enhanced LPC filter */
      filter_mem2(st->high, awk2, awk1, st->high, st->frame_size, st->lpcSize, 
                  st->mem_sp+st->lpcSize);
      filter_mem2(st->high, awk3, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
                  st->mem_sp);
   } else {
      /* Use regular filter */
      for (i=0;i<st->lpcSize;i++)
         st->mem_sp[st->lpcSize+i] = 0;
      iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
               st->mem_sp);
   }
   
   /*iir_mem2(st->exc, 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);

   for (i=0;i<st->full_frame_size;i++)
      out[i]=2*(st->y0[i]-st->y1[i]);
   
   if (dtx)
   {
      st->submodeID=saved_modeid;
   }

   return;
}

int sb_decode(void *state, SpeexBits *bits, float *out)
{
   int i, sub;
   SBDecState *st;
   int wideband;
   int ret;
   char *stack;
   float *low_pi_gain, *low_exc, *low_innov;
   float *awk1, *awk2, *awk3;
   int dtx;
   SpeexSBMode *mode;

   st = (SBDecState*)state;
   stack=st->stack;
   mode = (SpeexSBMode*)(st->mode->mode);

   /* Decode the low-band */
   ret = speex_decode(st->st_low, bits, st->x0d);

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

   /*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);

      for (i=0;i<st->full_frame_size;i++)
         out[i]=2*(st->y0[i]-st->y1[i]);

      return 0;

   }

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

   low_pi_gain = PUSH(stack, st->nbSubframes, float);
   low_exc = PUSH(stack, st->frame_size, float);
   low_innov = PUSH(stack, st->frame_size, float);
   speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
   speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);