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