apps/codecs/libspeex/nb_celp.c

   1 /* Copyright (C) 2002-2006 Jean-Marc Valin
   2    File: nb_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-speex.h"
  34 #endif
  35
  36 #include <math.h>
  37 #include "nb_celp.h"
  38 #include "lpc.h"
  39 #include "lsp.h"
  40 #include "ltp.h"
  41 #include "quant_lsp.h"
  42 #include "cb_search.h"
  43 #include "filters.h"
  44 #include "stack_alloc.h"
  45 #include "vq.h"
  46 #include "speex/speex_bits.h"
  47 #include "vbr.h"
  48 #include "arch.h"
  49 #include "math_approx.h"
  50 #include "os_support.h"
  51 #include "speex/speex_callbacks.h"
  52
  53 #ifdef VORBIS_PSYCHO
  54 #include "vorbis_psy.h"
  55 #endif
  56
  57 #ifndef M_PI
  58 #define M_PI           3.14159265358979323846  /* pi */
  59 #endif
  60
  61 #ifndef NULL
  62 #define NULL 0
  63 #endif
  64
  65 #define SUBMODE(x) st->submodes[st->submodeID]->x
  66
  67 /* Default size for the encoder and decoder stack (can be changed at compile time).
  68    This does not apply when using variable-size arrays or alloca. */
  69 #ifndef NB_ENC_STACK
  70 #define NB_ENC_STACK (8000*sizeof(spx_sig_t))
  71 #endif
  72
  73 #ifndef NB_DEC_STACK
  74 #define NB_DEC_STACK (4000*sizeof(spx_sig_t))
  75 #endif
  76
  77
  78 #ifdef FIXED_POINT
  79 const spx_word32_t ol_gain_table[32] ICONST_ATTR = {18900, 25150, 33468, 44536, 59265, 78865, 104946, 139653, 185838, 247297, 329081, 437913, 582736, 775454, 1031906, 1373169, 1827293, 2431601, 3235761, 4305867, 5729870, 7624808, 10146425, 13501971, 17967238, 23909222, 31816294, 42338330, 56340132, 74972501, 99766822, 132760927};
  80 const spx_word16_t exc_gain_quant_scal3_bound[7] ICONST_ATTR = {1841, 3883, 6051, 8062, 10444, 13580, 18560};
  81 const spx_word16_t exc_gain_quant_scal3[8] ICONST_ATTR = {1002, 2680, 5086, 7016, 9108, 11781, 15380, 21740};
  82 const spx_word16_t exc_gain_quant_scal1_bound[1] ICONST_ATTR = {14385};
  83 const spx_word16_t exc_gain_quant_scal1[2] ICONST_ATTR = {11546, 17224};
  84
  85 #define LSP_MARGIN 16
  86 #define LSP_DELTA1 6553
  87 #define LSP_DELTA2 1638
  88
  89 #else
  90
  91 const float exc_gain_quant_scal3_bound[7]={0.112338f, 0.236980f, 0.369316f, 0.492054f, 0.637471f, 0.828874f, 1.132784f};
  92 const float exc_gain_quant_scal3[8]={0.061130f, 0.163546f, 0.310413f, 0.428220f, 0.555887f, 0.719055f, 0.938694f, 1.326874f};
  93 const float exc_gain_quant_scal1_bound[1]={0.87798f};
  94 const float exc_gain_quant_scal1[2]={0.70469f, 1.05127f};
  95
  96 #define LSP_MARGIN .002f
  97 #define LSP_DELTA1 .2f
  98 #define LSP_DELTA2 .05f
  99
 100 #endif
 101
 102 #ifdef VORBIS_PSYCHO
 103 #define EXTRA_BUFFER 100
 104 #else
 105 #define EXTRA_BUFFER 0
 106 #endif
 107
 108
 109 #define sqr(x) ((x)*(x))
 110
 111 extern const spx_word16_t lag_window[];
 112 extern const spx_word16_t lpc_window[];
 113
 114 #ifndef SPEEX_DISABLE_ENCODER
 115 void *nb_encoder_init(const SpeexMode *m)
 116 {
 117    EncState *st;
 118    const SpeexNBMode *mode;
 119    int i;
 120
 121    mode=(const SpeexNBMode *)m->mode;
 122    st = (EncState*)speex_alloc(sizeof(EncState));
 123    if (!st)
 124       return NULL;
 125 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
 126    st->stack = NULL;
 127 #else
 128    st->stack = (char*)speex_alloc_scratch(NB_ENC_STACK);
 129 #endif
 130
 131    st->mode=m;
 132
 133    st->frameSize = mode->frameSize;
 134    st->nbSubframes=mode->frameSize/mode->subframeSize;
 135    st->subframeSize=mode->subframeSize;
 136    st->windowSize = st->frameSize+st->subframeSize;
 137    st->lpcSize = mode->lpcSize;
 138    st->gamma1=mode->gamma1;
 139    st->gamma2=mode->gamma2;
 140    st->min_pitch=mode->pitchStart;
 141    st->max_pitch=mode->pitchEnd;
 142    st->lpc_floor = mode->lpc_floor;
 143
 144    st->submodes=mode->submodes;
 145    st->submodeID=st->submodeSelect=mode->defaultSubmode;
 146    st->bounded_pitch = 1;
 147
 148    st->encode_submode = 1;
 149
 150 #ifdef VORBIS_PSYCHO
 151    st->psy = vorbis_psy_init(8000, 256);
 152    st->curve = (float*)speex_alloc(128*sizeof(float));
 153    st->old_curve = (float*)speex_alloc(128*sizeof(float));
 154    st->psy_window = (float*)speex_alloc(256*sizeof(float));
 155 #endif
 156
 157    st->cumul_gain = 1024;
 158
 159    /* Allocating input buffer */
 160    st->winBuf = (spx_word16_t*)speex_alloc((st->windowSize-st->frameSize)*sizeof(spx_word16_t));
 161    /* Allocating excitation buffer */
 162    st->excBuf = (spx_word16_t*)speex_alloc((mode->frameSize+mode->pitchEnd+2)*sizeof(spx_word16_t));
 163    st->exc = st->excBuf + mode->pitchEnd + 2;
 164    st->swBuf = (spx_word16_t*)speex_alloc((mode->frameSize+mode->pitchEnd+2)*sizeof(spx_word16_t));
 165    st->sw = st->swBuf + mode->pitchEnd + 2;
 166
 167    st->window= lpc_window;
 168
 169    /* Create the window for autocorrelation (lag-windowing) */
 170    st->lagWindow = lag_window;
 171
 172    st->old_lsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
 173    st->old_qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
 174    st->first = 1;
 175    for (i=0;i<st->lpcSize;i++)
 176       st->old_lsp[i]= DIV32(MULT16_16(QCONST16(3.1415927f, LSP_SHIFT), i+1), st->lpcSize+1);
 177
 178    st->mem_sp = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
 179    st->mem_sw = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
 180    st->mem_sw_whole = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
 181    st->mem_exc = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
 182    st->mem_exc2 = (spx_mem_t*)speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
 183
 184    st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
 185    st->innov_rms_save = NULL;
 186
 187    st->pitch = (int*)speex_alloc((st->nbSubframes)*sizeof(int));
 188
 189 #ifndef DISABLE_VBR
 190    st->vbr = (VBRState*)speex_alloc(sizeof(VBRState));
 191    vbr_init(st->vbr);
 192    st->vbr_quality = 8;
 193    st->vbr_enabled = 0;
 194    st->vbr_max = 0;
 195    st->vad_enabled = 0;
 196    st->dtx_enabled = 0;
 197    st->dtx_count=0;
 198    st->abr_enabled = 0;
 199    st->abr_drift = 0;
 200    st->abr_drift2 = 0;
 201 #endif /* #ifndef DISABLE_VBR */
 202
 203    st->plc_tuning = 2;
 204    st->complexity=2;
 205    st->sampling_rate=8000;
 206    st->isWideband = 0;
 207    st->highpass_enabled = 1;
 208
 209 #ifdef ENABLE_VALGRIND
 210    VALGRIND_MAKE_READABLE(st, NB_ENC_STACK);
 211 #endif
 212    return st;
 213 }
 214
 215 void nb_encoder_destroy(void *state)
 216 {
 217    EncState *st=(EncState *)state;
 218    /* Free all allocated memory */
 219 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
 220    speex_free_scratch(st->stack);
 221 #endif
 222
 223    speex_free (st->winBuf);
 224    speex_free (st->excBuf);
 225    speex_free (st->old_qlsp);
 226    speex_free (st->swBuf);
 227
 228    speex_free (st->old_lsp);
 229    speex_free (st->mem_sp);
 230    speex_free (st->mem_sw);
 231    speex_free (st->mem_sw_whole);
 232    speex_free (st->mem_exc);
 233    speex_free (st->mem_exc2);
 234    speex_free (st->pi_gain);
 235    speex_free (st->pitch);
 236
 237 #ifndef DISABLE_VBR
 238    vbr_destroy(st->vbr);
 239    speex_free (st->vbr);
 240 #endif /* #ifndef DISABLE_VBR */
 241
 242 #ifdef VORBIS_PSYCHO
 243    vorbis_psy_destroy(st->psy);
 244    speex_free (st->curve);
 245    speex_free (st->old_curve);
 246    speex_free (st->psy_window);
 247 #endif
 248
 249    /*Free state memory... should be last*/
 250    speex_free(st);
 251 }
 252
 253 int nb_encode(void *state, void *vin, SpeexBits *bits)
 254 {
 255    EncState *st;
 256    int i, sub, roots;
 257    int ol_pitch;
 258    spx_word16_t ol_pitch_coef;
 259    spx_word32_t ol_gain;
 260    VARDECL(spx_word16_t *ringing);
 261    VARDECL(spx_word16_t *target);
 262    VARDECL(spx_sig_t *innov);
 263    VARDECL(spx_word32_t *exc32);
 264    VARDECL(spx_mem_t *mem);
 265    VARDECL(spx_coef_t *bw_lpc1);
 266    VARDECL(spx_coef_t *bw_lpc2);
 267    VARDECL(spx_coef_t *lpc);
 268    VARDECL(spx_lsp_t *lsp);
 269    VARDECL(spx_lsp_t *qlsp);
 270    VARDECL(spx_lsp_t *interp_lsp);
 271    VARDECL(spx_lsp_t *interp_qlsp);
 272    VARDECL(spx_coef_t *interp_lpc);
 273    VARDECL(spx_coef_t *interp_qlpc);
 274    char *stack;
 275    VARDECL(spx_word16_t *syn_resp);
 276    VARDECL(spx_word16_t *real_exc);
 277
 278    spx_word32_t ener=0;
 279    spx_word16_t fine_gain;
 280    spx_word16_t *in = (spx_word16_t*)vin;
 281
 282    st=(EncState *)state;
 283    stack=st->stack;
 284
 285    ALLOC(lpc, st->lpcSize, spx_coef_t);
 286    ALLOC(bw_lpc1, st->lpcSize, spx_coef_t);
 287    ALLOC(bw_lpc2, st->lpcSize, spx_coef_t);
 288    ALLOC(lsp, st->lpcSize, spx_lsp_t);
 289    ALLOC(qlsp, st->lpcSize, spx_lsp_t);
 290    ALLOC(interp_lsp, st->lpcSize, spx_lsp_t);
 291    ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
 292    ALLOC(interp_lpc, st->lpcSize, spx_coef_t);
 293    ALLOC(interp_qlpc, st->lpcSize, spx_coef_t);
 294
 295    /* Move signals 1 frame towards the past */
 296    SPEEX_MOVE(st->excBuf, st->excBuf+st->frameSize, st->max_pitch+2);
 297    SPEEX_MOVE(st->swBuf, st->swBuf+st->frameSize, st->max_pitch+2);
 298
 299    if (st->highpass_enabled)
 300       highpass(in, in, st->frameSize, (st->isWideband?HIGHPASS_WIDEBAND:HIGHPASS_NARROWBAND)|HIGHPASS_INPUT, st->mem_hp);
 301
 302    {
 303       VARDECL(spx_word16_t *w_sig);
 304       VARDECL(spx_word16_t *autocorr);
 305       ALLOC(w_sig, st->windowSize, spx_word16_t);
 306       ALLOC(autocorr, st->lpcSize+1, spx_word16_t);
 307       /* Window for analysis */
 308       for (i=0;i<st->windowSize-st->frameSize;i++)
 309          w_sig[i] = EXTRACT16(SHR32(MULT16_16(st->winBuf[i],st->window[i]),SIG_SHIFT));
 310       for (;i<st->windowSize;i++)
 311          w_sig[i] = EXTRACT16(SHR32(MULT16_16(in[i-st->windowSize+st->frameSize],st->window[i]),SIG_SHIFT));
 312       /* Compute auto-correlation */
 313       _spx_autocorr(w_sig, autocorr, st->lpcSize+1, st->windowSize);
 314       autocorr[0] = ADD16(autocorr[0],MULT16_16_Q15(autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
 315
 316       /* Lag windowing: equivalent to filtering in the power-spectrum domain */
 317       for (i=0;i<st->lpcSize+1;i++)
 318          autocorr[i] = MULT16_16_Q14(autocorr[i],st->lagWindow[i]);
 319
 320       /* Levinson-Durbin */
 321       _spx_lpc(lpc, autocorr, st->lpcSize);
 322       /* LPC to LSPs (x-domain) transform */
 323       roots=lpc_to_lsp (lpc, st->lpcSize, lsp, 10, LSP_DELTA1, stack);
 324       /* Check if we found all the roots */
 325       if (roots!=st->lpcSize)
 326       {
 327          /*If we can't find all LSP's, do some damage control and use previous filter*/
 328          for (i=0;i<st->lpcSize;i++)
 329          {
 330             lsp[i]=st->old_lsp[i];
 331          }
 332       }
 333    }
 334
 335
 336
 337
 338    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
 339    {
 340       int diff = st->windowSize-st->frameSize;
 341       if (st->first)
 342          for (i=0;i<st->lpcSize;i++)
 343             interp_lsp[i] = lsp[i];
 344       else
 345          lsp_interpolate(st->old_lsp, lsp, interp_lsp, st->lpcSize, st->nbSubframes, st->nbSubframes<<1);
 346
 347       lsp_enforce_margin(interp_lsp, st->lpcSize, LSP_MARGIN);
 348
 349       /* Compute interpolated LPCs (unquantized) for whole frame*/
 350       lsp_to_lpc(interp_lsp, interp_lpc, st->lpcSize,stack);
 351
 352
 353       /*Open-loop pitch*/
 354       if (!st->submodes[st->submodeID] || (st->complexity>2 && SUBMODE(have_subframe_gain)<3) || SUBMODE(forced_pitch_gain) || SUBMODE(lbr_pitch) != -1
 355 #ifndef DISABLE_VBR
 356            || st->vbr_enabled || st->vad_enabled
 357 #endif
 358                   )
 359       {
 360          int nol_pitch[6];
 361          spx_word16_t nol_pitch_coef[6];
 362
 363          bw_lpc(st->gamma1, interp_lpc, bw_lpc1, st->lpcSize);
 364          bw_lpc(st->gamma2, interp_lpc, bw_lpc2, st->lpcSize);
 365
 366          SPEEX_COPY(st->sw, st->winBuf, diff);
 367          SPEEX_COPY(st->sw+diff, in, st->frameSize-diff);
 368          filter_mem16(st->sw, bw_lpc1, bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole, stack);
 369
 370          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize,
 371                                nol_pitch, nol_pitch_coef, 6, stack);
 372          ol_pitch=nol_pitch[0];
 373          ol_pitch_coef = nol_pitch_coef[0];
 374          /*Try to remove pitch multiples*/
 375          for (i=1;i<6;i++)
 376          {
 377 #ifdef FIXED_POINT
 378             if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],27853)) &&
 379 #else
 380             if ((nol_pitch_coef[i]>.85*nol_pitch_coef[0]) &&
 381 #endif
 382                 (ABS(2*nol_pitch[i]-ol_pitch)<=2 || ABS(3*nol_pitch[i]-ol_pitch)<=3 ||
 383                  ABS(4*nol_pitch[i]-ol_pitch)<=4 || ABS(5*nol_pitch[i]-ol_pitch)<=5))
 384             {
 385                /*ol_pitch_coef=nol_pitch_coef[i];*/
 386                ol_pitch = nol_pitch[i];
 387             }
 388          }
 389          /*if (ol_pitch>50)
 390            ol_pitch/=2;*/
 391          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
 392
 393       } else {
 394          ol_pitch=0;
 395          ol_pitch_coef=0;
 396       }
 397
 398       /*Compute "real" excitation*/
 399       SPEEX_COPY(st->exc, st->winBuf, diff);
 400       SPEEX_COPY(st->exc+diff, in, st->frameSize-diff);
 401       fir_mem16(st->exc, interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc, stack);
 402
 403       /* Compute open-loop excitation gain */
 404       {
 405          spx_word16_t g = compute_rms16(st->exc, st->frameSize);
 406          if (st->submodeID!=1 && ol_pitch>0)
 407             ol_gain = MULT16_16(g, MULT16_16_Q14(QCONST16(1.1,14),
 408                                 spx_sqrt(QCONST32(1.,28)-MULT16_32_Q15(QCONST16(.8,15),SHL32(MULT16_16(ol_pitch_coef,ol_pitch_coef),16)))));
 409          else
 410             ol_gain = SHL32(EXTEND32(g),SIG_SHIFT);
 411       }
 412    }
 413
 414 #ifdef VORBIS_PSYCHO
 415    SPEEX_MOVE(st->psy_window, st->psy_window+st->frameSize, 256-st->frameSize);
 416    SPEEX_COPY(&st->psy_window[256-st->frameSize], in, st->frameSize);
 417    compute_curve(st->psy, st->psy_window, st->curve);
 418    /*print_vec(st->curve, 128, "curve");*/
 419    if (st->first)
 420       SPEEX_COPY(st->old_curve, st->curve, 128);
 421 #endif
 422
 423    /*VBR stuff*/
 424 #ifndef DISABLE_VBR
 425    if (st->vbr && (st->vbr_enabled||st->vad_enabled))
 426    {
 427       float lsp_dist=0;
 428       for (i=0;i<st->lpcSize;i++)
 429          lsp_dist += (st->old_lsp[i] - lsp[i])*(st->old_lsp[i] - lsp[i]);
 430       lsp_dist /= LSP_SCALING*LSP_SCALING;
 431
 432       if (st->abr_enabled)
 433       {
 434          float qual_change=0;
 435          if (st->abr_drift2 * st->abr_drift > 0)
 436          {
 437             /* Only adapt if long-term and short-term drift are the same sign */
 438             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
 439             if (qual_change>.05)
 440                qual_change=.05;
 441             if (qual_change<-.05)
 442                qual_change=-.05;
 443          }
 444          st->vbr_quality += qual_change;
 445          if (st->vbr_quality>10)
 446             st->vbr_quality=10;
 447          if (st->vbr_quality<0)
 448             st->vbr_quality=0;
 449       }
 450
 451       st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, GAIN_SCALING_1*ol_pitch_coef);
 452       /*if (delta_qual<0)*/
 453       /*  delta_qual*=.1*(3+st->vbr_quality);*/
 454       if (st->vbr_enabled)
 455       {
 456          spx_int32_t mode;
 457          int choice=0;
 458          float min_diff=100;
 459          mode = 8;
 460          while (mode)
 461          {
 462             int v1;
 463             float thresh;
 464             v1=(int)floor(st->vbr_quality);
 465             if (v1==10)
 466                thresh = vbr_nb_thresh[mode][v1];
 467             else
 468                thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
 469             if (st->relative_quality > thresh &&
 470                 st->relative_quality-thresh<min_diff)
 471             {
 472                choice = mode;
 473                min_diff = st->relative_quality-thresh;
 474             }
 475             mode--;
 476          }
 477          mode=choice;
 478          if (mode==0)
 479          {
 480             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
 481             {
 482                mode=1;
 483                st->dtx_count=1;
 484             } else {
 485                mode=0;
 486                st->dtx_count++;
 487             }
 488          } else {
 489             st->dtx_count=0;
 490          }
 491
 492          speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
 493          if (st->vbr_max>0)
 494          {
 495             spx_int32_t rate;
 496             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &rate);
 497             if (rate > st->vbr_max)
 498             {
 499                rate = st->vbr_max;
 500                speex_encoder_ctl(state, SPEEX_SET_BITRATE, &rate);
 501             }
 502          }
 503
 504          if (st->abr_enabled)
 505          {
 506             spx_int32_t bitrate;
 507             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
 508             st->abr_drift+=(bitrate-st->abr_enabled);
 509             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
 510             st->abr_count += 1.0;
 511          }
 512
 513       } else {
 514          /*VAD only case*/
 515          int mode;
 516          if (st->relative_quality<2)
 517          {
 518             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
 519             {
 520                st->dtx_count=1;
 521                mode=1;
 522             } else {
 523                mode=0;
 524                st->dtx_count++;
 525             }
 526          } else {
 527             st->dtx_count = 0;
 528             mode=st->submodeSelect;
 529          }
 530          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
 531          st->submodeID=mode;
 532       }
 533    } else {
 534       st->relative_quality = -1;
 535    }
 536 #endif /* #ifndef DISABLE_VBR */
 537
 538    if (st->encode_submode)
 539    {
 540       /* First, transmit a zero for narrowband */
 541       speex_bits_pack(bits, 0, 1);
 542
 543       /* Transmit the sub-mode we use for this frame */
 544       speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
 545
 546    }
 547
 548    /* If null mode (no transmission), just set a couple things to zero*/
 549    if (st->submodes[st->submodeID] == NULL)
 550    {
 551       for (i=0;i<st->frameSize;i++)
 552          st->exc[i]=st->sw[i]=VERY_SMALL;
 553
 554       for (i=0;i<st->lpcSize;i++)
 555          st->mem_sw[i]=0;
 556       st->first=1;
 557       st->bounded_pitch = 1;
 558
 559       SPEEX_COPY(st->winBuf, in+2*st->frameSize-st->windowSize, st->windowSize-st->frameSize);
 560
 561       /* Clear memory (no need to really compute it) */
 562       for (i=0;i<st->lpcSize;i++)
 563          st->mem_sp[i] = 0;
 564       return 0;
 565
 566    }
 567
 568    /* LSP Quantization */
 569    if (st->first)
 570    {
 571       for (i=0;i<st->lpcSize;i++)
 572          st->old_lsp[i] = lsp[i];
 573    }
 574
 575
 576    /*Quantize LSPs*/
 577 #if 1 /*0 for unquantized*/
 578    SUBMODE(lsp_quant)(lsp, qlsp, st->lpcSize, bits);
 579 #else
 580    for (i=0;i<st->lpcSize;i++)
 581      qlsp[i]=lsp[i];
 582 #endif
 583
 584    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
 585    if (SUBMODE(lbr_pitch)!=-1)
 586    {
 587       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
 588    }
 589
 590    if (SUBMODE(forced_pitch_gain))
 591    {
 592       int quant;
 593       /* This just damps the pitch a bit, because it tends to be too aggressive when forced */
 594       ol_pitch_coef = MULT16_16_Q15(QCONST16(.9,15), ol_pitch_coef);
 595 #ifdef FIXED_POINT
 596       quant = PSHR16(MULT16_16_16(15, ol_pitch_coef),GAIN_SHIFT);
 597 #else
 598       quant = (int)floor(.5+15*ol_pitch_coef*GAIN_SCALING_1);
 599 #endif
 600       if (quant>15)
 601          quant=15;
 602       if (quant<0)
 603          quant=0;
 604       speex_bits_pack(bits, quant, 4);
 605       ol_pitch_coef=MULT16_16_P15(QCONST16(0.066667,15),SHL16(quant,GAIN_SHIFT));
 606    }
 607
 608
 609    /*Quantize and transmit open-loop excitation gain*/
 610 #ifdef FIXED_POINT
 611    {
 612       int qe = scal_quant32(ol_gain, ol_gain_table, 32);
 613       /*ol_gain = exp(qe/3.5)*SIG_SCALING;*/
 614       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
 615       speex_bits_pack(bits, qe, 5);
 616    }
 617 #else
 618    {
 619       int qe = (int)(floor(.5+3.5*log(ol_gain*1.0/SIG_SCALING)));
 620       if (qe<0)
 621          qe=0;
 622       if (qe>31)
 623          qe=31;
 624       ol_gain = exp(qe/3.5)*SIG_SCALING;
 625       speex_bits_pack(bits, qe, 5);
 626    }
 627 #endif
 628
 629
 630
 631    /* Special case for first frame */
 632    if (st->first)
 633    {
 634       for (i=0;i<st->lpcSize;i++)
 635          st->old_qlsp[i] = qlsp[i];
 636    }
 637
 638    /* Target signal */
 639    ALLOC(target, st->subframeSize, spx_word16_t);
 640    ALLOC(innov, st->subframeSize, spx_sig_t);
 641    ALLOC(exc32, st->subframeSize, spx_word32_t);
 642    ALLOC(ringing, st->subframeSize, spx_word16_t);
 643    ALLOC(syn_resp, st->subframeSize, spx_word16_t);
 644    ALLOC(real_exc, st->subframeSize, spx_word16_t);
 645    ALLOC(mem, st->lpcSize, spx_mem_t);
 646
 647    /* Loop on sub-frames */
 648    for (sub=0;sub<st->nbSubframes;sub++)
 649    {
 650       int   offset;
 651       spx_word16_t *sw;
 652       spx_word16_t *exc;
 653       int pitch;
 654       int response_bound = st->subframeSize;
 655
 656       /* Offset relative to start of frame */
 657       offset = st->subframeSize*sub;
 658       /* Excitation */
 659       exc=st->exc+offset;
 660       /* Weighted signal */
 661       sw=st->sw+offset;
 662
 663       /* LSP interpolation (quantized and unquantized) */
 664       lsp_interpolate(st->old_lsp, lsp, interp_lsp, st->lpcSize, sub, st->nbSubframes);
 665       lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);
 666
 667       /* Make sure the filters are stable */
 668       lsp_enforce_margin(interp_lsp, st->lpcSize, LSP_MARGIN);
 669       lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);
 670
 671       /* Compute interpolated LPCs (quantized and unquantized) */
 672       lsp_to_lpc(interp_lsp, interp_lpc, st->lpcSize,stack);
 673
 674       lsp_to_lpc(interp_qlsp, interp_qlpc, st->lpcSize, stack);
 675
 676       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
 677       {
 678          spx_word32_t pi_g=LPC_SCALING;
 679          for (i=0;i<st->lpcSize;i+=2)
 680          {
 681             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
 682             pi_g = ADD32(pi_g, SUB32(EXTEND32(interp_qlpc[i+1]),EXTEND32(interp_qlpc[i])));
 683          }
 684          st->pi_gain[sub] = pi_g;
 685       }
 686
 687 #ifdef VORBIS_PSYCHO
 688       {
 689          float curr_curve[128];
 690          float fact = ((float)sub+1.0f)/st->nbSubframes;
 691          for (i=0;i<128;i++)
 692             curr_curve[i] = (1.0f-fact)*st->old_curve[i] + fact*st->curve[i];
 693          curve_to_lpc(st->psy, curr_curve, bw_lpc1, bw_lpc2, 10);
 694       }
 695 #else
 696       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
 697       bw_lpc(st->gamma1, interp_lpc, bw_lpc1, st->lpcSize);
 698       if (st->gamma2>=0)
 699          bw_lpc(st->gamma2, interp_lpc, bw_lpc2, st->lpcSize);
 700       else
 701       {
 702          for (i=0;i<st->lpcSize;i++)
 703             bw_lpc2[i]=0;
 704       }
 705       /*print_vec(st->bw_lpc1, 10, "bw_lpc");*/
 706 #endif
 707
 708       /*FIXME: This will break if we change the window size */
 709       speex_assert(st->windowSize-st->frameSize == st->subframeSize);
 710       if (sub==0)
 711       {
 712          for (i=0;i<st->subframeSize;i++)
 713             real_exc[i] = sw[i] = st->winBuf[i];
 714       } else {
 715          for (i=0;i<st->subframeSize;i++)
 716             real_exc[i] = sw[i] = in[i+((sub-1)*st->subframeSize)];
 717       }
 718       fir_mem16(real_exc, interp_qlpc, real_exc, st->subframeSize, st->lpcSize, st->mem_exc2, stack);
 719
 720       if (st->complexity==0)
 721          response_bound >>= 1;
 722       compute_impulse_response(interp_qlpc, bw_lpc1, bw_lpc2, syn_resp, response_bound, st->lpcSize, stack);
 723       for (i=response_bound;i<st->subframeSize;i++)
 724          syn_resp[i]=VERY_SMALL;
 725
 726       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
 727       for (i=0;i<st->lpcSize;i++)
 728          mem[i]=SHL32(st->mem_sp[i],1);
 729       for (i=0;i<st->subframeSize;i++)
 730          ringing[i] = VERY_SMALL;
 731 #ifdef SHORTCUTS2
 732       iir_mem16(ringing, interp_qlpc, ringing, response_bound, st->lpcSize, mem, stack);
 733       for (i=0;i<st->lpcSize;i++)
 734          mem[i]=SHL32(st->mem_sw[i],1);
 735       filter_mem16(ringing, st->bw_lpc1, st->bw_lpc2, ringing, response_bound, st->lpcSize, mem, stack);
 736       SPEEX_MEMSET(&ringing[response_bound], 0, st->subframeSize-response_bound);
 737 #else
 738       iir_mem16(ringing, interp_qlpc, ringing, st->subframeSize, st->lpcSize, mem, stack);
 739       for (i=0;i<st->lpcSize;i++)
 740          mem[i]=SHL32(st->mem_sw[i],1);
 741       filter_mem16(ringing, bw_lpc1, bw_lpc2, ringing, st->subframeSize, st->lpcSize, mem, stack);
 742 #endif
 743
 744       /* Compute weighted signal */
 745       for (i=0;i<st->lpcSize;i++)
 746          mem[i]=st->mem_sw[i];
 747       filter_mem16(sw, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, mem, stack);
 748
 749       if (st->complexity==0)
 750          for (i=0;i<st->lpcSize;i++)
 751             st->mem_sw[i]=mem[i];
 752
 753       /* Compute target signal (saturation prevents overflows on clipped input speech) */
 754       for (i=0;i<st->subframeSize;i++)
 755          target[i]=EXTRACT16(SATURATE(SUB32(sw[i],PSHR32(ringing[i],1)),32767));
 756
 757       /* Reset excitation */
 758       SPEEX_MEMSET(exc, 0, st->subframeSize);
 759
 760       /* If we have a long-term predictor (otherwise, something's wrong) */
 761       speex_assert (SUBMODE(ltp_quant));
 762       {
 763          int pit_min, pit_max;
 764          /* Long-term prediction */
 765          if (SUBMODE(lbr_pitch) != -1)
 766          {
 767             /* Low bit-rate pitch handling */
 768             int margin;
 769             margin = SUBMODE(lbr_pitch);
 770             if (margin)
 771             {
 772                if (ol_pitch < st->min_pitch+margin-1)
 773                   ol_pitch=st->min_pitch+margin-1;
 774                if (ol_pitch > st->max_pitch-margin)
 775                   ol_pitch=st->max_pitch-margin;
 776                pit_min = ol_pitch-margin+1;
 777                pit_max = ol_pitch+margin;
 778             } else {
 779                pit_min=pit_max=ol_pitch;
 780             }
 781          } else {
 782             pit_min = st->min_pitch;
 783             pit_max = st->max_pitch;
 784          }
 785
 786          /* Force pitch to use only the current frame if needed */
 787          if (st->bounded_pitch && pit_max>offset)
 788             pit_max=offset;
 789
 790          /* Perform pitch search */
 791          pitch = SUBMODE(ltp_quant)(target, sw, interp_qlpc, bw_lpc1, bw_lpc2,
 792                                     exc32, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
 793                                     st->lpcSize, st->subframeSize, bits, stack,
 794                                     exc, syn_resp, st->complexity, 0, st->plc_tuning, &st->cumul_gain);
 795
 796          st->pitch[sub]=pitch;
 797       }
 798       /* Quantization of innovation */
 799       SPEEX_MEMSET(innov, 0, st->subframeSize);
 800
 801       /* FIXME: Make sure this is save from overflows (so far so good) */
 802       for (i=0;i<st->subframeSize;i++)
 803          real_exc[i] = EXTRACT16(SUB32(EXTEND32(real_exc[i]), PSHR32(exc32[i],SIG_SHIFT-1)));
 804
 805       ener = SHL32(EXTEND32(compute_rms16(real_exc, st->subframeSize)),SIG_SHIFT);
 806
 807       /*FIXME: Should use DIV32_16 and make sure result fits in 16 bits */
 808 #ifdef FIXED_POINT
 809       {
 810          spx_word32_t f = PDIV32(ener,PSHR32(ol_gain,SIG_SHIFT));
 811          if (f<=32767)
 812             fine_gain = f;
 813          else
 814             fine_gain = 32767;
 815       }
 816 #else
 817       fine_gain = PDIV32_16(ener,PSHR32(ol_gain,SIG_SHIFT));
 818 #endif
 819       /* Calculate gain correction for the sub-frame (if any) */
 820       if (SUBMODE(have_subframe_gain))
 821       {
 822          int qe;
 823          if (SUBMODE(have_subframe_gain)==3)
 824          {
 825             qe = scal_quant(fine_gain, exc_gain_quant_scal3_bound, 8);
 826             speex_bits_pack(bits, qe, 3);
 827             ener=MULT16_32_Q14(exc_gain_quant_scal3[qe],ol_gain);
 828          } else {
 829             qe = scal_quant(fine_gain, exc_gain_quant_scal1_bound, 2);
 830             speex_bits_pack(bits, qe, 1);
 831             ener=MULT16_32_Q14(exc_gain_quant_scal1[qe],ol_gain);
 832          }
 833       } else {
 834          ener=ol_gain;
 835       }
 836
 837       /*printf ("%f %f\n", ener, ol_gain);*/
 838
 839       /* Normalize innovation */
 840       signal_div(target, target, ener, st->subframeSize);
 841
 842       /* Quantize innovation */
 843       speex_assert (SUBMODE(innovation_quant));
 844       {
 845          /* Codebook search */
 846          SUBMODE(innovation_quant)(target, interp_qlpc, bw_lpc1, bw_lpc2,
 847                   SUBMODE(innovation_params), st->lpcSize, st->subframeSize,
 848                   innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
 849
 850          /* De-normalize innovation and update excitation */
 851          signal_mul(innov, innov, ener, st->subframeSize);
 852
 853          for (i=0;i<st->subframeSize;i++)
 854             exc[i] = EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));
 855
 856          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
 857          if (SUBMODE(double_codebook)) {
 858             char *tmp_stack=stack;
 859             VARDECL(spx_sig_t *innov2);
 860             ALLOC(innov2, st->subframeSize, spx_sig_t);
 861             SPEEX_MEMSET(innov2, 0, st->subframeSize);
 862             for (i=0;i<st->subframeSize;i++)
 863                target[i]=MULT16_16_P13(QCONST16(2.2f,13), target[i]);
 864             SUBMODE(innovation_quant)(target, interp_qlpc, bw_lpc1, bw_lpc2,
 865                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize,
 866                                       innov2, syn_resp, bits, stack, st->complexity, 0);
 867             signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545f,15),ener), st->subframeSize);
 868             for (i=0;i<st->subframeSize;i++)
 869                innov[i] = ADD32(innov[i],innov2[i]);
 870             stack = tmp_stack;
 871          }
 872          for (i=0;i<st->subframeSize;i++)
 873             exc[i] = EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));
 874          if (st->innov_rms_save)
 875          {
 876             st->innov_rms_save[sub] = compute_rms(innov, st->subframeSize);
 877          }
 878       }
 879
 880       /* Final signal synthesis from excitation */
 881       iir_mem16(exc, interp_qlpc, sw, st->subframeSize, st->lpcSize, st->mem_sp, stack);
 882
 883       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
 884       if (st->complexity!=0)
 885          filter_mem16(sw, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw, stack);
 886
 887    }
 888
 889    /* Store the LSPs for interpolation in the next frame */
 890    if (st->submodeID>=1)
 891    {
 892       for (i=0;i<st->lpcSize;i++)
 893          st->old_lsp[i] = lsp[i];
 894       for (i=0;i<st->lpcSize;i++)
 895          st->old_qlsp[i] = qlsp[i];
 896    }
 897
 898 #ifdef VORBIS_PSYCHO
 899    if (st->submodeID>=1)
 900       SPEEX_COPY(st->old_curve, st->curve, 128);
 901 #endif
 902
 903    if (st->submodeID==1)
 904    {
 905 #ifndef DISABLE_VBR
 906       if (st->dtx_count)
 907          speex_bits_pack(bits, 15, 4);
 908       else
 909 #endif
 910          speex_bits_pack(bits, 0, 4);
 911    }
 912
 913    /* The next frame will not be the first (Duh!) */
 914    st->first = 0;
 915    SPEEX_COPY(st->winBuf, in+2*st->frameSize-st->windowSize, st->windowSize-st->frameSize);
 916
 917    if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
 918       st->bounded_pitch = 1;
 919    else
 920       st->bounded_pitch = 0;
 921
 922    return 1;
 923 }
 924 #endif /* SPEEX_DISABLE_ENCODER */
 925
 926 static DecState global_decstate IBSS_ATTR;
 927
 928 void *nb_decoder_init(const SpeexMode *m)
 929 {
 930    DecState *st = &global_decstate;
 931    const SpeexNBMode *mode;
 932    int i;
 933
 934    mode=(const SpeexNBMode*)m->mode;
 935 /*
 936    st = (DecState *)speex_alloc(sizeof(DecState));
 937    if (!st)
 938       return NULL;
 939 */
 940    memset(st, 0, sizeof(*st));
 941 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
 942    st->stack = NULL;
 943 #else
 944    st->stack = (char*)speex_alloc_scratch(NB_DEC_STACK);
 945 #endif
 946
 947    st->mode=m;
 948
 949
 950    st->encode_submode = 1;
 951
 952    st->first=1;
 953    /* Codec parameters, should eventually have several "modes"*/
 954    st->frameSize = mode->frameSize;
 955    st->nbSubframes=mode->frameSize/mode->subframeSize;
 956    st->subframeSize=mode->subframeSize;
 957    st->lpcSize = mode->lpcSize;
 958    st->min_pitch=mode->pitchStart;
 959    st->max_pitch=mode->pitchEnd;
 960
 961    st->submodes=mode->submodes;
 962    st->submodeID=mode->defaultSubmode;
 963
 964    st->lpc_enh_enabled=1;
 965
 966    /* st->excBuf = (spx_word16_t*)speex_alloc((st->frameSize + 2*st->max_pitch + st->subframeSize + 12)*sizeof(spx_word16_t)); */
 967    st->exc = st->excBuf + 2*st->max_pitch + st->subframeSize + 6;
 968    SPEEX_MEMSET(st->excBuf, 0, st->frameSize + st->max_pitch);
 969
 970    /* st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t)); */
 971    /* st->old_qlsp = (spx_lsp_t*)speex_alloc(st->lpcSize*sizeof(spx_lsp_t)); */
 972    /* st->mem_sp = (spx_mem_t*)speex_alloc(st->lpcSize*sizeof(spx_mem_t)); */
 973    /* st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t)); */
 974    st->last_pitch = 40;
 975    st->count_lost=0;
 976    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
 977    st->pitch_gain_buf_idx = 0;
 978    st->seed = 1000;
 979
 980    st->sampling_rate=8000;
 981    st->last_ol_gain = 0;
 982
 983    st->user_callback.func = &speex_default_user_handler;
 984    st->user_callback.data = NULL;
 985    for (i=0;i<16;i++)
 986       st->speex_callbacks[i].func = NULL;
 987
 988    st->voc_m1=st->voc_m2=st->voc_mean=0;
 989    st->voc_offset=0;
 990    st->dtx_enabled=0;
 991    st->isWideband = 0;
 992    st->highpass_enabled = 1;
 993
 994 #ifdef CPU_COLDFIRE
 995    coldfire_set_macsr(0); // Integer mode
 996 #endif
 997 #ifdef ENABLE_VALGRIND
 998    VALGRIND_MAKE_READABLE(st, NB_DEC_STACK);
 999 #endif
1000    return st;
1001 }
1002
1003 void nb_decoder_destroy(void *state)
1004 {
1005    DecState *st;
1006    st=(DecState*)state;
1007
1008 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
1009    speex_free_scratch(st->stack);
1010 #endif
1011 /*
1012    speex_free (st->excBuf);
1013    speex_free (st->interp_qlpc);
1014    speex_free (st->old_qlsp);
1015    speex_free (st->mem_sp);
1016    speex_free (st->pi_gain);
1017
1018    speex_free(state);
1019 */
1020 }
1021
1022 #define median3(a, b, c)    ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
1023
1024 #ifdef FIXED_POINT
1025 const spx_word16_t attenuation[10] = {32767, 31483, 27923, 22861, 17278, 12055, 7764, 4616, 2533, 1283};
1026 #else
1027 const spx_word16_t attenuation[10] = {1., 0.961, 0.852, 0.698, 0.527, 0.368, 0.237, 0.141, 0.077, 0.039};
1028
1029 #endif
1030
1031 #ifndef ROCKBOX_VOICE_CODEC
1032 static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
1033 {
1034    int i;
1035    int pitch_val;
1036    spx_word16_t pitch_gain;
1037    spx_word16_t fact;
1038    spx_word16_t gain_med;
1039    spx_word16_t innov_gain;
1040    spx_word16_t noise_gain;
1041
1042    if (st->count_lost<10)
1043       fact = attenuation[st->count_lost];
1044    else
1045       fact = 0;
1046
1047    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
1048    if (gain_med < st->last_pitch_gain)
1049       st->last_pitch_gain = gain_med;
1050
1051 #ifdef FIXED_POINT
1052    pitch_gain = st->last_pitch_gain;
1053    if (pitch_gain>54)
1054       pitch_gain = 54;
1055    pitch_gain = SHL16(pitch_gain, 9);
1056 #else
1057    pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
1058    if (pitch_gain>.85)
1059       pitch_gain=.85;
1060 #endif
1061    pitch_gain = MULT16_16_Q15(fact,pitch_gain) + VERY_SMALL;
1062    /* FIXME: This was rms of innovation (not exc) */
1063    innov_gain = compute_rms16(st->exc, st->frameSize);
1064    noise_gain = MULT16_16_Q15(innov_gain, MULT16_16_Q15(fact, SUB16(Q15ONE,MULT16_16_Q15(pitch_gain,pitch_gain))));
1065    /* Shift all buffers by one frame */
1066    SPEEX_MOVE(st->excBuf, st->excBuf+st->frameSize, 2*st->max_pitch + st->subframeSize + 12);
1067
1068
1069    pitch_val = st->last_pitch + SHR32((spx_int32_t)speex_rand(1+st->count_lost, &st->seed),SIG_SHIFT);
1070    if (pitch_val > st->max_pitch)
1071       pitch_val = st->max_pitch;
1072    if (pitch_val < st->min_pitch)
1073       pitch_val = st->min_pitch;
1074    for (i=0;i<st->frameSize;i++)
1075    {
1076       st->exc[i]= MULT16_16_Q15(pitch_gain, (st->exc[i-pitch_val]+VERY_SMALL)) +
1077             speex_rand(noise_gain, &st->seed);
1078    }
1079
1080    bw_lpc(QCONST16(.98,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
1081    iir_mem16(&st->exc[-st->subframeSize], st->interp_qlpc, out, st->frameSize,
1082              st->lpcSize, st->mem_sp, stack);
1083    highpass(out, out, st->frameSize, HIGHPASS_NARROWBAND|HIGHPASS_OUTPUT, st->mem_hp);
1084
1085    st->first = 0;
1086    st->count_lost++;
1087    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = PSHR16(pitch_gain,9);
1088    if (st->pitch_gain_buf_idx > 2) /* rollover */
1089       st->pitch_gain_buf_idx = 0;
1090 }
1091 #endif
1092
1093 /* Just so we don't need to carry the complete wideband mode information */
1094 static const int wb_skip_table[8] = {0, 36, 112, 192, 352, 0, 0, 0};
1095
1096 int nb_decode(void *state, SpeexBits *bits, void *vout)
1097 {
1098    DecState *st;
1099    int i, sub;
1100    int pitch;
1101    spx_word16_t pitch_gain[3];
1102    spx_word32_t ol_gain=0;
1103    int ol_pitch=0;
1104    spx_word16_t ol_pitch_coef=0;
1105    int best_pitch=40;
1106    spx_word16_t best_pitch_gain=0;
1107    int wideband;
1108    int m;
1109    char *stack;
1110    VARDECL(spx_sig_t *innov);
1111    VARDECL(spx_word32_t *exc32);
1112    VARDECL(spx_coef_t *ak);
1113    VARDECL(spx_lsp_t *qlsp);
1114    spx_word16_t pitch_average=0;
1115
1116    spx_word16_t *out = (spx_word16_t*)vout;
1117    VARDECL(spx_lsp_t *interp_qlsp);
1118
1119    st=(DecState*)state;
1120    stack=st->stack;
1121
1122    /* Check if we're in DTX mode*/
1123    if (!bits && st->dtx_enabled)
1124    {
1125       st->submodeID=0;
1126    } else
1127    {
1128       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1129 #ifndef ROCKBOX_VOICE_CODEC
1130       if (!bits)
1131       {
1132          nb_decode_lost(st, out, stack);
1133          return 0;
1134       }
1135 #endif
1136
1137       if (st->encode_submode)
1138       {
1139
1140       /* Search for next narrowband block (handle requests, skip wideband blocks) */
1141       do {
1142          if (speex_bits_remaining(bits)<5)
1143             return -1;
1144          wideband = speex_bits_unpack_unsigned(bits, 1);
1145          if (wideband) /* Skip wideband block (for compatibility) */
1146          {
1147             int submode;
1148             int advance;
1149             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1150             /*speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);*/
1151             advance = wb_skip_table[submode];
1152             if (advance < 0)
1153             {
1154                speex_notify("Invalid mode encountered. The stream is corrupted.");
1155                return -2;
1156             }
1157             advance -= (SB_SUBMODE_BITS+1);
1158             speex_bits_advance(bits, advance);
1159
1160             if (speex_bits_remaining(bits)<5)
1161                return -1;
1162             wideband = speex_bits_unpack_unsigned(bits, 1);
1163             if (wideband)
1164             {
1165                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1166                /*speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);*/
1167                advance = wb_skip_table[submode];
1168                if (advance < 0)
1169                {
1170                   speex_notify("Invalid mode encountered. The stream is corrupted.");
1171                   return -2;
1172                }
1173                advance -= (SB_SUBMODE_BITS+1);
1174                speex_bits_advance(bits, advance);
1175                wideband = speex_bits_unpack_unsigned(bits, 1);
1176                if (wideband)
1177                {
1178                   speex_notify("More than two wideband layers found. The stream is corrupted.");
1179                   return -2;
1180                }
1181
1182             }
1183          }
1184          if (speex_bits_remaining(bits)<4)
1185             return -1;
1186          /* FIXME: Check for overflow */
1187          m = speex_bits_unpack_unsigned(bits, 4);
1188          if (m==15) /* We found a terminator */
1189          {
1190             return -1;
1191          } else if (m==14) /* Speex in-band request */
1192          {
1193             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1194             if (ret)
1195                return ret;
1196          } else if (m==13) /* User in-band request */
1197          {
1198             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1199             if (ret)
1200                return ret;
1201          } else if (m>8) /* Invalid mode */
1202          {
1203             speex_notify("Invalid mode encountered. The stream is corrupted.");
1204             return -2;
1205          }
1206
1207       } while (m>8);
1208
1209       /* Get the sub-mode that was used */
1210       st->submodeID = m;
1211       }
1212
1213    }
1214
1215    /* Shift all buffers by one frame */
1216    SPEEX_MOVE(st->excBuf, st->excBuf+st->frameSize, 2*st->max_pitch + st->subframeSize + 12);
1217
1218    /* If null mode (no transmission), just set a couple things to zero*/
1219    if (st->submodes[st->submodeID] == NULL)
1220    {
1221       VARDECL(spx_coef_t *lpc);
1222       ALLOC(lpc, st->lpcSize, spx_coef_t);
1223       bw_lpc(QCONST16(0.93f,15), st->interp_qlpc, lpc, st->lpcSize);
1224       {
1225          spx_word16_t innov_gain=0;
1226          /* FIXME: This was innov, not exc */
1227          innov_gain = compute_rms16(st->exc, st->frameSize);
1228          for (i=0;i<st->frameSize;i++)
1229             st->exc[i]=speex_rand(innov_gain, &st->seed);
1230       }
1231
1232
1233       st->first=1;
1234
1235       /* Final signal synthesis from excitation */
1236       iir_mem16(st->exc, lpc, out, st->frameSize, st->lpcSize, st->mem_sp, stack);
1237
1238       st->count_lost=0;
1239       return 0;
1240    }
1241
1242    ALLOC(qlsp, st->lpcSize, spx_lsp_t);
1243
1244    /* Unquantize LSPs */
1245    SUBMODE(lsp_unquant)(qlsp, st->lpcSize, bits);
1246
1247    /*Damp memory if a frame was lost and the LSP changed too much*/
1248    if (st->count_lost)
1249    {
1250       spx_word16_t fact;
1251       spx_word32_t lsp_dist=0;
1252       for (i=0;i<st->lpcSize;i++)
1253          lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - qlsp[i])));
1254 #ifdef FIXED_POINT
1255       fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));
1256 #else
1257       fact = .6*exp(-.2*lsp_dist);
1258 #endif
1259       for (i=0;i<st->lpcSize;i++)
1260          st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
1261    }
1262
1263
1264    /* Handle first frame and lost-packet case */
1265    if (st->first || st->count_lost)
1266    {
1267       for (i=0;i<st->lpcSize;i++)
1268          st->old_qlsp[i] = qlsp[i];
1269    }
1270
1271    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1272    if (SUBMODE(lbr_pitch)!=-1)
1273    {
1274       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1275    }
1276
1277    if (SUBMODE(forced_pitch_gain))
1278    {
1279       int quant;
1280       quant = speex_bits_unpack_unsigned(bits, 4);
1281       ol_pitch_coef=MULT16_16_P15(QCONST16(0.066667,15),SHL16(quant,GAIN_SHIFT));
1282    }
1283
1284    /* Get global excitation gain */
1285    {
1286       int qe;
1287       qe = speex_bits_unpack_unsigned(bits, 5);
1288 #ifdef FIXED_POINT
1289       /* FIXME: Perhaps we could slightly lower the gain here when the output is going to saturate? */
1290       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
1291 #else
1292       ol_gain = SIG_SCALING*exp(qe/3.5);
1293 #endif
1294    }
1295
1296    ALLOC(ak, st->lpcSize, spx_coef_t);
1297    ALLOC(innov, st->subframeSize, spx_sig_t);
1298    ALLOC(exc32, st->subframeSize, spx_word32_t);
1299
1300    if (st->submodeID==1)
1301    {
1302       int extra;
1303       extra = speex_bits_unpack_unsigned(bits, 4);
1304
1305       if (extra==15)
1306          st->dtx_enabled=1;
1307       else
1308          st->dtx_enabled=0;
1309    }
1310    if (st->submodeID>1)
1311       st->dtx_enabled=0;
1312
1313    /*Loop on subframes */
1314    for (sub=0;sub<st->nbSubframes;sub++)
1315    {
1316       int offset;
1317       spx_word16_t *exc;
1318       spx_word16_t *sp;
1319       spx_word16_t *innov_save = NULL;
1320       spx_word16_t tmp;
1321
1322       /* Offset relative to start of frame */
1323       offset = st->subframeSize*sub;
1324       /* Excitation */
1325       exc=st->exc+offset;
1326       /* Original signal */
1327       sp=out+offset;
1328       if (st->innov_save)
1329          innov_save = st->innov_save+offset;
1330
1331
1332       /* Reset excitation */
1333       SPEEX_MEMSET(exc, 0, st->subframeSize);
1334
1335       /*Adaptive codebook contribution*/
1336       speex_assert (SUBMODE(ltp_unquant));
1337       {
1338          int pit_min, pit_max;
1339          /* Handle pitch constraints if any */
1340          if (SUBMODE(lbr_pitch) != -1)
1341          {
1342             int margin;
1343             margin = SUBMODE(lbr_pitch);
1344             if (margin)
1345             {
1346 /* GT - need optimization?
1347                if (ol_pitch < st->min_pitch+margin-1)
1348                   ol_pitch=st->min_pitch+margin-1;
1349                if (ol_pitch > st->max_pitch-margin)
1350                   ol_pitch=st->max_pitch-margin;
1351                pit_min = ol_pitch-margin+1;
1352                pit_max = ol_pitch+margin;
1353 */
1354                pit_min = ol_pitch-margin+1;
1355                if (pit_min < st->min_pitch)
1356           pit_min = st->min_pitch;
1357                pit_max = ol_pitch+margin;
1358                if (pit_max > st->max_pitch)
1359           pit_max = st->max_pitch;
1360             } else {
1361                pit_min = pit_max = ol_pitch;
1362             }
1363          } else {
1364             pit_min = st->min_pitch;
1365             pit_max = st->max_pitch;
1366          }
1367
1368
1369
1370          SUBMODE(ltp_unquant)(exc, exc32, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params),
1371                  st->subframeSize, &pitch, &pitch_gain[0], bits, stack,
1372                  st->count_lost, offset, st->last_pitch_gain, 0);
1373
1374          /* Ensuring that things aren't blowing up as would happen if e.g. an encoder is
1375          crafting packets to make us produce NaNs and slow down the decoder (vague DoS threat).
1376          We can probably be even more aggressive and limit to 15000 or so. */
1377          sanitize_values32(exc32, NEG32(QCONST32(32000,SIG_SHIFT-1)), QCONST32(32000,SIG_SHIFT-1), st->subframeSize);
1378
1379          tmp = gain_3tap_to_1tap(pitch_gain);
1380
1381          pitch_average += tmp;
1382          if ((tmp>best_pitch_gain&&ABS(2*best_pitch-pitch)>=3&&ABS(3*best_pitch-pitch)>=4&&ABS(4*best_pitch-pitch)>=5)
1383               || (tmp>MULT16_16_Q15(QCONST16(.6,15),best_pitch_gain)&&(ABS(best_pitch-2*pitch)<3||ABS(best_pitch-3*pitch)<4||ABS(best_pitch-4*pitch)<5))
1384               || (MULT16_16_Q15(QCONST16(.67,15),tmp)>best_pitch_gain&&(ABS(2*best_pitch-pitch)<3||ABS(3*best_pitch-pitch)<4||ABS(4*best_pitch-pitch)<5)) )
1385          {
1386             best_pitch = pitch;
1387             if (tmp > best_pitch_gain)
1388                best_pitch_gain = tmp;
1389          }
1390       }
1391
1392       /* Unquantize the innovation */
1393       {
1394          int q_energy;
1395          spx_word32_t ener;
1396
1397          SPEEX_MEMSET(innov, 0, st->subframeSize);
1398
1399          /* Decode sub-frame gain correction */
1400          if (SUBMODE(have_subframe_gain)==3)
1401          {
1402             q_energy = speex_bits_unpack_unsigned(bits, 3);
1403             ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
1404          } else if (SUBMODE(have_subframe_gain)==1)
1405          {
1406             q_energy = speex_bits_unpack_unsigned(bits, 1);
1407             ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
1408          } else {
1409             ener = ol_gain;
1410          }
1411
1412          speex_assert (SUBMODE(innovation_unquant));
1413          {
1414             /*Fixed codebook contribution*/
1415             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack, &st->seed);
1416             /* De-normalize innovation and update excitation */
1417
1418             signal_mul(innov, innov, ener, st->subframeSize);
1419
1420             /* Decode second codebook (only for some modes) */
1421             if (SUBMODE(double_codebook))
1422             {
1423                char *tmp_stack=stack;
1424                VARDECL(spx_sig_t *innov2);
1425                ALLOC(innov2, st->subframeSize, spx_sig_t);
1426                SPEEX_MEMSET(innov2, 0, st->subframeSize);
1427                SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack, &st->seed);
1428                signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545f,15),ener), st->subframeSize);
1429                for (i=0;i<st->subframeSize;i++)
1430                   innov[i] = ADD32(innov[i], innov2[i]);
1431                stack = tmp_stack;
1432             }
1433             for (i=0;i<st->subframeSize;i++)
1434                exc[i]=EXTRACT16(SATURATE32(PSHR32(ADD32(SHL32(exc32[i],1),innov[i]),SIG_SHIFT),32767));
1435             /*print_vec(exc, 40, "innov");*/
1436             if (innov_save)
1437             {
1438                for (i=0;i<st->subframeSize;i++)
1439                   innov_save[i] = EXTRACT16(PSHR32(innov[i], SIG_SHIFT));
1440             }
1441          }
1442
1443          /*Vocoder mode*/
1444          if (st->submodeID==1)
1445          {
1446             spx_word16_t g=ol_pitch_coef;
1447             g=MULT16_16_P14(QCONST16(1.5f,14),(g-QCONST16(.2f,6)));
1448             if (g<0)
1449                g=0;
1450             if (g>GAIN_SCALING)
1451                g=GAIN_SCALING;
1452
1453             SPEEX_MEMSET(exc, 0, st->subframeSize);
1454             while (st->voc_offset<st->subframeSize)
1455             {
1456                /* exc[st->voc_offset]= g*sqrt(2*ol_pitch)*ol_gain;
1457                   Not quite sure why we need the factor of two in the sqrt */
1458                if (st->voc_offset>=0)
1459                   exc[st->voc_offset]=MULT16_16(spx_sqrt(MULT16_16_16(2,ol_pitch)),EXTRACT16(PSHR32(MULT16_16(g,PSHR32(ol_gain,SIG_SHIFT)),6)));
1460                st->voc_offset+=ol_pitch;
1461             }
1462             st->voc_offset -= st->subframeSize;
1463
1464             for (i=0;i<st->subframeSize;i++)
1465             {
1466                spx_word16_t exci=exc[i];
1467                exc[i]= ADD16(ADD16(MULT16_16_Q15(QCONST16(.7f,15),exc[i]) , MULT16_16_Q15(QCONST16(.3f,15),st->voc_m1)),
1468                              SUB16(MULT16_16_Q15(Q15_ONE-MULT16_16_16(QCONST16(.85f,9),g),EXTRACT16(PSHR32(innov[i],SIG_SHIFT))),
1469                                    MULT16_16_Q15(MULT16_16_16(QCONST16(.15f,9),g),EXTRACT16(PSHR32(st->voc_m2,SIG_SHIFT)))
1470                                   ));
1471                st->voc_m1 = exci;
1472                st->voc_m2=innov[i];
1473                st->voc_mean = EXTRACT16(PSHR32(ADD32(MULT16_16(QCONST16(.8f,15),st->voc_mean), MULT16_16(QCONST16(.2f,15),exc[i])), 15));
1474                exc[i]-=st->voc_mean;
1475             }
1476          }
1477
1478       }
1479    }
1480
1481    ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
1482
1483    if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0 && !st->count_lost)
1484    {
1485       multicomb(st->exc-st->subframeSize, out, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, best_pitch, 40, SUBMODE(comb_gain), stack);
1486       multicomb(st->exc+st->subframeSize, out+2*st->subframeSize, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, best_pitch, 40, SUBMODE(comb_gain), stack);
1487    } else {
1488       SPEEX_COPY(out, &st->exc[-st->subframeSize], st->frameSize);
1489    }
1490
1491    /* If the last packet was lost, re-scale the excitation to obtain the same energy as encoded in ol_gain */
1492    if (st->count_lost)
1493    {
1494       spx_word16_t exc_ener;
1495       spx_word32_t gain32;
1496       spx_word16_t gain;
1497       exc_ener = compute_rms16 (st->exc, st->frameSize);
1498       gain32 = PDIV32(ol_gain, ADD16(exc_ener,1));
1499 #ifdef FIXED_POINT
1500       if (gain32 > 32767)
1501          gain32 = 32767;
1502       gain = EXTRACT16(gain32);
1503 #else
1504       if (gain32 > 2)
1505          gain32=2;
1506       gain = gain32;
1507 #endif
1508       for (i=0;i<st->frameSize;i++)
1509       {
1510          st->exc[i] = MULT16_16_Q14(gain, st->exc[i]);
1511          out[i]=st->exc[i-st->subframeSize];
1512       }
1513    }
1514
1515    /*Loop on subframes */
1516    for (sub=0;sub<st->nbSubframes;sub++)
1517    {
1518       int offset;
1519       spx_word16_t *sp;
1520       spx_word16_t *exc;
1521       /* Offset relative to start of frame */
1522       offset = st->subframeSize*sub;
1523       /* Original signal */
1524       sp=out+offset;
1525       /* Excitation */
1526       exc=st->exc+offset;
1527
1528       /* LSP interpolation (quantized and unquantized) */
1529       lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1530
1531       /* Make sure the LSP's are stable */
1532       lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);
1533
1534       /* Compute interpolated LPCs (unquantized) */
1535       lsp_to_lpc(interp_qlsp, ak, st->lpcSize, stack);
1536
1537       /* Compute analysis filter at w=pi */
1538       {
1539          spx_word32_t pi_g=LPC_SCALING;
1540          for (i=0;i<st->lpcSize;i+=2)
1541          {
1542             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
1543             pi_g = ADD32(pi_g, SUB32(EXTEND32(ak[i+1]),EXTEND32(ak[i])));
1544          }
1545          st->pi_gain[sub] = pi_g;
1546       }
1547
1548       iir_mem16(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize,
1549                 st->mem_sp, stack);
1550
1551       for (i=0;i<st->lpcSize;i++)
1552          st->interp_qlpc[i] = ak[i];
1553
1554    }
1555
1556    if (st->highpass_enabled)
1557       highpass(out, out, st->frameSize, (st->isWideband?HIGHPASS_WIDEBAND:HIGHPASS_NARROWBAND)|HIGHPASS_OUTPUT, st->mem_hp);
1558    /*for (i=0;i<st->frameSize;i++)
1559      printf ("%d\n", (int)st->frame[i]);*/
1560
1561    /* Tracking output level */
1562    st->level = 1+PSHR32(ol_gain,SIG_SHIFT);
1563    st->max_level = MAX16(MULT16_16_Q15(QCONST16(.99f,15), st->max_level), st->level);
1564    st->min_level = MIN16(ADD16(1,MULT16_16_Q14(QCONST16(1.01f,14), st->min_level)), st->level);
1565    if (st->max_level < st->min_level+1)
1566       st->max_level = st->min_level+1;
1567    /*printf ("%f %f %f %d\n", og, st->min_level, st->max_level, update);*/
1568
1569    /* Store the LSPs for interpolation in the next frame */
1570    for (i=0;i<st->lpcSize;i++)
1571       st->old_qlsp[i] = qlsp[i];
1572
1573    /* The next frame will not be the first (Duh!) */
1574    st->first = 0;
1575    st->count_lost=0;
1576    st->last_pitch = best_pitch;
1577 #ifdef FIXED_POINT
1578    st->last_pitch_gain = PSHR16(pitch_average,2);
1579 #else
1580    st->last_pitch_gain = .25*pitch_average;
1581 #endif
1582    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1583    if (st->pitch_gain_buf_idx > 2) /* rollover */
1584       st->pitch_gain_buf_idx = 0;
1585
1586    st->last_ol_gain = ol_gain;
1587
1588    return 0;
1589 }
1590
1591 #ifndef SPEEX_DISABLE_ENCODER
1592 int nb_encoder_ctl(void *state, int request, void *ptr)
1593 {
1594    EncState *st;
1595    st=(EncState*)state;
1596    switch(request)
1597    {
1598    case SPEEX_GET_FRAME_SIZE:
1599       (*(spx_int32_t*)ptr) = st->frameSize;
1600       break;
1601    case SPEEX_SET_LOW_MODE:
1602    case SPEEX_SET_MODE:
1603       st->submodeSelect = st->submodeID = (*(spx_int32_t*)ptr);
1604       break;
1605    case SPEEX_GET_LOW_MODE:
1606    case SPEEX_GET_MODE:
1607       (*(spx_int32_t*)ptr) = st->submodeID;
1608       break;
1609 #ifndef DISABLE_VBR
1610       case SPEEX_SET_VBR:
1611       st->vbr_enabled = (*(spx_int32_t*)ptr);
1612       break;
1613    case SPEEX_GET_VBR:
1614       (*(spx_int32_t*)ptr) = st->vbr_enabled;
1615       break;
1616    case SPEEX_SET_VAD:
1617       st->vad_enabled = (*(spx_int32_t*)ptr);
1618       break;
1619    case SPEEX_GET_VAD:
1620       (*(spx_int32_t*)ptr) = st->vad_enabled;
1621       break;
1622    case SPEEX_SET_DTX:
1623       st->dtx_enabled = (*(spx_int32_t*)ptr);
1624       break;
1625    case SPEEX_GET_DTX:
1626       (*(spx_int32_t*)ptr) = st->dtx_enabled;
1627       break;
1628    case SPEEX_SET_ABR:
1629       st->abr_enabled = (*(spx_int32_t*)ptr);
1630       st->vbr_enabled = st->abr_enabled!=0;
1631       if (st->vbr_enabled)
1632       {
1633          spx_int32_t i=10;
1634          spx_int32_t rate, target;
1635          float vbr_qual;
1636          target = (*(spx_int32_t*)ptr);
1637          while (i>=0)
1638          {
1639             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1640             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1641             if (rate <= target)
1642                break;
1643             i--;
1644          }
1645          vbr_qual=i;
1646          if (vbr_qual<0)
1647             vbr_qual=0;
1648          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1649          st->abr_count=0;
1650          st->abr_drift=0;
1651          st->abr_drift2=0;
1652       }
1653
1654       break;
1655    case SPEEX_GET_ABR:
1656       (*(spx_int32_t*)ptr) = st->abr_enabled;
1657       break;
1658 #endif /* #ifndef DISABLE_VBR */
1659 #if !defined(DISABLE_VBR) && !defined(DISABLE_FLOAT_API)
1660    case SPEEX_SET_VBR_QUALITY:
1661       st->vbr_quality = (*(float*)ptr);
1662       break;
1663    case SPEEX_GET_VBR_QUALITY:
1664       (*(float*)ptr) = st->vbr_quality;
1665       break;
1666 #endif /* !defined(DISABLE_VBR) && !defined(DISABLE_FLOAT_API) */
1667    case SPEEX_SET_QUALITY:
1668       {
1669          int quality = (*(spx_int32_t*)ptr);
1670          if (quality < 0)
1671             quality = 0;
1672          if (quality > 10)
1673             quality = 10;
1674          st->submodeSelect = st->submodeID = ((const SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1675       }
1676       break;
1677    case SPEEX_SET_COMPLEXITY:
1678       st->complexity = (*(spx_int32_t*)ptr);
1679       if (st->complexity<0)
1680          st->complexity=0;
1681       break;
1682    case SPEEX_GET_COMPLEXITY:
1683       (*(spx_int32_t*)ptr) = st->complexity;
1684       break;
1685    case SPEEX_SET_BITRATE:
1686       {
1687          spx_int32_t i=10;
1688          spx_int32_t rate, target;
1689          target = (*(spx_int32_t*)ptr);
1690          while (i>=0)
1691          {
1692             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1693             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1694             if (rate <= target)
1695                break;
1696             i--;
1697          }
1698       }
1699       break;
1700    case SPEEX_GET_BITRATE:
1701       if (st->submodes[st->submodeID])
1702          (*(spx_int32_t*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1703       else
1704          (*(spx_int32_t*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1705       break;
1706    case SPEEX_SET_SAMPLING_RATE:
1707       st->sampling_rate = (*(spx_int32_t*)ptr);
1708       break;
1709    case SPEEX_GET_SAMPLING_RATE:
1710       (*(spx_int32_t*)ptr)=st->sampling_rate;
1711       break;
1712    case SPEEX_RESET_STATE:
1713       {
1714          int i;
1715          st->bounded_pitch = 1;
1716          st->first = 1;
1717          for (i=0;i<st->lpcSize;i++)
1718             st->old_lsp[i]= DIV32(MULT16_16(QCONST16(3.1415927f, LSP_SHIFT), i+1), st->lpcSize+1);
1719          for (i=0;i<st->lpcSize;i++)
1720             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1721          for (i=0;i<st->frameSize+st->max_pitch+1;i++)
1722             st->excBuf[i]=st->swBuf[i]=0;
1723          for (i=0;i<st->windowSize-st->frameSize;i++)
1724             st->winBuf[i]=0;
1725       }
1726       break;
1727    case SPEEX_SET_SUBMODE_ENCODING:
1728       st->encode_submode = (*(spx_int32_t*)ptr);
1729       break;
1730    case SPEEX_GET_SUBMODE_ENCODING:
1731       (*(spx_int32_t*)ptr) = st->encode_submode;
1732       break;
1733    case SPEEX_GET_LOOKAHEAD:
1734       (*(spx_int32_t*)ptr)=(st->windowSize-st->frameSize);
1735       break;
1736    case SPEEX_SET_PLC_TUNING:
1737       st->plc_tuning = (*(spx_int32_t*)ptr);
1738       if (st->plc_tuning>100)
1739          st->plc_tuning=100;
1740       break;
1741    case SPEEX_GET_PLC_TUNING:
1742       (*(spx_int32_t*)ptr)=(st->plc_tuning);
1743       break;
1744 #ifndef DISABLE_VBR
1745    case SPEEX_SET_VBR_MAX_BITRATE:
1746       st->vbr_max = (*(spx_int32_t*)ptr);
1747       break;
1748    case SPEEX_GET_VBR_MAX_BITRATE:
1749       (*(spx_int32_t*)ptr) = st->vbr_max;
1750       break;
1751 #endif /* #ifndef DISABLE_VBR */
1752    case SPEEX_SET_HIGHPASS:
1753       st->highpass_enabled = (*(spx_int32_t*)ptr);
1754       break;
1755    case SPEEX_GET_HIGHPASS:
1756       (*(spx_int32_t*)ptr) = st->highpass_enabled;
1757       break;
1758
1759    /* This is all internal stuff past this point */
1760    case SPEEX_GET_PI_GAIN:
1761       {
1762          int i;
1763          spx_word32_t *g = (spx_word32_t*)ptr;
1764          for (i=0;i<st->nbSubframes;i++)
1765             g[i]=st->pi_gain[i];
1766       }
1767       break;
1768    case SPEEX_GET_EXC:
1769       {
1770          int i;
1771          for (i=0;i<st->nbSubframes;i++)
1772             ((spx_word16_t*)ptr)[i] = compute_rms16(st->exc+i*st->subframeSize, st->subframeSize);
1773       }
1774       break;
1775 #ifndef DISABLE_VBR
1776    case SPEEX_GET_RELATIVE_QUALITY:
1777       (*(float*)ptr)=st->relative_quality;
1778       break;
1779 #endif /* #ifndef DISABLE_VBR */
1780    case SPEEX_SET_INNOVATION_SAVE:
1781       st->innov_rms_save = (spx_word16_t*)ptr;
1782       break;
1783    case SPEEX_SET_WIDEBAND:
1784       st->isWideband = *((spx_int32_t*)ptr);
1785       break;
1786    case SPEEX_GET_STACK:
1787       *((char**)ptr) = st->stack;
1788       break;
1789    default:
1790       speex_warning_int("Unknown nb_ctl request: ", request);
1791       return -1;
1792    }
1793    return 0;
1794 }
1795 #endif /* SPEEX_DISABLE_ENCODER */
1796
1797 int nb_decoder_ctl(void *state, int request, void *ptr)
1798 {
1799    DecState *st;
1800    st=(DecState*)state;
1801    switch(request)
1802    {
1803    case SPEEX_SET_LOW_MODE:
1804    case SPEEX_SET_MODE:
1805       st->submodeID = (*(spx_int32_t*)ptr);
1806       break;
1807    case SPEEX_GET_LOW_MODE:
1808    case SPEEX_GET_MODE:
1809       (*(spx_int32_t*)ptr) = st->submodeID;
1810       break;
1811    case SPEEX_SET_ENH:
1812       st->lpc_enh_enabled = *((spx_int32_t*)ptr);
1813       break;
1814    case SPEEX_GET_ENH:
1815       *((spx_int32_t*)ptr) = st->lpc_enh_enabled;
1816       break;
1817    case SPEEX_GET_FRAME_SIZE:
1818       (*(spx_int32_t*)ptr) = st->frameSize;
1819       break;
1820    case SPEEX_GET_BITRATE:
1821       if (st->submodes[st->submodeID])
1822          (*(spx_int32_t*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1823       else
1824          (*(spx_int32_t*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1825       break;
1826    case SPEEX_SET_SAMPLING_RATE:
1827       st->sampling_rate = (*(spx_int32_t*)ptr);
1828       break;
1829    case SPEEX_GET_SAMPLING_RATE:
1830       (*(spx_int32_t*)ptr)=st->sampling_rate;
1831       break;
1832    case SPEEX_SET_HANDLER:
1833       {
1834          SpeexCallback *c = (SpeexCallback*)ptr;
1835          st->speex_callbacks[c->callback_id].func=c->func;
1836          st->speex_callbacks[c->callback_id].data=c->data;
1837          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1838       }
1839       break;
1840    case SPEEX_SET_USER_HANDLER:
1841       {
1842          SpeexCallback *c = (SpeexCallback*)ptr;
1843          st->user_callback.func=c->func;
1844          st->user_callback.data=c->data;
1845          st->user_callback.callback_id=c->callback_id;
1846       }
1847       break;
1848    case SPEEX_RESET_STATE:
1849       {
1850          int i;
1851          for (i=0;i<st->lpcSize;i++)
1852             st->mem_sp[i]=0;
1853          for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
1854             st->excBuf[i]=0;
1855       }
1856       break;
1857    case SPEEX_SET_SUBMODE_ENCODING:
1858       st->encode_submode = (*(spx_int32_t*)ptr);
1859       break;
1860    case SPEEX_GET_SUBMODE_ENCODING:
1861       (*(spx_int32_t*)ptr) = st->encode_submode;
1862       break;
1863    case SPEEX_GET_LOOKAHEAD:
1864       (*(spx_int32_t*)ptr)=st->subframeSize;
1865       break;
1866    case SPEEX_SET_HIGHPASS:
1867       st->highpass_enabled = (*(spx_int32_t*)ptr);
1868       break;
1869    case SPEEX_GET_HIGHPASS:
1870       (*(spx_int32_t*)ptr) = st->highpass_enabled;
1871       break;
1872       /* FIXME: Convert to fixed-point and re-enable even when float API is disabled */
1873 #ifndef DISABLE_FLOAT_API
1874    case SPEEX_GET_ACTIVITY:
1875    {
1876       float ret;
1877       ret = log(st->level/st->min_level)/log(st->max_level/st->min_level);
1878       if (ret>1)
1879          ret = 1;
1880       /* Done in a strange way to catch NaNs as well */
1881       if (!(ret > 0))
1882          ret = 0;
1883       /*printf ("%f %f %f %f\n", st->level, st->min_level, st->max_level, ret);*/
1884       (*(spx_int32_t*)ptr) = (int)(100*ret);
1885    }
1886    break;
1887 #endif
1888    case SPEEX_GET_PI_GAIN:
1889       {
1890          int i;
1891          spx_word32_t *g = (spx_word32_t*)ptr;
1892          for (i=0;i<st->nbSubframes;i++)
1893             g[i]=st->pi_gain[i];
1894       }
1895       break;
1896    case SPEEX_GET_EXC:
1897       {
1898          int i;
1899          for (i=0;i<st->nbSubframes;i++)
1900             ((spx_word16_t*)ptr)[i] = compute_rms16(st->exc+i*st->subframeSize, st->subframeSize);
1901       }
1902       break;
1903    case SPEEX_GET_DTX_STATUS:
1904       *((spx_int32_t*)ptr) = st->dtx_enabled;
1905       break;
1906    case SPEEX_SET_INNOVATION_SAVE:
1907       st->innov_save = (spx_word16_t*)ptr;
1908       break;
1909    case SPEEX_SET_WIDEBAND:
1910       st->isWideband = *((spx_int32_t*)ptr);
1911       break;
1912    case SPEEX_GET_STACK:
1913       *((char**)ptr) = st->stack;
1914       break;
1915    default:
1916       speex_warning_int("Unknown nb_ctl request: ", request);
1917       return -1;
1918    }
1919    return 0;
1920 }