933eb7c43b6679599ed3604b6958fdc5a7630f68
[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          if (st->innov_rms_save)
633          {
634             st->innov_rms_save[sub] = eh;
635          }
636          st->exc_rms[sub] = eh;
637       } else {
638          spx_word16_t gc;       /*Q7*/
639          spx_word32_t scale;    /*Q14*/
640          spx_word16_t el;       /*Q0*/
641          el = low_exc_rms[sub]; /*Q0*/
642
643          gc = PDIV32_16(MULT16_16(filter_ratio,1+eh),1+el);
644
645          /* This is a kludge that cleans up a historical bug */
646          if (st->subframeSize==80)
647             gc = MULT16_16_P15(QCONST16(0.70711f,15),gc);
648          /*printf ("%f %f %f %f\n", el, eh, filter_ratio, gc);*/
649          {
650             int qgc = scal_quant(gc, gc_quant_bound, 16);
651             speex_bits_pack(bits, qgc, 4);
652             gc = MULT16_16_Q15(QCONST16(0.87360,15),gc_quant_bound[qgc]);
653          }
654          if (st->subframeSize==80)
655             gc = MULT16_16_P14(QCONST16(1.4142f,14), gc);
656
657          scale = SHL32(MULT16_16(PDIV32_16(SHL32(EXTEND32(gc),SIG_SHIFT-6),filter_ratio),(1+el)),6);
658
659          compute_impulse_response(st->interp_qlpc, bw_lpc1, bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
660
661          
662          /* Reset excitation */
663          for (i=0;i<st->subframeSize;i++)
664             res[i]=VERY_SMALL;
665          
666          /* Compute zero response (ringing) of A(z/g1) / ( A(z/g2) * Aq(z) ) */
667          for (i=0;i<st->lpcSize;i++)
668             mem[i]=st->mem_sp[i];
669          iir_mem16(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize, mem, stack);
670
671          for (i=0;i<st->lpcSize;i++)
672             mem[i]=st->mem_sw[i];
673          filter_mem16(res, bw_lpc1, bw_lpc2, res, st->subframeSize, st->lpcSize, mem, stack);
674
675          /* Compute weighted signal */
676          for (i=0;i<st->lpcSize;i++)
677             mem[i]=st->mem_sw[i];
678          filter_mem16(sp, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, mem, stack);
679
680          /* Compute target signal */
681          for (i=0;i<st->subframeSize;i++)
682             target[i]=SUB16(sw[i],res[i]);
683
684          signal_div(target, target, scale, st->subframeSize);
685
686          /* Reset excitation */
687          for (i=0;i<st->subframeSize;i++)
688             innov[i]=0;
689
690          /*print_vec(target, st->subframeSize, "\ntarget");*/
691          SUBMODE(innovation_quant)(target, st->interp_qlpc, bw_lpc1, bw_lpc2, 
692                                    SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
693                                    innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
694          /*print_vec(target, st->subframeSize, "after");*/
695
696          signal_mul(innov, innov, scale, st->subframeSize);
697
698          if (SUBMODE(double_codebook)) {
699             char *tmp_stack=stack;
700             VARDECL(spx_sig_t *innov2);
701             ALLOC(innov2, st->subframeSize, spx_sig_t);
702             for (i=0;i<st->subframeSize;i++)
703                innov2[i]=0;
704             for (i=0;i<st->subframeSize;i++)
705                target[i]=MULT16_16_P13(QCONST16(2.5f,13), target[i]);
706
707             SUBMODE(innovation_quant)(target, st->interp_qlpc, bw_lpc1, bw_lpc2, 
708                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
709                                       innov2, syn_resp, bits, stack, st->complexity, 0);
710             signal_mul(innov2, innov2, MULT16_32_P15(QCONST16(0.4f,15),scale), st->subframeSize);
711
712             for (i=0;i<st->subframeSize;i++)
713                innov[i] = ADD32(innov[i],innov2[i]);
714             stack = tmp_stack;
715          }
716          for (i=0;i<st->subframeSize;i++)
717             exc[i] = PSHR32(innov[i],SIG_SHIFT);
718
719          if (st->innov_rms_save)
720          {
721             st->innov_rms_save[sub] = MULT16_16_Q15(QCONST16(.70711f, 15), compute_rms(innov, st->subframeSize));
722          }
723          st->exc_rms[sub] = compute_rms16(exc, st->subframeSize);
724          
725
726       }
727
728       
729       /*Keep the previous memory*/
730       for (i=0;i<st->lpcSize;i++)
731          mem[i]=st->mem_sp[i];
732       /* Final signal synthesis from excitation */
733       iir_mem16(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp, stack);
734       
735       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
736       filter_mem16(sp, bw_lpc1, bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw, stack);
737    }
738
739    for (i=0;i<st->lpcSize;i++)
740       st->old_lsp[i] = lsp[i];
741    for (i=0;i<st->lpcSize;i++)
742       st->old_qlsp[i] = qlsp[i];
743
744    st->first=0;
745
746    return 1;
747 }
748
749
750
751
752
753 void *sb_decoder_init(const SpeexMode *m)
754 {
755    spx_int32_t tmp;
756    SBDecState *st;
757    const SpeexSBMode *mode;
758    st = (SBDecState*)speex_alloc(sizeof(SBDecState));
759    if (!st)
760       return NULL;
761 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
762    st->stack = NULL;
763 #else
764    st->stack = (char*)speex_alloc_scratch(SB_DEC_STACK);
765 #endif
766    st->mode = m;
767    mode=(const SpeexSBMode*)m->mode;
768
769    st->encode_submode = 1;
770
771
772
773
774    st->st_low = speex_decoder_init(mode->nb_mode);
775    st->full_frame_size = 2*mode->frameSize;
776    st->frame_size = mode->frameSize;
777    st->subframeSize = mode->subframeSize;
778    st->nbSubframes = mode->frameSize/mode->subframeSize;
779    st->lpcSize=mode->lpcSize;
780    speex_decoder_ctl(st->st_low, SPEEX_GET_SAMPLING_RATE, &st->sampling_rate);
781    st->sampling_rate*=2;
782    tmp=1;
783    speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, &tmp);
784
785    st->submodes=mode->submodes;
786    st->submodeID=mode->defaultSubmode;
787
788    st->first=1;
789
790    st->g0_mem = (spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
791    st->g1_mem = (spx_word32_t*)speex_alloc((QMF_ORDER)*sizeof(spx_word32_t));
792
793    st->excBuf = (spx_word16_t*)speex_alloc((st->subframeSize)*sizeof(spx_word16_t));
794
795    st->old_qlsp = (spx_lsp_t*)speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
796    st->interp_qlpc = (spx_coef_t*)speex_alloc(st->lpcSize*sizeof(spx_coef_t));
797
798    st->pi_gain = (spx_word32_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
799    st->exc_rms = (spx_word16_t*)speex_alloc((st->nbSubframes)*sizeof(spx_word16_t));
800    st->mem_sp = (spx_mem_t*)speex_alloc((2*st->lpcSize)*sizeof(spx_mem_t));
801    
802    st->innov_save = NULL;
803
804
805    st->lpc_enh_enabled=0;
806    st->seed = 1000;
807
808 #ifdef ENABLE_VALGRIND
809    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
810 #endif
811    return st;
812 }
813
814 void sb_decoder_destroy(void *state)
815 {
816    SBDecState *st;
817    st = (SBDecState*)state;
818    speex_decoder_destroy(st->st_low);
819 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
820    speex_free_scratch(st->stack);
821 #endif
822
823    speex_free(st->g0_mem);
824    speex_free(st->g1_mem);
825    speex_free(st->excBuf);
826    speex_free(st->old_qlsp);
827    speex_free(st->interp_qlpc);
828    speex_free(st->pi_gain);
829    speex_free(st->exc_rms);
830    speex_free(st->mem_sp);
831
832    speex_free(state);
833 }
834
835 static void sb_decode_lost(SBDecState *st, spx_word16_t *out, int dtx, char *stack)
836 {
837    int i;
838    int saved_modeid=0;
839
840    if (dtx)
841    {
842       saved_modeid=st->submodeID;
843       st->submodeID=1;
844    } else {
845       bw_lpc(QCONST16(0.99f,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
846    }
847
848    st->first=1;
849    
850    
851    /* Final signal synthesis from excitation */
852    if (!dtx)
853    {
854       st->last_ener =  MULT16_16_Q15(QCONST16(.9f,15),st->last_ener);
855    }
856    for (i=0;i<st->frame_size;i++)
857       out[i+st->frame_size] = speex_rand(st->last_ener, &st->seed);
858
859    iir_mem16(out+st->frame_size, st->interp_qlpc, out+st->frame_size, st->frame_size, st->lpcSize, 
860             st->mem_sp, stack);
861    
862    
863    /* Reconstruct the original */
864    qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
865    if (dtx)
866    {
867       st->submodeID=saved_modeid;
868    }
869
870    return;
871 }
872
873 int sb_decode(void *state, SpeexBits *bits, void *vout)
874 {
875    int i, sub;
876    SBDecState *st;
877    int wideband;
878    int ret;
879    char *stack;
880    VARDECL(spx_word32_t *low_pi_gain);
881    VARDECL(spx_word16_t *low_exc_rms);
882    VARDECL(spx_coef_t *ak);
883    VARDECL(spx_lsp_t *qlsp);
884    VARDECL(spx_lsp_t *interp_qlsp);
885    spx_int32_t dtx;
886    const SpeexSBMode *mode;
887    spx_word16_t *out = (spx_word16_t*)vout;
888    spx_word16_t *low_innov_alias;
889    spx_word32_t exc_ener_sum = 0;
890    
891    st = (SBDecState*)state;
892    stack=st->stack;
893    mode = (const SpeexSBMode*)(st->mode->mode);
894
895    low_innov_alias = out+st->frame_size;
896    speex_decoder_ctl(st->st_low, SPEEX_SET_INNOVATION_SAVE, low_innov_alias);
897    /* Decode the low-band */
898    ret = speex_decode_native(st->st_low, bits, out);
899
900    speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, &dtx);
901
902    /* If error decoding the narrowband part, propagate error */
903    if (ret!=0)
904    {
905       return ret;
906    }
907
908    if (!bits)
909    {
910       sb_decode_lost(st, out, dtx, stack);
911       return 0;
912    }
913
914    if (st->encode_submode)
915    {
916
917       /*Check "wideband bit"*/
918       if (speex_bits_remaining(bits)>0)
919          wideband = speex_bits_peek(bits);
920       else
921          wideband = 0;
922       if (wideband)
923       {
924          /*Regular wideband frame, read the submode*/
925          wideband = speex_bits_unpack_unsigned(bits, 1);
926          st->submodeID = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
927       } else
928       {
929          /*Was a narrowband frame, set "null submode"*/
930          st->submodeID = 0;
931       }
932       if (st->submodeID != 0 && st->submodes[st->submodeID] == NULL)
933       {
934          speex_warning("Invalid mode encountered: corrupted stream?");
935          return -2;
936       }
937    }
938
939    /* If null mode (no transmission), just set a couple things to zero*/
940    if (st->submodes[st->submodeID] == NULL)
941    {
942       if (dtx)
943       {
944          sb_decode_lost(st, out, 1, stack);
945          return 0;
946       }
947
948       for (i=0;i<st->frame_size;i++)
949          out[st->frame_size+i]=VERY_SMALL;
950
951       st->first=1;
952
953       /* Final signal synthesis from excitation */
954       iir_mem16(out+st->frame_size, st->interp_qlpc, out+st->frame_size, st->frame_size, st->lpcSize, st->mem_sp, stack);
955
956       qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
957
958       return 0;
959
960    }
961
962    ALLOC(low_pi_gain, st->nbSubframes, spx_word32_t);
963    ALLOC(low_exc_rms, st->nbSubframes, spx_word16_t);
964    speex_decoder_ctl(st->st_low, SPEEX_GET_PI_GAIN, low_pi_gain);
965    speex_decoder_ctl(st->st_low, SPEEX_GET_EXC, low_exc_rms);
966
967    ALLOC(qlsp, st->lpcSize, spx_lsp_t);
968    ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
969    SUBMODE(lsp_unquant)(qlsp, st->lpcSize, bits);
970    
971    if (st->first)
972    {
973       for (i=0;i<st->lpcSize;i++)
974          st->old_qlsp[i] = qlsp[i];
975    }
976    
977    ALLOC(ak, st->lpcSize, spx_coef_t);
978
979    for (sub=0;sub<st->nbSubframes;sub++)
980    {
981       VARDECL(spx_word32_t *exc);
982       spx_word16_t *innov_save=NULL;
983       spx_word16_t *sp;
984       spx_word16_t filter_ratio;
985       spx_word16_t el=0;
986       int offset;
987       spx_word32_t rl=0,rh=0;
988       
989       offset = st->subframeSize*sub;
990       sp=out+st->frame_size+offset;
991       ALLOC(exc, st->subframeSize, spx_word32_t);
992       /* Pointer for saving innovation */
993       if (st->innov_save)
994       {
995          innov_save = st->innov_save+2*offset;
996          for (i=0;i<2*st->subframeSize;i++)
997             innov_save[i]=0;
998       }
999       
1000       /* LSP interpolation */
1001       lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1002
1003       lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);
1004
1005       /* LSP to LPC */
1006       lsp_to_lpc(interp_qlsp, ak, st->lpcSize, stack);
1007
1008       /* Calculate reponse ratio between the low and high filter in the middle
1009          of the band (4000 Hz) */
1010       
1011          st->pi_gain[sub]=LPC_SCALING;
1012          rh = LPC_SCALING;
1013          for (i=0;i<st->lpcSize;i+=2)
1014          {
1015             rh += ak[i+1] - ak[i];
1016             st->pi_gain[sub] += ak[i] + ak[i+1];
1017          }
1018
1019          rl = low_pi_gain[sub];
1020 #ifdef FIXED_POINT
1021          filter_ratio=EXTRACT16(SATURATE(PDIV32(SHL32(ADD32(rl,82),7),ADD32(82,rh)),32767));
1022 #else
1023          filter_ratio=(rl+.01)/(rh+.01);
1024 #endif
1025       
1026       for (i=0;i<st->subframeSize;i++)
1027          exc[i]=0;
1028       if (!SUBMODE(innovation_unquant))
1029       {
1030          spx_word32_t g;
1031          int quant;
1032
1033          quant = speex_bits_unpack_unsigned(bits, 5);
1034          g= spx_exp(MULT16_16(QCONST16(.125f,11),(quant-10)));
1035          
1036          g = PDIV32(g, filter_ratio);
1037          
1038          for (i=0;i<st->subframeSize;i+=2)
1039          {
1040             exc[i]=SHL32(MULT16_32_P15(MULT16_16_Q15(mode->folding_gain,low_innov_alias[offset+i]),SHL32(g,6)),SIG_SHIFT);
1041             exc[i+1]=NEG32(SHL32(MULT16_32_P15(MULT16_16_Q15(mode->folding_gain,low_innov_alias[offset+i+1]),SHL32(g,6)),SIG_SHIFT));
1042          }
1043          
1044       } else {
1045          spx_word16_t gc;
1046          spx_word32_t scale;
1047          int qgc = speex_bits_unpack_unsigned(bits, 4);
1048          
1049          el = low_exc_rms[sub];
1050          gc = MULT16_16_Q15(QCONST16(0.87360,15),gc_quant_bound[qgc]);
1051
1052          if (st->subframeSize==80)
1053             gc = MULT16_16_P14(QCONST16(1.4142f,14),gc);
1054
1055          scale = SHL32(PDIV32(SHL32(MULT16_16(gc, el),3), filter_ratio),SIG_SHIFT-3);
1056          SUBMODE(innovation_unquant)(exc, SUBMODE(innovation_params), st->subframeSize, 
1057                                      bits, stack, &st->seed);
1058
1059          signal_mul(exc,exc,scale,st->subframeSize);
1060
1061          if (SUBMODE(double_codebook)) {
1062             char *tmp_stack=stack;
1063             VARDECL(spx_sig_t *innov2);
1064             ALLOC(innov2, st->subframeSize, spx_sig_t);
1065             for (i=0;i<st->subframeSize;i++)
1066                innov2[i]=0;
1067             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, 
1068                                         bits, stack, &st->seed);
1069             signal_mul(innov2, innov2, MULT16_32_P15(QCONST16(0.4f,15),scale), st->subframeSize);
1070             for (i=0;i<st->subframeSize;i++)
1071                exc[i] = ADD32(exc[i],innov2[i]);
1072             stack = tmp_stack;
1073          }
1074
1075       }
1076       
1077       if (st->innov_save)
1078       {
1079          for (i=0;i<st->subframeSize;i++)
1080             innov_save[2*i]=EXTRACT16(PSHR32(exc[i],SIG_SHIFT));
1081       }
1082       
1083       for (i=0;i<st->subframeSize;i++)
1084          sp[i]=st->excBuf[i];
1085       iir_mem16(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1086                st->mem_sp, stack);
1087       for (i=0;i<st->subframeSize;i++)
1088          st->excBuf[i]=EXTRACT16(PSHR32(exc[i],SIG_SHIFT));
1089       for (i=0;i<st->lpcSize;i++)
1090          st->interp_qlpc[i] = ak[i];
1091       st->exc_rms[sub] = compute_rms16(st->excBuf, st->subframeSize);
1092       exc_ener_sum = ADD32(exc_ener_sum, DIV32(MULT16_16(st->exc_rms[sub],st->exc_rms[sub]), st->nbSubframes));
1093    }
1094    st->last_ener = spx_sqrt(exc_ener_sum);
1095    
1096    qmf_synth(out, out+st->frame_size, h0, out, st->full_frame_size, QMF_ORDER, st->g0_mem, st->g1_mem, stack);
1097    for (i=0;i<st->lpcSize;i++)
1098       st->old_qlsp[i] = qlsp[i];
1099
1100    st->first=0;
1101
1102    return 0;
1103 }
1104
1105
1106 int sb_encoder_ctl(void *state, int request, void *ptr)
1107 {
1108    SBEncState *st;
1109    st=(SBEncState*)state;
1110    switch(request)
1111    {
1112    case SPEEX_GET_FRAME_SIZE:
1113       (*(spx_int32_t*)ptr) = st->full_frame_size;
1114       break;
1115    case SPEEX_SET_HIGH_MODE:
1116       st->submodeSelect = st->submodeID = (*(spx_int32_t*)ptr);
1117       break;
1118    case SPEEX_SET_LOW_MODE:
1119       speex_encoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1120       break;
1121    case SPEEX_SET_DTX:
1122       speex_encoder_ctl(st->st_low, SPEEX_SET_DTX, ptr);
1123       break;
1124    case SPEEX_GET_DTX:
1125       speex_encoder_ctl(st->st_low, SPEEX_GET_DTX, ptr);
1126       break;
1127    case SPEEX_GET_LOW_MODE:
1128       speex_encoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1129       break;
1130    case SPEEX_SET_MODE:
1131       speex_encoder_ctl(st, SPEEX_SET_QUALITY, ptr);
1132       break;
1133    case SPEEX_SET_VBR:
1134       st->vbr_enabled = (*(spx_int32_t*)ptr);
1135       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, ptr);
1136       break;
1137    case SPEEX_GET_VBR:
1138       (*(spx_int32_t*)ptr) = st->vbr_enabled;
1139       break;
1140    case SPEEX_SET_VAD:
1141       st->vad_enabled = (*(spx_int32_t*)ptr);
1142       speex_encoder_ctl(st->st_low, SPEEX_SET_VAD, ptr);
1143       break;
1144    case SPEEX_GET_VAD:
1145       (*(spx_int32_t*)ptr) = st->vad_enabled;
1146       break;
1147    case SPEEX_SET_VBR_QUALITY:
1148       {
1149          spx_int32_t q;
1150          float qual = (*(float*)ptr)+.6;
1151          st->vbr_quality = (*(float*)ptr);
1152          if (qual>10)
1153             qual=10;
1154          q=(int)floor(.5+*(float*)ptr);
1155          if (q>10)
1156             q=10;
1157          speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_QUALITY, &qual);
1158          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &q);
1159          break;
1160       }
1161    case SPEEX_GET_VBR_QUALITY:
1162       (*(float*)ptr) = st->vbr_quality;
1163       break;
1164    case SPEEX_SET_ABR:
1165       st->abr_enabled = (*(spx_int32_t*)ptr);
1166       st->vbr_enabled = st->abr_enabled!=0;
1167       speex_encoder_ctl(st->st_low, SPEEX_SET_VBR, &st->vbr_enabled);
1168       if (st->vbr_enabled) 
1169       {
1170          spx_int32_t i=10, rate, target;
1171          float vbr_qual;
1172          target = (*(spx_int32_t*)ptr);
1173          while (i>=0)
1174          {
1175             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1176             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1177             if (rate <= target)
1178                break;
1179             i--;
1180          }
1181          vbr_qual=i;
1182          if (vbr_qual<0)
1183             vbr_qual=0;
1184          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1185          st->abr_count=0;
1186          st->abr_drift=0;
1187          st->abr_drift2=0;
1188       }
1189       
1190       break;
1191    case SPEEX_GET_ABR:
1192       (*(spx_int32_t*)ptr) = st->abr_enabled;
1193       break;
1194    case SPEEX_SET_QUALITY:
1195       {
1196          spx_int32_t nb_qual;
1197          int quality = (*(spx_int32_t*)ptr);
1198          if (quality < 0)
1199             quality = 0;
1200          if (quality > 10)
1201             quality = 10;
1202          st->submodeSelect = st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1203          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1204          speex_encoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1205       }
1206       break;
1207    case SPEEX_SET_COMPLEXITY:
1208       speex_encoder_ctl(st->st_low, SPEEX_SET_COMPLEXITY, ptr);
1209       st->complexity = (*(spx_int32_t*)ptr);
1210       if (st->complexity<1)
1211          st->complexity=1;
1212       break;
1213    case SPEEX_GET_COMPLEXITY:
1214       (*(spx_int32_t*)ptr) = st->complexity;
1215       break;
1216    case SPEEX_SET_BITRATE:
1217       {
1218          spx_int32_t i=10;
1219          spx_int32_t rate, target;
1220          target = (*(spx_int32_t*)ptr);
1221          while (i>=0)
1222          {
1223             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1224             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1225             if (rate <= target)
1226                break;
1227             i--;
1228          }
1229       }
1230       break;
1231    case SPEEX_GET_BITRATE:
1232       speex_encoder_ctl(st->st_low, request, ptr);
1233       /*fprintf (stderr, "before: %d\n", (*(int*)ptr));*/
1234       if (st->submodes[st->submodeID])
1235          (*(spx_int32_t*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1236       else
1237          (*(spx_int32_t*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1238       /*fprintf (stderr, "after: %d\n", (*(int*)ptr));*/
1239       break;
1240    case SPEEX_SET_SAMPLING_RATE:
1241       {
1242          spx_int32_t tmp=(*(spx_int32_t*)ptr);
1243          st->sampling_rate = tmp;
1244          tmp>>=1;
1245          speex_encoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1246       }
1247       break;
1248    case SPEEX_GET_SAMPLING_RATE:
1249       (*(spx_int32_t*)ptr)=st->sampling_rate;
1250       break;
1251    case SPEEX_RESET_STATE:
1252       {
1253          int i;
1254          st->first = 1;
1255          for (i=0;i<st->lpcSize;i++)
1256             st->old_lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1257          for (i=0;i<st->lpcSize;i++)
1258             st->mem_sw[i]=st->mem_sp[i]=st->mem_sp2[i]=0;
1259          for (i=0;i<QMF_ORDER;i++)
1260             st->h0_mem[i]=st->h1_mem[i]=0;
1261       }
1262       break;
1263    case SPEEX_SET_SUBMODE_ENCODING:
1264       st->encode_submode = (*(spx_int32_t*)ptr);
1265       speex_encoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, ptr);
1266       break;
1267    case SPEEX_GET_SUBMODE_ENCODING:
1268       (*(spx_int32_t*)ptr) = st->encode_submode;
1269       break;
1270    case SPEEX_GET_LOOKAHEAD:
1271       speex_encoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
1272       (*(spx_int32_t*)ptr) = 2*(*(spx_int32_t*)ptr) + QMF_ORDER - 1;
1273       break;
1274    case SPEEX_SET_PLC_TUNING:
1275       speex_encoder_ctl(st->st_low, SPEEX_SET_PLC_TUNING, ptr);
1276       break;
1277    case SPEEX_GET_PLC_TUNING:
1278       speex_encoder_ctl(st->st_low, SPEEX_GET_PLC_TUNING, ptr);
1279       break;
1280    case SPEEX_SET_VBR_MAX_BITRATE:
1281       {
1282          st->vbr_max = (*(spx_int32_t*)ptr);
1283          if (SPEEX_SET_VBR_MAX_BITRATE<1)
1284          {
1285             speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_MAX_BITRATE, &st->vbr_max);
1286             st->vbr_max_high = 17600;
1287          } else {
1288             spx_int32_t low_rate;
1289             if (st->vbr_max >= 42200)
1290             {
1291                st->vbr_max_high = 17600;
1292             } else if (st->vbr_max >= 27800)
1293             {
1294                st->vbr_max_high = 9600;
1295             } else if (st->vbr_max > 20600)
1296             {
1297                st->vbr_max_high = 5600;
1298             } else {
1299                st->vbr_max_high = 1800;
1300             }
1301             if (st->subframeSize==80)
1302                st->vbr_max_high = 1800;
1303             low_rate = st->vbr_max - st->vbr_max_high;
1304             speex_encoder_ctl(st->st_low, SPEEX_SET_VBR_MAX_BITRATE, &low_rate);
1305          }
1306       }
1307       break;
1308    case SPEEX_GET_VBR_MAX_BITRATE:
1309       (*(spx_int32_t*)ptr) = st->vbr_max;
1310       break;
1311    case SPEEX_SET_HIGHPASS:
1312       speex_encoder_ctl(st->st_low, SPEEX_SET_HIGHPASS, ptr);
1313       break;
1314    case SPEEX_GET_HIGHPASS:
1315       speex_encoder_ctl(st->st_low, SPEEX_GET_HIGHPASS, ptr);
1316       break;
1317
1318
1319    /* This is all internal stuff past this point */
1320    case SPEEX_GET_PI_GAIN:
1321       {
1322          int i;
1323          spx_word32_t *g = (spx_word32_t*)ptr;
1324          for (i=0;i<st->nbSubframes;i++)
1325             g[i]=st->pi_gain[i];
1326       }
1327       break;
1328    case SPEEX_GET_EXC:
1329       {
1330          int i;
1331          for (i=0;i<st->nbSubframes;i++)
1332             ((spx_word16_t*)ptr)[i] = st->exc_rms[i];
1333       }
1334       break;
1335    case SPEEX_GET_RELATIVE_QUALITY:
1336       (*(float*)ptr)=st->relative_quality;
1337       break;
1338    case SPEEX_SET_INNOVATION_SAVE:
1339       st->innov_rms_save = (spx_word16_t*)ptr;
1340       break;
1341    case SPEEX_SET_WIDEBAND:
1342       speex_encoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, ptr);
1343       break;
1344
1345    default:
1346       speex_warning_int("Unknown nb_ctl request: ", request);
1347       return -1;
1348    }
1349    return 0;
1350 }
1351
1352 int sb_decoder_ctl(void *state, int request, void *ptr)
1353 {
1354    SBDecState *st;
1355    st=(SBDecState*)state;
1356    switch(request)
1357    {
1358    case SPEEX_SET_HIGH_MODE:
1359       st->submodeID = (*(spx_int32_t*)ptr);
1360       break;
1361    case SPEEX_SET_LOW_MODE:
1362       speex_decoder_ctl(st->st_low, SPEEX_SET_LOW_MODE, ptr);
1363       break;
1364    case SPEEX_GET_LOW_MODE:
1365       speex_decoder_ctl(st->st_low, SPEEX_GET_LOW_MODE, ptr);
1366       break;
1367    case SPEEX_GET_FRAME_SIZE:
1368       (*(spx_int32_t*)ptr) = st->full_frame_size;
1369       break;
1370    case SPEEX_SET_ENH:
1371       speex_decoder_ctl(st->st_low, request, ptr);
1372       st->lpc_enh_enabled = *((spx_int32_t*)ptr);
1373       break;
1374    case SPEEX_GET_ENH:
1375       *((spx_int32_t*)ptr) = st->lpc_enh_enabled;
1376       break;
1377    case SPEEX_SET_MODE:
1378    case SPEEX_SET_QUALITY:
1379       {
1380          spx_int32_t nb_qual;
1381          int quality = (*(spx_int32_t*)ptr);
1382          if (quality < 0)
1383             quality = 0;
1384          if (quality > 10)
1385             quality = 10;
1386          st->submodeID = ((const SpeexSBMode*)(st->mode->mode))->quality_map[quality];
1387          nb_qual = ((const SpeexSBMode*)(st->mode->mode))->low_quality_map[quality];
1388          speex_decoder_ctl(st->st_low, SPEEX_SET_MODE, &nb_qual);
1389       }
1390       break;
1391    case SPEEX_GET_BITRATE:
1392       speex_decoder_ctl(st->st_low, request, ptr);
1393       if (st->submodes[st->submodeID])
1394          (*(spx_int32_t*)ptr) += st->sampling_rate*SUBMODE(bits_per_frame)/st->full_frame_size;
1395       else
1396          (*(spx_int32_t*)ptr) += st->sampling_rate*(SB_SUBMODE_BITS+1)/st->full_frame_size;
1397       break;
1398    case SPEEX_SET_SAMPLING_RATE:
1399       {
1400          spx_int32_t tmp=(*(spx_int32_t*)ptr);
1401          st->sampling_rate = tmp;
1402          tmp>>=1;
1403          speex_decoder_ctl(st->st_low, SPEEX_SET_SAMPLING_RATE, &tmp);
1404       }
1405       break;
1406    case SPEEX_GET_SAMPLING_RATE:
1407       (*(spx_int32_t*)ptr)=st->sampling_rate;
1408       break;
1409    case SPEEX_SET_HANDLER:
1410       speex_decoder_ctl(st->st_low, SPEEX_SET_HANDLER, ptr);
1411       break;
1412    case SPEEX_SET_USER_HANDLER:
1413       speex_decoder_ctl(st->st_low, SPEEX_SET_USER_HANDLER, ptr);
1414       break;
1415    case SPEEX_RESET_STATE:
1416       {
1417          int i;
1418          for (i=0;i<2*st->lpcSize;i++)
1419             st->mem_sp[i]=0;
1420          for (i=0;i<QMF_ORDER;i++)
1421             st->g0_mem[i]=st->g1_mem[i]=0;
1422          st->last_ener=0;
1423       }
1424       break;
1425    case SPEEX_SET_SUBMODE_ENCODING:
1426       st->encode_submode = (*(spx_int32_t*)ptr);
1427       speex_decoder_ctl(st->st_low, SPEEX_SET_SUBMODE_ENCODING, ptr);
1428       break;
1429    case SPEEX_GET_SUBMODE_ENCODING:
1430       (*(spx_int32_t*)ptr) = st->encode_submode;
1431       break;
1432    case SPEEX_GET_LOOKAHEAD:
1433       speex_decoder_ctl(st->st_low, SPEEX_GET_LOOKAHEAD, ptr);
1434       (*(spx_int32_t*)ptr) = 2*(*(spx_int32_t*)ptr);
1435       break;
1436    case SPEEX_SET_HIGHPASS:
1437       speex_decoder_ctl(st->st_low, SPEEX_SET_HIGHPASS, ptr);
1438       break;
1439    case SPEEX_GET_HIGHPASS:
1440       speex_decoder_ctl(st->st_low, SPEEX_GET_HIGHPASS, ptr);
1441       break;
1442
1443    case SPEEX_GET_PI_GAIN:
1444       {
1445          int i;
1446          spx_word32_t *g = (spx_word32_t*)ptr;
1447          for (i=0;i<st->nbSubframes;i++)
1448             g[i]=st->pi_gain[i];
1449       }
1450       break;
1451    case SPEEX_GET_EXC:
1452       {
1453          int i;
1454          for (i=0;i<st->nbSubframes;i++)
1455             ((spx_word16_t*)ptr)[i] = st->exc_rms[i];
1456       }
1457       break;
1458    case SPEEX_GET_DTX_STATUS:
1459       speex_decoder_ctl(st->st_low, SPEEX_GET_DTX_STATUS, ptr);
1460       break;
1461    case SPEEX_SET_INNOVATION_SAVE:
1462       st->innov_save = (spx_word16_t*)ptr;
1463       break;
1464    case SPEEX_SET_WIDEBAND:
1465       speex_decoder_ctl(st->st_low, SPEEX_SET_WIDEBAND, ptr);
1466       break;
1467
1468    default:
1469       speex_warning_int("Unknown nb_ctl request: ", request);
1470       return -1;
1471    }
1472    return 0;
1473 }
1474
1475 #endif
1476