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