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