various minor portability fixes (mostly from tholo for OpenBSD)
[asterisk/asterisk.git] / codecs / gsm / src / gsm_explode.c
1 /*
2  * Copyright 1992 by Jutta Degener and Carsten Bormann, Technische
3  * Universitaet Berlin.  See the accompanying file "COPYRIGHT" for
4  * details.  THERE IS ABSOLUTELY NO WARRANTY FOR THIS SOFTWARE.
5  */
6
7 /* $Header$ */
8
9 #include "private.h"
10 #include "gsm.h"
11 #include "proto.h"
12
13 int gsm_explode P3((s, c, target), gsm s, gsm_byte * c, gsm_signal * target)
14 {
15 #       define  LARc    target
16 #       define  Nc      *((gsm_signal (*) [17])(target + 8))
17 #       define  bc      *((gsm_signal (*) [17])(target + 9))
18 #       define  Mc      *((gsm_signal (*) [17])(target + 10))
19 #       define  xmaxc   *((gsm_signal (*) [17])(target + 11))
20
21
22 #ifdef WAV49
23         if (s->wav_fmt) {
24
25                 uword sr = 0;
26
27                 if (s->frame_index == 1) {
28
29                         sr = *c++;
30                         LARc[0] = sr & 0x3f;  sr >>= 6;
31                         sr |= (uword)*c++ << 2;
32                         LARc[1] = sr & 0x3f;  sr >>= 6;
33                         sr |= (uword)*c++ << 4;
34                         LARc[2] = sr & 0x1f;  sr >>= 5;
35                         LARc[3] = sr & 0x1f;  sr >>= 5;
36                         sr |= (uword)*c++ << 2;
37                         LARc[4] = sr & 0xf;  sr >>= 4;
38                         LARc[5] = sr & 0xf;  sr >>= 4;
39                         sr |= (uword)*c++ << 2;                 /* 5 */
40                         LARc[6] = sr & 0x7;  sr >>= 3;
41                         LARc[7] = sr & 0x7;  sr >>= 3;
42                         sr |= (uword)*c++ << 4;
43                         Nc[0] = sr & 0x7f;  sr >>= 7;
44                         bc[0] = sr & 0x3;  sr >>= 2;
45                         Mc[0] = sr & 0x3;  sr >>= 2;
46                         sr |= (uword)*c++ << 1;
47                         xmaxc[0] = sr & 0x3f;  sr >>= 6;
48 #undef  xmc
49 #define xmc     (target + 12)
50                         xmc[0] = sr & 0x7;  sr >>= 3;
51                         sr = *c++;
52                         xmc[1] = sr & 0x7;  sr >>= 3;
53                         xmc[2] = sr & 0x7;  sr >>= 3;
54                         sr |= (uword)*c++ << 2;
55                         xmc[3] = sr & 0x7;  sr >>= 3;
56                         xmc[4] = sr & 0x7;  sr >>= 3;
57                         xmc[5] = sr & 0x7;  sr >>= 3;
58                         sr |= (uword)*c++ << 1;                 /* 10 */
59                         xmc[6] = sr & 0x7;  sr >>= 3;
60                         xmc[7] = sr & 0x7;  sr >>= 3;
61                         xmc[8] = sr & 0x7;  sr >>= 3;
62                         sr = *c++;
63                         xmc[9] = sr & 0x7;  sr >>= 3;
64                         xmc[10] = sr & 0x7;  sr >>= 3;
65                         sr |= (uword)*c++ << 2;
66                         xmc[11] = sr & 0x7;  sr >>= 3;
67                         xmc[12] = sr & 0x7;  sr >>= 3;
68                         sr |= (uword)*c++ << 4;
69                         Nc[1] = sr & 0x7f;  sr >>= 7;
70                         bc[1] = sr & 0x3;  sr >>= 2;
71                         Mc[1] = sr & 0x3;  sr >>= 2;
72                         sr |= (uword)*c++ << 1;
73                         xmaxc[1] = sr & 0x3f;  sr >>= 6;
74 #undef  xmc
75 #define xmc     (target + 29 - 13)
76
77                         xmc[13] = sr & 0x7;  sr >>= 3;
78                         sr = *c++;                              /* 15 */
79                         xmc[14] = sr & 0x7;  sr >>= 3;
80                         xmc[15] = sr & 0x7;  sr >>= 3;
81                         sr |= (uword)*c++ << 2;
82                         xmc[16] = sr & 0x7;  sr >>= 3;
83                         xmc[17] = sr & 0x7;  sr >>= 3;
84                         xmc[18] = sr & 0x7;  sr >>= 3;
85                         sr |= (uword)*c++ << 1;
86                         xmc[19] = sr & 0x7;  sr >>= 3;
87                         xmc[20] = sr & 0x7;  sr >>= 3;
88                         xmc[21] = sr & 0x7;  sr >>= 3;
89                         sr = *c++;
90                         xmc[22] = sr & 0x7;  sr >>= 3;
91                         xmc[23] = sr & 0x7;  sr >>= 3;
92                         sr |= (uword)*c++ << 2;
93                         xmc[24] = sr & 0x7;  sr >>= 3;
94                         xmc[25] = sr & 0x7;  sr >>= 3;
95                         sr |= (uword)*c++ << 4;                 /* 20 */
96                         Nc[2] = sr & 0x7f;  sr >>= 7;
97                         bc[2] = sr & 0x3;  sr >>= 2;
98                         Mc[2] = sr & 0x3;  sr >>= 2;
99                         sr |= (uword)*c++ << 1;
100                         xmaxc[2] = sr & 0x3f;  sr >>= 6;
101
102 #undef  xmc
103 #define xmc     (target + 46 - 26)
104
105                         xmc[26] = sr & 0x7;  sr >>= 3;
106                         sr = *c++;
107                         xmc[27] = sr & 0x7;  sr >>= 3;
108                         xmc[28] = sr & 0x7;  sr >>= 3;
109                         sr |= (uword)*c++ << 2;
110                         xmc[29] = sr & 0x7;  sr >>= 3;
111                         xmc[30] = sr & 0x7;  sr >>= 3;
112                         xmc[31] = sr & 0x7;  sr >>= 3;
113                         sr |= (uword)*c++ << 1;
114                         xmc[32] = sr & 0x7;  sr >>= 3;
115                         xmc[33] = sr & 0x7;  sr >>= 3;
116                         xmc[34] = sr & 0x7;  sr >>= 3;
117                         sr = *c++;                              /* 25 */
118                         xmc[35] = sr & 0x7;  sr >>= 3;
119                         xmc[36] = sr & 0x7;  sr >>= 3;
120                         sr |= (uword)*c++ << 2;
121                         xmc[37] = sr & 0x7;  sr >>= 3;
122                         xmc[38] = sr & 0x7;  sr >>= 3;
123                         sr |= (uword)*c++ << 4;
124                         Nc[3] = sr & 0x7f;  sr >>= 7;
125                         bc[3] = sr & 0x3;  sr >>= 2;
126                         Mc[3] = sr & 0x3;  sr >>= 2;
127                         sr |= (uword)*c++ << 1;
128                         xmaxc[3] = sr & 0x3f;  sr >>= 6;
129 #undef  xmc
130 #define xmc     (target + 63 - 39)
131
132                         xmc[39] = sr & 0x7;  sr >>= 3;
133                         sr = *c++;
134                         xmc[40] = sr & 0x7;  sr >>= 3;
135                         xmc[41] = sr & 0x7;  sr >>= 3;
136                         sr |= (uword)*c++ << 2;                 /* 30 */
137                         xmc[42] = sr & 0x7;  sr >>= 3;
138                         xmc[43] = sr & 0x7;  sr >>= 3;
139                         xmc[44] = sr & 0x7;  sr >>= 3;
140                         sr |= (uword)*c++ << 1;
141                         xmc[45] = sr & 0x7;  sr >>= 3;
142                         xmc[46] = sr & 0x7;  sr >>= 3;
143                         xmc[47] = sr & 0x7;  sr >>= 3;
144                         sr = *c++;
145                         xmc[48] = sr & 0x7;  sr >>= 3;
146                         xmc[49] = sr & 0x7;  sr >>= 3;
147                         sr |= (uword)*c++ << 2;
148                         xmc[50] = sr & 0x7;  sr >>= 3;
149                         xmc[51] = sr & 0x7;  sr >>= 3;
150
151                         s->frame_chain = sr & 0xf;
152                 }
153                 else {
154                         sr = s->frame_chain;
155                         sr |= (uword)*c++ << 4;                 /* 1 */
156                         LARc[0] = sr & 0x3f;  sr >>= 6;
157                         LARc[1] = sr & 0x3f;  sr >>= 6;
158                         sr = *c++;
159                         LARc[2] = sr & 0x1f;  sr >>= 5;
160                         sr |= (uword)*c++ << 3;
161                         LARc[3] = sr & 0x1f;  sr >>= 5;
162                         LARc[4] = sr & 0xf;  sr >>= 4;
163                         sr |= (uword)*c++ << 2;
164                         LARc[5] = sr & 0xf;  sr >>= 4;
165                         LARc[6] = sr & 0x7;  sr >>= 3;
166                         LARc[7] = sr & 0x7;  sr >>= 3;
167                         sr = *c++;                              /* 5 */
168                         Nc[0] = sr & 0x7f;  sr >>= 7;
169                         sr |= (uword)*c++ << 1;
170                         bc[0] = sr & 0x3;  sr >>= 2;
171                         Mc[0] = sr & 0x3;  sr >>= 2;
172                         sr |= (uword)*c++ << 5;
173                         xmaxc[0] = sr & 0x3f;  sr >>= 6;
174 #undef  xmc
175 #define xmc     (target + 12)
176                         xmc[0] = sr & 0x7;  sr >>= 3;
177                         xmc[1] = sr & 0x7;  sr >>= 3;
178                         sr |= (uword)*c++ << 1;
179                         xmc[2] = sr & 0x7;  sr >>= 3;
180                         xmc[3] = sr & 0x7;  sr >>= 3;
181                         xmc[4] = sr & 0x7;  sr >>= 3;
182                         sr = *c++;
183                         xmc[5] = sr & 0x7;  sr >>= 3;
184                         xmc[6] = sr & 0x7;  sr >>= 3;
185                         sr |= (uword)*c++ << 2;                 /* 10 */
186                         xmc[7] = sr & 0x7;  sr >>= 3;
187                         xmc[8] = sr & 0x7;  sr >>= 3;
188                         xmc[9] = sr & 0x7;  sr >>= 3;
189                         sr |= (uword)*c++ << 1;
190                         xmc[10] = sr & 0x7;  sr >>= 3;
191                         xmc[11] = sr & 0x7;  sr >>= 3;
192                         xmc[12] = sr & 0x7;  sr >>= 3;
193                         sr = *c++;
194                         Nc[1] = sr & 0x7f;  sr >>= 7;
195                         sr |= (uword)*c++ << 1;
196                         bc[1] = sr & 0x3;  sr >>= 2;
197                         Mc[1] = sr & 0x3;  sr >>= 2;
198                         sr |= (uword)*c++ << 5;
199                         xmaxc[1] = sr & 0x3f;  sr >>= 6;
200 #undef  xmc
201 #define xmc     (target + 29 - 13)
202
203                         xmc[13] = sr & 0x7;  sr >>= 3;
204                         xmc[14] = sr & 0x7;  sr >>= 3;
205                         sr |= (uword)*c++ << 1;                 /* 15 */
206                         xmc[15] = sr & 0x7;  sr >>= 3;
207                         xmc[16] = sr & 0x7;  sr >>= 3;
208                         xmc[17] = sr & 0x7;  sr >>= 3;
209                         sr = *c++;
210                         xmc[18] = sr & 0x7;  sr >>= 3;
211                         xmc[19] = sr & 0x7;  sr >>= 3;
212                         sr |= (uword)*c++ << 2;
213                         xmc[20] = sr & 0x7;  sr >>= 3;
214                         xmc[21] = sr & 0x7;  sr >>= 3;
215                         xmc[22] = sr & 0x7;  sr >>= 3;
216                         sr |= (uword)*c++ << 1;
217                         xmc[23] = sr & 0x7;  sr >>= 3;
218                         xmc[24] = sr & 0x7;  sr >>= 3;
219                         xmc[25] = sr & 0x7;  sr >>= 3;
220                         sr = *c++;
221                         Nc[2] = sr & 0x7f;  sr >>= 7;
222                         sr |= (uword)*c++ << 1;                 /* 20 */
223                         bc[2] = sr & 0x3;  sr >>= 2;
224                         Mc[2] = sr & 0x3;  sr >>= 2;
225                         sr |= (uword)*c++ << 5;
226                         xmaxc[2] = sr & 0x3f;  sr >>= 6;
227 #undef  xmc
228 #define xmc     (target + 46 - 26)
229                         xmc[26] = sr & 0x7;  sr >>= 3;
230                         xmc[27] = sr & 0x7;  sr >>= 3;
231                         sr |= (uword)*c++ << 1; 
232                         xmc[28] = sr & 0x7;  sr >>= 3;
233                         xmc[29] = sr & 0x7;  sr >>= 3;
234                         xmc[30] = sr & 0x7;  sr >>= 3;
235                         sr = *c++;
236                         xmc[31] = sr & 0x7;  sr >>= 3;
237                         xmc[32] = sr & 0x7;  sr >>= 3;
238                         sr |= (uword)*c++ << 2;
239                         xmc[33] = sr & 0x7;  sr >>= 3;
240                         xmc[34] = sr & 0x7;  sr >>= 3;
241                         xmc[35] = sr & 0x7;  sr >>= 3;
242                         sr |= (uword)*c++ << 1;                 /* 25 */
243                         xmc[36] = sr & 0x7;  sr >>= 3;
244                         xmc[37] = sr & 0x7;  sr >>= 3;
245                         xmc[38] = sr & 0x7;  sr >>= 3;
246                         sr = *c++;
247                         Nc[3] = sr & 0x7f;  sr >>= 7;
248                         sr |= (uword)*c++ << 1;         
249                         bc[3] = sr & 0x3;  sr >>= 2;
250                         Mc[3] = sr & 0x3;  sr >>= 2;
251                         sr |= (uword)*c++ << 5;
252                         xmaxc[3] = sr & 0x3f;  sr >>= 6;
253
254 #undef  xmc
255 #define xmc     (target + 63 - 39)
256
257                         xmc[39] = sr & 0x7;  sr >>= 3;
258                         xmc[40] = sr & 0x7;  sr >>= 3;
259                         sr |= (uword)*c++ << 1;
260                         xmc[41] = sr & 0x7;  sr >>= 3;
261                         xmc[42] = sr & 0x7;  sr >>= 3;
262                         xmc[43] = sr & 0x7;  sr >>= 3;
263                         sr = *c++;                              /* 30 */
264                         xmc[44] = sr & 0x7;  sr >>= 3;
265                         xmc[45] = sr & 0x7;  sr >>= 3;
266                         sr |= (uword)*c++ << 2;
267                         xmc[46] = sr & 0x7;  sr >>= 3;
268                         xmc[47] = sr & 0x7;  sr >>= 3;
269                         xmc[48] = sr & 0x7;  sr >>= 3;
270                         sr |= (uword)*c++ << 1;
271                         xmc[49] = sr & 0x7;  sr >>= 3;
272                         xmc[50] = sr & 0x7;  sr >>= 3;
273                         xmc[51] = sr & 0x7;  sr >>= 3;
274                 }
275         }
276         else 
277 #endif
278         {
279         /* GSM_MAGIC  = (*c >> 4) & 0xF; */
280
281         if (((*c >> 4) & 0x0F) != GSM_MAGIC) return -1;
282
283         LARc[0]  = (*c++ & 0xF) << 2;           /* 1 */
284         LARc[0] |= (*c >> 6) & 0x3;
285         LARc[1]  = *c++ & 0x3F;
286         LARc[2]  = (*c >> 3) & 0x1F;
287         LARc[3]  = (*c++ & 0x7) << 2;
288         LARc[3] |= (*c >> 6) & 0x3;
289         LARc[4]  = (*c >> 2) & 0xF;
290         LARc[5]  = (*c++ & 0x3) << 2;
291         LARc[5] |= (*c >> 6) & 0x3;
292         LARc[6]  = (*c >> 3) & 0x7;
293         LARc[7]  = *c++ & 0x7;
294
295         Nc[0]  = (*c >> 1) & 0x7F;
296
297         bc[0]  = (*c++ & 0x1) << 1;
298         bc[0] |= (*c >> 7) & 0x1;
299
300         Mc[0]  = (*c >> 5) & 0x3;
301
302         xmaxc[0]  = (*c++ & 0x1F) << 1;
303         xmaxc[0] |= (*c >> 7) & 0x1;
304
305 #undef  xmc
306 #define xmc     (target + 12)
307
308         xmc[0]  = (*c >> 4) & 0x7;
309         xmc[1]  = (*c >> 1) & 0x7;
310         xmc[2]  = (*c++ & 0x1) << 2;
311         xmc[2] |= (*c >> 6) & 0x3;
312         xmc[3]  = (*c >> 3) & 0x7;
313         xmc[4]  = *c++ & 0x7;
314         xmc[5]  = (*c >> 5) & 0x7;
315         xmc[6]  = (*c >> 2) & 0x7;
316         xmc[7]  = (*c++ & 0x3) << 1;            /* 10 */
317         xmc[7] |= (*c >> 7) & 0x1;
318         xmc[8]  = (*c >> 4) & 0x7;
319         xmc[9]  = (*c >> 1) & 0x7;
320         xmc[10]  = (*c++ & 0x1) << 2;
321         xmc[10] |= (*c >> 6) & 0x3;
322         xmc[11]  = (*c >> 3) & 0x7;
323         xmc[12]  = *c++ & 0x7;
324
325         Nc[1]  = (*c >> 1) & 0x7F;
326
327         bc[1]  = (*c++ & 0x1) << 1;
328         bc[1] |= (*c >> 7) & 0x1;
329
330         Mc[1]  = (*c >> 5) & 0x3;
331
332         xmaxc[1]  = (*c++ & 0x1F) << 1;
333         xmaxc[1] |= (*c >> 7) & 0x1;
334
335 #undef  xmc
336 #define xmc     (target + 29 - 13)
337
338         xmc[13]  = (*c >> 4) & 0x7;
339         xmc[14]  = (*c >> 1) & 0x7;
340         xmc[15]  = (*c++ & 0x1) << 2;
341         xmc[15] |= (*c >> 6) & 0x3;
342         xmc[16]  = (*c >> 3) & 0x7;
343         xmc[17]  = *c++ & 0x7;
344         xmc[18]  = (*c >> 5) & 0x7;
345         xmc[19]  = (*c >> 2) & 0x7;
346         xmc[20]  = (*c++ & 0x3) << 1;
347         xmc[20] |= (*c >> 7) & 0x1;
348         xmc[21]  = (*c >> 4) & 0x7;
349         xmc[22]  = (*c >> 1) & 0x7;
350         xmc[23]  = (*c++ & 0x1) << 2;
351         xmc[23] |= (*c >> 6) & 0x3;
352         xmc[24]  = (*c >> 3) & 0x7;
353         xmc[25]  = *c++ & 0x7;
354
355         Nc[2]  = (*c >> 1) & 0x7F;
356
357         bc[2]  = (*c++ & 0x1) << 1;             /* 20 */
358         bc[2] |= (*c >> 7) & 0x1;
359
360         Mc[2]  = (*c >> 5) & 0x3;
361
362         xmaxc[2]  = (*c++ & 0x1F) << 1;
363         xmaxc[2] |= (*c >> 7) & 0x1;
364
365 #undef  xmc
366 #define xmc     (target + 46 - 26)
367
368         xmc[26]  = (*c >> 4) & 0x7;
369         xmc[27]  = (*c >> 1) & 0x7;
370         xmc[28]  = (*c++ & 0x1) << 2;
371         xmc[28] |= (*c >> 6) & 0x3;
372         xmc[29]  = (*c >> 3) & 0x7;
373         xmc[30]  = *c++ & 0x7;
374         xmc[31]  = (*c >> 5) & 0x7;
375         xmc[32]  = (*c >> 2) & 0x7;
376         xmc[33]  = (*c++ & 0x3) << 1;
377         xmc[33] |= (*c >> 7) & 0x1;
378         xmc[34]  = (*c >> 4) & 0x7;
379         xmc[35]  = (*c >> 1) & 0x7;
380         xmc[36]  = (*c++ & 0x1) << 2;
381         xmc[36] |= (*c >> 6) & 0x3;
382         xmc[37]  = (*c >> 3) & 0x7;
383         xmc[38]  = *c++ & 0x7;
384
385         Nc[3]  = (*c >> 1) & 0x7F;
386
387         bc[3]  = (*c++ & 0x1) << 1;
388         bc[3] |= (*c >> 7) & 0x1;
389
390         Mc[3]  = (*c >> 5) & 0x3;
391
392         xmaxc[3]  = (*c++ & 0x1F) << 1;
393         xmaxc[3] |= (*c >> 7) & 0x1;
394
395 #undef  xmc
396 #define xmc     (target + 63 - 39)
397
398         xmc[39]  = (*c >> 4) & 0x7;
399         xmc[40]  = (*c >> 1) & 0x7;
400         xmc[41]  = (*c++ & 0x1) << 2;
401         xmc[41] |= (*c >> 6) & 0x3;
402         xmc[42]  = (*c >> 3) & 0x7;
403         xmc[43]  = *c++ & 0x7;                  /* 30  */
404         xmc[44]  = (*c >> 5) & 0x7;
405         xmc[45]  = (*c >> 2) & 0x7;
406         xmc[46]  = (*c++ & 0x3) << 1;
407         xmc[46] |= (*c >> 7) & 0x1;
408         xmc[47]  = (*c >> 4) & 0x7;
409         xmc[48]  = (*c >> 1) & 0x7;
410         xmc[49]  = (*c++ & 0x1) << 2;
411         xmc[49] |= (*c >> 6) & 0x3;
412         xmc[50]  = (*c >> 3) & 0x7;
413         xmc[51]  = *c & 0x7;                    /* 33 */
414         }
415
416         return 0;
417 }