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