1acd45e6cdb202457bf14ae187196809d4a7552b
[speexdsp.git] / libspeex / sb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: sb_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
33 #include <math.h>
34 #include "sb_celp.h"
35 #include "stdlib.h"
36 #include "filters.h"
37 #include "lpc.h"
38 #include "lsp.h"
39 #include "stack_alloc.h"
40 #include "cb_search.h"
41 #include "quant_lsp.h"
42 #include "vq.h"
43 #include "ltp.h"
44 #include "misc.h"
45
46 #ifndef M_PI
47 #define M_PI           3.14159265358979323846  /* pi */
48 #endif
49
50 #define sqr(x) ((x)*(x))
51
52 #define SUBMODE(x) st->submodes[st->submodeID]->x
53
54 #define QMF_ORDER 64
55 static float h0[64] = {
56    3.596189e-05, -0.0001123515,
57    -0.0001104587, 0.0002790277,
58    0.0002298438, -0.0005953563,
59    -0.0003823631, 0.00113826,
60    0.0005308539, -0.001986177,
61    -0.0006243724, 0.003235877,
62    0.0005743159, -0.004989147,
63    -0.0002584767, 0.007367171,
64    -0.0004857935, -0.01050689,
65    0.001894714, 0.01459396,
66    -0.004313674, -0.01994365,
67    0.00828756, 0.02716055,
68    -0.01485397, -0.03764973,
69    0.026447, 0.05543245,
70    -0.05095487, -0.09779096,
71    0.1382363, 0.4600981,
72    0.4600981, 0.1382363,
73    -0.09779096, -0.05095487,
74    0.05543245, 0.026447,
75    -0.03764973, -0.01485397,
76    0.02716055, 0.00828756,
77    -0.01994365, -0.004313674,
78    0.01459396, 0.001894714,
79    -0.01050689, -0.0004857935,
80    0.007367171, -0.0002584767,
81    -0.004989147, 0.0005743159,
82    0.003235877, -0.0006243724,
83    -0.001986177, 0.0005308539,
84    0.00113826, -0.0003823631,
85    -0.0005953563, 0.0002298438,
86    0.0002790277, -0.0001104587,
87    -0.0001123515, 3.596189e-05
88 };
89
90 static float h1[64] = {
91    3.596189e-05, 0.0001123515,
92    -0.0001104587, -0.0002790277,
93    0.0002298438, 0.0005953563,
94    -0.0003823631, -0.00113826,
95    0.0005308539, 0.001986177,
96    -0.0006243724, -0.003235877,
97    0.0005743159, 0.004989147,
98    -0.0002584767, -0.007367171,
99    -0.0004857935, 0.01050689,
100    0.001894714, -0.01459396,
101    -0.004313674, 0.01994365,
102    0.00828756, -0.02716055,
103    -0.01485397, 0.03764973,
104    0.026447, -0.05543245,
105    -0.05095487, 0.09779096,
106    0.1382363, -0.4600981,
107    0.4600981, -0.1382363,
108    -0.09779096, 0.05095487,
109    0.05543245, -0.026447,
110    -0.03764973, 0.01485397,
111    0.02716055, -0.00828756,
112    -0.01994365, 0.004313674,
113    0.01459396, -0.001894714,
114    -0.01050689, 0.0004857935,
115    0.007367171, 0.0002584767,
116    -0.004989147, -0.0005743159,
117    0.003235877, 0.0006243724,
118    -0.001986177, -0.0005308539,
119    0.00113826, 0.0003823631,
120    -0.0005953563, -0.0002298438,
121    0.0002790277, 0.0001104587,
122    -0.0001123515, -3.596189e-05
123 };
124
125 void *sb_encoder_init(SpeexMode *m)
126 {
127    int i;
128    SBEncState *st;
129    SpeexSBMode *mode;
130
131    st = (SBEncState*)speex_alloc(sizeof(SBEncState)+8000*sizeof(float));
132    st->mode = m;
133    mode = (SpeexSBMode*)m->mode;
134
135    st->stack = ((char*)st) + sizeof(SBEncState);
136
137    st->st_low = speex_encoder_init(mode->nb_mode);
138    st->full_frame_size = 2*mode->frameSize;
139    st->frame_size = mode->frameSize;
140    st->subframeSize = mode->subframeSize;
141    st->nbSubframes = mode->frameSize/mode->subframeSize;
142    st->windowSize = st->frame_size*3/2;
143    st->lpcSize=mode->lpcSize;
144    st->bufSize=mode->bufSize;
145
146    st->encode_submode = 1;
147    st->submodes=mode->submodes;
148    st->submodeSelect = st->submodeID=mode->defaultSubmode;
149    
150    i=9;
151    speex_encoder_ctl(st->st_low, SPEEX_SET_QUALITY, &i);
152
153    st->lag_factor = mode->lag_factor;
154    st->lpc_floor = mode->lpc_floor;
155    st->gamma1=mode->gamma1;
156    st->gamma2=mode->gamma2;
157    st->first=1;
158
159    st->x0d=PUSH(st->stack, st->frame_size, float);
160    st->x1d=PUSH(st->stack, st->frame_size, float);
161    st->high=PUSH(st->stack, st->full_frame_size, float);
162    st->y0=PUSH(st->stack, st->full_frame_size, float);
163    st->y1=PUSH(st->stack, st->full_frame_size, float);
164
165    st->h0_mem=PUSH(st->stack, QMF_ORDER, float);
166    st->h1_mem=PUSH(st->stack, QMF_ORDER, float);
167    st->g0_mem=PUSH(st->stack, QMF_ORDER, float);
168    st->g1_mem=PUSH(st->stack, QMF_ORDER, float);
169
170    st->buf=PUSH(st->stack, st->windowSize, float);
171    st->excBuf=PUSH(st->stack, st->bufSize, float);
172    st->exc = st->excBuf + st->bufSize - st->windowSize;
173
174    st->res=PUSH(st->stack, st->frame_size, float);
175    st->sw=PUSH(st->stack, st->frame_size, float);
176    st->target=PUSH(st->stack, st->frame_size, float);
177    /*Asymmetric "pseudo-Hamming" window*/
178    {
179       int part1, part2;
180       part1 = st->subframeSize*7/2;
181       part2 = st->subframeSize*5/2;
182       st->window = PUSH(st->stack, st->windowSize, float);
183       for (i=0;i<part1;i++)
184          st->window[i]=.54-.46*cos(M_PI*i/part1);
185       for (i=0;i<part2;i++)
186          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
187    }
188
189    st->lagWindow = PUSH(st->stack, st->lpcSize+1, float);
190    for (i=0;i<st->lpcSize+1;i++)
191       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
192
193    st->autocorr = PUSH(st->stack, st->lpcSize+1, spx_word16_t);
194    st->lpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
195    st->bw_lpc1 = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
196    st->bw_lpc2 = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
197    st->lsp = PUSH(st->stack, st->lpcSize, float);
198    st->qlsp = PUSH(st->stack, st->lpcSize, float);
199    st->old_lsp = PUSH(st->stack, st->lpcSize, float);
200    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
201    st->interp_lsp = PUSH(st->stack, st->lpcSize, float);
202    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
203    st->interp_lpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
204    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
205    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
206
207    st->mem_sp = PUSH(st->stack, st->lpcSize, spx_mem_t);
208    st->mem_sp2 = PUSH(st->stack, st->lpcSize, spx_mem_t);
209    st->mem_sw = PUSH(st->stack, st->lpcSize, spx_mem_t);
210
211    st->vbr_quality = 8;
212    st->vbr_enabled = 0;
213    st->vad_enabled = 0;
214    st->abr_enabled = 0;
215    st->relative_quality=0;
216
217    st->complexity=2;
218    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
219    st->sampling_rate*=2;
220
221    return st;
222 }
223
224 void sb_encoder_destroy(void *state)
225 {
226    SBEncState *st=(SBEncState*)state;
227
228    speex_encoder_destroy(st->st_low);
229
230    speex_free(st);
231 }
232
233
234 int sb_encode(void *state, float *in, SpeexBits *bits)
235 {
236    SBEncState *st;
237    int i, roots, sub;
238    char *stack;
239    spx_mem_t *mem;
240    float *innov, *syn_resp;
241    float *low_pi_gain, *low_exc, *low_innov;
242    SpeexSBMode *mode;
243    int dtx;
244
245    st = (SBEncState*)state;
246    stack=st->stack;
247    mode = (SpeexSBMode*)(st->mode->mode);
248
249    /* Compute the two sub-bands by filtering with h0 and h1*/
250    qmf_decomp(in, h0, st->x0d, st->x1d, st->full_frame_size, QMF_ORDER, st->h0_mem, stack);
251     
252    /* Encode the narrowband part*/
253    speex_encode(st->st_low, st->x0d, bits);
254
255    /* High-band buffering / sync with low band */
256    for (i=0;i<st->windowSize-st->frame_size;i++)
257       st->high[i] = st->high[st->frame_size+i];
258    for (i=0;i<st->frame_size;i++)
259       st->high[st->windowSize-st->frame_size+i]=st->x1d[i];
260
261    speex_move(st->excBuf, st->excBuf+st->frame_size, (st->bufSize-st->frame_size)*sizeof(float));
262
263
264    low_pi_gain = PUSH(stack, st->nbSubframes, float);
265    low_exc = PUSH(stack, st->frame_size, float);
266    low_innov = PUSH(stack, st->frame_size, float);
267    speex_encoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
268    speex_encoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
269    speex_encoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
270    
271    speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, &dtx);
272
273    if (dtx==0)
274       dtx=1;
275    else
276       dtx=0;
277
278    /* Start encoding the high-band */
279    for (i=0;i<st->windowSize;i++)
280       st->buf[i] = st->high[i] * st->window[i];
281
282    /* Compute auto-correlation */
283    _spx_autocorr(st->buf, st->autocorr, st->lpcSize+1, st->windowSize);
284
285    st->autocorr[0] += 1;        /* prevents NANs */
286    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
287    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
288    for (i=0;i<st->lpcSize+1;i++)
289       st->autocorr[i] *= st->lagWindow[i];
290
291    /* Levinson-Durbin */
292    _spx_lpc(st->lpc+1, st->autocorr, st->lpcSize);
293    st->lpc[0]=1;
294
295    /* LPC to LSPs (x-domain) transform */
296    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
297    if (roots!=st->lpcSize)
298    {
299       roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.02, stack);
300       if (roots!=st->lpcSize) {
301          /*If we can't find all LSP's, do some damage control and use a flat filter*/
302          for (i=0;i<st->lpcSize;i++)
303          {
304             st->lsp[i]=cos(M_PI*((float)(i+1))/(st->lpcSize+1));
305          }
306       }
307    }
308
309    /* x-domain to angle domain*/
310    for (i=0;i<st->lpcSize;i++)
311       st->lsp[i] = acos(st->lsp[i]);
312
313    /* VBR code */
314    if ((st->vbr_enabled || st->vad_enabled) && !dtx)
315    {
316       float e_low=0, e_high=0;
317       float ratio;
318       if (st->abr_enabled)
319       {
320          float qual_change=0;
321          if (st->abr_drift2 * st->abr_drift > 0)
322          {
323             /* Only adapt if long-term and short-term drift are the same sign */
324             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
325             if (qual_change>.1)
326                qual_change=.1;
327             if (qual_change<-.1)
328                qual_change=-.1;
329          }
330          st->vbr_quality += qual_change;
331          if (st->vbr_quality>10)
332             st->vbr_quality=10;
333          if (st->vbr_quality<0)
334             st->vbr_quality=0;
335       }
336
337
338       for (i=0;i<st->frame_size;i++)
339       {
340          e_low  += st->x0d[i]* st->x0d[i];
341          e_high += st->high[i]* st->high[i];
342       }
343       ratio = log((1+e_high)/(1+e_low));
344       speex_encoder_ctl(st->st_low, SPEEX_GET_RELATIVE_QUALITY, &st->relative_quality);
345       if (ratio<-4)
346          ratio=-4;
347       if (ratio>2)
348          ratio=2;
349       /*if (ratio>-2)*/
350       if (st->vbr_enabled) 
351       {
352          int modeid;
353          modeid = mode->nb_modes-1;
354          st->relative_quality+=1.0*(ratio+2);
355          if (st->relative_quality<-1)
356             st->relative_quality=-1;
357          while (modeid)
358          {
359             int v1;
360             float thresh;
361             v1=(int)floor(st->vbr_quality);
362             if (v1==10)
363                thresh = mode->vbr_thresh[modeid][v1];
364             else
365                thresh = (st->vbr_quality-v1)   * mode->vbr_thresh[modeid][v1+1] + 
366                         (1+v1-st->vbr_quality) * mode->vbr_thresh[modeid][v1];
367             if (st->relative_quality >= thresh)
368                break;
369             modeid--;
370          }
371          speex_encoder_ctl(state, SPEEX_SET_HIGH_MODE, &modeid);
372          if (st->abr_enabled)
373          {
374             int bitrate;
375             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
376             st->abr_drift+=(bitrate-st->abr_enabled);
377             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
378             st->abr_count += 1.0;
379          }
380
381       } else {
382          /* VAD only */
383          int modeid;
384          if (st->relative_quality<2.0)
385             modeid=1;
386          else
387             modeid=st->submodeSelect;
388          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
389          st->submodeID=modeid;
390
391       }
392       /*fprintf (stderr, "%f %f\n", ratio, low_qual);*/
393    }
394
395    if (st->encode_submode)
396    {
397       speex_bits_pack(bits, 1, 1);
398       if (dtx)
399          speex_bits_pack(bits, 0, SB_SUBMODE_BITS);
400       else
401          speex_bits_pack(bits, st->submodeID, SB_SUBMODE_BITS);
402    }
403
404    /* If null mode (no transmission), just set a couple things to zero*/
405    if (dtx || st->submodes[st->submodeID] == NULL)
406    {
407       for (i=0;i<st->frame_size;i++)
408          st->exc[i]=st->sw[i]=0;
409
410       for (i=0;i<st->lpcSize;i++)
411          st->mem_sw[i]=0;
412       st->first=1;
413
414       /* Final signal synthesis from excitation */
415       iir_mem2(st->exc, st->interp_qlpc, st->high, st->subframeSize, st->lpcSize, st->mem_sp);
416
417 #ifndef RELEASE
418
419       /* Reconstruct the original */
420       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
421       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
422
423       for (i=0;i<st->full_frame_size;i++)
424          in[i]=2*(st->y0[i]-st->y1[i]);
425 #endif
426
427       if (dtx)
428          return 0;
429       else
430          return 1;
431    }
432
433
434    /* LSP quantization */
435    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);   
436
437    if (st->first)
438    {
439       for (i=0;i<st->lpcSize;i++)
440          st->old_lsp[i] = st->lsp[i];
441       for (i=0;i<st->lpcSize;i++)
442          st->old_qlsp[i] = st->qlsp[i];
443    }
444    
445    mem=PUSH(stack, st->lpcSize, spx_mem_t);
446    syn_resp=PUSH(stack, st->subframeSize, float);
447    innov = PUSH(stack, st->subframeSize, float);
448
449    for (sub=0;sub<st->nbSubframes;sub++)
450    {
451       float *exc, *sp, *res, *target, *sw, tmp, filter_ratio;
452       int offset;
453       float rl, rh, eh=0, el=0;
454       int fold;
455
456       offset = st->subframeSize*sub;
457       sp=st->high+offset;
458       exc=st->exc+offset;
459       res=st->res+offset;
460       target=st->target+offset;
461       sw=st->sw+offset;
462       
463       /* LSP interpolation (quantized and unquantized) */
464       tmp = (1.0 + sub)/st->nbSubframes;
465       for (i=0;i<st->lpcSize;i++)
466          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
467       for (i=0;i<st->lpcSize;i++)
468          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
469       
470       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .05);
471       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .05);
472
473       /* Compute interpolated LPCs (quantized and unquantized) */
474       for (i=0;i<st->lpcSize;i++)
475          st->interp_lsp[i] = cos(st->interp_lsp[i]);
476       for (i=0;i<st->lpcSize;i++)
477          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
478
479
480       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
481       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
482
483       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
484       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
485
486       /* Compute mid-band (4000 Hz for wideband) response of low-band and high-band
487          filters */
488       rl=rh=0;
489       tmp=1;
490       st->pi_gain[sub]=0;
491       for (i=0;i<=st->lpcSize;i++)
492       {
493          rh += tmp*st->interp_qlpc[i];
494          tmp = -tmp;
495          st->pi_gain[sub]+=st->interp_qlpc[i];
496       }
497       rl = low_pi_gain[sub];
498       rl=1/(fabs(rl)+.01);
499       rh=1/(fabs(rh)+.01);
500       /* Compute ratio, will help predict the gain */
501       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
502
503       fold = filter_ratio<5;
504       /*printf ("filter_ratio %f\n", filter_ratio);*/
505       fold=0;
506
507       /* Compute "real excitation" */
508       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
509       /* Compute energy of low-band and high-band excitation */
510       for (i=0;i<st->subframeSize;i++)
511          eh+=sqr(exc[i]);
512
513       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
514          float g;
515          /*speex_bits_pack(bits, 1, 1);*/
516          for (i=0;i<st->subframeSize;i++)
517             el+=sqr(low_innov[offset+i]);
518
519          /* Gain to use if we want to use the low-band excitation for high-band */
520          g=eh/(.01+el);
521          g=sqrt(g);
522
523          g *= filter_ratio;
524          /*print_vec(&g, 1, "gain factor");*/
525          /* Gain quantization */
526          {
527             int quant = (int) floor(.5 + 10 + 8.0 * log((g+.0001)));
528             /*speex_warning_int("tata", quant);*/
529             if (quant<0)
530                quant=0;
531             if (quant>31)
532                quant=31;
533             speex_bits_pack(bits, quant, 5);
534             g= .1*exp(quant/9.4);
535          }
536          /*printf ("folding gain: %f\n", g);*/
537          g /= filter_ratio;
538
539       } else {
540          float gc, scale, scale_1;
541
542          for (i=0;i<st->subframeSize;i++)
543             el+=sqr(low_exc[offset+i]);
544          /*speex_bits_pack(bits, 0, 1);*/
545
546          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
547          {
548             int qgc = (int)floor(.5+3.7*(log(gc)+2));
549             if (qgc<0)
550                qgc=0;
551             if (qgc>15)
552                qgc=15;
553             speex_bits_pack(bits, qgc, 4);
554             gc = exp((1/3.7)*qgc-2);
555          }
556
557          scale = gc*sqrt(1+el)/filter_ratio;
558          scale_1 = 1/scale;
559
560          for (i=0;i<st->subframeSize;i++)
561             exc[i]=0;
562          exc[0]=1;
563          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
564          
565          /* Reset excitation */
566          for (i=0;i<st->subframeSize;i++)
567             exc[i]=0;
568          
569          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
570          for (i=0;i<st->lpcSize;i++)
571             mem[i]=st->mem_sp[i];
572          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
573
574          for (i=0;i<st->lpcSize;i++)
575             mem[i]=st->mem_sw[i];
576          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
577
578          /* Compute weighted signal */
579          for (i=0;i<st->lpcSize;i++)
580             mem[i]=st->mem_sw[i];
581          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
582
583          /* Compute target signal */
584          for (i=0;i<st->subframeSize;i++)
585             target[i]=sw[i]-res[i];
586
587          for (i=0;i<st->subframeSize;i++)
588            exc[i]=0;
589
590
591          for (i=0;i<st->subframeSize;i++)
592             target[i]*=scale_1;
593          
594          /* Reset excitation */
595          for (i=0;i<st->subframeSize;i++)
596             innov[i]=0;
597
598          /*print_vec(target, st->subframeSize, "\ntarget");*/
599          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
600                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
601                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
602          /*print_vec(target, st->subframeSize, "after");*/
603
604          for (i=0;i<st->subframeSize;i++)
605             exc[i] += innov[i]*scale;
606
607          if (SUBMODE(double_codebook)) {
608             char *tmp_stack=stack;
609             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
610             for (i=0;i<st->subframeSize;i++)
611                innov2[i]=0;
612             for (i=0;i<st->subframeSize;i++)
613                target[i]*=2.5;
614             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
615                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
616                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
617             for (i=0;i<st->subframeSize;i++)
618                innov2[i]*=scale*(1/2.5);
619             for (i=0;i<st->subframeSize;i++)
620                exc[i] += innov2[i];
621          }
622
623       }
624
625          /*Keep the previous memory*/
626          for (i=0;i<st->lpcSize;i++)
627             mem[i]=st->mem_sp[i];
628          /* Final signal synthesis from excitation */
629          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
630          
631          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
632          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
633    }
634
635
636 #ifndef RELEASE
637
638    /* Reconstruct the original */
639    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
640    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
641
642    for (i=0;i<st->full_frame_size;i++)
643       in[i]=2*(st->y0[i]-st->y1[i]);
644 #endif
645    for (i=0;i<st->lpcSize;i++)
646       st->old_lsp[i] = st->lsp[i];
647    for (i=0;i<st->lpcSize;i++)
648       st->old_qlsp[i] = st->qlsp[i];
649
650    st->first=0;
651
652    return 1;
653 }
654
655
656
657
658
659 void *sb_decoder_init(SpeexMode *m)
660 {
661    SBDecState *st;
662    SpeexSBMode *mode;
663    st = (SBDecState*)speex_alloc(sizeof(SBDecState)+6000*sizeof(float));
664    st->mode = m;
665    mode=(SpeexSBMode*)m->mode;
666
667    st->encode_submode = 1;
668
669    st->stack = ((char*)st) + sizeof(SBDecState);
670
671
672
673    st->st_low = speex_decoder_init(mode->nb_mode);
674    st->full_frame_size = 2*mode->frameSize;
675    st->frame_size = mode->frameSize;
676    st->subframeSize = mode->subframeSize;
677    st->nbSubframes = mode->frameSize/mode->subframeSize;
678    st->lpcSize=mode->lpcSize;
679    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
680    st->sampling_rate*=2;
681
682    st->submodes=mode->submodes;
683    st->submodeID=mode->defaultSubmode;
684
685    st->first=1;
686
687
688    st->x0d=PUSH(st->stack, st->frame_size, float);
689    st->x1d=PUSH(st->stack, st->frame_size, float);
690    st->high=PUSH(st->stack, st->full_frame_size, float);
691    st->y0=PUSH(st->stack, st->full_frame_size, float);
692    st->y1=PUSH(st->stack, st->full_frame_size, float);
693
694    st->h0_mem=PUSH(st->stack, QMF_ORDER, float);
695    st->h1_mem=PUSH(st->stack, QMF_ORDER, float);
696    st->g0_mem=PUSH(st->stack, QMF_ORDER, float);
697    st->g1_mem=PUSH(st->stack, QMF_ORDER, float);
698
699    st->exc=PUSH(st->stack, st->frame_size, float);
700
701    st->qlsp = PUSH(st->stack, st->lpcSize, float);
702    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
703    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
704    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
705
706    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
707    st->mem_sp = PUSH(st->stack, 2*st->lpcSize, spx_mem_t);
708    
709    st->lpc_enh_enabled=0;
710
711    return st;
712 }
713
714 void sb_decoder_destroy(void *state)
715 {
716    SBDecState *st;
717    st = (SBDecState*)state;
718    speex_decoder_destroy(st->st_low);
719
720    speex_free(state);
721 }
722
723 static void sb_decode_lost(SBDecState *st, float *out, int dtx, char *stack)
724 {
725    int i;
726    spx_coef_t *awk1, *awk2, *awk3;
727    int saved_modeid=0;
728
729    if (dtx)
730    {
731       saved_modeid=st->submodeID;
732       st->submodeID=1;
733    } else {
734       bw_lpc(0.99, st->interp_qlpc, st->interp_qlpc, st->lpcSize);
735    }
736
737    st->first=1;
738    
739    awk1=PUSH(stack, st->lpcSize+1, spx_coef_t);
740    awk2=PUSH(stack, st->lpcSize+1, spx_coef_t);
741    awk3=PUSH(stack, st->lpcSize+1, spx_coef_t);
742    
743    if (st->lpc_enh_enabled)
744    {
745       float r=.9;
746       
747       float k1,k2,k3;
748       if (st->submodes[st->submodeID] != NULL)
749       {
750          k1=SUBMODE(lpc_enh_k1);
751          k2=SUBMODE(lpc_enh_k2);
752       } else {
753          k1=k2=.7;
754       }
755       k3=(1-(1-r*k1)/(1-r*k2))/r;
756       k3=k1-k2;
757       if (!st->lpc_enh_enabled)
758       {
759          k1=k2;
760          k3=0;
761       }
762       bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
763       bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
764       bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
765       /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
766    }
767    
768    
769    /* Final signal synthesis from excitation */
770    if (!dtx)
771    {
772       for (i=0;i<st->frame_size;i++)
773          st->exc[i] *= .9;
774    }
775
776    for (i=0;i<st->frame_size;i++)
777       st->high[i]=st->exc[i];
778
779    if (st->lpc_enh_enabled)
780    {
781       /* Use enhanced LPC filter */
782       filter_mem2(st->high, awk2, awk1, st->high, st->frame_size, st->lpcSize, 
783                   st->mem_sp+st->lpcSize);
784       filter_mem2(st->high, awk3, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
785                   st->mem_sp);
786    } else {
787       /* Use regular filter */
788       for (i=0;i<st->lpcSize;i++)
789          st->mem_sp[st->lpcSize+i] = 0;
790       iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
791                st->mem_sp);
792    }
793    
794    /*iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);*/
795    
796    /* Reconstruct the original */
797    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
798    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
799
800    for (i=0;i<st->full_frame_size;i++)
801       out[i]=2*(st->y0[i]-st->y1[i]);
802    
803    if (dtx)
804    {
805       st->submodeID=saved_modeid;
806    }
807
808    return;
809 }
810
811 int sb_decode(void *state, SpeexBits *bits, float *out)
812 {
813    int i, sub;
814    SBDecState *st;
815    int wideband;
816    int ret;
817    char *stack;
818    float *low_pi_gain, *low_exc, *low_innov;
819    spx_coef_t *awk1, *awk2, *awk3;
820    int dtx;
821    SpeexSBMode *mode;
822
823    st = (SBDecState*)state;
824    stack=st->stack;
825    mode = (SpeexSBMode*)(st->mode->mode);
826
827    /* Decode the low-band */
828    ret = speex_decode(st->st_low, bits, st->x0d);
829
830    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
831
832    /* If error decoding the narrowband part, propagate error */
833    if (ret!=0)
834    {
835       return ret;
836    }
837
838    if (!bits)
839    {
840       sb_decode_lost(st, out, dtx, stack);
841       return 0;
842    }
843
844    if (st->encode_submode)
845    {
846
847       /*Check "wideband bit"*/
848       if (speex_bits_remaining(bits)>0)
849          wideband = speex_bits_peek(bits);
850       else
851          wideband = 0;
852       if (wideband)
853       {
854          /*Regular wideband frame, read the submode*/
855          wideband = speex_bits_unpack_unsigned(bits, 1);
856          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
857       } else
858       {
859          /*Was a narrowband frame, set "null submode"*/
860          st->submodeID = 0;
861       }
862       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
863       {
864          speex_warning("Invalid mode encountered: corrupted stream?");
865          return -2;
866       }
867    }
868
869    /* If null mode (no transmission), just set a couple things to zero*/
870    if (st->submodes[st->submodeID] == NULL)
871    {
872       if (dtx)
873       {
874          sb_decode_lost(st, out, 1, stack);
875          return 0;
876       }
877
878       for (i=0;i<st->frame_size;i++)
879          st->exc[i]=0;
880
881       st->first=1;
882
883       /* Final signal synthesis from excitation */
884       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
885
886       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
887       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
888
889       for (i=0;i<st->full_frame_size;i++)
890          out[i]=2*(st->y0[i]-st->y1[i]);
891
892       return 0;
893
894    }
895
896    for (i=0;i<st->frame_size;i++)
897       st->exc[i]=0;
898
899    low_pi_gain = PUSH(stack, st->nbSubframes, float);
900    low_exc = PUSH(stack, st->frame_size, float);
901    low_innov = PUSH(stack, st->frame_size, float);
902    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
903    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
904    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
905
906    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
907    
908    if (st->first)
909    {
910       for (i=0;i<st->lpcSize;i++)
911          st->old_qlsp[i] = st->qlsp[i];
912    }
913    
914    awk1=PUSH(stack, st->lpcSize+1, spx_coef_t);
915    awk2=PUSH(stack, st->lpcSize+1, spx_coef_t);
916    awk3=PUSH(stack, st->lpcSize+1, spx_coef_t);
917
918    for (sub=0;sub<st->nbSubframes;sub++)
919    {
920       float *exc, *sp, tmp, filter_ratio, el=0;
921       int offset;
922       float rl=0,rh=0;
923       
924       offset = st->subframeSize*sub;
925       sp=st->high+offset;
926       exc=st->exc+offset;
927       
928       /* LSP interpolation */
929       tmp = (1.0 + sub)/st->nbSubframes;
930       for (i=0;i<st->lpcSize;i++)
931          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
932
933       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .05);
934
935       /* LSPs to x-domain */
936       for (i=0;i<st->lpcSize;i++)
937          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
938
939       /* LSP to LPC */
940       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
941
942
943       if (st->lpc_enh_enabled)
944       {
945          float r=.9;
946          
947          float k1,k2,k3;
948          k1=SUBMODE(lpc_enh_k1);
949          k2=SUBMODE(lpc_enh_k2);
950          k3=(1-(1-r*k1)/(1-r*k2))/r;
951          k3=k1-k2;
952          if (!st->lpc_enh_enabled)
953          {
954             k1=k2;
955             k3=0;
956          }
957          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
958          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
959          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
960          /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
961       }
962
963
964       /* Calculate reponse ratio between the low and high filter in the middle
965          of the band (4000 Hz) */
966       
967          tmp=1;
968          st->pi_gain[sub]=0;
969          for (i=0;i<=st->lpcSize;i++)
970          {
971             rh += tmp*st->interp_qlpc[i];
972             tmp = -tmp;
973             st->pi_gain[sub]+=st->interp_qlpc[i];
974          }
975          rl = low_pi_gain[sub];
976          rl=1/(fabs(rl)+.01);
977          rh=1/(fabs(rh)+.01);
978          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
979       
980       
981       for (i=0;i<st->subframeSize;i++)
982          exc[i]=0;
983       if (!SUBMODE(innovation_unquant))
984       {
985          float g;
986          int quant;
987
988          for (i=0;i<st->subframeSize;i++)
989             el+=sqr(low_innov[offset+i]);
990          quant = speex_bits_unpack_unsigned(bits, 5);
991          g= exp(((float)quant-10)/8.0);
992          
993          /*printf ("unquant folding gain: %f\n", g);*/
994          g /= filter_ratio;
995          
996          /* High-band excitation using the low-band excitation and a gain */
997          for (i=0;i<st->subframeSize;i++)
998             exc[i]=mode->folding_gain*g*low_innov[offset+i];
999          /*speex_rand_vec(mode->folding_gain*g*sqrt(el/st->subframeSize), exc, st->subframeSize);*/
1000       } else {
1001          float gc, scale;
1002          int qgc = speex_bits_unpack_unsigned(bits, 4);
1003          for (i=0;i<st->subframeSize;i++)
1004             el+=sqr(low_exc[offset+i]);
1005
1006
1007          gc = exp((1/3.7)*qgc-2);
1008
1009          scale = gc*sqrt(1+el)/filter_ratio;
1010
1011
1012          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1013                                 bits, stack);
1014          for (i=0;i<st->subframeSize;i++)
1015             exc[i]*=scale;
1016
1017          if (SUBMODE(double_codebook)) {
1018             char *tmp_stack=stack;
1019             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1020             for (i=0;i<st->subframeSize;i++)
1021                innov2[i]=0;
1022             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1023                                 bits, tmp_stack);
1024             for (i=0;i<st->subframeSize;i++)
1025                innov2[i]*=scale*(1/2.5);
1026             for (i=0;i<st->subframeSize;i++)
1027                exc[i] += innov2[i];
1028          }
1029
1030       }
1031
1032       for (i=0;i<st->subframeSize;i++)
1033          sp[i]=exc[i];
1034       if (st->lpc_enh_enabled)
1035       {
1036          /* Use enhanced LPC filter */
1037          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1038                      st->mem_sp+st->lpcSize);
1039          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1040                      st->mem_sp);
1041       } else {
1042          /* Use regular filter */
1043          for (i=0;i<st->lpcSize;i++)
1044             st->mem_sp[st->lpcSize+i] = 0;
1045          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1046                      st->mem_sp);
1047       }
1048       /*iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);*/
1049
1050    }
1051
1052    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1053    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1054
1055    for (i=0;i<st->full_frame_size;i++)
1056       out[i]=2*(st->y0[i]-st->y1[i]);
1057
1058    for (i=0;i<st->lpcSize;i++)
1059       st->old_qlsp[i] = st->qlsp[i];
1060
1061    st->first=0;
1062
1063    return 0;
1064 }
1065
1066
1067 int sb_encoder_ctl(void *state, int request, void *ptr)
1068 {
1069    SBEncState *st;
1070    st=(SBEncState*)state;
1071    switch(request)
1072    {
1073    case SPEEX_GET_FRAME_SIZE:
1074       (*(int*)ptr) = st->full_frame_size;
1075       break;
1076    case SPEEX_SET_HIGH_MODE:
1077       st->submodeSelect = st->submodeID = (*(int*)ptr);
1078       break;
1079    case SPEEX_SET_LOW_MODE:
1080       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1081       break;
1082    case SPEEX_SET_DTX:
1083       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
1084       break;
1085    case SPEEX_GET_DTX:
1086       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
1087       break;
1088    case SPEEX_GET_LOW_MODE:
1089       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1090       break;
1091    case SPEEX_SET_MODE:
1092       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1093       break;
1094    case SPEEX_SET_VBR:
1095       st->vbr_enabled = (*(int*)ptr);
1096       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1097       break;
1098    case SPEEX_GET_VBR:
1099       (*(int*)ptr) = st->vbr_enabled;
1100       break;
1101    case SPEEX_SET_VAD:
1102       st->vad_enabled = (*(int*)ptr);
1103       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1104       break;
1105    case SPEEX_GET_VAD:
1106       (*(int*)ptr) = st->vad_enabled;
1107       break;
1108    case SPEEX_SET_VBR_QUALITY:
1109       {
1110          int q;
1111          float qual = (*(float*)ptr)+.6;
1112          st->vbr_quality = (*(float*)ptr);
1113          if (qual>10)
1114             qual=10;
1115          q=(int)floor(.5+*(float*)ptr);
1116          if (q>10)
1117             q=10;
1118          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1119          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1120          break;
1121       }
1122    case SPEEX_SET_ABR:
1123       st->abr_enabled = (*(int*)ptr);
1124       st->vbr_enabled = 1;
1125       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
1126       {
1127          int i=10, rate, target;
1128          float vbr_qual;
1129          target = (*(int*)ptr);
1130          while (i>=0)
1131          {
1132             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1133             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1134             if (rate <= target)
1135                break;
1136             i--;
1137          }
1138          vbr_qual=i;
1139          if (vbr_qual<0)
1140             vbr_qual=0;
1141          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1142          st->abr_count=0;
1143          st->abr_drift=0;
1144          st->abr_drift2=0;
1145       }
1146       
1147       break;
1148    case SPEEX_GET_ABR:
1149       (*(int*)ptr) = st->abr_enabled;
1150       break;
1151    case SPEEX_SET_QUALITY:
1152       {
1153          int nb_qual;
1154          int quality = (*(int*)ptr);
1155          if (quality < 0)
1156             quality = 0;
1157          if (quality > 10)
1158             quality = 10;
1159          st->submodeSelect = st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1160          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1161          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1162       }
1163       break;
1164    case SPEEX_SET_COMPLEXITY:
1165       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1166       st->complexity = (*(int*)ptr);
1167       if (st->complexity<1)
1168          st->complexity=1;
1169       break;
1170    case SPEEX_GET_COMPLEXITY:
1171       (*(int*)ptr) = st->complexity;
1172       break;
1173    case SPEEX_SET_BITRATE:
1174       {
1175          int i=10, rate, target;
1176          target = (*(int*)ptr);
1177          while (i>=0)
1178          {
1179             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1180             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1181             if (rate <= target)
1182                break;
1183             i--;
1184          }
1185       }
1186       break;
1187    case SPEEX_GET_BITRATE:
1188       speex_encoder_ctl(st->st_low, request, ptr);
1189       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1190       if (st->submodes[st->submodeID])
1191          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1192       else
1193          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1194       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1195       break;
1196    case SPEEX_SET_SAMPLING_RATE:
1197       {
1198          int tmp=(*(int*)ptr);
1199          st->sampling_rate = tmp;
1200          tmp>>=1;
1201          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1202       }
1203       break;
1204    case SPEEX_GET_SAMPLING_RATE:
1205       (*(int*)ptr)=st->sampling_rate;
1206       break;
1207    case SPEEX_RESET_STATE:
1208       {
1209          int i;
1210          st->first = 1;
1211          for (i=0;i<st->lpcSize;i++)
1212             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1213          for (i=0;i<st->lpcSize;i++)
1214             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1215          for (i=0;i<st->bufSize;i++)
1216             st->excBuf[i]=0;
1217          for (i=0;i<QMF_ORDER;i++)
1218             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1219       }
1220       break;
1221    case SPEEX_SET_SUBMODE_ENCODING:
1222       st->encode_submode = (*(int*)ptr);
1223       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1224       break;
1225    case SPEEX_GET_SUBMODE_ENCODING:
1226       (*(int*)ptr) = st->encode_submode;
1227       break;
1228    case SPEEX_GET_PI_GAIN:
1229       {
1230          int i;
1231          float *g = (float*)ptr;
1232          for (i=0;i<st->nbSubframes;i++)
1233             g[i]=st->pi_gain[i];
1234       }
1235       break;
1236    case SPEEX_GET_EXC:
1237       {
1238          int i;
1239          float *e = (float*)ptr;
1240          for (i=0;i<st->full_frame_size;i++)
1241             e[i]=0;
1242          for (i=0;i<st->frame_size;i++)
1243             e[2*i]=2*st->exc[i];
1244       }
1245       break;
1246    case SPEEX_GET_INNOV:
1247       {
1248          int i;
1249          float *e = (float*)ptr;
1250          for (i=0;i<st->full_frame_size;i++)
1251             e[i]=0;
1252          for (i=0;i<st->frame_size;i++)
1253             e[2*i]=2*st->exc[i];
1254       }
1255       break;
1256    case SPEEX_GET_RELATIVE_QUALITY:
1257       (*(float*)ptr)=st->relative_quality;
1258       break;
1259    default:
1260       speex_warning_int("Unknown nb_ctl request: ", request);
1261       return -1;
1262    }
1263    return 0;
1264 }
1265
1266 int sb_decoder_ctl(void *state, int request, void *ptr)
1267 {
1268    SBDecState *st;
1269    st=(SBDecState*)state;
1270    switch(request)
1271    {
1272    case SPEEX_SET_HIGH_MODE:
1273       st->submodeID = (*(int*)ptr);
1274       break;
1275    case SPEEX_SET_LOW_MODE:
1276       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1277       break;
1278    case SPEEX_GET_LOW_MODE:
1279       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1280       break;
1281    case SPEEX_GET_FRAME_SIZE:
1282       (*(int*)ptr) = st->full_frame_size;
1283       break;
1284    case SPEEX_SET_ENH:
1285       speex_decoder_ctl(st->st_low, request, ptr);
1286       st->lpc_enh_enabled = *((int*)ptr);
1287       break;
1288    case SPEEX_SET_MODE:
1289    case SPEEX_SET_QUALITY:
1290       {
1291          int nb_qual;
1292          int quality = (*(int*)ptr);
1293          if (quality < 0)
1294             quality = 0;
1295          if (quality > 10)
1296             quality = 10;
1297          st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1298          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1299          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1300       }
1301       break;
1302    case SPEEX_GET_BITRATE:
1303       speex_decoder_ctl(st->st_low, request, ptr);
1304       if (st->submodes[st->submodeID])
1305          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1306       else
1307          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1308       break;
1309    case SPEEX_SET_SAMPLING_RATE:
1310       {
1311          int tmp=(*(int*)ptr);
1312          st->sampling_rate = tmp;
1313          tmp>>=1;
1314          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1315       }
1316       break;
1317    case SPEEX_GET_SAMPLING_RATE:
1318       (*(int*)ptr)=st->sampling_rate;
1319       break;
1320    case SPEEX_SET_HANDLER:
1321       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1322       break;
1323    case SPEEX_SET_USER_HANDLER:
1324       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1325       break;
1326    case SPEEX_RESET_STATE:
1327       {
1328          int i;
1329          for (i=0;i<2*st->lpcSize;i++)
1330             st->mem_sp[i]=0;
1331          for (i=0;i<QMF_ORDER;i++)
1332             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1333       }
1334       break;
1335    case SPEEX_SET_SUBMODE_ENCODING:
1336       st->encode_submode = (*(int*)ptr);
1337       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1338       break;
1339    case SPEEX_GET_SUBMODE_ENCODING:
1340       (*(int*)ptr) = st->encode_submode;
1341       break;
1342    case SPEEX_GET_PI_GAIN:
1343       {
1344          int i;
1345          float *g = (float*)ptr;
1346          for (i=0;i<st->nbSubframes;i++)
1347             g[i]=st->pi_gain[i];
1348       }
1349       break;
1350    case SPEEX_GET_EXC:
1351       {
1352          int i;
1353          float *e = (float*)ptr;
1354          for (i=0;i<st->full_frame_size;i++)
1355             e[i]=0;
1356          for (i=0;i<st->frame_size;i++)
1357             e[2*i]=2*st->exc[i];
1358       }
1359       break;
1360    case SPEEX_GET_INNOV:
1361       {
1362          int i;
1363          float *e = (float*)ptr;
1364          for (i=0;i<st->full_frame_size;i++)
1365             e[i]=0;
1366          for (i=0;i<st->frame_size;i++)
1367             e[2*i]=2*st->exc[i];
1368       }
1369       break;
1370    case SPEEX_GET_DTX_STATUS:
1371       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
1372       break;
1373    default:
1374       speex_warning_int("Unknown nb_ctl request: ", request);
1375       return -1;
1376    }
1377    return 0;
1378 }