1 /*
2  * SpanDSP - a series of DSP components for telephony
3  *
4  * g722_decode.c - The ITU G.722 codec, decode part.
5  *
6  * Written by Steve Underwood <steveu@coppice.org>
7  *
8  * Copyright (C) 2005 Steve Underwood
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 in part on a single channel 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  * $Id: g722_decode.c,v 1.2 2012/08/12 08:06:58 sobomax Exp $
27  */
28 
29 /*! \file */
30 
31 #include <stdio.h>
32 #include <inttypes.h>
33 #include <memory.h>
34 #include <stdlib.h>
35 
36 #include "g722.h"
37 #include "g722_private.h"
38 
39 #if !defined(FALSE)
40 #define FALSE 0
41 #endif
42 #if !defined(TRUE)
43 #define TRUE (!FALSE)
44 #endif
45 
saturate(int32_t amp)46 static __inline__ int16_t saturate(int32_t amp)
47 {
48     int16_t amp16;
49 
50     /* Hopefully this is optimised for the common case - not clipping */
51     amp16 = (int16_t) amp;
52     if (amp == amp16)
53         return amp16;
54     if (amp > INT16_MAX)
55         return  INT16_MAX;
56     return  INT16_MIN;
57 }
58 /*- End of function --------------------------------------------------------*/
59 
60 static void block4(G722_DEC_CTX *s, int band, int d);
61 
block4(G722_DEC_CTX * s,int band,int d)62 static void block4(G722_DEC_CTX *s, int band, int d)
63 {
64     int wd1;
65     int wd2;
66     int wd3;
67     int i;
68 
69     /* Block 4, RECONS */
70     s->band[band].d[0] = d;
71     s->band[band].r[0] = saturate(s->band[band].s + d);
72 
73     /* Block 4, PARREC */
74     s->band[band].p[0] = saturate(s->band[band].sz + d);
75 
76     /* Block 4, UPPOL2 */
77     for (i = 0;  i < 3;  i++)
78         s->band[band].sg[i] = s->band[band].p[i] >> 15;
79     wd1 = saturate(s->band[band].a[1] << 2);
80 
81     wd2 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  -wd1  :  wd1;
82     if (wd2 > 32767)
83         wd2 = 32767;
84     wd3 = (s->band[band].sg[0] == s->band[band].sg[2])  ?  128  :  -128;
85     wd3 += (wd2 >> 7);
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_decoder_new(int rate,int options)152 G722_DEC_CTX *g722_decoder_new(int rate, int options)
153 {
154     G722_DEC_CTX *s;
155 
156     if ((s = (G722_DEC_CTX *) malloc(sizeof(*s))) == NULL)
157         return NULL;
158     memset(s, 0, sizeof(*s));
159     if (rate == 48000)
160         s->bits_per_sample = 6;
161     else if (rate == 56000)
162         s->bits_per_sample = 7;
163     else
164         s->bits_per_sample = 8;
165     if ((options & G722_SAMPLE_RATE_8000))
166         s->eight_k = TRUE;
167     if ((options & G722_PACKED)  &&  s->bits_per_sample != 8)
168         s->packed = TRUE;
169     else
170         s->packed = FALSE;
171     s->band[0].det = 32;
172     s->band[1].det = 8;
173     return s;
174 }
175 /*- End of function --------------------------------------------------------*/
176 
g722_decoder_destroy(G722_DEC_CTX * s)177 int g722_decoder_destroy(G722_DEC_CTX *s)
178 {
179     free(s);
180     return 0;
181 }
182 /*- End of function --------------------------------------------------------*/
183 
g722_decode(G722_DEC_CTX * s,const uint8_t g722_data[],int len,int16_t amp[])184 int g722_decode(G722_DEC_CTX *s, const uint8_t g722_data[], int len, int16_t amp[])
185 {
186     static const int wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 };
187     static const int rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3,  2, 1, 0 };
188     static const int ilb[32] =
189     {
190         2048, 2093, 2139, 2186, 2233, 2282, 2332,
191         2383, 2435, 2489, 2543, 2599, 2656, 2714,
192         2774, 2834, 2896, 2960, 3025, 3091, 3158,
193         3228, 3298, 3371, 3444, 3520, 3597, 3676,
194         3756, 3838, 3922, 4008
195     };
196     static const int wh[3] = {0, -214, 798};
197     static const int rh2[4] = {2, 1, 2, 1};
198     static const int qm2[4] = {-7408, -1616,  7408,   1616};
199     static const int qm4[16] =
200     {
201               0, -20456, -12896,  -8968,
202           -6288,  -4240,  -2584,  -1200,
203           20456,  12896,   8968,   6288,
204            4240,   2584,   1200,      0
205     };
206     static const int qm5[32] =
207     {
208            -280,   -280, -23352, -17560,
209          -14120, -11664,  -9752,  -8184,
210           -6864,  -5712,  -4696,  -3784,
211           -2960,  -2208,  -1520,   -880,
212           23352,  17560,  14120,  11664,
213            9752,   8184,   6864,   5712,
214            4696,   3784,   2960,   2208,
215            1520,    880,    280,   -280
216     };
217     static const int qm6[64] =
218     {
219            -136,   -136,   -136,   -136,
220          -24808, -21904, -19008, -16704,
221          -14984, -13512, -12280, -11192,
222          -10232,  -9360,  -8576,  -7856,
223           -7192,  -6576,  -6000,  -5456,
224           -4944,  -4464,  -4008,  -3576,
225           -3168,  -2776,  -2400,  -2032,
226           -1688,  -1360,  -1040,   -728,
227           24808,  21904,  19008,  16704,
228           14984,  13512,  12280,  11192,
229           10232,   9360,   8576,   7856,
230            7192,   6576,   6000,   5456,
231            4944,   4464,   4008,   3576,
232            3168,   2776,   2400,   2032,
233            1688,   1360,   1040,    728,
234             432,    136,   -432,   -136
235     };
236     static const int qmf_coeffs[12] =
237     {
238            3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
239     };
240 
241     int dlowt;
242     int rlow;
243     int ihigh;
244     int dhigh;
245     int rhigh;
246     int xout1;
247     int xout2;
248     int wd1;
249     int wd2;
250     int wd3;
251     int code;
252     int outlen;
253     int i;
254     int j;
255 
256     outlen = 0;
257     rhigh = 0;
258     for (j = 0;  j < len;  )
259     {
260         if (s->packed)
261         {
262             /* Unpack the code bits */
263             if (s->in_bits < s->bits_per_sample)
264             {
265                 s->in_buffer |= (g722_data[j++] << s->in_bits);
266                 s->in_bits += 8;
267             }
268             code = s->in_buffer & ((1 << s->bits_per_sample) - 1);
269             s->in_buffer >>= s->bits_per_sample;
270             s->in_bits -= s->bits_per_sample;
271         }
272         else
273         {
274             code = g722_data[j++];
275         }
276 
277         switch (s->bits_per_sample)
278         {
279         default:
280         case 8:
281             wd1 = code & 0x3F;
282             ihigh = (code >> 6) & 0x03;
283             wd2 = qm6[wd1];
284             wd1 >>= 2;
285             break;
286         case 7:
287             wd1 = code & 0x1F;
288             ihigh = (code >> 5) & 0x03;
289             wd2 = qm5[wd1];
290             wd1 >>= 1;
291             break;
292         case 6:
293             wd1 = code & 0x0F;
294             ihigh = (code >> 4) & 0x03;
295             wd2 = qm4[wd1];
296             break;
297         }
298         /* Block 5L, LOW BAND INVQBL */
299         wd2 = (s->band[0].det*wd2) >> 15;
300         /* Block 5L, RECONS */
301         rlow = s->band[0].s + wd2;
302         /* Block 6L, LIMIT */
303         if (rlow > 16383)
304             rlow = 16383;
305         else if (rlow < -16384)
306             rlow = -16384;
307 
308         /* Block 2L, INVQAL */
309         wd2 = qm4[wd1];
310         dlowt = (s->band[0].det*wd2) >> 15;
311 
312         /* Block 3L, LOGSCL */
313         wd2 = rl42[wd1];
314         wd1 = (s->band[0].nb*127) >> 7;
315         wd1 += wl[wd2];
316         if (wd1 < 0)
317             wd1 = 0;
318         else if (wd1 > 18432)
319             wd1 = 18432;
320         s->band[0].nb = wd1;
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, dlowt);
329 
330         if (!s->eight_k)
331         {
332             /* Block 2H, INVQAH */
333             wd2 = qm2[ihigh];
334             dhigh = (s->band[1].det*wd2) >> 15;
335             /* Block 5H, RECONS */
336             rhigh = dhigh + s->band[1].s;
337             /* Block 6H, LIMIT */
338             if (rhigh > 16383)
339                 rhigh = 16383;
340             else if (rhigh < -16384)
341                 rhigh = -16384;
342 
343             /* Block 2H, INVQAH */
344             wd2 = rh2[ihigh];
345             wd1 = (s->band[1].nb*127) >> 7;
346             wd1 += wh[wd2];
347             if (wd1 < 0)
348                 wd1 = 0;
349             else if (wd1 > 22528)
350                 wd1 = 22528;
351             s->band[1].nb = wd1;
352 
353             /* Block 3H, SCALEH */
354             wd1 = (s->band[1].nb >> 6) & 31;
355             wd2 = 10 - (s->band[1].nb >> 11);
356             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
357             s->band[1].det = wd3 << 2;
358 
359             block4(s, 1, dhigh);
360         }
361 
362         if (s->itu_test_mode)
363         {
364             amp[outlen++] = (int16_t) (rlow << 1);
365             amp[outlen++] = (int16_t) (rhigh << 1);
366         }
367         else
368         {
369             if (s->eight_k)
370             {
371                 amp[outlen++] = (int16_t) (rlow << 1);
372             }
373             else
374             {
375                 /* Apply the receive QMF */
376                 for (i = 0;  i < 22;  i++)
377                     s->x[i] = s->x[i + 2];
378                 s->x[22] = rlow + rhigh;
379                 s->x[23] = rlow - rhigh;
380 
381                 xout1 = 0;
382                 xout2 = 0;
383                 for (i = 0;  i < 12;  i++)
384                 {
385                     xout2 += s->x[2*i]*qmf_coeffs[i];
386                     xout1 += s->x[2*i + 1]*qmf_coeffs[11 - i];
387                 }
388                 amp[outlen++] = (int16_t) (xout1 >> 11);
389                 amp[outlen++] = (int16_t) (xout2 >> 11);
390             }
391         }
392     }
393     return outlen;
394 }
395 /*- End of function --------------------------------------------------------*/
396 /*- End of file ------------------------------------------------------------*/
397