Add support for ICE/STUN/TURN in res_rtp_asterisk and chan_sip.
[asterisk/asterisk.git] / res / pjproject / pjmedia / src / pjmedia-codec / g722 / g722_enc.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_enc.h"
30
31 #if defined(PJMEDIA_HAS_G722_CODEC) && (PJMEDIA_HAS_G722_CODEC != 0)
32
33 #define SATURATE(v, max, min) \
34     if (v>max) v = max; \
35     else if (v<min) v = min
36
37 /* QMF tap coefficients */
38 const int g722_qmf_coeff[24] = {
39      3,     -11,    -11,    53,     12,     -156,
40     32,     362,    -210,   -805,   951,    3876,
41     3876,   951,    -805,   -210,   362,    32,
42     -156,   12,     53,     -11,    -11,    3
43 };
44
45
46 static int block1l (int xl, int sl, int detl)
47 {
48     int il ;
49
50     int i, el, sil, mil, wd, wd1, hdu ;
51
52     static const int q6[32] = {
53         0, 35, 72, 110, 150, 190, 233, 276, 323,
54         370, 422, 473, 530, 587, 650, 714, 786,
55         858, 940, 1023, 1121, 1219, 1339, 1458,
56         1612, 1765, 1980, 2195, 2557, 2919, 0, 0
57     };
58
59     static const int iln[32] = {
60         0, 63, 62, 31, 30, 29, 28, 27, 26, 25,
61         24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14,
62         13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 0 
63     };
64
65     static const int ilp[32] = {
66         0, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52,
67         51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41,
68         40, 39, 38, 37, 36, 35, 34, 33, 32, 0 
69     };
70
71     /* SUBTRA */
72
73     el = xl - sl ;
74     SATURATE(el, 32767, -32768);
75
76     /* QUANTL */
77
78     sil = el >> 15 ;
79     if (sil == 0 )  wd = el ;
80     else wd = (32767 - el) & 32767 ;
81
82     mil = 1 ;
83
84     for (i = 1; i < 30; i++) {
85         hdu = (q6[i] << 3) * detl;
86         wd1 = (hdu >> 15) ;
87         if (wd >= wd1)  mil = (i + 1) ;
88         else break ;
89     }
90
91     if (sil == -1 ) il = iln[mil] ;
92     else il = ilp[mil] ;
93
94     return (il) ;
95 }
96
97 static int block2l (int il, int detl)
98 {
99     int dlt;
100     int ril, wd2 ;
101     static const int qm4[16] = {
102         0,      -20456, -12896, -8968,
103         -6288,  -4240,  -2584,  -1200,
104         20456,  12896,  8968,   6288,
105         4240,   2584,   1200,   0
106     };
107
108     /* INVQAL */
109     ril = il >> 2 ;
110     wd2 = qm4[ril] ;
111     dlt = (detl * wd2) >> 15 ;
112
113     return (dlt) ;
114 }
115
116 static int block3l (g722_enc_t *enc, int il)
117 {
118     int detl;
119     int ril, il4, wd, wd1, wd2, wd3, nbpl, depl ;
120     static int const wl[8] = {
121         -60, -30, 58, 172, 334, 538, 1198, 3042
122     } ;
123     static int const rl42[16] = {
124         0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
125     };
126     static const int ilb[32] = {
127         2048, 2093, 2139, 2186, 2233, 2282, 2332,
128         2383, 2435, 2489, 2543, 2599, 2656, 2714,
129         2774, 2834, 2896, 2960, 3025, 3091, 3158,
130         3228, 3298, 3371, 3444, 3520, 3597, 3676,
131         3756, 3838, 3922, 4008
132     };
133
134     /* LOGSCL */
135
136     ril = il >> 2 ;
137     il4 = rl42[ril] ;
138     
139     wd = (enc->nbl * 32512) >> 15 ;
140     nbpl = wd + wl[il4] ;
141
142     if (nbpl <     0) nbpl = 0 ;
143     if (nbpl > 18432) nbpl = 18432 ;
144
145     /* SCALEL */
146
147     wd1 =  (nbpl >> 6) & 31 ;
148     wd2 = nbpl >> 11 ;
149     if ((8 - wd2) < 0)    wd3 = ilb[wd1] << (wd2 - 8) ;
150     else   wd3 = ilb[wd1] >> (8 - wd2) ;
151     depl = wd3 << 2 ;
152
153     /* DELAYA */
154     enc->nbl = nbpl ;
155
156     /* DELAYL */
157     detl = depl ;
158
159 #ifdef DEBUG_VERBOSE
160         printf ("BLOCK3L il=%4d, ril=%4d, il4=%4d, nbl=%4d, wd=%4d, nbpl=%4d\n",
161                  il, ril, il4, enc->nbl, wd, nbpl) ;
162         printf ("wd1=%4d, wd2=%4d, wd3=%4d, depl=%4d, detl=%4d\n",
163                 wd1, wd2, wd3, depl, detl) ;
164 #endif
165
166     return (detl) ;
167 }
168
169 static int block4l (g722_enc_t *enc, int dl)
170 {
171     int sl = enc->slow;
172     int i ;
173     int wd, wd1, wd2, wd3, wd4, wd5 /*, wd6 */;
174
175     enc->dlt[0] = dl;
176     
177     /* RECONS */
178
179     enc->rlt[0] = sl + enc->dlt[0] ;
180     SATURATE(enc->rlt[0], 32767, -32768);
181
182     /* PARREC */
183
184     enc->plt[0] = enc->dlt[0] + enc->szl ;
185     SATURATE(enc->plt[0], 32767, -32768);
186
187     /* UPPOL2 */
188
189     enc->sgl[0] = enc->plt[0] >> 15 ;
190     enc->sgl[1] = enc->plt[1] >> 15 ;
191     enc->sgl[2] = enc->plt[2] >> 15 ;
192
193     wd1 = enc->al[1] << 2;
194     SATURATE(wd1, 32767, -32768);
195
196     if ( enc->sgl[0] == enc->sgl[1] )  wd2 = - wd1 ;
197     else  wd2 = wd1 ;
198     if ( wd2 > 32767 ) wd2 = 32767;
199
200     wd2 = wd2 >> 7 ;
201
202     if ( enc->sgl[0] == enc->sgl[2] )  wd3 = 128 ;
203     else  wd3 = - 128 ;
204
205     wd4 = wd2 + wd3 ;
206     wd5 = (enc->al[2] * 32512) >> 15 ;
207
208     enc->apl[2] = wd4 + wd5 ;
209     SATURATE(enc->apl[2], 12288, -12288);
210
211     /* UPPOL1 */
212
213     enc->sgl[0] = enc->plt[0] >> 15 ;
214     enc->sgl[1] = enc->plt[1] >> 15 ;
215
216     if ( enc->sgl[0] == enc->sgl[1] )  wd1 = 192 ;
217     else  wd1 = - 192 ;
218
219     wd2 = (enc->al[1] * 32640) >> 15 ;
220
221     enc->apl[1] = wd1 + wd2 ;
222     SATURATE(enc->apl[1], 32767, -32768);
223
224     wd3 = (15360 - enc->apl[2]) ;
225     SATURATE(wd3, 32767, -32768);
226
227     if ( enc->apl[1] >  wd3)  enc->apl[1] =  wd3 ;
228     if ( enc->apl[1] < -wd3)  enc->apl[1] = -wd3 ;
229
230     /* UPZERO */
231
232     if ( enc->dlt[0] == 0 )  wd1 = 0 ;
233     else wd1 = 128 ;
234
235     enc->sgl[0] = enc->dlt[0] >> 15 ;
236
237     for ( i = 1; i < 7; i++ ) {
238         enc->sgl[i] = enc->dlt[i] >> 15 ;
239         if ( enc->sgl[i] == enc->sgl[0] )  wd2 = wd1 ;
240         else wd2 = - wd1 ;
241         wd3 = (enc->bl[i] * 32640) >> 15 ;
242         enc->bpl[i] = wd2 + wd3 ;
243         SATURATE(enc->bpl[i], 32767, -32768);
244     }
245
246     /* DELAYA */
247
248     for ( i = 6; i > 0; i-- ) {
249         enc->dlt[i] = enc->dlt[i-1] ;
250         enc->bl[i]  = enc->bpl[i] ;
251     }
252
253     for ( i = 2; i > 0; i-- ) {
254         enc->rlt[i] = enc->rlt[i-1] ;
255         enc->plt[i] = enc->plt[i-1] ;
256         enc->al[i] = enc->apl[i] ;
257     }
258
259     /* FILTEP */
260
261     wd1 = enc->rlt[1] + enc->rlt[1];
262     SATURATE(wd1, 32767, -32768);
263     wd1 = ( enc->al[1] * wd1 ) >> 15 ;
264
265     wd2 = enc->rlt[2] + enc->rlt[2];
266     SATURATE(wd2, 32767, -32768);
267     wd2 = ( enc->al[2] * wd2 ) >> 15 ;
268
269     enc->spl = wd1 + wd2 ;
270     SATURATE(enc->spl, 32767, -32768);
271
272     /* FILTEZ */
273
274     enc->szl = 0 ;
275     for (i=6; i>0; i--) {
276         wd = enc->dlt[i] + enc->dlt[i];
277         SATURATE(wd, 32767, -32768);
278         enc->szl += (enc->bl[i] * wd) >> 15 ;
279         SATURATE(enc->szl, 32767, -32768);
280     }
281
282     /* PREDIC */
283
284     sl = enc->spl + enc->szl ;
285     SATURATE(sl, 32767, -32768);
286
287     return (sl) ;
288 }
289
290 static int block1h (int xh, int sh, int deth)
291 {
292     int ih ;
293
294     int eh, sih, mih, wd, wd1, hdu ;
295
296     static const int ihn[3] = { 0, 1, 0 } ;
297     static const int ihp[3] = { 0, 3, 2 } ;
298
299     /* SUBTRA */
300
301     eh = xh - sh ;
302     SATURATE(eh, 32767, -32768);
303
304     /* QUANTH */
305
306     sih = eh >> 15 ;
307     if (sih == 0 )  wd = eh ;
308     else wd = (32767 - eh) & 32767 ;
309
310     hdu = (564 << 3) * deth;
311     wd1 = (hdu >> 15) ;
312     if (wd >= wd1)  mih = 2 ;
313     else mih = 1 ;
314
315     if (sih == -1 ) ih = ihn[mih] ;
316     else ih = ihp[mih] ;
317
318     return (ih) ;
319 }
320
321 static int block2h (int ih, int deth)
322 {
323     int dh ;
324     int wd2 ;
325     static const int qm2[4] = {-7408, -1616, 7408, 1616};
326     
327     /* INVQAH */
328
329     wd2 = qm2[ih] ;
330     dh = (deth * wd2) >> 15 ;
331
332     return (dh) ;
333 }
334
335 static int block3h (g722_enc_t *enc, int ih)
336 {
337     int deth ;
338     int ih2, wd, wd1, wd2, wd3, nbph, deph ;
339     static const int wh[3] = {0, -214, 798} ;
340     static const int rh2[4] = {2, 1, 2, 1} ;
341     static const int ilb[32] = {
342         2048, 2093, 2139, 2186, 2233, 2282, 2332,
343         2383, 2435, 2489, 2543, 2599, 2656, 2714,
344         2774, 2834, 2896, 2960, 3025, 3091, 3158,
345         3228, 3298, 3371, 3444, 3520, 3597, 3676,
346         3756, 3838, 3922, 4008
347     };
348
349     /* LOGSCH */
350
351     ih2 = rh2[ih] ;
352     wd = (enc->nbh * 32512) >> 15 ;
353     nbph = wd + wh[ih2] ;
354
355     if (nbph <     0) nbph = 0 ;
356     if (nbph > 22528) nbph = 22528 ;
357
358     /* SCALEH */
359
360     wd1 =  (nbph >> 6) & 31 ;
361     wd2 = nbph >> 11 ;
362     if ((10-wd2) < 0) wd3 = ilb[wd1] << (wd2-10) ;
363     else wd3 = ilb[wd1] >> (10-wd2) ;
364     deph = wd3 << 2 ;
365
366     /* DELAYA */
367     enc->nbh = nbph ;
368     /* DELAYH */
369     deth = deph ;
370
371     return (deth) ;
372 }
373
374 static int block4h (g722_enc_t *enc, int d)
375 {
376     int sh = enc->shigh;
377     int i ;
378     int wd, wd1, wd2, wd3, wd4, wd5 /*, wd6 */;
379
380     enc->dh[0] = d;
381
382     /* RECONS */
383
384     enc->rh[0] = sh + enc->dh[0] ;
385     SATURATE(enc->rh[0], 32767, -32768);
386
387     /* PARREC */
388
389     enc->ph[0] = enc->dh[0] + enc->szh ;
390     SATURATE(enc->ph[0], 32767, -32768);
391
392     /* UPPOL2 */
393
394     enc->sgh[0] = enc->ph[0] >> 15 ;
395     enc->sgh[1] = enc->ph[1] >> 15 ;
396     enc->sgh[2] = enc->ph[2] >> 15 ;
397
398     wd1 = enc->ah[1] << 2;
399     SATURATE(wd1, 32767, -32768);
400
401     if ( enc->sgh[0] == enc->sgh[1] )  wd2 = - wd1 ;
402     else  wd2 = wd1 ;
403     if ( wd2 > 32767 ) wd2 = 32767;
404
405     wd2 = wd2 >> 7 ;
406
407     if ( enc->sgh[0] == enc->sgh[2] )  wd3 = 128 ;
408     else  wd3 = - 128 ;
409
410     wd4 = wd2 + wd3 ;
411     wd5 = (enc->ah[2] * 32512) >> 15 ;
412
413     enc->aph[2] = wd4 + wd5 ;
414     SATURATE(enc->aph[2], 12288, -12288);
415
416     /* UPPOL1 */
417
418     enc->sgh[0] = enc->ph[0] >> 15 ;
419     enc->sgh[1] = enc->ph[1] >> 15 ;
420
421     if ( enc->sgh[0] == enc->sgh[1] )  wd1 = 192 ;
422     else wd1 = - 192 ;
423
424     wd2 = (enc->ah[1] * 32640) >> 15 ;
425
426     enc->aph[1] = wd1 + wd2 ;
427     SATURATE(enc->aph[1], 32767, -32768);
428
429     wd3 = (15360 - enc->aph[2]) ;
430     SATURATE(wd3, 32767, -32768);
431
432     if ( enc->aph[1] >  wd3)  enc->aph[1] =  wd3 ;
433     else if ( enc->aph[1] < -wd3)  enc->aph[1] = -wd3 ;
434
435     /* UPZERO */
436
437     if ( enc->dh[0] == 0 )  wd1 = 0 ;
438     else wd1 = 128 ;
439
440     enc->sgh[0] = enc->dh[0] >> 15 ;
441
442     for ( i = 1; i < 7; i++ ) {
443         enc->sgh[i] = enc->dh[i] >> 15 ;
444         if ( enc->sgh[i] == enc->sgh[0] )  wd2 = wd1 ;
445         else wd2 = - wd1 ;
446         wd3 = (enc->bh[i] * 32640) >> 15 ;
447         enc->bph[i] = wd2 + wd3 ;
448         SATURATE(enc->bph[i], 32767, -32768);
449     }
450
451     /* DELAYA */
452     for ( i = 6; i > 0; i-- ) {
453         enc->dh[i] = enc->dh[i-1] ;
454         enc->bh[i]  = enc->bph[i] ;
455     }
456
457     for ( i = 2; i > 0; i-- ) {
458         enc->rh[i] = enc->rh[i-1] ;
459         enc->ph[i] = enc->ph[i-1] ;
460         enc->ah[i] = enc->aph[i] ;
461     }
462
463     /* FILTEP */
464
465     wd1 = enc->rh[1] + enc->rh[1];
466     SATURATE(wd1, 32767, -32768);
467     wd1 = ( enc->ah[1] * wd1 ) >> 15 ;
468
469     wd2 = enc->rh[2] + enc->rh[2];
470     SATURATE(wd2, 32767, -32768);
471     wd2 = ( enc->ah[2] * wd2 ) >> 15 ;
472
473     enc->sph = wd1 + wd2 ;
474     SATURATE(enc->sph, 32767, -32768);
475
476     /* FILTEZ */
477
478     enc->szh = 0 ;
479     for (i=6; i>0; i--) {
480         wd = enc->dh[i] + enc->dh[i];
481         SATURATE(wd, 32767, -32768);
482         enc->szh += (enc->bh[i] * wd) >> 15 ;
483         SATURATE(enc->szh, 32767, -32768);
484     }
485
486     /* PREDIC */
487
488     sh = enc->sph + enc->szh ;
489     SATURATE(sh, 32767, -32768);
490
491     return (sh) ;
492 }
493
494 /* PROCESS PCM THROUGH THE QMF FILTER */
495 static void tx_qmf(g722_enc_t *enc, int pcm1, int pcm2, int *lo, int *hi)
496 {
497     int sumodd, sumeven;
498     int i;
499
500     pj_memmove(&enc->x[2], enc->x, 22 * sizeof(enc->x[0]));
501     enc->x[1] = pcm1; 
502     enc->x[0] = pcm2;
503
504     sumodd = 0;
505     for (i=1; i<24; i+=2) sumodd += enc->x[i] * g722_qmf_coeff[i];
506
507     sumeven = 0;
508     for (i=0; i<24; i+=2) sumeven += enc->x[i] * g722_qmf_coeff[i];
509
510     *lo  = (sumeven + sumodd) >> 13  ;
511     *hi = (sumeven - sumodd) >> 13  ;
512
513     SATURATE(*lo, 16383, -16384);
514     SATURATE(*hi, 16383, -16383);
515 }
516
517
518 PJ_DEF(pj_status_t) g722_enc_init(g722_enc_t *enc)
519 {
520     PJ_ASSERT_RETURN(enc, PJ_EINVAL);
521     
522     pj_bzero(enc, sizeof(g722_enc_t));
523
524     enc->detlow = 32;
525     enc->dethigh = 8;
526
527     return PJ_SUCCESS;
528 }
529
530 PJ_DEF(pj_status_t) g722_enc_encode( g722_enc_t *enc, 
531                                      pj_int16_t in[], 
532                                      pj_size_t nsamples,
533                                      void *out,
534                                      pj_size_t *out_size)
535 {
536     unsigned i;
537     int xlow, ilow, dlowt;
538     int xhigh, ihigh, dhigh;
539     pj_uint8_t *out_ = (pj_uint8_t*) out;
540
541     PJ_ASSERT_RETURN(enc && in && nsamples && out && out_size, PJ_EINVAL);
542     PJ_ASSERT_RETURN(nsamples % 2 == 0, PJ_EINVAL);
543     PJ_ASSERT_RETURN(*out_size >= (nsamples >> 1), PJ_ETOOSMALL);
544     
545     for(i = 0; i < nsamples; i += 2) {
546         tx_qmf(enc, in[i], in[i+1], &xlow, &xhigh);
547
548         /* low band encoder */
549         ilow = block1l (xlow, enc->slow, enc->detlow) ;
550         dlowt = block2l (ilow, enc->detlow) ;
551         enc->detlow = block3l (enc, ilow) ;
552         enc->slow = block4l (enc, dlowt) ;
553
554         /* high band encoder */
555         ihigh = block1h (xhigh, enc->shigh, enc->dethigh) ;
556         dhigh = block2h (ihigh, enc->dethigh) ;
557         enc->dethigh = block3h (enc, ihigh) ;
558         enc->shigh = block4h (enc, dhigh) ;
559
560         /* bits mix low & high adpcm */
561         out_[i/2] = (pj_uint8_t)((ihigh << 6) | ilow);
562     }
563
564     *out_size = nsamples >> 1;
565
566     return PJ_SUCCESS;
567 }
568
569 PJ_DEF(pj_status_t) g722_enc_deinit(g722_enc_t *enc)
570 {
571     pj_bzero(enc, sizeof(g722_enc_t));
572
573     return PJ_SUCCESS;
574 }
575
576 #endif