1 /*
2  * VoIPcodecs - a series of DSP components for telephony
3  *
4  * g722_encode.c - The ITU G.722 codec, encode part.
5  *
6  * Written by Steve Underwood <steveu@coppice.org>
7  *
8  * Copyright (C) 2005 Steve Underwood
9  *
10  * All rights reserved.
11  *
12  * This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License version 2, or
14  * the Lesser GNU General Public License version 2.1, as published by
15  * the Free Software Foundation.
16  *
17  * This program is distributed in the hope that it will be useful,
18  * but WITHOUT ANY WARRANTY; without even the implied warranty of
19  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
20  * GNU General Public License for more details.
21  *
22  * You should have received a copy of the GNU General Public License
23  * along with this program; if not, write to the Free Software
24  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
25  *
26  * Based on a single channel 64kbps only G.722 codec which is:
27  *
28  *****    Copyright (c) CMU    1993      *****
29  * Computer Science, Speech Group
30  * Chengxiang Lu and Alex Hauptmann
31  *
32  * $Id: g722_encode.c,v 1.18 2008/02/09 15:32:56 steveu Exp $
33  */
34 
35 /*! \file */
36 
37 #ifdef HAVE_CONFIG_H
38 #include <config.h>
39 #endif
40 
41 #include "inttypes.h"
42 #include <memory.h>
43 #include <stdlib.h>
44 #if defined(HAVE_TGMATH_H)
45 #include <tgmath.h>
46 #endif
47 #include <math.h>
48 
49 #include "telephony.h"
50 #include "dc_restore.h"
51 #include "g722.h"
52 
block4(g722_encode_state_t * s,int band,int d)53 static void block4(g722_encode_state_t *s, int band, int d)
54 {
55     int wd1;
56     int wd2;
57     int wd3;
58     int i;
59 
60     /* Block 4, RECONS */
61     s->band[band].d[0] = d;
62     s->band[band].r[0] = saturate(s->band[band].s + d);
63 
64     /* Block 4, PARREC */
65     s->band[band].p[0] = saturate(s->band[band].sz + d);
66 
67     /* Block 4, UPPOL2 */
68     for (i = 0;  i < 3;  i++)
69         s->band[band].sg[i] = s->band[band].p[i] >> 15;
70     wd1 = saturate(s->band[band].a[1] << 2);
71 
72     wd2 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  -wd1  :  wd1;
73     if (wd2 > 32767)
74         wd2 = 32767;
75     wd3 = (wd2 >> 7) + ((s->band[band].sg[0] == s->band[band].sg[2])  ?  128  :  -128);
76     wd3 += (s->band[band].a[2]*32512) >> 15;
77     if (wd3 > 12288)
78         wd3 = 12288;
79     else if (wd3 < -12288)
80         wd3 = -12288;
81     s->band[band].ap[2] = wd3;
82 
83     /* Block 4, UPPOL1 */
84     s->band[band].sg[0] = s->band[band].p[0] >> 15;
85     s->band[band].sg[1] = s->band[band].p[1] >> 15;
86     wd1 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  192  :  -192;
87     wd2 = (s->band[band].a[1]*32640) >> 15;
88 
89     s->band[band].ap[1] = saturate(wd1 + wd2);
90     wd3 = saturate(15360 - s->band[band].ap[2]);
91     if (s->band[band].ap[1] > wd3)
92         s->band[band].ap[1] = wd3;
93     else if (s->band[band].ap[1] < -wd3)
94         s->band[band].ap[1] = -wd3;
95 
96     /* Block 4, UPZERO */
97     wd1 = (d == 0)  ?  0  :  128;
98     s->band[band].sg[0] = d >> 15;
99     for (i = 1;  i < 7;  i++)
100     {
101         s->band[band].sg[i] = s->band[band].d[i] >> 15;
102         wd2 = (s->band[band].sg[i] == s->band[band].sg[0])  ?  wd1  :  -wd1;
103         wd3 = (s->band[band].b[i]*32640) >> 15;
104         s->band[band].bp[i] = saturate(wd2 + wd3);
105     }
106 
107     /* Block 4, DELAYA */
108     for (i = 6;  i > 0;  i--)
109     {
110         s->band[band].d[i] = s->band[band].d[i - 1];
111         s->band[band].b[i] = s->band[band].bp[i];
112     }
113 
114     for (i = 2;  i > 0;  i--)
115     {
116         s->band[band].r[i] = s->band[band].r[i - 1];
117         s->band[band].p[i] = s->band[band].p[i - 1];
118         s->band[band].a[i] = s->band[band].ap[i];
119     }
120 
121     /* Block 4, FILTEP */
122     wd1 = saturate(s->band[band].r[1] + s->band[band].r[1]);
123     wd1 = (s->band[band].a[1]*wd1) >> 15;
124     wd2 = saturate(s->band[band].r[2] + s->band[band].r[2]);
125     wd2 = (s->band[band].a[2]*wd2) >> 15;
126     s->band[band].sp = saturate(wd1 + wd2);
127 
128     /* Block 4, FILTEZ */
129     s->band[band].sz = 0;
130     for (i = 6;  i > 0;  i--)
131     {
132         wd1 = saturate(s->band[band].d[i] + s->band[band].d[i]);
133         s->band[band].sz += (s->band[band].b[i]*wd1) >> 15;
134     }
135     s->band[band].sz = saturate(s->band[band].sz);
136 
137     /* Block 4, PREDIC */
138     s->band[band].s = saturate(s->band[band].sp + s->band[band].sz);
139 }
140 /*- End of function --------------------------------------------------------*/
141 
g722_encode_init(g722_encode_state_t * s,int rate,int options)142 g722_encode_state_t *g722_encode_init(g722_encode_state_t *s, int rate, int options)
143 {
144     if (s == NULL)
145     {
146         if ((s = (g722_encode_state_t *) malloc(sizeof(*s))) == NULL)
147             return NULL;
148     }
149     memset(s, 0, sizeof(*s));
150     if (rate == 48000)
151         s->bits_per_sample = 6;
152     else if (rate == 56000)
153         s->bits_per_sample = 7;
154     else
155         s->bits_per_sample = 8;
156     if ((options & G722_SAMPLE_RATE_8000))
157         s->eight_k = TRUE;
158     if ((options & G722_PACKED)  &&  s->bits_per_sample != 8)
159         s->packed = TRUE;
160     else
161         s->packed = FALSE;
162     s->band[0].det = 32;
163     s->band[1].det = 8;
164     return s;
165 }
166 /*- End of function --------------------------------------------------------*/
167 
g722_encode_release(g722_encode_state_t * s)168 int g722_encode_release(g722_encode_state_t *s)
169 {
170     free(s);
171     return 0;
172 }
173 /*- End of function --------------------------------------------------------*/
174 
g722_encode(g722_encode_state_t * s,uint8_t g722_data[],const int16_t amp[],int len)175 int g722_encode(g722_encode_state_t *s, uint8_t g722_data[], const int16_t amp[], int len)
176 {
177     static const int q6[32] =
178     {
179            0,   35,   72,  110,  150,  190,  233,  276,
180          323,  370,  422,  473,  530,  587,  650,  714,
181          786,  858,  940, 1023, 1121, 1219, 1339, 1458,
182         1612, 1765, 1980, 2195, 2557, 2919,    0,    0
183     };
184     static const int iln[32] =
185     {
186          0, 63, 62, 31, 30, 29, 28, 27,
187         26, 25, 24, 23, 22, 21, 20, 19,
188         18, 17, 16, 15, 14, 13, 12, 11,
189         10,  9,  8,  7,  6,  5,  4,  0
190     };
191     static const int ilp[32] =
192     {
193          0, 61, 60, 59, 58, 57, 56, 55,
194         54, 53, 52, 51, 50, 49, 48, 47,
195         46, 45, 44, 43, 42, 41, 40, 39,
196         38, 37, 36, 35, 34, 33, 32,  0
197     };
198     static const int wl[8] =
199     {
200         -60, -30, 58, 172, 334, 538, 1198, 3042
201     };
202     static const int rl42[16] =
203     {
204         0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
205     };
206     static const int ilb[32] =
207     {
208         2048, 2093, 2139, 2186, 2233, 2282, 2332,
209         2383, 2435, 2489, 2543, 2599, 2656, 2714,
210         2774, 2834, 2896, 2960, 3025, 3091, 3158,
211         3228, 3298, 3371, 3444, 3520, 3597, 3676,
212         3756, 3838, 3922, 4008
213     };
214     static const int qm4[16] =
215     {
216              0, -20456, -12896, -8968,
217          -6288,  -4240,  -2584, -1200,
218          20456,  12896,   8968,  6288,
219           4240,   2584,   1200,     0
220     };
221     static const int qm2[4] =
222     {
223         -7408,  -1616,   7408,   1616
224     };
225     static const int qmf_coeffs[12] =
226     {
227            3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
228     };
229     static const int ihn[3] = {0, 1, 0};
230     static const int ihp[3] = {0, 3, 2};
231     static const int wh[3] = {0, -214, 798};
232     static const int rh2[4] = {2, 1, 2, 1};
233 
234     int dlow;
235     int dhigh;
236     int el;
237     int wd;
238     int wd1;
239     int ril;
240     int wd2;
241     int il4;
242     int ih2;
243     int wd3;
244     int eh;
245     int mih;
246     int i;
247     int j;
248     /* Low and high band PCM from the QMF */
249     int xlow;
250     int xhigh;
251     int g722_bytes;
252     /* Even and odd tap accumulators */
253     int sumeven;
254     int sumodd;
255     int ihigh;
256     int ilow;
257     int code;
258 
259     g722_bytes = 0;
260     xhigh = 0;
261     for (j = 0;  j < len;  )
262     {
263         if (s->itu_test_mode)
264         {
265             xlow =
266             xhigh = amp[j++] >> 1;
267         }
268         else
269         {
270             if (s->eight_k)
271             {
272                 xlow = amp[j++];
273             }
274             else
275             {
276                 /* Apply the transmit QMF */
277                 /* Shuffle the buffer down */
278                 memcpy(s->x, &s->x[2], 22*sizeof(s->x[0]));
279                 s->x[22] = amp[j++];
280                 s->x[23] = amp[j++];
281 
282                 /* Discard every other QMF output */
283                 sumeven = 0;
284                 sumodd = 0;
285                 for (i = 0;  i < 12;  i++)
286                 {
287                     sumodd += s->x[2*i]*qmf_coeffs[i];
288                     sumeven += s->x[2*i + 1]*qmf_coeffs[11 - i];
289                 }
290                 xlow = (sumeven + sumodd) >> 13;
291                 xhigh = (sumeven - sumodd) >> 13;
292             }
293         }
294         /* Block 1L, SUBTRA */
295         el = saturate(xlow - s->band[0].s);
296 
297         /* Block 1L, QUANTL */
298         wd = (el >= 0)  ?  el  :  -(el + 1);
299 
300         for (i = 1;  i < 30;  i++)
301         {
302             wd1 = (q6[i]*s->band[0].det) >> 12;
303             if (wd < wd1)
304                 break;
305         }
306         ilow = (el < 0)  ?  iln[i]  :  ilp[i];
307 
308         /* Block 2L, INVQAL */
309         ril = ilow >> 2;
310         wd2 = qm4[ril];
311         dlow = (s->band[0].det*wd2) >> 15;
312 
313         /* Block 3L, LOGSCL */
314         il4 = rl42[ril];
315         wd = (s->band[0].nb*127) >> 7;
316         s->band[0].nb = wd + wl[il4];
317         if (s->band[0].nb < 0)
318             s->band[0].nb = 0;
319         else if (s->band[0].nb > 18432)
320             s->band[0].nb = 18432;
321 
322         /* Block 3L, SCALEL */
323         wd1 = (s->band[0].nb >> 6) & 31;
324         wd2 = 8 - (s->band[0].nb >> 11);
325         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
326         s->band[0].det = wd3 << 2;
327 
328         block4(s, 0, dlow);
329 
330         if (s->eight_k)
331         {
332             /* Just leave the high bits as zero */
333             code = (0xC0 | ilow) >> (8 - s->bits_per_sample);
334         }
335         else
336         {
337             /* Block 1H, SUBTRA */
338             eh = saturate(xhigh - s->band[1].s);
339 
340             /* Block 1H, QUANTH */
341             wd = (eh >= 0)  ?  eh  :  -(eh + 1);
342             wd1 = (564*s->band[1].det) >> 12;
343             mih = (wd >= wd1)  ?  2  :  1;
344             ihigh = (eh < 0)  ?  ihn[mih]  :  ihp[mih];
345 
346             /* Block 2H, INVQAH */
347             wd2 = qm2[ihigh];
348             dhigh = (s->band[1].det*wd2) >> 15;
349 
350             /* Block 3H, LOGSCH */
351             ih2 = rh2[ihigh];
352             wd = (s->band[1].nb*127) >> 7;
353             s->band[1].nb = wd + wh[ih2];
354             if (s->band[1].nb < 0)
355                 s->band[1].nb = 0;
356             else if (s->band[1].nb > 22528)
357                 s->band[1].nb = 22528;
358 
359             /* Block 3H, SCALEH */
360             wd1 = (s->band[1].nb >> 6) & 31;
361             wd2 = 10 - (s->band[1].nb >> 11);
362             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
363             s->band[1].det = wd3 << 2;
364 
365             block4(s, 1, dhigh);
366             code = ((ihigh << 6) | ilow) >> (8 - s->bits_per_sample);
367         }
368 
369         if (s->packed)
370         {
371             /* Pack the code bits */
372             s->out_buffer |= (code << s->out_bits);
373             s->out_bits += s->bits_per_sample;
374             if (s->out_bits >= 8)
375             {
376                 g722_data[g722_bytes++] = (uint8_t) (s->out_buffer & 0xFF);
377                 s->out_bits -= 8;
378                 s->out_buffer >>= 8;
379             }
380         }
381         else
382         {
383             g722_data[g722_bytes++] = (uint8_t) code;
384         }
385     }
386     return g722_bytes;
387 }
388 /*- End of function --------------------------------------------------------*/
389 /*- End of file ------------------------------------------------------------*/
390