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