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