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