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