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