Add support for ICE/STUN/TURN in res_rtp_asterisk and chan_sip.
[asterisk/asterisk.git] / res / pjproject / pjmedia / src / pjmedia-codec / g722 / g722_dec.c
1 /* $Id$ */
2 /* 
3  * Copyright (C) 2008-2011 Teluu Inc. (http://www.teluu.com)
4  * Copyright (C) 2003-2008 Benny Prijono <benny@prijono.org>
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
19  */
20 /*
21  * Based on implementation found in Carnegie Mellon Speech Group Software
22  * depository (ftp://ftp.cs.cmu.edu/project/fgdata/index.html). No copyright
23  * was claimed in the original source codes.
24  */
25 #include <pjmedia/errno.h>
26 #include <pj/assert.h>
27 #include <pj/pool.h>
28
29 #include "g722_dec.h"
30
31 #if defined(PJMEDIA_HAS_G722_CODEC) && (PJMEDIA_HAS_G722_CODEC != 0)
32
33 #define MODE 1
34
35 #define SATURATE(v, max, min) \
36     if (v>max) v = max; \
37     else if (v<min) v = min
38
39 extern const int g722_qmf_coeff[24];
40
41 static const int qm4[16] = 
42 {
43         0, -20456, -12896, -8968,
44     -6288,  -4240,  -2584, -1200,
45     20456,  12896,   8968,  6288,
46      4240,   2584,   1200,     0
47 };
48 static const int ilb[32] = {
49     2048, 2093, 2139, 2186, 2233, 2282, 2332,
50     2383, 2435, 2489, 2543, 2599, 2656, 2714, 
51     2774, 2834, 2896, 2960, 3025, 3091, 3158, 
52     3228, 3298, 3371, 3444, 3520, 3597, 3676,
53     3756, 3838, 3922, 4008
54 };
55
56
57 static int block2l (int il, int detl)
58 {
59     int dlt ;
60     int ril, wd2 ;
61
62     /* INVQAL */
63     ril = il >> 2 ;
64     wd2 = qm4[ril] ;
65     dlt = (detl * wd2) >> 15 ;
66
67     return (dlt) ;
68 }
69
70
71 static int block3l (g722_dec_t *dec, int il)
72 {
73     int detl ;
74     int ril, il4, wd, wd1, wd2, wd3, nbpl, depl ;
75     static const int  wl[8] = {
76         -60, -30, 58, 172, 334, 538, 1198, 3042
77     };
78     static const int rl42[16] = {
79         0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
80     };
81
82     /* LOGSCL */
83     ril = il >> 2 ;
84     il4 = rl42[ril] ;
85     wd = (dec->nbl * 32512) >> 15 ;
86     nbpl = wd + wl[il4] ;
87
88     if (nbpl <     0) nbpl = 0 ;
89     if (nbpl > 18432) nbpl = 18432 ;
90
91     /* SCALEL */
92     wd1 =  (nbpl >> 6) & 31 ;
93     wd2 = nbpl >> 11 ;
94     if ((8 - wd2) < 0)   wd3 = ilb[wd1] << (wd2 - 8) ;
95     else wd3 = ilb[wd1] >> (8 - wd2) ;
96     depl = wd3 << 2 ;
97
98     /* DELAYA */
99     dec->nbl = nbpl ;
100     /* DELAYL */
101     detl = depl ;
102
103     return (detl) ;
104 }
105
106
107 static int block4l (g722_dec_t *dec, int dl)
108 {
109     int sl = dec->slow ;
110     int i ;
111     int wd, wd1, wd2, wd3, wd4, wd5/*, wd6 */;
112
113     dec->dlt[0] = dl;
114
115     /* RECONS */ 
116     dec->rlt[0] = sl + dec->dlt[0] ;
117     SATURATE(dec->rlt[0], 32767, -32768);
118
119     /* PARREC */
120     dec->plt[0] = dec->dlt[0] + dec->szl ;
121     SATURATE(dec->plt[0], 32767, -32768);
122
123     /* UPPOL2 */
124     dec->sgl[0] = dec->plt[0] >> 15 ;
125     dec->sgl[1] = dec->plt[1] >> 15 ;
126     dec->sgl[2] = dec->plt[2] >> 15 ;
127
128     wd1 = dec->al[1] << 2;
129     SATURATE(wd1, 32767, -32768);
130
131     if ( dec->sgl[0] == dec->sgl[1] )  wd2 = - wd1 ;
132     else  wd2 = wd1 ;
133     if (wd2 > 32767) wd2 = 32767;
134     wd2 = wd2 >> 7 ;
135
136     if ( dec->sgl[0] == dec->sgl[2] )  wd3 = 128 ; 
137     else  wd3 = - 128 ;
138
139     wd4 = wd2 + wd3 ;
140     wd5 = (dec->al[2] * 32512) >> 15 ;
141
142     dec->apl[2] = wd4 + wd5 ;
143     SATURATE(dec->apl[2], 12288, -12288);
144     
145     /* UPPOL1 */
146     dec->sgl[0] = dec->plt[0] >> 15 ;
147     dec->sgl[1] = dec->plt[1] >> 15 ;
148
149     if ( dec->sgl[0] == dec->sgl[1] )  wd1 = 192 ;
150     else  wd1 = - 192 ;
151
152     wd2 = (dec->al[1] * 32640) >> 15 ;
153
154     dec->apl[1] = wd1 + wd2 ;
155     SATURATE(dec->apl[1], 32767, -32768);
156
157     wd3 = (15360 - dec->apl[2]) ;
158     SATURATE(wd3, 32767, -32768);
159     if ( dec->apl[1] >  wd3)  dec->apl[1] =  wd3 ;
160     if ( dec->apl[1] < -wd3)  dec->apl[1] = -wd3 ;
161
162     /* UPZERO */
163     if ( dec->dlt[0] == 0 )  wd1 = 0 ;
164     else  wd1 = 128 ;
165
166     dec->sgl[0] = dec->dlt[0] >> 15 ;
167
168     for ( i = 1; i < 7; i++ ) {
169         dec->sgl[i] = dec->dlt[i] >> 15 ;
170         if ( dec->sgl[i] == dec->sgl[0] )  wd2 = wd1 ;
171         else  wd2 = - wd1 ;
172         wd3 = (dec->bl[i] * 32640) >> 15 ;
173         dec->bpl[i] = wd2 + wd3 ;
174         SATURATE(dec->bpl[i], 32767, -32768);
175     }
176
177     /* DELAYA */
178     for ( i = 6; i > 0; i-- ) {
179         dec->dlt[i] = dec->dlt[i-1] ;
180         dec->bl[i]  = dec->bpl[i] ;
181     }
182
183     for ( i = 2; i > 0; i-- ) {
184         dec->rlt[i] = dec->rlt[i-1] ;
185         dec->plt[i] = dec->plt[i-1] ;
186         dec->al[i]  = dec->apl[i] ;
187     }
188
189     /* FILTEP */
190     wd1 = dec->rlt[1] << 1;
191     SATURATE(wd1, 32767, -32768);
192     wd1 = ( dec->al[1] * wd1 ) >> 15 ;
193
194     wd2 = dec->rlt[2] << 1;
195     SATURATE(wd2, 32767, -32768);
196     wd2 = ( dec->al[2] * wd2 ) >> 15 ;
197
198     dec->spl = wd1 + wd2 ;
199     SATURATE(dec->spl, 32767, -32768);
200
201     /* FILTEZ */
202     dec->szl = 0 ;
203     for (i=6; i>0; i--) {
204         wd = dec->dlt[i] << 1;
205         SATURATE(wd, 32767, -32768);
206         dec->szl += (dec->bl[i] * wd) >> 15 ;
207         SATURATE(dec->szl, 32767, -32768);
208     }
209
210     /* PREDIC */
211     sl = dec->spl + dec->szl ;
212     SATURATE(sl, 32767, -32768);
213
214     return (sl) ;
215 }
216
217 static int block5l (int ilr, int sl, int detl, int mode)
218 {
219     int yl ;
220     int ril, dl, wd2 = 0;
221     static const int qm5[32] = {
222           -280,   -280, -23352, -17560,
223         -14120, -11664,  -9752,  -8184,
224          -6864,  -5712,  -4696,  -3784,
225          -2960,  -2208,  -1520,   -880,
226          23352,  17560,  14120,  11664,
227           9752,   8184,   6864,   5712,
228           4696,   3784,   2960,   2208,
229           1520,    880,    280,   -280
230     };
231     static const int qm6[64] = {
232         -136,   -136,   -136,   -136,
233         -24808, -21904, -19008, -16704,
234         -14984, -13512, -12280, -11192,
235         -10232, -9360,  -8576,  -7856,
236         -7192,  -6576,  -6000,  -5456,
237         -4944,  -4464,  -4008,  -3576,
238         -3168,  -2776,  -2400,  -2032,
239         -1688,  -1360,  -1040,  -728,
240         24808,  21904,  19008,  16704,
241         14984,  13512,  12280,  11192,
242         10232,  9360,   8576,   7856,
243         7192,   6576,   6000,   5456,
244         4944,   4464,   4008,   3576,
245         3168,   2776,   2400,   2032,
246         1688,   1360,   1040,   728,
247         432,    136,    -432,   -136
248     };
249     
250     /* INVQBL */
251     if (mode == 1) {
252         ril = ilr ;
253         wd2 = qm6[ril] ;
254     }
255
256     if (mode == 2) {
257         ril = ilr >> 1 ;
258         wd2 = qm5[ril] ;
259     }
260
261     if (mode == 3) {
262         ril = ilr >> 2 ;
263         wd2 = qm4[ril] ;
264     }
265
266     dl = (detl * wd2 ) >> 15 ;
267
268     /* RECONS */
269     yl = sl + dl ;
270     SATURATE(yl, 32767, -32768);
271
272     return (yl) ;
273 }
274
275 static int block6l (int yl)
276 {
277     int rl ;
278
279     rl = yl ;
280     SATURATE(rl, 16383, -16384);
281
282     return (rl) ;
283 }
284
285 static int block2h (int ih, int deth)
286 {
287     int dh ;
288     int wd2 ;
289     static const int qm2[4] = {-7408, -1616, 7408, 1616} ;
290     
291     /* INVQAH */
292     wd2 = qm2[ih] ;
293     dh = (deth * wd2) >> 15 ;
294
295     return (dh) ;
296 }
297
298 static int block3h (g722_dec_t *dec, int ih)
299 {
300     int deth ;
301     int ih2, wd, wd1, wd2, wd3, nbph, deph ;
302     static const int wh[3] = {0, -214, 798} ;
303     static const int rh2[4] = {2, 1, 2, 1} ;
304     
305     /* LOGSCH */
306     ih2 = rh2[ih] ;
307     wd = (dec->nbh * 32512) >> 15 ;
308     nbph = wd + wh[ih2] ;
309
310     if (nbph <     0) nbph = 0 ;
311     if (nbph > 22528) nbph = 22528 ;
312
313
314     /* SCALEH */
315     wd1 =  (nbph >> 6) & 31 ;
316     wd2 = nbph >> 11 ;
317     if ((10 - wd2) < 0) wd3 = ilb[wd1] << (wd2 - 10) ;
318     else wd3 = ilb[wd1] >> (10 - wd2) ;
319     deph = wd3 << 2 ;
320
321     /* DELAYA */
322     dec->nbh = nbph ;
323     
324     /* DELAYH */
325     deth = deph ;
326
327     return (deth) ;
328 }
329
330 static int block4h (g722_dec_t *dec, int d)
331 {
332     int sh = dec->shigh;
333     int i ;
334     int wd, wd1, wd2, wd3, wd4, wd5/*, wd6 */;
335
336     dec->dh[0] = d;
337
338     /* RECONS */ 
339     dec->rh[0] = sh + dec->dh[0] ;
340     SATURATE(dec->rh[0], 32767, -32768);
341
342     /* PARREC */
343     dec->ph[0] = dec->dh[0] + dec->szh ;
344     SATURATE(dec->ph[0], 32767, -32768);
345
346     /* UPPOL2 */
347     dec->sgh[0] = dec->ph[0] >> 15 ;
348     dec->sgh[1] = dec->ph[1] >> 15 ;
349     dec->sgh[2] = dec->ph[2] >> 15 ;
350
351     wd1 = dec->ah[1] << 2;
352     SATURATE(wd1, 32767, -32768);
353
354     if ( dec->sgh[0] == dec->sgh[1] )  wd2 = - wd1 ;
355     else  wd2 = wd1 ;
356     if (wd2 > 32767) wd2 = 32767;
357
358     wd2 = wd2 >> 7 ;
359
360     if ( dec->sgh[0] == dec->sgh[2] )  wd3 = 128 ; 
361     else  wd3 = - 128 ;
362
363     wd4 = wd2 + wd3 ;
364     wd5 = (dec->ah[2] * 32512) >> 15 ;
365
366     dec->aph[2] = wd4 + wd5 ;
367     SATURATE(dec->aph[2], 12288, -12288);
368     
369     /* UPPOL1 */
370     dec->sgh[0] = dec->ph[0] >> 15 ;
371     dec->sgh[1] = dec->ph[1] >> 15 ;
372
373     if ( dec->sgh[0] == dec->sgh[1] )  wd1 = 192 ;
374     else  wd1 = - 192 ;
375
376     wd2 = (dec->ah[1] * 32640) >> 15 ;
377
378     dec->aph[1] = wd1 + wd2 ;
379     SATURATE(dec->aph[1], 32767, -32768);
380     //dec->aph[2]?
381     //if (aph[2] > 32767) aph[2] = 32767;
382     //if (aph[2] < -32768) aph[2] = -32768;
383
384     wd3 = (15360 - dec->aph[2]) ;
385     SATURATE(wd3, 32767, -32768);
386     if ( dec->aph[1] >  wd3)  dec->aph[1] =  wd3 ;
387     if ( dec->aph[1] < -wd3)  dec->aph[1] = -wd3 ;
388
389     /* UPZERO */
390     if ( dec->dh[0] == 0 )  wd1 = 0 ;
391     if ( dec->dh[0] != 0 )  wd1 = 128 ;
392
393     dec->sgh[0] = dec->dh[0] >> 15 ;
394
395     for ( i = 1; i < 7; i++ ) {
396         dec->sgh[i] = dec->dh[i] >> 15 ;
397         if ( dec->sgh[i] == dec->sgh[0] )  wd2 = wd1 ;
398         else wd2 = - wd1 ;
399         wd3 = (dec->bh[i] * 32640) >> 15 ;
400         dec->bph[i] = wd2 + wd3 ;
401     }
402  
403     /* DELAYA */
404     for ( i = 6; i > 0; i-- ) {
405         dec->dh[i] = dec->dh[i-1] ;
406         dec->bh[i] = dec->bph[i] ;
407     }
408
409     for ( i = 2; i > 0; i-- ) {
410         dec->rh[i] = dec->rh[i-1] ;
411         dec->ph[i] = dec->ph[i-1] ;
412         dec->ah[i] = dec->aph[i] ;
413     }
414
415     /* FILTEP */
416     wd1 = dec->rh[1] << 1 ;
417     SATURATE(wd1, 32767, -32768);
418     wd1 = ( dec->ah[1] * wd1 ) >> 15 ;
419
420     wd2 = dec->rh[2] << 1;
421     SATURATE(wd2, 32767, -32768);
422     wd2 = ( dec->ah[2] * wd2 ) >> 15 ;
423
424     dec->sph = wd1 + wd2 ;
425     SATURATE(dec->sph, 32767, -32768);
426
427     /* FILTEZ */
428     dec->szh = 0 ;
429     for (i=6; i>0; i--) {
430         wd = dec->dh[i] << 1;
431         SATURATE(wd, 32767, -32768);
432         dec->szh += (dec->bh[i] * wd) >> 15 ;
433         SATURATE(dec->szh, 32767, -32768);
434     }
435
436     /* PREDIC */
437     sh = dec->sph + dec->szh ;
438     SATURATE(sh, 32767, -32768);
439
440     return (sh) ;
441 }
442
443 static int block5h (int dh, int sh)
444 {
445     int rh ;
446
447     rh = dh + sh;
448     SATURATE(rh, 16383, -16384);
449
450     return (rh) ;
451 }
452
453 void rx_qmf(g722_dec_t *dec, int rl, int rh, int *xout1, int *xout2)
454 {
455     int i;
456
457     pj_memmove(&dec->xd[1], dec->xd, 11*sizeof(dec->xd[0]));
458     pj_memmove(&dec->xs[1], dec->xs, 11*sizeof(dec->xs[0]));
459
460     /* RECA */
461     dec->xd[0] = rl - rh ;
462     if (dec->xd[0] > 16383) dec->xd[0] = 16383;
463     else if (dec->xd[0] < -16384) dec->xd[0] = -16384;
464     
465     /* RECB */
466     dec->xs[0] = rl + rh ;
467     if (dec->xs[0] > 16383) dec->xs[0] = 16383;
468     else if (dec->xs[0] < -16384) dec->xs[0] = -16384;
469     
470     /* ACCUMC */
471     *xout1 = 0;
472     for (i=0; i<12; ++i) *xout1 += dec->xd[i] * g722_qmf_coeff[2*i];
473     *xout1 = *xout1 >> 12 ;
474     if (*xout1 >  16383)  *xout1 =  16383 ;
475     else if (*xout1 < -16384)  *xout1 = -16384 ;
476     
477     /* ACCUMD */
478     *xout2 = 0;
479     for (i=0; i<12; ++i) *xout2 += dec->xs[i] * g722_qmf_coeff[2*i+1];
480     *xout2  = *xout2  >> 12 ;
481     if (*xout2  >  16383)  *xout2  =  16383 ;
482     else if (*xout2  < -16384)  *xout2  = -16384 ;
483 }
484
485
486 PJ_DEF(pj_status_t) g722_dec_init(g722_dec_t *dec)
487 {
488     PJ_ASSERT_RETURN(dec, PJ_EINVAL);
489
490     pj_bzero(dec, sizeof(g722_dec_t));
491
492     dec->detlow = 32;
493     dec->dethigh = 8;
494
495     return PJ_SUCCESS;
496 }
497
498 PJ_DEF(pj_status_t) g722_dec_decode( g722_dec_t *dec, 
499                                     void *in, 
500                                     pj_size_t in_size,
501                                     pj_int16_t out[],
502                                     pj_size_t *nsamples)
503 {
504     unsigned i;
505     int ilowr, ylow, rlow, dlowt;
506     int ihigh, rhigh, dhigh;
507     int pcm1, pcm2;
508     pj_uint8_t *in_ = (pj_uint8_t*) in;
509
510     PJ_ASSERT_RETURN(dec && in && in_size && out && nsamples, PJ_EINVAL);
511     PJ_ASSERT_RETURN(*nsamples >= (in_size << 1), PJ_ETOOSMALL);
512
513     for(i = 0; i < in_size; ++i) {
514         ilowr = in_[i] & 63;
515         ihigh = (in_[i] >> 6) & 3;
516
517         /* low band decoder */
518         ylow = block5l (ilowr, dec->slow, dec->detlow, MODE) ;  
519         rlow = block6l (ylow) ;
520         dlowt = block2l (ilowr, dec->detlow) ;
521         dec->detlow = block3l (dec, ilowr) ;
522         dec->slow = block4l (dec, dlowt) ;
523         /* rlow <= output low band pcm */
524
525         /* high band decoder */
526         dhigh = block2h (ihigh, dec->dethigh) ;
527         rhigh = block5h (dhigh, dec->shigh) ;
528         dec->dethigh = block3h (dec, ihigh) ;
529         dec->shigh = block4h (dec, dhigh) ;
530         /* rhigh <= output high band pcm */
531
532         rx_qmf(dec, rlow, rhigh, &pcm1, &pcm2);
533         out[i*2]   = (pj_int16_t)pcm1;
534         out[i*2+1] = (pj_int16_t)pcm2;
535     }
536
537     *nsamples = in_size << 1;
538
539     return PJ_SUCCESS;
540 }
541
542 PJ_DEF(pj_status_t) g722_dec_deinit(g722_dec_t *dec)
543 {
544     pj_bzero(dec, sizeof(g722_dec_t));
545
546     return PJ_SUCCESS;
547 }
548
549 #endif