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