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