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