src/host/layer23/src/mobile/subscriber.c

   1 /*
   2  * (C) 2010 by Andreas Eversberg <jolly@eversberg.eu>
   3  *
   4  * All Rights Reserved
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  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 along
  17  * with this program; if not, write to the Free Software Foundation, Inc.,
  18  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  19  *
  20  */
  21
  22 #include <stdint.h>
  23 #include <errno.h>
  24 #include <string.h>
  25 #include <arpa/inet.h>
  26 #include <osmocore/talloc.h>
  27
  28 #include <osmocom/bb/common/logging.h>
  29 #include <osmocom/bb/common/osmocom_data.h>
  30 #include <osmocom/bb/common/networks.h>
  31 #include <osmocom/bb/mobile/vty.h>
  32
  33 void *l23_ctx;
  34
  35 static void subscr_sim_query_cb(struct osmocom_ms *ms, struct msgb *msg);
  36 static void subscr_sim_update_cb(struct osmocom_ms *ms, struct msgb *msg);
  37 static void subscr_sim_key_cb(struct osmocom_ms *ms, struct msgb *msg);
  38
  39 /*
  40  * support
  41  */
  42
  43 char *gsm_check_imsi(const char *imsi)
  44 {
  45         int i;
  46
  47         if (!imsi || strlen(imsi) != 15)
  48                 return "IMSI must have 15 digits!";
  49
  50         for (i = 0; i < strlen(imsi); i++) {
  51                 if (imsi[i] < '0' || imsi[i] > '9')
  52                         return "IMSI must have digits 0 to 9 only!";
  53         }
  54
  55         return NULL;
  56 }
  57
  58 static char *sim_decode_bcd(uint8_t *data, uint8_t length)
  59 {
  60         int i, j = 0;
  61         static char result[32], c;
  62
  63         for (i = 0; i < (length << 1); i++) {
  64                 if ((i & 1))
  65                         c = (data[i >> 1] >> 4) + '0';
  66                 else
  67                         c = (data[i >> 1] & 0xf) + '0';
  68                 if (c == 0xf)
  69                         break;
  70                 result[j++] = c;
  71                 if (j == sizeof(result) - 1)
  72                         break;
  73         }
  74         result[j] = '\0';
  75
  76         return result;
  77 }
  78
  79 /*
  80  * init/exit
  81  */
  82
  83 int gsm_subscr_init(struct osmocom_ms *ms)
  84 {
  85         struct gsm_subscriber *subscr = &ms->subscr;
  86
  87         memset(subscr, 0, sizeof(*subscr));
  88         subscr->ms = ms;
  89
  90         /* set TMSI / LAC invalid */
  91         subscr->tmsi = 0xffffffff;
  92         subscr->lac = 0x0000;
  93
  94         /* set key invalid */
  95         subscr->key_seq = 7;
  96
  97         /* init lists */
  98         INIT_LLIST_HEAD(&subscr->plmn_list);
  99         INIT_LLIST_HEAD(&subscr->plmn_na);
 100
 101         /* open SIM */
 102         subscr->sim_handle_query = sim_open(ms, subscr_sim_query_cb);
 103         subscr->sim_handle_update = sim_open(ms, subscr_sim_update_cb);
 104         subscr->sim_handle_key = sim_open(ms, subscr_sim_key_cb);
 105         subscr->sim_state = SUBSCR_SIM_NULL;
 106
 107         return 0;
 108 }
 109
 110 int gsm_subscr_exit(struct osmocom_ms *ms)
 111 {
 112         struct gsm_subscriber *subscr = &ms->subscr;
 113         struct llist_head *lh, *lh2;
 114
 115         if (subscr->sim_handle_query) {
 116                 sim_close(ms, subscr->sim_handle_query);
 117                 subscr->sim_handle_query = 0;
 118         }
 119         if (subscr->sim_handle_update) {
 120                 sim_close(ms, subscr->sim_handle_update);
 121                 subscr->sim_handle_update = 0;
 122         }
 123         if (subscr->sim_handle_key) {
 124                 sim_close(ms, subscr->sim_handle_key);
 125                 subscr->sim_handle_key = 0;
 126         }
 127
 128         /* flush lists */
 129         llist_for_each_safe(lh, lh2, &subscr->plmn_list) {
 130                 llist_del(lh);
 131                 talloc_free(lh);
 132         }
 133         llist_for_each_safe(lh, lh2, &subscr->plmn_na) {
 134                 llist_del(lh);
 135                 talloc_free(lh);
 136         }
 137
 138         return 0;
 139 }
 140
 141 /*
 142  * test card
 143  */
 144
 145 /* Attach test card, no SIM must be currently attached */
 146 int gsm_subscr_testcard(struct osmocom_ms *ms, uint16_t mcc, uint16_t mnc)
 147 {
 148         struct gsm_settings *set = &ms->settings;
 149         struct gsm_subscriber *subscr = &ms->subscr;
 150         struct msgb *nmsg;
 151         char *error;
 152
 153         if (subscr->sim_valid) {
 154                 LOGP(DMM, LOGL_ERROR, "Cannot insert card, until current card "
 155                         "is detached.\n");
 156                 return -EBUSY;
 157         }
 158
 159         error = gsm_check_imsi(set->test_imsi);
 160         if (error) {
 161                 LOGP(DMM, LOGL_ERROR, "%s\n", error);
 162                 return -EINVAL;
 163         }
 164
 165         /* reset subscriber */
 166         gsm_subscr_exit(ms);
 167         gsm_subscr_init(ms);
 168
 169         subscr->sim_type = GSM_SIM_TYPE_TEST;
 170         sprintf(subscr->sim_name, "test");
 171         subscr->sim_valid = 1;
 172         subscr->ustate = GSM_SIM_U2_NOT_UPDATED;
 173         subscr->acc_barr = set->test_barr; /* we may access barred cell */
 174         subscr->acc_class = 0xffff; /* we have any access class */
 175         subscr->plmn_valid = set->test_rplmn_valid;
 176         subscr->plmn_mcc = mcc;
 177         subscr->plmn_mnc = mnc;
 178         subscr->always_search_hplmn = set->test_always;
 179         subscr->t6m_hplmn = 1; /* try to find home network every 6 min */
 180         strcpy(subscr->imsi, set->test_imsi);
 181
 182         LOGP(DMM, LOGL_INFO, "(ms %s) Inserting test card (IMSI=%s %s, %s)\n",
 183                 ms->name, subscr->imsi, gsm_imsi_mcc(subscr->imsi),
 184                 gsm_imsi_mnc(subscr->imsi));
 185
 186         if (subscr->plmn_valid)
 187                 LOGP(DMM, LOGL_INFO, "-> Test card registered to %s %s "
 188                         "(%s, %s)\n", gsm_print_mcc(mcc), gsm_print_mnc(mnc),
 189                         gsm_get_mcc(mcc), gsm_get_mnc(mcc, mnc));
 190         else
 191                 LOGP(DMM, LOGL_INFO, "-> Test card not registered\n");
 192
 193         /* insert card */
 194         nmsg = gsm48_mmr_msgb_alloc(GSM48_MMR_REG_REQ);
 195         if (!nmsg)
 196                 return -ENOMEM;
 197         gsm48_mmr_downmsg(ms, nmsg);
 198
 199         return 0;
 200 }
 201
 202 /*
 203  * sim card
 204  */
 205
 206 static int subscr_sim_iccid(struct osmocom_ms *ms, uint8_t *data,
 207         uint8_t length)
 208 {
 209         struct gsm_subscriber *subscr = &ms->subscr;
 210
 211         strcpy(subscr->iccid, sim_decode_bcd(data, length));
 212         sprintf(subscr->sim_name, "sim-%s", subscr->iccid);
 213         LOGP(DMM, LOGL_INFO, "received ICCID %s from SIM\n", subscr->iccid);
 214
 215         return 0;
 216 }
 217
 218 static int subscr_sim_imsi(struct osmocom_ms *ms, uint8_t *data,
 219         uint8_t length)
 220 {
 221         struct gsm_subscriber *subscr = &ms->subscr;
 222
 223         /* get actual length */
 224         if (length < 1)
 225                 return -EINVAL;
 226         if (data[0] + 1 < length) {
 227                 LOGP(DMM, LOGL_NOTICE, "invalid length = %d\n", length);
 228                 return -EINVAL;
 229         }
 230         length = data[0];
 231         if ((length << 1) > GSM_IMSI_LENGTH - 1 || (length << 1) < 6) {
 232                 LOGP(DMM, LOGL_NOTICE, "IMSI invalid length = %d\n",
 233                         length << 1);
 234                 return -EINVAL;
 235         }
 236
 237         strncpy(subscr->imsi, sim_decode_bcd(data + 1, length),
 238                 sizeof(subscr->imsi - 1));
 239
 240         LOGP(DMM, LOGL_INFO, "received IMSI %s from SIM\n", subscr->imsi);
 241
 242         return 0;
 243 }
 244
 245 static int subscr_sim_loci(struct osmocom_ms *ms, uint8_t *data,
 246         uint8_t length)
 247 {
 248         struct gsm_subscriber *subscr = &ms->subscr;
 249         struct gsm1111_ef_loci *loci;
 250
 251         if (length < 11)
 252                 return -EINVAL;
 253         loci = (struct gsm1111_ef_loci *) data;
 254
 255         /* TMSI */
 256         subscr->tmsi = ntohl(loci->tmsi);
 257
 258         /* LAI */
 259         gsm48_decode_lai(&loci->lai, &subscr->mcc, &subscr->mnc, &subscr->lac);
 260
 261         /* location update status */
 262         switch (loci->lupd_status & 0x07) {
 263         case 0x00:
 264                 subscr->ustate = GSM_SIM_U1_UPDATED;
 265                 break;
 266         case 0x02:
 267         case 0x03:
 268                 subscr->ustate = GSM_SIM_U3_ROAMING_NA;
 269                 break;
 270         default:
 271                 subscr->ustate = GSM_SIM_U2_NOT_UPDATED;
 272         }
 273
 274         LOGP(DMM, LOGL_INFO, "received LOCI from SIM\n");
 275
 276         return 0;
 277 }
 278
 279 static int subscr_sim_msisdn(struct osmocom_ms *ms, uint8_t *data,
 280         uint8_t length)
 281 {
 282         struct gsm_subscriber *subscr = &ms->subscr;
 283         struct gsm1111_ef_adn *adn;
 284
 285         if (length < sizeof(*adn))
 286                 return -EINVAL;
 287         adn = (struct gsm1111_ef_adn *) (data + length - sizeof(*adn));
 288
 289         /* empty */
 290         subscr->msisdn[0] = '\0';
 291         if (adn->len_bcd <= 1)
 292                 return 0;
 293
 294         /* number */
 295         if (adn->ton_npi == 1)
 296                 strcpy(subscr->msisdn, "+");
 297         if (adn->ton_npi == 2)
 298                 strcpy(subscr->msisdn, "0");
 299         strncat(subscr->msisdn, sim_decode_bcd(adn->number, adn->len_bcd - 1),
 300                 sizeof(subscr->msisdn) - 2);
 301
 302         LOGP(DMM, LOGL_INFO, "received MSISDN %s from SIM\n", subscr->msisdn);
 303
 304         return 0;
 305 }
 306
 307 static int subscr_sim_kc(struct osmocom_ms *ms, uint8_t *data,
 308         uint8_t length)
 309 {
 310         struct gsm_subscriber *subscr = &ms->subscr;
 311
 312         if (length < 9)
 313                 return -EINVAL;
 314
 315         /* key */
 316         memcpy(subscr->key, data, 8);
 317
 318         /* key sequence */
 319         subscr->key_seq = data[8] & 0x07;
 320
 321         LOGP(DMM, LOGL_INFO, "received KEY from SIM\n");
 322
 323         return 0;
 324 }
 325
 326 static int subscr_sim_plmnsel(struct osmocom_ms *ms, uint8_t *data,
 327         uint8_t length)
 328 {
 329         struct gsm_subscriber *subscr = &ms->subscr;
 330         struct gsm_sub_plmn_list *plmn;
 331         struct llist_head *lh, *lh2;
 332         uint8_t lai[5];
 333         uint16_t dummy_lac;
 334
 335         /* flush list */
 336         llist_for_each_safe(lh, lh2, &subscr->plmn_list) {
 337                 llist_del(lh);
 338                 talloc_free(lh);
 339         }
 340
 341         while(length >= 3) {
 342                 /* end of list inside mandatory fields */
 343                 if (data[0] == 0xff && data[1] == 0xff && data[2] == 0x0ff)
 344                         break;
 345
 346                 /* add to list */
 347                 plmn = talloc_zero(l23_ctx, struct gsm_sub_plmn_list);
 348                 if (!plmn)
 349                         return -ENOMEM;
 350                 lai[0] = data[0];
 351                 lai[1] = data[1];
 352                 lai[2] = data[2];
 353                 gsm48_decode_lai((struct gsm48_loc_area_id *)lai, &plmn->mcc,
 354                         &plmn->mnc, &dummy_lac);
 355                 llist_add_tail(&plmn->entry, &subscr->plmn_list);
 356
 357                 LOGP(DMM, LOGL_INFO, "received PLMN selector (mcc=%s mnc=%s) "
 358                         "from SIM\n",
 359                         gsm_print_mcc(plmn->mcc), gsm_print_mnc(plmn->mnc));
 360
 361                 data += 3;
 362                 length -= 3;
 363         }
 364
 365         return 0;
 366 }
 367
 368 static int subscr_sim_hpplmn(struct osmocom_ms *ms, uint8_t *data,
 369         uint8_t length)
 370 {
 371         struct gsm_subscriber *subscr = &ms->subscr;
 372
 373         if (length < 1)
 374                 return -EINVAL;
 375
 376         /* HPLMN search interval */
 377         subscr->t6m_hplmn = *data; /* multiple of 6 minutes */
 378
 379         LOGP(DMM, LOGL_INFO, "received HPPLMN %d from SIM\n",
 380                 subscr->t6m_hplmn);
 381
 382         return 0;
 383 }
 384
 385 static int subscr_sim_spn(struct osmocom_ms *ms, uint8_t *data,
 386         uint8_t length)
 387 {
 388         struct gsm_subscriber *subscr = &ms->subscr;
 389         int i;
 390
 391         /* UCS2 code not supported */
 392         if (length < 17 || data[1] >= 0x80)
 393                 return -ENOTSUP;
 394
 395         data++;
 396         for (i = 0; i < 16; i++) {
 397                 if (*data == 0xff)
 398                         break;
 399                 subscr->sim_spn[i] = *data++;
 400         }
 401         subscr->sim_spn[i] = '\0';
 402
 403         LOGP(DMM, LOGL_INFO, "received SPN %s from SIM\n", subscr->sim_spn);
 404
 405         return 0;
 406 }
 407
 408 static int subscr_sim_acc(struct osmocom_ms *ms, uint8_t *data,
 409         uint8_t length)
 410 {
 411         struct gsm_subscriber *subscr = &ms->subscr;
 412         uint16_t ac;
 413
 414         if (length < 2)
 415                 return -EINVAL;
 416
 417         /* cell access */
 418         memcpy(&ac, data, sizeof(ac));
 419         subscr->acc_class = ntohs(ac);
 420
 421         LOGP(DMM, LOGL_INFO, "received ACC %04x from SIM\n", subscr->acc_class);
 422
 423         return 0;
 424 }
 425
 426 static int subscr_sim_fplmn(struct osmocom_ms *ms, uint8_t *data,
 427         uint8_t length)
 428 {
 429         struct gsm_subscriber *subscr = &ms->subscr;
 430         struct gsm_sub_plmn_na *na;
 431         struct llist_head *lh, *lh2;
 432         uint8_t lai[5];
 433         uint16_t dummy_lac;
 434
 435         /* flush list */
 436         llist_for_each_safe(lh, lh2, &subscr->plmn_na) {
 437                 llist_del(lh);
 438                 talloc_free(lh);
 439         }
 440
 441         while (length >= 3) {
 442                 /* end of list inside mandatory fields */
 443                 if (data[0] == 0xff && data[1] == 0xff && data[2] == 0x0ff)
 444                         break;
 445
 446                 /* add to list */
 447                 na = talloc_zero(l23_ctx, struct gsm_sub_plmn_na);
 448                 if (!na)
 449                         return -ENOMEM;
 450                 lai[0] = data[0];
 451                 lai[1] = data[1];
 452                 lai[2] = data[2];
 453                 gsm48_decode_lai((struct gsm48_loc_area_id *)lai, &na->mcc,
 454                         &na->mnc, &dummy_lac);
 455                 na->cause = 0;
 456                 llist_add_tail(&na->entry, &subscr->plmn_na);
 457
 458                 data += 3;
 459                 length -= 3;
 460         }
 461         return 0;
 462 }
 463
 464 static struct subscr_sim_file {
 465         uint8_t         mandatory;
 466         uint16_t        path[MAX_SIM_PATH_LENGTH];
 467         uint16_t        file;
 468         int             (*func)(struct osmocom_ms *ms, uint8_t *data,
 469                                 uint8_t length);
 470 } subscr_sim_files[] = {
 471         { 1, { 0 },         0x2fe2, subscr_sim_iccid },
 472         { 1, { 0x7f20, 0 }, 0x6f07, subscr_sim_imsi },
 473         { 1, { 0x7f20, 0 }, 0x6f7e, subscr_sim_loci },
 474         { 0, { 0x7f20, 0 }, 0x6f40, subscr_sim_msisdn },
 475         { 0, { 0x7f20, 0 }, 0x6f20, subscr_sim_kc },
 476         { 0, { 0x7f20, 0 }, 0x6f30, subscr_sim_plmnsel },
 477         { 0, { 0x7f20, 0 }, 0x6f31, subscr_sim_hpplmn },
 478         { 0, { 0x7f20, 0 }, 0x6f46, subscr_sim_spn },
 479         { 0, { 0x7f20, 0 }, 0x6f78, subscr_sim_acc },
 480         { 0, { 0x7f20, 0 }, 0x6f7b, subscr_sim_fplmn },
 481         { 0, { 0 },         0,      NULL }
 482 };
 483
 484 /* request file from SIM */
 485 static int subscr_sim_request(struct osmocom_ms *ms)
 486 {
 487         struct gsm_subscriber *subscr = &ms->subscr;
 488         struct subscr_sim_file *sf = &subscr_sim_files[subscr->sim_file_index];
 489         struct msgb *nmsg;
 490         struct sim_hdr *nsh;
 491         int i;
 492
 493         /* we are done, fire up PLMN and cell selection process */
 494         if (!sf->func) {
 495                 LOGP(DMM, LOGL_INFO, "(ms %s) Done reading SIM card "
 496                         "(IMSI=%s %s, %s)\n", ms->name, subscr->imsi,
 497                         gsm_imsi_mcc(subscr->imsi), gsm_imsi_mnc(subscr->imsi));
 498
 499                 /* if LAI is valid, set RPLMN */
 500                 if (subscr->lac > 0x0000 && subscr->lac < 0xfffe) {
 501                         subscr->plmn_valid = 1;
 502                         subscr->plmn_mcc = subscr->mcc;
 503                         subscr->plmn_mnc = subscr->mnc;
 504                         LOGP(DMM, LOGL_INFO, "-> SIM card registered to %s %s "
 505                                 "(%s, %s)\n", gsm_print_mcc(subscr->plmn_mcc),
 506                                 gsm_print_mnc(subscr->plmn_mnc),
 507                                 gsm_get_mcc(subscr->plmn_mcc),
 508                                 gsm_get_mnc(subscr->plmn_mcc,
 509                                         subscr->plmn_mnc));
 510                 } else
 511                         LOGP(DMM, LOGL_INFO, "-> SIM card not registered\n");
 512
 513                 /* insert card */
 514                 nmsg = gsm48_mmr_msgb_alloc(GSM48_MMR_REG_REQ);
 515                 if (!nmsg)
 516                         return -ENOMEM;
 517                 gsm48_mmr_downmsg(ms, nmsg);
 518
 519                 return 0;
 520         }
 521
 522         /* trigger SIM reading */
 523         nmsg = gsm_sim_msgb_alloc(subscr->sim_handle_query,
 524                 SIM_JOB_READ_BINARY);
 525         if (!nmsg)
 526                 return -ENOMEM;
 527         nsh = (struct sim_hdr *) nmsg->data;
 528         i = 0;
 529         while (sf->path[i]) {
 530                 nsh->path[i] = sf->path[i];
 531                 i++;
 532         }
 533         nsh->path[i] = 0; /* end of path */
 534         nsh->file = sf->file;
 535         LOGP(DMM, LOGL_INFO, "Requesting SIM file 0x%04x\n", nsh->file);
 536         sim_job(ms, nmsg);
 537
 538         return 0;
 539 }
 540
 541 static void subscr_sim_query_cb(struct osmocom_ms *ms, struct msgb *msg)
 542 {
 543         struct gsm_subscriber *subscr = &ms->subscr;
 544         struct sim_hdr *sh = (struct sim_hdr *) msg->data;
 545         uint8_t *payload = msg->data + sizeof(*sh);
 546         uint16_t payload_len = msg->len - sizeof(*sh);
 547         int rc;
 548
 549         /* error handling */
 550         if (sh->job_type == SIM_JOB_ERROR) {
 551                 uint8_t cause = payload[0];
 552
 553                 switch (cause) {
 554                         /* unlocking required */
 555                 case SIM_CAUSE_PIN1_REQUIRED:
 556                         LOGP(DMM, LOGL_INFO, "PIN is required, %d tries left\n",
 557                                 payload[1]);
 558
 559                         vty_notify(ms, NULL);
 560                         vty_notify(ms, "Please give PIN for ICCID %s (you have "
 561                                 "%d tries left)\n", subscr->iccid, payload[1]);
 562                         subscr->sim_pin_required = 1;
 563                         break;
 564                 case SIM_CAUSE_PIN1_BLOCKED:
 565                         LOGP(DMM, LOGL_NOTICE, "PIN is blocked\n");
 566
 567                         vty_notify(ms, NULL);
 568                         vty_notify(ms, "PIN is blocked\n");
 569                         break;
 570                 default:
 571                         LOGP(DMM, LOGL_NOTICE, "SIM reading failed\n");
 572
 573                         vty_notify(ms, NULL);
 574                         vty_notify(ms, "SIM failed, replace SIM!\n");
 575                 }
 576                 msgb_free(msg);
 577
 578                 return;
 579         }
 580
 581         /* call function do decode SIM reply */
 582         rc = subscr_sim_files[subscr->sim_file_index].func(ms, payload,
 583                 payload_len);
 584
 585         if (rc) {
 586                 LOGP(DMM, LOGL_NOTICE, "SIM reading failed, file invalid\n");
 587                 if (subscr_sim_files[subscr->sim_file_index].mandatory) {
 588                         vty_notify(ms, NULL);
 589                         vty_notify(ms, "SIM failed, data invalid, replace "
 590                                 "SIM!\n");
 591                         msgb_free(msg);
 592
 593                         return;
 594                 }
 595         }
 596
 597         msgb_free(msg);
 598
 599         /* trigger next file */
 600         subscr->sim_file_index++;
 601         subscr_sim_request(ms);
 602 }
 603
 604 /* enter PIN */
 605 void gsm_subscr_sim_pin(struct osmocom_ms *ms, char *pin)
 606 {
 607         struct gsm_subscriber *subscr = &ms->subscr;
 608         struct msgb *nmsg;
 609
 610         if (!subscr->sim_pin_required) {
 611                 LOGP(DMM, LOGL_ERROR, "No PIN required now\n");
 612                 return;
 613         }
 614
 615         subscr->sim_pin_required = 0;
 616         LOGP(DMM, LOGL_INFO, "Unlocking PIN %s\n", pin);
 617         nmsg = gsm_sim_msgb_alloc(subscr->sim_handle_update,
 618                 SIM_JOB_PIN1_UNLOCK);
 619         if (!nmsg)
 620                 return;
 621         memcpy(msgb_put(nmsg, strlen(pin)), pin, strlen(pin));
 622         sim_job(ms, nmsg);
 623 }
 624
 625 /* Attach SIM reader, no SIM must be currently attached */
 626 int gsm_subscr_simcard(struct osmocom_ms *ms)
 627 {
 628         struct gsm_subscriber *subscr = &ms->subscr;
 629
 630         if (subscr->sim_valid) {
 631                 LOGP(DMM, LOGL_ERROR, "Cannot attach card, until current card "
 632                         "is detached.\n");
 633                 return -EBUSY;
 634         }
 635
 636         /* reset subscriber */
 637         gsm_subscr_exit(ms);
 638         gsm_subscr_init(ms);
 639
 640         subscr->sim_type = GSM_SIM_TYPE_READER;
 641         sprintf(subscr->sim_name, "sim");
 642         subscr->sim_valid = 1;
 643         subscr->ustate = GSM_SIM_U2_NOT_UPDATED;
 644
 645         /* start with first index */
 646         subscr->sim_file_index = 0;
 647         return subscr_sim_request(ms);
 648 }
 649
 650 /* update plmn not allowed list on SIM */
 651 static int subscr_write_plmn_na(struct osmocom_ms *ms)
 652 {
 653         struct gsm_subscriber *subscr = &ms->subscr;
 654         struct msgb *nmsg;
 655         struct sim_hdr *nsh;
 656         struct gsm_sub_plmn_na *na, *nas[4] = { NULL, NULL, NULL, NULL };
 657         int count = 0, i;
 658         uint8_t *data;
 659         uint8_t lai[5];
 660
 661         /* skip, if no real valid SIM */
 662         if (subscr->sim_type != GSM_SIM_TYPE_READER || !subscr->sim_valid)
 663                 return 0;
 664
 665         /* get tail list from "PLMN not allowed" */
 666         llist_for_each_entry(na, &subscr->plmn_na, entry) {
 667                 if (count < 4)
 668                         nas[count] = na;
 669                 else {
 670                         nas[0] = nas[1];
 671                         nas[1] = nas[2];
 672                         nas[2] = nas[3];
 673                         nas[3] = na;
 674                 }
 675                 count++;
 676         }
 677
 678         /* write to SIM */
 679         LOGP(DMM, LOGL_INFO, "Updating FPLMN on SIM\n");
 680         nmsg = gsm_sim_msgb_alloc(subscr->sim_handle_update,
 681                 SIM_JOB_UPDATE_BINARY);
 682         if (!nmsg)
 683                 return -ENOMEM;
 684         nsh = (struct sim_hdr *) nmsg->data;
 685         data = msgb_put(nmsg, 12);
 686         nsh->path[0] = 0x7f20;
 687         nsh->path[1] = 0;
 688         nsh->file = 0x6f7b;
 689         for (i = 0; i < 4; i++) {
 690                 if (nas[i]) {
 691                         gsm48_encode_lai((struct gsm48_loc_area_id *)lai,
 692                         nas[i]->mcc, nas[i]->mnc, 0);
 693                         *data++ = lai[0];
 694                         *data++ = lai[1];
 695                         *data++ = lai[2];
 696                 } else {
 697                         *data++ = 0xff;
 698                         *data++ = 0xff;
 699                         *data++ = 0xff;
 700                 }
 701         }
 702         sim_job(ms, nmsg);
 703
 704         return 0;
 705 }
 706
 707 /* update LOCI on SIM */
 708 int gsm_subscr_write_loci(struct osmocom_ms *ms)
 709 {
 710         struct gsm_subscriber *subscr = &ms->subscr;
 711         struct msgb *nmsg;
 712         struct sim_hdr *nsh;
 713         struct gsm1111_ef_loci *loci;
 714
 715         /* skip, if no real valid SIM */
 716         if (subscr->sim_type != GSM_SIM_TYPE_READER || !subscr->sim_valid)
 717                 return 0;
 718
 719         LOGP(DMM, LOGL_INFO, "Updating LOCI on SIM\n");
 720
 721         /* write to SIM */
 722         nmsg = gsm_sim_msgb_alloc(subscr->sim_handle_update,
 723                 SIM_JOB_UPDATE_BINARY);
 724         if (!nmsg)
 725                 return -ENOMEM;
 726         nsh = (struct sim_hdr *) nmsg->data;
 727         nsh->path[0] = 0x7f20;
 728         nsh->path[1] = 0;
 729         nsh->file = 0x6f7e;
 730         loci = (struct gsm1111_ef_loci *)msgb_put(nmsg, sizeof(*loci));
 731
 732         /* TMSI */
 733         loci->tmsi = htonl(subscr->tmsi);
 734
 735         /* LAI */
 736         gsm48_encode_lai(&loci->lai, subscr->mcc, subscr->mnc, subscr->lac);
 737
 738         /* TMSI time */
 739         loci->tmsi_time = 0xff;
 740
 741         /* location update status */
 742         switch (subscr->ustate) {
 743         case GSM_SIM_U1_UPDATED:
 744                 loci->lupd_status = 0x00;
 745                 break;
 746         case GSM_SIM_U3_ROAMING_NA:
 747                 loci->lupd_status = 0x03;
 748                 break;
 749         default:
 750                 loci->lupd_status = 0x01;
 751         }
 752
 753         sim_job(ms, nmsg);
 754
 755         return 0;
 756 }
 757
 758 static void subscr_sim_update_cb(struct osmocom_ms *ms, struct msgb *msg)
 759 {
 760         struct sim_hdr *sh = (struct sim_hdr *) msg->data;
 761         uint8_t *payload = msg->data + sizeof(*sh);
 762
 763         /* error handling */
 764         if (sh->job_type == SIM_JOB_ERROR)
 765                 LOGP(DMM, LOGL_NOTICE, "SIM update failed (cause %d)\n",
 766                         *payload);
 767
 768         msgb_free(msg);
 769 }
 770
 771 int gsm_subscr_generate_kc(struct osmocom_ms *ms, uint8_t key_seq,
 772         uint8_t *rand, uint8_t no_sim)
 773 {
 774         struct gsm_subscriber *subscr = &ms->subscr;
 775         struct msgb *nmsg;
 776         struct sim_hdr *nsh;
 777
 778         /* not a SIM */
 779         if (subscr->sim_type != GSM_SIM_TYPE_READER || !subscr->sim_valid
 780          || no_sim) {
 781                 struct gsm48_mm_event *nmme;
 782
 783                 LOGP(DMM, LOGL_INFO, "Sending dummy authentication response\n");
 784                 nmsg = gsm48_mmevent_msgb_alloc(GSM48_MM_EVENT_AUTH_RESPONSE);
 785                 if (!nmsg)
 786                         return -ENOMEM;
 787                 nmme = (struct gsm48_mm_event *) nmsg->data;
 788                 nmme->sres[0] = 0x12;
 789                 nmme->sres[1] = 0x34;
 790                 nmme->sres[2] = 0x56;
 791                 nmme->sres[3] = 0x78;
 792                 gsm48_mmevent_msg(ms, nmsg);
 793
 794                 return 0;
 795         }
 796
 797         LOGP(DMM, LOGL_INFO, "Generating KEY at SIM\n");
 798
 799         /* command to SIM */
 800         nmsg = gsm_sim_msgb_alloc(subscr->sim_handle_key, SIM_JOB_RUN_GSM_ALGO);
 801         if (!nmsg)
 802                 return -ENOMEM;
 803         nsh = (struct sim_hdr *) nmsg->data;
 804         nsh->path[0] = 0x7f20;
 805         nsh->path[1] = 0;
 806
 807         /* random */
 808         memcpy(msgb_put(nmsg, 16), rand, 16);
 809
 810         /* store sequence */
 811         subscr->key_seq = key_seq;
 812
 813         sim_job(ms, nmsg);
 814
 815         return 0;
 816 }
 817
 818 static void subscr_sim_key_cb(struct osmocom_ms *ms, struct msgb *msg)
 819 {
 820         struct gsm_subscriber *subscr = &ms->subscr;
 821         struct sim_hdr *sh = (struct sim_hdr *) msg->data;
 822         uint8_t *payload = msg->data + sizeof(*sh);
 823         uint16_t payload_len = msg->len - sizeof(*sh);
 824         struct msgb *nmsg;
 825         struct sim_hdr *nsh;
 826         struct gsm48_mm_event *nmme;
 827         uint8_t *data;
 828
 829         /* error handling */
 830         if (sh->job_type == SIM_JOB_ERROR) {
 831                 LOGP(DMM, LOGL_NOTICE, "key generation on SIM failed "
 832                         "(cause %d)\n", *payload);
 833
 834                 msgb_free(msg);
 835
 836                 return;
 837         }
 838
 839         if (payload_len < 12) {
 840                 LOGP(DMM, LOGL_NOTICE, "response from SIM too short\n");
 841                 return;
 842         }
 843
 844         /* store key */
 845         memcpy(subscr->key, payload + 4, 8);
 846
 847         /* write to SIM */
 848         LOGP(DMM, LOGL_INFO, "Updating KC on SIM\n");
 849         nmsg = gsm_sim_msgb_alloc(subscr->sim_handle_update,
 850                 SIM_JOB_UPDATE_BINARY);
 851         if (!nmsg)
 852                 return;
 853         nsh = (struct sim_hdr *) nmsg->data;
 854         nsh->path[0] = 0x7f20;
 855         nsh->path[1] = 0;
 856         nsh->file = 0x6f20;
 857         data = msgb_put(nmsg, 9);
 858         memcpy(data, subscr->key, 8);
 859         data[8] = subscr->key_seq;
 860         sim_job(ms, nmsg);
 861
 862         /* return signed response */
 863         nmsg = gsm48_mmevent_msgb_alloc(GSM48_MM_EVENT_AUTH_RESPONSE);
 864         if (!nmsg)
 865                 return;
 866         nmme = (struct gsm48_mm_event *) nmsg->data;
 867         nmme->sres[0] = 0x12;
 868         nmme->sres[1] = 0x34;
 869         nmme->sres[2] = 0x56;
 870         nmme->sres[3] = 0x78;
 871         gsm48_mmevent_msg(ms, nmsg);
 872
 873         msgb_free(msg);
 874 }
 875
 876 /*
 877  * detach
 878  */
 879
 880 /* Detach card */
 881 int gsm_subscr_remove(struct osmocom_ms *ms)
 882 {
 883         struct gsm_subscriber *subscr = &ms->subscr;
 884         struct msgb *nmsg;
 885
 886         if (!subscr->sim_valid) {
 887                 LOGP(DMM, LOGL_ERROR, "Cannot remove card, no card present\n");
 888                 return -EINVAL;
 889         }
 890
 891         /* remove card */
 892         nmsg = gsm48_mmr_msgb_alloc(GSM48_MMR_NREG_REQ);
 893         if (!nmsg)
 894                 return -ENOMEM;
 895         gsm48_mmr_downmsg(ms, nmsg);
 896
 897         return 0;
 898 }
 899
 900 /*
 901  * state and lists
 902  */
 903
 904 static const char *subscr_ustate_names[] = {
 905         "U0_NULL",
 906         "U1_UPDATED",
 907         "U2_NOT_UPDATED",
 908         "U3_ROAMING_NA"
 909 };
 910
 911 /* change to new U state */
 912 void new_sim_ustate(struct gsm_subscriber *subscr, int state)
 913 {
 914         LOGP(DMM, LOGL_INFO, "(ms %s) new state %s -> %s\n", subscr->ms->name,
 915                 subscr_ustate_names[subscr->ustate],
 916                 subscr_ustate_names[state]);
 917
 918         subscr->ustate = state;
 919 }
 920
 921 /* del forbidden PLMN */
 922 int gsm_subscr_del_forbidden_plmn(struct gsm_subscriber *subscr, uint16_t mcc,
 923         uint16_t mnc)
 924 {
 925         struct gsm_sub_plmn_na *na;
 926
 927         llist_for_each_entry(na, &subscr->plmn_na, entry) {
 928                 if (na->mcc == mcc && na->mnc == mnc) {
 929                         LOGP(DPLMN, LOGL_INFO, "Delete from list of forbidden "
 930                                 "PLMNs (mcc=%s, mnc=%s)\n",
 931                                 gsm_print_mcc(mcc), gsm_print_mnc(mnc));
 932                         llist_del(&na->entry);
 933                         talloc_free(na);
 934                         /* update plmn not allowed list on SIM */
 935                         subscr_write_plmn_na(subscr->ms);
 936                         return 0;
 937                 }
 938         }
 939
 940         return -EINVAL;
 941 }
 942
 943 /* add forbidden PLMN */
 944 int gsm_subscr_add_forbidden_plmn(struct gsm_subscriber *subscr, uint16_t mcc,
 945                                         uint16_t mnc, uint8_t cause)
 946 {
 947         struct gsm_sub_plmn_na *na;
 948
 949         /* if already in the list, remove and add to tail */
 950         gsm_subscr_del_forbidden_plmn(subscr, mcc, mnc);
 951
 952         LOGP(DPLMN, LOGL_INFO, "Add to list of forbidden PLMNs "
 953                 "(mcc=%s, mnc=%s)\n", gsm_print_mcc(mcc), gsm_print_mnc(mnc));
 954         na = talloc_zero(l23_ctx, struct gsm_sub_plmn_na);
 955         if (!na)
 956                 return -ENOMEM;
 957         na->mcc = mcc;
 958         na->mnc = mnc;
 959         na->cause = cause;
 960         llist_add_tail(&na->entry, &subscr->plmn_na);
 961
 962         /* don't add Home PLMN to SIM */
 963         if (subscr->sim_valid && gsm_match_mnc(mcc, mnc, subscr->imsi))
 964                 return -EINVAL;
 965
 966         /* update plmn not allowed list on SIM */
 967         subscr_write_plmn_na(subscr->ms);
 968
 969         return 0;
 970 }
 971
 972 /* search forbidden PLMN */
 973 int gsm_subscr_is_forbidden_plmn(struct gsm_subscriber *subscr, uint16_t mcc,
 974                                         uint16_t mnc)
 975 {
 976         struct gsm_sub_plmn_na *na;
 977
 978         llist_for_each_entry(na, &subscr->plmn_na, entry) {
 979                 if (na->mcc == mcc && na->mnc == mnc)
 980                         return 1;
 981         }
 982
 983         return 0;
 984 }
 985
 986 int gsm_subscr_dump_forbidden_plmn(struct osmocom_ms *ms,
 987                         void (*print)(void *, const char *, ...), void *priv)
 988 {
 989         struct gsm_subscriber *subscr = &ms->subscr;
 990         struct gsm_sub_plmn_na *temp;
 991
 992         print(priv, "MCC    |MNC    |cause\n");
 993         print(priv, "-------+-------+-------\n");
 994         llist_for_each_entry(temp, &subscr->plmn_na, entry)
 995                 print(priv, "%s    |%s%s    |#%d\n",
 996                         gsm_print_mcc(temp->mcc), gsm_print_mnc(temp->mnc),
 997                         ((temp->mnc & 0x00f) == 0x00f) ? " ":"", temp->cause);
 998
 999         return 0;
1000 }
1001
1002 /* dump subscriber */
1003 void gsm_subscr_dump(struct gsm_subscriber *subscr,
1004                         void (*print)(void *, const char *, ...), void *priv)
1005 {
1006         int i;
1007         struct gsm_sub_plmn_list *plmn_list;
1008         struct gsm_sub_plmn_na *plmn_na;
1009
1010         print(priv, "Mobile Subscriber of MS '%s':\n", subscr->ms->name);
1011
1012         if (!subscr->sim_valid) {
1013                 print(priv, " No SIM present.\n");
1014                 return;
1015         }
1016
1017         print(priv, " IMSI: %s\n", subscr->imsi);
1018         if (subscr->msisdn[0])
1019                 print(priv, " MSISDN: %s\n", subscr->msisdn);
1020         print(priv, " Status: %s  IMSI %s", subscr_ustate_names[subscr->ustate],
1021                 (subscr->imsi_attached) ? "attached" : "detached");
1022         if (subscr->tmsi != 0xffffffff)
1023                 print(priv, "  TSMI  %08x", subscr->tmsi);
1024         if (subscr->lac > 0x0000 && subscr->lac < 0xfffe)
1025                 print(priv, "  LAI: MCC %s  MNC %s  LAC 0x%04x  (%s, %s)\n",
1026                         gsm_print_mcc(subscr->mcc),
1027                         gsm_print_mnc(subscr->mnc), subscr->lac,
1028                         gsm_get_mcc(subscr->mcc),
1029                         gsm_get_mnc(subscr->mcc, subscr->mnc));
1030         else
1031                 print(priv, "  LAI: invalid\n");
1032         if (subscr->key_seq != 7) {
1033                 print(priv, " Key: sequence %d ");
1034                 for (i = 0; i < sizeof(subscr->key); i++)
1035                         print(priv, " %02x", subscr->key[i]);
1036                 print(priv, "\n");
1037         }
1038         if (subscr->plmn_valid)
1039                 print(priv, " Registered PLMN: MCC %s  MNC %s  (%s, %s)\n",
1040                         gsm_print_mcc(subscr->plmn_mcc),
1041                         gsm_print_mnc(subscr->plmn_mnc),
1042                         gsm_get_mcc(subscr->plmn_mcc),
1043                         gsm_get_mnc(subscr->plmn_mcc, subscr->plmn_mnc));
1044         print(priv, " Access barred cells: %s\n",
1045                 (subscr->acc_barr) ? "yes" : "no");
1046         print(priv, " Access classes:");
1047         for (i = 0; i < 16; i++)
1048                 if ((subscr->acc_class & (1 << i)))
1049                         print(priv, " C%d", i);
1050         print(priv, "\n");
1051         if (!llist_empty(&subscr->plmn_list)) {
1052                 print(priv, " List of preferred PLMNs:\n");
1053                 print(priv, "        MCC    |MNC\n");
1054                 print(priv, "        -------+-------\n");
1055                 llist_for_each_entry(plmn_list, &subscr->plmn_list, entry)
1056                         print(priv, "        %s    |%s\n",
1057                         gsm_print_mcc(plmn_list->mcc),
1058                         gsm_print_mnc(plmn_list->mnc));
1059         }
1060         if (!llist_empty(&subscr->plmn_na)) {
1061                 print(priv, " List of forbidden PLMNs:\n");
1062                 print(priv, "        MCC    |MNC    |cause\n");
1063                 print(priv, "        -------+-------+-------\n");
1064                 llist_for_each_entry(plmn_na, &subscr->plmn_na, entry)
1065                         print(priv, "        %s    |%s%s    |#%d\n",
1066                                 gsm_print_mcc(plmn_na->mcc),
1067                                 gsm_print_mnc(plmn_na->mnc),
1068                                 ((plmn_na->mnc & 0x00f) == 0x00f) ? " ":"",
1069                                 plmn_na->cause);
1070         }
1071 }
1072