fixed-point: LPC/LSP cleanup
[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]=LPC_SCALING;
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       rh /= LPC_SCALING;
498       st->pi_gain[sub] /= LPC_SCALING;
499
500       rl = low_pi_gain[sub];
501       rl=1/(fabs(rl)+.01);
502       rh=1/(fabs(rh)+.01);
503       /* Compute ratio, will help predict the gain */
504       filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
505
506       fold = filter_ratio<5;
507       /*printf ("filter_ratio %f\n", filter_ratio);*/
508       fold=0;
509
510       /* Compute "real excitation" */
511       fir_mem2(sp, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem_sp2);
512       /* Compute energy of low-band and high-band excitation */
513       for (i=0;i<st->subframeSize;i++)
514          eh+=sqr(exc[i]);
515
516       if (!SUBMODE(innovation_quant)) {/* 1 for spectral folding excitation, 0 for stochastic */
517          float g;
518          /*speex_bits_pack(bits, 1, 1);*/
519          for (i=0;i<st->subframeSize;i++)
520             el+=sqr(low_innov[offset+i]);
521
522          /* Gain to use if we want to use the low-band excitation for high-band */
523          g=eh/(.01+el);
524          g=sqrt(g);
525
526          g *= filter_ratio;
527          /*print_vec(&g, 1, "gain factor");*/
528          /* Gain quantization */
529          {
530             int quant = (int) floor(.5 + 10 + 8.0 * log((g+.0001)));
531             /*speex_warning_int("tata", quant);*/
532             if (quant<0)
533                quant=0;
534             if (quant>31)
535                quant=31;
536             speex_bits_pack(bits, quant, 5);
537             g= .1*exp(quant/9.4);
538          }
539          /*printf ("folding gain: %f\n", g);*/
540          g /= filter_ratio;
541
542       } else {
543          float gc, scale, scale_1;
544
545          for (i=0;i<st->subframeSize;i++)
546             el+=sqr(low_exc[offset+i]);
547          /*speex_bits_pack(bits, 0, 1);*/
548
549          gc = sqrt(1+eh)*filter_ratio/sqrt((1+el)*st->subframeSize);
550          {
551             int qgc = (int)floor(.5+3.7*(log(gc)+2));
552             if (qgc<0)
553                qgc=0;
554             if (qgc>15)
555                qgc=15;
556             speex_bits_pack(bits, qgc, 4);
557             gc = exp((1/3.7)*qgc-2);
558          }
559
560          scale = gc*sqrt(1+el)/filter_ratio;
561          scale_1 = 1/scale;
562
563          for (i=0;i<st->subframeSize;i++)
564             exc[i]=0;
565          exc[0]=1;
566          syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
567          
568          /* Reset excitation */
569          for (i=0;i<st->subframeSize;i++)
570             exc[i]=0;
571          
572          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
573          for (i=0;i<st->lpcSize;i++)
574             mem[i]=st->mem_sp[i];
575          iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
576
577          for (i=0;i<st->lpcSize;i++)
578             mem[i]=st->mem_sw[i];
579          filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
580
581          /* Compute weighted signal */
582          for (i=0;i<st->lpcSize;i++)
583             mem[i]=st->mem_sw[i];
584          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
585
586          /* Compute target signal */
587          for (i=0;i<st->subframeSize;i++)
588             target[i]=sw[i]-res[i];
589
590          for (i=0;i<st->subframeSize;i++)
591            exc[i]=0;
592
593
594          for (i=0;i<st->subframeSize;i++)
595             target[i]*=scale_1;
596          
597          /* Reset excitation */
598          for (i=0;i<st->subframeSize;i++)
599             innov[i]=0;
600
601          /*print_vec(target, st->subframeSize, "\ntarget");*/
602          SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
603                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
604                                    innov, syn_resp, bits, stack, (st->complexity+1)>>1);
605          /*print_vec(target, st->subframeSize, "after");*/
606
607          for (i=0;i<st->subframeSize;i++)
608             exc[i] += innov[i]*scale;
609
610          if (SUBMODE(double_codebook)) {
611             char *tmp_stack=stack;
612             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
613             for (i=0;i<st->subframeSize;i++)
614                innov2[i]=0;
615             for (i=0;i<st->subframeSize;i++)
616                target[i]*=2.5;
617             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
618                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
619                                       innov2, syn_resp, bits, tmp_stack, (st->complexity+1)>>1);
620             for (i=0;i<st->subframeSize;i++)
621                innov2[i]*=scale*(1/2.5);
622             for (i=0;i<st->subframeSize;i++)
623                exc[i] += innov2[i];
624          }
625
626       }
627
628          /*Keep the previous memory*/
629          for (i=0;i<st->lpcSize;i++)
630             mem[i]=st->mem_sp[i];
631          /* Final signal synthesis from excitation */
632          iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
633          
634          /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
635          filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
636    }
637
638
639 #ifndef RELEASE
640
641    /* Reconstruct the original */
642    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
643    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
644
645    for (i=0;i<st->full_frame_size;i++)
646       in[i]=2*(st->y0[i]-st->y1[i]);
647 #endif
648    for (i=0;i<st->lpcSize;i++)
649       st->old_lsp[i] = st->lsp[i];
650    for (i=0;i<st->lpcSize;i++)
651       st->old_qlsp[i] = st->qlsp[i];
652
653    st->first=0;
654
655    return 1;
656 }
657
658
659
660
661
662 void *sb_decoder_init(SpeexMode *m)
663 {
664    SBDecState *st;
665    SpeexSBMode *mode;
666    st = (SBDecState*)speex_alloc(sizeof(SBDecState)+6000*sizeof(float));
667    st->mode = m;
668    mode=(SpeexSBMode*)m->mode;
669
670    st->encode_submode = 1;
671
672    st->stack = ((char*)st) + sizeof(SBDecState);
673
674
675
676    st->st_low = speex_decoder_init(mode->nb_mode);
677    st->full_frame_size = 2*mode->frameSize;
678    st->frame_size = mode->frameSize;
679    st->subframeSize = mode->subframeSize;
680    st->nbSubframes = mode->frameSize/mode->subframeSize;
681    st->lpcSize=mode->lpcSize;
682    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
683    st->sampling_rate*=2;
684
685    st->submodes=mode->submodes;
686    st->submodeID=mode->defaultSubmode;
687
688    st->first=1;
689
690
691    st->x0d=PUSH(st->stack, st->frame_size, float);
692    st->x1d=PUSH(st->stack, st->frame_size, float);
693    st->high=PUSH(st->stack, st->full_frame_size, float);
694    st->y0=PUSH(st->stack, st->full_frame_size, float);
695    st->y1=PUSH(st->stack, st->full_frame_size, float);
696
697    st->h0_mem=PUSH(st->stack, QMF_ORDER, float);
698    st->h1_mem=PUSH(st->stack, QMF_ORDER, float);
699    st->g0_mem=PUSH(st->stack, QMF_ORDER, float);
700    st->g1_mem=PUSH(st->stack, QMF_ORDER, float);
701
702    st->exc=PUSH(st->stack, st->frame_size, float);
703
704    st->qlsp = PUSH(st->stack, st->lpcSize, float);
705    st->old_qlsp = PUSH(st->stack, st->lpcSize, float);
706    st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);
707    st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, spx_coef_t);
708
709    st->pi_gain = PUSH(st->stack, st->nbSubframes, float);
710    st->mem_sp = PUSH(st->stack, 2*st->lpcSize, spx_mem_t);
711    
712    st->lpc_enh_enabled=0;
713
714    return st;
715 }
716
717 void sb_decoder_destroy(void *state)
718 {
719    SBDecState *st;
720    st = (SBDecState*)state;
721    speex_decoder_destroy(st->st_low);
722
723    speex_free(state);
724 }
725
726 static void sb_decode_lost(SBDecState *st, float *out, int dtx, char *stack)
727 {
728    int i;
729    spx_coef_t *awk1, *awk2, *awk3;
730    int saved_modeid=0;
731
732    if (dtx)
733    {
734       saved_modeid=st->submodeID;
735       st->submodeID=1;
736    } else {
737       bw_lpc(0.99, st->interp_qlpc, st->interp_qlpc, st->lpcSize);
738    }
739
740    st->first=1;
741    
742    awk1=PUSH(stack, st->lpcSize+1, spx_coef_t);
743    awk2=PUSH(stack, st->lpcSize+1, spx_coef_t);
744    awk3=PUSH(stack, st->lpcSize+1, spx_coef_t);
745    
746    if (st->lpc_enh_enabled)
747    {
748       float r=.9;
749       
750       float k1,k2,k3;
751       if (st->submodes[st->submodeID] != NULL)
752       {
753          k1=SUBMODE(lpc_enh_k1);
754          k2=SUBMODE(lpc_enh_k2);
755       } else {
756          k1=k2=.7;
757       }
758       k3=(1-(1-r*k1)/(1-r*k2))/r;
759       k3=k1-k2;
760       if (!st->lpc_enh_enabled)
761       {
762          k1=k2;
763          k3=0;
764       }
765       bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
766       bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
767       bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
768       /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
769    }
770    
771    
772    /* Final signal synthesis from excitation */
773    if (!dtx)
774    {
775       for (i=0;i<st->frame_size;i++)
776          st->exc[i] *= .9;
777    }
778
779    for (i=0;i<st->frame_size;i++)
780       st->high[i]=st->exc[i];
781
782    if (st->lpc_enh_enabled)
783    {
784       /* Use enhanced LPC filter */
785       filter_mem2(st->high, awk2, awk1, st->high, st->frame_size, st->lpcSize, 
786                   st->mem_sp+st->lpcSize);
787       filter_mem2(st->high, awk3, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
788                   st->mem_sp);
789    } else {
790       /* Use regular filter */
791       for (i=0;i<st->lpcSize;i++)
792          st->mem_sp[st->lpcSize+i] = 0;
793       iir_mem2(st->high, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, 
794                st->mem_sp);
795    }
796    
797    /*iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);*/
798    
799    /* Reconstruct the original */
800    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
801    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
802
803    for (i=0;i<st->full_frame_size;i++)
804       out[i]=2*(st->y0[i]-st->y1[i]);
805    
806    if (dtx)
807    {
808       st->submodeID=saved_modeid;
809    }
810
811    return;
812 }
813
814 int sb_decode(void *state, SpeexBits *bits, float *out)
815 {
816    int i, sub;
817    SBDecState *st;
818    int wideband;
819    int ret;
820    char *stack;
821    float *low_pi_gain, *low_exc, *low_innov;
822    spx_coef_t *awk1, *awk2, *awk3;
823    int dtx;
824    SpeexSBMode *mode;
825
826    st = (SBDecState*)state;
827    stack=st->stack;
828    mode = (SpeexSBMode*)(st->mode->mode);
829
830    /* Decode the low-band */
831    ret = speex_decode(st->st_low, bits, st->x0d);
832
833    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
834
835    /* If error decoding the narrowband part, propagate error */
836    if (ret!=0)
837    {
838       return ret;
839    }
840
841    if (!bits)
842    {
843       sb_decode_lost(st, out, dtx, stack);
844       return 0;
845    }
846
847    if (st->encode_submode)
848    {
849
850       /*Check "wideband bit"*/
851       if (speex_bits_remaining(bits)>0)
852          wideband = speex_bits_peek(bits);
853       else
854          wideband = 0;
855       if (wideband)
856       {
857          /*Regular wideband frame, read the submode*/
858          wideband = speex_bits_unpack_unsigned(bits, 1);
859          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
860       } else
861       {
862          /*Was a narrowband frame, set "null submode"*/
863          st->submodeID = 0;
864       }
865       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
866       {
867          speex_warning("Invalid mode encountered: corrupted stream?");
868          return -2;
869       }
870    }
871
872    /* If null mode (no transmission), just set a couple things to zero*/
873    if (st->submodes[st->submodeID] == NULL)
874    {
875       if (dtx)
876       {
877          sb_decode_lost(st, out, 1, stack);
878          return 0;
879       }
880
881       for (i=0;i<st->frame_size;i++)
882          st->exc[i]=0;
883
884       st->first=1;
885
886       /* Final signal synthesis from excitation */
887       iir_mem2(st->exc, st->interp_qlpc, st->high, st->frame_size, st->lpcSize, st->mem_sp);
888
889       fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
890       fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
891
892       for (i=0;i<st->full_frame_size;i++)
893          out[i]=2*(st->y0[i]-st->y1[i]);
894
895       return 0;
896
897    }
898
899    for (i=0;i<st->frame_size;i++)
900       st->exc[i]=0;
901
902    low_pi_gain = PUSH(stack, st->nbSubframes, float);
903    low_exc = PUSH(stack, st->frame_size, float);
904    low_innov = PUSH(stack, st->frame_size, float);
905    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
906    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc);
907    speex_decoder_ctl(st->st_low, SPEEX_GET_INNOV, low_innov);
908
909    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
910    
911    if (st->first)
912    {
913       for (i=0;i<st->lpcSize;i++)
914          st->old_qlsp[i] = st->qlsp[i];
915    }
916    
917    awk1=PUSH(stack, st->lpcSize+1, spx_coef_t);
918    awk2=PUSH(stack, st->lpcSize+1, spx_coef_t);
919    awk3=PUSH(stack, st->lpcSize+1, spx_coef_t);
920
921    for (sub=0;sub<st->nbSubframes;sub++)
922    {
923       float *exc, *sp, tmp, filter_ratio, el=0;
924       int offset;
925       float rl=0,rh=0;
926       
927       offset = st->subframeSize*sub;
928       sp=st->high+offset;
929       exc=st->exc+offset;
930       
931       /* LSP interpolation */
932       tmp = (1.0 + sub)/st->nbSubframes;
933       for (i=0;i<st->lpcSize;i++)
934          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
935
936       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .05);
937
938       /* LSPs to x-domain */
939       for (i=0;i<st->lpcSize;i++)
940          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
941
942       /* LSP to LPC */
943       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
944
945
946       if (st->lpc_enh_enabled)
947       {
948          float r=.9;
949          
950          float k1,k2,k3;
951          k1=SUBMODE(lpc_enh_k1);
952          k2=SUBMODE(lpc_enh_k2);
953          k3=(1-(1-r*k1)/(1-r*k2))/r;
954          k3=k1-k2;
955          if (!st->lpc_enh_enabled)
956          {
957             k1=k2;
958             k3=0;
959          }
960          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
961          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
962          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
963          /*fprintf (stderr, "%f %f %f\n", k1, k2, k3);*/
964       }
965
966
967       /* Calculate reponse ratio between the low and high filter in the middle
968          of the band (4000 Hz) */
969       
970          tmp=1;
971          st->pi_gain[sub]=0;
972          for (i=0;i<=st->lpcSize;i++)
973          {
974             rh += tmp*st->interp_qlpc[i];
975             tmp = -tmp;
976             st->pi_gain[sub]+=st->interp_qlpc[i];
977          }
978          rh /= LPC_SCALING;
979          st->pi_gain[sub] /= LPC_SCALING;
980
981          rl = low_pi_gain[sub];
982          rl=1/(fabs(rl)+.01);
983          rh=1/(fabs(rh)+.01);
984          filter_ratio=fabs(.01+rh)/(.01+fabs(rl));
985       
986       
987       for (i=0;i<st->subframeSize;i++)
988          exc[i]=0;
989       if (!SUBMODE(innovation_unquant))
990       {
991          float g;
992          int quant;
993
994          for (i=0;i<st->subframeSize;i++)
995             el+=sqr(low_innov[offset+i]);
996          quant = speex_bits_unpack_unsigned(bits, 5);
997          g= exp(((float)quant-10)/8.0);
998          
999          /*printf ("unquant folding gain: %f\n", g);*/
1000          g /= filter_ratio;
1001          
1002          /* High-band excitation using the low-band excitation and a gain */
1003          for (i=0;i<st->subframeSize;i++)
1004             exc[i]=mode->folding_gain*g*low_innov[offset+i];
1005          /*speex_rand_vec(mode->folding_gain*g*sqrt(el/st->subframeSize), exc, st->subframeSize);*/
1006       } else {
1007          float gc, scale;
1008          int qgc = speex_bits_unpack_unsigned(bits, 4);
1009          for (i=0;i<st->subframeSize;i++)
1010             el+=sqr(low_exc[offset+i]);
1011
1012
1013          gc = exp((1/3.7)*qgc-2);
1014
1015          scale = gc*sqrt(1+el)/filter_ratio;
1016
1017
1018          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1019                                 bits, stack);
1020          for (i=0;i<st->subframeSize;i++)
1021             exc[i]*=scale;
1022
1023          if (SUBMODE(double_codebook)) {
1024             char *tmp_stack=stack;
1025             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1026             for (i=0;i<st->subframeSize;i++)
1027                innov2[i]=0;
1028             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1029                                 bits, tmp_stack);
1030             for (i=0;i<st->subframeSize;i++)
1031                innov2[i]*=scale*(1/2.5);
1032             for (i=0;i<st->subframeSize;i++)
1033                exc[i] += innov2[i];
1034          }
1035
1036       }
1037
1038       for (i=0;i<st->subframeSize;i++)
1039          sp[i]=exc[i];
1040       if (st->lpc_enh_enabled)
1041       {
1042          /* Use enhanced LPC filter */
1043          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1044                      st->mem_sp+st->lpcSize);
1045          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1046                      st->mem_sp);
1047       } else {
1048          /* Use regular filter */
1049          for (i=0;i<st->lpcSize;i++)
1050             st->mem_sp[st->lpcSize+i] = 0;
1051          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1052                      st->mem_sp);
1053       }
1054       /*iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);*/
1055
1056    }
1057
1058    fir_mem_up(st->x0d, h0, st->y0, st->full_frame_size, QMF_ORDER, st->g0_mem, stack);
1059    fir_mem_up(st->high, h1, st->y1, st->full_frame_size, QMF_ORDER, st->g1_mem, stack);
1060
1061    for (i=0;i<st->full_frame_size;i++)
1062       out[i]=2*(st->y0[i]-st->y1[i]);
1063
1064    for (i=0;i<st->lpcSize;i++)
1065       st->old_qlsp[i] = st->qlsp[i];
1066
1067    st->first=0;
1068
1069    return 0;
1070 }
1071
1072
1073 int sb_encoder_ctl(void *state, int request, void *ptr)
1074 {
1075    SBEncState *st;
1076    st=(SBEncState*)state;
1077    switch(request)
1078    {
1079    case SPEEX_GET_FRAME_SIZE:
1080       (*(int*)ptr) = st->full_frame_size;
1081       break;
1082    case SPEEX_SET_HIGH_MODE:
1083       st->submodeSelect = st->submodeID = (*(int*)ptr);
1084       break;
1085    case SPEEX_SET_LOW_MODE:
1086       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1087       break;
1088    case SPEEX_SET_DTX:
1089       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
1090       break;
1091    case SPEEX_GET_DTX:
1092       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
1093       break;
1094    case SPEEX_GET_LOW_MODE:
1095       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1096       break;
1097    case SPEEX_SET_MODE:
1098       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1099       break;
1100    case SPEEX_SET_VBR:
1101       st->vbr_enabled = (*(int*)ptr);
1102       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1103       break;
1104    case SPEEX_GET_VBR:
1105       (*(int*)ptr) = st->vbr_enabled;
1106       break;
1107    case SPEEX_SET_VAD:
1108       st->vad_enabled = (*(int*)ptr);
1109       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1110       break;
1111    case SPEEX_GET_VAD:
1112       (*(int*)ptr) = st->vad_enabled;
1113       break;
1114    case SPEEX_SET_VBR_QUALITY:
1115       {
1116          int q;
1117          float qual = (*(float*)ptr)+.6;
1118          st->vbr_quality = (*(float*)ptr);
1119          if (qual>10)
1120             qual=10;
1121          q=(int)floor(.5+*(float*)ptr);
1122          if (q>10)
1123             q=10;
1124          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1125          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1126          break;
1127       }
1128    case SPEEX_SET_ABR:
1129       st->abr_enabled = (*(int*)ptr);
1130       st->vbr_enabled = 1;
1131       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
1132       {
1133          int i=10, rate, target;
1134          float vbr_qual;
1135          target = (*(int*)ptr);
1136          while (i>=0)
1137          {
1138             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1139             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1140             if (rate <= target)
1141                break;
1142             i--;
1143          }
1144          vbr_qual=i;
1145          if (vbr_qual<0)
1146             vbr_qual=0;
1147          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1148          st->abr_count=0;
1149          st->abr_drift=0;
1150          st->abr_drift2=0;
1151       }
1152       
1153       break;
1154    case SPEEX_GET_ABR:
1155       (*(int*)ptr) = st->abr_enabled;
1156       break;
1157    case SPEEX_SET_QUALITY:
1158       {
1159          int nb_qual;
1160          int quality = (*(int*)ptr);
1161          if (quality < 0)
1162             quality = 0;
1163          if (quality > 10)
1164             quality = 10;
1165          st->submodeSelect = st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1166          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1167          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1168       }
1169       break;
1170    case SPEEX_SET_COMPLEXITY:
1171       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1172       st->complexity = (*(int*)ptr);
1173       if (st->complexity<1)
1174          st->complexity=1;
1175       break;
1176    case SPEEX_GET_COMPLEXITY:
1177       (*(int*)ptr) = st->complexity;
1178       break;
1179    case SPEEX_SET_BITRATE:
1180       {
1181          int i=10, rate, target;
1182          target = (*(int*)ptr);
1183          while (i>=0)
1184          {
1185             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1186             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1187             if (rate <= target)
1188                break;
1189             i--;
1190          }
1191       }
1192       break;
1193    case SPEEX_GET_BITRATE:
1194       speex_encoder_ctl(st->st_low, request, ptr);
1195       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1196       if (st->submodes[st->submodeID])
1197          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1198       else
1199          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1200       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1201       break;
1202    case SPEEX_SET_SAMPLING_RATE:
1203       {
1204          int tmp=(*(int*)ptr);
1205          st->sampling_rate = tmp;
1206          tmp>>=1;
1207          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1208       }
1209       break;
1210    case SPEEX_GET_SAMPLING_RATE:
1211       (*(int*)ptr)=st->sampling_rate;
1212       break;
1213    case SPEEX_RESET_STATE:
1214       {
1215          int i;
1216          st->first = 1;
1217          for (i=0;i<st->lpcSize;i++)
1218             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1219          for (i=0;i<st->lpcSize;i++)
1220             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1221          for (i=0;i<st->bufSize;i++)
1222             st->excBuf[i]=0;
1223          for (i=0;i<QMF_ORDER;i++)
1224             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1225       }
1226       break;
1227    case SPEEX_SET_SUBMODE_ENCODING:
1228       st->encode_submode = (*(int*)ptr);
1229       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1230       break;
1231    case SPEEX_GET_SUBMODE_ENCODING:
1232       (*(int*)ptr) = st->encode_submode;
1233       break;
1234    case SPEEX_GET_PI_GAIN:
1235       {
1236          int i;
1237          float *g = (float*)ptr;
1238          for (i=0;i<st->nbSubframes;i++)
1239             g[i]=st->pi_gain[i];
1240       }
1241       break;
1242    case SPEEX_GET_EXC:
1243       {
1244          int i;
1245          float *e = (float*)ptr;
1246          for (i=0;i<st->full_frame_size;i++)
1247             e[i]=0;
1248          for (i=0;i<st->frame_size;i++)
1249             e[2*i]=2*st->exc[i];
1250       }
1251       break;
1252    case SPEEX_GET_INNOV:
1253       {
1254          int i;
1255          float *e = (float*)ptr;
1256          for (i=0;i<st->full_frame_size;i++)
1257             e[i]=0;
1258          for (i=0;i<st->frame_size;i++)
1259             e[2*i]=2*st->exc[i];
1260       }
1261       break;
1262    case SPEEX_GET_RELATIVE_QUALITY:
1263       (*(float*)ptr)=st->relative_quality;
1264       break;
1265    default:
1266       speex_warning_int("Unknown nb_ctl request: ", request);
1267       return -1;
1268    }
1269    return 0;
1270 }
1271
1272 int sb_decoder_ctl(void *state, int request, void *ptr)
1273 {
1274    SBDecState *st;
1275    st=(SBDecState*)state;
1276    switch(request)
1277    {
1278    case SPEEX_SET_HIGH_MODE:
1279       st->submodeID = (*(int*)ptr);
1280       break;
1281    case SPEEX_SET_LOW_MODE:
1282       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1283       break;
1284    case SPEEX_GET_LOW_MODE:
1285       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1286       break;
1287    case SPEEX_GET_FRAME_SIZE:
1288       (*(int*)ptr) = st->full_frame_size;
1289       break;
1290    case SPEEX_SET_ENH:
1291       speex_decoder_ctl(st->st_low, request, ptr);
1292       st->lpc_enh_enabled = *((int*)ptr);
1293       break;
1294    case SPEEX_SET_MODE:
1295    case SPEEX_SET_QUALITY:
1296       {
1297          int nb_qual;
1298          int quality = (*(int*)ptr);
1299          if (quality < 0)
1300             quality = 0;
1301          if (quality > 10)
1302             quality = 10;
1303          st->submodeID = ((SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1304          nb_qual = ((SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1305          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1306       }
1307       break;
1308    case SPEEX_GET_BITRATE:
1309       speex_decoder_ctl(st->st_low, request, ptr);
1310       if (st->submodes[st->submodeID])
1311          (*(int*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1312       else
1313          (*(int*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1314       break;
1315    case SPEEX_SET_SAMPLING_RATE:
1316       {
1317          int tmp=(*(int*)ptr);
1318          st->sampling_rate = tmp;
1319          tmp>>=1;
1320          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1321       }
1322       break;
1323    case SPEEX_GET_SAMPLING_RATE:
1324       (*(int*)ptr)=st->sampling_rate;
1325       break;
1326    case SPEEX_SET_HANDLER:
1327       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1328       break;
1329    case SPEEX_SET_USER_HANDLER:
1330       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1331       break;
1332    case SPEEX_RESET_STATE:
1333       {
1334          int i;
1335          for (i=0;i<2*st->lpcSize;i++)
1336             st->mem_sp[i]=0;
1337          for (i=0;i<QMF_ORDER;i++)
1338             st->h0_mem[i]=st->h1_mem[i]=st->g0_mem[i]=st->g1_mem[i]=0;
1339       }
1340       break;
1341    case SPEEX_SET_SUBMODE_ENCODING:
1342       st->encode_submode = (*(int*)ptr);
1343       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, &ptr);
1344       break;
1345    case SPEEX_GET_SUBMODE_ENCODING:
1346       (*(int*)ptr) = st->encode_submode;
1347       break;
1348    case SPEEX_GET_PI_GAIN:
1349       {
1350          int i;
1351          float *g = (float*)ptr;
1352          for (i=0;i<st->nbSubframes;i++)
1353             g[i]=st->pi_gain[i];
1354       }
1355       break;
1356    case SPEEX_GET_EXC:
1357       {
1358          int i;
1359          float *e = (float*)ptr;
1360          for (i=0;i<st->full_frame_size;i++)
1361             e[i]=0;
1362          for (i=0;i<st->frame_size;i++)
1363             e[2*i]=2*st->exc[i];
1364       }
1365       break;
1366    case SPEEX_GET_INNOV:
1367       {
1368          int i;
1369          float *e = (float*)ptr;
1370          for (i=0;i<st->full_frame_size;i++)
1371             e[i]=0;
1372          for (i=0;i<st->frame_size;i++)
1373             e[2*i]=2*st->exc[i];
1374       }
1375       break;
1376    case SPEEX_GET_DTX_STATUS:
1377       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
1378       break;
1379    default:
1380       speex_warning_int("Unknown nb_ctl request: ", request);
1381       return -1;
1382    }
1383    return 0;
1384 }