src/simutil.c

   1 /*
   2  *
   3  *  oFono - Open Source Telephony
   4  *
   5  *  Copyright (C) 2008-2011  Intel Corporation. All rights reserved.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License version 2 as
   9  *  published by the Free Software Foundation.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program; if not, write to the Free Software
  18  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #include <string.h>
  27 #include <stdlib.h>
  28
  29 #include <glib.h>
  30
  31 #include <ofono/types.h>
  32 #include "simutil.h"
  33 #include "util.h"
  34 #include "smsutil.h"
  35
  36 struct sim_eons {
  37         struct sim_eons_operator_info *pnn_list;
  38         GSList *opl_list;
  39         gboolean pnn_valid;
  40         int pnn_max;
  41 };
  42
  43 struct spdi_operator {
  44         char mcc[OFONO_MAX_MCC_LENGTH + 1];
  45         char mnc[OFONO_MAX_MNC_LENGTH + 1];
  46 };
  47
  48 struct opl_operator {
  49         char mcc[OFONO_MAX_MCC_LENGTH + 1];
  50         char mnc[OFONO_MAX_MNC_LENGTH + 1];
  51         guint16 lac_tac_low;
  52         guint16 lac_tac_high;
  53         guint8 id;
  54 };
  55
  56 #define MF      1
  57 #define DF      2
  58 #define EF      4
  59
  60 #define BINARY 0
  61 #define RECORD 1
  62 #define CYCLIC 3
  63
  64 #define ALW     0
  65 #define PIN     1
  66 #define PIN2    2
  67 #define ADM     4
  68 #define NEV     15
  69
  70 #define ROOTMF 0x3F00
  71
  72 static struct sim_ef_info ef_db[] = {
  73 {       0x2F05, ROOTMF, ROOTMF, EF, BINARY, 0,          ALW,    PIN     },
  74 {       0x2FE2, ROOTMF, ROOTMF, EF, BINARY, 10,         ALW,    NEV     },
  75 {       0x4F20, 0x5F50, 0x5F50, EF, BINARY, 0,          PIN,    ADM     },
  76 {       0x5F50, 0x7F10, 0x7F10, DF, 0, 0,               PIN,    ADM     },
  77 {       0x6F05, 0x7F20, 0x7FFF, EF, BINARY, 0,          ALW,    PIN     },
  78 {       0x6F07, 0x7F20, 0x7FFF, EF, BINARY, 9,          PIN,    ADM     },
  79 {       0x6F11, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    PIN     },
  80 {       0x6F13, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    PIN     },
  81 {       0x6F14, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    ADM     },
  82 {       0x6F15, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    PIN     },
  83 {       0x6F16, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    ADM     },
  84 {       0x6F17, 0x7F20, 0x7FFF, EF, RECORD, 0,          PIN,    PIN     },
  85 {       0x6F18, 0x7F20, 0x7FFF, EF, BINARY, 10,         PIN,    ADM     },
  86 {       0x6F19, 0x7F20, 0x7FFF, EF, RECORD, 0,          PIN,    PIN     },
  87 {       0x6F38, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    ADM     },
  88 {       0x6F3A, 0x7F10, 0x7F10, EF, RECORD, 0,          PIN,    PIN     },
  89 {       0x6F3B, 0x7F10, 0x7FFF, EF, RECORD, 0,          PIN,    PIN2    },
  90 {       0x6F40, 0x7F10, 0x7FFF, EF, RECORD, 0,          PIN,    PIN     },
  91 {       0x6F45, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    PIN     },
  92 {       0x6F46, 0x7F20, 0x7FFF, EF, BINARY, 17,         ALW,    ADM     },
  93 {       0x6F48, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    ADM     },
  94 {       0x6F49, 0x7F10, 0x7FFF, EF, RECORD, 0,          PIN,    ADM     },
  95 {       0x6F4D, 0x7F20, 0x7FFF, EF, RECORD, 0,          PIN,    PIN2    },
  96 {       0x6F50, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    PIN     },
  97 {       0x6F56, 0x0000, 0x7FFF, EF, BINARY, 0,          PIN,    PIN2    },
  98 {       0x6FAD, 0x7F20, 0x7FFF, EF, BINARY, 0,          ALW,    ADM     },
  99 {       0x6FAE, 0x7F20, 0x0000, EF, BINARY, 1,          ALW,    ADM     },
 100 {       0x6FB7, 0x7F20, 0x7FFF, EF, BINARY, 0,          ALW,    ADM     },
 101 {       0x6FC5, 0x7F20, 0x7FFF, EF, RECORD, 0,          ALW,    ADM     },
 102 {       0x6FC6, 0x7F20, 0x7FFF, EF, RECORD, 0,          ALW,    ADM     },
 103 {       0x6FC7, 0x7F20, 0x7FFF, EF, RECORD, 0,          PIN,    PIN     },
 104 {       0x6FC9, 0x7F20, 0x7FFF, EF, RECORD, 0,          PIN,    PIN     },
 105 {       0x6FCA, 0x7F20, 0x7FFF, EF, RECORD, 0,          PIN,    PIN     },
 106 {       0x6FCB, 0x7F20, 0x7FFF, EF, RECORD, 16,         PIN,    PIN     },
 107 {       0x6FCD, 0x7F20, 0x7FFF, EF, BINARY, 0,          PIN,    ADM     },
 108 {       0x6FDE, 0x7F20, 0x7FFF, EF, BINARY, 0,          ALW,    ADM     },
 109 {       0x7F10, ROOTMF, ROOTMF, DF, 0, 0,               0,      0       },
 110 {       0x7F20, ROOTMF, ROOTMF, DF, 0, 0,               0,      0       },
 111 {       0x7FFF, 0x0000, ROOTMF, DF, 0, 0,               0,      0       }
 112 };
 113
 114 void simple_tlv_iter_init(struct simple_tlv_iter *iter,
 115                                 const unsigned char *pdu, unsigned int len)
 116 {
 117         iter->pdu = pdu;
 118         iter->max = len;
 119         iter->pos = 0;
 120         iter->tag = 0;
 121         iter->len = 0;
 122         iter->data = NULL;
 123 }
 124
 125 gboolean simple_tlv_iter_next(struct simple_tlv_iter *iter)
 126 {
 127         const unsigned char *pdu = iter->pdu + iter->pos;
 128         const unsigned char *end = iter->pdu + iter->max;
 129         unsigned char tag;
 130         unsigned short len;
 131
 132         if (pdu == end)
 133                 return FALSE;
 134
 135         tag = *pdu;
 136         pdu++;
 137
 138         /*
 139          * ISO 7816-4, Section 5.2.1:
 140          *
 141          * The tag field consists of a single byte encoding a tag number from
 142          * 1 to 254.  The values 00 and FF are invalid for tag fields.
 143          *
 144          * The length field consists of one or three consecutive bytes.
 145          *      - If the first byte is not set to FF, then the length field
 146          *        consists of a single byte encoding a number from zero to
 147          *        254 and denoted N.
 148          *      - If the first byte is set to FF, then the length field
 149          *        continues on the subsequent two bytes with any value
 150          *        encoding a number from zero to 65535 and denoted N
 151          *
 152          * If N is zero, there is no value field, i.e. data object is empty.
 153          */
 154         if (pdu == end)
 155                 return FALSE;
 156
 157         len = *pdu++;
 158
 159         if (len == 0xFF) {
 160                 if ((pdu + 2) > end)
 161                         return FALSE;
 162
 163                 len = (pdu[0] << 8) | pdu[1];
 164
 165                 pdu += 2;
 166         }
 167
 168         if (pdu + len > end)
 169                 return FALSE;
 170
 171         iter->tag = tag;
 172         iter->len = len;
 173         iter->data = pdu;
 174
 175         iter->pos = pdu + len - iter->pdu;
 176
 177         return TRUE;
 178 }
 179
 180 unsigned char simple_tlv_iter_get_tag(struct simple_tlv_iter *iter)
 181 {
 182         return iter->tag;
 183 }
 184
 185 unsigned short simple_tlv_iter_get_length(struct simple_tlv_iter *iter)
 186 {
 187         return iter->len;
 188 }
 189
 190 const unsigned char *simple_tlv_iter_get_data(struct simple_tlv_iter *iter)
 191 {
 192         return iter->data;
 193 }
 194
 195 void comprehension_tlv_iter_init(struct comprehension_tlv_iter *iter,
 196                                         const unsigned char *pdu,
 197                                         unsigned int len)
 198 {
 199         iter->pdu = pdu;
 200         iter->max = len;
 201         iter->pos = 0;
 202         iter->tag = 0;
 203         iter->cr = FALSE;
 204         iter->data = 0;
 205 }
 206
 207 /* Comprehension TLVs defined in Section 7 of ETSI TS 101.220 */
 208 gboolean comprehension_tlv_iter_next(struct comprehension_tlv_iter *iter)
 209 {
 210         const unsigned char *pdu = iter->pdu + iter->pos;
 211         const unsigned char *end = iter->pdu + iter->max;
 212         unsigned short tag;
 213         unsigned short len;
 214         gboolean cr;
 215
 216         if (pdu == end)
 217                 return FALSE;
 218
 219         if (*pdu == 0x00 || *pdu == 0xFF || *pdu == 0x80)
 220                 return FALSE;
 221
 222         cr = bit_field(*pdu, 7, 1);
 223         tag = bit_field(*pdu, 0, 7);
 224         pdu++;
 225
 226         /*
 227          * ETSI TS 101.220, Section 7.1.1.2
 228          *
 229          * If byte 1 of the tag is equal to 0x7F, then the tag is encoded
 230          * on the following two bytes, with bit 8 of the 2nd byte of the tag
 231          * being the CR flag.
 232          */
 233         if (tag == 0x7F) {
 234                 if ((pdu + 2) > end)
 235                         return FALSE;
 236
 237                 cr = bit_field(pdu[0], 7, 1);
 238                 tag = ((pdu[0] & 0x7f) << 8) | pdu[1];
 239
 240                 if (tag < 0x0001 || tag > 0x7fff)
 241                         return FALSE;
 242
 243                 pdu += 2;
 244         }
 245
 246         if (pdu == end)
 247                 return FALSE;
 248
 249         len = *pdu++;
 250
 251         if (len >= 0x80) {
 252                 unsigned int extended_bytes = len - 0x80;
 253                 unsigned int i;
 254
 255                 if (extended_bytes == 0 || extended_bytes > 3)
 256                         return FALSE;
 257
 258                 if ((pdu + extended_bytes) > end)
 259                         return FALSE;
 260
 261                 if (pdu[0] == 0)
 262                         return FALSE;
 263
 264                 for (len = 0, i = 0; i < extended_bytes; i++)
 265                         len = (len << 8) | *pdu++;
 266         }
 267
 268         if (pdu + len > end)
 269                 return FALSE;
 270
 271         iter->tag = tag;
 272         iter->cr = cr;
 273         iter->len = len;
 274         iter->data = pdu;
 275
 276         iter->pos = pdu + len - iter->pdu;
 277
 278         return TRUE;
 279 }
 280
 281 unsigned short comprehension_tlv_iter_get_tag(
 282                                         struct comprehension_tlv_iter *iter)
 283 {
 284         return iter->tag;
 285 }
 286
 287 gboolean comprehension_tlv_get_cr(struct comprehension_tlv_iter *iter)
 288 {
 289         return iter->cr;
 290 }
 291
 292 unsigned int comprehension_tlv_iter_get_length(
 293                                         struct comprehension_tlv_iter *iter)
 294 {
 295         return iter->len;
 296 }
 297
 298 const unsigned char *comprehension_tlv_iter_get_data(
 299                                         struct comprehension_tlv_iter *iter)
 300 {
 301         return iter->data;
 302 }
 303
 304 void comprehension_tlv_iter_copy(struct comprehension_tlv_iter *from,
 305                                         struct comprehension_tlv_iter *to)
 306 {
 307         to->max = from->max;
 308         to->pos = from->pos;
 309         to->pdu = from->pdu;
 310         to->tag = from->tag;
 311         to->cr = from->cr;
 312         to->len = from->len;
 313         to->data = from->data;
 314 }
 315
 316 void ber_tlv_iter_init(struct ber_tlv_iter *iter, const unsigned char *pdu,
 317                         unsigned int len)
 318 {
 319         iter->pdu = pdu;
 320         iter->max = len;
 321         iter->pos = 0;
 322 }
 323
 324 unsigned int ber_tlv_iter_get_tag(struct ber_tlv_iter *iter)
 325 {
 326         return iter->tag;
 327 }
 328
 329 enum ber_tlv_data_type ber_tlv_iter_get_class(struct ber_tlv_iter *iter)
 330 {
 331         return iter->class;
 332 }
 333
 334 enum ber_tlv_data_encoding_type
 335         ber_tlv_iter_get_encoding(struct ber_tlv_iter *iter)
 336 {
 337         return iter->encoding;
 338 }
 339
 340 unsigned char ber_tlv_iter_get_short_tag(struct ber_tlv_iter *iter)
 341 {
 342         if (iter->tag > 30)
 343                 return 0;
 344
 345         return iter->tag | (iter->encoding << 5) | (iter->class << 6);
 346 }
 347
 348 unsigned int ber_tlv_iter_get_length(struct ber_tlv_iter *iter)
 349 {
 350         return iter->len;
 351 }
 352
 353 const unsigned char *ber_tlv_iter_get_data(struct ber_tlv_iter *iter)
 354 {
 355         return iter->data;
 356 }
 357
 358 /* BER TLV structure is defined in ISO/IEC 7816-4 */
 359 gboolean ber_tlv_iter_next(struct ber_tlv_iter *iter)
 360 {
 361         const unsigned char *pdu = iter->pdu + iter->pos;
 362         const unsigned char *end = iter->pdu + iter->max;
 363         unsigned int tag;
 364         unsigned int len;
 365         enum ber_tlv_data_type class;
 366         enum ber_tlv_data_encoding_type encoding;
 367
 368         while ((pdu < end) && (*pdu == 0x00 || *pdu == 0xff))
 369                 pdu++;
 370
 371         if (pdu == end)
 372                 return FALSE;
 373
 374         class = bit_field(*pdu, 6, 2);
 375         encoding = bit_field(*pdu, 5, 1);
 376         tag = bit_field(*pdu, 0, 5);
 377
 378         pdu++;
 379
 380         /*
 381          * ISO 7816-4, Section 5.2.2.1:
 382          * "If bits 5 to 1 of the first byte of the tag are not
 383          * all set to 1, then they encode a tag number from zero
 384          * to thirty and the tag field consists of a single byte.
 385          *
 386          * Otherwise, the tag field continues on one or more
 387          * subsequent bytes
 388          *      - Bit 8 of each subsequent byte shall be set to 1,
 389          *        unless it is the last subsequent byte
 390          *      - Bits 7 to 1 of the first subsequent byte shall not be
 391          *        all set to 0
 392          *      - Bits 7 to 1 of the first subsequent byte, followed by
 393          *        bits 7 to 1 of each further subsequent byte, up to
 394          *        and including bits 7 to 1 of the last subsequent
 395          *        byte encode a tag number.
 396          */
 397         if (tag == 0x1f) {
 398                 if (pdu == end)
 399                         return FALSE;
 400
 401                 /* First byte of the extended tag cannot contain 0 */
 402                 if ((*pdu & 0x7f) == 0)
 403                         return FALSE;
 404
 405                 tag = 0;
 406
 407                 while ((pdu < end) && (*pdu & 0x80)) {
 408                         tag = (tag << 7) | (*pdu & 0x7f);
 409                         pdu++;
 410                 }
 411
 412                 if (pdu == end)
 413                         return FALSE;
 414
 415                 tag = (tag << 7) | *pdu;
 416                 pdu++;
 417         }
 418
 419         if (pdu == end)
 420                 return FALSE;
 421
 422         len = *pdu++;
 423
 424         if (len >= 0x80) {
 425                 unsigned int extended_bytes = len - 0x80;
 426                 unsigned int i;
 427
 428                 if (extended_bytes == 0 || extended_bytes > 4)
 429                         return FALSE;
 430
 431                 if ((pdu + extended_bytes) > end)
 432                         return FALSE;
 433
 434                 if (pdu[0] == 0)
 435                         return FALSE;
 436
 437                 for (len = 0, i = 0; i < extended_bytes; i++)
 438                         len = (len << 8) | *pdu++;
 439         }
 440
 441         if (pdu + len > end)
 442                 return FALSE;
 443
 444         iter->tag = tag;
 445         iter->class = class;
 446         iter->encoding = encoding;
 447         iter->len = len;
 448         iter->data = pdu;
 449
 450         iter->pos = pdu + len - iter->pdu;
 451
 452         return TRUE;
 453 }
 454
 455 void ber_tlv_iter_recurse(struct ber_tlv_iter *iter,
 456                                 struct ber_tlv_iter *recurse)
 457 {
 458         recurse->pdu = iter->data;
 459         recurse->max = iter->len;
 460         recurse->pos = 0;
 461 }
 462
 463 void ber_tlv_iter_recurse_simple(struct ber_tlv_iter *iter,
 464                                         struct simple_tlv_iter *container)
 465 {
 466         simple_tlv_iter_init(container, iter->data, iter->len);
 467 }
 468
 469 void ber_tlv_iter_recurse_comprehension(struct ber_tlv_iter *iter,
 470                                         struct comprehension_tlv_iter *recurse)
 471 {
 472         comprehension_tlv_iter_init(recurse, iter->data, iter->len);
 473 }
 474
 475 static const guint8 *ber_tlv_find_by_tag(const guint8 *pdu, guint8 in_tag,
 476                                                 int in_len, int *out_len)
 477 {
 478         struct ber_tlv_iter iter;
 479
 480         ber_tlv_iter_init(&iter, pdu, in_len);
 481
 482         while (ber_tlv_iter_next(&iter)) {
 483                 if (ber_tlv_iter_get_short_tag(&iter) != in_tag)
 484                         continue;
 485
 486                 if (out_len)
 487                         *out_len = ber_tlv_iter_get_length(&iter);
 488
 489                 return ber_tlv_iter_get_data(&iter);
 490         }
 491
 492         return NULL;
 493 }
 494
 495 #define MAX_BER_TLV_HEADER 8
 496
 497 gboolean ber_tlv_builder_init(struct ber_tlv_builder *builder,
 498                                 unsigned char *pdu, unsigned int size)
 499 {
 500         if (size < MAX_BER_TLV_HEADER)
 501                 return FALSE;
 502
 503         builder->pdu = pdu;
 504         builder->pos = 0;
 505         builder->max = size;
 506         builder->parent = NULL;
 507         builder->tag = 0xff;
 508         builder->len = 0;
 509
 510         return TRUE;
 511 }
 512
 513 #define BTLV_LEN_FIELD_SIZE_NEEDED(a)                           \
 514         ((a) <= 0x7f ? 1 :                                      \
 515                 ((a) <= 0xff ? 2 :                              \
 516                         ((a) <= 0xffff ? 3 :                    \
 517                                 ((a) <= 0xffffff ? 4 : 5))))
 518
 519 #define BTLV_TAG_FIELD_SIZE_NEEDED(a)                           \
 520         ((a) <= 0x1e ? 1 :                                      \
 521                 ((a) <= 0x7f ? 2 : 3))
 522
 523 static void ber_tlv_builder_write_header(struct ber_tlv_builder *builder)
 524 {
 525         int tag_size = BTLV_TAG_FIELD_SIZE_NEEDED(builder->tag);
 526         int len_size = BTLV_LEN_FIELD_SIZE_NEEDED(builder->len);
 527         int offset = MAX_BER_TLV_HEADER - tag_size - len_size;
 528         unsigned char *pdu = builder->pdu + builder->pos;
 529
 530         /* Pad with stuff bytes */
 531         memset(pdu, 0xff, offset);
 532
 533         /* Write the tag */
 534         pdu[offset++] = (builder->class << 6) |
 535                                 (builder->encoding << 5) |
 536                                         (tag_size == 1 ? builder->tag : 0x1f);
 537
 538         if (tag_size == 3)
 539                 pdu[offset++] = 0x80 | (builder->tag >> 7);
 540
 541         if (tag_size > 2)
 542                 pdu[offset++] = builder->tag & 0x7f;
 543
 544         /* Write the length */
 545         if (len_size > 1) {
 546                 int i;
 547
 548                 pdu[offset++] = 0x80 + len_size - 1;
 549
 550                 for (i = len_size - 2; i >= 0; i--)
 551                         pdu[offset++] = (builder->len >> (i * 8)) & 0xff;
 552         } else
 553                 pdu[offset++] = builder->len;
 554 }
 555
 556 gboolean ber_tlv_builder_next(struct ber_tlv_builder *builder,
 557                                 enum ber_tlv_data_type class,
 558                                 enum ber_tlv_data_encoding_type encoding,
 559                                 unsigned int new_tag)
 560 {
 561         if (builder->tag != 0xff) {
 562                 ber_tlv_builder_write_header(builder);
 563                 builder->pos += MAX_BER_TLV_HEADER + builder->len;
 564         }
 565
 566         if (ber_tlv_builder_set_length(builder, 0) == FALSE)
 567                 return FALSE;
 568
 569         builder->class = class;
 570         builder->encoding = encoding;
 571         builder->tag = new_tag;
 572
 573         return TRUE;
 574 }
 575
 576 /*
 577  * Resize the TLV because the content of Value field needs more space.
 578  * If this TLV is part of another TLV, resize that one too.
 579  */
 580 gboolean ber_tlv_builder_set_length(struct ber_tlv_builder *builder,
 581                                         unsigned int new_len)
 582 {
 583         unsigned int new_pos = builder->pos + MAX_BER_TLV_HEADER + new_len;
 584
 585         if (new_pos > builder->max)
 586                 return FALSE;
 587
 588         if (builder->parent)
 589                 ber_tlv_builder_set_length(builder->parent, new_pos);
 590
 591         builder->len = new_len;
 592
 593         return TRUE;
 594 }
 595
 596 unsigned char *ber_tlv_builder_get_data(struct ber_tlv_builder *builder)
 597 {
 598         return builder->pdu + builder->pos + MAX_BER_TLV_HEADER;
 599 }
 600
 601 gboolean ber_tlv_builder_recurse(struct ber_tlv_builder *builder,
 602                                         struct ber_tlv_builder *recurse)
 603 {
 604         unsigned char *end = builder->pdu + builder->max;
 605         unsigned char *data = ber_tlv_builder_get_data(builder);
 606
 607         if (ber_tlv_builder_init(recurse, data, end - data) == FALSE)
 608                 return FALSE;
 609
 610         recurse->parent = builder;
 611
 612         return TRUE;
 613 }
 614
 615 gboolean ber_tlv_builder_recurse_comprehension(struct ber_tlv_builder *builder,
 616                                 struct comprehension_tlv_builder *recurse)
 617 {
 618         unsigned char *end = builder->pdu + builder->max;
 619         unsigned char *data = ber_tlv_builder_get_data(builder);
 620
 621         if (comprehension_tlv_builder_init(recurse, data, end - data) == FALSE)
 622                 return FALSE;
 623
 624         recurse->parent = builder;
 625
 626         return TRUE;
 627 }
 628
 629 void ber_tlv_builder_optimize(struct ber_tlv_builder *builder,
 630                                 unsigned char **out_pdu, unsigned int *out_len)
 631 {
 632         unsigned int len;
 633         unsigned char *pdu;
 634
 635         ber_tlv_builder_write_header(builder);
 636
 637         len = builder->pos + MAX_BER_TLV_HEADER + builder->len;
 638
 639         for (pdu = builder->pdu; *pdu == 0xff; pdu++)
 640                 len--;
 641
 642         if (out_pdu)
 643                 *out_pdu = pdu;
 644
 645         if (out_len)
 646                 *out_len = len;
 647 }
 648
 649 gboolean comprehension_tlv_builder_init(
 650                                 struct comprehension_tlv_builder *builder,
 651                                 unsigned char *pdu, unsigned int size)
 652 {
 653         if (size < 2)
 654                 return FALSE;
 655
 656         builder->pdu = pdu;
 657         builder->pos = 0;
 658         builder->max = size;
 659         builder->parent = NULL;
 660         builder->len = 0;
 661
 662         builder->pdu[0] = 0;
 663
 664         return TRUE;
 665 }
 666
 667 #define CTLV_TAG_FIELD_SIZE(a)                  \
 668         bit_field((a), 0, 7) == 0x7f ? 3 : 1    \
 669
 670 #define CTLV_LEN_FIELD_SIZE(a)                  \
 671         (a) >= 0x80 ? (a) - 0x7f : 1            \
 672
 673 gboolean comprehension_tlv_builder_next(
 674                                 struct comprehension_tlv_builder *builder,
 675                                 gboolean cr, unsigned short tag)
 676 {
 677         unsigned char *tlv = builder->pdu + builder->pos;
 678         unsigned int prev_size = 0;
 679         unsigned int new_size;
 680
 681         /* Tag is invalid when we start, means we've just been inited */
 682         if (tlv[0] != 0) {
 683                 unsigned int tag_size = CTLV_TAG_FIELD_SIZE(tlv[0]);
 684                 prev_size = builder->len + tag_size;
 685                 prev_size += CTLV_LEN_FIELD_SIZE(tlv[tag_size]);
 686         }
 687
 688         new_size = (tag < 0x7f ? 1 : 3) + 1;
 689
 690         if (builder->pos + prev_size + new_size > builder->max)
 691                 return FALSE;
 692
 693         builder->pos += prev_size;
 694
 695         if (tag >= 0x7f) {
 696                 builder->pdu[builder->pos + 0] = 0x7f;
 697                 builder->pdu[builder->pos + 1] = (cr ? 0x80 : 0) | (tag >> 8);
 698                 builder->pdu[builder->pos + 2] = tag & 0xff;
 699         } else
 700                 builder->pdu[builder->pos + 0] = (cr ? 0x80 : 0x00) | tag;
 701
 702         builder->len = 0;
 703         builder->pdu[builder->pos + new_size - 1] = 0; /* Length */
 704
 705         return TRUE;
 706 }
 707
 708 /*
 709  * Resize the TLV because the content of Value field needs more space.
 710  * If this TLV is part of another TLV, resize that one too.
 711  */
 712 gboolean comprehension_tlv_builder_set_length(
 713                                 struct comprehension_tlv_builder *builder,
 714                                 unsigned int new_len)
 715 {
 716         unsigned char *tlv = builder->pdu + builder->pos;
 717         unsigned int tag_size = CTLV_TAG_FIELD_SIZE(tlv[0]);
 718         unsigned int len_size, new_len_size;
 719         unsigned int new_ctlv_len;
 720         unsigned int len;
 721
 722         len_size = CTLV_LEN_FIELD_SIZE(tlv[tag_size]);
 723         new_len_size = BTLV_LEN_FIELD_SIZE_NEEDED(new_len);
 724         new_ctlv_len = tag_size + new_len_size + new_len;
 725
 726         /* Check there is enough space */
 727         if (builder->pos + new_ctlv_len > builder->max)
 728                 return FALSE;
 729
 730         if (builder->parent)
 731                 ber_tlv_builder_set_length(builder->parent,
 732                                                 builder->pos + new_ctlv_len);
 733
 734         len = MIN(builder->len, new_len);
 735         if (len > 0 && new_len_size != len_size)
 736                 memmove(tlv + tag_size + new_len_size,
 737                                 tlv + tag_size + len_size, len);
 738
 739         builder->len = new_len;
 740
 741         /* Write new length */
 742         if (new_len_size > 1) {
 743                 int i;
 744                 unsigned int offset = tag_size;
 745
 746                 tlv[offset++] = 0x80 + new_len_size - 1;
 747
 748                 for (i = new_len_size - 2; i >= 0; i--)
 749                         tlv[offset++] = (builder->len >> (i * 8)) & 0xff;
 750         } else
 751                 tlv[tag_size] = builder->len;
 752
 753         return TRUE;
 754 }
 755
 756 unsigned char *comprehension_tlv_builder_get_data(
 757                                 struct comprehension_tlv_builder *builder)
 758 {
 759         unsigned char *tlv = builder->pdu + builder->pos;
 760         unsigned int tag_size = CTLV_TAG_FIELD_SIZE(*tlv);
 761         unsigned int len_size = CTLV_LEN_FIELD_SIZE(tlv[tag_size]);
 762
 763         return tlv + tag_size + len_size;
 764 }
 765
 766 static char *sim_network_name_parse(const unsigned char *buffer, int length,
 767                                         gboolean *add_ci)
 768 {
 769         char *ret = NULL;
 770         unsigned char *endp;
 771         unsigned char dcs;
 772         int i;
 773         gboolean ci = FALSE;
 774
 775         if (length < 1)
 776                 return NULL;
 777
 778         dcs = *buffer++;
 779         length--;
 780
 781         /*
 782          * "The MS should add the letters for the Country's Initials and a
 783          * separator (e.g. a space)"
 784          */
 785         if (is_bit_set(dcs, 4))
 786                 ci = TRUE;
 787
 788         switch (dcs & (7 << 4)) {
 789         case 0x00:
 790                 endp = memchr(buffer, 0xff, length);
 791                 if (endp)
 792                         length = endp - buffer;
 793                 ret = convert_gsm_to_utf8(buffer, length,
 794                                 NULL, NULL, 0xff);
 795                 break;
 796         case 0x10:
 797                 if ((length % 2) == 1) {
 798                         if (buffer[length - 1] != 0xff)
 799                                 return NULL;
 800
 801                         length = length - 1;
 802                 }
 803
 804                 for (i = 0; i < length; i += 2)
 805                         if (buffer[i] == 0xff && buffer[i + 1] == 0xff)
 806                                 break;
 807
 808                 ret = g_convert((const char *) buffer, length,
 809                                         "UTF-8//TRANSLIT", "UCS-2BE",
 810                                         NULL, NULL, NULL);
 811                 break;
 812         }
 813
 814         if (add_ci)
 815                 *add_ci = ci;
 816
 817         return ret;
 818 }
 819
 820 void sim_parse_mcc_mnc(const guint8 *bcd, char *mcc, char *mnc)
 821 {
 822         static const char digit_lut[] = "0123456789*#abd\0";
 823         guint8 digit;
 824
 825         digit = (bcd[0] >> 0) & 0xf;
 826         *mcc++ = digit_lut[digit];
 827
 828         digit = (bcd[0] >> 4) & 0xf;
 829         *mcc++ = digit_lut[digit];
 830
 831         digit = (bcd[1] >> 0) & 0xf;
 832         *mcc++ = digit_lut[digit];
 833
 834         digit = (bcd[2] >> 0) & 0xf;
 835         *mnc++ = digit_lut[digit];
 836
 837         digit = (bcd[2] >> 4) & 0xf;
 838         *mnc++ = digit_lut[digit];
 839
 840         digit = (bcd[1] >> 4) & 0xf;
 841         *mnc++ = digit_lut[digit];
 842 }
 843
 844 static inline int to_semi_oct(char in)
 845 {
 846         int digit;
 847
 848         switch (in) {
 849         case '0':
 850         case '1':
 851         case '2':
 852         case '3':
 853         case '4':
 854         case '5':
 855         case '6':
 856         case '7':
 857         case '8':
 858         case '9':
 859                 digit = in - '0';
 860                 break;
 861         case '*':
 862                 digit = 10;
 863                 break;
 864         case '#':
 865                 digit = 11;
 866                 break;
 867         case 'C':
 868         case 'c':
 869                 digit = 12;
 870                 break;
 871         case '?':
 872                 digit = 13;
 873                 break;
 874         case 'E':
 875         case 'e':
 876                 digit = 14;
 877                 break;
 878         default:
 879                 digit = -1;
 880                 break;
 881         }
 882
 883         return digit;
 884 }
 885
 886 void sim_encode_mcc_mnc(guint8 *out, const char *mcc, const char *mnc)
 887 {
 888         out[0] = to_semi_oct(mcc[0]);
 889         out[0] |= to_semi_oct(mcc[1]) << 4;
 890
 891         out[1] = mcc[2] ? to_semi_oct(mcc[2]) : 0xf;
 892         out[1] |= (mnc[2] ? to_semi_oct(mnc[2]) : 0xf) << 4;
 893
 894         out[2] = to_semi_oct(mnc[0]);
 895         out[2] |= to_semi_oct(mnc[1]) << 4;
 896 }
 897
 898 static gint spdi_operator_compare(gconstpointer a, gconstpointer b)
 899 {
 900         const struct spdi_operator *opa = a;
 901         const struct spdi_operator *opb = b;
 902         gint r = strcmp(opa->mcc, opb->mcc);
 903
 904         if (r)
 905                 return r;
 906
 907         return strcmp(opa->mnc, opb->mnc);
 908 }
 909
 910 struct sim_spdi {
 911         GSList *operators;
 912 };
 913
 914 struct sim_spdi *sim_spdi_new(const guint8 *tlv, int length)
 915 {
 916         const guint8 *plmn_list_tlv;
 917         const guint8 *plmn_list;
 918         struct sim_spdi *spdi;
 919         struct spdi_operator *oper;
 920         int tlv_length;
 921         int list_length;
 922
 923         if (length < 7)
 924                 return NULL;
 925
 926         plmn_list_tlv = ber_tlv_find_by_tag(tlv, 0xA3, length, &tlv_length);
 927
 928         if (plmn_list_tlv == NULL)
 929                 return NULL;
 930
 931         plmn_list = ber_tlv_find_by_tag(plmn_list_tlv, 0x80, tlv_length,
 932                                                 &list_length);
 933
 934         if (plmn_list == NULL)
 935                 return NULL;
 936
 937         spdi = g_new0(struct sim_spdi, 1);
 938
 939         for (list_length /= 3; list_length--; plmn_list += 3) {
 940                 if ((plmn_list[0] & plmn_list[1] & plmn_list[2]) == 0xff)
 941                         continue;
 942
 943                 oper = g_new0(struct spdi_operator, 1);
 944
 945                 sim_parse_mcc_mnc(plmn_list, oper->mcc, oper->mnc);
 946                 spdi->operators = g_slist_insert_sorted(spdi->operators, oper,
 947                                                 spdi_operator_compare);
 948         }
 949
 950         return spdi;
 951 }
 952
 953 gboolean sim_spdi_lookup(struct sim_spdi *spdi,
 954                                 const char *mcc, const char *mnc)
 955 {
 956         struct spdi_operator spdi_op;
 957
 958         if (spdi == NULL)
 959                 return FALSE;
 960
 961         g_strlcpy(spdi_op.mcc, mcc, sizeof(spdi_op.mcc));
 962         g_strlcpy(spdi_op.mnc, mnc, sizeof(spdi_op.mnc));
 963
 964         return g_slist_find_custom(spdi->operators, &spdi_op,
 965                                         spdi_operator_compare) != NULL;
 966 }
 967
 968 void sim_spdi_free(struct sim_spdi *spdi)
 969 {
 970         if (spdi == NULL)
 971                 return;
 972
 973         g_slist_foreach(spdi->operators, (GFunc)g_free, NULL);
 974         g_slist_free(spdi->operators);
 975         g_free(spdi);
 976 }
 977
 978 static void pnn_operator_free(struct sim_eons_operator_info *oper)
 979 {
 980         if (oper == NULL)
 981                 return;
 982
 983         g_free(oper->info);
 984         g_free(oper->shortname);
 985         g_free(oper->longname);
 986 }
 987
 988 struct sim_eons *sim_eons_new(int pnn_records)
 989 {
 990         struct sim_eons *eons = g_new0(struct sim_eons, 1);
 991
 992         eons->pnn_list = g_new0(struct sim_eons_operator_info, pnn_records);
 993         eons->pnn_max = pnn_records;
 994
 995         return eons;
 996 }
 997
 998 gboolean sim_eons_pnn_is_empty(struct sim_eons *eons)
 999 {
1000         return !eons->pnn_valid;
1001 }
1002
1003 void sim_eons_add_pnn_record(struct sim_eons *eons, int record,
1004                                 const guint8 *tlv, int length)
1005 {
1006         const unsigned char *name;
1007         int namelength;
1008         struct sim_eons_operator_info *oper = &eons->pnn_list[record-1];
1009
1010         name = ber_tlv_find_by_tag(tlv, 0x43, length, &namelength);
1011
1012         if (name == NULL || !namelength)
1013                 return;
1014
1015         oper->longname = sim_network_name_parse(name, namelength,
1016                                                 &oper->long_ci);
1017
1018         name = ber_tlv_find_by_tag(tlv, 0x45, length, &namelength);
1019
1020         if (name && namelength)
1021                 oper->shortname = sim_network_name_parse(name, namelength,
1022                                                         &oper->short_ci);
1023
1024         name = ber_tlv_find_by_tag(tlv, 0x80, length, &namelength);
1025
1026         if (name && namelength)
1027                 oper->info = sim_string_to_utf8(name, namelength);
1028
1029         eons->pnn_valid = TRUE;
1030 }
1031
1032 static struct opl_operator *opl_operator_alloc(const guint8 *record)
1033 {
1034         struct opl_operator *oper = g_new0(struct opl_operator, 1);
1035
1036         sim_parse_mcc_mnc(record, oper->mcc, oper->mnc);
1037         record += 3;
1038
1039         oper->lac_tac_low = (record[0] << 8) | record[1];
1040         record += 2;
1041         oper->lac_tac_high = (record[0] << 8) | record[1];
1042         record += 2;
1043
1044         oper->id = record[0];
1045
1046         return oper;
1047 }
1048
1049 void sim_eons_add_opl_record(struct sim_eons *eons,
1050                                 const guint8 *contents, int length)
1051 {
1052         struct opl_operator *oper;
1053
1054         oper = opl_operator_alloc(contents);
1055
1056         if (oper->id > eons->pnn_max) {
1057                 g_free(oper);
1058                 return;
1059         }
1060
1061         eons->opl_list = g_slist_prepend(eons->opl_list, oper);
1062 }
1063
1064 void sim_eons_optimize(struct sim_eons *eons)
1065 {
1066         eons->opl_list = g_slist_reverse(eons->opl_list);
1067 }
1068
1069 void sim_eons_free(struct sim_eons *eons)
1070 {
1071         int i;
1072
1073         if (eons == NULL)
1074                 return;
1075
1076         for (i = 0; i < eons->pnn_max; i++)
1077                 pnn_operator_free(eons->pnn_list + i);
1078
1079         g_free(eons->pnn_list);
1080
1081         g_slist_foreach(eons->opl_list, (GFunc)g_free, NULL);
1082         g_slist_free(eons->opl_list);
1083
1084         g_free(eons);
1085 }
1086
1087 static const struct sim_eons_operator_info *
1088         sim_eons_lookup_common(struct sim_eons *eons,
1089                                 const char *mcc, const char *mnc,
1090                                 gboolean have_lac, guint16 lac)
1091 {
1092         GSList *l;
1093         const struct opl_operator *opl;
1094         int i;
1095
1096         for (l = eons->opl_list; l; l = l->next) {
1097                 opl = l->data;
1098
1099                 for (i = 0; i < OFONO_MAX_MCC_LENGTH; i++)
1100                         if (mcc[i] != opl->mcc[i] &&
1101                                         !(opl->mcc[i] == 'b' && mcc[i]))
1102                                 break;
1103                 if (i < OFONO_MAX_MCC_LENGTH)
1104                         continue;
1105
1106                 for (i = 0; i < OFONO_MAX_MNC_LENGTH; i++)
1107                         if (mnc[i] != opl->mnc[i] &&
1108                                         !(opl->mnc[i] == 'b' && mnc[i]))
1109                                 break;
1110                 if (i < OFONO_MAX_MNC_LENGTH)
1111                         continue;
1112
1113                 if (opl->lac_tac_low == 0 && opl->lac_tac_high == 0xfffe)
1114                         break;
1115
1116                 if (have_lac == FALSE)
1117                         continue;
1118
1119                 if ((lac >= opl->lac_tac_low) && (lac <= opl->lac_tac_high))
1120                         break;
1121         }
1122
1123         if (l == NULL)
1124                 return NULL;
1125
1126         opl = l->data;
1127
1128         /* 0 is not a valid record id */
1129         if (opl->id == 0)
1130                 return NULL;
1131
1132         return &eons->pnn_list[opl->id - 1];
1133 }
1134
1135 const struct sim_eons_operator_info *sim_eons_lookup(struct sim_eons *eons,
1136                                                 const char *mcc,
1137                                                 const char *mnc)
1138 {
1139         return sim_eons_lookup_common(eons, mcc, mnc, FALSE, 0);
1140 }
1141
1142 const struct sim_eons_operator_info *sim_eons_lookup_with_lac(
1143                                                         struct sim_eons *eons,
1144                                                         const char *mcc,
1145                                                         const char *mnc,
1146                                                         guint16 lac)
1147 {
1148         return sim_eons_lookup_common(eons, mcc, mnc, TRUE, lac);
1149 }
1150
1151 /*
1152  * Extract extended BCD format defined in 3GPP 11.11, 31.102.  The format
1153  * is different from what is defined in 3GPP 24.008 and 23.040 (sms).
1154  *
1155  * Here the digits with values 'C', 'D' and 'E' are treated differently,
1156  * for more details see 31.102 Table 4.4
1157  *
1158  * 'C' - DTMF Control Digit Separator, represented as 'c' by this function
1159  * 'D' - Wild Value, represented as a '?' by this function
1160  * 'E' - RFU, used to be used as a Shift Operator in 11.11
1161  * 'F' - Endmark
1162  *
1163  * Note that a second or subsequent 'C' BCD value will be interpreted as a
1164  * 3 second pause.
1165  */
1166 void sim_extract_bcd_number(const unsigned char *buf, int len, char *out)
1167 {
1168         static const char digit_lut[] = "0123456789*#c?e\0";
1169         unsigned char oct;
1170         int i;
1171
1172         for (i = 0; i < len; i++) {
1173                 oct = buf[i];
1174
1175                 out[i*2] = digit_lut[oct & 0x0f];
1176                 out[i*2+1] = digit_lut[(oct & 0xf0) >> 4];
1177         }
1178
1179         out[i*2] = '\0';
1180 }
1181
1182 void sim_encode_bcd_number(const char *number, unsigned char *out)
1183 {
1184         while (number[0] != '\0' && number[1] != '\0') {
1185                 *out = to_semi_oct(*number++);
1186                 *out++ |= to_semi_oct(*number++) << 4;
1187         }
1188
1189         if (*number)
1190                 *out = to_semi_oct(*number) | 0xf0;
1191 }
1192
1193 gboolean sim_adn_parse(const unsigned char *data, int length,
1194                         struct ofono_phone_number *ph, char **identifier)
1195 {
1196         int number_len;
1197         int ton_npi;
1198         const unsigned char *alpha;
1199         int alpha_length;
1200
1201         if (length < 14)
1202                 return FALSE;
1203
1204         alpha = data;
1205         alpha_length = length - 14;
1206
1207         data += alpha_length;
1208
1209         number_len = *data++;
1210         ton_npi = *data++;
1211
1212         if (number_len > 11 || ton_npi == 0xff)
1213                 return FALSE;
1214
1215         ph->type = ton_npi;
1216
1217         /* BCD coded, however the TON/NPI is given by the first byte */
1218         number_len -= 1;
1219         sim_extract_bcd_number(data, number_len, ph->number);
1220
1221         if (identifier == NULL)
1222                 return TRUE;
1223
1224         /* Alpha-Identifier field */
1225         if (alpha_length > 0)
1226                 *identifier = sim_string_to_utf8(alpha, alpha_length);
1227         else
1228                 *identifier = NULL;
1229
1230         return TRUE;
1231 }
1232
1233 void sim_adn_build(unsigned char *data, int length,
1234                         const struct ofono_phone_number *ph,
1235                         const char *identifier)
1236 {
1237         int number_len = strlen(ph->number);
1238         unsigned char *alpha = NULL;
1239         int alpha_written = 0;
1240         int alpha_length;
1241
1242         alpha_length = length - 14;
1243
1244         /* Alpha-Identifier field */
1245         if (alpha_length > 0) {
1246                 if (identifier)
1247                         alpha = utf8_to_sim_string(identifier, alpha_length,
1248                                                         &alpha_written);
1249                 if (alpha) {
1250                         memcpy(data, alpha, alpha_written);
1251                         g_free(alpha);
1252                 }
1253
1254                 memset(data + alpha_written, 0xff,
1255                                 alpha_length - alpha_written);
1256                 data += alpha_length;
1257         }
1258
1259         number_len = (number_len + 1) / 2;
1260         *data++ = number_len + 1;
1261         *data++ = ph->type;
1262
1263         sim_encode_bcd_number(ph->number, data);
1264         memset(data + number_len, 0xff, 10 - number_len);
1265         data += 10;
1266
1267         /* CCP1 unused */
1268         *data++ = 0xff;
1269         /* Ext1 unused */
1270         *data++ = 0xff;
1271 }
1272
1273 static int find_ef_by_id(const void *key, const void *value)
1274 {
1275         unsigned short id = GPOINTER_TO_UINT(key);
1276         const struct sim_ef_info *info = value;
1277
1278         return id - info->id;
1279 }
1280
1281 struct sim_ef_info *sim_ef_db_lookup(unsigned short id)
1282 {
1283         struct sim_ef_info *result;
1284         unsigned int nelem = sizeof(ef_db) / sizeof(struct sim_ef_info);
1285
1286         result = bsearch(GUINT_TO_POINTER((unsigned int) id), ef_db, nelem,
1287                                 sizeof(struct sim_ef_info), find_ef_by_id);
1288
1289         return result;
1290 }
1291
1292 unsigned int sim_ef_db_get_path_2g(unsigned short id, unsigned char out_path[])
1293 {
1294         struct sim_ef_info *info;
1295         unsigned int nelem = sizeof(ef_db) / sizeof(struct sim_ef_info);
1296         unsigned char path[6];
1297         int i = 0;
1298         int j;
1299
1300         info = bsearch(GUINT_TO_POINTER((unsigned int) id), ef_db, nelem,
1301                                 sizeof(struct sim_ef_info), find_ef_by_id);
1302         if (info == NULL)
1303                 return 0;
1304
1305         path[i++] = info->parent2g & 0xff;
1306         path[i++] = info->parent2g >> 8;
1307
1308         while (info->parent2g != ROOTMF) {
1309                 info = bsearch(GUINT_TO_POINTER((unsigned int) info->parent2g),
1310                                 ef_db, nelem, sizeof(struct sim_ef_info),
1311                                 find_ef_by_id);
1312                 if (info == NULL)
1313                         return 0;
1314
1315                 path[i++] = info->parent2g & 0xff;
1316                 path[i++] = info->parent2g >> 8;
1317         }
1318
1319         for (j = 0; j < i; j++)
1320                 out_path[j] = path[i - j - 1];
1321
1322         return i;
1323 }
1324
1325 unsigned int sim_ef_db_get_path_3g(unsigned short id, unsigned char out_path[])
1326 {
1327         struct sim_ef_info *info;
1328         unsigned int nelem = sizeof(ef_db) / sizeof(struct sim_ef_info);
1329         unsigned char path[6];
1330         int i = 0;
1331         int j;
1332
1333         info = bsearch(GUINT_TO_POINTER((unsigned int) id), ef_db, nelem,
1334                                 sizeof(struct sim_ef_info), find_ef_by_id);
1335         if (info == NULL)
1336                 return 0;
1337
1338         path[i++] = info->parent3g & 0xff;
1339         path[i++] = info->parent3g >> 8;
1340
1341         while (info->parent3g != ROOTMF) {
1342                 info = bsearch(GUINT_TO_POINTER((unsigned int) info->parent3g),
1343                                 ef_db, nelem, sizeof(struct sim_ef_info),
1344                                 find_ef_by_id);
1345                 if (info == NULL)
1346                         return 0;
1347
1348                 path[i++] = info->parent3g & 0xff;
1349                 path[i++] = info->parent3g >> 8;
1350         }
1351
1352         for (j = 0; j < i; j++)
1353                 out_path[j] = path[i - j - 1];
1354
1355         return i;
1356 }
1357
1358 gboolean sim_parse_3g_get_response(const unsigned char *data, int len,
1359                                         int *file_len, int *record_len,
1360                                         int *structure, unsigned char *access,
1361                                         unsigned short *efid)
1362 {
1363         const unsigned char *fcp;
1364         int fcp_length;
1365         const unsigned char *tlv;
1366         int tlv_length;
1367         int i;
1368         int flen, rlen, str;
1369         unsigned short id;
1370         unsigned char acc[3];
1371         struct sim_ef_info *info;
1372
1373         fcp = ber_tlv_find_by_tag(data, 0x62, len, &fcp_length);
1374
1375         if (fcp == NULL)
1376                 return FALSE;
1377
1378         /*
1379          * Find the file size tag 0x80 according to
1380          * ETSI 102.221 Section 11.1.1.3.2
1381          */
1382         tlv = ber_tlv_find_by_tag(fcp, 0x80, fcp_length, &tlv_length);
1383
1384         if (tlv == NULL || tlv_length < 2)
1385                 return FALSE;
1386
1387         flen = tlv[0];
1388         for (i = 1; i < tlv_length; i++)
1389                 flen = (flen << 8) | tlv[i];
1390
1391         tlv = ber_tlv_find_by_tag(fcp, 0x83, fcp_length, &tlv_length);
1392
1393         if (tlv == NULL || tlv_length != 2)
1394                 return FALSE;
1395
1396         id = (tlv[0] << 8) | tlv[1];
1397
1398         tlv = ber_tlv_find_by_tag(fcp, 0x82, fcp_length, &tlv_length);
1399
1400         if (tlv == NULL || (tlv_length != 2 && tlv_length != 5))
1401                 return FALSE;
1402
1403         if (tlv[1] != 0x21)
1404                 return FALSE;
1405
1406         switch (tlv[0] & 0x3) {
1407         case 1: /* Transparent */
1408                 str = 0x00;
1409                 break;
1410         case 2: /* Linear Fixed */
1411                 str = 0x01;
1412                 break;
1413         case 6: /* Cyclic */
1414                 str = 0x03;
1415                 break;
1416         default:
1417                 return FALSE;
1418         };
1419
1420         /* For cyclic or linear fixed we need record size & num records */
1421         if (str != 0x00 && tlv_length != 5)
1422                 return FALSE;
1423
1424         /*
1425          * strictly speaking the record length is 16 bit, but the valid
1426          * range is 0x01 to 0xFF according to 102.221
1427          */
1428         if (str != 0x00)
1429                 rlen = tlv[3];
1430         else
1431                 rlen = 0;
1432
1433         /*
1434          * The 3G response data contains references to EFarr which actually
1435          * contains the security attributes.  These are usually not carried
1436          * along with the response data unlike in 2G.  Instead of querying
1437          * this, we simply look it up in our database.  We fudge it somewhat
1438          * and guess if the file isn't found.
1439          */
1440         info = sim_ef_db_lookup(id);
1441
1442         if (str == 0x03)
1443                 acc[1] = 0x1f;
1444         else
1445                 acc[1] = 0xff;
1446
1447         acc[2] = 0x44;
1448
1449         if (info == NULL)
1450                 acc[0] = 0x11;
1451         else
1452                 acc[0] = (info->perm_read << 4) | info->perm_update;
1453
1454         if (file_len)
1455                 *file_len = flen;
1456
1457         if (record_len)
1458                 *record_len = rlen;
1459
1460         if (efid)
1461                 *efid = id;
1462
1463         if (structure)
1464                 *structure = str;
1465
1466         if (access)
1467                 memcpy(access, acc, 3);
1468
1469         return TRUE;
1470 }
1471
1472 gboolean sim_parse_2g_get_response(const unsigned char *response, int len,
1473                                         int *file_len, int *record_len,
1474                                         int *structure, unsigned char *access,
1475                                         unsigned char *file_status)
1476 {
1477         if (len < 14 || response[6] != 0x04)
1478                 return FALSE;
1479
1480         if ((response[13] == 0x01 || response[13] == 0x03) && len < 15)
1481                 return FALSE;
1482
1483         *file_len = (response[2] << 8) | response[3];
1484         *structure = response[13];
1485
1486         access[0] = response[8];
1487         access[1] = response[9];
1488         access[2] = response[10];
1489
1490         *file_status = response[11];
1491
1492         if (response[13] == 0x01 || response[13] == 0x03)
1493                 *record_len = response[14];
1494         else
1495                 *record_len = 0;
1496
1497         return TRUE;
1498 }
1499
1500 gboolean sim_ust_is_available(unsigned char *efust, unsigned char len,
1501                                                 enum sim_ust_service index)
1502 {
1503         if (index >= len * 8u)
1504                 return FALSE;
1505
1506         return (efust[index / 8] >> (index % 8)) & 1;
1507 }
1508
1509 gboolean sim_est_is_active(unsigned char *efest, unsigned char len,
1510                                                 enum sim_est_service index)
1511 {
1512         if (index >= len * 8u)
1513                 return FALSE;
1514
1515         return (efest[index / 8] >> (index % 8)) & 1;
1516 }
1517
1518 gboolean sim_sst_is_available(unsigned char *efsst, unsigned char len,
1519                                                 enum sim_sst_service index)
1520 {
1521         if (index >= len * 4u)
1522                 return FALSE;
1523
1524         return (efsst[index / 4] >> ((index % 4) * 2)) & 1;
1525 }
1526
1527 gboolean sim_sst_is_active(unsigned char *efsst, unsigned char len,
1528                                                 enum sim_sst_service index)
1529 {
1530         if (index >= len * 4u)
1531                 return FALSE;
1532
1533         return (efsst[index / 4] >> (((index % 4) * 2) + 1)) & 1;
1534 }
1535
1536 gboolean sim_cphs_is_active(unsigned char *cphs, enum sim_cphs_service index)
1537 {
1538         if (index >= 2 * 4u)
1539                 return FALSE;
1540
1541         return ((cphs[index / 4] >> ((index % 4) * 2)) & 3) == 3;
1542 }
1543
1544 GSList *sim_parse_app_template_entries(const unsigned char *buffer, int len)
1545 {
1546         GSList *ret = NULL;
1547         const unsigned char *dataobj;
1548         int dataobj_len;
1549
1550         /* Find all the application entries */
1551         while ((dataobj = ber_tlv_find_by_tag(buffer, 0x61, len,
1552                                                 &dataobj_len)) != NULL) {
1553                 struct sim_app_record app;
1554                 const unsigned char *aid, *label;
1555                 int label_len;
1556
1557                 /* Find the aid (mandatory) */
1558                 aid = ber_tlv_find_by_tag(dataobj, 0x4f, dataobj_len,
1559                                                 &app.aid_len);
1560                 if (!aid || app.aid_len < 0x01 || app.aid_len > 0x10)
1561                         goto error;
1562
1563                 memcpy(app.aid, aid, app.aid_len);
1564
1565                 /* Find the label (optional) */
1566                 label = ber_tlv_find_by_tag(dataobj, 0x50, dataobj_len,
1567                                                 &label_len);
1568                 if (label) {
1569                         /*
1570                          * Label field uses the extra complicated
1571                          * encoding in 102.221 Annex A
1572                          */
1573                         app.label = sim_string_to_utf8(label, label_len);
1574
1575                         if (app.label == NULL)
1576                                 goto error;
1577                 } else
1578                         app.label = NULL;
1579
1580                 ret = g_slist_prepend(ret, g_memdup(&app, sizeof(app)));
1581
1582                 len -= (dataobj - buffer) + dataobj_len;
1583                 buffer = dataobj + dataobj_len;
1584         }
1585
1586         return ret;
1587
1588 error:
1589         while (ret) {
1590                 GSList *t = ret;
1591                 struct sim_app_record *app = ret->data;
1592
1593                 g_free(app->label);
1594                 g_free(app);
1595
1596                 ret = ret->next;
1597                 g_slist_free_1(t);
1598         }
1599
1600         return NULL;
1601 }