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