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