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