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