1 /*
2  * SpanDSP - a series of DSP components for telephony
3  *
4  * g722_encode.c - The ITU G.722 codec, encode part.
5  *
6  * Written by Steve Underwood <steveu@coppice.org>
7  *
8  * Copyright (C) 2005 Steve Underwood
9  *
10  * All rights reserved.
11  *
12  *  Despite my general liking of the GPL, I place my own contributions
13  *  to this code in the public domain for the benefit of all mankind -
14  *  even the slimy ones who might try to proprietize my work and use it
15  *  to my detriment.
16  *
17  * Based on a single channel 64kbps only G.722 codec which is:
18  *
19  *****    Copyright (c) CMU    1993      *****
20  * Computer Science, Speech Group
21  * Chengxiang Lu and Alex Hauptmann
22  *
23  * $Id: g722_encode.c,v 1.14 2006/07/07 16:37:49 steveu Exp $
24  */
25 
26 /*! \file */
27 
28 #include <stdio.h>
29 #include <string.h>
30 #include <stdlib.h>
31 
32 #include "g722_typedefs.h"
33 #include "g722_enc_dec.h"
34 
35 #if !defined(FALSE)
36 #define FALSE 0
37 #endif
38 #if !defined(TRUE)
39 #define TRUE (!FALSE)
40 #endif
41 
42 #define PACKED_OUTPUT   (0)
43 #define BITS_PER_SAMPLE (8)
44 
45 #ifndef BUILD_FEATURE_G722_USE_INTRINSIC_SAT
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 > 0x7FFF)
55         return  0x7FFF;
56     return  0x8000;
57 }
58 #else
saturate(int32_t val)59 static __inline int16_t saturate(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 /*- End of function --------------------------------------------------------*/
71 
block4(g722_band_t * band,int d)72 static void block4(g722_band_t *band, int d)
73 {
74     int wd1;
75     int wd2;
76     int wd3;
77     int i;
78     int sg[7];
79     int ap1, ap2;
80     int sg0, sgi;
81     int sz;
82 
83     /* Block 4, RECONS */
84     band->d[0] = d;
85     band->r[0] = saturate(band->s + d);
86 
87     /* Block 4, PARREC */
88     band->p[0] = saturate(band->sz + d);
89 
90     /* Block 4, UPPOL2 */
91     for (i = 0;  i < 3;  i++)
92         sg[i] = band->p[i] >> 15;
93     wd1 = saturate(band->a[1] << 2);
94 
95     wd2 = (sg[0] == sg[1])  ?  -wd1  :  wd1;
96     if (wd2 > 32767)
97         wd2 = 32767;
98 
99     ap2 = (wd2 >> 7) + ((sg[0] == sg[2])  ?  128  :  -128);
100     ap2 += (band->a[2]*32512) >> 15;
101     if (ap2 > 12288)
102         ap2 = 12288;
103     else if (ap2 < -12288)
104         ap2 = -12288;
105     band->ap[2] = ap2;
106 
107     /* Block 4, UPPOL1 */
108     sg[0] = band->p[0] >> 15;
109     sg[1] = band->p[1] >> 15;
110     wd1 = (sg[0] == sg[1])  ?  192  :  -192;
111     wd2 = (band->a[1]*32640) >> 15;
112 
113     ap1 = saturate(wd1 + wd2);
114     wd3 = saturate(15360 - band->ap[2]);
115     if (ap1 > wd3)
116         ap1 = wd3;
117     else if (ap1 < -wd3)
118         ap1 = -wd3;
119     band->ap[1] = ap1;
120 
121     /* Block 4, UPZERO */
122     /* Block 4, FILTEZ */
123     wd1 = (d == 0)  ?  0  :  128;
124 
125     sg0 = sg[0] = d >> 15;
126     for (i = 1;  i < 7;  i++)
127     {
128 	sgi = band->d[i] >> 15;
129 	wd2 = (sgi == sg0) ? wd1 : -wd1;
130         wd3 = (band->b[i]*32640) >> 15;
131         band->bp[i] = saturate(wd2 + wd3);
132     }
133 
134     /* Block 4, DELAYA */
135     sz = 0;
136     for (i = 6;  i > 0;  i--)
137     {
138 	int bi;
139 
140         band->d[i] = band->d[i - 1];
141         bi = band->b[i] = band->bp[i];
142         wd1 = saturate(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     {
149         band->r[i] = band->r[i - 1];
150         band->p[i] = band->p[i - 1];
151         band->a[i] = band->ap[i];
152     }
153 
154     /* Block 4, FILTEP */
155     wd1 = saturate(band->r[1] + band->r[1]);
156     wd1 = (band->a[1]*wd1) >> 15;
157     wd2 = saturate(band->r[2] + band->r[2]);
158     wd2 = (band->a[2]*wd2) >> 15;
159     band->sp = saturate(wd1 + wd2);
160 
161     /* Block 4, PREDIC */
162     band->s = saturate(band->sp + band->sz);
163 }
164 /*- End of function --------------------------------------------------------*/
165 
g722_encode_init(g722_encode_state_t * s,unsigned int rate,int options)166 g722_encode_state_t *g722_encode_init(g722_encode_state_t *s,
167                                              unsigned int rate, int options)
168 {
169     if (s == NULL)
170     {
171 #ifdef G722_SUPPORT_MALLOC
172         if ((s = (g722_encode_state_t *) malloc(sizeof(*s))) == NULL)
173 #endif
174             return NULL;
175     }
176     memset(s, 0, sizeof(*s));
177     if (rate == 48000)
178         s->bits_per_sample = 6;
179     else if (rate == 56000)
180         s->bits_per_sample = 7;
181     else
182         s->bits_per_sample = 8;
183     s->band[0].det = 32;
184     s->band[1].det = 8;
185     return s;
186 }
187 /*- End of function --------------------------------------------------------*/
188 
g722_encode_release(g722_encode_state_t * s)189 int g722_encode_release(g722_encode_state_t *s)
190 {
191     free(s);
192     return 0;
193 }
194 /*- End of function --------------------------------------------------------*/
195 
196 /* WebRtc, tlegrand:
197  * Only define the following if bit-exactness with reference implementation
198  * is needed. Will only have any effect if input signal is saturated.
199  */
200 //#define RUN_LIKE_REFERENCE_G722
201 #ifdef RUN_LIKE_REFERENCE_G722
limitValues(int16_t rl)202 int16_t limitValues (int16_t rl)
203 {
204 
205     int16_t yl;
206 
207     yl = (rl > 16383) ? 16383 : ((rl < -16384) ? -16384 : rl);
208 
209     return (yl);
210 }
211 /*- End of function --------------------------------------------------------*/
212 #endif
213 
214 static int16_t q6[32] =
215 {
216        0,   35,   72,  110,  150,  190,  233,  276,
217      323,  370,  422,  473,  530,  587,  650,  714,
218      786,  858,  940, 1023, 1121, 1219, 1339, 1458,
219     1612, 1765, 1980, 2195, 2557, 2919,    0,    0
220 };
221 static int16_t iln[32] =
222 {
223      0, 63, 62, 31, 30, 29, 28, 27,
224     26, 25, 24, 23, 22, 21, 20, 19,
225     18, 17, 16, 15, 14, 13, 12, 11,
226     10,  9,  8,  7,  6,  5,  4,  0
227 };
228 static int16_t ilp[32] =
229 {
230      0, 61, 60, 59, 58, 57, 56, 55,
231     54, 53, 52, 51, 50, 49, 48, 47,
232     46, 45, 44, 43, 42, 41, 40, 39,
233     38, 37, 36, 35, 34, 33, 32,  0
234 };
235 static int16_t wl[8] =
236 {
237     -60, -30, 58, 172, 334, 538, 1198, 3042
238 };
239 static int16_t rl42[16] =
240 {
241     0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
242 };
243 static int16_t ilb[32] =
244 {
245     2048, 2093, 2139, 2186, 2233, 2282, 2332,
246     2383, 2435, 2489, 2543, 2599, 2656, 2714,
247     2774, 2834, 2896, 2960, 3025, 3091, 3158,
248     3228, 3298, 3371, 3444, 3520, 3597, 3676,
249     3756, 3838, 3922, 4008
250 };
251 static int16_t qm4[16] =
252 {
253          0, -20456, -12896, -8968,
254      -6288,  -4240,  -2584, -1200,
255      20456,  12896,   8968,  6288,
256       4240,   2584,   1200,     0
257 };
258 static int16_t qm2[4] =
259 {
260     -7408,  -1616,   7408,   1616
261 };
262 static int16_t qmf_coeffs[12] =
263 {
264        3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
265 };
266 static int16_t ihn[3] = {0, 1, 0};
267 static int16_t ihp[3] = {0, 3, 2};
268 static int16_t wh[3] = {0, -214, 798};
269 static int16_t rh2[4] = {2, 1, 2, 1};
270 
g722_encode(g722_encode_state_t * s,uint8_t g722_data[],const int16_t amp[],int len)271 int g722_encode(g722_encode_state_t *s, uint8_t g722_data[],
272                        const int16_t amp[], int len)
273 {
274     int dlow;
275     int dhigh;
276     int el;
277     int wd;
278     int wd1;
279     int ril;
280     int wd2;
281     int il4;
282     int ih2;
283     int wd3;
284     int eh;
285     int mih;
286     int i;
287     int j;
288     /* Low and high band PCM from the QMF */
289     int xlow;
290     int xhigh;
291     int g722_bytes;
292     /* Even and odd tap accumulators */
293     int sumeven;
294     int sumodd;
295     int ihigh;
296     int ilow;
297     int code;
298 
299     g722_bytes = 0;
300     xhigh = 0;
301     for (j = 0;  j < len;  )
302     {
303         if (s->itu_test_mode)
304         {
305             xlow =
306             xhigh = amp[j++] >> 1;
307         }
308         else
309         {
310             {
311                 /* Apply the transmit QMF */
312                 /* Shuffle the buffer down */
313                 for (i = 0;  i < 22;  i++)
314                     s->x[i] = s->x[i + 2];
315                 //TODO: if len is odd, then this can be a buffer overrun
316                 s->x[22] = amp[j++];
317                 s->x[23] = amp[j++];
318 
319                 /* Discard every other QMF output */
320                 sumeven = 0;
321                 sumodd = 0;
322                 for (i = 0;  i < 12;  i++)
323                 {
324                     sumodd += s->x[2*i]*qmf_coeffs[i];
325                     sumeven += s->x[2*i + 1]*qmf_coeffs[11 - i];
326                 }
327                 /* We shift by 12 to allow for the QMF filters (DC gain = 4096), plus 1
328                    to allow for us summing two filters, plus 1 to allow for the 15 bit
329                    input to the G.722 algorithm. */
330                 xlow = (sumeven + sumodd) >> 14;
331                 xhigh = (sumeven - sumodd) >> 14;
332 
333 #ifdef RUN_LIKE_REFERENCE_G722
334                 /* The following lines are only used to verify bit-exactness
335                  * with reference implementation of G.722. Higher precision
336                  * is achieved without limiting the values.
337                  */
338                 xlow = limitValues(xlow);
339                 xhigh = limitValues(xhigh);
340 #endif
341             }
342         }
343         /* Block 1L, SUBTRA */
344         el = saturate(xlow - s->band[0].s);
345 
346         /* Block 1L, QUANTL */
347         wd = (el >= 0)  ?  el  :  -(el + 1);
348 
349         for (i = 1;  i < 30;  i++)
350         {
351             wd1 = (q6[i]*s->band[0].det) >> 12;
352             if (wd < wd1)
353                 break;
354         }
355         ilow = (el < 0)  ?  iln[i]  :  ilp[i];
356 
357         /* Block 2L, INVQAL */
358         ril = ilow >> 2;
359         wd2 = qm4[ril];
360         dlow = (s->band[0].det*wd2) >> 15;
361 
362         /* Block 3L, LOGSCL */
363         il4 = rl42[ril];
364         wd = (s->band[0].nb*127) >> 7;
365         s->band[0].nb = wd + wl[il4];
366         if (s->band[0].nb < 0)
367             s->band[0].nb = 0;
368         else if (s->band[0].nb > 18432)
369             s->band[0].nb = 18432;
370 
371         /* Block 3L, SCALEL */
372         wd1 = (s->band[0].nb >> 6) & 31;
373         wd2 = 8 - (s->band[0].nb >> 11);
374         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
375         s->band[0].det = wd3 << 2;
376 
377         block4(&s->band[0], dlow);
378         {
379 	    int nb;
380 
381             /* Block 1H, SUBTRA */
382             eh = saturate(xhigh - s->band[1].s);
383 
384             /* Block 1H, QUANTH */
385             wd = (eh >= 0)  ?  eh  :  -(eh + 1);
386             wd1 = (564*s->band[1].det) >> 12;
387             mih = (wd >= wd1)  ?  2  :  1;
388             ihigh = (eh < 0)  ?  ihn[mih]  :  ihp[mih];
389 
390             /* Block 2H, INVQAH */
391             wd2 = qm2[ihigh];
392             dhigh = (s->band[1].det*wd2) >> 15;
393 
394             /* Block 3H, LOGSCH */
395             ih2 = rh2[ihigh];
396             wd = (s->band[1].nb*127) >> 7;
397 
398             nb = wd + wh[ih2];
399             if (nb < 0)
400                 nb = 0;
401             else if (nb > 22528)
402                 nb = 22528;
403 	    s->band[1].nb = nb;
404 
405             /* Block 3H, SCALEH */
406             wd1 = (s->band[1].nb >> 6) & 31;
407             wd2 = 10 - (s->band[1].nb >> 11);
408             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
409             s->band[1].det = wd3 << 2;
410 
411             block4(&s->band[1], dhigh);
412 #if   BITS_PER_SAMPLE == 8
413             code = ((ihigh << 6) | ilow);
414 #elif BITS_PER_SAMPLE == 7
415             code = ((ihigh << 6) | ilow) >> 1;
416 #elif BITS_PER_SAMPLE == 6
417             code = ((ihigh << 6) | ilow) >> 2;
418 #endif
419         }
420 
421 #if PACKED_OUTPUT == 1
422             /* Pack the code bits */
423             s->out_buffer |= (code << s->out_bits);
424             s->out_bits += s->bits_per_sample;
425             if (s->out_bits >= 8)
426             {
427                 g722_data[g722_bytes++] = (uint8_t) (s->out_buffer & 0xFF);
428                 s->out_bits -= 8;
429                 s->out_buffer >>= 8;
430             }
431 #else
432             g722_data[g722_bytes++] = (uint8_t) code;
433 #endif
434     }
435     return g722_bytes;
436 }
437 /*- End of function --------------------------------------------------------*/
438 /*- End of file ------------------------------------------------------------*/
439