Home | History | Annotate | Download | only in libspeex
      1 /* Copyright (C) 2002-2006 Jean-Marc Valin
      2    File: sb_celp.c
      3 
      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 */
     31 
     32 #ifdef HAVE_CONFIG_H
     33 #include "config.h"
     34 #endif
     35 
     36 #include <math.h>
     37 #include "sb_celp.h"
     38 #include "filters.h"
     39 #include "lpc.h"
     40 #include "lsp.h"
     41 #include "stack_alloc.h"
     42 #include "cb_search.h"
     43 #include "quant_lsp.h"
     44 #include "vq.h"
     45 #include "ltp.h"
     46 #include "arch.h"
     47 #include "math_approx.h"
     48 #include "os_support.h"
     49 
     50 #ifndef NULL
     51 #define NULL 0
     52 #endif
     53 
     54 /* Default size for the encoder and decoder stack (can be changed at compile time).
     55    This does not apply when using variable-size arrays or alloca. */
     56 #ifndef SB_ENC_STACK
     57 #define SB_ENC_STACK (10000*sizeof(spx_sig_t))
     58 #endif
     59 
     60 #ifndef SB_DEC_STACK
     61 #define SB_DEC_STACK (6000*sizeof(spx_sig_t))
     62 #endif
     63 
     64 
     65 #ifdef DISABLE_WIDEBAND
     66 void *sb_encoder_init(const SpeexMode *m)
     67 {
     68    speex_fatal("Wideband and Ultra-wideband are disabled");
     69    return NULL;
     70 }
     71 void sb_encoder_destroy(void *state)
     72 {
     73    speex_fatal("Wideband and Ultra-wideband are disabled");
     74 }
     75 int sb_encode(void *state, void *vin, SpeexBits *bits)
     76 {
     77    speex_fatal("Wideband and Ultra-wideband are disabled");
     78    return -2;
     79 }
     80 void *sb_decoder_init(const SpeexMode *m)
     81 {
     82    speex_fatal("Wideband and Ultra-wideband are disabled");
     83    return NULL;
     84 }
     85 void sb_decoder_destroy(void *state)
     86 {
     87    speex_fatal("Wideband and Ultra-wideband are disabled");
     88 }
     89 int sb_decode(void *state, SpeexBits *bits, void *vout)
     90 {
     91    speex_fatal("Wideband and Ultra-wideband are disabled");
     92    return -2;
     93 }
     94 int sb_encoder_ctl(void *state, int request, void *ptr)
     95 {
     96    speex_fatal("Wideband and Ultra-wideband are disabled");
     97    return -2;
     98 }
     99 int sb_decoder_ctl(void *state, int request, void *ptr)
    100 {
    101    speex_fatal("Wideband and Ultra-wideband are disabled");
    102    return -2;
    103 }
    104 #else
    105 
    106 
    107 #ifndef M_PI
    108 #define M_PI           3.14159265358979323846  /* pi */
    109 #endif
    110 
    111 #define sqr(x) ((x)*(x))
    112 
    113 #define SUBMODE(x) st->submodes[st->submodeID]->x
    114 
    115 #ifdef FIXED_POINT
    116 static const spx_word16_t gc_quant_bound[16] = {125, 164, 215, 282, 370, 484, 635, 832, 1090, 1428, 1871, 2452, 3213, 4210, 5516, 7228};
    117 static const spx_word16_t fold_quant_bound[32] = {
    118    39, 44, 50, 57, 64, 73, 83, 94,
    119    106, 120, 136, 154, 175, 198, 225, 255,
    120    288, 327, 370, 420, 476, 539, 611, 692,
    121    784, 889, 1007, 1141, 1293, 1465, 1660, 1881};
    122 #define LSP_MARGIN 410
    123 #define LSP_DELTA1 6553
    124 #define LSP_DELTA2 1638
    125 
    126 #else
    127 
    128 static const spx_word16_t gc_quant_bound[16] = {
    129       0.97979, 1.28384, 1.68223, 2.20426, 2.88829, 3.78458, 4.95900, 6.49787,
    130       8.51428, 11.15642, 14.61846, 19.15484, 25.09895, 32.88761, 43.09325, 56.46588};
    131 static const spx_word16_t fold_quant_bound[32] = {
    132    0.30498, 0.34559, 0.39161, 0.44375, 0.50283, 0.56979, 0.64565, 0.73162,
    133    0.82903, 0.93942, 1.06450, 1.20624, 1.36685, 1.54884, 1.75506, 1.98875,
    134    2.25355, 2.55360, 2.89361, 3.27889, 3.71547, 4.21018, 4.77076, 5.40598,
    135    6.12577, 6.94141, 7.86565, 8.91295, 10.09969, 11.44445, 12.96826, 14.69497};
    136 
    137 #define LSP_MARGIN .05
    138 #define LSP_DELTA1 .2
    139 #define LSP_DELTA2 .05
    140 
    141 #endif
    142 
    143 #define QMF_ORDER 64
    144 
    145 #ifdef FIXED_POINT
    146 static const spx_word16_t h0[64] = {2, -7, -7, 18, 15, -39, -25, 75, 35, -130, -41, 212, 38, -327, -17, 483, -32, -689, 124, 956, -283, -1307, 543, 1780, -973, -2467, 1733, 3633, -3339, -6409, 9059, 30153, 30153, 9059, -6409, -3339, 3633, 1733, -2467, -973, 1780, 543, -1307, -283, 956, 124, -689, -32, 483, -17, -327, 38, 212, -41, -130, 35, 75, -25, -39, 15, 18, -7, -7, 2};
    147 
    148 #else
    149 static const float h0[64] = {
    150    3.596189e-05f, -0.0001123515f,
    151    -0.0001104587f, 0.0002790277f,
    152    0.0002298438f, -0.0005953563f,
    153    -0.0003823631f, 0.00113826f,
    154    0.0005308539f, -0.001986177f,
    155    -0.0006243724f, 0.003235877f,
    156    0.0005743159f, -0.004989147f,
    157    -0.0002584767f, 0.007367171f,
    158    -0.0004857935f, -0.01050689f,
    159    0.001894714f, 0.01459396f,
    160    -0.004313674f, -0.01994365f,
    161    0.00828756f, 0.02716055f,
    162    -0.01485397f, -0.03764973f,
    163    0.026447f, 0.05543245f,
    164    -0.05095487f, -0.09779096f,
    165    0.1382363f, 0.4600981f,
    166    0.4600981f, 0.1382363f,
    167    -0.09779096f, -0.05095487f,
    168    0.05543245f, 0.026447f,
    169    -0.03764973f, -0.01485397f,
    170    0.02716055f, 0.00828756f,
    171    -0.01994365f, -0.004313674f,
    172    0.01459396f, 0.001894714f,
    173    -0.01050689f, -0.0004857935f,
    174    0.007367171f, -0.0002584767f,
    175    -0.004989147f, 0.0005743159f,
    176    0.003235877f, -0.0006243724f,
    177    -0.001986177f, 0.0005308539f,
    178    0.00113826f, -0.0003823631f,
    179    -0.0005953563f, 0.0002298438f,
    180    0.0002790277f, -0.0001104587f,
    181    -0.0001123515f, 3.596189e-05f
    182 };
    183 
    184 #endif
    185 
    186 extern const spx_word16_t lag_window[];
    187 extern const spx_word16_t lpc_window[];
    188 
    189 
    190 void *sb_encoder_init(const SpeexMode *m)
    191 {
    192    int i;
    193    spx_int32_t tmp;
    194    SBEncState *st;
    195    const SpeexSBMode *mode;
    196 
    197    st = (SBEncState*)speex_alloc(sizeof(SBEncState));
    198    if (!st)
    199       return NULL;
    200    st->mode = m;
    201    mode = (const SpeexSBMode*)m->mode;
    202 
    203 
    204    st->st_low = speex_encoder_init(mode->nb_mode);
    205 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
    206    st->stack = NULL;
    207 #else
    208    /*st->stack = (char*)speex_alloc_scratch(SB_ENC_STACK);*/
    209    speex_encoder_ctl(st->st_low, SPEEX_GET_STACK, &st->stack);
    210 #endif
    211 
    212    st->full_frame_size = 2*mode->frameSize;
    213    st->frame_size = mode->frameSize;
    214    st->subframeSize = mode->subframeSize;
    215    st->nbSubframes = mode->frameSize/mode->subframeSize;
    216    st->windowSize = st->frame_size+st->subframeSize;
    217    st->lpcSize=mode->lpcSize;
    218 
    219    st->encode_submode = 1;
    220    st->submodes=mode->submodes;
    221    st->submodeSelect = st->submodeID=mode->defaultSubmode;
    222 
    223    tmp=9;
    224    speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &tmp);
    225    tmp=1;
    226    speex_encoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);
    227 
    228    st->lpc_floor = mode->lpc_floor;
    229    st->gamma1=mode->gamma1;
    230    st->gamma2=mode->gamma2;
    231    st->first=1;
    232 
    233    st->high=(spx_word16_t*)speex_alloc((st->windowSize-st->frame_size)*sizeof(spx_word16_t));
    234 
    235    st->h0_mem=(spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
    236    st->h1_mem=(spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
    237 
    238    st->window= lpc_window;
    239 
    240    st->lagWindow = lag_window;
    241 
    242    st->old_lsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
    243    st->old_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
    244    st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
    245    st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
    246    st->exc_rms = (spx_word16_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word16_t));
    247    st->innov_rms_save = NULL;
    248 
    249    st->mem_sp = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
    250    st->mem_sp2 = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
    251    st->mem_sw = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
    252 
    253    for (i=0;i<st->lpcSize;i++)
    254       st->old_lsp[i]= DIV32(MULT16_16(QCONST16(3.1415927f, LSP_SHIFT), i+1), st->lpcSize+1);
    255 
    256 #ifndef DISABLE_VBR
    257    st->vbr_quality = 8;
    258    st->vbr_enabled = 0;
    259    st->vbr_max = 0;
    260    st->vbr_max_high = 20000;  /* We just need a big value here */
    261    st->vad_enabled = 0;
    262    st->abr_enabled = 0;
    263    st->relative_quality=0;
    264 #endif /* #ifndef DISABLE_VBR */
    265 
    266    st->complexity=2;
    267    speex_encoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
    268    st->sampling_rate*=2;
    269 #ifdef ENABLE_VALGRIND
    270    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
    271 #endif
    272    return st;
    273 }
    274 
    275 void sb_encoder_destroy(void *state)
    276 {
    277    SBEncState *st=(SBEncState*)state;
    278 
    279    speex_encoder_destroy(st->st_low);
    280 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
    281    /*speex_free_scratch(st->stack);*/
    282 #endif
    283 
    284    speex_free(st->high);
    285 
    286    speex_free(st->h0_mem);
    287    speex_free(st->h1_mem);
    288 
    289    speex_free(st->old_lsp);
    290    speex_free(st->old_qlsp);
    291    speex_free(st->interp_qlpc);
    292    speex_free(st->pi_gain);
    293    speex_free(st->exc_rms);
    294 
    295    speex_free(st->mem_sp);
    296    speex_free(st->mem_sp2);
    297    speex_free(st->mem_sw);
    298 
    299 
    300    speex_free(st);
    301 }
    302 
    303 
    304 int sb_encode(void *state, void *vin, SpeexBits *bits)
    305 {
    306    SBEncState *st;
    307    int i, roots, sub;
    308    char *stack;
    309    VARDECL(spx_mem_t *mem);
    310    VARDECL(spx_sig_t *innov);
    311    VARDECL(spx_word16_t *target);
    312    VARDECL(spx_word16_t *syn_resp);
    313    VARDECL(spx_word32_t *low_pi_gain);
    314    spx_word16_t *low;
    315    spx_word16_t *high;
    316    VARDECL(spx_word16_t *low_exc_rms);
    317    VARDECL(spx_word16_t *low_innov_rms);
    318    const SpeexSBMode *mode;
    319    spx_int32_t dtx;
    320    spx_word16_t *in = (spx_word16_t*)vin;
    321    spx_word16_t e_low=0, e_high=0;
    322    VARDECL(spx_coef_t *lpc);
    323    VARDECL(spx_coef_t *interp_lpc);
    324    VARDECL(spx_coef_t *bw_lpc1);
    325    VARDECL(spx_coef_t *bw_lpc2);
    326    VARDECL(spx_lsp_t *lsp);
    327    VARDECL(spx_lsp_t *qlsp);
    328    VARDECL(spx_lsp_t *interp_lsp);
    329    VARDECL(spx_lsp_t *interp_qlsp);
    330 
    331    st = (SBEncState*)state;
    332    stack=st->stack;
    333    mode = (const SpeexSBMode*)(st->mode->mode);
    334    low = in;
    335    high = in+st->frame_size;
    336 
    337    /* High-band buffering / sync with low band */
    338    /* Compute the two sub-bands by filtering with QMF h0*/
    339    qmf_decomp(in, h0, low, high, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
    340 
    341 #ifndef DISABLE_VBR
    342    if (st->vbr_enabled || st->vad_enabled)
    343    {
    344       /* Need to compute things here before the signal is trashed by the encoder */
    345       /*FIXME: Are the two signals (low, high) in sync? */
    346       e_low = compute_rms16(low, st->frame_size);
    347       e_high = compute_rms16(high, st->frame_size);
    348    }
    349 #endif /* #ifndef DISABLE_VBR */
    350 
    351    ALLOC(low_innov_rms, st->nbSubframes, spx_word16_t);
    352    speex_encoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_rms);
    353    /* Encode the narrowband part*/
    354    speex_encode_native(st->st_low, low, bits);
    355 
    356    high = high - (st->windowSize-st->frame_size);
    357    SPEEX_COPY(high, st->high, st->windowSize-st->frame_size);
    358    SPEEX_COPY(st->high, &high[st->frame_size], st->windowSize-st->frame_size);
    359 
    360 
    361    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
    362    ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
    363    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
    364    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);
    365 
    366    speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
    367 
    368    if (dtx==0)
    369       dtx=1;
    370    else
    371       dtx=0;
    372 
    373    ALLOC(lpc, st->lpcSize, spx_coef_t);
    374    ALLOC(interp_lpc, st->lpcSize, spx_coef_t);
    375    ALLOC(bw_lpc1, st->lpcSize, spx_coef_t);
    376    ALLOC(bw_lpc2, st->lpcSize, spx_coef_t);
    377 
    378    ALLOC(lsp, st->lpcSize, spx_lsp_t);
    379    ALLOC(qlsp, st->lpcSize, spx_lsp_t);
    380    ALLOC(interp_lsp, st->lpcSize, spx_lsp_t);
    381    ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
    382 
    383    {
    384       VARDECL(spx_word16_t *autocorr);
    385       VARDECL(spx_word16_t *w_sig);
    386       ALLOC(autocorr, st->lpcSize+1, spx_word16_t);
    387       ALLOC(w_sig, st->windowSize, spx_word16_t);
    388       /* Window for analysis */
    389       /* FIXME: This is a kludge */
    390       if (st->subframeSize==80)
    391       {
    392          for (i=0;i<st->windowSize;i++)
    393             w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i>>1]),SIG_SHIFT));
    394       } else {
    395          for (i=0;i<st->windowSize;i++)
    396             w_sig[i] = EXTRACT16(SHR32(MULT16_16(high[i],st->window[i]),SIG_SHIFT));
    397       }
    398       /* Compute auto-correlation */
    399       _spx_autocorr(w_sig, autocorr, st->lpcSize+1, st->windowSize);
    400       autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
    401 
    402       /* Lag windowing: equivalent to filtering in the power-spectrum domain */
    403       for (i=0;i<st->lpcSize+1;i++)
    404          autocorr[i] = MULT16_16_Q14(autocorr[i],st->lagWindow[i]);
    405 
    406       /* Levinson-Durbin */
    407       _spx_lpc(lpc, autocorr, st->lpcSize);
    408    }
    409 
    410    /* LPC to LSPs (x-domain) transform */
    411    roots=lpc_to_lsp (lpc, st->lpcSize, lsp, 10, LSP_DELTA1, stack);
    412    if (roots!=st->lpcSize)
    413    {
    414       roots = lpc_to_lsp (lpc, st->lpcSize, lsp, 10, LSP_DELTA2, stack);
    415       if (roots!=st->lpcSize) {
    416          /*If we can't find all LSP's, do some damage control and use a flat filter*/
    417          for (i=0;i<st->lpcSize;i++)
    418          {
    419             lsp[i]=st->old_lsp[i];
    420          }
    421       }
    422    }
    423 
    424 #ifndef DISABLE_VBR
    425    /* VBR code */
    426    if ((st->vbr_enabled || st->vad_enabled) && !dtx)
    427    {
    428       float ratio;
    429       if (st->abr_enabled)
    430       {
    431          float qual_change=0;
    432          if (st->abr_drift2 * st->abr_drift > 0)
    433          {
    434             /* Only adapt if long-term and short-term drift are the same sign */
    435             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
    436             if (qual_change>.1)
    437                qual_change=.1;
    438             if (qual_change<-.1)
    439                qual_change=-.1;
    440          }
    441          st->vbr_quality += qual_change;
    442          if (st->vbr_quality>10)
    443             st->vbr_quality=10;
    444          if (st->vbr_quality<0)
    445             st->vbr_quality=0;
    446       }
    447 
    448 
    449       ratio = 2*log((1.f+e_high)/(1.f+e_low));
    450 
    451       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &st->relative_quality);
    452       if (ratio<-4)
    453          ratio=-4;
    454       if (ratio>2)
    455          ratio=2;
    456       /*if (ratio>-2)*/
    457       if (st->vbr_enabled)
    458       {
    459          spx_int32_t modeid;
    460          modeid = mode->nb_modes-1;
    461          st->relative_quality+=1.0*(ratio+2);
    462 	 if (st->relative_quality<-1)
    463             st->relative_quality=-1;
    464          while (modeid)
    465          {
    466             int v1;
    467             float thresh;
    468             v1=(int)floor(st->vbr_quality);
    469             if (v1==10)
    470                thresh = mode->vbr_thresh[modeid][v1];
    471             else
    472                thresh = (st->vbr_quality-v1)   * mode->vbr_thresh[modeid][v1+1] +
    473                         (1+v1-st->vbr_quality) * mode->vbr_thresh[modeid][v1];
    474             if (st->relative_quality >= thresh && st->sampling_rate*st->submodes[modeid]->bits_per_frame/st->full_frame_size <= st->vbr_max_high)
    475                break;
    476             modeid--;
    477          }
    478          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &modeid);
    479          if (st->abr_enabled)
    480          {
    481             spx_int32_t bitrate;
    482             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
    483             st->abr_drift+=(bitrate-st->abr_enabled);
    484             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
    485             st->abr_count += 1.0;
    486          }
    487 
    488       } else {
    489          /* VAD only */
    490          int modeid;
    491          if (st->relative_quality<2.0)
    492             modeid=1;
    493          else
    494             modeid=st->submodeSelect;
    495          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
    496          st->submodeID=modeid;
    497 
    498       }
    499       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
    500    }
    501 #endif /* #ifndef DISABLE_VBR */
    502 
    503    if (st->encode_submode)
    504    {
    505       speex_bits_pack(bits, 1, 1);
    506       if (dtx)
    507          speex_bits_pack(bits, 0, SB_SUBMODE_BITS);
    508       else
    509          speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
    510    }
    511 
    512    /* If null mode (no transmission), just set a couple things to zero*/
    513    if (dtx || st->submodes[st->submodeID] == NULL)
    514    {
    515       for (i=0;i<st->frame_size;i++)
    516          high[i]=VERY_SMALL;
    517 
    518       for (i=0;i<st->lpcSize;i++)
    519          st->mem_sw[i]=0;
    520       st->first=1;
    521 
    522       /* Final signal synthesis from excitation */
    523       iir_mem16(high, st->interp_qlpc, high, st->frame_size, st->lpcSize, st->mem_sp, stack);
    524 
    525       if (dtx)
    526          return 0;
    527       else
    528          return 1;
    529    }
    530 
    531 
    532    /* LSP quantization */
    533    SUBMODE(lsp_quant)(lsp, qlsp, st->lpcSize, bits);
    534 
    535    if (st->first)
    536    {
    537       for (i=0;i<st->lpcSize;i++)
    538          st->old_lsp[i] = lsp[i];
    539       for (i=0;i<st->lpcSize;i++)
    540          st->old_qlsp[i] = qlsp[i];
    541    }
    542 
    543    ALLOC(mem, st->lpcSize, spx_mem_t);
    544    ALLOC(syn_resp, st->subframeSize, spx_word16_t);
    545    ALLOC(innov, st->subframeSize, spx_sig_t);
    546    ALLOC(target, st->subframeSize, spx_word16_t);
    547 
    548    for (sub=0;sub<st->nbSubframes;sub++)
    549    {
    550       VARDECL(spx_word16_t *exc);
    551       VARDECL(spx_word16_t *res);
    552       VARDECL(spx_word16_t *sw);
    553       spx_word16_t *sp;
    554       spx_word16_t filter_ratio;     /*Q7*/
    555       int offset;
    556       spx_word32_t rl, rh;           /*Q13*/
    557       spx_word16_t eh=0;
    558 
    559       offset = st->subframeSize*sub;
    560       sp=high+offset;
    561       ALLOC(exc, st->subframeSize, spx_word16_t);
    562       ALLOC(res, st->subframeSize, spx_word16_t);
    563       ALLOC(sw, st->subframeSize, spx_word16_t);
    564 
    565       /* LSP interpolation (quantized and unquantized) */
    566       lsp_interpolate(st->old_lsp, lsp, interp_lsp, st->lpcSize, sub, st->nbSubframes);
    567       lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);
    568 
    569       lsp_enforce_margin(interp_lsp, st->lpcSize, LSP_MARGIN);
    570       lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);
    571 
    572       lsp_to_lpc(interp_lsp, interp_lpc, st->lpcSize,stack);
    573       lsp_to_lpc(interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
    574 
    575       bw_lpc(st->gamma1, interp_lpc, bw_lpc1, st->lpcSize);
    576       bw_lpc(st->gamma2, interp_lpc, bw_lpc2, st->lpcSize);
    577 
    578       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
    579          filters */
    580       st->pi_gain[sub]=LPC_SCALING;
    581       rh = LPC_SCALING;
    582       for (i=0;i<st->lpcSize;i+=2)
    583       {
    584          rh += st->interp_qlpc[i+1] - st->interp_qlpc[i];
    585          st->pi_gain[sub] += st->interp_qlpc[i] + st->interp_qlpc[i+1];
    586       }
    587 
    588       rl = low_pi_gain[sub];
    589 #ifdef FIXED_POINT
    590       filter_ratio=EXTRACT16(SATURATE(PDIV32(SHL32(ADD32(rl,82),7),ADD32(82,rh)),32767));
    591 #else
    592       filter_ratio=(rl+.01)/(rh+.01);
    593 #endif
    594 
    595       /* Compute "real excitation" */
    596       fir_mem16(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2, stack);
    597       /* Compute energy of low-band and high-band excitation */
    598 
    599       eh = compute_rms16(exc, st->subframeSize);
    600 
    601       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
    602          spx_word32_t g;   /*Q7*/
    603          spx_word16_t el;  /*Q0*/
    604          el = low_innov_rms[sub];
    605 
    606          /* Gain to use if we want to use the low-band excitation for high-band */
    607          g=PDIV32(MULT16_16(filter_ratio,eh),EXTEND32(ADD16(1,el)));
    608 
    609 #if 0
    610          {
    611             char *tmp_stack=stack;
    612             float *tmp_sig;
    613             float g2;
    614             ALLOC(tmp_sig, st->subframeSize, spx_sig_t);
    615             for (i=0;i<st->lpcSize;i++)
    616                mem[i]=st->mem_sp[i];
    617             iir_mem2(st->low_innov+offset, st->interp_qlpc, tmp_sig, st->subframeSize, st->lpcSize, mem);
    618             g2 = compute_rms(sp, st->subframeSize)/(.01+compute_rms(tmp_sig, st->subframeSize));
    619             /*fprintf (stderr, "gains: %f %f\n", g, g2);*/
    620             g = g2;
    621             stack = tmp_stack;
    622          }
    623 #endif
    624 
    625          /*print_vec(&g, 1, "gain factor");*/
    626          /* Gain quantization */
    627          {
    628             int quant = scal_quant(g, fold_quant_bound, 32);
    629             /*speex_warning_int("tata", quant);*/
    630             if (quant<0)
    631                quant=0;
    632             if (quant>31)
    633                quant=31;
    634             speex_bits_pack(bits, quant, 5);
    635          }
    636          if (st->innov_rms_save)
    637          {
    638             st->innov_rms_save[sub] = eh;
    639          }
    640          st->exc_rms[sub] = eh;
    641       } else {
    642          spx_word16_t gc;       /*Q7*/
    643          spx_word32_t scale;    /*Q14*/
    644          spx_word16_t el;       /*Q0*/
    645          el = low_exc_rms[sub]; /*Q0*/
    646 
    647          gc = PDIV32_16(MULT16_16(filter_ratio,1+eh),1+el);
    648 
    649          /* This is a kludge that cleans up a historical bug */
    650          if (st->subframeSize==80)
    651             gc = MULT16_16_P15(QCONST16(0.70711f,15),gc);
    652          /*printf ("%f %f %f %f\n", el, eh, filter_ratio, gc);*/
    653          {
    654             int qgc = scal_quant(gc, gc_quant_bound, 16);
    655             speex_bits_pack(bits, qgc, 4);
    656             gc = MULT16_16_Q15(QCONST16(0.87360,15),gc_quant_bound[qgc]);
    657          }
    658          if (st->subframeSize==80)
    659             gc = MULT16_16_P14(QCONST16(1.4142f,14), gc);
    660 
    661          scale = SHL32(MULT16_16(PDIV32_16(SHL32(EXTEND32(gc),SIG_SHIFT-6),filter_ratio),(1+el)),6);
    662 
    663          compute_impulse_response(st->interp_qlpc, bw_lpc1, bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
    664 
    665 
    666          /* Reset excitation */
    667          for (i=0;i<st->subframeSize;i++)
    668             res[i]=VERY_SMALL;
    669 
    670          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
    671          for (i=0;i<st->lpcSize;i++)
    672             mem[i]=st->mem_sp[i];
    673          iir_mem16(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize, mem, stack);
    674 
    675          for (i=0;i<st->lpcSize;i++)
    676             mem[i]=st->mem_sw[i];
    677          filter_mem16(res, bw_lpc1, bw_lpc2, res, st->subframeSize, st->lpcSize, mem, stack);
    678 
    679          /* Compute weighted signal */
    680          for (i=0;i<st->lpcSize;i++)
    681             mem[i]=st->mem_sw[i];
    682          filter_mem16(sp, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, mem, stack);
    683 
    684          /* Compute target signal */
    685          for (i=0;i<st->subframeSize;i++)
    686             target[i]=SUB16(sw[i],res[i]);
    687 
    688          signal_div(target, target, scale, st->subframeSize);
    689 
    690          /* Reset excitation */
    691          SPEEX_MEMSET(innov, 0, st->subframeSize);
    692 
    693          /*print_vec(target, st->subframeSize, "\ntarget");*/
    694          SUBMODE(innovation_quant)(target, st->interp_qlpc, bw_lpc1, bw_lpc2,
    695                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize,
    696                                    innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
    697          /*print_vec(target, st->subframeSize, "after");*/
    698 
    699          signal_mul(innov, innov, scale, st->subframeSize);
    700 
    701          if (SUBMODE(double_codebook)) {
    702             char *tmp_stack=stack;
    703             VARDECL(spx_sig_t *innov2);
    704             ALLOC(innov2, st->subframeSize, spx_sig_t);
    705             SPEEX_MEMSET(innov2, 0, st->subframeSize);
    706             for (i=0;i<st->subframeSize;i++)
    707                target[i]=MULT16_16_P13(QCONST16(2.5f,13), target[i]);
    708 
    709             SUBMODE(innovation_quant)(target, st->interp_qlpc, bw_lpc1, bw_lpc2,
    710                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize,
    711                                       innov2, syn_resp, bits, stack, st->complexity, 0);
    712             signal_mul(innov2, innov2, MULT16_32_P15(QCONST16(0.4f,15),scale), st->subframeSize);
    713 
    714             for (i=0;i<st->subframeSize;i++)
    715                innov[i] = ADD32(innov[i],innov2[i]);
    716             stack = tmp_stack;
    717          }
    718          for (i=0;i<st->subframeSize;i++)
    719             exc[i] = PSHR32(innov[i],SIG_SHIFT);
    720 
    721          if (st->innov_rms_save)
    722          {
    723             st->innov_rms_save[sub] = MULT16_16_Q15(QCONST16(.70711f, 15), compute_rms(innov, st->subframeSize));
    724          }
    725          st->exc_rms[sub] = compute_rms16(exc, st->subframeSize);
    726 
    727 
    728       }
    729 
    730 
    731       /*Keep the previous memory*/
    732       for (i=0;i<st->lpcSize;i++)
    733          mem[i]=st->mem_sp[i];
    734       /* Final signal synthesis from excitation */
    735       iir_mem16(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp, stack);
    736 
    737       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
    738       filter_mem16(sp, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw, stack);
    739    }
    740 
    741    for (i=0;i<st->lpcSize;i++)
    742       st->old_lsp[i] = lsp[i];
    743    for (i=0;i<st->lpcSize;i++)
    744       st->old_qlsp[i] = qlsp[i];
    745 
    746    st->first=0;
    747 
    748    return 1;
    749 }
    750 
    751 
    752 
    753 
    754 
    755 void *sb_decoder_init(const SpeexMode *m)
    756 {
    757    spx_int32_t tmp;
    758    SBDecState *st;
    759    const SpeexSBMode *mode;
    760    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
    761    if (!st)
    762       return NULL;
    763    st->mode = m;
    764    mode=(const SpeexSBMode*)m->mode;
    765    st->encode_submode = 1;
    766 
    767    st->st_low = speex_decoder_init(mode->nb_mode);
    768 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
    769    st->stack = NULL;
    770 #else
    771    /*st->stack = (char*)speex_alloc_scratch(SB_DEC_STACK);*/
    772    speex_decoder_ctl(st->st_low, SPEEX_GET_STACK, &st->stack);
    773 #endif
    774 
    775    st->full_frame_size = 2*mode->frameSize;
    776    st->frame_size = mode->frameSize;
    777    st->subframeSize = mode->subframeSize;
    778    st->nbSubframes = mode->frameSize/mode->subframeSize;
    779    st->lpcSize=mode->lpcSize;
    780    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
    781    st->sampling_rate*=2;
    782    tmp=1;
    783    speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);
    784 
    785    st->submodes=mode->submodes;
    786    st->submodeID=mode->defaultSubmode;
    787 
    788    st->first=1;
    789 
    790    st->g0_mem = (spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
    791    st->g1_mem = (spx_word16_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word16_t));
    792 
    793    st->excBuf = (spx_word16_t*)speex_alloc((st->subframeSize)*sizeof(spx_word16_t));
    794 
    795    st->old_qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
    796    st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
    797 
    798    st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
    799    st->exc_rms = (spx_word16_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word16_t));
    800    st->mem_sp = (spx_mem_t*)speex_alloc((2*st->lpcSize)*sizeof(spx_mem_t));
    801 
    802    st->innov_save = NULL;
    803 
    804 
    805    st->lpc_enh_enabled=0;
    806    st->seed = 1000;
    807 
    808 #ifdef ENABLE_VALGRIND
    809    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
    810 #endif
    811    return st;
    812 }
    813 
    814 void sb_decoder_destroy(void *state)
    815 {
    816    SBDecState *st;
    817    st = (SBDecState*)state;
    818    speex_decoder_destroy(st->st_low);
    819 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
    820    /*speex_free_scratch(st->stack);*/
    821 #endif
    822 
    823    speex_free(st->g0_mem);
    824    speex_free(st->g1_mem);
    825    speex_free(st->excBuf);
    826    speex_free(st->old_qlsp);
    827    speex_free(st->interp_qlpc);
    828    speex_free(st->pi_gain);
    829    speex_free(st->exc_rms);
    830    speex_free(st->mem_sp);
    831 
    832    speex_free(state);
    833 }
    834 
    835 static void sb_decode_lost(SBDecState *st, spx_word16_t *out, int dtx, char *stack)
    836 {
    837    int i;
    838    int saved_modeid=0;
    839 
    840    if (dtx)
    841    {
    842       saved_modeid=st->submodeID;
    843       st->submodeID=1;
    844    } else {
    845       bw_lpc(QCONST16(0.99f,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
    846    }
    847 
    848    st->first=1;
    849 
    850 
    851    /* Final signal synthesis from excitation */
    852    if (!dtx)
    853    {
    854       st->last_ener =  MULT16_16_Q15(QCONST16(.9f,15),st->last_ener);
    855    }
    856    for (i=0;i<st->frame_size;i++)
    857       out[i+st->frame_size] = speex_rand(st->last_ener, &st->seed);
    858 
    859    iir_mem16(out+st->frame_size, st->interp_qlpc, out+st->frame_size, st->frame_size, st->lpcSize,
    860             st->mem_sp, stack);
    861 
    862 
    863    /* Reconstruct the original */
    864    qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
    865    if (dtx)
    866    {
    867       st->submodeID=saved_modeid;
    868    }
    869 
    870    return;
    871 }
    872 
    873 int sb_decode(void *state, SpeexBits *bits, void *vout)
    874 {
    875    int i, sub;
    876    SBDecState *st;
    877    int wideband;
    878    int ret;
    879    char *stack;
    880    VARDECL(spx_word32_t *low_pi_gain);
    881    VARDECL(spx_word16_t *low_exc_rms);
    882    VARDECL(spx_coef_t *ak);
    883    VARDECL(spx_lsp_t *qlsp);
    884    VARDECL(spx_lsp_t *interp_qlsp);
    885    spx_int32_t dtx;
    886    const SpeexSBMode *mode;
    887    spx_word16_t *out = (spx_word16_t*)vout;
    888    spx_word16_t *low_innov_alias;
    889    spx_word32_t exc_ener_sum = 0;
    890 
    891    st = (SBDecState*)state;
    892    stack=st->stack;
    893    mode = (const SpeexSBMode*)(st->mode->mode);
    894 
    895    low_innov_alias = out+st->frame_size;
    896    speex_decoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_alias);
    897    /* Decode the low-band */
    898    ret = speex_decode_native(st->st_low, bits, out);
    899 
    900    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
    901 
    902    /* If error decoding the narrowband part, propagate error */
    903    if (ret!=0)
    904    {
    905       return ret;
    906    }
    907 
    908    if (!bits)
    909    {
    910       sb_decode_lost(st, out, dtx, stack);
    911       return 0;
    912    }
    913 
    914    if (st->encode_submode)
    915    {
    916 
    917       /*Check "wideband bit"*/
    918       if (speex_bits_remaining(bits)>0)
    919          wideband = speex_bits_peek(bits);
    920       else
    921          wideband = 0;
    922       if (wideband)
    923       {
    924          /*Regular wideband frame, read the submode*/
    925          wideband = speex_bits_unpack_unsigned(bits, 1);
    926          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
    927       } else
    928       {
    929          /*Was a narrowband frame, set "null submode"*/
    930          st->submodeID = 0;
    931       }
    932       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
    933       {
    934          speex_notify("Invalid mode encountered. The stream is corrupted.");
    935          return -2;
    936       }
    937    }
    938 
    939    /* If null mode (no transmission), just set a couple things to zero*/
    940    if (st->submodes[st->submodeID] == NULL)
    941    {
    942       if (dtx)
    943       {
    944          sb_decode_lost(st, out, 1, stack);
    945          return 0;
    946       }
    947 
    948       for (i=0;i<st->frame_size;i++)
    949          out[st->frame_size+i]=VERY_SMALL;
    950 
    951       st->first=1;
    952 
    953       /* Final signal synthesis from excitation */
    954       iir_mem16(out+st->frame_size, st->interp_qlpc, out+st->frame_size, st->frame_size, st->lpcSize, st->mem_sp, stack);
    955 
    956       qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
    957 
    958       return 0;
    959 
    960    }
    961 
    962    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
    963    ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
    964    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
    965    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);
    966 
    967    ALLOC(qlsp, st->lpcSize, spx_lsp_t);
    968    ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
    969    SUBMODE(lsp_unquant)(qlsp, st->lpcSize, bits);
    970 
    971    if (st->first)
    972    {
    973       for (i=0;i<st->lpcSize;i++)
    974          st->old_qlsp[i] = qlsp[i];
    975    }
    976 
    977    ALLOC(ak, st->lpcSize, spx_coef_t);
    978 
    979    for (sub=0;sub<st->nbSubframes;sub++)
    980    {
    981       VARDECL(spx_word32_t *exc);
    982       spx_word16_t *innov_save=NULL;
    983       spx_word16_t *sp;
    984       spx_word16_t filter_ratio;
    985       spx_word16_t el=0;
    986       int offset;
    987       spx_word32_t rl=0,rh=0;
    988 
    989       offset = st->subframeSize*sub;
    990       sp=out+st->frame_size+offset;
    991       ALLOC(exc, st->subframeSize, spx_word32_t);
    992       /* Pointer for saving innovation */
    993       if (st->innov_save)
    994       {
    995          innov_save = st->innov_save+2*offset;
    996          SPEEX_MEMSET(innov_save, 0, 2*st->subframeSize);
    997       }
    998 
    999       /* LSP interpolation */
   1000       lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);
   1001 
   1002       lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);
   1003 
   1004       /* LSP to LPC */
   1005       lsp_to_lpc(interp_qlsp, ak, st->lpcSize, stack);
   1006 
   1007       /* Calculate reponse ratio between the low and high filter in the middle
   1008          of the band (4000 Hz) */
   1009 
   1010          st->pi_gain[sub]=LPC_SCALING;
   1011          rh = LPC_SCALING;
   1012          for (i=0;i<st->lpcSize;i+=2)
   1013          {
   1014             rh += ak[i+1] - ak[i];
   1015             st->pi_gain[sub] += ak[i] + ak[i+1];
   1016          }
   1017 
   1018          rl = low_pi_gain[sub];
   1019 #ifdef FIXED_POINT
   1020          filter_ratio=EXTRACT16(SATURATE(PDIV32(SHL32(ADD32(rl,82),7),ADD32(82,rh)),32767));
   1021 #else
   1022          filter_ratio=(rl+.01)/(rh+.01);
   1023 #endif
   1024 
   1025       SPEEX_MEMSET(exc, 0, st->subframeSize);
   1026       if (!SUBMODE(innovation_unquant))
   1027       {
   1028          spx_word32_t g;
   1029          int quant;
   1030 
   1031          quant = speex_bits_unpack_unsigned(bits, 5);
   1032          g= spx_exp(MULT16_16(QCONST16(.125f,11),(quant-10)));
   1033 
   1034          g = PDIV32(g, filter_ratio);
   1035 
   1036          for (i=0;i<st->subframeSize;i+=2)
   1037          {
   1038             exc[i]=SHL32(MULT16_32_P15(MULT16_16_Q15(mode->folding_gain,low_innov_alias[offset+i]),SHL32(g,6)),SIG_SHIFT);
   1039             exc[i+1]=NEG32(SHL32(MULT16_32_P15(MULT16_16_Q15(mode->folding_gain,low_innov_alias[offset+i+1]),SHL32(g,6)),SIG_SHIFT));
   1040          }
   1041 
   1042       } else {
   1043          spx_word16_t gc;
   1044          spx_word32_t scale;
   1045          int qgc = speex_bits_unpack_unsigned(bits, 4);
   1046 
   1047          el = low_exc_rms[sub];
   1048          gc = MULT16_16_Q15(QCONST16(0.87360,15),gc_quant_bound[qgc]);
   1049 
   1050          if (st->subframeSize==80)
   1051             gc = MULT16_16_P14(QCONST16(1.4142f,14),gc);
   1052 
   1053          scale = SHL32(PDIV32(SHL32(MULT16_16(gc, el),3), filter_ratio),SIG_SHIFT-3);
   1054          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize,
   1055                                      bits, stack, &st->seed);
   1056 
   1057          signal_mul(exc,exc,scale,st->subframeSize);
   1058 
   1059          if (SUBMODE(double_codebook)) {
   1060             char *tmp_stack=stack;
   1061             VARDECL(spx_sig_t *innov2);
   1062             ALLOC(innov2, st->subframeSize, spx_sig_t);
   1063             SPEEX_MEMSET(innov2, 0, st->subframeSize);
   1064             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize,
   1065                                         bits, stack, &st->seed);
   1066             signal_mul(innov2, innov2, MULT16_32_P15(QCONST16(0.4f,15),scale), st->subframeSize);
   1067             for (i=0;i<st->subframeSize;i++)
   1068                exc[i] = ADD32(exc[i],innov2[i]);
   1069             stack = tmp_stack;
   1070          }
   1071 
   1072       }
   1073 
   1074       if (st->innov_save)
   1075       {
   1076          for (i=0;i<st->subframeSize;i++)
   1077             innov_save[2*i]=EXTRACT16(PSHR32(exc[i],SIG_SHIFT));
   1078       }
   1079 
   1080       iir_mem16(st->excBuf, st->interp_qlpc, sp, st->subframeSize, st->lpcSize,
   1081                st->mem_sp, stack);
   1082       for (i=0;i<st->subframeSize;i++)
   1083          st->excBuf[i]=EXTRACT16(PSHR32(exc[i],SIG_SHIFT));
   1084       for (i=0;i<st->lpcSize;i++)
   1085          st->interp_qlpc[i] = ak[i];
   1086       st->exc_rms[sub] = compute_rms16(st->excBuf, st->subframeSize);
   1087       exc_ener_sum = ADD32(exc_ener_sum, DIV32(MULT16_16(st->exc_rms[sub],st->exc_rms[sub]), st->nbSubframes));
   1088    }
   1089    st->last_ener = spx_sqrt(exc_ener_sum);
   1090 
   1091    qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
   1092    for (i=0;i<st->lpcSize;i++)
   1093       st->old_qlsp[i] = qlsp[i];
   1094 
   1095    st->first=0;
   1096 
   1097    return 0;
   1098 }
   1099 
   1100 
   1101 int sb_encoder_ctl(void *state, int request, void *ptr)
   1102 {
   1103    SBEncState *st;
   1104    st=(SBEncState*)state;
   1105    switch(request)
   1106    {
   1107    case SPEEX_GET_FRAME_SIZE:
   1108       (*(spx_int32_t*)ptr) = st->full_frame_size;
   1109       break;
   1110    case SPEEX_SET_HIGH_MODE:
   1111       st->submodeSelect = st->submodeID = (*(spx_int32_t*)ptr);
   1112       break;
   1113    case SPEEX_SET_LOW_MODE:
   1114       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
   1115       break;
   1116    case SPEEX_SET_DTX:
   1117       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
   1118       break;
   1119    case SPEEX_GET_DTX:
   1120       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
   1121       break;
   1122    case SPEEX_GET_LOW_MODE:
   1123       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
   1124       break;
   1125    case SPEEX_SET_MODE:
   1126       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
   1127       break;
   1128 #ifndef DISABLE_VBR
   1129    case SPEEX_SET_VBR:
   1130       st->vbr_enabled = (*(spx_int32_t*)ptr);
   1131       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
   1132       break;
   1133    case SPEEX_GET_VBR:
   1134       (*(spx_int32_t*)ptr) = st->vbr_enabled;
   1135       break;
   1136    case SPEEX_SET_VAD:
   1137       st->vad_enabled = (*(spx_int32_t*)ptr);
   1138       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
   1139       break;
   1140    case SPEEX_GET_VAD:
   1141       (*(spx_int32_t*)ptr) = st->vad_enabled;
   1142       break;
   1143 #endif /* #ifndef DISABLE_VBR */
   1144 #if !defined(DISABLE_VBR) && !defined(DISABLE_FLOAT_API)
   1145    case SPEEX_SET_VBR_QUALITY:
   1146       {
   1147          spx_int32_t q;
   1148          float qual = (*(float*)ptr)+.6;
   1149          st->vbr_quality = (*(float*)ptr);
   1150          if (qual>10)
   1151             qual=10;
   1152          q=(int)floor(.5+*(float*)ptr);
   1153          if (q>10)
   1154             q=10;
   1155          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
   1156          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
   1157          break;
   1158       }
   1159    case SPEEX_GET_VBR_QUALITY:
   1160       (*(float*)ptr) = st->vbr_quality;
   1161       break;
   1162 #endif /* #if !defined(DISABLE_VBR) && !defined(DISABLE_FLOAT_API) */
   1163 #ifndef DISABLE_VBR
   1164    case SPEEX_SET_ABR:
   1165       st->abr_enabled = (*(spx_int32_t*)ptr);
   1166       st->vbr_enabled = st->abr_enabled!=0;
   1167       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
   1168       if (st->vbr_enabled)
   1169       {
   1170          spx_int32_t i=10, rate, target;
   1171          float vbr_qual;
   1172          target = (*(spx_int32_t*)ptr);
   1173          while (i>=0)
   1174          {
   1175             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
   1176             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
   1177             if (rate <= target)
   1178                break;
   1179             i--;
   1180          }
   1181          vbr_qual=i;
   1182          if (vbr_qual<0)
   1183             vbr_qual=0;
   1184          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
   1185          st->abr_count=0;
   1186          st->abr_drift=0;
   1187          st->abr_drift2=0;
   1188       }
   1189 
   1190       break;
   1191    case SPEEX_GET_ABR:
   1192       (*(spx_int32_t*)ptr) = st->abr_enabled;
   1193       break;
   1194 #endif /* #ifndef DISABLE_VBR */
   1195 
   1196    case SPEEX_SET_QUALITY:
   1197       {
   1198          spx_int32_t nb_qual;
   1199          int quality = (*(spx_int32_t*)ptr);
   1200          if (quality < 0)
   1201             quality = 0;
   1202          if (quality > 10)
   1203             quality = 10;
   1204          st->submodeSelect = st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
   1205          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
   1206          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
   1207       }
   1208       break;
   1209    case SPEEX_SET_COMPLEXITY:
   1210       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
   1211       st->complexity = (*(spx_int32_t*)ptr);
   1212       if (st->complexity<1)
   1213          st->complexity=1;
   1214       break;
   1215    case SPEEX_GET_COMPLEXITY:
   1216       (*(spx_int32_t*)ptr) = st->complexity;
   1217       break;
   1218    case SPEEX_SET_BITRATE:
   1219       {
   1220          spx_int32_t i=10;
   1221          spx_int32_t rate, target;
   1222          target = (*(spx_int32_t*)ptr);
   1223          while (i>=0)
   1224          {
   1225             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
   1226             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
   1227             if (rate <= target)
   1228                break;
   1229             i--;
   1230          }
   1231       }
   1232       break;
   1233    case SPEEX_GET_BITRATE:
   1234       speex_encoder_ctl(st->st_low, request, ptr);
   1235       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
   1236       if (st->submodes[st->submodeID])
   1237          (*(spx_int32_t*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
   1238       else
   1239          (*(spx_int32_t*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
   1240       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
   1241       break;
   1242    case SPEEX_SET_SAMPLING_RATE:
   1243       {
   1244          spx_int32_t tmp=(*(spx_int32_t*)ptr);
   1245          st->sampling_rate = tmp;
   1246          tmp>>=1;
   1247          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
   1248       }
   1249       break;
   1250    case SPEEX_GET_SAMPLING_RATE:
   1251       (*(spx_int32_t*)ptr)=st->sampling_rate;
   1252       break;
   1253    case SPEEX_RESET_STATE:
   1254       {
   1255          int i;
   1256          st->first = 1;
   1257          for (i=0;i<st->lpcSize;i++)
   1258             st->old_lsp[i]= DIV32(MULT16_16(QCONST16(3.1415927f, LSP_SHIFT), i+1), st->lpcSize+1);
   1259          for (i=0;i<st->lpcSize;i++)
   1260             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
   1261          for (i=0;i<QMF_ORDER;i++)
   1262             st->h0_mem[i]=st->h1_mem[i]=0;
   1263       }
   1264       break;
   1265    case SPEEX_SET_SUBMODE_ENCODING:
   1266       st->encode_submode = (*(spx_int32_t*)ptr);
   1267       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, ptr);
   1268       break;
   1269    case SPEEX_GET_SUBMODE_ENCODING:
   1270       (*(spx_int32_t*)ptr) = st->encode_submode;
   1271       break;
   1272    case SPEEX_GET_LOOKAHEAD:
   1273       speex_encoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
   1274       (*(spx_int32_t*)ptr) = 2*(*(spx_int32_t*)ptr) + QMF_ORDER - 1;
   1275       break;
   1276    case SPEEX_SET_PLC_TUNING:
   1277       speex_encoder_ctl(st->st_low, SPEEX_SET_PLC_TUNING, ptr);
   1278       break;
   1279    case SPEEX_GET_PLC_TUNING:
   1280       speex_encoder_ctl(st->st_low, SPEEX_GET_PLC_TUNING, ptr);
   1281       break;
   1282 #ifndef DISABLE_VBR
   1283    case SPEEX_SET_VBR_MAX_BITRATE:
   1284       {
   1285          st->vbr_max = (*(spx_int32_t*)ptr);
   1286          if (SPEEX_SET_VBR_MAX_BITRATE<1)
   1287          {
   1288             speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_MAX_BITRATE, &st->vbr_max);
   1289             st->vbr_max_high = 17600;
   1290          } else {
   1291             spx_int32_t low_rate;
   1292             if (st->vbr_max >= 42200)
   1293             {
   1294                st->vbr_max_high = 17600;
   1295             } else if (st->vbr_max >= 27800)
   1296             {
   1297                st->vbr_max_high = 9600;
   1298             } else if (st->vbr_max > 20600)
   1299             {
   1300                st->vbr_max_high = 5600;
   1301             } else {
   1302                st->vbr_max_high = 1800;
   1303             }
   1304             if (st->subframeSize==80)
   1305                st->vbr_max_high = 1800;
   1306             low_rate = st->vbr_max - st->vbr_max_high;
   1307             speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_MAX_BITRATE, &low_rate);
   1308          }
   1309       }
   1310       break;
   1311    case SPEEX_GET_VBR_MAX_BITRATE:
   1312       (*(spx_int32_t*)ptr) = st->vbr_max;
   1313       break;
   1314 #endif /* #ifndef DISABLE_VBR */
   1315    case SPEEX_SET_HIGHPASS:
   1316       speex_encoder_ctl(st->st_low, SPEEX_SET_HIGHPASS, ptr);
   1317       break;
   1318    case SPEEX_GET_HIGHPASS:
   1319       speex_encoder_ctl(st->st_low, SPEEX_GET_HIGHPASS, ptr);
   1320       break;
   1321 
   1322 
   1323    /* This is all internal stuff past this point */
   1324    case SPEEX_GET_PI_GAIN:
   1325       {
   1326          int i;
   1327          spx_word32_t *g = (spx_word32_t*)ptr;
   1328          for (i=0;i<st->nbSubframes;i++)
   1329             g[i]=st->pi_gain[i];
   1330       }
   1331       break;
   1332    case SPEEX_GET_EXC:
   1333       {
   1334          int i;
   1335          for (i=0;i<st->nbSubframes;i++)
   1336             ((spx_word16_t*)ptr)[i] = st->exc_rms[i];
   1337       }
   1338       break;
   1339 #ifndef DISABLE_VBR
   1340    case SPEEX_GET_RELATIVE_QUALITY:
   1341       (*(float*)ptr)=st->relative_quality;
   1342       break;
   1343 #endif /* #ifndef DISABLE_VBR */
   1344    case SPEEX_SET_INNOVATION_SAVE:
   1345       st->innov_rms_save = (spx_word16_t*)ptr;
   1346       break;
   1347    case SPEEX_SET_WIDEBAND:
   1348       speex_encoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, ptr);
   1349       break;
   1350    case SPEEX_GET_STACK:
   1351       *((char**)ptr) = st->stack;
   1352       break;
   1353    default:
   1354       speex_warning_int("Unknown nb_ctl request: ", request);
   1355       return -1;
   1356    }
   1357    return 0;
   1358 }
   1359 
   1360 int sb_decoder_ctl(void *state, int request, void *ptr)
   1361 {
   1362    SBDecState *st;
   1363    st=(SBDecState*)state;
   1364    switch(request)
   1365    {
   1366    case SPEEX_SET_HIGH_MODE:
   1367       st->submodeID = (*(spx_int32_t*)ptr);
   1368       break;
   1369    case SPEEX_SET_LOW_MODE:
   1370       speex_decoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
   1371       break;
   1372    case SPEEX_GET_LOW_MODE:
   1373       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
   1374       break;
   1375    case SPEEX_GET_FRAME_SIZE:
   1376       (*(spx_int32_t*)ptr) = st->full_frame_size;
   1377       break;
   1378    case SPEEX_SET_ENH:
   1379       speex_decoder_ctl(st->st_low, request, ptr);
   1380       st->lpc_enh_enabled = *((spx_int32_t*)ptr);
   1381       break;
   1382    case SPEEX_GET_ENH:
   1383       *((spx_int32_t*)ptr) = st->lpc_enh_enabled;
   1384       break;
   1385    case SPEEX_SET_MODE:
   1386    case SPEEX_SET_QUALITY:
   1387       {
   1388          spx_int32_t nb_qual;
   1389          int quality = (*(spx_int32_t*)ptr);
   1390          if (quality < 0)
   1391             quality = 0;
   1392          if (quality > 10)
   1393             quality = 10;
   1394          st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
   1395          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
   1396          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
   1397       }
   1398       break;
   1399    case SPEEX_GET_BITRATE:
   1400       speex_decoder_ctl(st->st_low, request, ptr);
   1401       if (st->submodes[st->submodeID])
   1402          (*(spx_int32_t*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
   1403       else
   1404          (*(spx_int32_t*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
   1405       break;
   1406    case SPEEX_SET_SAMPLING_RATE:
   1407       {
   1408          spx_int32_t tmp=(*(spx_int32_t*)ptr);
   1409          st->sampling_rate = tmp;
   1410          tmp>>=1;
   1411          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
   1412       }
   1413       break;
   1414    case SPEEX_GET_SAMPLING_RATE:
   1415       (*(spx_int32_t*)ptr)=st->sampling_rate;
   1416       break;
   1417    case SPEEX_SET_HANDLER:
   1418       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
   1419       break;
   1420    case SPEEX_SET_USER_HANDLER:
   1421       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
   1422       break;
   1423    case SPEEX_RESET_STATE:
   1424       {
   1425          int i;
   1426          for (i=0;i<2*st->lpcSize;i++)
   1427             st->mem_sp[i]=0;
   1428          for (i=0;i<QMF_ORDER;i++)
   1429             st->g0_mem[i]=st->g1_mem[i]=0;
   1430          st->last_ener=0;
   1431       }
   1432       break;
   1433    case SPEEX_SET_SUBMODE_ENCODING:
   1434       st->encode_submode = (*(spx_int32_t*)ptr);
   1435       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, ptr);
   1436       break;
   1437    case SPEEX_GET_SUBMODE_ENCODING:
   1438       (*(spx_int32_t*)ptr) = st->encode_submode;
   1439       break;
   1440    case SPEEX_GET_LOOKAHEAD:
   1441       speex_decoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
   1442       (*(spx_int32_t*)ptr) = 2*(*(spx_int32_t*)ptr);
   1443       break;
   1444    case SPEEX_SET_HIGHPASS:
   1445       speex_decoder_ctl(st->st_low, SPEEX_SET_HIGHPASS, ptr);
   1446       break;
   1447    case SPEEX_GET_HIGHPASS:
   1448       speex_decoder_ctl(st->st_low, SPEEX_GET_HIGHPASS, ptr);
   1449       break;
   1450    case SPEEX_GET_ACTIVITY:
   1451       speex_decoder_ctl(st->st_low, SPEEX_GET_ACTIVITY, ptr);
   1452       break;
   1453    case SPEEX_GET_PI_GAIN:
   1454       {
   1455          int i;
   1456          spx_word32_t *g = (spx_word32_t*)ptr;
   1457          for (i=0;i<st->nbSubframes;i++)
   1458             g[i]=st->pi_gain[i];
   1459       }
   1460       break;
   1461    case SPEEX_GET_EXC:
   1462       {
   1463          int i;
   1464          for (i=0;i<st->nbSubframes;i++)
   1465             ((spx_word16_t*)ptr)[i] = st->exc_rms[i];
   1466       }
   1467       break;
   1468    case SPEEX_GET_DTX_STATUS:
   1469       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
   1470       break;
   1471    case SPEEX_SET_INNOVATION_SAVE:
   1472       st->innov_save = (spx_word16_t*)ptr;
   1473       break;
   1474    case SPEEX_SET_WIDEBAND:
   1475       speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, ptr);
   1476       break;
   1477    case SPEEX_GET_STACK:
   1478       *((char**)ptr) = st->stack;
   1479       break;
   1480    default:
   1481       speex_warning_int("Unknown nb_ctl request: ", request);
   1482       return -1;
   1483    }
   1484    return 0;
   1485 }
   1486 
   1487 #endif
   1488 
   1489