src/lib/dl/ext/speex/sb_celp.c

   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