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