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