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