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
sb_encoder_init(const SpeexMode * m)61 void *sb_encoder_init(const SpeexMode *m)
62 {
63    speex_error("Wideband and Ultra-wideband are disabled");
64    return NULL;
65 }
sb_encoder_destroy(void * state)66 void sb_encoder_destroy(void *state)
67 {
68    speex_error("Wideband and Ultra-wideband are disabled");
69 }
sb_encode(void * state,void * vin,SpeexBits * bits)70 int sb_encode(void *state, void *vin, SpeexBits *bits)
71 {
72    speex_error("Wideband and Ultra-wideband are disabled");
73    return -2;
74 }
sb_decoder_init(const SpeexMode * m)75 void *sb_decoder_init(const SpeexMode *m)
76 {
77    speex_error("Wideband and Ultra-wideband are disabled");
78    return NULL;
79 }
sb_decoder_destroy(void * state)80 void sb_decoder_destroy(void *state)
81 {
82    speex_error("Wideband and Ultra-wideband are disabled");
83 }
sb_decode(void * state,SpeexBits * bits,void * vout)84 int sb_decode(void *state, SpeexBits *bits, void *vout)
85 {
86    speex_error("Wideband and Ultra-wideband are disabled");
87    return -2;
88 }
sb_encoder_ctl(void * state,int request,void * ptr)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 }
sb_decoder_ctl(void * state,int request,void * ptr)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 
mix_and_saturate(spx_word32_t * x0,spx_word32_t * x1,spx_word16_t * out,int len)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 
sb_encoder_init(const SpeexMode * m)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 
sb_encoder_destroy(void * state)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 
sb_encode(void * state,void * vin,SpeexBits * bits)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 
sb_decoder_init(const SpeexMode * m)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 
sb_decoder_destroy(void * state)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 
sb_decode_lost(SBDecState * st,spx_word16_t * out,int dtx,char * stack)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 
sb_decode(void * state,SpeexBits * bits,void * vout)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 
sb_encoder_ctl(void * state,int request,void * ptr)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 
sb_decoder_ctl(void * state,int request,void * ptr)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 
1614