Sync froyo ril blob from hyc's codebase.
[xdandroid:rootfs.git] / gsm.c
1 #include "gsm.h"
2 #include <stdlib.h>
3
4 /** UTILITIES
5  **/
6 byte_t
7 gsm_int_to_bcdi( int  value )
8 {
9     return (byte_t)((value / 10) | ((value % 10) << 4));
10 }
11
12 int
13 gsm_int_from_bcdi( byte_t  val )
14 {
15     int  ret = 0;
16
17     if ((val & 0xf0) <= 0x90)
18         ret = (val >> 4);
19
20     if ((val & 0x0f) <= 0x90)
21         ret |= (val % 0xf)*10;
22
23     return ret;
24 }
25
26
27 static int
28 gsm_bcdi_to_ascii( cbytes_t  bcd, int  bcdlen, bytes_t  dst )
29 {
30     static byte_t  bcdichars[14] = "0123456789*#,N";
31
32     int  result = 0;
33     int  shift  = 0;
34
35     while (bcdlen > 0) {
36         int  c = (bcd[0] >> shift) & 0xf;
37
38         if (c == 0xf && bcdlen == 1)
39             break;
40
41         if (c < 14) {
42             if (dst) dst[result] = bcdichars[c];
43             result += 1;
44         }
45         bcdlen --;
46         shift += 4;
47         if (shift == 8) {
48             bcd++;
49             shift = 0;
50         }
51     }
52     return result;
53 }
54
55
56 static int
57 gsm_bcdi_from_ascii( cbytes_t  ascii, int  asciilen, bytes_t  dst )
58 {
59     cbytes_t  end    = ascii + asciilen;
60     int       result = 0;
61     int       phase  = 0x01;
62
63     while (ascii < end) {
64         int  c = *ascii++;
65         int  d;
66
67         if (c == '*')
68             c = 11;
69         else if (c == '#')
70             c = 12;
71         else if (c == ',')
72             c = 13;
73         else if (c == 'N')
74             c = 14;
75         else {
76             c -= '0';
77             if ((unsigned)c >= 10)
78                 break;
79         }
80         phase = (phase << 4) | c;
81         if (phase & 0x100) {
82             if (dst) dst[result] = (byte_t) phase;
83             result += 1;
84             phase   = 0x01;
85         }
86     }
87     if (phase != 0x01) {
88         if (dst) dst[result] = (byte_t)( phase | 0xf0 );
89         result += 1;
90     }
91     return  result;
92 }
93
94
95 int
96 gsm_hexchar_to_int( char  c )
97 {
98     if ((unsigned)(c - '0') < 10)
99         return c - '0';
100     if ((unsigned)(c - 'a') < 6)
101         return 10 + (c - 'a');
102     if ((unsigned)(c - 'A') < 6)
103         return 10 + (c - 'A');
104     return -1;
105 }
106
107 int
108 gsm_hexchar_to_int0( char  c )
109 {
110     int  ret = gsm_hexchar_to_int(c);
111
112     return (ret < 0) ? 0 : ret;
113 }
114
115 int
116 gsm_hex2_to_byte( const char*  hex )
117 {
118     int  hi = gsm_hexchar_to_int(hex[0]);
119     int  lo = gsm_hexchar_to_int(hex[1]);
120
121     if (hi < 0 || lo < 0)
122         return -1;
123
124     return ( (hi << 4) | lo );
125 }
126
127 int
128 gsm_hex4_to_short( const char*  hex )
129 {
130     int  hi = gsm_hex2_to_byte(hex);
131     int  lo = gsm_hex2_to_byte(hex+2);
132
133     if (hi < 0 || lo < 0)
134         return -1;
135
136     return ((hi << 8) | lo);
137 }
138
139 int
140 gsm_hex2_to_byte0( const char*  hex )
141 {
142     int  hi = gsm_hexchar_to_int0(hex[0]);
143     int  lo = gsm_hexchar_to_int0(hex[1]);
144
145     return (byte_t)( (hi << 4) | lo );
146 }
147
148 void
149 gsm_hex_from_byte( char*  hex, int val )
150 {
151     static const char  hexdigits[] = "0123456789ABCDEF";
152
153     hex[0] = hexdigits[(val >> 4) & 15];
154     hex[1] = hexdigits[val & 15];
155 }
156
157 void
158 gsm_hex_from_short( char*  hex, int  val )
159 {
160     gsm_hex_from_byte( hex,   (val >> 8) );
161     gsm_hex_from_byte( hex+2, val );
162 }
163
164
165
166 /** HEX
167  **/
168 void
169 gsm_hex_to_bytes( cbytes_t  hex, int  hexlen, bytes_t  dst )
170 {
171     int  nn;
172
173     for (nn = 0; nn < hexlen/2; nn++ ) {
174         dst[nn] = (byte_t) gsm_hex2_to_byte0( hex+2*nn );
175     }
176     if (hexlen & 1) {
177         dst[nn] = gsm_hexchar_to_int0( hex[2*nn] ) << 4;
178     }
179 }
180
181 void
182 gsm_hex_from_bytes( char*  hex, cbytes_t  src, int  srclen )
183 {
184     int  nn;
185
186     for (nn = 0; nn < srclen; nn++) {
187         gsm_hex_from_byte( hex + 2*nn, src[nn] );
188     }
189 }
190
191 /** ROPES
192  **/
193
194 void
195 gsm_rope_init( GsmRope  rope )
196 {
197     rope->data  = NULL;
198     rope->pos   = 0;
199     rope->max   = 0;
200     rope->error = 0;
201 }
202
203 void
204 gsm_rope_init_alloc( GsmRope  rope, int  count )
205 {
206     rope->data  = rope->data0;
207     rope->pos   = 0;
208     rope->max   = sizeof(rope->data0);
209     rope->error = 0;
210
211     if (count > 0) {
212         rope->data = calloc( count, 1 );
213         rope->max  = count;
214
215         if (rope->data == NULL) {
216             rope->error = 1;
217             rope->max   = 0;
218         }
219     }
220 }
221
222 int
223 gsm_rope_done( GsmRope  rope )
224 {
225     int  result = rope->error;
226
227     if (rope->data && rope->data != rope->data0)
228         free(rope->data);
229
230     rope->data  = NULL;
231     rope->pos   = 0;
232     rope->max   = 0;
233     rope->error = 0;
234
235     return result;
236 }
237
238
239 bytes_t
240 gsm_rope_done_acquire( GsmRope  rope, int  *psize )
241 {
242     bytes_t  result = rope->data;
243
244     *psize = rope->pos;
245     if (result == rope->data0) {
246         result = malloc(  rope->pos );
247         if (result != NULL)
248             memcpy( result, rope->data, rope->pos );
249     }
250     return result;
251 }
252
253
254 int
255 gsm_rope_ensure( GsmRope  rope, int  new_count )
256 {
257     if (rope->data != NULL) {
258         int       old_max  = rope->max;
259         bytes_t   old_data = rope->data == rope->data0 ? NULL : rope->data;
260         int       new_max  = old_max;
261         bytes_t   new_data;
262
263         while (new_max < new_count) {
264             new_max += (new_max >> 1) + 4;
265         }
266         new_data = realloc( old_data, new_max );
267         if (new_data == NULL) {
268             rope->error = 1;
269             return -1;
270         }
271         rope->data = new_data;
272         rope->max  = new_max;
273     } else {
274         rope->max = new_count;
275     }
276     return 0;
277 }
278
279 static int
280 gsm_rope_can_grow( GsmRope  rope, int  count )
281 {
282     if (!rope->data || rope->error)
283         return 0;
284
285     if (rope->pos + count > rope->max)
286     {
287         if (rope->data == NULL)
288             rope->max = rope->pos + count;
289
290         else if (rope->error ||
291                  gsm_rope_ensure( rope, rope->pos + count ) < 0)
292             return 0;
293     }
294     return 1;
295 }
296
297 void
298 gsm_rope_add_c( GsmRope  rope,  char  c )
299 {
300     if (gsm_rope_can_grow(rope, 1)) {
301         rope->data[ rope->pos ] = (byte_t) c;
302     }
303     rope->pos += 1;
304 }
305
306 void
307 gsm_rope_add( GsmRope  rope, const void*  buf, int  buflen )
308 {
309     if (gsm_rope_can_grow(rope, buflen)) {
310         memcpy( rope->data + rope->pos, (const char*)buf, buflen );
311     }
312     rope->pos += buflen;
313 }
314
315 void*
316 gsm_rope_reserve( GsmRope  rope, int  count )
317 {
318     void*  result = NULL;
319
320     if (gsm_rope_can_grow(rope, count))
321     {
322         if (rope->data != NULL)
323             result = rope->data + rope->pos;
324     }
325     rope->pos += count;
326
327     return result;
328 }
329
330 /* skip a given number of Unicode characters in a utf-8 byte string */
331 cbytes_t
332 utf8_skip( cbytes_t   utf8,
333            cbytes_t   utf8end,
334            int        count)
335 {
336     cbytes_t  p   = utf8;
337     cbytes_t  end = utf8end;
338
339     for ( ; count > 0; count-- ) {
340         int  c;
341
342         if (p > end)
343             break;
344
345         c = *p++;
346         if (c > 128) {
347             while (p < end && (p[0] & 0xc0) == 0x80)
348                 p++;
349         }
350     }
351     return  p;
352 }
353
354 static __inline__ int
355 utf8_next( cbytes_t  *pp, cbytes_t  end )
356 {
357     cbytes_t  p      = *pp;
358     int       result = -1;
359
360     if (p < end) {
361         int  c= *p++;
362         if (c >= 128) {
363             if ((c & 0xe0) == 0xc0)
364                 c &= 0x1f;
365             else if ((c & 0xf0) == 0xe0)
366                 c &= 0x0f;
367             else
368                 c &= 0x07;
369
370             while (p < end && (p[0] & 0xc0) == 0x80) {
371                 c = (c << 6) | (p[0] & 0x3f);
372             }
373         }
374         result = c;
375         *pp    = p;
376     }
377 Exit:
378     return result;
379 }
380
381
382 __inline__ int
383 utf8_write( bytes_t  utf8, int  offset, int  v )
384 {
385     int  result;
386
387     if (v < 128) {
388         result = 1;
389         if (utf8)
390             utf8[offset] = (byte_t) v;
391     } else if (v < 0x800) {
392         result = 2;
393         if (utf8) {
394             utf8[offset+0] = (byte_t)( 0xc0 | (v >> 6) );
395             utf8[offset+1] = (byte_t)( 0x80 | (v & 0x3f) );
396         }
397     } else if (v < 0x10000) {
398         result = 3;
399         if (utf8) {
400             utf8[offset+0] = (byte_t)( 0xe0 |  (v >> 12) );
401             utf8[offset+1] = (byte_t)( 0x80 | ((v >> 6) & 0x3f) );
402             utf8[offset+2] = (byte_t)( 0x80 |  (v & 0x3f) );
403         }
404     } else {
405         result = 4;
406         if (utf8) {
407             utf8[offset+0] = (byte_t)( 0xf0 | ((v >> 18) & 0x7) );
408             utf8[offset+1] = (byte_t)( 0x80 | ((v >> 12) & 0x3f) );
409             utf8[offset+2] = (byte_t)( 0x80 | ((v >> 6) & 0x3f) );
410             utf8[offset+3] = (byte_t)( 0x80 |  (v & 0x3f) );
411         }
412     }
413     return  result;
414 }
415
416 static __inline__ int
417 ucs2_write( bytes_t  ucs2, int  offset, int  v )
418 {
419     if (ucs2) {
420         ucs2[offset+0] = (byte_t) (v >> 8);
421         ucs2[offset+1] = (byte_t) (v);
422     }
423     return 2;
424 }
425
426 int
427 utf8_check( cbytes_t   p, int  utf8len )
428 {
429     cbytes_t  end    = p + utf8len;
430     int       result = 0;
431
432     if (p) {
433         while (p < end) {
434             int  c = *p++;
435             if (c >= 128) {
436                 int  len;
437                 if ((c & 0xe0) == 0xc0) {
438                     len = 1;
439                 }
440                 else if ((c & 0xf0) == 0xe0) {
441                     len = 2;
442                 }
443                 else if ((c & 0xf8) == 0xf0) {
444                     len = 3;
445                 }
446                 else
447                     goto Exit;  /* malformed utf-8 */
448
449                 if (p+len > end) /* string too short */
450                     goto Exit;
451
452                 for ( ; len > 0; len--, p++ ) {
453                     if ((p[0] & 0xc0) != 0x80)
454                         goto Exit;
455                 }
456             }
457         }
458         result = 1;
459     }
460 Exit:
461     return result;
462 }
463
464 /** UCS2 to UTF8
465  **/
466
467 /* convert a UCS2 string into a UTF8 byte string, assumes 'buf' is correctly sized */
468 int
469 ucs2_to_utf8( cbytes_t  ucs2,
470               int       ucs2len,
471               bytes_t   buf )
472 {
473     int  nn;
474     int  result = 0;
475
476     for (nn = 0; nn < ucs2len; ucs2 += 2, nn++) {
477         int  c= (ucs2[0] << 8) | ucs2[1];
478         result += utf8_write(buf, result, c);
479     }
480     return result;
481 }
482
483 /* count the number of UCS2 chars contained in a utf8 byte string */
484 int
485 utf8_to_ucs2( cbytes_t  utf8,
486               int       utf8len,
487               bytes_t   ucs2 )
488 {
489     cbytes_t  p      = utf8;
490     cbytes_t  end    = p + utf8len;
491     int       result = 0;
492
493     while (p < end) {
494         int  c = utf8_next(&p, end);
495
496         if (c < 0)
497             break;
498
499         result += ucs2_write(ucs2, result, c);
500     }
501     return result/2;
502 }
503
504
505
506 /** GSM ALPHABET
507  **/
508
509 #define  GSM_7BITS_ESCAPE   0x1b
510 #define  GSM_7BITS_UNKNOWN  0
511
512 static const unsigned short   gsm7bits_to_unicode[128] = {
513   '@', 0xa3,  '$', 0xa5, 0xe8, 0xe9, 0xf9, 0xec, 0xf2, 0xc7, '\n', 0xd8, 0xf8, '\r', 0xc5, 0xe5,
514 0x394,  '_',0x3a6,0x393,0x39b,0x3a9,0x3a0,0x3a8,0x3a3,0x398,0x39e,    0, 0xc6, 0xe6, 0xdf, 0xc9,
515   ' ',  '!',  '"',  '#', 0xa4,  '%',  '&', '\'',  '(',  ')',  '*',  '+',  ',',  '-',  '.',  '/',
516   '0',  '1',  '2',  '3',  '4',  '5',  '6',  '7',  '8',  '9',  ':',  ';',  '<',  '=',  '>',  '?',
517  0xa1,  'A',  'B',  'C',  'D',  'E',  'F',  'G',  'H',  'I',  'J',  'K',  'L',  'M',  'N',  'O',
518   'P',  'Q',  'R',  'S',  'T',  'U',  'V',  'W',  'X',  'Y',  'Z', 0xc4, 0xd6,0x147, 0xdc, 0xa7,
519  0xbf,  'a',  'b',  'c',  'd',  'e',  'f',  'g',  'h',  'i',  'j',  'k',  'l',  'm',  'n',  'o',
520   'p',  'q',  'r',  's',  't',  'u',  'v',  'w',  'x',  'y',  'z', 0xe4, 0xf6, 0xf1, 0xfc, 0xe0,
521 };
522
523 static const unsigned short  gsm7bits_extend_to_unicode[128] = {
524     0,   0,   0,   0,   0,   0,   0,   0,   0,   0,'\f',   0,   0,   0,   0,   0,
525     0,   0,   0,   0, '^',   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
526     0,   0,   0,   0,   0,   0,   0,   0, '{', '}',   0,   0,   0,   0,   0,'\\',
527     0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, '[', '~', ']',   0,
528   '|',   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
529     0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
530     0,   0,   0,   0,   0,0x20ac, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
531     0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
532 };
533
534
535 static int
536 unichar_to_gsm7( int  unicode )
537 {
538     int  nn;
539     for (nn = 0; nn < 128; nn++) {
540         if (gsm7bits_to_unicode[nn] == unicode) {
541             return nn;
542         }
543     }
544     return -1;
545 }
546
547 static int
548 unichar_to_gsm7_extend( int  unichar )
549 {
550     int  nn;
551     for (nn = 0; nn < 128; nn++) {
552         if (gsm7bits_extend_to_unicode[nn] == unichar) {
553             return nn;
554         }
555     }
556     return -1;
557 }
558
559
560 /* return the number of septets needed to encode a unicode charcode */
561 static int
562 unichar_to_gsm7_count( int  unicode )
563 {
564     int  nn;
565
566     nn = unichar_to_gsm7(unicode);
567     if (nn >= 0)
568         return 1;
569
570     nn = unichar_to_gsm7_extend(unicode);
571     if (nn >= 0)
572         return 2;
573
574     return 0;
575 }
576
577
578 int
579 utf8_check_gsm7( cbytes_t  utf8,
580                  int       utf8len )
581 {
582     cbytes_t  utf8end = utf8 + utf8len;
583
584     while (utf8 < utf8end) {
585         int  c = utf8_next( &utf8, utf8end );
586         if (unichar_to_gsm7_count(c) == 0)
587             return 0;
588     }
589     return 1;
590 }
591
592
593 int
594 utf8_from_gsm7( cbytes_t  src,
595                 int       septet_offset,
596                 int       septet_count,
597                 bytes_t   utf8 )
598 {
599     int  shift   = (septet_offset & 7);
600     int  escaped = 0;
601     int  result  = 0;
602
603     src += (septet_offset >> 3);
604     for ( ; septet_count > 0; septet_count-- )
605     {
606         int  c = (src[0] >> shift) & 0x7f;
607         int  v;
608
609         if (shift > 1) {
610             c = ((src[1] << (8-shift)) | c) & 0x7f;
611         }
612
613         if (escaped) {
614             v = gsm7bits_extend_to_unicode[c];
615         } else if (c == GSM_7BITS_ESCAPE) {
616             escaped = 1;
617             goto NextSeptet;
618         } else {
619             v = gsm7bits_to_unicode[c];
620         }
621
622         result += utf8_write( utf8, result, v );
623
624     NextSeptet:
625         shift += 7;
626         if (shift >= 8) {
627             shift -= 8;
628             src   += 1;
629         }
630     }
631     return  result;
632 }
633
634
635 int
636 utf8_from_gsm8( cbytes_t  src, int  count, bytes_t  utf8 )
637 {
638     int  result  = 0;
639     int  escaped = 0;
640
641
642     for ( ; count > 0; count-- )
643     {
644         int  c = *src++;
645         int  v;
646
647         if (c == 0xff)
648             break;
649
650         if (c == GSM_7BITS_ESCAPE) {
651             if (escaped) { /* two escape characters => one space */
652                 c = 0x20;
653                 escaped = 0;
654             } else {
655                 escaped = 1;
656                 continue;
657             }
658         }
659         else
660         {
661             if (c >= 0x80) {
662                 c       = 0x20;
663                 escaped = 0;
664             } else if (escaped) {
665                 c = gsm7bits_extend_to_unicode[c];
666             } else
667                 c = gsm7bits_to_unicode[c];
668         }
669
670         result += utf8_write( utf8, result, c );
671     }
672     return  result;
673 }
674
675 /* convert a GSM 7-bit message into a unicode character array
676  * the 'dst' array must contain at least 160 chars. the function
677  * returns the number of characters decoded
678  *
679  * assumes the 'dst' array has at least septet_count items, returns the
680  * number of unichars really written
681  */
682 int
683 ucs2_from_gsm7( bytes_t   ucs2,
684                 cbytes_t  src,
685                 int       septet_offset,
686                 int       septet_count )
687 {
688     const unsigned char*  p     = src + (septet_offset >> 3);
689     int                   shift = (septet_offset & 7);
690     int                   escaped = 0;
691     int                   result  = 0;
692
693     for ( ; septet_count > 0; septet_count-- )
694     {
695         unsigned  val  = (p[0] >> shift) & 0x7f;
696
697         if (shift > 1)
698             val = (val | (p[1] << (8-shift))) & 0x7f;
699
700         if (escaped) {
701             int  c = gsm7bits_to_unicode[val];
702
703             result += ucs2_write(ucs2, result, c);
704             escaped = 0;
705         }
706         else if (val == GSM_7BITS_ESCAPE) {
707             escaped = 1;
708         }
709         else {
710             val = gsm7bits_extend_to_unicode[val];
711             if (val == 0)
712                 val = 0x20;
713
714             result += ucs2_write( ucs2, result, val );
715         }
716     }
717     return result/2;
718 }
719
720
721 /* count the number of septets required to write a utf8 string */
722 static int
723 utf8_to_gsm7_count( cbytes_t  utf8, int  utf8len )
724 {
725     cbytes_t  utf8end = utf8 + utf8len;
726     int       result  = 0;
727
728     while ( utf8 < utf8end ) {
729         int  len;
730         int  c = utf8_next( &utf8, utf8end );
731
732         if (c < 0)
733             break;
734
735         len = unichar_to_gsm7_count(c);
736         if (len == 0)    /* replace non-representables with space */
737             len = 1;
738
739         result += len;
740     }
741     return result;
742 }
743
744 typedef struct {
745     bytes_t   dst;
746     unsigned  pad;
747     int       bits;
748     int       offset;
749 } BWriterRec, *BWriter;
750
751 static void
752 bwriter_init( BWriter  writer, bytes_t  dst, int  start )
753 {
754     int  shift = start & 7;
755
756     writer->dst    = dst + (start >> 3);
757     writer->pad    = 0;
758     writer->bits   = shift;
759     writer->offset = start;
760
761     if (shift > 0) {
762         writer->pad  = writer->dst[0] & ~(0xFF << shift);
763     }
764 }
765
766 static void
767 bwriter_add7( BWriter  writer, unsigned  value )
768 {
769     writer->pad  |= (unsigned)(value << writer->bits);
770     writer->bits += 7;
771     if (writer->bits >= 8) {
772         writer->dst[0] = (byte_t)writer->pad;
773         writer->bits  -= 8;
774         writer->pad  >>= 8;
775         writer->dst   += 1;
776     }
777     writer->offset += 7;
778 }
779
780 static int
781 bwriter_done( BWriter  writer )
782 {
783     if (writer->bits > 0) {
784         writer->dst[0] = (byte_t)writer->pad;
785         writer->pad    = 0;
786         writer->bits   = 0;
787         writer->dst   += 1;
788     }
789     return writer->offset;
790 }
791
792 /* convert a utf8 string to a gsm7 byte string - return the number of septets written */
793 int
794 utf8_to_gsm7( cbytes_t  utf8, int  utf8len, bytes_t  dst, int offset )
795 {
796     const unsigned char*  utf8end = utf8 + utf8len;
797     BWriterRec            writer[1];
798
799     if (dst == NULL)
800         return utf8_to_gsm7_count(utf8, utf8len);
801
802     bwriter_init( writer, dst, offset );
803     while ( utf8 < utf8end ) {
804         int  c = utf8_next( &utf8, utf8end );
805         int  nn;
806
807         if (c < 0)
808             break;
809
810         nn = unichar_to_gsm7(c);
811         if (nn >= 0) {
812             bwriter_add7( writer, nn );
813             continue;
814         }
815
816         nn = unichar_to_gsm7_extend(c);
817         if (nn >= 0) {
818             bwriter_add7( writer, GSM_7BITS_ESCAPE );
819             bwriter_add7( writer, nn );
820             continue;
821         }
822
823         /* unknown => replaced by space */
824         bwriter_add7( writer, 0x20 );
825     }
826     return  bwriter_done( writer );
827 }
828
829
830 int
831 utf8_to_gsm8( cbytes_t  utf8, int  utf8len, bytes_t  dst )
832 {
833     const unsigned char*  utf8end = utf8 + utf8len;
834     int                   result  = 0;
835
836     while ( utf8 < utf8end ) {
837         int  c = utf8_next( &utf8, utf8end );
838         int  nn;
839
840         if (c < 0)
841             break;
842
843         nn = unichar_to_gsm7(c);
844         if (nn >= 0) {
845             if (dst)
846                 dst[result] = (byte_t)nn;
847             result += 1;
848             continue;
849         }
850
851         nn = unichar_to_gsm7_extend(c);
852         if (nn >= 0) {
853             if (dst) {
854                 dst[result+0] = (byte_t) GSM_7BITS_ESCAPE;
855                 dst[result+1] = (byte_t) nn;
856             }
857             result += 2;
858             continue;
859         }
860
861         /* unknown => space */
862         if (dst)
863             dst[result] = 0x20;
864         result += 1;
865     }
866     return  result;
867 }
868
869
870 int
871 ucs2_to_gsm7( cbytes_t  ucs2, int  ucs2len, bytes_t  dst, int offset )
872 {
873     const unsigned char*  ucs2end = ucs2 + ucs2len*2;
874     BWriterRec            writer[1];
875
876     bwriter_init( writer, dst, offset );
877     while ( ucs2 < ucs2end ) {
878         int  c = *ucs2++;
879         int  nn;
880
881         for (nn = 0; nn < 128; nn++) {
882             if ( gsm7bits_to_unicode[nn] == c ) {
883                 bwriter_add7( writer, nn );
884                 goto NextUnicode;
885             }
886         }
887         for (nn = 0; nn < 128; nn++) {
888             if ( gsm7bits_extend_to_unicode[nn] == c ) {
889                 bwriter_add7( writer, GSM_7BITS_ESCAPE );
890                 bwriter_add7( writer, nn );
891                 goto NextUnicode;
892             }
893         }
894
895         /* unknown */
896         bwriter_add7( writer, 0x20 );
897
898     NextUnicode:
899         ;
900     }
901     return  bwriter_done( writer );
902 }
903
904
905 int
906 ucs2_to_gsm8( cbytes_t  ucs2, int  ucs2len, bytes_t  dst )
907 {
908     const unsigned char*  ucs2end = ucs2 + ucs2len*2;
909     bytes_t               dst0    = dst;
910
911     while ( ucs2 < ucs2end ) {
912         int  c = *ucs2++;
913         int  nn;
914
915         for (nn = 0; nn < 128; nn++) {
916             if ( gsm7bits_to_unicode[nn] == c ) {
917                 *dst++ = (byte_t)nn;
918                 goto NextUnicode;
919             }
920         }
921         for (nn = 0; nn < 128; nn++) {
922             if ( gsm7bits_extend_to_unicode[nn] == c ) {
923                 dst[0] = (byte_t) GSM_7BITS_ESCAPE;
924                 dst[1] = (byte_t) nn;
925                 dst   += 2;
926                 goto NextUnicode;
927             }
928         }
929
930         /* unknown */
931         *dst++ = 0x20;
932
933     NextUnicode:
934         ;
935     }
936     return (dst - dst0);
937 }
938
939 int
940 gsm_bcdnum_to_ascii( cbytes_t  bcd, int  count, bytes_t  dst )
941 {
942     int  result = 0;
943     int  shift  = 0;
944
945     while (count > 0) {
946         int  c = (bcd[0] >> shift) & 0xf;
947
948         if (c == 15 && count == 1)  /* ignore trailing 0xf */
949             break;
950
951         if (c >= 14)
952             c = 0;
953
954         if (dst) dst[result] = "0123456789*#,N"[c];
955         result += 1;
956
957         shift += 4;
958         if (shift == 8) {
959             shift = 0;
960             bcd += 1;
961         }
962     }
963     return  result;
964 }
965
966
967 int
968 gsm_bcdnum_from_ascii( cbytes_t  ascii, int  asciilen, bytes_t  dst )
969 {
970     cbytes_t  end = ascii + asciilen;
971     int  result = 0;
972     int  phase = 0x01;
973
974     while (ascii < end) {
975         int  c = *ascii++;
976
977         if (c == '*')
978             c = 10;
979         else if (c == '#')
980             c = 11;
981         else if (c == ',')
982             c = 12;
983         else if (c == 'N')
984             c = 13;
985         else {
986             c -= '0';
987             if ((unsigned)c >= 10U)
988                 return -1;
989         }
990         phase   = (phase << 4) | c;
991         result += 1;
992         if (phase & 0x100) {
993             if (dst) dst[result/2] = (byte_t) phase;
994             phase   = 0x01;
995         }
996     }
997
998     if (result & 1) {
999         if (dst) dst[result/2] = (byte_t)(phase | 0xf0);
1000     }
1001     return result;
1002 }
1003
1004 /** ADN: Abbreviated Dialing Number
1005  **/
1006
1007 #define  ADN_FOOTER_SIZE     14
1008 #define  ADN_OFFSET_NUMBER_LENGTH   0
1009 #define  ADN_OFFSET_TON_NPI         1
1010 #define  ADN_OFFSET_NUMBER_START    2
1011 #define  ADN_OFFSET_NUMBER_END      11
1012 #define  ADN_OFFSET_CAPABILITY_ID   12
1013 #define  ADN_OFFSET_EXTENSION_ID    13
1014
1015 /* see 10.5.1 of 3GPP 51.011 */
1016 static int
1017 sim_adn_alpha_to_utf8( cbytes_t  alpha, cbytes_t  end, bytes_t  dst )
1018 {
1019     int  result = 0;
1020     int  c, is_ucs2 = 0;
1021
1022     /* ignore trailing 0xff */
1023     while (alpha < end && end[-1] == 0xff)
1024         end--;
1025
1026     if (alpha >= end)
1027         return 0;
1028
1029     if (alpha[0] == 0x80) { /* UCS/2 source encoding */
1030         int  count, len;
1031
1032         alpha += 1;
1033         result = ucs2_to_utf8( alpha, (end-alpha)/2, dst );
1034     }
1035     else
1036     {
1037         int  is_ucs2 = 0;
1038         int  len = 0, base = 0;
1039
1040         if (alpha+3 <= end && alpha[0] == 0x81) {
1041             is_ucs2 = 1;
1042             len     = alpha[1];
1043             base    = alpha[2] << 7;
1044             alpha  += 3;
1045             if (len > end-alpha)
1046                 len = end-alpha;
1047         } else if (alpha+4 <= end && alpha[0] == 0x82) {
1048             is_ucs2 = 1;
1049             len     = alpha[1];
1050             base    = (alpha[2] << 8) | alpha[3];
1051             alpha  += 4;
1052             if (len > end-alpha)
1053                 len = end-alpha;
1054         }
1055
1056         if (is_ucs2) {
1057             end = alpha + len;
1058             while (alpha < end) {
1059                 int  c = alpha[0];
1060                 if (c >= 0x80) {
1061                     result += utf8_write(dst, result, base + (c & 0x7f));
1062                     alpha  += 1;
1063                 } else {
1064                     /* GSM character set */
1065                     int   count;
1066                     for (count = 0; alpha+count < end && alpha[count] < 128; count++)
1067                         ;
1068                     result += utf8_from_gsm8(alpha, count, (dst ? dst+result : NULL));
1069                     alpha  += count;
1070                 }
1071             }
1072         }
1073         else {
1074             result = utf8_from_gsm8(alpha, end-alpha, dst);
1075         }
1076     }
1077     return result;
1078 }
1079
1080 static int
1081 sim_adn_alpha_from_utf8( cbytes_t  utf8, int  utf8len, bytes_t  dst )
1082 {
1083     int   result = 0;
1084
1085     if (utf8_check_gsm7(utf8, utf8len)) {
1086         /* GSM 7-bit compatible, encode directly as 8-bit string */
1087         result = utf8_to_gsm8(utf8, utf8len, dst);
1088     } else {
1089         /* otherwise, simply try UCS-2 encoding, nothing more serious at the moment */
1090         if (dst) {
1091             dst[0] = 0x80;
1092         }
1093         result = 1 + utf8_to_ucs2(utf8, utf8len, dst ? (dst+1) : NULL)*2;
1094     }
1095     return  result;
1096 }
1097
1098 int
1099 sim_adn_record_from_bytes( SimAdnRecord  rec, cbytes_t  data, int  len )
1100 {
1101     cbytes_t  end    = data + len;
1102     cbytes_t  footer = end - ADN_FOOTER_SIZE;
1103     int       num_len;
1104
1105     rec->adn.alpha[0]  = 0;
1106     rec->adn.number[0] = 0;
1107     rec->ext_record    = 0xff;
1108
1109     if (len < ADN_FOOTER_SIZE)
1110         return -1;
1111
1112     /* alpha is optional */
1113     if (len > ADN_FOOTER_SIZE) {
1114         cbytes_t  dataend = data + len - ADN_FOOTER_SIZE;
1115         int       count   = sim_adn_alpha_to_utf8(data, dataend, NULL);
1116
1117         if (count > sizeof(rec->adn.alpha)-1)  /* too long */
1118             return -1;
1119
1120         sim_adn_alpha_to_utf8(data, dataend, rec->adn.alpha);
1121         rec->adn.alpha[count] = 0;
1122     }
1123
1124     num_len = footer[ADN_OFFSET_NUMBER_LENGTH];
1125     if (num_len > 11)
1126         return -1;
1127
1128     /* decode TON and number to ASCII, NOTE: this is lossy !! */
1129     {
1130         int      ton    = footer[ADN_OFFSET_TON_NPI];
1131         bytes_t  number = rec->adn.number;
1132         int      len    = sizeof(rec->adn.number)-1;
1133         int      count;
1134
1135         if (ton != 0x81 && ton != 0x91)
1136             return -1;
1137
1138         if (ton == 0x91) {
1139             *number++ = '+';
1140             len      -= 1;
1141         }
1142
1143         count = gsm_bcdnum_to_ascii( footer + ADN_OFFSET_NUMBER_START,
1144                                      num_len*2, number );
1145         number[count] = 0;
1146     }
1147     return 0;
1148 }
1149
1150 int
1151 sim_adn_record_to_bytes( SimAdnRecord  rec, bytes_t   data, int  datalen )
1152 {
1153     bytes_t   end    = data + datalen;
1154     bytes_t   footer = end - ADN_FOOTER_SIZE;
1155     int       ton    = 0x81;
1156     cbytes_t  number = rec->adn.number;
1157     int       len;
1158
1159     if (number[0] == '+') {
1160         ton     = 0x91;
1161         number += 1;
1162     }
1163     footer[0] = (strlen(number)+1)/2 + 1;
1164         return 0;
1165 }