开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 嵌入式/单片机编程 > Windows CE > 查看源码
nb_celp.c
资源名称：tcpmp.rar　[点击查看]
上传用户：wstnjxml
上传日期：2014-04-03
资源大小：7248k
文件大小：60k
源码类别：

Windows CE

开发平台：

C/C++

nb_celp.c：源码内容
  1. /* Copyright (C) 2002 Jean-Marc Valin 
  2.    File: nb_celp.c
  4.    Redistribution and use in source and binary forms, with or without
  5.    modification, are permitted provided that the following conditions
  6.    are met:
  7.    
  8.    - Redistributions of source code must retain the above copyright
  9.    notice, this list of conditions and the following disclaimer.
  10.    
  11.    - Redistributions in binary form must reproduce the above copyright
  12.    notice, this list of conditions and the following disclaimer in the
  13.    documentation and/or other materials provided with the distribution.
  14.    
  15.    - Neither the name of the Xiph.org Foundation nor the names of its
  16.    contributors may be used to endorse or promote products derived from
  17.    this software without specific prior written permission.
  18.    
  19.    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  20.    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  21.    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  22.    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  23.    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  24.    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  25.    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  26.    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  27.    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  28.    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  29.    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30. */
  32. #ifdef HAVE_CONFIG_H
  33. #include "config.h"
  34. #endif
  36. #include <math.h>
  37. #include "nb_celp.h"
  38. #include "lpc.h"
  39. #include "lsp.h"
  40. #include "ltp.h"
  41. #include "quant_lsp.h"
  42. #include "cb_search.h"
  43. #include "filters.h"
  44. #include "stack_alloc.h"
  45. #include "vq.h"
  46. #include <speex/speex_bits.h>
  47. #include "vbr.h"
  48. #include "misc.h"
  49. #include <speex/speex_callbacks.h>
  51. #ifndef M_PI
  52. #define M_PI           3.14159265358979323846  /* pi */
  53. #endif
  55. #ifndef NULL
  56. #define NULL 0
  57. #endif
  59. #define SUBMODE(x) st->submodes[st->submodeID]->x
  61. /* Default size for the encoder and decoder stack (can be changed at compile time).
  62.    This does not apply when using variable-size arrays or alloca. */
  63. #ifndef NB_ENC_STACK
  64. #define NB_ENC_STACK (8000*sizeof(spx_sig_t))
  65. #endif
  67. #ifndef NB_DEC_STACK
  68. #define NB_DEC_STACK (4000*sizeof(spx_sig_t))
  69. #endif
  72. #ifdef FIXED_POINT
  73. const spx_word32_t ol_gain_table[32]={18900, 25150, 33468, 44536, 59265, 78865, 104946, 139653, 185838, 247297, 329081, 437913, 582736, 775454, 1031906, 1373169, 1827293, 2431601, 3235761, 4305867, 5729870, 7624808, 10146425, 13501971, 17967238, 23909222, 31816294, 42338330, 56340132, 74972501, 99766822, 132760927};
  74. const spx_word16_t exc_gain_quant_scal3_bound[7]={1841, 3883, 6051, 8062, 10444, 13580, 18560};
  75. const spx_word16_t exc_gain_quant_scal3[8]={1002, 2680, 5086, 7016, 9108, 11781, 15380, 21740};
  76. const spx_word16_t exc_gain_quant_scal1_bound[1]={14385};
  77. const spx_word16_t exc_gain_quant_scal1[2]={11546, 17224};
  79. #define LSP_MARGIN 16
  80. #define LSP_DELTA1 6553
  81. #define LSP_DELTA2 1638
  83. #else
  85. const float exc_gain_quant_scal3_bound[7]={0.112338, 0.236980, 0.369316, 0.492054, 0.637471, 0.828874, 1.132784};
  86. const float exc_gain_quant_scal3[8]={0.061130, 0.163546, 0.310413, 0.428220, 0.555887, 0.719055, 0.938694, 1.326874};
  87. const float exc_gain_quant_scal1_bound[1]={0.87798};
  88. const float exc_gain_quant_scal1[2]={0.70469, 1.05127};
  90. #define LSP_MARGIN .002
  91. #define LSP_DELTA1 .2
  92. #define LSP_DELTA2 .05
  94. #endif
  99. #define sqr(x) ((x)*(x))
  101. void *nb_encoder_init(const SpeexMode *m)
  102. {
  103.    EncState *st;
  104.    const SpeexNBMode *mode;
  105.    int i;
  107.    mode=(const SpeexNBMode *)m->mode;
  108.    st = (EncState*)speex_alloc(sizeof(EncState));
  109.    if (!st)
  110.       return NULL;
  111. #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
  112.    st->stack = NULL;
  113. #else
  114.    st->stack = (char*)speex_alloc_scratch(NB_ENC_STACK);
  115. #endif
  116.    
  117.    st->mode=m;
  119.    st->frameSize = mode->frameSize;
  120.    st->windowSize = st->frameSize*3/2;
  121.    st->nbSubframes=mode->frameSize/mode->subframeSize;
  122.    st->subframeSize=mode->subframeSize;
  123.    st->lpcSize = mode->lpcSize;
  124.    st->gamma1=mode->gamma1;
  125.    st->gamma2=mode->gamma2;
  126.    st->min_pitch=mode->pitchStart;
  127.    st->max_pitch=mode->pitchEnd;
  128.    st->lag_factor=mode->lag_factor;
  129.    st->lpc_floor = mode->lpc_floor;
  130.   
  131.    st->submodes=mode->submodes;
  132.    st->submodeID=st->submodeSelect=mode->defaultSubmode;
  133.    st->bounded_pitch = 1;
  135.    st->encode_submode = 1;
  136. #ifdef EPIC_48K
  137.    st->lbr_48k=mode->lbr48k;
  138. #endif
  140.    /* Allocating input buffer */
  141.    st->inBuf = speex_alloc((st->windowSize)*sizeof(spx_sig_t));
  142.    st->frame = st->inBuf;
  143.    /* Allocating excitation buffer */
  144.    st->excBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t));
  145.    st->exc = st->excBuf + mode->pitchEnd + 1;
  146.    st->swBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t));
  147.    st->sw = st->swBuf + mode->pitchEnd + 1;
  149.    st->innov = speex_alloc((st->frameSize)*sizeof(spx_sig_t));
  151.    /* Asymmetric "pseudo-Hamming" window */
  152.    {
  153.       int part1, part2;
  154.       part1=st->frameSize - (st->subframeSize>>1);
  155.       part2=(st->frameSize>>1) + (st->subframeSize>>1);
  156.       st->window = speex_alloc((st->windowSize)*sizeof(spx_word16_t));
  157.       for (i=0;i<part1;i++)
  158.          st->window[i]=(spx_word16_t)(SIG_SCALING*(.54-.46*cos(M_PI*i/part1)));
  159.       for (i=0;i<part2;i++)
  160.          st->window[part1+i]=(spx_word16_t)(SIG_SCALING*(.54+.46*cos(M_PI*i/part2)));
  161.    }
  162.    /* Create the window for autocorrelation (lag-windowing) */
  163.    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
  164.    for (i=0;i<st->lpcSize+1;i++)
  165.       st->lagWindow[i]=16384*exp(-.5*sqr(2*M_PI*st->lag_factor*i));
  167.    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
  169.    st->lpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
  170.    st->interp_lpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
  171.    st->interp_qlpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
  172.    st->bw_lpc1 = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
  173.    st->bw_lpc2 = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
  175.    st->lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
  176.    st->qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
  177.    st->old_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
  178.    st->old_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
  179.    st->interp_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
  180.    st->interp_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
  182.    st->first = 1;
  183.    for (i=0;i<st->lpcSize;i++)
  184.    {
  185.       st->lsp[i]=LSP_SCALING*(M_PI*((float)(i+1)))/(st->lpcSize+1);
  186.    }
  188.    st->mem_sp = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
  189.    st->mem_sw = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
  190.    st->mem_sw_whole = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
  191.    st->mem_exc = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
  193.    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
  195.    st->pitch = speex_alloc((st->nbSubframes)*sizeof(int));
  197.    st->vbr = speex_alloc(sizeof(VBRState));
  198.    vbr_init(st->vbr);
  199.    st->vbr_quality = 8;
  200.    st->vbr_enabled = 0;
  201.    st->vad_enabled = 0;
  202.    st->dtx_enabled = 0;
  203.    st->abr_enabled = 0;
  204.    st->abr_drift = 0;
  206.    st->plc_tuning = 2;
  207.    st->complexity=2;
  208.    st->sampling_rate=8000;
  209.    st->dtx_count=0;
  211. #ifdef ENABLE_VALGRIND
  212.    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
  213. #endif
  214.    return st;
  215. }
  217. void nb_encoder_destroy(void *state)
  218. {
  219.    EncState *st=(EncState *)state;
  220.    /* Free all allocated memory */
  221. #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
  222.    speex_free_scratch(st->stack);
  223. #endif
  225.    speex_free (st->inBuf);
  226.    speex_free (st->excBuf);
  227.    speex_free (st->innov);
  228.    speex_free (st->interp_qlpc);
  229.    speex_free (st->qlsp);
  230.    speex_free (st->old_qlsp);
  231.    speex_free (st->interp_qlsp);
  232.    speex_free (st->swBuf);
  234.    speex_free (st->window);
  235.    speex_free (st->lagWindow);
  236.    speex_free (st->autocorr);
  237.    speex_free (st->lpc);
  238.    speex_free (st->lsp);
  240.    speex_free (st->interp_lpc);
  241.    speex_free (st->bw_lpc1);
  242.    speex_free (st->bw_lpc2);
  243.    speex_free (st->old_lsp);
  244.    speex_free (st->interp_lsp);
  245.    speex_free (st->mem_sp);
  246.    speex_free (st->mem_sw);
  247.    speex_free (st->mem_sw_whole);
  248.    speex_free (st->mem_exc);
  249.    speex_free (st->pi_gain);
  250.    speex_free (st->pitch);
  252.    vbr_destroy(st->vbr);
  253.    speex_free (st->vbr);
  255.    /*Free state memory... should be last*/
  256.    speex_free(st);
  257. }
  259. int nb_encode(void *state, void *vin, SpeexBits *bits)
  260. {
  261.    EncState *st;
  262.    int i, sub, roots;
  263.    int ol_pitch;
  264.    spx_word16_t ol_pitch_coef;
  265.    spx_word32_t ol_gain;
  266.    VARDECL(spx_sig_t *res);
  267.    VARDECL(spx_sig_t *target);
  268.    VARDECL(spx_mem_t *mem);
  269.    char *stack;
  270.    VARDECL(spx_word16_t *syn_resp);
  271.    VARDECL(spx_sig_t *real_exc);
  272. #ifdef EPIC_48K
  273.    int pitch_half[2];
  274.    int ol_pitch_id=0;
  275. #endif
  276.    spx_word16_t *in = vin;
  278.    st=(EncState *)state;
  279.    stack=st->stack;
  281.    /* Copy new data in input buffer */
  282.    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->windowSize-st->frameSize)*sizeof(spx_sig_t));
  283.    for (i=0;i<st->frameSize;i++)
  284.       st->inBuf[st->windowSize-st->frameSize+i] = SHL32(EXTEND32(in[i]), SIG_SHIFT);
  286.    /* Move signals 1 frame towards the past */
  287.    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t));
  288.    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t));
  290.    {
  291.       VARDECL(spx_word16_t *w_sig);
  292.       ALLOC(w_sig, st->windowSize, spx_word16_t);
  293.       /* Window for analysis */
  294.       for (i=0;i<st->windowSize;i++)
  295.          w_sig[i] = EXTRACT16(SHR32(MULT16_16(EXTRACT16(SHR32(st->frame[i],SIG_SHIFT)),st->window[i]),SIG_SHIFT));
  297.       /* Compute auto-correlation */
  298.       _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize);
  299.    }
  300.    st->autocorr[0] = (spx_word16_t) (st->autocorr[0]*st->lpc_floor); /* Noise floor in auto-correlation domain */
  302.    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
  303.    for (i=0;i<st->lpcSize+1;i++)
  304.       st->autocorr[i] = MULT16_16_Q14(st->autocorr[i],st->lagWindow[i]);
  306.    /* Levinson-Durbin */
  307.    _spx_lpc(st->lpc, st->autocorr, st->lpcSize);
  309.    /* LPC to LSPs (x-domain) transform */
  310.    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, LSP_DELTA1, stack);
  311.    /* Check if we found all the roots */
  312.    if (roots!=st->lpcSize)
  313.    {
  314.       /* Search again if we can afford it */
  315.       if (st->complexity>1)
  316.          roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, LSP_DELTA2, stack);
  317.       if (roots!=st->lpcSize) 
  318.       {
  319.          /*If we can't find all LSP's, do some damage control and use previous filter*/
  320.          for (i=0;i<st->lpcSize;i++)
  321.          {
  322.             st->lsp[i]=st->old_lsp[i];
  323.          }
  324.       }
  325.    }
  329.    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
  330.    {
  331.       if (st->first)
  332.          for (i=0;i<st->lpcSize;i++)
  333.             st->interp_lsp[i] = st->lsp[i];
  334.       else
  335.          lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, st->nbSubframes, st->nbSubframes<<1);
  337.       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);
  339.       /* Compute interpolated LPCs (unquantized) for whole frame*/
  340.       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
  343.       /*Open-loop pitch*/
  344.       if (!st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) ||
  345.           SUBMODE(lbr_pitch) != -1)
  346.       {
  347.          int nol_pitch[6];
  348.          spx_word16_t nol_pitch_coef[6];
  349.          
  350.          bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
  351.          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
  352.          
  353.          filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
  355.          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
  356.                                nol_pitch, nol_pitch_coef, 6, stack);
  357.          ol_pitch=nol_pitch[0];
  358.          ol_pitch_coef = nol_pitch_coef[0];
  359.          /*Try to remove pitch multiples*/
  360.          for (i=1;i<6;i++)
  361.          {
  362. #ifdef FIXED_POINT
  363.             if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],27853)) && 
  364. #else
  365.             if ((nol_pitch_coef[i]>.85*nol_pitch_coef[0]) && 
  366. #endif
  367.                 (ABS(2*nol_pitch[i]-ol_pitch)<=2 || ABS(3*nol_pitch[i]-ol_pitch)<=3 || 
  368.                  ABS(4*nol_pitch[i]-ol_pitch)<=4 || ABS(5*nol_pitch[i]-ol_pitch)<=5))
  369.             {
  370.                /*ol_pitch_coef=nol_pitch_coef[i];*/
  371.                ol_pitch = nol_pitch[i];
  372.             }
  373.          }
  374.          /*if (ol_pitch>50)
  375.            ol_pitch/=2;*/
  376.          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
  378. #ifdef EPIC_48K
  379.          if (st->lbr_48k)
  380.          {
  381.             if (ol_pitch < st->min_pitch+2)
  382.                ol_pitch = st->min_pitch+2;
  383.             if (ol_pitch > st->max_pitch-2)
  384.                ol_pitch = st->max_pitch-2;
  385.             open_loop_nbest_pitch(st->sw, ol_pitch-2, ol_pitch+2, st->frameSize>>1, 
  386.                                   &pitch_half[0], nol_pitch_coef, 1, stack);
  387.             open_loop_nbest_pitch(st->sw+(st->frameSize>>1), pitch_half[0]-1, pitch_half[0]+2, st->frameSize>>1, 
  388.                                   &pitch_half[1], nol_pitch_coef, 1, stack);
  389.          }
  390. #endif
  391.       } else {
  392.          ol_pitch=0;
  393.          ol_pitch_coef=0;
  394.       }
  395.       /*Compute "real" excitation*/
  396.       fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);
  398.       /* Compute open-loop excitation gain */
  399. #ifdef EPIC_48K
  400.       if (st->lbr_48k)
  401.       {
  402.          float ol1=0,ol2=0;
  403.          float ol_gain2;
  404.          ol1 = compute_rms(st->exc, st->frameSize>>1);
  405.          ol2 = compute_rms(st->exc+(st->frameSize>>1), st->frameSize>>1);
  406.          ol1 *= ol1*(st->frameSize>>1);
  407.          ol2 *= ol2*(st->frameSize>>1);
  409.          ol_gain2=ol1;
  410.          if (ol2>ol1)
  411.             ol_gain2=ol2;
  412.          ol_gain2 = sqrt(2*ol_gain2*(ol1+ol2))*1.3*(1-.5*GAIN_SCALING_1*GAIN_SCALING_1*ol_pitch_coef*ol_pitch_coef);
  413.       
  414.          ol_gain=SHR(sqrt(1+ol_gain2/st->frameSize),SIG_SHIFT);
  416.       } else {
  417. #endif
  418.          ol_gain = SHL32(EXTEND32(compute_rms(st->exc, st->frameSize)),SIG_SHIFT);
  419. #ifdef EPIC_48K
  420.       }
  421. #endif
  422.    }
  424.    /*VBR stuff*/
  425.    if (st->vbr && (st->vbr_enabled||st->vad_enabled))
  426.    {
  427.       float lsp_dist=0;
  428.       for (i=0;i<st->lpcSize;i++)
  429.          lsp_dist += (st->old_lsp[i] - st->lsp[i])*(st->old_lsp[i] - st->lsp[i]);
  430.       lsp_dist /= LSP_SCALING*LSP_SCALING;
  431.       
  432.       if (st->abr_enabled)
  433.       {
  434.          float qual_change=0;
  435.          if (st->abr_drift2 * st->abr_drift > 0)
  436.          {
  437.             /* Only adapt if long-term and short-term drift are the same sign */
  438.             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
  439.             if (qual_change>.05)
  440.                qual_change=.05;
  441.             if (qual_change<-.05)
  442.                qual_change=-.05;
  443.          }
  444.          st->vbr_quality += qual_change;
  445.          if (st->vbr_quality>10)
  446.             st->vbr_quality=10;
  447.          if (st->vbr_quality<0)
  448.             st->vbr_quality=0;
  449.       }
  451.       st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, GAIN_SCALING_1*ol_pitch_coef);
  452.       /*if (delta_qual<0)*/
  453.       /*  delta_qual*=.1*(3+st->vbr_quality);*/
  454.       if (st->vbr_enabled) 
  455.       {
  456.          int mode;
  457.          int choice=0;
  458.          float min_diff=100;
  459.          mode = 8;
  460.          while (mode)
  461.          {
  462.             int v1;
  463.             float thresh;
  464.             v1=(int)floor(st->vbr_quality);
  465.             if (v1==10)
  466.                thresh = vbr_nb_thresh[mode][v1];
  467.             else
  468.                thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
  469.             if (st->relative_quality > thresh && 
  470.                 st->relative_quality-thresh<min_diff)
  471.             {
  472.                choice = mode;
  473.                min_diff = st->relative_quality-thresh;
  474.             }
  475.             mode--;
  476.          }
  477.          mode=choice;
  478.          if (mode==0)
  479.          {
  480.             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
  481.             {
  482.                mode=1;
  483.                st->dtx_count=1;
  484.             } else {
  485.                mode=0;
  486.                st->dtx_count++;
  487.             }
  488.          } else {
  489.             st->dtx_count=0;
  490.          }
  492.          speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
  494.          if (st->abr_enabled)
  495.          {
  496.             int bitrate;
  497.             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
  498.             st->abr_drift+=(bitrate-st->abr_enabled);
  499.             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
  500.             st->abr_count += 1.0;
  501.          }
  503.       } else {
  504.          /*VAD only case*/
  505.          int mode;
  506.          if (st->relative_quality<2)
  507.          {
  508.             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
  509.             {
  510.                st->dtx_count=1;
  511.                mode=1;
  512.             } else {
  513.                mode=0;
  514.                st->dtx_count++;
  515.             }
  516.          } else {
  517.             st->dtx_count = 0;
  518.             mode=st->submodeSelect;
  519.          }
  520.          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
  521.          st->submodeID=mode;
  522.       } 
  523.    } else {
  524.       st->relative_quality = -1;
  525.    }
  527.    if (st->encode_submode)
  528.    {
  529. #ifdef EPIC_48K
  530.    if (!st->lbr_48k) {
  531. #endif
  533.    /* First, transmit a zero for narrowband */
  534.    speex_bits_pack(bits, 0, 1);
  536.    /* Transmit the sub-mode we use for this frame */
  537.    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
  539. #ifdef EPIC_48K
  540.    }
  541. #endif
  542.    }
  544.    /* If null mode (no transmission), just set a couple things to zero*/
  545.    if (st->submodes[st->submodeID] == NULL)
  546.    {
  547.       for (i=0;i<st->frameSize;i++)
  548.          st->exc[i]=st->sw[i]=VERY_SMALL;
  550.       for (i=0;i<st->lpcSize;i++)
  551.          st->mem_sw[i]=0;
  552.       st->first=1;
  553.       st->bounded_pitch = 1;
  555.       /* Final signal synthesis from excitation */
  556.       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
  558. #ifdef RESYNTH
  559.       for (i=0;i<st->frameSize;i++)
  560.          in[i]=st->frame[i];
  561. #endif
  562.       return 0;
  564.    }
  566.    /* LSP Quantization */
  567.    if (st->first)
  568.    {
  569.       for (i=0;i<st->lpcSize;i++)
  570.          st->old_lsp[i] = st->lsp[i];
  571.    }
  574.    /*Quantize LSPs*/
  575. #if 1 /*0 for unquantized*/
  576.    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
  577. #else
  578.    for (i=0;i<st->lpcSize;i++)
  579.      st->qlsp[i]=st->lsp[i];
  580. #endif
  582. #ifdef EPIC_48K
  583.    if (st->lbr_48k) {
  584.       speex_bits_pack(bits, pitch_half[0]-st->min_pitch, 7);
  585.       speex_bits_pack(bits, pitch_half[1]-pitch_half[0]+1, 2);
  586.       
  587.       {
  588.          int quant = (int)floor(.5+7.4*GAIN_SCALING_1*ol_pitch_coef);
  589.          if (quant>7)
  590.             quant=7;
  591.          if (quant<0)
  592.             quant=0;
  593.          ol_pitch_id=quant;
  594.          speex_bits_pack(bits, quant, 3);
  595.          ol_pitch_coef=GAIN_SCALING*0.13514*quant;
  596.          
  597.       }
  598.       {
  599.          int qe = (int)(floor(.5+2.1*log(ol_gain*1.0/SIG_SCALING)))-2;
  600.          if (qe<0)
  601.             qe=0;
  602.          if (qe>15)
  603.             qe=15;
  604.          ol_gain = exp((qe+2)/2.1)*SIG_SCALING;
  605.          speex_bits_pack(bits, qe, 4);
  606.       }
  608.    } else {
  609. #endif
  611.    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
  612.    if (SUBMODE(lbr_pitch)!=-1)
  613.    {
  614.       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
  615.    } 
  617.    if (SUBMODE(forced_pitch_gain))
  618.    {
  619.       int quant;
  620.       quant = (int)floor(.5+15*ol_pitch_coef*GAIN_SCALING_1);
  621.       if (quant>15)
  622.          quant=15;
  623.       if (quant<0)
  624.          quant=0;
  625.       speex_bits_pack(bits, quant, 4);
  626.       ol_pitch_coef=GAIN_SCALING*0.066667*quant;
  627.    }
  628.    
  629.    
  630.    /*Quantize and transmit open-loop excitation gain*/
  631. #ifdef FIXED_POINT
  632.    {
  633.       int qe = scal_quant32(ol_gain, ol_gain_table, 32);
  634.       /*ol_gain = exp(qe/3.5)*SIG_SCALING;*/
  635.       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
  636.       speex_bits_pack(bits, qe, 5);
  637.    }
  638. #else
  639.    {
  640.       int qe = (int)(floor(.5+3.5*log(ol_gain*1.0/SIG_SCALING)));
  641.       if (qe<0)
  642.          qe=0;
  643.       if (qe>31)
  644.          qe=31;
  645.       ol_gain = exp(qe/3.5)*SIG_SCALING;
  646.       speex_bits_pack(bits, qe, 5);
  647.    }
  648. #endif
  651. #ifdef EPIC_48K
  652.    }
  653. #endif
  656.    /* Special case for first frame */
  657.    if (st->first)
  658.    {
  659.       for (i=0;i<st->lpcSize;i++)
  660.          st->old_qlsp[i] = st->qlsp[i];
  661.    }
  663.    /* Filter response */
  664.    ALLOC(res, st->subframeSize, spx_sig_t);
  665.    /* Target signal */
  666.    ALLOC(target, st->subframeSize, spx_sig_t);
  667.    ALLOC(syn_resp, st->subframeSize, spx_word16_t);
  668.    ALLOC(real_exc, st->subframeSize, spx_sig_t);
  669.    ALLOC(mem, st->lpcSize, spx_mem_t);
  671.    /* Loop on sub-frames */
  672.    for (sub=0;sub<st->nbSubframes;sub++)
  673.    {
  674.       int   offset;
  675.       spx_sig_t *sp, *sw, *exc;
  676.       int pitch;
  677.       int response_bound = st->subframeSize;
  678. #ifdef EPIC_48K
  679.       if (st->lbr_48k)
  680.       {
  681.          if (sub*2 < st->nbSubframes)
  682.             ol_pitch = pitch_half[0];
  683.          else
  684.             ol_pitch = pitch_half[1];
  685.       }
  686. #endif
  688.       /* Offset relative to start of frame */
  689.       offset = st->subframeSize*sub;
  690.       /* Original signal */
  691.       sp=st->frame+offset;
  692.       /* Excitation */
  693.       exc=st->exc+offset;
  694.       /* Weighted signal */
  695.       sw=st->sw+offset;
  697.       /* LSP interpolation (quantized and unquantized) */
  698.       lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, sub, st->nbSubframes);
  699.       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
  701.       /* Make sure the filters are stable */
  702.       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);
  703.       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
  705.       /* Compute interpolated LPCs (quantized and unquantized) */
  706.       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
  708.       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
  710.       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
  711.       {
  712.          spx_word32_t pi_g=LPC_SCALING;
  713.          for (i=0;i<st->lpcSize;i+=2)
  714.          {
  715.             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
  716.             pi_g = ADD32(pi_g, SUB32(st->interp_qlpc[i+1],st->interp_qlpc[i]));
  717.          }
  718.          st->pi_gain[sub] = pi_g;
  719.       }
  722.       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
  723.       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
  724.       if (st->gamma2>=0)
  725.          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
  726.       else
  727.       {
  728.          st->bw_lpc2[0]=1;
  729.          for (i=1;i<=st->lpcSize;i++)
  730.             st->bw_lpc2[i]=0;
  731.       }
  733.       for (i=0;i<st->subframeSize;i++)
  734.          real_exc[i] = exc[i];
  735.       
  736.       if (st->complexity==0)
  737.          response_bound >>= 1;
  738.       compute_impulse_response(st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, response_bound, st->lpcSize, stack);
  739.       for (i=response_bound;i<st->subframeSize;i++)
  740.          syn_resp[i]=VERY_SMALL;
  741.       
  742.       /* Reset excitation */
  743.       for (i=0;i<st->subframeSize;i++)
  744.          exc[i]=VERY_SMALL;
  746.       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
  747.       for (i=0;i<st->lpcSize;i++)
  748.          mem[i]=st->mem_sp[i];
  749. #ifdef SHORTCUTS2
  750.       iir_mem2(exc, st->interp_qlpc, exc, response_bound, st->lpcSize, mem);
  751.       for (i=0;i<st->lpcSize;i++)
  752.          mem[i]=st->mem_sw[i];
  753.       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, response_bound, st->lpcSize, mem);
  754.       for (i=response_bound;i<st->subframeSize;i++)
  755.          res[i]=0;
  756. #else
  757.       iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
  758.       for (i=0;i<st->lpcSize;i++)
  759.          mem[i]=st->mem_sw[i];
  760.       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
  761. #endif
  762.       
  763.       /* Compute weighted signal */
  764.       for (i=0;i<st->lpcSize;i++)
  765.          mem[i]=st->mem_sw[i];
  766.       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
  767.       
  768.       if (st->complexity==0)
  769.          for (i=0;i<st->lpcSize;i++)
  770.             st->mem_sw[i]=mem[i];
  771.       
  772.       /* Compute target signal */
  773.       for (i=0;i<st->subframeSize;i++)
  774.          target[i]=sw[i]-res[i];
  776.       for (i=0;i<st->subframeSize;i++)
  777.          exc[i]=0;
  779.       /* If we have a long-term predictor (otherwise, something's wrong) */
  780.       if (SUBMODE(ltp_quant))
  781.       {
  782.          int pit_min, pit_max;
  783.          /* Long-term prediction */
  784.          if (SUBMODE(lbr_pitch) != -1)
  785.          {
  786.             /* Low bit-rate pitch handling */
  787.             int margin;
  788.             margin = SUBMODE(lbr_pitch);
  789.             if (margin)
  790.             {
  791.                if (ol_pitch < st->min_pitch+margin-1)
  792.                   ol_pitch=st->min_pitch+margin-1;
  793.                if (ol_pitch > st->max_pitch-margin)
  794.                   ol_pitch=st->max_pitch-margin;
  795.                pit_min = ol_pitch-margin+1;
  796.                pit_max = ol_pitch+margin;
  797.             } else {
  798.                pit_min=pit_max=ol_pitch;
  799.             }
  800.          } else {
  801.             pit_min = st->min_pitch;
  802.             pit_max = st->max_pitch;
  803.          }
  804.          
  805.          /* Force pitch to use only the current frame if needed */
  806.          if (st->bounded_pitch && pit_max>offset)
  807.             pit_max=offset;
  809. #ifdef EPIC_48K
  810.          if (st->lbr_48k)
  811.          {
  812.             pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
  813.                                        exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
  814.                                        st->lpcSize, st->subframeSize, bits, stack, 
  815.                                        exc, syn_resp, st->complexity, ol_pitch_id, st->plc_tuning);
  816.          } else {
  817. #endif
  819.          /* Perform pitch search */
  820.          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
  821.                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
  822.                                     st->lpcSize, st->subframeSize, bits, stack, 
  823.                                     exc, syn_resp, st->complexity, 0, st->plc_tuning);
  824. #ifdef EPIC_48K
  825.          }
  826. #endif
  828.          st->pitch[sub]=pitch;
  829.       } else {
  830.          speex_error ("No pitch prediction, what's wrong");
  831.       }
  833.       /* Quantization of innovation */
  834.       {
  835.          spx_sig_t *innov;
  836.          spx_word32_t ener=0;
  837.          spx_word16_t fine_gain;
  839.          innov = st->innov+sub*st->subframeSize;
  840.          for (i=0;i<st->subframeSize;i++)
  841.             innov[i]=0;
  842.          
  843.          for (i=0;i<st->subframeSize;i++)
  844.             real_exc[i] = SUB32(real_exc[i], exc[i]);
  846.          ener = SHL32(EXTEND32(compute_rms(real_exc, st->subframeSize)),SIG_SHIFT);
  847.          
  848.          /*FIXME: Should use DIV32_16 and make sure result fits in 16 bits */
  849. #ifdef FIXED_POINT
  850.          {
  851.             spx_word32_t f = DIV32(ener,PSHR32(ol_gain,SIG_SHIFT));
  852.             if (f<=32767)
  853.                fine_gain = f;
  854.             else
  855.                fine_gain = 32767;
  856.          }
  857. #else
  858.          fine_gain = DIV32_16(ener,PSHR32(ol_gain,SIG_SHIFT));
  859. #endif
  860.          /* Calculate gain correction for the sub-frame (if any) */
  861.          if (SUBMODE(have_subframe_gain)) 
  862.          {
  863.             int qe;
  864.             if (SUBMODE(have_subframe_gain)==3)
  865.             {
  866.                qe = scal_quant(fine_gain, exc_gain_quant_scal3_bound, 8);
  867.                speex_bits_pack(bits, qe, 3);
  868.                ener=MULT16_32_Q14(exc_gain_quant_scal3[qe],ol_gain);
  869.             } else {
  870.                qe = scal_quant(fine_gain, exc_gain_quant_scal1_bound, 2);
  871.                speex_bits_pack(bits, qe, 1);
  872.                ener=MULT16_32_Q14(exc_gain_quant_scal1[qe],ol_gain);               
  873.             }
  874.          } else {
  875.             ener=ol_gain;
  876.          }
  878.          /*printf ("%f %fn", ener, ol_gain);*/
  880.          /* Normalize innovation */
  881.          signal_div(target, target, ener, st->subframeSize);
  883.          /* Quantize innovation */
  884.          if (SUBMODE(innovation_quant))
  885.          {
  886.             /* Codebook search */
  887.             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
  888.                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
  889.                                       innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
  890.             
  891.             /* De-normalize innovation and update excitation */
  892.             signal_mul(innov, innov, ener, st->subframeSize);
  894.             for (i=0;i<st->subframeSize;i++)
  895.                exc[i] = ADD32(exc[i],innov[i]);
  896.          } else {
  897.             speex_error("No fixed codebook");
  898.          }
  900.          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
  901.          if (SUBMODE(double_codebook)) {
  902.             char *tmp_stack=stack;
  903.             VARDECL(spx_sig_t *innov2);
  904.             ALLOC(innov2, st->subframeSize, spx_sig_t);
  905.             for (i=0;i<st->subframeSize;i++)
  906.                innov2[i]=0;
  907.             for (i=0;i<st->subframeSize;i++)
  908.                target[i]*=2.2;
  909.             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
  910.                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
  911.                                       innov2, syn_resp, bits, stack, st->complexity, 0);
  912.             signal_mul(innov2, innov2, (spx_word32_t) (ener*(1/2.2)), st->subframeSize);
  913.             for (i=0;i<st->subframeSize;i++)
  914.                exc[i] = ADD32(exc[i],innov2[i]);
  915.             stack = tmp_stack;
  916.          }
  918.       }
  920.       /* Final signal synthesis from excitation */
  921.       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
  923.       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
  924.       if (st->complexity!=0)
  925.          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
  926.       
  927.    }
  929.    /* Store the LSPs for interpolation in the next frame */
  930.    if (st->submodeID>=1)
  931.    {
  932.       for (i=0;i<st->lpcSize;i++)
  933.          st->old_lsp[i] = st->lsp[i];
  934.       for (i=0;i<st->lpcSize;i++)
  935.          st->old_qlsp[i] = st->qlsp[i];
  936.    }
  938.    if (st->submodeID==1)
  939.    {
  940.       if (st->dtx_count)
  941.          speex_bits_pack(bits, 15, 4);
  942.       else
  943.          speex_bits_pack(bits, 0, 4);
  944.    }
  946.    /* The next frame will not be the first (Duh!) */
  947.    st->first = 0;
  949. #ifdef RESYNTH
  950.    /* Replace input by synthesized speech */
  951.    for (i=0;i<st->frameSize;i++)
  952.    {
  953.       spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
  954.       if (sig>32767)
  955.          sig = 32767;
  956.       if (sig<-32767)
  957.          sig = -32767;
  958.      in[i]=sig;
  959.    }
  960. #endif
  962.    if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
  963.       st->bounded_pitch = 1;
  964.    else
  965.       st->bounded_pitch = 0;
  967.    return 1;
  968. }
  971. void *nb_decoder_init(const SpeexMode *m)
  972. {
  973.    DecState *st;
  974.    const SpeexNBMode *mode;
  975.    int i;
  977.    mode=(const SpeexNBMode*)m->mode;
  978.    st = (DecState *)speex_alloc(sizeof(DecState));
  979.    if (!st)
  980.       return NULL;
  981. #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
  982.    st->stack = NULL;
  983. #else
  984.    st->stack = (char*)speex_alloc_scratch(NB_DEC_STACK);
  985. #endif
  987.    st->mode=m;
  990.    st->encode_submode = 1;
  991. #ifdef EPIC_48K
  992.    st->lbr_48k=mode->lbr48k;
  993. #endif
  995.    st->first=1;
  996.    /* Codec parameters, should eventually have several "modes"*/
  997.    st->frameSize = mode->frameSize;
  998.    st->nbSubframes=mode->frameSize/mode->subframeSize;
  999.    st->subframeSize=mode->subframeSize;
  1000.    st->lpcSize = mode->lpcSize;
  1001.    st->min_pitch=mode->pitchStart;
  1002.    st->max_pitch=mode->pitchEnd;
  1004.    st->submodes=mode->submodes;
  1005.    st->submodeID=mode->defaultSubmode;
  1007.    st->lpc_enh_enabled=0;
  1010.    st->inBuf = speex_alloc((st->frameSize)*sizeof(spx_sig_t));
  1011.    st->frame = st->inBuf;
  1012.    st->excBuf = speex_alloc((st->frameSize + st->max_pitch + 1)*sizeof(spx_sig_t));
  1013.    st->exc = st->excBuf + st->max_pitch + 1;
  1014.    for (i=0;i<st->frameSize;i++)
  1015.       st->inBuf[i]=0;
  1016.    for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
  1017.       st->excBuf[i]=0;
  1018.    st->innov = speex_alloc((st->frameSize)*sizeof(spx_sig_t));
  1020.    st->interp_qlpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
  1021.    st->qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
  1022.    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
  1023.    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
  1024.    st->mem_sp = speex_alloc((5*st->lpcSize)*sizeof(spx_mem_t));
  1025.    st->comb_mem = speex_alloc(sizeof(CombFilterMem));
  1026.    comb_filter_mem_init (st->comb_mem);
  1028.    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
  1029.    st->last_pitch = 40;
  1030.    st->count_lost=0;
  1031.    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
  1032.    st->pitch_gain_buf_idx = 0;
  1033.    st->seed = 1000;
  1034.    
  1035.    st->sampling_rate=8000;
  1036.    st->last_ol_gain = 0;
  1038.    st->user_callback.func = &speex_default_user_handler;
  1039.    st->user_callback.data = NULL;
  1040.    for (i=0;i<16;i++)
  1041.       st->speex_callbacks[i].func = NULL;
  1043.    st->voc_m1=st->voc_m2=st->voc_mean=0;
  1044.    st->voc_offset=0;
  1045.    st->dtx_enabled=0;
  1046. #ifdef ENABLE_VALGRIND
  1047.    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
  1048. #endif
  1049.    return st;
  1050. }
  1052. void nb_decoder_destroy(void *state)
  1053. {
  1054.    DecState *st;
  1055.    st=(DecState*)state;
  1056.    
  1057. #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
  1058.    speex_free_scratch(st->stack);
  1059. #endif
  1061.    speex_free (st->inBuf);
  1062.    speex_free (st->excBuf);
  1063.    speex_free (st->innov);
  1064.    speex_free (st->interp_qlpc);
  1065.    speex_free (st->qlsp);
  1066.    speex_free (st->old_qlsp);
  1067.    speex_free (st->interp_qlsp);
  1068.    speex_free (st->mem_sp);
  1069.    speex_free (st->comb_mem);
  1070.    speex_free (st->pi_gain);
  1072.    speex_free(state);
  1073. }
  1075. #define median3(a, b, c) ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
  1077. #ifdef FIXED_POINT
  1078. const spx_word16_t attenuation[10] = {32767, 31483, 27923, 22861, 17278, 12055, 7764, 4616, 2533, 1283};
  1079. #else
  1080. const spx_word16_t attenuation[10] = {1., 0.961, 0.852, 0.698, 0.527, 0.368, 0.237, 0.141, 0.077, 0.039};
  1082. #endif
  1084. static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
  1085. {
  1086.    int i, sub;
  1087.    VARDECL(spx_coef_t *awk1);
  1088.    VARDECL(spx_coef_t *awk2);
  1089.    VARDECL(spx_coef_t *awk3);
  1090.    spx_word16_t pitch_gain;
  1091.    spx_word16_t fact;
  1092.    spx_word16_t gain_med;
  1093.    spx_word16_t innov_gain;
  1094.    
  1095.    if (st->count_lost<10)
  1096.       fact = attenuation[st->count_lost];
  1097.    else
  1098.       fact = 0;
  1100.    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
  1101.    if (gain_med < st->last_pitch_gain)
  1102.       st->last_pitch_gain = gain_med;
  1103.    
  1104. #ifdef FIXED_POINT
  1105.    pitch_gain = st->last_pitch_gain;
  1106.    if (pitch_gain>62)
  1107.       pitch_gain = 62;
  1108.    pitch_gain = SHL(pitch_gain, 9);
  1109. #else   
  1110.    pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
  1111.    if (pitch_gain>.95)
  1112.       pitch_gain=.95;
  1113. #endif
  1115.    pitch_gain = MULT16_16_Q15(fact,pitch_gain) + VERY_SMALL;
  1117.    /* Shift all buffers by one frame */
  1118.    /*speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(spx_sig_t));*/
  1119.    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch + 1)*sizeof(spx_sig_t));
  1121.    ALLOC(awk1, (st->lpcSize+1), spx_coef_t);
  1122.    ALLOC(awk2, (st->lpcSize+1), spx_coef_t);
  1123.    ALLOC(awk3, (st->lpcSize+1), spx_coef_t);
  1125.    for (sub=0;sub<st->nbSubframes;sub++)
  1126.    {
  1127.       int offset;
  1128.       spx_sig_t *sp, *exc;
  1129.       /* Offset relative to start of frame */
  1130.       offset = st->subframeSize*sub;
  1131.       /* Original signal */
  1132.       sp=st->frame+offset;
  1133.       /* Excitation */
  1134.       exc=st->exc+offset;
  1135.       /* Excitation after post-filter*/
  1137.       /* Calculate perceptually enhanced LPC filter */
  1138.       if (st->lpc_enh_enabled)
  1139.       {
  1140.          spx_word16_t k1,k2,k3;
  1141.          if (st->submodes[st->submodeID] != NULL)
  1142.          {
  1143.             k1=SUBMODE(lpc_enh_k1);
  1144.             k2=SUBMODE(lpc_enh_k2);
  1145.             k3=SUBMODE(lpc_enh_k3);
  1146.          } else {
  1147.             k1=k2=.7*GAMMA_SCALING;
  1148.             k3=.0;
  1149.          }
  1150.          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
  1151.          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
  1152.          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
  1153.       }
  1154.         
  1155.       /* Make up a plausible excitation */
  1156.       /* FIXME: THIS CAN BE IMPROVED */
  1157.       /*if (pitch_gain>.95)
  1158.         pitch_gain=.95;*/
  1159.       innov_gain = compute_rms(st->innov, st->frameSize);
  1160.       for (i=0;i<st->subframeSize;i++)
  1161.       {
  1162.          exc[i]= MULT16_32_Q15(pitch_gain, (exc[i-st->last_pitch]+VERY_SMALL)) + 
  1163.                MULT16_32_Q15(fact, MULT16_32_Q15(sqrt(SHL(Q15ONE,15)-SHL(pitch_gain,15)),speex_rand(innov_gain, &st->seed)));
  1164.       }
  1165.       
  1166.       for (i=0;i<st->subframeSize;i++)
  1167.          sp[i]=exc[i];
  1168.       
  1169.       /* Signal synthesis */
  1170.       if (st->lpc_enh_enabled)
  1171.       {
  1172.          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
  1173.                      st->mem_sp+st->lpcSize);
  1174.          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
  1175.                      st->mem_sp);
  1176.       } else {
  1177.          for (i=0;i<st->lpcSize;i++)
  1178.             st->mem_sp[st->lpcSize+i] = 0;
  1179.          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
  1180.                      st->mem_sp);
  1181.       }      
  1182.    }
  1184.    for (i=0;i<st->frameSize;i++)
  1185.    {
  1186.       spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
  1187.       if (sig>32767)
  1188.          sig = 32767;
  1189.       if (sig<-32767)
  1190.          sig = -32767;
  1191.      out[i]=sig;
  1192.    }
  1193.    
  1194.    st->first = 0;
  1195.    st->count_lost++;
  1196.    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = PSHR(pitch_gain,9);
  1197.    if (st->pitch_gain_buf_idx > 2) /* rollover */
  1198.       st->pitch_gain_buf_idx = 0;
  1199. }
  1201. int nb_decode(void *state, SpeexBits *bits, void *vout)
  1202. {
  1203.    DecState *st;
  1204.    int i, sub;
  1205.    int pitch;
  1206.    spx_word16_t pitch_gain[3];
  1207.    spx_word32_t ol_gain=0;
  1208.    int ol_pitch=0;
  1209.    spx_word16_t ol_pitch_coef=0;
  1210.    int best_pitch=40;
  1211.    spx_word16_t best_pitch_gain=0;
  1212.    int wideband;
  1213.    int m;
  1214.    char *stack;
  1215.    VARDECL(spx_coef_t *awk1);
  1216.    VARDECL(spx_coef_t *awk2);
  1217.    VARDECL(spx_coef_t *awk3);
  1218.    spx_word16_t pitch_average=0;
  1219. #ifdef EPIC_48K
  1220.    int pitch_half[2];
  1221.    int ol_pitch_id=0;
  1222. #endif
  1223.    spx_word16_t *out = vout;
  1225.    st=(DecState*)state;
  1226.    stack=st->stack;
  1228.    if (st->encode_submode)
  1229.    {
  1230. #ifdef EPIC_48K
  1231.    if (!st->lbr_48k) {
  1232. #endif
  1234.    /* Check if we're in DTX mode*/
  1235.    if (!bits && st->dtx_enabled)
  1236.    {
  1237.       st->submodeID=0;
  1238.    } else 
  1239.    {
  1240.       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
  1241.       if (!bits)
  1242.       {
  1243.          nb_decode_lost(st, out, stack);
  1244.          return 0;
  1245.       }
  1247.       /* Search for next narrowband block (handle requests, skip wideband blocks) */
  1248.       do {
  1249.          if (speex_bits_remaining(bits)<5)
  1250.             return -1;
  1251.          wideband = speex_bits_unpack_unsigned(bits, 1);
  1252.          if (wideband) /* Skip wideband block (for compatibility) */
  1253.          {
  1254.             int submode;
  1255.             int advance;
  1256.             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
  1257.             speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
  1258.             if (advance < 0)
  1259.             {
  1260.                speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
  1261.                return -2;
  1262.             } 
  1263.             advance -= (SB_SUBMODE_BITS+1);
  1264.             speex_bits_advance(bits, advance);
  1265.             
  1266.             if (speex_bits_remaining(bits)<5)
  1267.                return -1;
  1268.             wideband = speex_bits_unpack_unsigned(bits, 1);
  1269.             if (wideband)
  1270.             {
  1271.                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
  1272.                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
  1273.                if (advance < 0)
  1274.                {
  1275.                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
  1276.                   return -2;
  1277.                } 
  1278.                advance -= (SB_SUBMODE_BITS+1);
  1279.                speex_bits_advance(bits, advance);
  1280.                wideband = speex_bits_unpack_unsigned(bits, 1);
  1281.                if (wideband)
  1282.                {
  1283.                   speex_warning ("More than two wideband layers found: corrupted stream?");
  1284.                   return -2;
  1285.                }
  1287.             }
  1288.          }
  1289.          if (speex_bits_remaining(bits)<4)
  1290.             return -1;
  1291.          /* FIXME: Check for overflow */
  1292.          m = speex_bits_unpack_unsigned(bits, 4);
  1293.          if (m==15) /* We found a terminator */
  1294.          {
  1295.             return -1;
  1296.          } else if (m==14) /* Speex in-band request */
  1297.          {
  1298.             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
  1299.             if (ret)
  1300.                return ret;
  1301.          } else if (m==13) /* User in-band request */
  1302.          {
  1303.             int ret = st->user_callback.func(bits, state, st->user_callback.data);
  1304.             if (ret)
  1305.                return ret;
  1306.          } else if (m>8) /* Invalid mode */
  1307.          {
  1308.             speex_warning("Invalid mode encountered: corrupted stream?");
  1309.             return -2;
  1310.          }
  1311.       
  1312.       } while (m>8);
  1314.       /* Get the sub-mode that was used */
  1315.       st->submodeID = m;
  1317.    }
  1318. #ifdef EPIC_48K
  1319.    }
  1320. #endif
  1321.    }
  1323.    /* Shift all buffers by one frame */
  1324.    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch + 1)*sizeof(spx_sig_t));
  1326.    /* If null mode (no transmission), just set a couple things to zero*/
  1327.    if (st->submodes[st->submodeID] == NULL)
  1328.    {
  1329.       VARDECL(spx_coef_t *lpc);
  1330.       ALLOC(lpc, st->lpcSize, spx_coef_t);
  1331.       bw_lpc(GAMMA_SCALING*.93, st->interp_qlpc, lpc, st->lpcSize);
  1332.       {
  1333.          float innov_gain=0;
  1334.          float pgain=GAIN_SCALING_1*st->last_pitch_gain;
  1335.          if (pgain>.6)
  1336.             pgain=.6;
  1337.  innov_gain = compute_rms(st->innov, st->frameSize);
  1338.          for (i=0;i<st->frameSize;i++)
  1339.             st->exc[i]=VERY_SMALL;
  1340.          speex_rand_vec(innov_gain, st->exc, st->frameSize);
  1341.       }
  1344.       st->first=1;
  1346.       /* Final signal synthesis from excitation */
  1347.       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
  1349.       for (i=0;i<st->frameSize;i++)
  1350.       {
  1351.          spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
  1352.          if (sig>32767)
  1353.             sig = 32767;
  1354.          if (sig<-32767)
  1355.             sig = -32767;
  1356.          out[i]=sig;
  1357.       }
  1359.       st->count_lost=0;
  1360.       return 0;
  1361.    }
  1363.    /* Unquantize LSPs */
  1364.    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
  1366.    /*Damp memory if a frame was lost and the LSP changed too much*/
  1367.    if (st->count_lost)
  1368.    {
  1369.       spx_word16_t fact;
  1370.       spx_word32_t lsp_dist=0;
  1371.       for (i=0;i<st->lpcSize;i++)
  1372.          lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - st->qlsp[i])));
  1373. #ifdef FIXED_POINT
  1374.       fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));      
  1375. #else
  1376.       fact = .6*exp(-.2*lsp_dist);
  1377. #endif
  1378.       for (i=0;i<2*st->lpcSize;i++)
  1379.          st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
  1380.    }
  1383.    /* Handle first frame and lost-packet case */
  1384.    if (st->first || st->count_lost)
  1385.    {
  1386.       for (i=0;i<st->lpcSize;i++)
  1387.          st->old_qlsp[i] = st->qlsp[i];
  1388.    }
  1390. #ifdef EPIC_48K
  1391.    if (st->lbr_48k) {
  1392.       pitch_half[0] = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
  1393.       pitch_half[1] = pitch_half[0]+speex_bits_unpack_unsigned(bits, 2)-1;
  1395.       ol_pitch_id = speex_bits_unpack_unsigned(bits, 3);
  1396.       ol_pitch_coef=GAIN_SCALING*0.13514*ol_pitch_id;
  1398.       {
  1399.          int qe;
  1400.          qe = speex_bits_unpack_unsigned(bits, 4);
  1401.          ol_gain = SIG_SCALING*exp((qe+2)/2.1),SIG_SHIFT;
  1402.       }
  1404.    } else {
  1405. #endif
  1407.    /* Get open-loop pitch estimation for low bit-rate pitch coding */
  1408.    if (SUBMODE(lbr_pitch)!=-1)
  1409.    {
  1410.       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
  1411.    } 
  1412.    
  1413.    if (SUBMODE(forced_pitch_gain))
  1414.    {
  1415.       int quant;
  1416.       quant = speex_bits_unpack_unsigned(bits, 4);
  1417.       ol_pitch_coef=GAIN_SCALING*0.066667*quant;
  1418.    }
  1419.    
  1420.    /* Get global excitation gain */
  1421.    {
  1422.       int qe;
  1423.       qe = speex_bits_unpack_unsigned(bits, 5);
  1424. #ifdef FIXED_POINT
  1425.       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
  1426. #else
  1427.       ol_gain = SIG_SCALING*exp(qe/3.5);
  1428. #endif
  1429.    }
  1430. #ifdef EPIC_48K
  1431.    }
  1432. #endif
  1434.    ALLOC(awk1, st->lpcSize+1, spx_coef_t);
  1435.    ALLOC(awk2, st->lpcSize+1, spx_coef_t);
  1436.    ALLOC(awk3, st->lpcSize+1, spx_coef_t);
  1438.    if (st->submodeID==1)
  1439.    {
  1440.       int extra;
  1441.       extra = speex_bits_unpack_unsigned(bits, 4);
  1443.       if (extra==15)
  1444.          st->dtx_enabled=1;
  1445.       else
  1446.          st->dtx_enabled=0;
  1447.    }
  1448.    if (st->submodeID>1)
  1449.       st->dtx_enabled=0;
  1451.    /*Loop on subframes */
  1452.    for (sub=0;sub<st->nbSubframes;sub++)
  1453.    {
  1454.       int offset;
  1455.       spx_sig_t *sp, *exc;
  1456.       spx_word16_t tmp;
  1458. #ifdef EPIC_48K
  1459.       if (st->lbr_48k)
  1460.       {
  1461.          if (sub*2 < st->nbSubframes)
  1462.             ol_pitch = pitch_half[0];
  1463.          else
  1464.             ol_pitch = pitch_half[1];
  1465.       }
  1466. #endif
  1468.       /* Offset relative to start of frame */
  1469.       offset = st->subframeSize*sub;
  1470.       /* Original signal */
  1471.       sp=st->frame+offset;
  1472.       /* Excitation */
  1473.       exc=st->exc+offset;
  1474.       /* Excitation after post-filter*/
  1476.       /* LSP interpolation (quantized and unquantized) */
  1477.       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
  1479.       /* Make sure the LSP's are stable */
  1480.       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
  1483.       /* Compute interpolated LPCs (unquantized) */
  1484.       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
  1486.       /* Compute enhanced synthesis filter */
  1487.       if (st->lpc_enh_enabled)
  1488.       {
  1489.          bw_lpc(SUBMODE(lpc_enh_k1), st->interp_qlpc, awk1, st->lpcSize);
  1490.          bw_lpc(SUBMODE(lpc_enh_k2), st->interp_qlpc, awk2, st->lpcSize);
  1491.          bw_lpc(SUBMODE(lpc_enh_k3), st->interp_qlpc, awk3, st->lpcSize);
  1492.       }
  1494.       /* Compute analysis filter at w=pi */
  1495.       {
  1496.          spx_word32_t pi_g=LPC_SCALING;
  1497.          for (i=0;i<st->lpcSize;i+=2)
  1498.          {
  1499.             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
  1500.             pi_g = ADD32(pi_g, SUB32(st->interp_qlpc[i+1],st->interp_qlpc[i]));
  1501.          }
  1502.          st->pi_gain[sub] = pi_g;
  1503.       }
  1505.       /* Reset excitation */
  1506.       for (i=0;i<st->subframeSize;i++)
  1507.          exc[i]=0;
  1509.       /*Adaptive codebook contribution*/
  1510.       if (SUBMODE(ltp_unquant))
  1511.       {
  1512.          int pit_min, pit_max;
  1513.          /* Handle pitch constraints if any */
  1514.          if (SUBMODE(lbr_pitch) != -1)
  1515.          {
  1516.             int margin;
  1517.             margin = SUBMODE(lbr_pitch);
  1518.             if (margin)
  1519.             {
  1520. /* GT - need optimization?
  1521.                if (ol_pitch < st->min_pitch+margin-1)
  1522.                   ol_pitch=st->min_pitch+margin-1;
  1523.                if (ol_pitch > st->max_pitch-margin)
  1524.                   ol_pitch=st->max_pitch-margin;
  1525.                pit_min = ol_pitch-margin+1;
  1526.                pit_max = ol_pitch+margin;
  1527. */
  1528.                pit_min = ol_pitch-margin+1;
  1529.                if (pit_min < st->min_pitch)
  1530.   pit_min = st->min_pitch;
  1531.                pit_max = ol_pitch+margin;
  1532.                if (pit_max > st->max_pitch)
  1533.   pit_max = st->max_pitch;
  1534.             } else {
  1535.                pit_min = pit_max = ol_pitch;
  1536.             }
  1537.          } else {
  1538.             pit_min = st->min_pitch;
  1539.             pit_max = st->max_pitch;
  1540.          }
  1543. #ifdef EPIC_48K
  1544.          if (st->lbr_48k)
  1545.          {
  1546.              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
  1547.                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
  1548.                                   st->count_lost, offset, st->last_pitch_gain, ol_pitch_id);
  1549.          } else {
  1550. #endif
  1552.              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
  1553.                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
  1554.                                   st->count_lost, offset, st->last_pitch_gain, 0);
  1556. #ifdef EPIC_48K
  1557.          }
  1558. #endif
  1560.          
  1561.          /* If we had lost frames, check energy of last received frame */
  1562.          if (st->count_lost && ol_gain < st->last_ol_gain)
  1563.          {
  1564.             /*float fact = (float)ol_gain/(st->last_ol_gain+1);
  1565.             for (i=0;i<st->subframeSize;i++)
  1566.             exc[i]*=fact;*/
  1567.             spx_word16_t fact = DIV32_16(SHL32(EXTEND32(ol_gain),15),st->last_ol_gain+1);
  1568.             for (i=0;i<st->subframeSize;i++)
  1569.                exc[i] = MULT16_32_Q15(fact, exc[i]);
  1570.          }
  1572.          tmp = gain_3tap_to_1tap(pitch_gain);
  1574.          pitch_average += tmp;
  1575.          if (tmp>best_pitch_gain)
  1576.          {
  1577.             best_pitch = pitch;
  1578.     best_pitch_gain = tmp;
  1579.          }
  1580.       } else {
  1581.          speex_error("No pitch prediction, what's wrong");
  1582.       }
  1583.       
  1584.       /* Unquantize the innovation */
  1585.       {
  1586.          int q_energy;
  1587.          spx_word32_t ener;
  1588.          spx_sig_t *innov;
  1589.          
  1590.          innov = st->innov+sub*st->subframeSize;
  1591.          for (i=0;i<st->subframeSize;i++)
  1592.             innov[i]=0;
  1594.          /* Decode sub-frame gain correction */
  1595.          if (SUBMODE(have_subframe_gain)==3)
  1596.          {
  1597.             q_energy = speex_bits_unpack_unsigned(bits, 3);
  1598.             ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
  1599.          } else if (SUBMODE(have_subframe_gain)==1)
  1600.          {
  1601.             q_energy = speex_bits_unpack_unsigned(bits, 1);
  1602.             ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
  1603.          } else {
  1604.             ener = ol_gain;
  1605.          }
  1606.                   
  1607.          if (SUBMODE(innovation_unquant))
  1608.          {
  1609.             /*Fixed codebook contribution*/
  1610.             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
  1611.          } else {
  1612.             speex_error("No fixed codebook");
  1613.          }
  1615.          /* De-normalize innovation and update excitation */
  1616. #ifdef FIXED_POINT
  1617.          signal_mul(innov, innov, ener, st->subframeSize);
  1618. #else
  1619.          signal_mul(innov, innov, ener, st->subframeSize);
  1620. #endif
  1621.          /*Vocoder mode*/
  1622.          if (st->submodeID==1) 
  1623.          {
  1624.             float g=ol_pitch_coef*GAIN_SCALING_1;
  1626.             
  1627.             for (i=0;i<st->subframeSize;i++)
  1628.                exc[i]=0;
  1629.             while (st->voc_offset<st->subframeSize)
  1630.             {
  1631.                if (st->voc_offset>=0)
  1632.                   exc[st->voc_offset]=SIG_SCALING*sqrt(1.0*ol_pitch);
  1633.                st->voc_offset+=ol_pitch;
  1634.             }
  1635.             st->voc_offset -= st->subframeSize;
  1637.             g=.5+2*(g-.6);
  1638.             if (g<0)
  1639.                g=0;
  1640.             if (g>1)
  1641.                g=1;
  1642.             for (i=0;i<st->subframeSize;i++)
  1643.             {
  1644.                float exci=exc[i];
  1645.                exc[i]=.8*g*exc[i]*ol_gain/SIG_SCALING + .6*g*st->voc_m1*ol_gain/SIG_SCALING + .5*g*innov[i] - .5*g*st->voc_m2 + (1-g)*innov[i];
  1646.                st->voc_m1 = exci;
  1647.                st->voc_m2=innov[i];
  1648.                st->voc_mean = .95*st->voc_mean + .05*exc[i];
  1649.                exc[i]-=st->voc_mean;
  1650.             }
  1651.          } else {
  1652.             for (i=0;i<st->subframeSize;i++)
  1653.                exc[i]=ADD32(exc[i],innov[i]);
  1654.             /*print_vec(exc, 40, "innov");*/
  1655.          }
  1656.          /* Decode second codebook (only for some modes) */
  1657.          if (SUBMODE(double_codebook))
  1658.          {
  1659.             char *tmp_stack=stack;
  1660.             VARDECL(spx_sig_t *innov2);
  1661.             ALLOC(innov2, st->subframeSize, spx_sig_t);
  1662.             for (i=0;i<st->subframeSize;i++)
  1663.                innov2[i]=0;
  1664.             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack);
  1665.             signal_mul(innov2, innov2, (spx_word32_t) (ener*(1/2.2)), st->subframeSize);
  1666.             for (i=0;i<st->subframeSize;i++)
  1667.                exc[i] = ADD32(exc[i],innov2[i]);
  1668.             stack = tmp_stack;
  1669.          }
  1671.       }
  1673.       for (i=0;i<st->subframeSize;i++)
  1674.          sp[i]=exc[i];
  1676.       /* Signal synthesis */
  1677.       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0)
  1678.          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
  1679.                               pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
  1681.       if (st->lpc_enh_enabled)
  1682.       {
  1683.          /* Use enhanced LPC filter */
  1684.          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
  1685.                      st->mem_sp+st->lpcSize);
  1686.          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
  1687.                      st->mem_sp);
  1688.       } else {
  1689.          /* Use regular filter */
  1690.          for (i=0;i<st->lpcSize;i++)
  1691.             st->mem_sp[st->lpcSize+i] = 0;
  1692.          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
  1693.                      st->mem_sp);
  1694.       }
  1695.    }
  1696.    
  1697.    /*Copy output signal*/   
  1698.    for (i=0;i<st->frameSize;i++)
  1699.    {
  1700.       spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
  1701.       if (sig>32767)
  1702.          sig = 32767;
  1703.       if (sig<-32767)
  1704.          sig = -32767;
  1705.      out[i]=sig;
  1706.    }
  1708.    /*for (i=0;i<st->frameSize;i++)
  1709.      printf ("%dn", (int)st->frame[i]);*/
  1711.    /* Store the LSPs for interpolation in the next frame */
  1712.    for (i=0;i<st->lpcSize;i++)
  1713.       st->old_qlsp[i] = st->qlsp[i];
  1715.    /* The next frame will not be the first (Duh!) */
  1716.    st->first = 0;
  1717.    st->count_lost=0;
  1718.    st->last_pitch = best_pitch;
  1719. #ifdef FIXED_POINT
  1720.    st->last_pitch_gain = PSHR16(pitch_average,2);
  1721. #else
  1722.    st->last_pitch_gain = .25*pitch_average;   
  1723. #endif
  1724.    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
  1725.    if (st->pitch_gain_buf_idx > 2) /* rollover */
  1726.       st->pitch_gain_buf_idx = 0;
  1728.    st->last_ol_gain = ol_gain;
  1730.    return 0;
  1731. }
  1733. int nb_encoder_ctl(void *state, int request, void *ptr)
  1734. {
  1735.    EncState *st;
  1736.    st=(EncState*)state;     
  1737.    switch(request)
  1738.    {
  1739.    case SPEEX_GET_FRAME_SIZE:
  1740.       (*(int*)ptr) = st->frameSize;
  1741.       break;
  1742.    case SPEEX_SET_LOW_MODE:
  1743.    case SPEEX_SET_MODE:
  1744.       st->submodeSelect = st->submodeID = (*(int*)ptr);
  1745.       break;
  1746.    case SPEEX_GET_LOW_MODE:
  1747.    case SPEEX_GET_MODE:
  1748.       (*(int*)ptr) = st->submodeID;
  1749.       break;
  1750.    case SPEEX_SET_VBR:
  1751.       st->vbr_enabled = (*(int*)ptr);
  1752.       break;
  1753.    case SPEEX_GET_VBR:
  1754.       (*(int*)ptr) = st->vbr_enabled;
  1755.       break;
  1756.    case SPEEX_SET_VAD:
  1757.       st->vad_enabled = (*(int*)ptr);
  1758.       break;
  1759.    case SPEEX_GET_VAD:
  1760.       (*(int*)ptr) = st->vad_enabled;
  1761.       break;
  1762.    case SPEEX_SET_DTX:
  1763.       st->dtx_enabled = (*(int*)ptr);
  1764.       break;
  1765.    case SPEEX_GET_DTX:
  1766.       (*(int*)ptr) = st->dtx_enabled;
  1767.       break;
  1768.    case SPEEX_SET_ABR:
  1769.       st->abr_enabled = (*(int*)ptr);
  1770.       st->vbr_enabled = 1;
  1771.       {
  1772.          int i=10, rate, target;
  1773.          float vbr_qual;
  1774.          target = (*(int*)ptr);
  1775.          while (i>=0)
  1776.          {
  1777.             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
  1778.             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
  1779.             if (rate <= target)
  1780.                break;
  1781.             i--;
  1782.          }
  1783.          vbr_qual=i;
  1784.          if (vbr_qual<0)
  1785.             vbr_qual=0;
  1786.          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
  1787.          st->abr_count=0;
  1788.          st->abr_drift=0;
  1789.          st->abr_drift2=0;
  1790.       }
  1791.       
  1792.       break;
  1793.    case SPEEX_GET_ABR:
  1794.       (*(int*)ptr) = st->abr_enabled;
  1795.       break;
  1796.    case SPEEX_SET_VBR_QUALITY:
  1797.       st->vbr_quality = (*(float*)ptr);
  1798.       break;
  1799.    case SPEEX_GET_VBR_QUALITY:
  1800.       (*(float*)ptr) = st->vbr_quality;
  1801.       break;
  1802.    case SPEEX_SET_QUALITY:
  1803.       {
  1804.          int quality = (*(int*)ptr);
  1805.          if (quality < 0)
  1806.             quality = 0;
  1807.          if (quality > 10)
  1808.             quality = 10;
  1809.          st->submodeSelect = st->submodeID = ((const SpeexNBMode*)(st->mode->mode))->quality_map[quality];
  1810.       }
  1811.       break;
  1812.    case SPEEX_SET_COMPLEXITY:
  1813.       st->complexity = (*(int*)ptr);
  1814.       if (st->complexity<0)
  1815.          st->complexity=0;
  1816.       break;
  1817.    case SPEEX_GET_COMPLEXITY:
  1818.       (*(int*)ptr) = st->complexity;
  1819.       break;
  1820.    case SPEEX_SET_BITRATE:
  1821.       {
  1822.          int i=10, rate, target;
  1823.          target = (*(int*)ptr);
  1824.          while (i>=0)
  1825.          {
  1826.             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
  1827.             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
  1828.             if (rate <= target)
  1829.                break;
  1830.             i--;
  1831.          }
  1832.       }
  1833.       break;
  1834.    case SPEEX_GET_BITRATE:
  1835.       if (st->submodes[st->submodeID])
  1836.          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
  1837.       else
  1838.          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
  1839.       break;
  1840.    case SPEEX_SET_SAMPLING_RATE:
  1841.       st->sampling_rate = (*(int*)ptr);
  1842.       break;
  1843.    case SPEEX_GET_SAMPLING_RATE:
  1844.       (*(int*)ptr)=st->sampling_rate;
  1845.       break;
  1846.    case SPEEX_RESET_STATE:
  1847.       {
  1848.          int i;
  1849.          st->bounded_pitch = 1;
  1850.          st->first = 1;
  1851.          for (i=0;i<st->lpcSize;i++)
  1852.             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
  1853.          for (i=0;i<st->lpcSize;i++)
  1854.             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
  1855.          for (i=0;i<st->frameSize+st->max_pitch+1;i++)
  1856.             st->excBuf[i]=st->swBuf[i]=0;
  1857.          for (i=0;i<st->windowSize;i++)
  1858.             st->inBuf[i]=0;
  1859.       }
  1860.       break;
  1861.    case SPEEX_SET_SUBMODE_ENCODING:
  1862.       st->encode_submode = (*(int*)ptr);
  1863.       break;
  1864.    case SPEEX_GET_SUBMODE_ENCODING:
  1865.       (*(int*)ptr) = st->encode_submode;
  1866.       break;
  1867.    case SPEEX_GET_LOOKAHEAD:
  1868.       (*(int*)ptr)=(st->windowSize-st->frameSize);
  1869.       break;
  1870.    case SPEEX_SET_PLC_TUNING:
  1871.       st->plc_tuning = (*(int*)ptr);
  1872.       if (st->plc_tuning>100)
  1873.          st->plc_tuning=100;
  1874.       break;
  1875.    case SPEEX_GET_PLC_TUNING:
  1876.       (*(int*)ptr)=(st->plc_tuning);
  1877.       break;
  1878.    case SPEEX_GET_PI_GAIN:
  1879.       {
  1880.          int i;
  1881.          spx_word32_t *g = (spx_word32_t*)ptr;
  1882.          for (i=0;i<st->nbSubframes;i++)
  1883.             g[i]=st->pi_gain[i];
  1884.       }
  1885.       break;
  1886.    case SPEEX_GET_EXC:
  1887.       {
  1888.          int i;
  1889.          spx_sig_t *e = (spx_sig_t*)ptr;
  1890.          for (i=0;i<st->frameSize;i++)
  1891.             e[i]=st->exc[i];
  1892.       }
  1893.       break;
  1894.    case SPEEX_GET_INNOV:
  1895.       {
  1896.          int i;
  1897.          spx_sig_t *e = (spx_sig_t*)ptr;
  1898.          for (i=0;i<st->frameSize;i++)
  1899.             e[i]=st->innov[i];
  1900.       }
  1901.       break;
  1902.    case SPEEX_GET_RELATIVE_QUALITY:
  1903.       (*(float*)ptr)=st->relative_quality;
  1904.       break;
  1905.    default:
  1906.       speex_warning_int("Unknown nb_ctl request: ", request);
  1907.       return -1;
  1908.    }
  1909.    return 0;
  1910. }
  1912. int nb_decoder_ctl(void *state, int request, void *ptr)
  1913. {
  1914.    DecState *st;
  1915.    st=(DecState*)state;
  1916.    switch(request)
  1917.    {
  1918.    case SPEEX_SET_LOW_MODE:
  1919.    case SPEEX_SET_MODE:
  1920.       st->submodeID = (*(int*)ptr);
  1921.       break;
  1922.    case SPEEX_GET_LOW_MODE:
  1923.    case SPEEX_GET_MODE:
  1924.       (*(int*)ptr) = st->submodeID;
  1925.       break;
  1926.    case SPEEX_SET_ENH:
  1927.       st->lpc_enh_enabled = *((int*)ptr);
  1928.       break;
  1929.    case SPEEX_GET_ENH:
  1930.       *((int*)ptr) = st->lpc_enh_enabled;
  1931.       break;
  1932.    case SPEEX_GET_FRAME_SIZE:
  1933.       (*(int*)ptr) = st->frameSize;
  1934.       break;
  1935.    case SPEEX_GET_BITRATE:
  1936.       if (st->submodes[st->submodeID])
  1937.          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
  1938.       else
  1939.          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
  1940.       break;
  1941.    case SPEEX_SET_SAMPLING_RATE:
  1942.       st->sampling_rate = (*(int*)ptr);
  1943.       break;
  1944.    case SPEEX_GET_SAMPLING_RATE:
  1945.       (*(int*)ptr)=st->sampling_rate;
  1946.       break;
  1947.    case SPEEX_SET_HANDLER:
  1948.       {
  1949.          SpeexCallback *c = (SpeexCallback*)ptr;
  1950.          st->speex_callbacks[c->callback_id].func=c->func;
  1951.          st->speex_callbacks[c->callback_id].data=c->data;
  1952.          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
  1953.       }
  1954.       break;
  1955.    case SPEEX_SET_USER_HANDLER:
  1956.       {
  1957.          SpeexCallback *c = (SpeexCallback*)ptr;
  1958.          st->user_callback.func=c->func;
  1959.          st->user_callback.data=c->data;
  1960.          st->user_callback.callback_id=c->callback_id;
  1961.       }
  1962.       break;
  1963.    case SPEEX_RESET_STATE:
  1964.       {
  1965.          int i;
  1966.          for (i=0;i<2*st->lpcSize;i++)
  1967.             st->mem_sp[i]=0;
  1968.          for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
  1969.             st->excBuf[i]=0;
  1970.          for (i=0;i<st->frameSize;i++)
  1971.             st->inBuf[i] = 0;
  1972.       }
  1973.       break;
  1974.    case SPEEX_SET_SUBMODE_ENCODING:
  1975.       st->encode_submode = (*(int*)ptr);
  1976.       break;
  1977.    case SPEEX_GET_SUBMODE_ENCODING:
  1978.       (*(int*)ptr) = st->encode_submode;
  1979.       break;
  1980.    case SPEEX_GET_PI_GAIN:
  1981.       {
  1982.          int i;
  1983.          spx_word32_t *g = (spx_word32_t*)ptr;
  1984.          for (i=0;i<st->nbSubframes;i++)
  1985.             g[i]=st->pi_gain[i];
  1986.       }
  1987.       break;
  1988.    case SPEEX_GET_EXC:
  1989.       {
  1990.          int i;
  1991.          spx_sig_t *e = (spx_sig_t*)ptr;
  1992.          for (i=0;i<st->frameSize;i++)
  1993.             e[i]=st->exc[i];
  1994.       }
  1995.       break;
  1996.    case SPEEX_GET_INNOV:
  1997.       {
  1998.          int i;
  1999.          spx_sig_t *e = (spx_sig_t*)ptr;
  2000.          for (i=0;i<st->frameSize;i++)
  2001.             e[i]=st->innov[i];
  2002.       }
  2003.       break;
  2004.    case SPEEX_GET_DTX_STATUS:
  2005.       *((int*)ptr) = st->dtx_enabled;
  2006.       break;
  2007.    default:
  2008.       speex_warning_int("Unknown nb_ctl request: ", request);
  2009.       return -1;
  2010.    }
  2011.    return 0;
  2012. }