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  * $Id: g722_decode.c 194722 2009-05-15 17:59:08Z russell $
22  */
23 
24 /*! \file */
25 
26 #include <string.h>
27 #include <inttypes.h>
28 #include <stdlib.h>
29 
30 #include "g722_typedefs.h"
31 #include "g722_enc_dec.h"
32 
33 #if !defined(FALSE)
34 #define FALSE 0
35 #endif
36 #if !defined(TRUE)
37 #define TRUE (!FALSE)
38 #endif
39 
40 #define PACKED_INPUT    (0)
41 #define BITS_PER_SAMPLE (8)
42 
43 #ifndef BUILD_FEATURE_G722_USE_INTRINSIC_SAT
__ssat16(int32_t amp)44 static __inline int16_t __ssat16(int32_t amp)
45 {
46     int16_t amp16;
47 
48     /* Hopefully this is optimised for the common case - not clipping */
49     amp16 = (int16_t) amp;
50     if (amp == amp16)
51         return amp16;
52     if (amp > 0x7fff)
53         return  0x7fff;
54     return  0x8000;
55 }
56 /*- End of function --------------------------------------------------------*/
57 #else
__ssat16(int32_t val)58 static __inline int16_t __ssat16( int32_t val)
59 {
60     register int32_t res;
61     __asm volatile (
62         "SSAT %0, #16, %1\n\t"
63         :"=r"(res)
64         :"r"(val)
65         :);
66     return (int16_t)res;
67 }
68 #endif
69 
70 /*- End of function --------------------------------------------------------*/
71 
72 static void block4(g722_band_t *band, int d);
73 
block4(g722_band_t * band,int d)74 static void block4(g722_band_t *band, int d)
75 {
76     int wd1;
77     int wd2;
78     int wd3;
79     int i;
80     int sg[7];
81     int ap1, ap2;
82     int sg0, sgi;
83     int sz;
84 
85     /* Block 4, RECONS */
86     band->d[0] = d;
87     band->r[0] = __ssat16(band->s + d);
88 
89     /* Block 4, PARREC */
90     band->p[0] = __ssat16(band->sz + d);
91 
92     /* Block 4, UPPOL2 */
93     for (i = 0;  i < 3;  i++)
94     {
95         sg[i] = band->p[i] >> 15;
96     }
97     wd1 = __ssat16(band->a[1] << 2);
98 
99     wd2 = (sg[0] == sg[1])  ?  -wd1  :  wd1;
100     if (wd2 > 32767)
101         wd2 = 32767;
102 
103     ap2 = (sg[0] == sg[2])  ?  128  :  -128;
104     ap2 += (wd2 >> 7);
105     ap2 += (band->a[2]*32512) >> 15;
106     if (ap2 > 12288)
107         ap2 = 12288;
108     else if (ap2 < -12288)
109         ap2 = -12288;
110     band->ap[2] = ap2;
111 
112     /* Block 4, UPPOL1 */
113     sg[0] = band->p[0] >> 15;
114     sg[1] = band->p[1] >> 15;
115     wd1 = (sg[0] == sg[1])  ?  192  :  -192;
116     wd2 = (band->a[1]*32640) >> 15;
117 
118     ap1 = __ssat16(wd1 + wd2);
119     wd3 = __ssat16(15360 - band->ap[2]);
120     if (ap1 > wd3)
121         ap1 = wd3;
122     else if (ap1 < -wd3)
123         ap1 = -wd3;
124     band->ap[1] = ap1;
125 
126     /* Block 4, UPZERO */
127     /* Block 4, FILTEZ */
128     wd1 = (d == 0)  ?  0  :  128;
129 
130     sg0 = sg[0] = d >> 15;
131     for (i = 1;  i < 7;  i++)
132     {
133 	sgi = band->d[i] >> 15;
134         wd2 = (sgi == sg0)  ?  wd1  :  -wd1;
135         wd3 = (band->b[i]*32640) >> 15;
136         band->bp[i] = __ssat16(wd2 + wd3);
137     }
138 
139     /* Block 4, DELAYA */
140     sz = 0;
141     for (i = 6;  i > 0;  i--)
142     {
143 	int bi;
144 
145         band->d[i] = band->d[i - 1];
146 	bi = band->b[i] = band->bp[i];
147 	wd1 = __ssat16(band->d[i] + band->d[i]);
148 	sz += (bi*wd1) >> 15;
149     }
150     band->sz = sz;
151 
152     for (i = 2;  i > 0;  i--)
153     {
154         band->r[i] = band->r[i - 1];
155         band->p[i] = band->p[i - 1];
156         band->a[i] = band->ap[i];
157     }
158 
159     /* Block 4, FILTEP */
160     wd1 = __ssat16(band->r[1] + band->r[1]);
161     wd1 = (band->a[1]*wd1) >> 15;
162     wd2 = __ssat16(band->r[2] + band->r[2]);
163     wd2 = (band->a[2]*wd2) >> 15;
164     band->sp = __ssat16(wd1 + wd2);
165 
166     /* Block 4, PREDIC */
167     band->s = __ssat16(band->sp + band->sz);
168 }
169 /*- End of function --------------------------------------------------------*/
170 
g722_decode_init(g722_decode_state_t * s,unsigned int rate,int options)171 g722_decode_state_t *g722_decode_init(g722_decode_state_t *s, unsigned int rate, int options)
172 {
173     if (s == NULL)
174     {
175 #ifdef G722_SUPPORT_MALLOC
176         if ((s = (g722_decode_state_t *) malloc(sizeof(*s))) == NULL)
177 #endif
178             return NULL;
179     }
180     memset(s, 0, sizeof(*s));
181     if (rate == 48000)
182         s->bits_per_sample = 6;
183     else if (rate == 56000)
184         s->bits_per_sample = 7;
185     else
186         s->bits_per_sample = 8;
187     s->dac_pcm = options & G722_FORMAT_DAC12;
188     s->band[0].det = 32;
189     s->band[1].det = 8;
190     return s;
191 }
192 /*- End of function --------------------------------------------------------*/
193 
g722_decode_release(g722_decode_state_t * s)194 int g722_decode_release(g722_decode_state_t *s)
195 {
196     free(s);
197     return 0;
198 }
199 /*- End of function --------------------------------------------------------*/
200 
201 static int16_t wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 };
202 static int16_t rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3,  2, 1, 0 };
203 static int16_t ilb[32] =
204 {
205     2048, 2093, 2139, 2186, 2233, 2282, 2332,
206     2383, 2435, 2489, 2543, 2599, 2656, 2714,
207     2774, 2834, 2896, 2960, 3025, 3091, 3158,
208     3228, 3298, 3371, 3444, 3520, 3597, 3676,
209     3756, 3838, 3922, 4008
210 };
211 
212 static int16_t wh[3] = {0, -214, 798};
213 static int16_t rh2[4] = {2, 1, 2, 1};
214 static int16_t qm2[4] = {-7408, -1616,  7408,   1616};
215 static int16_t qm4[16] =
216 {
217         0, -20456, -12896,  -8968,
218     -6288,  -4240,  -2584,  -1200,
219     20456,  12896,   8968,   6288,
220      4240,   2584,   1200,      0
221 };
222 #if 0
223 static const int qm5[32] =
224 {
225       -280,   -280, -23352, -17560,
226     -14120, -11664,  -9752,  -8184,
227      -6864,  -5712,  -4696,  -3784,
228      -2960,  -2208,  -1520,   -880,
229      23352,  17560,  14120,  11664,
230       9752,   8184,   6864,   5712,
231       4696,   3784,   2960,   2208,
232       1520,    880,    280,   -280
233 };
234 #endif
235 static int16_t qm6[64] =
236 {
237       -136,   -136,   -136,   -136,
238     -24808, -21904, -19008, -16704,
239     -14984, -13512, -12280, -11192,
240     -10232,  -9360,  -8576,  -7856,
241      -7192,  -6576,  -6000,  -5456,
242      -4944,  -4464,  -4008,  -3576,
243      -3168,  -2776,  -2400,  -2032,
244      -1688,  -1360,  -1040,   -728,
245      24808,  21904,  19008,  16704,
246      14984,  13512,  12280,  11192,
247      10232,   9360,   8576,   7856,
248       7192,   6576,   6000,   5456,
249       4944,   4464,   4008,   3576,
250       3168,   2776,   2400,   2032,
251       1688,   1360,   1040,    728,
252        432,    136,   -432,   -136
253 };
254 static int16_t qmf_coeffs_even[12] =
255 {
256       3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
257 };
258 static int16_t qmf_coeffs_odd[12] =
259 {
260     -11,   53, -156,  362, -805, 3876, 951,  -210,   32,   12,  -11,    3
261 };
262 
g722_decode(g722_decode_state_t * s,int16_t amp[],const uint8_t g722_data[],int len,uint16_t gain)263 uint32_t g722_decode(g722_decode_state_t *s, int16_t amp[], const uint8_t g722_data[], int len, uint16_t gain)
264 {
265 
266     int dlowt;
267     int rlow;
268     int ihigh;
269     int dhigh;
270     int rhigh;
271     int xout1;
272     int xout2;
273     int wd1;
274     int wd2;
275     int wd3;
276     int code;
277     uint32_t outlen;
278     int i;
279     int j;
280 
281     outlen = 0;
282     rhigh = 0;
283 
284     for (j = 0;  j < len;  )
285     {
286 #if PACKED_INPUT == 1
287         /* Unpack the code bits */
288         if (s->in_bits < s->bits_per_sample)
289         {
290             s->in_buffer |= (g722_data[j++] << s->in_bits);
291             s->in_bits += 8;
292         }
293         code = s->in_buffer & ((1 << s->bits_per_sample) - 1);
294         s->in_buffer >>= s->bits_per_sample;
295         s->in_bits -= s->bits_per_sample;
296 #else
297         code = g722_data[j++];
298 #endif
299 
300 #if BITS_PER_SAMPLE == 8
301         wd1 = code & 0x3F;
302         ihigh = (code >> 6) & 0x03;
303         wd2 = qm6[wd1];
304         wd1 >>= 2;
305 #elif BITS_PER_SAMPLE == 7
306         wd1 = code & 0x1F;
307         ihigh = (code >> 5) & 0x03;
308         wd2 = qm5[wd1];
309         wd1 >>= 1;
310 #elif BITS_PER_SAMPLE == 6
311        wd1 = code & 0x0F;
312        ihigh = (code >> 4) & 0x03;
313        wd2 = qm4[wd1];
314 #endif
315         /* Block 5L, LOW BAND INVQBL */
316         wd2 = (s->band[0].det*wd2) >> 15;
317         /* Block 5L, RECONS */
318         rlow = s->band[0].s + wd2;
319         /* Block 6L, LIMIT */
320 
321         // ANDREA
322         // rlow=ssat(rlow,2<<14)
323         if (rlow > 16383)
324         {
325             rlow = 16383;
326         }
327         else if (rlow < -16384)
328         {
329             rlow = -16384;
330         }
331 
332         /* Block 2L, INVQAL */
333         wd2 = qm4[wd1];
334         dlowt = (s->band[0].det*wd2) >> 15;
335 
336         /* Block 3L, LOGSCL */
337         wd2 = rl42[wd1];
338         wd1 = (s->band[0].nb*127) >> 7;
339         wd1 += wl[wd2];
340         if (wd1 < 0)
341         {
342             wd1 = 0;
343         }
344         else if (wd1 > 18432)
345         {
346             wd1 = 18432;
347         }
348         s->band[0].nb = wd1;
349 
350         /* Block 3L, SCALEL */
351         wd1 = (s->band[0].nb >> 6) & 31;
352         wd2 = 8 - (s->band[0].nb >> 11);
353         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
354         s->band[0].det = wd3 << 2;
355 
356         block4(&s->band[0], dlowt);
357 
358         /* Block 2H, INVQAH */
359         wd2 = qm2[ihigh];
360         dhigh = (s->band[1].det*wd2) >> 15;
361         /* Block 5H, RECONS */
362         rhigh = dhigh + s->band[1].s;
363         /* Block 6H, LIMIT */
364 
365         // ANDREA
366         // rhigh=ssat(rhigh,2<<14)
367 
368         if (rhigh > 16383)
369             rhigh = 16383;
370         else if (rhigh < -16384)
371             rhigh = -16384;
372 
373         /* Block 2H, INVQAH */
374         wd2 = rh2[ihigh];
375         wd1 = (s->band[1].nb*127) >> 7;
376         wd1 += wh[wd2];
377         if (wd1 < 0)
378             wd1 = 0;
379         else if (wd1 > 22528)
380             wd1 = 22528;
381         s->band[1].nb = wd1;
382 
383         /* Block 3H, SCALEH */
384         wd1 = (s->band[1].nb >> 6) & 31;
385         wd2 = 10 - (s->band[1].nb >> 11);
386         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
387         s->band[1].det = wd3 << 2;
388 
389         block4(&s->band[1], dhigh);
390 
391         /* Apply the receive QMF */
392         for (i = 0;  i < 22;  i++)
393             s->x[i] = s->x[i + 2];
394         s->x[22] = rlow + rhigh;
395         s->x[23] = rlow - rhigh;
396 
397         // we should get PERF numbers for the following loop
398         xout1 = 0;
399         xout2 = 0;
400         for (i = 0;  i < 12;  i++)
401         {
402             xout2 += s->x[2*i]   * qmf_coeffs_even[i];
403             xout1 += s->x[2*i+1] * qmf_coeffs_odd[i];
404         }
405         xout1 = NLDECOMPRESS_PREPROCESS_SAMPLE_WITH_GAIN((int16_t) __ssat16(xout1 >> 11), gain);
406         xout2 = NLDECOMPRESS_PREPROCESS_SAMPLE_WITH_GAIN((int16_t) __ssat16(xout2 >> 11), gain);
407         if (s->dac_pcm)
408         {
409             amp[outlen++] = ((int16_t) (xout1 >> 4) + 2048);
410             amp[outlen++] = ((int16_t) (xout2 >> 4) + 2048);
411         }
412         else
413         {
414             amp[outlen++] = xout1;
415             amp[outlen++] = xout2;
416         }
417     }
418     return outlen;
419 }
420 /*- End of function --------------------------------------------------------*/
421 /*- End of file ------------------------------------------------------------*/
422