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
block1l(int xl,int sl,int detl)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
block2l(int il,int detl)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
block3l(g722_enc_t * enc,int il)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
block4l(g722_enc_t * enc,int dl)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
block1h(int xh,int sh,int deth)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
block2h(int ih,int deth)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
block3h(g722_enc_t * enc,int ih)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
block4h(g722_enc_t * enc,int d)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 */
tx_qmf(g722_enc_t * enc,int pcm1,int pcm2,int * lo,int * hi)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
g722_enc_init(g722_enc_t * enc)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
g722_enc_encode(g722_enc_t * enc,pj_int16_t in[],pj_size_t nsamples,void * out,pj_size_t * out_size)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
g722_enc_deinit(g722_enc_t * enc)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
577