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