The trunk version of this patch also includes a couple more small clean fixes
[asterisk/asterisk.git] / codecs / g722 / g722_encode.c
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$
24  */
25
26 /*! \file */
27
28 #ifdef HAVE_CONFIG_H
29 #include <config.h>
30 #endif
31
32 #include <stdio.h>
33 #include <inttypes.h>
34 #include <memory.h>
35 #include <stdlib.h>
36 #if 0
37 #include <tgmath.h>
38 #endif
39
40 #include "g722.h"
41
42 #if !defined(FALSE)
43 #define FALSE 0
44 #endif
45 #if !defined(TRUE)
46 #define TRUE (!FALSE)
47 #endif
48
49 static __inline__ int16_t saturate(int32_t amp)
50 {
51     int16_t amp16;
52
53     /* Hopefully this is optimised for the common case - not clipping */
54     amp16 = (int16_t) amp;
55     if (amp == amp16)
56         return amp16;
57     if (amp > INT16_MAX)
58         return  INT16_MAX;
59     return  INT16_MIN;
60 }
61 /*- End of function --------------------------------------------------------*/
62
63 static void block4(g722_encode_state_t *s, int band, int d)
64 {
65     int wd1;
66     int wd2;
67     int wd3;
68     int i;
69
70     /* Block 4, RECONS */
71     s->band[band].d[0] = d;
72     s->band[band].r[0] = saturate(s->band[band].s + d);
73
74     /* Block 4, PARREC */
75     s->band[band].p[0] = saturate(s->band[band].sz + d);
76
77     /* Block 4, UPPOL2 */
78     for (i = 0;  i < 3;  i++)
79         s->band[band].sg[i] = s->band[band].p[i] >> 15;
80     wd1 = saturate(s->band[band].a[1] << 2);
81
82     wd2 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  -wd1  :  wd1;
83     if (wd2 > 32767)
84         wd2 = 32767;
85     wd3 = (wd2 >> 7) + ((s->band[band].sg[0] == s->band[band].sg[2])  ?  128  :  -128);
86     wd3 += (s->band[band].a[2]*32512) >> 15;
87     if (wd3 > 12288)
88         wd3 = 12288;
89     else if (wd3 < -12288)
90         wd3 = -12288;
91     s->band[band].ap[2] = wd3;
92
93     /* Block 4, UPPOL1 */
94     s->band[band].sg[0] = s->band[band].p[0] >> 15;
95     s->band[band].sg[1] = s->band[band].p[1] >> 15;
96     wd1 = (s->band[band].sg[0] == s->band[band].sg[1])  ?  192  :  -192;
97     wd2 = (s->band[band].a[1]*32640) >> 15;
98
99     s->band[band].ap[1] = saturate(wd1 + wd2);
100     wd3 = saturate(15360 - s->band[band].ap[2]);
101     if (s->band[band].ap[1] > wd3)
102         s->band[band].ap[1] = wd3;
103     else if (s->band[band].ap[1] < -wd3)
104         s->band[band].ap[1] = -wd3;
105
106     /* Block 4, UPZERO */
107     wd1 = (d == 0)  ?  0  :  128;
108     s->band[band].sg[0] = d >> 15;
109     for (i = 1;  i < 7;  i++)
110     {
111         s->band[band].sg[i] = s->band[band].d[i] >> 15;
112         wd2 = (s->band[band].sg[i] == s->band[band].sg[0])  ?  wd1  :  -wd1;
113         wd3 = (s->band[band].b[i]*32640) >> 15;
114         s->band[band].bp[i] = saturate(wd2 + wd3);
115     }
116
117     /* Block 4, DELAYA */
118     for (i = 6;  i > 0;  i--)
119     {
120         s->band[band].d[i] = s->band[band].d[i - 1];
121         s->band[band].b[i] = s->band[band].bp[i];
122     }
123     
124     for (i = 2;  i > 0;  i--)
125     {
126         s->band[band].r[i] = s->band[band].r[i - 1];
127         s->band[band].p[i] = s->band[band].p[i - 1];
128         s->band[band].a[i] = s->band[band].ap[i];
129     }
130
131     /* Block 4, FILTEP */
132     wd1 = saturate(s->band[band].r[1] + s->band[band].r[1]);
133     wd1 = (s->band[band].a[1]*wd1) >> 15;
134     wd2 = saturate(s->band[band].r[2] + s->band[band].r[2]);
135     wd2 = (s->band[band].a[2]*wd2) >> 15;
136     s->band[band].sp = saturate(wd1 + wd2);
137
138     /* Block 4, FILTEZ */
139     s->band[band].sz = 0;
140     for (i = 6;  i > 0;  i--)
141     {
142         wd1 = saturate(s->band[band].d[i] + s->band[band].d[i]);
143         s->band[band].sz += (s->band[band].b[i]*wd1) >> 15;
144     }
145     s->band[band].sz = saturate(s->band[band].sz);
146
147     /* Block 4, PREDIC */
148     s->band[band].s = saturate(s->band[band].sp + s->band[band].sz);
149 }
150 /*- End of function --------------------------------------------------------*/
151
152 g722_encode_state_t *g722_encode_init(g722_encode_state_t *s, int rate, int options)
153 {
154     if (s == NULL)
155     {
156         if ((s = (g722_encode_state_t *) malloc(sizeof(*s))) == NULL)
157             return NULL;
158     }
159     memset(s, 0, sizeof(*s));
160     if (rate == 48000)
161         s->bits_per_sample = 6;
162     else if (rate == 56000)
163         s->bits_per_sample = 7;
164     else
165         s->bits_per_sample = 8;
166     if ((options & G722_SAMPLE_RATE_8000))
167         s->eight_k = TRUE;
168     if ((options & G722_PACKED)  &&  s->bits_per_sample != 8)
169         s->packed = TRUE;
170     else
171         s->packed = FALSE;
172     s->band[0].det = 32;
173     s->band[1].det = 8;
174     return s;
175 }
176 /*- End of function --------------------------------------------------------*/
177
178 int g722_encode_release(g722_encode_state_t *s)
179 {
180     free(s);
181     return 0;
182 }
183 /*- End of function --------------------------------------------------------*/
184
185 int g722_encode(g722_encode_state_t *s, uint8_t g722_data[], const int16_t amp[], int len)
186 {
187     static const int q6[32] =
188     {
189            0,   35,   72,  110,  150,  190,  233,  276,
190          323,  370,  422,  473,  530,  587,  650,  714,
191          786,  858,  940, 1023, 1121, 1219, 1339, 1458,
192         1612, 1765, 1980, 2195, 2557, 2919,    0,    0
193     };
194     static const int iln[32] =
195     {
196          0, 63, 62, 31, 30, 29, 28, 27,
197         26, 25, 24, 23, 22, 21, 20, 19,
198         18, 17, 16, 15, 14, 13, 12, 11,
199         10,  9,  8,  7,  6,  5,  4,  0
200     };
201     static const int ilp[32] =
202     {
203          0, 61, 60, 59, 58, 57, 56, 55,
204         54, 53, 52, 51, 50, 49, 48, 47,
205         46, 45, 44, 43, 42, 41, 40, 39,
206         38, 37, 36, 35, 34, 33, 32,  0
207     };
208     static const int wl[8] =
209     {
210         -60, -30, 58, 172, 334, 538, 1198, 3042
211     };
212     static const int rl42[16] =
213     {
214         0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
215     };
216     static const int ilb[32] =
217     {
218         2048, 2093, 2139, 2186, 2233, 2282, 2332,
219         2383, 2435, 2489, 2543, 2599, 2656, 2714,
220         2774, 2834, 2896, 2960, 3025, 3091, 3158,
221         3228, 3298, 3371, 3444, 3520, 3597, 3676,
222         3756, 3838, 3922, 4008
223     };
224     static const int qm4[16] =
225     {
226              0, -20456, -12896, -8968,
227          -6288,  -4240,  -2584, -1200,
228          20456,  12896,   8968,  6288,
229           4240,   2584,   1200,     0
230     };
231     static const int qm2[4] =
232     {
233         -7408,  -1616,   7408,   1616
234     };
235     static const int qmf_coeffs[12] =
236     {
237            3,  -11,   12,   32, -210,  951, 3876, -805,  362, -156,   53,  -11,
238     };
239     static const int ihn[3] = {0, 1, 0};
240     static const int ihp[3] = {0, 3, 2};
241     static const int wh[3] = {0, -214, 798};
242     static const int rh2[4] = {2, 1, 2, 1};
243
244     int dlow;
245     int dhigh;
246     int el;
247     int wd;
248     int wd1;
249     int ril;
250     int wd2;
251     int il4;
252     int ih2;
253     int wd3;
254     int eh;
255     int mih;
256     int i;
257     int j;
258     /* Low and high band PCM from the QMF */
259     int xlow;
260     int xhigh;
261     int g722_bytes;
262     /* Even and odd tap accumulators */
263     int sumeven;
264     int sumodd;
265     int ihigh;
266     int ilow;
267     int code;
268
269     g722_bytes = 0;
270     xhigh = 0;
271     for (j = 0;  j < len;  )
272     {
273         if (s->itu_test_mode)
274         {
275             xlow =
276             xhigh = amp[j++] >> 1;
277         }
278         else
279         {
280             if (s->eight_k)
281             {
282                 xlow = amp[j++];
283             }
284             else
285             {
286                 /* Apply the transmit QMF */
287                 /* Shuffle the buffer down */
288                 for (i = 0;  i < 22;  i++)
289                     s->x[i] = s->x[i + 2];
290                 s->x[22] = amp[j++];
291                 s->x[23] = amp[j++];
292     
293                 /* Discard every other QMF output */
294                 sumeven = 0;
295                 sumodd = 0;
296                 for (i = 0;  i < 12;  i++)
297                 {
298                     sumodd += s->x[2*i]*qmf_coeffs[i];
299                     sumeven += s->x[2*i + 1]*qmf_coeffs[11 - i];
300                 }
301                 xlow = (sumeven + sumodd) >> 13;
302                 xhigh = (sumeven - sumodd) >> 13;
303             }
304         }
305         /* Block 1L, SUBTRA */
306         el = saturate(xlow - s->band[0].s);
307
308         /* Block 1L, QUANTL */
309         wd = (el >= 0)  ?  el  :  -(el + 1);
310
311         for (i = 1;  i < 30;  i++)
312         {
313             wd1 = (q6[i]*s->band[0].det) >> 12;
314             if (wd < wd1)
315                 break;
316         }
317         ilow = (el < 0)  ?  iln[i]  :  ilp[i];
318
319         /* Block 2L, INVQAL */
320         ril = ilow >> 2;
321         wd2 = qm4[ril];
322         dlow = (s->band[0].det*wd2) >> 15;
323
324         /* Block 3L, LOGSCL */
325         il4 = rl42[ril];
326         wd = (s->band[0].nb*127) >> 7;
327         s->band[0].nb = wd + wl[il4];
328         if (s->band[0].nb < 0)
329             s->band[0].nb = 0;
330         else if (s->band[0].nb > 18432)
331             s->band[0].nb = 18432;
332
333         /* Block 3L, SCALEL */
334         wd1 = (s->band[0].nb >> 6) & 31;
335         wd2 = 8 - (s->band[0].nb >> 11);
336         wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
337         s->band[0].det = wd3 << 2;
338
339         block4(s, 0, dlow);
340         
341         if (s->eight_k)
342         {
343             /* Just leave the high bits as zero */
344             code = (0xC0 | ilow) >> (8 - s->bits_per_sample);
345         }
346         else
347         {
348             /* Block 1H, SUBTRA */
349             eh = saturate(xhigh - s->band[1].s);
350
351             /* Block 1H, QUANTH */
352             wd = (eh >= 0)  ?  eh  :  -(eh + 1);
353             wd1 = (564*s->band[1].det) >> 12;
354             mih = (wd >= wd1)  ?  2  :  1;
355             ihigh = (eh < 0)  ?  ihn[mih]  :  ihp[mih];
356
357             /* Block 2H, INVQAH */
358             wd2 = qm2[ihigh];
359             dhigh = (s->band[1].det*wd2) >> 15;
360
361             /* Block 3H, LOGSCH */
362             ih2 = rh2[ihigh];
363             wd = (s->band[1].nb*127) >> 7;
364             s->band[1].nb = wd + wh[ih2];
365             if (s->band[1].nb < 0)
366                 s->band[1].nb = 0;
367             else if (s->band[1].nb > 22528)
368                 s->band[1].nb = 22528;
369
370             /* Block 3H, SCALEH */
371             wd1 = (s->band[1].nb >> 6) & 31;
372             wd2 = 10 - (s->band[1].nb >> 11);
373             wd3 = (wd2 < 0)  ?  (ilb[wd1] << -wd2)  :  (ilb[wd1] >> wd2);
374             s->band[1].det = wd3 << 2;
375
376             block4(s, 1, dhigh);
377             code = ((ihigh << 6) | ilow) >> (8 - s->bits_per_sample);
378         }
379
380         if (s->packed)
381         {
382             /* Pack the code bits */
383             s->out_buffer |= (code << s->out_bits);
384             s->out_bits += s->bits_per_sample;
385             if (s->out_bits >= 8)
386             {
387                 g722_data[g722_bytes++] = (uint8_t) (s->out_buffer & 0xFF);
388                 s->out_bits -= 8;
389                 s->out_buffer >>= 8;
390             }
391         }
392         else
393         {
394             g722_data[g722_bytes++] = (uint8_t) code;
395         }
396     }
397     return g722_bytes;
398 }
399 /*- End of function --------------------------------------------------------*/
400 /*- End of file ------------------------------------------------------------*/