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