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