various minor portability fixes (mostly from tholo for OpenBSD)
[asterisk/asterisk.git] / codecs / gsm / src / gsm_implode.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
11 #include "gsm.h"
12 #include "proto.h"
13
14 void gsm_implode P3((s, source, c), gsm s, gsm_signal * source, gsm_byte * c)
15 {
16         /*      variable        size    index
17
18                 GSM_MAGIC       4       -
19
20                 LARc[0]         6       0
21                 LARc[1]         6       1
22                 LARc[2]         5       2
23                 LARc[3]         5       3
24                 LARc[4]         4       4
25                 LARc[5]         4       5
26                 LARc[6]         3       6
27                 LARc[7]         3       7
28
29                 Nc[0]           7       8
30                 bc[0]           2       9
31                 Mc[0]           2       10
32                 xmaxc[0]        6       11
33                 xmc[0]          3       12
34                 xmc[1]          3       13
35                 xmc[2]          3       14
36                 xmc[3]          3       15
37                 xmc[4]          3       16
38                 xmc[5]          3       17
39                 xmc[6]          3       18
40                 xmc[7]          3       19
41                 xmc[8]          3       20
42                 xmc[9]          3       21
43                 xmc[10]         3       22
44                 xmc[11]         3       23
45                 xmc[12]         3       24
46
47                 Nc[1]           7       25
48                 bc[1]           2       26
49                 Mc[1]           2       27
50                 xmaxc[1]        6       28
51                 xmc[13]         3       29
52                 xmc[14]         3       30
53                 xmc[15]         3       31
54                 xmc[16]         3       32
55                 xmc[17]         3       33
56                 xmc[18]         3       34
57                 xmc[19]         3       35
58                 xmc[20]         3       36
59                 xmc[21]         3       37
60                 xmc[22]         3       38
61                 xmc[23]         3       39
62                 xmc[24]         3       40
63                 xmc[25]         3       41
64
65                 Nc[2]           7       42
66                 bc[2]           2       43
67                 Mc[2]           2       44
68                 xmaxc[2]        6       45
69                 xmc[26]         3       46
70                 xmc[27]         3       47
71                 xmc[28]         3       48
72                 xmc[29]         3       49
73                 xmc[30]         3       50
74                 xmc[31]         3       51
75                 xmc[32]         3       52
76                 xmc[33]         3       53
77                 xmc[34]         3       54
78                 xmc[35]         3       55
79                 xmc[36]         3       56
80                 xmc[37]         3       57
81                 xmc[38]         3       58
82
83                 Nc[3]           7       59
84                 bc[3]           2       60
85                 Mc[3]           2       61
86                 xmaxc[3]        6       62
87                 xmc[39]         3       63
88                 xmc[40]         3       64
89                 xmc[41]         3       65
90                 xmc[42]         3       66
91                 xmc[43]         3       67
92                 xmc[44]         3       68
93                 xmc[45]         3       69
94                 xmc[46]         3       70
95                 xmc[47]         3       71
96                 xmc[48]         3       72
97                 xmc[49]         3       73
98                 xmc[50]         3       74
99                 xmc[51]         3       75
100         */
101
102         /*      There are 76 parameters per frame.  The first eight are
103          *      unique.  The remaining 68 are four identical subframes of
104          *      17 parameters each.  gsm_implode converts from a representation
105          *      of these parameters as values in one array of signed words
106          *      to the "packed" version of a GSM frame.
107          */
108
109 #       define  LARc    source
110 #       define  Nc      *((gsm_signal (*) [17])(source + 8))
111 #       define  bc      *((gsm_signal (*) [17])(source + 9))
112 #       define  Mc      *((gsm_signal (*) [17])(source + 10))
113 #       define  xmaxc   *((gsm_signal (*) [17])(source + 11))
114
115 #ifdef WAV49
116         if (s->wav_fmt) {
117
118                 uword sr = 0;
119                 if (s->frame_index == 0) {
120
121                         sr = *c++;
122                         LARc[0] = sr & 0x3f;  sr >>= 6;
123                         sr |= (uword)*c++ << 2;
124                         LARc[1] = sr & 0x3f;  sr >>= 6;
125                         sr |= (uword)*c++ << 4;
126                         LARc[2] = sr & 0x1f;  sr >>= 5;
127                         LARc[3] = sr & 0x1f;  sr >>= 5;
128                         sr |= (uword)*c++ << 2;
129                         LARc[4] = sr & 0xf;  sr >>= 4;
130                         LARc[5] = sr & 0xf;  sr >>= 4;
131                         sr |= (uword)*c++ << 2;                 /* 5 */
132                         LARc[6] = sr & 0x7;  sr >>= 3;
133                         LARc[7] = sr & 0x7;  sr >>= 3;
134                         sr |= (uword)*c++ << 4;
135                         Nc[0] = sr & 0x7f;  sr >>= 7;
136                         bc[0] = sr & 0x3;  sr >>= 2;
137                         Mc[0] = sr & 0x3;  sr >>= 2;
138                         sr |= (uword)*c++ << 1;
139                         xmaxc[0] = sr & 0x3f;  sr >>= 6;
140 #undef  xmc
141 #define xmc     (source + 12)
142                         xmc[0] = sr & 0x7;  sr >>= 3;
143                         sr = *c++;
144                         xmc[1] = sr & 0x7;  sr >>= 3;
145                         xmc[2] = sr & 0x7;  sr >>= 3;
146                         sr |= (uword)*c++ << 2;
147                         xmc[3] = sr & 0x7;  sr >>= 3;
148                         xmc[4] = sr & 0x7;  sr >>= 3;
149                         xmc[5] = sr & 0x7;  sr >>= 3;
150                         sr |= (uword)*c++ << 1;                 /* 10 */
151                         xmc[6] = sr & 0x7;  sr >>= 3;
152                         xmc[7] = sr & 0x7;  sr >>= 3;
153                         xmc[8] = sr & 0x7;  sr >>= 3;
154                         sr = *c++;
155                         xmc[9] = sr & 0x7;  sr >>= 3;
156                         xmc[10] = sr & 0x7;  sr >>= 3;
157                         sr |= (uword)*c++ << 2;
158                         xmc[11] = sr & 0x7;  sr >>= 3;
159                         xmc[12] = sr & 0x7;  sr >>= 3;
160                         sr |= (uword)*c++ << 4;
161                         Nc[1] = sr & 0x7f;  sr >>= 7;
162                         bc[1] = sr & 0x3;  sr >>= 2;
163                         Mc[1] = sr & 0x3;  sr >>= 2;
164                         sr |= (uword)*c++ << 1;
165                         xmaxc[1] = sr & 0x3f;  sr >>= 6;
166 #undef  xmc
167 #define xmc     (source + 29 - 13)
168                         xmc[13] = sr & 0x7;  sr >>= 3;
169                         sr = *c++;                              /* 15 */
170                         xmc[14] = sr & 0x7;  sr >>= 3;
171                         xmc[15] = sr & 0x7;  sr >>= 3;
172                         sr |= (uword)*c++ << 2;
173                         xmc[16] = sr & 0x7;  sr >>= 3;
174                         xmc[17] = sr & 0x7;  sr >>= 3;
175                         xmc[18] = sr & 0x7;  sr >>= 3;
176                         sr |= (uword)*c++ << 1;
177                         xmc[19] = sr & 0x7;  sr >>= 3;
178                         xmc[20] = sr & 0x7;  sr >>= 3;
179                         xmc[21] = sr & 0x7;  sr >>= 3;
180                         sr = *c++;
181                         xmc[22] = sr & 0x7;  sr >>= 3;
182                         xmc[23] = sr & 0x7;  sr >>= 3;
183                         sr |= (uword)*c++ << 2;
184                         xmc[24] = sr & 0x7;  sr >>= 3;
185                         xmc[25] = sr & 0x7;  sr >>= 3;
186                         sr |= (uword)*c++ << 4;                 /* 20 */
187                         Nc[2] = sr & 0x7f;  sr >>= 7;
188                         bc[2] = sr & 0x3;  sr >>= 2;
189                         Mc[2] = sr & 0x3;  sr >>= 2;
190                         sr |= (uword)*c++ << 1;
191                         xmaxc[2] = sr & 0x3f;  sr >>= 6;
192 #undef  xmc
193 #define xmc     (source + 46 - 26)
194                         xmc[26] = sr & 0x7;  sr >>= 3;
195                         sr = *c++;
196                         xmc[27] = sr & 0x7;  sr >>= 3;
197                         xmc[28] = sr & 0x7;  sr >>= 3;
198                         sr |= (uword)*c++ << 2;
199                         xmc[29] = sr & 0x7;  sr >>= 3;
200                         xmc[30] = sr & 0x7;  sr >>= 3;
201                         xmc[31] = sr & 0x7;  sr >>= 3;
202                         sr |= (uword)*c++ << 1;
203                         xmc[32] = sr & 0x7;  sr >>= 3;
204                         xmc[33] = sr & 0x7;  sr >>= 3;
205                         xmc[34] = sr & 0x7;  sr >>= 3;
206                         sr = *c++;                              /* 25 */
207                         xmc[35] = sr & 0x7;  sr >>= 3;
208                         xmc[36] = sr & 0x7;  sr >>= 3;
209                         sr |= (uword)*c++ << 2;
210                         xmc[37] = sr & 0x7;  sr >>= 3;
211                         xmc[38] = sr & 0x7;  sr >>= 3;
212                         sr |= (uword)*c++ << 4;
213                         Nc[3] = sr & 0x7f;  sr >>= 7;
214                         bc[3] = sr & 0x3;  sr >>= 2;
215                         Mc[3] = sr & 0x3;  sr >>= 2;
216                         sr |= (uword)*c++ << 1;
217                         xmaxc[3] = sr & 0x3f;  sr >>= 6;
218 #undef  xmc
219 #define xmc     (source + 63 - 39)
220
221                         xmc[39] = sr & 0x7;  sr >>= 3;
222                         sr = *c++;
223                         xmc[40] = sr & 0x7;  sr >>= 3;
224                         xmc[41] = sr & 0x7;  sr >>= 3;
225                         sr |= (uword)*c++ << 2;                 /* 30 */
226                         xmc[42] = sr & 0x7;  sr >>= 3;
227                         xmc[43] = sr & 0x7;  sr >>= 3;
228                         xmc[44] = sr & 0x7;  sr >>= 3;
229                         sr |= (uword)*c++ << 1;
230                         xmc[45] = sr & 0x7;  sr >>= 3;
231                         xmc[46] = sr & 0x7;  sr >>= 3;
232                         xmc[47] = sr & 0x7;  sr >>= 3;
233                         sr = *c++;
234                         xmc[48] = sr & 0x7;  sr >>= 3;
235                         xmc[49] = sr & 0x7;  sr >>= 3;
236                         sr |= (uword)*c++ << 2;
237                         xmc[50] = sr & 0x7;  sr >>= 3;
238                         xmc[51] = sr & 0x7;  sr >>= 3;
239
240                         s->frame_chain = sr & 0xf;
241                 }
242                 else {
243                         sr = s->frame_chain;
244                         sr |= (uword)*c++ << 4;                 /* 1 */
245                         LARc[0] = sr & 0x3f;  sr >>= 6;
246                         LARc[1] = sr & 0x3f;  sr >>= 6;
247                         sr = *c++;
248                         LARc[2] = sr & 0x1f;  sr >>= 5;
249                         sr |= (uword)*c++ << 3;
250                         LARc[3] = sr & 0x1f;  sr >>= 5;
251                         LARc[4] = sr & 0xf;  sr >>= 4;
252                         sr |= (uword)*c++ << 2;
253                         LARc[5] = sr & 0xf;  sr >>= 4;
254                         LARc[6] = sr & 0x7;  sr >>= 3;
255                         LARc[7] = sr & 0x7;  sr >>= 3;
256                         sr = *c++;                              /* 5 */
257                         Nc[0] = sr & 0x7f;  sr >>= 7;
258                         sr |= (uword)*c++ << 1;
259                         bc[0] = sr & 0x3;  sr >>= 2;
260                         Mc[0] = sr & 0x3;  sr >>= 2;
261                         sr |= (uword)*c++ << 5;
262                         xmaxc[0] = sr & 0x3f;  sr >>= 6;
263 #undef  xmc
264 #define xmc     (source + 12)
265                         xmc[0] = sr & 0x7;  sr >>= 3;
266                         xmc[1] = sr & 0x7;  sr >>= 3;
267                         sr |= (uword)*c++ << 1;
268                         xmc[2] = sr & 0x7;  sr >>= 3;
269                         xmc[3] = sr & 0x7;  sr >>= 3;
270                         xmc[4] = sr & 0x7;  sr >>= 3;
271                         sr = *c++;
272                         xmc[5] = sr & 0x7;  sr >>= 3;
273                         xmc[6] = sr & 0x7;  sr >>= 3;
274                         sr |= (uword)*c++ << 2;                 /* 10 */
275                         xmc[7] = sr & 0x7;  sr >>= 3;
276                         xmc[8] = sr & 0x7;  sr >>= 3;
277                         xmc[9] = sr & 0x7;  sr >>= 3;
278                         sr |= (uword)*c++ << 1;
279                         xmc[10] = sr & 0x7;  sr >>= 3;
280                         xmc[11] = sr & 0x7;  sr >>= 3;
281                         xmc[12] = sr & 0x7;  sr >>= 3;
282                         sr = *c++;
283                         Nc[1] = sr & 0x7f;  sr >>= 7;
284                         sr |= (uword)*c++ << 1;
285                         bc[1] = sr & 0x3;  sr >>= 2;
286                         Mc[1] = sr & 0x3;  sr >>= 2;
287                         sr |= (uword)*c++ << 5;
288                         xmaxc[1] = sr & 0x3f;  sr >>= 6;
289 #undef  xmc
290 #define xmc     (source + 29 - 13)
291                         xmc[13] = sr & 0x7;  sr >>= 3;
292                         xmc[14] = sr & 0x7;  sr >>= 3;
293                         sr |= (uword)*c++ << 1;                 /* 15 */
294                         xmc[15] = sr & 0x7;  sr >>= 3;
295                         xmc[16] = sr & 0x7;  sr >>= 3;
296                         xmc[17] = sr & 0x7;  sr >>= 3;
297                         sr = *c++;
298                         xmc[18] = sr & 0x7;  sr >>= 3;
299                         xmc[19] = sr & 0x7;  sr >>= 3;
300                         sr |= (uword)*c++ << 2;
301                         xmc[20] = sr & 0x7;  sr >>= 3;
302                         xmc[21] = sr & 0x7;  sr >>= 3;
303                         xmc[22] = sr & 0x7;  sr >>= 3;
304                         sr |= (uword)*c++ << 1;
305                         xmc[23] = sr & 0x7;  sr >>= 3;
306                         xmc[24] = sr & 0x7;  sr >>= 3;
307                         xmc[25] = sr & 0x7;  sr >>= 3;
308                         sr = *c++;
309                         Nc[2] = sr & 0x7f;  sr >>= 7;
310                         sr |= (uword)*c++ << 1;                 /* 20 */
311                         bc[2] = sr & 0x3;  sr >>= 2;
312                         Mc[2] = sr & 0x3;  sr >>= 2;
313                         sr |= (uword)*c++ << 5;
314                         xmaxc[2] = sr & 0x3f;  sr >>= 6;
315 #undef  xmc
316 #define xmc     (source + 46 - 26)
317                         xmc[26] = sr & 0x7;  sr >>= 3;
318                         xmc[27] = sr & 0x7;  sr >>= 3;
319                         sr |= (uword)*c++ << 1; 
320                         xmc[28] = sr & 0x7;  sr >>= 3;
321                         xmc[29] = sr & 0x7;  sr >>= 3;
322                         xmc[30] = sr & 0x7;  sr >>= 3;
323                         sr = *c++;
324                         xmc[31] = sr & 0x7;  sr >>= 3;
325                         xmc[32] = sr & 0x7;  sr >>= 3;
326                         sr |= (uword)*c++ << 2;
327                         xmc[33] = sr & 0x7;  sr >>= 3;
328                         xmc[34] = sr & 0x7;  sr >>= 3;
329                         xmc[35] = sr & 0x7;  sr >>= 3;
330                         sr |= (uword)*c++ << 1;                 /* 25 */
331                         xmc[36] = sr & 0x7;  sr >>= 3;
332                         xmc[37] = sr & 0x7;  sr >>= 3;
333                         xmc[38] = sr & 0x7;  sr >>= 3;
334                         sr = *c++;
335                         Nc[3] = sr & 0x7f;  sr >>= 7;
336                         sr |= (uword)*c++ << 1;         
337                         bc[3] = sr & 0x3;  sr >>= 2;
338                         Mc[3] = sr & 0x3;  sr >>= 2;
339                         sr |= (uword)*c++ << 5;
340                         xmaxc[3] = sr & 0x3f;  sr >>= 6;
341 #undef  xmc
342 #define xmc     (source + 63 - 39)
343
344                         xmc[39] = sr & 0x7;  sr >>= 3;
345                         xmc[40] = sr & 0x7;  sr >>= 3;
346                         sr |= (uword)*c++ << 1;
347                         xmc[41] = sr & 0x7;  sr >>= 3;
348                         xmc[42] = sr & 0x7;  sr >>= 3;
349                         xmc[43] = sr & 0x7;  sr >>= 3;
350                         sr = *c++;                              /* 30 */
351                         xmc[44] = sr & 0x7;  sr >>= 3;
352                         xmc[45] = sr & 0x7;  sr >>= 3;
353                         sr |= (uword)*c++ << 2;
354                         xmc[46] = sr & 0x7;  sr >>= 3;
355                         xmc[47] = sr & 0x7;  sr >>= 3;
356                         xmc[48] = sr & 0x7;  sr >>= 3;
357                         sr |= (uword)*c++ << 1;
358                         xmc[49] = sr & 0x7;  sr >>= 3;
359                         xmc[50] = sr & 0x7;  sr >>= 3;
360                         xmc[51] = sr & 0x7;  sr >>= 3;
361                 }
362         }
363         else
364 #endif 
365         {
366
367         *c++ =   ((GSM_MAGIC & 0xF) << 4)               /* 1 */
368                | ((LARc[0] >> 2) & 0xF);
369         *c++ =   ((LARc[0] & 0x3) << 6)
370                | (LARc[1] & 0x3F);
371         *c++ =   ((LARc[2] & 0x1F) << 3)
372                | ((LARc[3] >> 2) & 0x7);
373         *c++ =   ((LARc[3] & 0x3) << 6)
374                | ((LARc[4] & 0xF) << 2)
375                | ((LARc[5] >> 2) & 0x3);
376         *c++ =   ((LARc[5] & 0x3) << 6)
377                | ((LARc[6] & 0x7) << 3)
378                | (LARc[7] & 0x7);
379
380
381         *c++ =   ((Nc[0] & 0x7F) << 1)
382
383
384                | ((bc[0] >> 1) & 0x1);
385         *c++ =   ((bc[0] & 0x1) << 7)
386
387
388                | ((Mc[0] & 0x3) << 5)
389
390                | ((xmaxc[0] >> 1) & 0x1F);
391         *c++ =   ((xmaxc[0] & 0x1) << 7)
392
393 #undef xmc
394 #define xmc     (source + 12)
395
396                | ((xmc[0] & 0x7) << 4)
397                | ((xmc[1] & 0x7) << 1)
398                | ((xmc[2] >> 2) & 0x1);
399         *c++ =   ((xmc[2] & 0x3) << 6)
400                | ((xmc[3] & 0x7) << 3)
401                | (xmc[4] & 0x7);
402         *c++ =   ((xmc[5] & 0x7) << 5)                  /* 10 */
403                | ((xmc[6] & 0x7) << 2)
404                | ((xmc[7] >> 1) & 0x3);
405         *c++ =   ((xmc[7] & 0x1) << 7)
406                | ((xmc[8] & 0x7) << 4)
407                | ((xmc[9] & 0x7) << 1)
408                | ((xmc[10] >> 2) & 0x1);
409         *c++ =   ((xmc[10] & 0x3) << 6)
410                | ((xmc[11] & 0x7) << 3)
411                | (xmc[12] & 0x7);
412
413
414         *c++ =   ((Nc[1] & 0x7F) << 1)
415
416
417                | ((bc[1] >> 1) & 0x1);
418         *c++ =   ((bc[1] & 0x1) << 7)
419
420
421                | ((Mc[1] & 0x3) << 5)
422
423
424                | ((xmaxc[1] >> 1) & 0x1F);
425         *c++ =   ((xmaxc[1] & 0x1) << 7)
426
427 #undef  xmc
428 #define xmc     (source + 29 - 13)
429
430                | ((xmc[13] & 0x7) << 4)
431                | ((xmc[14] & 0x7) << 1)
432                | ((xmc[15] >> 2) & 0x1);
433         *c++ =   ((xmc[15] & 0x3) << 6)
434                | ((xmc[16] & 0x7) << 3)
435                | (xmc[17] & 0x7);
436         *c++ =   ((xmc[18] & 0x7) << 5)
437                | ((xmc[19] & 0x7) << 2)
438                | ((xmc[20] >> 1) & 0x3);
439         *c++ =   ((xmc[20] & 0x1) << 7)
440                | ((xmc[21] & 0x7) << 4)
441                | ((xmc[22] & 0x7) << 1)
442                | ((xmc[23] >> 2) & 0x1);
443         *c++ =   ((xmc[23] & 0x3) << 6)
444                | ((xmc[24] & 0x7) << 3)
445                | (xmc[25] & 0x7);
446
447
448         *c++ =   ((Nc[2] & 0x7F) << 1)                  /* 20 */
449
450
451                | ((bc[2] >> 1) & 0x1);
452         *c++ =   ((bc[2] & 0x1) << 7)
453
454
455                | ((Mc[2] & 0x3) << 5)
456
457
458                | ((xmaxc[2] >> 1) & 0x1F);
459         *c++ =   ((xmaxc[2] & 0x1) << 7)
460
461 #undef  xmc
462 #define xmc     (source + 46 - 26)
463
464                | ((xmc[26] & 0x7) << 4)
465                | ((xmc[27] & 0x7) << 1)
466                | ((xmc[28] >> 2) & 0x1);
467         *c++ =   ((xmc[28] & 0x3) << 6)
468                | ((xmc[29] & 0x7) << 3)
469                | (xmc[30] & 0x7);
470         *c++ =   ((xmc[31] & 0x7) << 5)
471                | ((xmc[32] & 0x7) << 2)
472                | ((xmc[33] >> 1) & 0x3);
473         *c++ =   ((xmc[33] & 0x1) << 7)
474                | ((xmc[34] & 0x7) << 4)
475                | ((xmc[35] & 0x7) << 1)
476                | ((xmc[36] >> 2) & 0x1);
477         *c++ =   ((xmc[36] & 0x3) << 6)
478                | ((xmc[37] & 0x7) << 3)
479                | (xmc[38] & 0x7);
480
481
482         *c++ =   ((Nc[3] & 0x7F) << 1)
483
484
485                | ((bc[3] >> 1) & 0x1);
486         *c++ =   ((bc[3] & 0x1) << 7)
487
488
489                | ((Mc[3] & 0x3) << 5)
490
491
492                | ((xmaxc[3] >> 1) & 0x1F);
493         *c++ =   ((xmaxc[3] & 0x1) << 7)
494
495 #undef  xmc
496 #define xmc     (source + 63 - 39)
497
498                | ((xmc[39] & 0x7) << 4)
499                | ((xmc[40] & 0x7) << 1)
500                | ((xmc[41] >> 2) & 0x1);
501         *c++ =   ((xmc[41] & 0x3) << 6)                 /* 30 */
502                | ((xmc[42] & 0x7) << 3)
503                | (xmc[43] & 0x7);
504         *c++ =   ((xmc[44] & 0x7) << 5)
505                | ((xmc[45] & 0x7) << 2)
506                | ((xmc[46] >> 1) & 0x3);
507         *c++ =   ((xmc[46] & 0x1) << 7)
508                | ((xmc[47] & 0x7) << 4)
509                | ((xmc[48] & 0x7) << 1)
510                | ((xmc[49] >> 2) & 0x1);
511         *c++ =   ((xmc[49] & 0x3) << 6)
512                | ((xmc[50] & 0x7) << 3)
513                | (xmc[51] & 0x7);
514         }
515 }