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