src/host/layer23/src/gsm322.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 <stdio.h>
  25 #include <string.h>
  26 #include <stdlib.h>
  27
  28 #include <osmocore/msgb.h>
  29 #include <osmocore/talloc.h>
  30 #include <osmocore/utils.h>
  31 #include <osmocore/gsm48.h>
  32 #include <osmocore/signal.h>
  33
  34 #include <osmocom/logging.h>
  35 #include <osmocom/l1ctl.h>
  36 #include <osmocom/file.h>
  37 #include <osmocom/osmocom_data.h>
  38 #include <osmocom/networks.h>
  39
  40 extern void *l23_ctx;
  41
  42 static void gsm322_cs_timeout(void *arg);
  43 static int gsm322_cs_select(struct osmocom_ms *ms, int any);
  44 static int gsm322_m_switch_on(struct osmocom_ms *ms, struct msgb *msg);
  45
  46 /*
  47  * notes
  48  */
  49
  50 /* Cell selection process
  51  *
  52  * The process depends on states and events (finites state machine).
  53  *
  54  * During states of cell selection or cell re-selection, the search for a cell
  55  * is performed in two steps:
  56  *
  57  *  1. Measurement of received level of all relevant frequencies (rx-lev)
  58  *
  59  *  2. Receive system information messages of all relevant frequencies
  60  *
  61  * During this process, the results are stored in a list of all frequencies.
  62  * This list is checked whenever a cell is selected. It depends on the results
  63  * if the cell is 'suitable' and 'allowable' to 'camp' on.
  64  *
  65  * This list is also used to generate a list of available networks.
  66  */
  67
  68 /* PLMN selection process
  69  *
  70  * The PLMN (Public Land Mobile Network = Operator's Network) has two different
  71  * search processes:
  72  *
  73  *  1. Automatic search
  74  *
  75  *  2. Manual search
  76  *
  77  * The process depends on states and events (finites state machine).
  78  *
  79  */
  80
  81 /* File format of BA list:
  82  *
  83  * uint16_t     mcc
  84  * uint16_t     mcc
  85  * uint8_t      freq[128];
  86  *      where frequency 0 is bit 0 of first byte
  87  *
  88  * If not end-of-file, the next BA list is stored.
  89  */
  90
  91 /* List of lists:
  92  *
  93  * * subscr->plmn_list
  94  *
  95  * The "PLMN Selector list" stores prefered networks to select during PLMN
  96  * search process. This list is also stored in the SIM.
  97  *
  98  * * subscr->plmn_na
  99  *
 100  * The "forbidden PLMNs" list stores all networks that rejected us. The stored
 101  * network will not be used when searching PLMN automatically. This list is
 102  * also stored din the SIM.
 103  *
 104  * * plmn->forbidden_la
 105  *
 106  * The "forbidden LAs for roaming" list stores all location areas where roaming
 107  * was not allowed.
 108  *
 109  * * cs->list[1024]
 110  *
 111  * This list stores measurements and cell informations during cell selection
 112  * process. It can be used to speed up repeated cell selection.
 113  *
 114  * * cs->ba_list
 115  *
 116  * This list stores a map of frequencies used for a PLMN. If this lists exists
 117  * for a PLMN, it helps to speedup cell scan process.
 118  *
 119  * * plmn->sorted_plmn
 120  *
 121  * This list is generated whenever a PLMN search is started and a list of PLMNs
 122  * is required. It consists of home PLMN, PLMN Selector list, and PLMNs found
 123  * during scan process.
 124  */
 125
 126 /*
 127  * event messages
 128  */
 129
 130 static const struct value_string gsm322_event_names[] = {
 131         { GSM322_EVENT_SWITCH_ON,       "EVENT_SWITCH_ON" },
 132         { GSM322_EVENT_SWITCH_OFF,      "EVENT_SWITCH_OFF" },
 133         { GSM322_EVENT_SIM_INSERT,      "EVENT_SIM_INSERT" },
 134         { GSM322_EVENT_SIM_REMOVE,      "EVENT_SIM_REMOVE" },
 135         { GSM322_EVENT_REG_FAILED,      "EVENT_REG_FAILED" },
 136         { GSM322_EVENT_ROAMING_NA,      "EVENT_ROAMING_NA" },
 137         { GSM322_EVENT_INVALID_SIM,     "EVENT_INVALID_SIM" },
 138         { GSM322_EVENT_REG_SUCCESS,     "EVENT_REG_SUCCESS" },
 139         { GSM322_EVENT_NEW_PLMN,        "EVENT_NEW_PLMN" },
 140         { GSM322_EVENT_ON_PLMN,         "EVENT_ON_PLMN" },
 141         { GSM322_EVENT_HPLMN_SEARCH,    "EVENT_HPLMN_SEARCH" },
 142         { GSM322_EVENT_HPLMN_FOUND,     "EVENT_HPLMN_FOUND" },
 143         { GSM322_EVENT_HPLMN_NOT_FOUND, "EVENT_HPLMN_NOT_FOUND" },
 144         { GSM322_EVENT_USER_RESEL,      "EVENT_USER_RESEL" },
 145         { GSM322_EVENT_PLMN_AVAIL,      "EVENT_PLMN_AVAIL" },
 146         { GSM322_EVENT_CHOSE_PLMN,      "EVENT_CHOSE_PLMN" },
 147         { GSM322_EVENT_SEL_MANUAL,      "EVENT_SEL_MANUAL" },
 148         { GSM322_EVENT_SEL_AUTO,        "EVENT_SEL_AUTO" },
 149         { GSM322_EVENT_CELL_FOUND,      "EVENT_CELL_FOUND" },
 150         { GSM322_EVENT_NO_CELL_FOUND,   "EVENT_NO_CELL_FOUND" },
 151         { GSM322_EVENT_LEAVE_IDLE,      "EVENT_LEAVE_IDLE" },
 152         { GSM322_EVENT_RET_IDLE,        "EVENT_RET_IDLE" },
 153         { GSM322_EVENT_CELL_RESEL,      "EVENT_CELL_RESEL" },
 154         { GSM322_EVENT_SYSINFO,         "EVENT_SYSINFO" },
 155         { 0,                            NULL }
 156 };
 157
 158 const char *get_event_name(int value)
 159 {
 160         return get_value_string(gsm322_event_names, value);
 161 }
 162
 163
 164 /* allocate a 03.22 event message */
 165 struct msgb *gsm322_msgb_alloc(int msg_type)
 166 {
 167         struct msgb *msg;
 168         struct gsm322_msg *gm;
 169
 170         msg = msgb_alloc_headroom(sizeof(*gm), 0, "GSM 03.22 event");
 171         if (!msg)
 172                 return NULL;
 173
 174         gm = (struct gsm322_msg *)msgb_put(msg, sizeof(*gm));
 175         gm->msg_type = msg_type;
 176
 177         return msg;
 178 }
 179
 180 /* queue PLMN selection message */
 181 int gsm322_plmn_sendmsg(struct osmocom_ms *ms, struct msgb *msg)
 182 {
 183         struct gsm322_plmn *plmn = &ms->plmn;
 184
 185         msgb_enqueue(&plmn->event_queue, msg);
 186
 187         return 0;
 188 }
 189
 190 /* queue cell selection message */
 191 int gsm322_cs_sendmsg(struct osmocom_ms *ms, struct msgb *msg)
 192 {
 193         struct gsm322_cellsel *cs = &ms->cellsel;
 194
 195         msgb_enqueue(&cs->event_queue, msg);
 196
 197         return 0;
 198 }
 199
 200 /*
 201  * support
 202  */
 203
 204 /* del forbidden PLMN */
 205 int gsm322_del_forbidden_plmn(struct osmocom_ms *ms, uint16_t mcc,
 206         uint16_t mnc)
 207 {
 208         struct gsm_subscriber *subscr = &ms->subscr;
 209         struct gsm_sub_plmn_na *na;
 210
 211         llist_for_each_entry(na, &subscr->plmn_na, entry) {
 212                 if (na->mcc == mcc && na->mnc == mnc) {
 213                         LOGP(DPLMN, LOGL_INFO, "Delete from list of forbidden "
 214                                 "PLMNs (mcc=%03d, mnc=%02d)\n", mcc, mnc);
 215                         llist_del(&na->entry);
 216                         talloc_free(na);
 217 #ifdef TODO
 218                         update plmn not allowed list on sim
 219 #endif
 220                         return 0;
 221                 }
 222         }
 223
 224         return -EINVAL;
 225 }
 226
 227 /* add forbidden PLMN */
 228 int gsm322_add_forbidden_plmn(struct osmocom_ms *ms, uint16_t mcc,
 229         uint16_t mnc, uint8_t cause)
 230 {
 231         struct gsm_subscriber *subscr = &ms->subscr;
 232         struct gsm_sub_plmn_na *na;
 233
 234         /* don't add Home PLMN */
 235         if (subscr->sim_valid && mcc == subscr->mcc && mnc == subscr->mnc)
 236                 return -EINVAL;
 237
 238         LOGP(DPLMN, LOGL_INFO, "Add to list of forbidden PLMNs "
 239                 "(mcc=%03d, mnc=%02d)\n", mcc, mnc);
 240         na = talloc_zero(l23_ctx, struct gsm_sub_plmn_na);
 241         if (!na)
 242                 return -ENOMEM;
 243         na->mcc = mcc;
 244         na->mnc = mnc;
 245         na->cause = cause;
 246         llist_add_tail(&na->entry, &subscr->plmn_na);
 247
 248 #ifdef TODO
 249         update plmn not allowed list on sim
 250 #endif
 251
 252         return 0;
 253 }
 254
 255 /* search forbidden PLMN */
 256 int gsm322_is_forbidden_plmn(struct osmocom_ms *ms, uint16_t mcc, uint16_t mnc)
 257 {
 258         struct gsm_subscriber *subscr = &ms->subscr;
 259         struct gsm_sub_plmn_na *na;
 260
 261         llist_for_each_entry(na, &subscr->plmn_na, entry) {
 262                 if (na->mcc == mcc && na->mnc == mnc)
 263                         return 1;
 264         }
 265
 266         return 0;
 267 }
 268
 269 /* del forbidden LA */
 270 int gsm322_del_forbidden_la(struct osmocom_ms *ms, uint16_t mcc,
 271         uint16_t mnc, uint16_t lac)
 272 {
 273         struct gsm322_plmn *plmn = &ms->plmn;
 274         struct gsm322_la_list *la;
 275
 276         llist_for_each_entry(la, &plmn->forbidden_la, entry) {
 277                 if (la->mcc == mcc && la->mnc == mnc && la->lac == lac) {
 278                         LOGP(DPLMN, LOGL_INFO, "Delete from list of forbidden "
 279                                 "LAs (mcc=%03d, mnc=%02d, lac=%04x)\n",
 280                                 mcc, mnc, lac);
 281                         llist_del(&la->entry);
 282                         talloc_free(la);
 283                         return 0;
 284                 }
 285         }
 286
 287         return -EINVAL;
 288 }
 289
 290 /* add forbidden LA */
 291 int gsm322_add_forbidden_la(struct osmocom_ms *ms, uint16_t mcc,
 292         uint16_t mnc, uint16_t lac, uint8_t cause)
 293 {
 294         struct gsm322_plmn *plmn = &ms->plmn;
 295         struct gsm322_la_list *la;
 296
 297         LOGP(DPLMN, LOGL_INFO, "Add to list of forbidden LAs "
 298                 "(mcc=%03d, mnc=%02d, lac=%04x)\n", mcc, mnc, lac);
 299         la = talloc_zero(l23_ctx, struct gsm322_la_list);
 300         if (!la)
 301                 return -ENOMEM;
 302         la->mcc = mcc;
 303         la->mnc = mnc;
 304         la->lac = lac;
 305         la->cause = cause;
 306         llist_add_tail(&la->entry, &plmn->forbidden_la);
 307
 308         return 0;
 309 }
 310
 311 /* search forbidden LA */
 312 int gsm322_is_forbidden_la(struct osmocom_ms *ms, uint16_t mcc, uint16_t mnc,
 313         uint16_t lac)
 314 {
 315         struct gsm322_plmn *plmn = &ms->plmn;
 316         struct gsm322_la_list *la;
 317
 318         llist_for_each_entry(la, &plmn->forbidden_la, entry) {
 319                 if (la->mcc == mcc && la->mnc == mnc && la->lac == lac)
 320                         return 1;
 321         }
 322
 323         return 0;
 324 }
 325
 326 /* search for PLMN in all BA lists */
 327 static struct gsm322_ba_list *gsm322_find_ba_list(struct gsm322_cellsel *cs,
 328         uint16_t mcc, uint16_t mnc)
 329 {
 330         struct gsm322_ba_list *ba, *ba_found = NULL;
 331
 332         /* search for BA list */
 333         llist_for_each_entry(ba, &cs->ba_list, entry) {
 334                 if (ba->mcc == mcc
 335                  && ba->mnc == mnc) {
 336                         ba_found = ba;
 337                         break;
 338                 }
 339         }
 340
 341         return ba_found;
 342 }
 343
 344 /*
 345  * timer
 346  */
 347
 348 /*plmn search timer event */
 349 static void plmn_timer_timeout(void *arg)
 350 {
 351         struct gsm322_plmn *plmn = arg;
 352         struct msgb *nmsg;
 353
 354         LOGP(DPLMN, LOGL_INFO, "HPLMN search timer has fired.\n");
 355
 356         /* indicate PLMN selection T timeout */
 357         nmsg = gsm322_msgb_alloc(GSM322_EVENT_HPLMN_SEARCH);
 358         if (!nmsg)
 359                 return;
 360         gsm322_plmn_sendmsg(plmn->ms, nmsg);
 361 }
 362
 363 /* start plmn search timer */
 364 static void start_plmn_timer(struct gsm322_plmn *plmn, int secs)
 365 {
 366         LOGP(DPLMN, LOGL_INFO, "Starting HPLMN search timer with %d minutes.\n",
 367                 secs / 60);
 368         plmn->timer.cb = plmn_timer_timeout;
 369         plmn->timer.data = plmn;
 370         bsc_schedule_timer(&plmn->timer, secs, 0);
 371 }
 372
 373 /* stop plmn search timer */
 374 static void stop_plmn_timer(struct gsm322_plmn *plmn)
 375 {
 376         if (bsc_timer_pending(&plmn->timer)) {
 377                 LOGP(DPLMN, LOGL_INFO, "Stopping pending timer.\n");
 378                 bsc_del_timer(&plmn->timer);
 379         }
 380 }
 381
 382 /* start cell selection timer */
 383 static void start_cs_timer(struct gsm322_cellsel *cs, int sec, int micro)
 384 {
 385         LOGP(DCS, LOGL_INFO, "Starting CS timer with %d seconds.\n", sec);
 386         cs->timer.cb = gsm322_cs_timeout;
 387         cs->timer.data = cs;
 388         bsc_schedule_timer(&cs->timer, sec, micro);
 389 }
 390
 391 /* stop cell selection timer */
 392 static void stop_cs_timer(struct gsm322_cellsel *cs)
 393 {
 394         if (bsc_timer_pending(&cs->timer)) {
 395                 LOGP(DCS, LOGL_INFO, "stopping pending CS timer.\n");
 396                 bsc_del_timer(&cs->timer);
 397         }
 398 }
 399
 400 /*
 401  * state change
 402  */
 403
 404 static const char *plmn_a_state_names[] = {
 405         "A0_NULL",
 406         "A1_TRYING_RPLMN",
 407         "A2_ON_PLMN",
 408         "A3_TRYING_PLMN",
 409         "A4_WAIT_FOR_PLMN",
 410         "A5_HPLMN",
 411         "A6_NO_SIM"
 412 };
 413
 414 static const char *plmn_m_state_names[] = {
 415         "M1_NULL",
 416         "M1_TRYING_RPLMN",
 417         "M2_ON_PLMN",
 418         "M3_NOT_ON_PLMN",
 419         "M4_TRYING_PLMN",
 420         "M5_NO_SIM"
 421 };
 422
 423 static const char *cs_state_names[] = {
 424         "C0_NULL",
 425         "C1_NORMAL_CELL_SEL",
 426         "C2_STORED_CELL_SEL",
 427         "C3_CAMPED_NORMALLY",
 428         "C4_NORMAL_CELL_RESEL",
 429         "C5_CHOOSE_CELL",
 430         "C6_ANY_CELL_SEL",
 431         "C7_CAMPED_ANY_CELL",
 432         "C8_ANY_CELL_RESEL",
 433         "C9_CHOOSE_ANY_CELL",
 434         "HPLMN_SEARCH"
 435 };
 436
 437
 438 /* new automatic PLMN search state */
 439 static void new_a_state(struct gsm322_plmn *plmn, int state)
 440 {
 441         if (plmn->mode != PLMN_MODE_AUTO) {
 442                 LOGP(DPLMN, LOGL_FATAL, "not in auto mode, please fix!\n");
 443                 return;
 444         }
 445
 446         stop_plmn_timer(plmn);
 447
 448         if (state < 0 || state >= (sizeof(plmn_a_state_names) / sizeof(char *)))
 449                 return;
 450
 451         LOGP(DPLMN, LOGL_INFO, "new state %s -> %s\n",
 452                 plmn_a_state_names[plmn->state], plmn_a_state_names[state]);
 453
 454         plmn->state = state;
 455 }
 456
 457 /* new manual PLMN search state */
 458 static void new_m_state(struct gsm322_plmn *plmn, int state)
 459 {
 460         if (plmn->mode != PLMN_MODE_MANUAL) {
 461                 LOGP(DPLMN, LOGL_FATAL, "not in manual mode, please fix!\n");
 462                 return;
 463         }
 464
 465         if (state < 0 || state >= (sizeof(plmn_m_state_names) / sizeof(char *)))
 466                 return;
 467
 468         LOGP(DPLMN, LOGL_INFO, "new state %s -> %s\n",
 469                 plmn_m_state_names[plmn->state], plmn_m_state_names[state]);
 470
 471         plmn->state = state;
 472 }
 473
 474 /* new Cell selection state */
 475 static void new_c_state(struct gsm322_cellsel *cs, int state)
 476 {
 477         if (state < 0 || state >= (sizeof(cs_state_names) / sizeof(char *)))
 478                 return;
 479
 480         LOGP(DCS, LOGL_INFO, "new state %s -> %s\n",
 481                 cs_state_names[cs->state], cs_state_names[state]);
 482
 483         /* stop cell selection timer, if running */
 484         stop_cs_timer(cs);
 485
 486         /* stop scanning of power measurement */
 487         if (cs->powerscan) {
 488 #ifdef TODO
 489                 stop power scanning
 490 #endif
 491                 cs->powerscan = 0;
 492         }
 493
 494         cs->state = state;
 495 }
 496
 497 /*
 498  * list of PLMNs
 499  */
 500
 501 /* 4.4.3 create sorted list of PLMN
 502  *
 503  * the source of entries are
 504  *
 505  * - HPLMN
 506  * - entries found in the SIM's PLMN Selector list
 507  * - scanned PLMNs above -85 dB (random order)
 508  * - scanned PLMNs below or equal -85 (by received level)
 509  *
 510  * NOTE:
 511  *
 512  * The list only includes networks found at last scan.
 513  *
 514  * The list always contains HPLMN if available, even if not used by PLMN
 515  * search process at some conditions.
 516  *
 517  * The list contains all PLMNs even if not allowed, so entries have to be
 518  * removed when selecting from the list. (In case we use manual cell selection,
 519  * we need to provide non-allowed networks also.)
 520  */
 521 static int gsm322_sort_list(struct osmocom_ms *ms)
 522 {
 523         struct gsm322_plmn *plmn = &ms->plmn;
 524         struct gsm322_cellsel *cs = &ms->cellsel;
 525         struct gsm_subscriber *subscr = &ms->subscr;
 526         struct gsm_sub_plmn_list *sim_entry;
 527         struct gsm_sub_plmn_na *na_entry;
 528         struct llist_head temp_list;
 529         struct gsm322_plmn_list *temp, *found;
 530         struct llist_head *lh, *lh2;
 531         int i, entries, move;
 532         int8_t search_db = 0;
 533
 534         /* flush list */
 535         llist_for_each_safe(lh, lh2, &plmn->sorted_plmn) {
 536                 llist_del(lh);
 537                 talloc_free(lh);
 538         }
 539
 540         /* Create a temporary list of all networks */
 541         INIT_LLIST_HEAD(&temp_list);
 542         for (i = 0; i <= 1023; i++) {
 543                 if (!(cs->list[i].flags & GSM322_CS_FLAG_TEMP_AA))
 544                         continue;
 545
 546                 /* search if network has multiple cells */
 547                 found = NULL;
 548                 llist_for_each_entry(temp, &temp_list, entry) {
 549                         if (temp->mcc == cs->list[i].mcc
 550                          && temp->mnc == cs->list[i].mnc)
 551                                 found = temp;
 552                                 break;
 553                 }
 554                 /* update or create */
 555                 if (found) {
 556                         if (cs->list[i].rxlev_db > found->rxlev_db)
 557                                 found->rxlev_db = cs->list[i].rxlev_db;
 558                 } else {
 559                         temp = talloc_zero(l23_ctx, struct gsm322_plmn_list);
 560                         if (!temp)
 561                                 return -ENOMEM;
 562                         temp->mcc = cs->list[i].mcc;
 563                         temp->mnc = cs->list[i].mnc;
 564                         temp->rxlev_db = cs->list[i].rxlev_db;
 565                         llist_add_tail(&temp->entry, &temp_list);
 566                 }
 567         }
 568
 569         /* move Home PLMN, if in list */
 570         if (subscr->sim_valid) {
 571                 found = NULL;
 572                 llist_for_each_entry(temp, &temp_list, entry) {
 573                         if (temp->mcc == subscr->mcc
 574                          && temp->mnc == subscr->mnc) {
 575                                 found = temp;
 576                                 break;
 577                         }
 578                 }
 579                 if (found) {
 580                         llist_del(&found->entry);
 581                         llist_add_tail(&found->entry, &plmn->sorted_plmn);
 582                 }
 583         }
 584
 585         /* move entries if in SIM's PLMN Selector list */
 586         llist_for_each_entry(sim_entry, &subscr->plmn_list, entry) {
 587                 found = NULL;
 588                 llist_for_each_entry(temp, &temp_list, entry) {
 589                         if (temp->mcc == sim_entry->mcc
 590                          && temp->mnc == sim_entry->mnc) {
 591                                 found = temp;
 592                                 break;
 593                         }
 594                 }
 595                 if (found) {
 596                         llist_del(&found->entry);
 597                         llist_add_tail(&found->entry, &plmn->sorted_plmn);
 598                 }
 599         }
 600
 601         /* move PLMN above -85 dBm in random order */
 602         entries = 0;
 603         llist_for_each_entry(temp, &temp_list, entry) {
 604                 if (temp->rxlev_db > -85)
 605                         entries++;
 606         }
 607         while(entries) {
 608                 move = random() % entries;
 609                 i = 0;
 610                 llist_for_each_entry(temp, &temp_list, entry) {
 611                         if (temp->rxlev_db > -85) {
 612                                 if (i == move) {
 613                                         llist_del(&temp->entry);
 614                                         llist_add_tail(&temp->entry,
 615                                                 &plmn->sorted_plmn);
 616                                         break;
 617                                 }
 618                                 i++;
 619                         }
 620                 }
 621                 entries--;
 622         }
 623
 624         /* move ohter PLMN in decreasing order */
 625         while(1) {
 626                 found = NULL;
 627                 llist_for_each_entry(temp, &temp_list, entry) {
 628                         if (!found
 629                          || temp->rxlev_db > search_db) {
 630                                 search_db = temp->rxlev_db;
 631                                 found = temp;
 632                         }
 633                 }
 634                 if (!found)
 635                         break;
 636                 llist_del(&found->entry);
 637                 llist_add_tail(&found->entry, &plmn->sorted_plmn);
 638         }
 639
 640         /* mark forbidden PLMNs, if in list of forbidden networks */
 641         i = 0;
 642         llist_for_each_entry(temp, &plmn->sorted_plmn, entry) {
 643                 llist_for_each_entry(na_entry, &subscr->plmn_na, entry) {
 644                         if (temp->mcc == na_entry->mcc
 645                          && temp->mnc == na_entry->mnc) {
 646                                 temp->cause = na_entry->cause;
 647                                 break;
 648                         }
 649                 }
 650                 LOGP(DPLMN, LOGL_INFO, "Crating Sorted PLMN list. "
 651                         "(%02d: mcc=%03d mnc=%02d allowed=%s rx-lev=%d)\n",
 652                         i, temp->mcc, temp->mnc, (temp->cause) ? "no ":"yes",
 653                         temp->rxlev_db);
 654                 i++;
 655         }
 656
 657         return 0;
 658 }
 659
 660 /*
 661  * handler for automatic search
 662  */
 663
 664 /* go On PLMN state */
 665 static int gsm322_a_go_on_plmn(struct osmocom_ms *ms, struct msgb *msg)
 666 {
 667         struct gsm322_plmn *plmn = &ms->plmn;
 668         struct gsm_subscriber *subscr = &ms->subscr;
 669
 670         /* set last registered PLMN */
 671         subscr->plmn_valid = 1;
 672         subscr->plmn_mcc = plmn->mcc;
 673         subscr->plmn_mnc = plmn->mnc;
 674 #ifdef TODO
 675         store on sim
 676 #endif
 677
 678         new_a_state(plmn, GSM322_A2_ON_PLMN);
 679
 680         /* start timer, if on VPLMN of home country OR special case */
 681         if ((plmn->mcc == subscr->mcc && plmn->mcc != subscr->mnc)
 682          || (subscr->always_search_hplmn && (plmn->mcc != subscr->mnc
 683                 || plmn->mcc != subscr->mnc))) {
 684                 if (subscr->sim_valid && subscr->t6m_hplmn)
 685                         start_plmn_timer(plmn, subscr->t6m_hplmn * 360);
 686                 else
 687                         start_plmn_timer(plmn, 30 * 360);
 688         } else
 689                 stop_plmn_timer(plmn);
 690
 691         return 0;
 692 }
 693
 694 /* indicate selected PLMN */
 695 static int gsm322_a_indicate_selected(struct osmocom_ms *ms, struct msgb *msg)
 696 {
 697 #ifdef TODO
 698         indicate selected plmn to user
 699 #endif
 700
 701         return gsm322_a_go_on_plmn(ms, msg);
 702 }
 703
 704 /* no (more) PLMN in list */
 705 static int gsm322_a_no_more_plmn(struct osmocom_ms *ms, struct msgb *msg)
 706 {
 707         struct gsm322_plmn *plmn = &ms->plmn;
 708         struct gsm322_cellsel *cs = &ms->cellsel;
 709         struct gsm_subscriber *subscr = &ms->subscr;
 710         struct msgb *nmsg;
 711         int found;
 712
 713         /* any PLMN available */
 714         found = gsm322_cs_select(ms, 0);
 715
 716         /* if no PLMN in list */
 717         if (found < 0) {
 718                 if (subscr->plmn_valid) {
 719                         LOGP(DPLMN, LOGL_INFO, "Select RPLMN.\n");
 720                         plmn->mcc = subscr->plmn_mcc;
 721                         plmn->mnc = subscr->plmn_mnc;
 722                 } else {
 723                         LOGP(DPLMN, LOGL_INFO, "Select HPLMN.\n");
 724                         plmn->mcc = subscr->mcc;
 725                         plmn->mnc = subscr->mnc;
 726                 }
 727
 728                 new_a_state(plmn, GSM322_A4_WAIT_FOR_PLMN);
 729
 730                 /* we must forward this, otherwhise "Any cell selection"
 731                  * will not start automatically.
 732                  */
 733                 nmsg = gsm322_msgb_alloc(GSM322_EVENT_NO_CELL_FOUND);
 734                 if (!nmsg)
 735                         return -ENOMEM;
 736                 gsm322_cs_sendmsg(ms, nmsg);
 737
 738                 return 0;
 739         }
 740
 741         /* select first PLMN in list */
 742         plmn->mcc = cs->list[found].mcc;
 743         plmn->mnc = cs->list[found].mnc;
 744
 745         LOGP(DPLMN, LOGL_INFO, "PLMN available (mcc=%03d mnc=%02d  %s, %s)\n",
 746                 plmn->mcc, plmn->mnc,
 747                 gsm_get_mcc(plmn->mcc), gsm_get_mnc(plmn->mcc, plmn->mnc));
 748
 749         /* indicate New PLMN */
 750         nmsg = gsm322_msgb_alloc(GSM322_EVENT_NEW_PLMN);
 751         if (!nmsg)
 752                 return -ENOMEM;
 753         gsm322_cs_sendmsg(ms, nmsg);
 754
 755         /* go On PLMN */
 756         return gsm322_a_indicate_selected(ms, msg);
 757 }
 758
 759 /* select first PLMN in list */
 760 static int gsm322_a_sel_first_plmn(struct osmocom_ms *ms, struct msgb *msg)
 761 {
 762         struct gsm322_plmn *plmn = &ms->plmn;
 763         struct msgb *nmsg;
 764         struct gsm322_plmn_list *plmn_entry;
 765         struct gsm322_plmn_list *plmn_first = NULL;
 766         int i;
 767
 768         /* generate list */
 769         gsm322_sort_list(ms);
 770
 771         /* select first entry */
 772         i = 0;
 773         llist_for_each_entry(plmn_entry, &plmn->sorted_plmn, entry) {
 774                 /* if RPLMN is HPLMN, we skip that */
 775                 if (plmn->state == GSM322_A1_TRYING_RPLMN
 776                  && plmn_entry->mcc == plmn->mcc
 777                  && plmn_entry->mnc == plmn->mnc) {
 778                         i++;
 779                         continue;
 780                 }
 781                 /* select first allowed network */
 782                 if (!plmn_entry->cause) {
 783                         plmn_first = plmn_entry;
 784                         break;
 785                 }
 786                 i++;
 787         }
 788         plmn->plmn_curr = i;
 789
 790         /* if no PLMN in list */
 791         if (!plmn_first) {
 792                 LOGP(DPLMN, LOGL_INFO, "No PLMN in list.\n");
 793                 gsm322_a_no_more_plmn(ms, msg);
 794
 795                 return 0;
 796         }
 797
 798         LOGP(DPLMN, LOGL_INFO, "Selecting PLMN from list. (%02d: mcc=%03d "
 799                 "mnc=%02d  %s, %s)\n", plmn->plmn_curr, plmn_first->mcc,
 800                 plmn_first->mnc, gsm_get_mcc(plmn_first->mcc),
 801                 gsm_get_mnc(plmn_first->mcc, plmn_first->mnc));
 802
 803         /* set current network */
 804         plmn->mcc = plmn_first->mcc;
 805         plmn->mnc = plmn_first->mnc;
 806
 807         new_a_state(plmn, GSM322_A3_TRYING_PLMN);
 808
 809         /* indicate New PLMN */
 810         nmsg = gsm322_msgb_alloc(GSM322_EVENT_NEW_PLMN);
 811         if (!nmsg)
 812                 return -ENOMEM;
 813         gsm322_cs_sendmsg(ms, nmsg);
 814
 815         return 0;
 816 }
 817
 818 /* select next PLMN in list */
 819 static int gsm322_a_sel_next_plmn(struct osmocom_ms *ms, struct msgb *msg)
 820 {
 821         struct gsm322_plmn *plmn = &ms->plmn;
 822         struct msgb *nmsg;
 823         struct gsm322_plmn_list *plmn_entry;
 824         struct gsm322_plmn_list *plmn_next = NULL;
 825         int i, ii;
 826
 827         /* select next entry from list */
 828         i = 0;
 829         ii = plmn->plmn_curr + 1;
 830         llist_for_each_entry(plmn_entry, &plmn->sorted_plmn, entry) {
 831                 /* skip previously selected networks */
 832                 if (i < ii) {
 833                         i++;
 834                         continue;
 835                 }
 836                 /* select next allowed network */
 837                 if (!plmn_entry->cause) {
 838                         plmn_next = plmn_entry;
 839                         break;
 840                 }
 841                 i++;
 842         }
 843         plmn->plmn_curr = i;
 844
 845         /* if no more PLMN in list */
 846         if (!plmn_next) {
 847                 LOGP(DPLMN, LOGL_INFO, "No more PLMN in list.\n");
 848                 gsm322_a_no_more_plmn(ms, msg);
 849                 return 0;
 850         }
 851
 852
 853         LOGP(DPLMN, LOGL_INFO, "Selecting PLMN from list. (%02d: mcc=%03d "
 854                 "mnc=%02d  %s, %s)\n", plmn->plmn_curr, plmn_next->mcc,
 855                 plmn_next->mnc,
 856                 gsm_get_mcc(plmn->mcc), gsm_get_mnc(plmn->mcc, plmn->mnc));
 857
 858         /* set next network */
 859         plmn->mcc = plmn_next->mcc;
 860         plmn->mnc = plmn_next->mnc;
 861
 862         new_a_state(plmn, GSM322_A3_TRYING_PLMN);
 863
 864         /* indicate New PLMN */
 865         nmsg = gsm322_msgb_alloc(GSM322_EVENT_NEW_PLMN);
 866         if (!nmsg)
 867                 return -ENOMEM;
 868         gsm322_cs_sendmsg(ms, nmsg);
 869
 870         return 0;
 871 }
 872
 873 /* User re-selection event */
 874 static int gsm322_a_user_reselection(struct osmocom_ms *ms, struct msgb *msg)
 875 {
 876         struct gsm322_plmn *plmn = &ms->plmn;
 877         struct gsm322_plmn_list *plmn_entry;
 878         struct gsm322_plmn_list *plmn_found = NULL;
 879
 880         /* search current PLMN in list */
 881         llist_for_each_entry(plmn_entry, &plmn->sorted_plmn, entry) {
 882                 if (plmn_entry->mcc == plmn->mcc
 883                  && plmn_entry->mnc == plmn->mnc)
 884                         plmn_found = plmn_entry;
 885                         break;
 886         }
 887
 888         /* abort if list is empty */
 889         if (!plmn_found) {
 890                 LOGP(DPLMN, LOGL_INFO, "Selected PLMN not in list, strange!\n");
 891                 return 0;
 892         }
 893
 894         LOGP(DPLMN, LOGL_INFO, "Movin selected PLMN to the bottom of the list "
 895                 "and restarting PLMN search process.\n");
 896
 897         /* move entry to end of list */
 898         llist_del(&plmn_found->entry);
 899         llist_add_tail(&plmn_found->entry, &plmn->sorted_plmn);
 900
 901         /* select first PLMN in list */
 902         return gsm322_a_sel_first_plmn(ms, msg);
 903 }
 904
 905 /* PLMN becomes available */
 906 static int gsm322_a_plmn_avail(struct osmocom_ms *ms, struct msgb *msg)
 907 {
 908         struct gsm322_plmn *plmn = &ms->plmn;
 909         struct gsm_subscriber *subscr = &ms->subscr;
 910         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
 911
 912         if (subscr->plmn_valid && subscr->plmn_mcc == gm->mcc
 913          && subscr->plmn_mnc == gm->mnc) {
 914                 /* go On PLMN */
 915                 plmn->mcc = gm->mcc;
 916                 plmn->mnc = gm->mnc;
 917                 LOGP(DPLMN, LOGL_INFO, "HPLMN became available.\n");
 918                 return gsm322_a_go_on_plmn(ms, msg);
 919         } else {
 920                 /* select first PLMN in list */
 921                 LOGP(DPLMN, LOGL_INFO, "PLMN became available, start PLMN "
 922                         "search process.\n");
 923                 return gsm322_a_sel_first_plmn(ms, msg);
 924         }
 925 }
 926
 927 /* loss of radio coverage */
 928 static int gsm322_a_loss_of_radio(struct osmocom_ms *ms, struct msgb *msg)
 929 {
 930         struct gsm322_plmn *plmn = &ms->plmn;
 931         struct gsm322_cellsel *cs = &ms->cellsel;
 932         int found;
 933
 934         /* any PLMN available */
 935         found = gsm322_cs_select(ms, 0);
 936
 937         /* if PLMN in list */
 938         if (found >= 0) {
 939                 LOGP(DPLMN, LOGL_INFO, "PLMN available (mcc=%03d mnc=%02d  "
 940                         "%s, %s)\n", cs->list[found].mcc, cs->list[found].mnc,
 941                         gsm_get_mcc(cs->list[found].mcc),
 942                         gsm_get_mnc(cs->list[found].mcc, cs->list[found].mnc));
 943                 return gsm322_a_sel_first_plmn(ms, msg);
 944         }
 945
 946         LOGP(DPLMN, LOGL_INFO, "PLMN not available.\n");
 947
 948         plmn->mcc = plmn->mnc = 0;
 949
 950         new_a_state(plmn, GSM322_A4_WAIT_FOR_PLMN);
 951
 952         return 0;
 953 }
 954
 955 /* MS is switched on OR SIM is inserted OR removed */
 956 static int gsm322_a_switch_on(struct osmocom_ms *ms, struct msgb *msg)
 957 {
 958         struct gsm_subscriber *subscr = &ms->subscr;
 959         struct gsm322_plmn *plmn = &ms->plmn;
 960         struct msgb *nmsg;
 961
 962         if (!subscr->sim_valid) {
 963                 LOGP(DPLMN, LOGL_INFO, "Switch on without SIM.\n");
 964                 new_a_state(plmn, GSM322_A6_NO_SIM);
 965
 966                 return 0;
 967         }
 968
 969         /* if there is a registered PLMN */
 970         if (subscr->plmn_valid) {
 971                 /* select the registered PLMN */
 972                 plmn->mcc = subscr->plmn_mcc;
 973                 plmn->mnc = subscr->plmn_mnc;
 974
 975                 LOGP(DPLMN, LOGL_INFO, "Use RPLMN (mcc=%03d mnc=%02d  "
 976                         "%s, %s)\n", plmn->mcc, plmn->mnc,
 977                         gsm_get_mcc(plmn->mcc),
 978                         gsm_get_mnc(plmn->mcc, plmn->mnc));
 979
 980                 new_a_state(plmn, GSM322_A1_TRYING_RPLMN);
 981
 982                 /* indicate New PLMN */
 983                 nmsg = gsm322_msgb_alloc(GSM322_EVENT_NEW_PLMN);
 984                 if (!nmsg)
 985                         return -ENOMEM;
 986                 gsm322_cs_sendmsg(ms, nmsg);
 987
 988                 return 0;
 989         }
 990
 991         /* select first PLMN in list */
 992         return gsm322_a_sel_first_plmn(ms, msg);
 993 }
 994
 995 /* MS is switched off */
 996 static int gsm322_a_switch_off(struct osmocom_ms *ms, struct msgb *msg)
 997 {
 998         struct gsm322_plmn *plmn = &ms->plmn;
 999
1000         new_a_state(plmn, GSM322_A0_NULL);
1001
1002         return 0;
1003 }
1004
1005 /* SIM is removed */
1006 static int gsm322_a_sim_removed(struct osmocom_ms *ms, struct msgb *msg)
1007 {
1008         struct msgb *nmsg;
1009
1010         /* indicate SIM remove to cell selection process */
1011         nmsg = gsm322_msgb_alloc(GSM322_EVENT_SIM_REMOVE);
1012         if (!nmsg)
1013                 return -ENOMEM;
1014         gsm322_cs_sendmsg(ms, nmsg);
1015
1016         return gsm322_a_switch_on(ms, msg);
1017 }
1018
1019 /* location update response: "Roaming not allowed" */
1020 static int gsm322_a_roaming_na(struct osmocom_ms *ms, struct msgb *msg)
1021 {
1022         /* store in list of forbidden LAs is done in gsm48* */
1023
1024         return gsm322_a_sel_first_plmn(ms, msg);
1025 }
1026
1027 /* On VPLMN of home country and timeout occurs */
1028 static int gsm322_a_hplmn_search(struct osmocom_ms *ms, struct msgb *msg)
1029 {
1030         struct gsm322_plmn *plmn = &ms->plmn;
1031         struct gsm322_cellsel *cs = &ms->cellsel;
1032         struct msgb *nmsg;
1033
1034         /* try again later, if not idle */
1035         if (cs->state != GSM322_C3_CAMPED_NORMALLY) {
1036                 LOGP(DPLMN, LOGL_INFO, "Not camping normal, wait some more.\n");
1037                 start_plmn_timer(plmn, 60);
1038
1039                 return 0;
1040         }
1041
1042         new_a_state(plmn, GSM322_A5_HPLMN_SEARCH);
1043
1044         /* initiate search at cell selection */
1045         nmsg = gsm322_msgb_alloc(GSM322_EVENT_HPLMN_SEARCH);
1046         if (!nmsg)
1047                 return -ENOMEM;
1048         gsm322_cs_sendmsg(ms, nmsg);
1049
1050         return 0;
1051 }
1052
1053 /* manual mode selected */
1054 static int gsm322_a_sel_manual(struct osmocom_ms *ms, struct msgb *msg)
1055 {
1056         struct gsm322_plmn *plmn = &ms->plmn;
1057
1058         /* restart state machine */
1059         gsm322_a_switch_off(ms, msg);
1060         plmn->mode = PLMN_MODE_MANUAL;
1061         gsm322_m_switch_on(ms, msg);
1062
1063         return 0;
1064 }
1065
1066 /*
1067  * handler for manual search
1068  */
1069
1070 /* go Not on PLMN state */
1071 static int gsm322_m_go_not_on_plmn(struct osmocom_ms *ms, struct msgb *msg)
1072 {
1073         struct gsm322_plmn *plmn = &ms->plmn;
1074
1075         new_m_state(plmn, GSM322_M3_NOT_ON_PLMN);
1076
1077         return 0;
1078 }
1079
1080 /* display PLMNs and to Not on PLMN */
1081 static int gsm322_m_display_plmns(struct osmocom_ms *ms, struct msgb *msg)
1082 {
1083         /* generate list */
1084         gsm322_sort_list(ms);
1085
1086 #ifdef TODO
1087         display PLMNs to user
1088 #endif
1089
1090         /* go Not on PLMN state */
1091         return gsm322_m_go_not_on_plmn(ms, msg);
1092 }
1093
1094 /* MS is switched on OR SIM is inserted OR removed */
1095 static int gsm322_m_switch_on(struct osmocom_ms *ms, struct msgb *msg)
1096 {
1097         struct gsm_subscriber *subscr = &ms->subscr;
1098         struct gsm322_plmn *plmn = &ms->plmn;
1099
1100         if (!subscr->sim_valid) {
1101                 LOGP(DPLMN, LOGL_INFO, "Switch on without SIM.\n");
1102                 new_m_state(plmn, GSM322_M5_NO_SIM);
1103
1104                 return 0;
1105         }
1106
1107         /* if there is a registered PLMN */
1108         if (subscr->plmn_valid) {
1109                 /* select the registered PLMN */
1110                 plmn->mcc = subscr->plmn_mcc;
1111                 plmn->mnc = subscr->plmn_mnc;
1112
1113                 LOGP(DPLMN, LOGL_INFO, "Use RPLMN (mcc=%03d mnc=%02d  "
1114                         "%s, %s)\n", plmn->mcc, plmn->mnc,
1115                         gsm_get_mcc(plmn->mcc),
1116                         gsm_get_mnc(plmn->mcc, plmn->mnc));
1117
1118                 new_m_state(plmn, GSM322_M1_TRYING_RPLMN);
1119
1120                 return 0;
1121         }
1122
1123         /* display PLMNs */
1124         return gsm322_m_display_plmns(ms, msg);
1125 }
1126
1127 /* MS is switched off */
1128 static int gsm322_m_switch_off(struct osmocom_ms *ms, struct msgb *msg)
1129 {
1130         struct gsm322_plmn *plmn = &ms->plmn;
1131
1132         stop_plmn_timer(plmn);
1133
1134         new_m_state(plmn, GSM322_M0_NULL);
1135
1136         return 0;
1137 }
1138
1139 /* SIM is removed */
1140 static int gsm322_m_sim_removed(struct osmocom_ms *ms, struct msgb *msg)
1141 {
1142         struct gsm322_plmn *plmn = &ms->plmn;
1143         struct msgb *nmsg;
1144
1145         stop_plmn_timer(plmn);
1146
1147         /* indicate SIM remove to cell selection process */
1148         nmsg = gsm322_msgb_alloc(GSM322_EVENT_SIM_REMOVE);
1149         if (!nmsg)
1150                 return -ENOMEM;
1151         gsm322_cs_sendmsg(ms, nmsg);
1152
1153         return gsm322_m_switch_on(ms, msg);
1154 }
1155
1156 /* go to On PLMN state */
1157 static int gsm322_m_go_on_plmn(struct osmocom_ms *ms, struct msgb *msg)
1158 {
1159         struct gsm322_plmn *plmn = &ms->plmn;
1160         struct gsm_subscriber *subscr = &ms->subscr;
1161
1162         /* if selected PLMN is in list of forbidden PLMNs */
1163         gsm322_del_forbidden_plmn(ms, plmn->mcc, plmn->mnc);
1164
1165         /* set last registered PLMN */
1166         subscr->plmn_valid = 1;
1167         subscr->plmn_mcc = plmn->mcc;
1168         subscr->plmn_mnc = plmn->mnc;
1169 #ifdef TODO
1170         store on sim
1171 #endif
1172
1173         new_m_state(plmn, GSM322_M2_ON_PLMN);
1174
1175         return 0;
1176 }
1177
1178 /* indicate selected PLMN */
1179 static int gsm322_m_indicate_selected(struct osmocom_ms *ms, struct msgb *msg)
1180 {
1181 #ifdef TODO
1182         indicate selected plmn to user
1183 #endif
1184
1185         return gsm322_m_go_on_plmn(ms, msg);
1186 }
1187
1188 /* previously selected PLMN becomes available again */
1189 static int gsm322_m_plmn_avail(struct osmocom_ms *ms, struct msgb *msg)
1190 {
1191         struct gsm322_plmn *plmn = &ms->plmn;
1192
1193         new_m_state(plmn, GSM322_M1_TRYING_RPLMN);
1194
1195         return 0;
1196 }
1197
1198 /* the user has selected given PLMN */
1199 static int gsm322_m_choose_plmn(struct osmocom_ms *ms, struct msgb *msg)
1200 {
1201         struct gsm322_plmn *plmn = &ms->plmn;
1202         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
1203
1204         /* use user selection */
1205         plmn->mcc = gm->mcc;
1206         plmn->mnc = gm->mnc;
1207
1208         LOGP(DPLMN, LOGL_INFO, "User selects PLMN. (mcc=%03d mnc=%02d  "
1209                 "%s, %s)\n", plmn->mcc, plmn->mnc,
1210                 gsm_get_mcc(plmn->mcc), gsm_get_mnc(plmn->mcc, plmn->mnc));
1211
1212         new_m_state(plmn, GSM322_M4_TRYING_PLMN);
1213
1214         return 0;
1215 }
1216
1217 /* auto mode selected */
1218 static int gsm322_m_sel_auto(struct osmocom_ms *ms, struct msgb *msg)
1219 {
1220         struct gsm322_plmn *plmn = &ms->plmn;
1221
1222         /* restart state machine */
1223         gsm322_m_switch_off(ms, msg);
1224         plmn->mode = PLMN_MODE_AUTO;
1225         gsm322_a_switch_on(ms, msg);
1226
1227         return 0;
1228 }
1229
1230 /* if no cell is found in other states than in *_TRYING_* states */
1231 static int gsm322_am_no_cell_found(struct osmocom_ms *ms, struct msgb *msg)
1232 {
1233         struct msgb *nmsg;
1234
1235         /* Tell cell selection process to handle "no cell found". */
1236         nmsg = gsm322_msgb_alloc(GSM322_EVENT_NO_CELL_FOUND);
1237         if (!nmsg)
1238                 return -ENOMEM;
1239         gsm322_cs_sendmsg(ms, nmsg);
1240
1241         return 0;
1242 }
1243
1244 /*
1245  * cell scanning process
1246  */
1247
1248 /* select a suitable and allowable cell */
1249 static int gsm322_cs_select(struct osmocom_ms *ms, int any)
1250 {
1251         struct gsm322_cellsel *cs = &ms->cellsel;
1252         struct gsm_subscriber *subscr = &ms->subscr;
1253         int i, found = -1, power = 0;
1254         uint8_t flags, mask;
1255         uint16_t acc_class;
1256
1257         /* set out access class depending on the cell selection type */
1258         if (any) {
1259                 acc_class = subscr->acc_class | 0x0400; /* add emergency */
1260                 LOGP(DCS, LOGL_INFO, "Using access class with Emergency "
1261                         "class.\n");
1262         } else {
1263                 acc_class = subscr->acc_class & 0xfbff; /* remove emergency */
1264                 LOGP(DCS, LOGL_INFO, "Using access class without Emergency "
1265                         "class\n");
1266         }
1267
1268         /* flags to match */
1269         mask = GSM322_CS_FLAG_SUPPORT | GSM322_CS_FLAG_POWER
1270                 | GSM322_CS_FLAG_SIGNAL | GSM322_CS_FLAG_SYSINFO;
1271         if (cs->state == GSM322_C2_STORED_CELL_SEL)
1272                 mask |= GSM322_CS_FLAG_BA;
1273         flags = mask; /* all masked flags are requied */
1274
1275         /* loop through all scanned frequencies and select cell */
1276         for (i = 0; i <= 1023; i++) {
1277                 cs->list[i].flags &= ~GSM322_CS_FLAG_TEMP_AA;
1278
1279                 /* channel has no informations for us */
1280                 if ((cs->list[i].flags & mask) != flags) {
1281                         continue;
1282                 }
1283
1284                 /* check C1 criteria not fullfilled */
1285                 // TODO: C1 is also dependant on power class and max power
1286                 if (cs->list[i].rxlev_db < cs->list[i].min_db) {
1287                         LOGP(DCS, LOGL_INFO, "Skip frequency %d: C1 criteria "
1288                                 "not met. (rxlev=%d < min=%d)\n", i,
1289                                 cs->list[i].rxlev_db, cs->list[i].min_db);
1290                         continue;
1291                 }
1292
1293                 /* if cell is barred and we don't override */
1294                 if (!subscr->acc_barr
1295                  && (cs->list[i].flags & GSM322_CS_FLAG_BARRED)) {
1296                         LOGP(DCS, LOGL_INFO, "Skip frequency %d: Cell is "
1297                                 "barred.\n", i);
1298                         continue;
1299                 }
1300
1301                 /* if cell is in list of forbidden LAs */
1302                 if (!subscr->acc_barr
1303                  && (cs->list[i].flags & GSM322_CS_FLAG_FORBIDD)) {
1304                         LOGP(DCS, LOGL_INFO, "Skip frequency %d: Cell is in "
1305                                 "list of forbidden LAs. (mcc=%03d mnc=%02d "
1306                                 "lai=%04x)\n", i, cs->list[i].mcc,
1307                                 cs->list[i].mnc, cs->list[i].lac);
1308                         continue;
1309                 }
1310
1311                 /* if we have no access to the cell and we don't override */
1312                 if (!subscr->acc_barr
1313                  && !(acc_class & (cs->list[i].class_barr ^ 0xffff))) {
1314                         LOGP(DCS, LOGL_INFO, "Skip frequency %d: Class is "
1315                                 "barred for out access. (access=%04x "
1316                                 "barred=%04x)\n", i, acc_class,
1317                                 cs->list[i].class_barr);
1318                         continue;
1319                 }
1320
1321                 /* store temporary available and allowable flag */
1322                 cs->list[i].flags |= GSM322_CS_FLAG_TEMP_AA;
1323
1324                 /* if we search a specific PLMN, but it does not match */
1325                 if (!any && (cs->mcc != cs->list[i].mcc
1326                                 || cs->mnc != cs->list[i].mnc)) {
1327                         LOGP(DCS, LOGL_INFO, "Skip frequency %d: PLMN of cell "
1328                                 "does not match target PLMN. (mcc=%03d "
1329                                 "mnc=%02d)\n", i, cs->list[i].mcc,
1330                                 cs->list[i].mnc);
1331                         continue;
1332                 }
1333
1334                 LOGP(DCS, LOGL_INFO, "Cell frequency %d: Cell found, (rxlev=%d "
1335                         "mcc=%03d mnc=%02d lac=%04x  %s, %s)\n", i,
1336                         cs->list[i].rxlev_db, cs->list[i].mcc, cs->list[i].mnc,
1337                         cs->list[i].lac, gsm_get_mcc(cs->list[i].mcc),
1338                         gsm_get_mnc(cs->list[i].mcc, cs->list[i].mnc));
1339
1340                 /* find highest power cell */
1341                 if (found < 0 || cs->list[i].rxlev_db > power) {
1342                         power = cs->list[i].rxlev_db;
1343                         found = i;
1344                 }
1345         }
1346
1347         gsm322_dump_sorted_plmn(ms);
1348
1349         if (found >= 0)
1350                 LOGP(DCS, LOGL_INFO, "Cell frequency %d selected.\n", found);
1351
1352         return found;
1353 }
1354
1355 /* tune to first/next unscanned frequency and search for PLMN */
1356 static int gsm322_cs_scan(struct osmocom_ms *ms)
1357 {
1358         struct gsm322_cellsel *cs = &ms->cellsel;
1359         struct gsm_subscriber *subscr = &ms->subscr;
1360         struct gsm48_sysinfo *s = &ms->sysinfo;
1361         struct gsm48_rrlayer *rr = &ms->rrlayer;
1362         int i, j;
1363         uint8_t mask, flags;
1364         uint32_t weight = 0, test = cs->scan_state;
1365
1366         if (rr->state != GSM48_RR_ST_IDLE) {
1367                 LOGP(DCS, LOGL_FATAL, "This must only happen in IDLE mode, "
1368                         "please fix!\n");
1369                 return -EINVAL;
1370         }
1371
1372         /* special prositive case for HPLMN search */
1373         if (cs->state == GSM322_HPLMN_SEARCH && s->mcc == subscr->mcc
1374          && s->mnc == subscr->mnc) {
1375                 struct msgb *nmsg;
1376
1377                 nmsg = gsm322_msgb_alloc(GSM322_EVENT_HPLMN_FOUND);
1378                 LOGP(DCS, LOGL_INFO, "HPLMN cell available.\n");
1379                 if (!nmsg)
1380                         return -ENOMEM;
1381                 gsm322_plmn_sendmsg(ms, nmsg);
1382
1383                 return 0;
1384         }
1385
1386         /* search for strongest unscanned cell */
1387         mask = GSM322_CS_FLAG_SUPPORT | GSM322_CS_FLAG_POWER
1388                 | GSM322_CS_FLAG_SIGNAL;
1389         if (cs->state == GSM322_C2_STORED_CELL_SEL)
1390                 mask |= GSM322_CS_FLAG_BA;
1391         flags = mask; /* all masked flags are requied */
1392         for (i = 0; i <= 1023; i++) {
1393                 /* skip if band has enough frequencies scanned (3.2.1) */
1394                 for (j = 0; gsm_sup_smax[j].max; j++) {
1395                         if (gsm_sup_smax[j].end > gsm_sup_smax[j].start) {
1396                                 if (gsm_sup_smax[j].start >= i
1397                                  && gsm_sup_smax[j].end <= i)
1398                                         break;
1399                         } else {
1400                                 if (gsm_sup_smax[j].end <= i
1401                                  || gsm_sup_smax[j].start >= i)
1402                                         break;
1403                         }
1404                 }
1405                 if (gsm_sup_smax[j].max) {
1406                         if (gsm_sup_smax[j].temp == gsm_sup_smax[j].max)
1407                                 continue;
1408                 }
1409                 /* search for unscanned frequency */
1410                 if ((cs->list[i].flags & mask) == flags) {
1411                         /* weight depends on the power level
1412                          * if it is the same, it depends on arfcn
1413                          */
1414                         test = cs->list[i].rxlev_db + 128;
1415                         test = (test << 16) | i;
1416                         if (test >= cs->scan_state)
1417                                 continue;
1418                         if (test > weight)
1419                                 weight = test;
1420                 }
1421         }
1422         cs->scan_state = weight;
1423
1424         if (!weight)
1425                 gsm322_dump_cs_list(ms);
1426
1427         /* special negative case for HPLMN search */
1428         if (cs->state == GSM322_HPLMN_SEARCH && !weight) {
1429                 struct msgb *nmsg;
1430
1431                 nmsg = gsm322_msgb_alloc(GSM322_EVENT_HPLMN_NOT_FOUND);
1432                 LOGP(DCS, LOGL_INFO, "No HPLMN cell available.\n");
1433                 if (!nmsg)
1434                         return -ENOMEM;
1435                 gsm322_plmn_sendmsg(ms, nmsg);
1436
1437                 /* re-tune back to current VPLMN */
1438                 l1ctl_tx_ccch_req(ms, cs->arfcn);
1439                 cs->ccch_active = 0;
1440
1441                 return 0;
1442         }
1443
1444         /* if all frequencies have been searched */
1445         if (!weight) {
1446                 struct msgb *nmsg;
1447                 int found, any;
1448
1449                 LOGP(DCS, LOGL_INFO, "All frequencies scanned.\n");
1450
1451                 /* just see, if we search for any cell */
1452                 if (cs->state == GSM322_C6_ANY_CELL_SEL
1453                  || cs->state == GSM322_C8_ANY_CELL_RESEL
1454                  || cs->state == GSM322_C9_CHOOSE_ANY_CELL)
1455                         any = 1;
1456
1457                 found = gsm322_cs_select(ms, any);
1458
1459                 /* if found */
1460                 if (found >= 0) {
1461                         LOGP(DCS, LOGL_INFO, "Tune to frequency %d.\n", found);
1462                         /* tune */
1463                         cs->arfcn = found;
1464                         l1ctl_tx_ccch_req(ms, cs->arfcn);
1465                         cs->ccch_active = 0;
1466
1467                         /* Clear system information. */
1468                         gsm48_sysinfo_init(ms);
1469
1470                         /* tell PLMN process about available PLMNs */
1471                         nmsg = gsm322_msgb_alloc(GSM322_EVENT_PLMN_AVAIL);
1472                         if (!nmsg)
1473                                 return -ENOMEM;
1474                         gsm322_plmn_sendmsg(ms, nmsg);
1475
1476                         nmsg = gsm322_msgb_alloc(GSM322_EVENT_CELL_FOUND);
1477                 } else {
1478                         nmsg = gsm322_msgb_alloc(GSM322_EVENT_NO_CELL_FOUND);
1479                         LOGP(DCS, LOGL_INFO, "No cell available.\n");
1480                 }
1481                 if (!nmsg)
1482                         return -ENOMEM;
1483                 gsm322_cs_sendmsg(ms, nmsg);
1484
1485                 return 0;
1486         }
1487
1488         /* NOTE: We might already have system information from previous
1489          * scan. But we need recent informations, so we scan again!
1490          */
1491
1492         /* Tune to frequency for a while, to receive broadcasts. */
1493         cs->arfcn = weight & 1023;
1494         LOGP(DCS, LOGL_INFO, "Scanning frequency %d.\n", cs->arfcn);
1495         l1ctl_tx_ccch_req(ms, cs->arfcn);
1496         cs->ccch_active = 0;
1497
1498         /* Clear system information. */
1499         gsm48_sysinfo_init(ms);
1500
1501         /* increase scan counter for each maximum scan range */
1502         if (gsm_sup_smax[j].max)
1503                 gsm_sup_smax[j].temp++;
1504         return 0;
1505 }
1506
1507 /* check if cell is now suitable and allowable */
1508 static int gsm322_cs_store(struct osmocom_ms *ms)
1509 {
1510         struct gsm322_cellsel *cs = &ms->cellsel;
1511         struct gsm48_sysinfo *s = &ms->sysinfo;
1512         int i = cs->scan_state & 1023;
1513
1514         if (cs->state != GSM322_C2_STORED_CELL_SEL
1515          && cs->state != GSM322_C1_NORMAL_CELL_SEL
1516          && cs->state != GSM322_C6_ANY_CELL_SEL
1517          && cs->state != GSM322_C4_NORMAL_CELL_RESEL
1518          && cs->state != GSM322_C8_ANY_CELL_RESEL
1519          && cs->state != GSM322_C5_CHOOSE_CELL
1520          && cs->state != GSM322_C9_CHOOSE_ANY_CELL) {
1521                 LOGP(DCS, LOGL_FATAL, "This must only happen during cell "
1522                         "(re-)selection, please fix!\n");
1523                 return -EINVAL;
1524         }
1525
1526         /* store sysinfo */
1527         cs->list[i].flags |= GSM322_CS_FLAG_SYSINFO;
1528         if (s->cell_barr)
1529                 cs->list[i].flags |= GSM322_CS_FLAG_BARRED;
1530         else
1531                 cs->list[i].flags &= ~GSM322_CS_FLAG_BARRED;
1532         cs->list[i].min_db = s->rxlev_acc_min_db;
1533         cs->list[i].class_barr = s->class_barr;
1534         cs->list[i].max_pwr = s->ms_txpwr_max_ccch;
1535
1536         /* store selected network */
1537         if (s->mcc) {
1538                 cs->list[i].mcc = s->mcc;
1539                 cs->list[i].mnc = s->mnc;
1540                 cs->list[i].lac = s->lac;
1541
1542                 if (gsm322_is_forbidden_la(ms, s->mcc, s->mnc, s->lac))
1543                         cs->list[i].flags |= GSM322_CS_FLAG_FORBIDD;
1544                 else
1545                         cs->list[i].flags &= ~GSM322_CS_FLAG_FORBIDD;
1546         }
1547
1548         LOGP(DCS, LOGL_INFO, "Scan frequency %d: Cell found. (rxlev=%d "
1549                 "mcc=%03d mnc=%02d lac=%04x)\n", i, cs->list[i].rxlev_db,
1550                 cs->list[i].mcc, cs->list[i].mnc, cs->list[i].lac);
1551
1552         /* tune to next cell */
1553         return gsm322_cs_scan(ms);
1554 }
1555
1556 /* process system information when returing to idle mode */
1557 struct gsm322_ba_list *gsm322_cs_sysinfo_sacch(struct osmocom_ms *ms)
1558 {
1559         struct gsm322_cellsel *cs = &ms->cellsel;
1560         struct gsm48_sysinfo *s = &ms->sysinfo;
1561         struct gsm322_ba_list *ba = NULL;
1562         int i;
1563         uint8_t freq[128];
1564
1565         /* collect system information received during dedicated mode */
1566         if (s->si5
1567          && (!s->nb_ext_ind_si5
1568           || (s->si5bis && s->nb_ext_ind_si5 && !s->nb_ext_ind_si5bis)
1569           || (s->si5bis && s->si5ter && s->nb_ext_ind_si5
1570                 && s->nb_ext_ind_si5bis))) {
1571                 /* find or create ba list */
1572                 ba = gsm322_find_ba_list(cs, s->mcc, s->mnc);
1573                 if (!ba) {
1574                         ba = talloc_zero(l23_ctx, struct gsm322_ba_list);
1575                         if (!ba)
1576                                 return NULL;
1577                         ba->mcc = s->mcc;
1578                         ba->mnc = s->mnc;
1579                         llist_add_tail(&ba->entry, &cs->ba_list);
1580                 }
1581                 /* update (add) ba list */
1582                 memcpy(freq, ba->freq, sizeof(freq));
1583                 for (i = 0; i <= 1023; i++) {
1584                         if ((s->freq[i].mask & FREQ_TYPE_REP))
1585                                 freq[i >> 3] |= (1 << (i & 7));
1586                 }
1587                 if (!!memcmp(freq, ba->freq, sizeof(freq))) {
1588                         LOGP(DCS, LOGL_INFO, "New BA list (mcc=%d mnc=%d  "
1589                                 "%s, %s).\n", ba->mcc, ba->mnc,
1590                                 gsm_get_mcc(ba->mcc),
1591                                 gsm_get_mnc(ba->mcc, ba->mnc));
1592                         memcpy(ba->freq, freq, sizeof(freq));
1593                 }
1594         }
1595
1596         return ba;
1597 }
1598
1599 /* process system information during camping on a cell */
1600 static int gsm322_c_camp_sysinfo_bcch(struct osmocom_ms *ms, struct msgb *msg)
1601 {
1602         struct gsm322_cellsel *cs = &ms->cellsel;
1603         struct gsm48_sysinfo *s = &ms->sysinfo;
1604         struct gsm_subscriber *subscr = &ms->subscr;
1605         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
1606         struct msgb *nmsg;
1607
1608         if (gm->sysinfo == GSM48_MT_RR_SYSINFO_1) {
1609                 /* check if cell becomes barred */
1610                 if (!subscr->acc_barr && s->cell_barr) {
1611                         LOGP(DCS, LOGL_INFO, "Cell becomes barred.\n");
1612                         trigger_resel:
1613                         /* mark cell as unscanned */
1614                         cs->list[cs->arfcn].flags &= ~GSM322_CS_FLAG_SYSINFO;
1615                         /* trigger reselection event */
1616                         nmsg = gsm322_msgb_alloc(GSM322_EVENT_CELL_RESEL);
1617                         if (!nmsg)
1618                                 return -ENOMEM;
1619                         gsm322_cs_sendmsg(ms, nmsg);
1620
1621                         return 0;
1622                 }
1623                 /* check if cell access becomes barred */
1624                 if (!((subscr->acc_class & 0xfbff)
1625                         & (cs->list[cs->arfcn].class_barr ^ 0xffff))) {
1626                         LOGP(DCS, LOGL_INFO, "Cell access becomes barred.\n");
1627                         goto trigger_resel;
1628                 }
1629         }
1630
1631         return 0;
1632 }
1633
1634 /* process system information during channel scanning */
1635 static int gsm322_c_scan_sysinfo_bcch(struct osmocom_ms *ms, struct msgb *msg)
1636 {
1637         struct gsm322_cellsel *cs = &ms->cellsel;
1638         struct gsm48_rrlayer *rr = &ms->rrlayer;
1639         struct gsm48_sysinfo *s = &ms->sysinfo;
1640         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
1641         struct gsm322_ba_list *ba;
1642         int i;
1643         uint8_t freq[128];
1644
1645         if (rr->state != GSM48_RR_ST_IDLE) {
1646                 LOGP(DCS, LOGL_FATAL, "This must only happen in IDLE mode, "
1647                         "please fix!\n");
1648                 return -EINVAL;
1649         }
1650
1651         /* no sysinfo if we are not done with power scan */
1652         if (cs->powerscan) {
1653                 LOGP(DCS, LOGL_INFO, "Ignoring sysinfo during power scan.\n");
1654                 return -EINVAL;
1655         }
1656
1657         /* Store BA if we have full system info about cells and neigbor cells.
1658          * Depending on the extended bit in the channel description,
1659          * we require more or less system informations about neighbor cells
1660          */
1661         if (s->mcc
1662          && s->mnc
1663          && (gm->sysinfo == GSM48_MT_RR_SYSINFO_1
1664           || gm->sysinfo == GSM48_MT_RR_SYSINFO_2
1665           || gm->sysinfo == GSM48_MT_RR_SYSINFO_2bis
1666           || gm->sysinfo == GSM48_MT_RR_SYSINFO_2ter)
1667          && s->si1
1668          && s->si2
1669          && (!s->nb_ext_ind_si2
1670           || (s->si2bis && s->nb_ext_ind_si2 && !s->nb_ext_ind_si2bis)
1671           || (s->si2bis && s->si2ter && s->nb_ext_ind_si2
1672                 && s->nb_ext_ind_si2bis))) {
1673                 /* find or create ba list */
1674                 ba = gsm322_find_ba_list(cs, s->mcc, s->mnc);
1675                 if (!ba) {
1676                         ba = talloc_zero(l23_ctx, struct gsm322_ba_list);
1677                         if (!ba)
1678                                 return -ENOMEM;
1679                         ba->mcc = s->mcc;
1680                         ba->mnc = s->mnc;
1681                         llist_add_tail(&ba->entry, &cs->ba_list);
1682                 }
1683                 /* update ba list */
1684                 memset(freq, 0, sizeof(freq));
1685                 freq[cs->arfcn >> 3] |= (1 << (cs->arfcn & 7));
1686                 for (i = 0; i <= 1023; i++) {
1687                         if ((s->freq[i].mask &
1688                                 (FREQ_TYPE_SERV | FREQ_TYPE_NCELL)))
1689                                 freq[i >> 3] |= (1 << (i & 7));
1690                 }
1691                 if (!!memcmp(freq, ba->freq, sizeof(freq))) {
1692                         LOGP(DCS, LOGL_INFO, "New BA list (mcc=%d mnc=%d  "
1693                                 "%s, %s).\n", ba->mcc, ba->mnc,
1694                                 gsm_get_mcc(ba->mcc),
1695                                 gsm_get_mnc(ba->mcc, ba->mnc));
1696                         memcpy(ba->freq, freq, sizeof(freq));
1697                 }
1698         }
1699
1700 #warning testing
1701 printf("%d %d %d\n", s->si1, s->si2, s->si3);
1702         /* all relevant system informations received */
1703         if (s->si1 && s->si2 && s->si3
1704          && (!s->nb_ext_ind_si2
1705           || (s->si2bis && s->nb_ext_ind_si2 && !s->nb_ext_ind_si2bis)
1706           || (s->si2bis && s->si2ter && s->nb_ext_ind_si2
1707                 && s->nb_ext_ind_si2bis))) {
1708                 /* stop timer */
1709                 stop_cs_timer(cs);
1710
1711                 gsm48_sysinfo_dump(ms);
1712
1713                 /* store sysinfo and continue scan */
1714                 return gsm322_cs_store(ms);
1715         }
1716
1717         /* wait for more sysinfo or timeout */
1718         return 0;
1719 }
1720
1721 static void gsm322_cs_timeout(void *arg)
1722 {
1723         struct gsm322_cellsel *cs = arg;
1724         struct osmocom_ms *ms = cs->ms;
1725         int i = cs->scan_state & 1023;
1726
1727         LOGP(DCS, LOGL_INFO, "Cell selection timer has fired.\n");
1728         LOGP(DCS, LOGL_INFO, "Scan frequency %d: Cell not found. (rxlev=%d)\n",
1729                 i, cs->list[i].rxlev_db);
1730
1731         /* remove system information */
1732         cs->list[i].flags &= ~GSM322_CS_FLAG_SYSINFO;
1733
1734         /* tune to next cell */
1735         gsm322_cs_scan(ms);
1736
1737         return;
1738 }
1739
1740 /*
1741  * power scan process
1742  */
1743
1744 /* search for block of unscanned freqeuncies and start scanning */
1745 static int gsm322_cs_powerscan(struct osmocom_ms *ms)
1746 {
1747         struct gsm322_cellsel *cs = &ms->cellsel;
1748         struct gsm48_rrlayer *rr = &ms->rrlayer;
1749         int i, s = -1, e;
1750         uint8_t mask, flags;
1751
1752         if (rr->state != GSM48_RR_ST_IDLE) {
1753                 LOGP(DCS, LOGL_FATAL, "This must only happen in IDLE mode, "
1754                         "please fix!\n");
1755                 return -EINVAL;
1756         }
1757
1758         again:
1759
1760         /* search for first frequency to scan */
1761         mask = GSM322_CS_FLAG_SUPPORT | GSM322_CS_FLAG_POWER;
1762         flags = GSM322_CS_FLAG_SUPPORT;
1763         if (cs->state == GSM322_C2_STORED_CELL_SEL) {
1764                 LOGP(DCS, LOGL_FATAL, "Scanning power for stored BA list.\n");
1765                 mask |= GSM322_CS_FLAG_BA;
1766                 flags |= GSM322_CS_FLAG_BA;
1767         } else
1768                 LOGP(DCS, LOGL_FATAL, "Scanning power for all frequencies.\n");
1769         for (i = 0; i <= 1023; i++) {
1770                 if ((cs->list[i].flags & mask) == flags) {
1771                         s = e = i;
1772                         break;
1773                 }
1774         }
1775
1776         /* if there is no more frequency, we can tune to that cell */
1777         if (s < 0) {
1778                 int found = 0;
1779
1780                 /* stop power level scanning */
1781                 cs->powerscan = 0;
1782
1783                 /* check if not signal is found */
1784                 for (i = 0; i <= 1023; i++) {
1785                         if ((cs->list[i].flags & GSM322_CS_FLAG_SIGNAL))
1786                                 found++;
1787                 }
1788                 if (!found) {
1789                         struct msgb *nmsg;
1790
1791                         LOGP(DCS, LOGL_INFO, "Found no frequency.\n");
1792                         /* on normal cell selection, start over */
1793                         if (cs->state == GSM322_C1_NORMAL_CELL_SEL) {
1794                                 for (i = 0; i <= 1023; i++) {
1795                                         /* clear flag that this was scanned */
1796                                         cs->list[i].flags &=
1797                                                 ~(GSM322_CS_FLAG_POWER
1798                                                 | GSM322_CS_FLAG_SIGNAL
1799                                                 | GSM322_CS_FLAG_SYSINFO);
1800                                 }
1801                                 goto again;
1802                         }
1803                         /* on other cell selection, indicate "no cell found" */
1804                         /* NOTE: PLMN search process handles it.
1805                          * If not handled there, CS process gets indicated.
1806                          * If we would continue to process CS, then we might get
1807                          * our list of scanned cells disturbed.
1808                          */
1809                         nmsg = gsm322_msgb_alloc(GSM322_EVENT_NO_CELL_FOUND);
1810                         if (!nmsg)
1811                                 return -ENOMEM;
1812                         gsm322_plmn_sendmsg(ms, nmsg);
1813
1814                         return 0;
1815                 }
1816                 LOGP(DCS, LOGL_INFO, "Found %d frequencies.\n", found);
1817                 cs->scan_state = 0xffffffff; /* higher than high */
1818                 /* clear counter of scanned frequencies of each range */
1819                 for (i = 0; gsm_sup_smax[i].max; i++)
1820                         gsm_sup_smax[i].temp = 0;
1821                 return gsm322_cs_scan(ms);
1822         }
1823
1824         /* search last frequency to scan (en block) */
1825         e = i;
1826         for (i = s + 1; i <= 1023; i++) {
1827                 if ((cs->list[i].flags & mask) == flags)
1828                         e = i;
1829                 else
1830                         break;
1831         }
1832
1833         LOGP(DCS, LOGL_INFO, "Scanning frequecies. (%d..%d)\n", s, e);
1834
1835         /* start scan on radio interface */
1836         cs->powerscan = 1;
1837         return l1ctl_tx_pm_req_range(ms, s, e);
1838 }
1839
1840 static int gsm322_l1_signal(unsigned int subsys, unsigned int signal,
1841                      void *handler_data, void *signal_data)
1842 {
1843         struct osmocom_ms *ms;
1844         struct gsm322_cellsel *cs;
1845         struct osmobb_meas_res *mr;
1846         int i;
1847         int8_t rxlev_db;
1848
1849         if (subsys != SS_L1CTL)
1850                 return 0;
1851
1852         switch (signal) {
1853         case S_L1CTL_PM_RES:
1854                 mr = signal_data;
1855                 ms = mr->ms;
1856                 cs = &ms->cellsel;
1857                 if (!cs->powerscan)
1858                         return -EINVAL;
1859                 i = mr->band_arfcn & 1023;
1860                 rxlev_db = mr->rx_lev - 110;
1861                 cs->list[i].rxlev_db = rxlev_db;
1862                 cs->list[i].flags |= GSM322_CS_FLAG_POWER;
1863                 if (rxlev_db >= ms->support.min_rxlev_db) {
1864                         cs->list[i].flags |= GSM322_CS_FLAG_SIGNAL;
1865                         LOGP(DCS, LOGL_INFO, "Found signal (frequency %d "
1866                                 "rxlev %d)\n", i, cs->list[i].rxlev_db);
1867                 }
1868                 break;
1869         case S_L1CTL_PM_DONE:
1870                 LOGP(DCS, LOGL_INFO, "Done with power scanning range.\n");
1871                 ms = signal_data;
1872                 cs = &ms->cellsel;
1873                 if (!cs->powerscan)
1874                         return -EINVAL;
1875                 gsm322_cs_powerscan(ms);
1876                 break;
1877         case S_L1CTL_CCCH_RESP:
1878                 ms = signal_data;
1879                 cs = &ms->cellsel;
1880                 if (!cs->ccch_active) {
1881                         LOGP(DCS, LOGL_INFO, "Channel activated.\n");
1882                         cs->ccch_active = 1;
1883                         /* set timer for reading BCCH */
1884                         if (cs->state == GSM322_C2_STORED_CELL_SEL
1885                          || cs->state == GSM322_C1_NORMAL_CELL_SEL
1886                          || cs->state == GSM322_C6_ANY_CELL_SEL
1887                          || cs->state == GSM322_C4_NORMAL_CELL_RESEL
1888                          || cs->state == GSM322_C8_ANY_CELL_RESEL
1889                          || cs->state == GSM322_C5_CHOOSE_CELL
1890                          || cs->state == GSM322_C9_CHOOSE_ANY_CELL)
1891                                 start_cs_timer(cs, 4, 0);
1892                                         // TODO: timer depends on BCCH config
1893                 }
1894                 break;
1895         }
1896         return 0;
1897 }
1898
1899 /*
1900  * handler for cell selection process
1901  */
1902
1903 /* start HPLMN search */
1904 static int gsm322_c_hplmn_search(struct osmocom_ms *ms, struct msgb *msg)
1905 {
1906         struct gsm322_cellsel *cs = &ms->cellsel;
1907         int i;
1908
1909         new_c_state(cs, GSM322_HPLMN_SEARCH);
1910
1911         /* mark all frequencies except our own BA to be scanned */
1912         for (i = 0; i <= 1023; i++) {
1913                 if ((cs->list[i].flags & GSM322_CS_FLAG_SYSINFO)
1914                  && !(cs->list[i].flags & GSM322_CS_FLAG_BA))
1915                         cs->list[i].flags &= ~(GSM322_CS_FLAG_POWER
1916                                                 | GSM322_CS_FLAG_SIGNAL
1917                                                 | GSM322_CS_FLAG_SYSINFO);
1918         }
1919
1920         /* start power scan */
1921         return gsm322_cs_powerscan(ms);
1922 }
1923
1924 /* start stored cell selection */
1925 static int gsm322_c_stored_cell_sel(struct osmocom_ms *ms, struct gsm322_ba_list *ba)
1926 {
1927         struct gsm322_cellsel *cs = &ms->cellsel;
1928         int i;
1929
1930         new_c_state(cs, GSM322_C2_STORED_CELL_SEL);
1931
1932         /* flag all frequencies that are in current band allocation */
1933         for (i = 0; i <= 1023; i++) {
1934                 if ((ba->freq[i >> 3] & (1 << (i & 7))))
1935                         cs->list[i].flags |= GSM322_CS_FLAG_BA;
1936                 else
1937                         cs->list[i].flags &= ~GSM322_CS_FLAG_BA;
1938         }
1939
1940         /* start power scan */
1941         return gsm322_cs_powerscan(ms);
1942 }
1943
1944 /* start noraml cell selection */
1945 static int gsm322_c_normal_cell_sel(struct osmocom_ms *ms, struct msgb *msg)
1946 {
1947         struct gsm322_cellsel *cs = &ms->cellsel;
1948         int i;
1949
1950         /* except for stored cell selection state, we weed to rescan ?? */
1951         if (cs->state != GSM322_C2_STORED_CELL_SEL) {
1952                 for (i = 0; i <= 1023; i++)
1953                         cs->list[i].flags &= ~(GSM322_CS_FLAG_POWER
1954                                                 | GSM322_CS_FLAG_SIGNAL
1955                                                 | GSM322_CS_FLAG_SYSINFO);
1956         }
1957
1958         new_c_state(cs, GSM322_C1_NORMAL_CELL_SEL);
1959
1960         /* start power scan */
1961         return gsm322_cs_powerscan(ms);
1962 }
1963
1964 /* start any cell selection */
1965 static int gsm322_c_any_cell_sel(struct osmocom_ms *ms, struct msgb *msg)
1966 {
1967         struct gsm322_cellsel *cs = &ms->cellsel;
1968         int i;
1969
1970         /* in case we already tried any cell selection, power scan again */
1971         if (cs->state == GSM322_C6_ANY_CELL_SEL) {
1972                 for (i = 0; i <= 1023; i++)
1973                         cs->list[i].flags &= ~(GSM322_CS_FLAG_POWER
1974                                                 | GSM322_CS_FLAG_SIGNAL
1975                                                 | GSM322_CS_FLAG_SYSINFO);
1976         } else {
1977                 new_c_state(cs, GSM322_C6_ANY_CELL_SEL);
1978         }
1979
1980         cs->mcc = cs->mnc = 0;
1981
1982         /* start power scan */
1983         return gsm322_cs_powerscan(ms);
1984 }
1985
1986 /* start noraml cell re-selection */
1987 static int gsm322_c_normal_cell_resel(struct osmocom_ms *ms, struct msgb *msg)
1988 {
1989         struct gsm322_cellsel *cs = &ms->cellsel;
1990
1991         new_c_state(cs, GSM322_C4_NORMAL_CELL_RESEL);
1992
1993         /* NOTE: We keep our scan info we have so far.
1994          * This may cause a skip in power scan. */
1995
1996         /* start power scan */
1997         return gsm322_cs_powerscan(ms);
1998 }
1999
2000 /* start any cell re-selection */
2001 static int gsm322_c_any_cell_resel(struct osmocom_ms *ms, struct msgb *msg)
2002 {
2003         struct gsm322_cellsel *cs = &ms->cellsel;
2004
2005         new_c_state(cs, GSM322_C8_ANY_CELL_RESEL);
2006
2007         /* NOTE: We keep our scan info we have so far.
2008          * This may cause a skip in power scan. */
2009
2010         /* start power scan */
2011         return gsm322_cs_powerscan(ms);
2012 }
2013
2014 /* create temporary ba range with given frequency ranges */
2015 struct gsm322_ba_list *gsm322_cs_ba_range(struct osmocom_ms *ms,
2016         uint32_t *range, uint8_t ranges)
2017 {
2018         static struct gsm322_ba_list ba;
2019         uint16_t lower, higher;
2020
2021         memset(&ba, 0, sizeof(ba));
2022
2023         while(ranges--) {
2024                 lower = *range & 1023;
2025                 higher = (*range >> 16) & 1023;
2026                 range++;
2027                 LOGP(DCS, LOGL_INFO, "Use BA range: %d..%d\n", lower, higher);
2028                 /* GSM 05.08 6.3 */
2029                 while (1) {
2030                         ba.freq[lower >> 3] |= 1 << (lower & 7);
2031                         if (lower == higher)
2032                                 break;
2033                         lower = (lower + 1) & 1023;
2034                 }
2035         }
2036
2037         return &ba;
2038 }
2039
2040 /* common part of gsm322_c_choose_cell and gsm322_c_choose_any_cell */
2041 static int gsm322_cs_choose(struct osmocom_ms *ms)
2042 {
2043         struct gsm322_cellsel *cs = &ms->cellsel;
2044         struct gsm322_ba_list *ba = NULL;
2045         int i;
2046
2047 #ifdef TODO
2048         if (message->ranges)
2049                 ba = gsm322_cs_ba_range(ms, message->range, message->ranges);
2050         else {
2051                 LOGP(DCS, LOGL_INFO, "No BA range(s), try sysinfo.\n");
2052 #endif
2053                 /* get and update BA of last received sysinfo 5* */
2054                 ba = gsm322_cs_sysinfo_sacch(ms);
2055 #ifdef TODO
2056         }
2057 #endif
2058
2059         if (!ba) {
2060                 struct msgb *nmsg;
2061
2062                 LOGP(DCS, LOGL_INFO, "No BA list.\n");
2063
2064                 /* tell CS to start over */
2065                 nmsg = gsm322_msgb_alloc(GSM322_EVENT_NO_CELL_FOUND);
2066                 if (!nmsg)
2067                         return -ENOMEM;
2068                 gsm322_cs_sendmsg(ms, nmsg);
2069         }
2070
2071         /* flag all frequencies that are in current band allocation */
2072         for (i = 0; i <= 1023; i++) {
2073                 if (cs->state == GSM322_C5_CHOOSE_CELL) {
2074                         if ((ba->freq[i >> 3] & (1 << (i & 7))))
2075                                 cs->list[i].flags |= GSM322_CS_FLAG_BA;
2076                         else
2077                                 cs->list[i].flags &= ~GSM322_CS_FLAG_BA;
2078                 }
2079                 cs->list[i].flags &= ~(GSM322_CS_FLAG_POWER
2080                                         | GSM322_CS_FLAG_SIGNAL
2081                                         | GSM322_CS_FLAG_SYSINFO);
2082         }
2083 #ifdef TODO
2084         remove this:
2085 #else
2086         /* use hacked frequency */
2087         cs->list[ms->test_arfcn].flags |= GSM322_CS_FLAG_POWER;
2088         cs->list[ms->test_arfcn].flags |= GSM322_CS_FLAG_SIGNAL;
2089         cs->list[ms->test_arfcn].rxlev_db = -40;
2090 #endif
2091
2092         /* start power scan */
2093         return gsm322_cs_powerscan(ms);
2094 }
2095
2096 /* start 'Choose cell' after returning to idle mode */
2097 static int gsm322_c_choose_cell(struct osmocom_ms *ms, struct msgb *msg)
2098 {
2099         struct gsm322_cellsel *cs = &ms->cellsel;
2100
2101         new_c_state(cs, GSM322_C5_CHOOSE_CELL);
2102
2103         return gsm322_cs_choose(ms);
2104 }
2105
2106 /* start 'Choose any cell' after returning to idle mode */
2107 static int gsm322_c_choose_any_cell(struct osmocom_ms *ms, struct msgb *msg)
2108 {
2109         struct gsm322_cellsel *cs = &ms->cellsel;
2110
2111         new_c_state(cs, GSM322_C9_CHOOSE_ANY_CELL);
2112
2113         return gsm322_cs_choose(ms);
2114 }
2115
2116 /* a new PLMN is selected by PLMN search process */
2117 static int gsm322_c_new_plmn(struct osmocom_ms *ms, struct msgb *msg)
2118 {
2119         struct gsm322_cellsel *cs = &ms->cellsel;
2120         struct gsm322_plmn *plmn = &ms->plmn;
2121         struct gsm322_ba_list *ba;
2122
2123         cs->mcc = plmn->mcc;
2124         cs->mnc = plmn->mnc;
2125
2126         /* search for BA list */
2127         ba = gsm322_find_ba_list(cs, plmn->mcc, plmn->mnc);
2128
2129         if (ba) {
2130                 LOGP(DCS, LOGL_INFO, "Start stored cell selection.\n");
2131                 return gsm322_c_stored_cell_sel(ms, ba);
2132         } else {
2133                 LOGP(DCS, LOGL_INFO, "Start normal cell selection.\n");
2134                 return gsm322_c_normal_cell_sel(ms, msg);
2135         }
2136 }
2137
2138 /* a suitable cell was found, so we camp normally */
2139 static int gsm322_c_camp_normally(struct osmocom_ms *ms, struct msgb *msg)
2140 {
2141         struct gsm322_cellsel *cs = &ms->cellsel;
2142         struct msgb *nmsg;
2143
2144         nmsg = gsm48_mmevent_msgb_alloc(GSM48_MM_EVENT_NEW_LAI);
2145         if (!nmsg)
2146                 return -ENOMEM;
2147         gsm48_mmevent_msg(ms, nmsg);
2148
2149         new_c_state(cs, GSM322_C3_CAMPED_NORMALLY);
2150
2151         return 0;
2152 }
2153
2154 /* a not suitable cell was found, so we camp on any cell */
2155 static int gsm322_c_camp_any_cell(struct osmocom_ms *ms, struct msgb *msg)
2156 {
2157         struct gsm322_cellsel *cs = &ms->cellsel;
2158
2159         new_c_state(cs, GSM322_C7_CAMPED_ANY_CELL);
2160
2161         return 0;
2162 }
2163
2164 /* go connected mode */
2165 static int gsm322_c_conn_mode_1(struct osmocom_ms *ms, struct msgb *msg)
2166 {
2167         struct gsm322_cellsel *cs = &ms->cellsel;
2168
2169         /* stop camping process */
2170
2171         /* be sure to go to current camping frequency on return */
2172         LOGP(DCS, LOGL_INFO, "Going to camping frequency %d.\n", cs->arfcn);
2173         l1ctl_tx_ccch_req(ms, cs->arfcn);
2174         cs->ccch_active = 0;
2175
2176         return 0;
2177 }
2178
2179 static int gsm322_c_conn_mode_2(struct osmocom_ms *ms, struct msgb *msg)
2180 {
2181         struct gsm322_cellsel *cs = &ms->cellsel;
2182
2183         /* stop camping process */
2184
2185         /* be sure to go to current camping frequency on return */
2186         LOGP(DCS, LOGL_INFO, "Going to camping frequency %d.\n", cs->arfcn);
2187         l1ctl_tx_ccch_req(ms, cs->arfcn);
2188         cs->ccch_active = 0;
2189
2190         return 0;
2191 }
2192
2193 /* switch on */
2194 static int gsm322_c_switch_on(struct osmocom_ms *ms, struct msgb *msg)
2195 {
2196         struct gsm_subscriber *subscr = &ms->subscr;
2197
2198         /* if no SIM is is MS */
2199         if (!subscr->sim_valid) {
2200                 LOGP(DCS, LOGL_INFO, "Switch on without SIM.\n");
2201                 return gsm322_c_any_cell_sel(ms, msg);
2202         LOGP(DCS, LOGL_INFO, "Switch on with SIM inserted.\n");
2203         }
2204
2205         /* stay in NULL state until PLMN is selected */
2206
2207         return 0;
2208 }
2209
2210 /*
2211  * state machines
2212  */
2213
2214 /* state machine for automatic PLMN selection events */
2215 static struct plmnastatelist {
2216         uint32_t        states;
2217         int             type;
2218         int             (*rout) (struct osmocom_ms *ms, struct msgb *msg);
2219 } plmnastatelist[] = {
2220         {SBIT(GSM322_A0_NULL),
2221          GSM322_EVENT_SWITCH_ON, gsm322_a_switch_on},
2222         {ALL_STATES,
2223          GSM322_EVENT_SWITCH_OFF, gsm322_a_switch_off},
2224         {SBIT(GSM322_A6_NO_SIM),
2225          GSM322_EVENT_SIM_INSERT, gsm322_a_switch_on},
2226         {ALL_STATES,
2227          GSM322_EVENT_SIM_REMOVE, gsm322_a_sim_removed},
2228         {ALL_STATES,
2229          GSM322_EVENT_INVALID_SIM, gsm322_a_sim_removed},
2230         {SBIT(GSM322_A1_TRYING_RPLMN),
2231          GSM322_EVENT_REG_FAILED, gsm322_a_sel_first_plmn},
2232         {SBIT(GSM322_A1_TRYING_RPLMN),
2233          GSM322_EVENT_NO_CELL_FOUND, gsm322_a_sel_first_plmn},
2234         {SBIT(GSM322_A1_TRYING_RPLMN) | SBIT(GSM322_A3_TRYING_PLMN),
2235          GSM322_EVENT_REG_SUCCESS, gsm322_a_indicate_selected},
2236         {SBIT(GSM322_A2_ON_PLMN),
2237          GSM322_EVENT_ROAMING_NA, gsm322_a_roaming_na},
2238         {SBIT(GSM322_A2_ON_PLMN),
2239          GSM322_EVENT_HPLMN_SEARCH, gsm322_a_hplmn_search},
2240         {SBIT(GSM322_A2_ON_PLMN),
2241          GSM322_EVENT_NO_CELL_FOUND, gsm322_a_loss_of_radio},
2242         {SBIT(GSM322_A2_ON_PLMN),
2243          GSM322_EVENT_USER_RESEL, gsm322_a_user_reselection},
2244         {SBIT(GSM322_A3_TRYING_PLMN),
2245          GSM322_EVENT_REG_FAILED, gsm322_a_sel_next_plmn},
2246         {SBIT(GSM322_A3_TRYING_PLMN),
2247          GSM322_EVENT_NO_CELL_FOUND, gsm322_a_sel_next_plmn},
2248         {SBIT(GSM322_A5_HPLMN_SEARCH),
2249          GSM322_EVENT_HPLMN_FOUND, gsm322_a_sel_first_plmn},
2250         {SBIT(GSM322_A5_HPLMN_SEARCH),
2251          GSM322_EVENT_HPLMN_NOT_FOUND, gsm322_a_go_on_plmn},
2252         {SBIT(GSM322_A4_WAIT_FOR_PLMN),
2253          GSM322_EVENT_PLMN_AVAIL, gsm322_a_plmn_avail},
2254         {ALL_STATES,
2255          GSM322_EVENT_SEL_MANUAL, gsm322_a_sel_manual},
2256         {ALL_STATES,
2257          GSM322_EVENT_NO_CELL_FOUND, gsm322_am_no_cell_found},
2258 };
2259
2260 #define PLMNASLLEN \
2261         (sizeof(plmnastatelist) / sizeof(struct plmnastatelist))
2262
2263 static int gsm322_a_event(struct osmocom_ms *ms, struct msgb *msg)
2264 {
2265         struct gsm322_plmn *plmn = &ms->plmn;
2266         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
2267         int msg_type = gm->msg_type;
2268         int rc;
2269         int i;
2270
2271         LOGP(DPLMN, LOGL_INFO, "(ms %s) Event '%s' for automatic PLMN "
2272                 "selection in state %s\n", ms->name, get_event_name(msg_type),
2273                 plmn_a_state_names[plmn->state]);
2274         /* find function for current state and message */
2275         for (i = 0; i < PLMNASLLEN; i++)
2276                 if ((msg_type == plmnastatelist[i].type)
2277                  && ((1 << plmn->state) & plmnastatelist[i].states))
2278                         break;
2279         if (i == PLMNASLLEN) {
2280                 LOGP(DPLMN, LOGL_NOTICE, "Event unhandled at this state.\n");
2281                 return 0;
2282         }
2283
2284         rc = plmnastatelist[i].rout(ms, msg);
2285
2286         return rc;
2287 }
2288
2289 /* state machine for manual PLMN selection events */
2290 static struct plmnmstatelist {
2291         uint32_t        states;
2292         int             type;
2293         int             (*rout) (struct osmocom_ms *ms, struct msgb *msg);
2294 } plmnmstatelist[] = {
2295         {SBIT(GSM322_M0_NULL),
2296          GSM322_EVENT_SWITCH_ON, gsm322_m_switch_on},
2297         {ALL_STATES,
2298          GSM322_EVENT_SWITCH_OFF, gsm322_m_switch_off},
2299         {SBIT(GSM322_M5_NO_SIM),
2300          GSM322_EVENT_SIM_INSERT, gsm322_m_switch_on},
2301         {ALL_STATES,
2302          GSM322_EVENT_SIM_REMOVE, gsm322_m_sim_removed},
2303         {SBIT(GSM322_M1_TRYING_RPLMN),
2304          GSM322_EVENT_REG_FAILED, gsm322_m_display_plmns},
2305         {SBIT(GSM322_M1_TRYING_RPLMN),
2306          GSM322_EVENT_NO_CELL_FOUND, gsm322_m_display_plmns},
2307         {SBIT(GSM322_M1_TRYING_RPLMN),
2308          GSM322_EVENT_REG_SUCCESS, gsm322_m_indicate_selected},
2309         {SBIT(GSM322_M2_ON_PLMN),
2310          GSM322_EVENT_ROAMING_NA, gsm322_m_display_plmns},
2311         {SBIT(GSM322_M1_TRYING_RPLMN) | SBIT(GSM322_M2_ON_PLMN) |
2312          SBIT(GSM322_M4_TRYING_PLMN),
2313          GSM322_EVENT_INVALID_SIM, gsm322_m_sim_removed},
2314         {SBIT(GSM322_M2_ON_PLMN),
2315          GSM322_EVENT_USER_RESEL, gsm322_m_display_plmns},
2316         {SBIT(GSM322_M3_NOT_ON_PLMN),
2317          GSM322_EVENT_PLMN_AVAIL, gsm322_m_plmn_avail},
2318         {SBIT(GSM322_M3_NOT_ON_PLMN),
2319          GSM322_EVENT_CHOSE_PLMN, gsm322_m_choose_plmn},
2320         {SBIT(GSM322_M4_TRYING_PLMN),
2321          GSM322_EVENT_REG_SUCCESS, gsm322_m_go_on_plmn},
2322         {SBIT(GSM322_M4_TRYING_PLMN),
2323          GSM322_EVENT_REG_FAILED, gsm322_m_go_not_on_plmn},
2324         {SBIT(GSM322_M4_TRYING_PLMN),
2325          GSM322_EVENT_NO_CELL_FOUND, gsm322_m_go_not_on_plmn},
2326         {ALL_STATES,
2327          GSM322_EVENT_SEL_AUTO, gsm322_m_sel_auto},
2328         {ALL_STATES,
2329          GSM322_EVENT_NO_CELL_FOUND, gsm322_am_no_cell_found},
2330 };
2331
2332 #define PLMNMSLLEN \
2333         (sizeof(plmnmstatelist) / sizeof(struct plmnmstatelist))
2334
2335 static int gsm322_m_event(struct osmocom_ms *ms, struct msgb *msg)
2336 {
2337         struct gsm322_plmn *plmn = &ms->plmn;
2338         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
2339         int msg_type = gm->msg_type;
2340         int rc;
2341         int i;
2342
2343         LOGP(DPLMN, LOGL_INFO, "(ms %s) Event '%s' for manual PLMN selection "
2344                 "in state %s\n", ms->name, get_event_name(msg_type),
2345                 plmn_m_state_names[plmn->state]);
2346         /* find function for current state and message */
2347         for (i = 0; i < PLMNMSLLEN; i++)
2348                 if ((msg_type == plmnmstatelist[i].type)
2349                  && ((1 << plmn->state) & plmnmstatelist[i].states))
2350                         break;
2351         if (i == PLMNMSLLEN) {
2352                 LOGP(DPLMN, LOGL_NOTICE, "Event unhandled at this state.\n");
2353                 return 0;
2354         }
2355
2356         rc = plmnmstatelist[i].rout(ms, msg);
2357
2358         return rc;
2359 }
2360
2361 /* dequeue GSM 03.22 PLMN events */
2362 int gsm322_plmn_dequeue(struct osmocom_ms *ms)
2363 {
2364         struct gsm322_plmn *plmn = &ms->plmn;
2365         struct msgb *msg;
2366         int work = 0;
2367
2368         while ((msg = msgb_dequeue(&plmn->event_queue))) {
2369                 /* send event to PLMN select process */
2370                 if (plmn->mode == PLMN_MODE_AUTO)
2371                         gsm322_a_event(ms, msg);
2372                 else
2373                         gsm322_m_event(ms, msg);
2374                 msgb_free(msg);
2375                 work = 1; /* work done */
2376         }
2377
2378         return work;
2379 }
2380
2381 /* state machine for channel selection events */
2382 static struct cellselstatelist {
2383         uint32_t        states;
2384         int             type;
2385         int             (*rout) (struct osmocom_ms *ms, struct msgb *msg);
2386 } cellselstatelist[] = {
2387         {ALL_STATES,
2388          GSM322_EVENT_SWITCH_ON, gsm322_c_switch_on},
2389         {ALL_STATES,
2390          GSM322_EVENT_SIM_REMOVE, gsm322_c_any_cell_sel},
2391         {ALL_STATES,
2392          GSM322_EVENT_NEW_PLMN, gsm322_c_new_plmn},
2393         {SBIT(GSM322_C1_NORMAL_CELL_SEL) | SBIT(GSM322_C2_STORED_CELL_SEL) |
2394          SBIT(GSM322_C4_NORMAL_CELL_RESEL) | SBIT(GSM322_C5_CHOOSE_CELL),
2395          GSM322_EVENT_CELL_FOUND, gsm322_c_camp_normally},
2396         {SBIT(GSM322_C9_CHOOSE_ANY_CELL) | SBIT(GSM322_C6_ANY_CELL_SEL) |
2397          SBIT(GSM322_C4_NORMAL_CELL_RESEL),
2398          GSM322_EVENT_CELL_FOUND, gsm322_c_camp_any_cell},
2399         {SBIT(GSM322_C1_NORMAL_CELL_SEL) | SBIT(GSM322_C6_ANY_CELL_SEL) |
2400          SBIT(GSM322_C9_CHOOSE_ANY_CELL) | SBIT(GSM322_C8_ANY_CELL_RESEL) |
2401          SBIT(GSM322_C0_NULL),
2402          GSM322_EVENT_NO_CELL_FOUND, gsm322_c_any_cell_sel},
2403         {SBIT(GSM322_C2_STORED_CELL_SEL) | SBIT(GSM322_C5_CHOOSE_CELL) |
2404          SBIT(GSM322_C4_NORMAL_CELL_RESEL),
2405          GSM322_EVENT_NO_CELL_FOUND, gsm322_c_normal_cell_sel},
2406         {SBIT(GSM322_C3_CAMPED_NORMALLY),
2407          GSM322_EVENT_LEAVE_IDLE, gsm322_c_conn_mode_1},
2408         {SBIT(GSM322_C7_CAMPED_ANY_CELL),
2409          GSM322_EVENT_LEAVE_IDLE, gsm322_c_conn_mode_2},
2410         {SBIT(GSM322_C3_CAMPED_NORMALLY),
2411          GSM322_EVENT_RET_IDLE, gsm322_c_choose_cell},
2412         {SBIT(GSM322_C7_CAMPED_ANY_CELL),
2413          GSM322_EVENT_RET_IDLE, gsm322_c_choose_any_cell},
2414         {SBIT(GSM322_C3_CAMPED_NORMALLY),
2415          GSM322_EVENT_CELL_RESEL, gsm322_c_normal_cell_resel},
2416         {SBIT(GSM322_C7_CAMPED_ANY_CELL),
2417          GSM322_EVENT_CELL_RESEL, gsm322_c_any_cell_resel},
2418         {SBIT(GSM322_C7_CAMPED_ANY_CELL),
2419          GSM322_EVENT_CELL_FOUND, gsm322_c_normal_cell_sel},
2420         {SBIT(GSM322_C1_NORMAL_CELL_SEL) | SBIT(GSM322_C2_STORED_CELL_SEL) |
2421          SBIT(GSM322_C4_NORMAL_CELL_RESEL) | SBIT(GSM322_C5_CHOOSE_CELL) |
2422          SBIT(GSM322_C9_CHOOSE_ANY_CELL) | SBIT(GSM322_C8_ANY_CELL_RESEL) |
2423          SBIT(GSM322_C6_ANY_CELL_SEL),
2424          GSM322_EVENT_SYSINFO, gsm322_c_scan_sysinfo_bcch},
2425         {SBIT(GSM322_C3_CAMPED_NORMALLY) | SBIT(GSM322_C7_CAMPED_ANY_CELL),
2426          GSM322_EVENT_SYSINFO, gsm322_c_camp_sysinfo_bcch},
2427         {SBIT(GSM322_C3_CAMPED_NORMALLY),
2428          GSM322_EVENT_HPLMN_SEARCH, gsm322_c_hplmn_search}
2429 };
2430
2431 #define CELLSELSLLEN \
2432         (sizeof(cellselstatelist) / sizeof(struct cellselstatelist))
2433
2434 int gsm322_c_event(struct osmocom_ms *ms, struct msgb *msg)
2435 {
2436         struct gsm322_cellsel *cs = &ms->cellsel;
2437         struct gsm322_msg *gm = (struct gsm322_msg *) msg->data;
2438         int msg_type = gm->msg_type;
2439         int rc;
2440         int i;
2441
2442         LOGP(DCS, LOGL_INFO, "(ms %s) Event '%s' for Cell selection in state "
2443                 "%s\n", ms->name, get_event_name(msg_type),
2444                 cs_state_names[cs->state]);
2445         /* find function for current state and message */
2446         for (i = 0; i < CELLSELSLLEN; i++)
2447                 if ((msg_type == cellselstatelist[i].type)
2448                  && ((1 << cs->state) & cellselstatelist[i].states))
2449                         break;
2450         if (i == CELLSELSLLEN) {
2451                 LOGP(DCS, LOGL_NOTICE, "Event unhandled at this state.\n");
2452                 return 0;
2453         }
2454
2455         rc = cellselstatelist[i].rout(ms, msg);
2456
2457         return rc;
2458 }
2459
2460 /* dequeue GSM 03.22 cell selection events */
2461 int gsm322_cs_dequeue(struct osmocom_ms *ms)
2462 {
2463         struct gsm322_cellsel *cs = &ms->cellsel;
2464         struct msgb *msg;
2465         int work = 0;
2466
2467         while ((msg = msgb_dequeue(&cs->event_queue))) {
2468                 /* send event to cell selection process */
2469                 gsm322_c_event(ms, msg);
2470                 msgb_free(msg);
2471                 work = 1; /* work done */
2472         }
2473
2474         return work;
2475 }
2476
2477 /*
2478  * dump lists
2479  */
2480
2481 int gsm322_dump_sorted_plmn(struct osmocom_ms *ms)
2482 {
2483         struct gsm322_plmn *plmn = &ms->plmn;
2484         struct gsm322_plmn_list *temp;
2485
2486         printf("MCC    |MNC    |allowed|rx-lev\n");
2487         printf("-------+-------+-------+-------\n");
2488         llist_for_each_entry(temp, &plmn->sorted_plmn, entry) {
2489                 printf("%03d    |%02d     |%s    |%d\n", temp->mcc, temp->mnc,
2490                         (temp->cause) ? "no ":"yes", temp->rxlev_db);
2491         }
2492
2493         return 0;
2494 }
2495
2496 int gsm322_dump_cs_list(struct osmocom_ms *ms)
2497 {
2498         struct gsm322_cellsel *cs = &ms->cellsel;
2499         int i, j;
2500
2501         printf("rx-lev |MCC    |MNC    |forb.LA|barred,0123456789abcdef|"
2502                 "min-db |max-pwr\n"
2503                 "-------+-------+-------+-------+-----------------------+"
2504                 "-------+-------\n");
2505         for (i = 0; i <= 1023; i++) {
2506                 if (!(cs->list[i].flags & GSM322_CS_FLAG_SIGNAL))
2507                         continue;
2508                 printf("%4d   |", cs->list[i].rxlev_db);
2509                 if ((cs->list[i].flags & GSM322_CS_FLAG_SYSINFO)) {
2510                         printf("%03d    |%02d     |", cs->list[i].mcc,
2511                                 cs->list[i].mnc);
2512                         if ((cs->list[i].flags & GSM322_CS_FLAG_FORBIDD))
2513                                 printf("yes    |");
2514                         else
2515                                 printf("no     |");
2516                         if ((cs->list[i].flags & GSM322_CS_FLAG_BARRED))
2517                                 printf("yes    ");
2518                         else
2519                                 printf("no     ");
2520                         for (j = 0; j < 16; j++) {
2521                                 if ((cs->list[i].class_barr & (1 << j)))
2522                                         printf("*");
2523                                 else
2524                                         printf(" ");
2525                         }
2526                         printf("|%4d   |%4d\n", cs->list[i].min_db,
2527                                 cs->list[i].max_pwr);
2528                 } else
2529                         printf("n/a    |n/a    |       |                       "
2530                                 "|n/a    |n/a\n");
2531         }
2532
2533         return 0;
2534 }
2535
2536 int gsm322_dump_sim_plmn(struct osmocom_ms *ms)
2537 {
2538         struct gsm_subscriber *subscr = &ms->subscr;
2539         struct gsm_sub_plmn_list *temp;
2540
2541         printf("MCC    |MNC\n");
2542         printf("-------+-------\n");
2543         llist_for_each_entry(temp, &subscr->plmn_list, entry)
2544                 printf("%03d    |%02d\n", temp->mcc, temp->mnc);
2545
2546         return 0;
2547 }
2548
2549 int gsm322_dump_forbidden_plmn(struct osmocom_ms *ms)
2550 {
2551         struct gsm_subscriber *subscr = &ms->subscr;
2552         struct gsm_sub_plmn_na *temp;
2553
2554         printf("MCC    |MNC    |cause\n");
2555         printf("-------+-------+-------\n");
2556         llist_for_each_entry(temp, &subscr->plmn_na, entry)
2557                 printf("%03d    |%02d     |#%d\n", temp->mcc, temp->mnc,
2558                         temp->cause);
2559
2560         return 0;
2561 }
2562
2563 int gsm322_dump_forbidden_la(struct osmocom_ms *ms)
2564 {
2565         struct gsm322_plmn *plmn = &ms->plmn;
2566         struct gsm322_la_list *temp;
2567
2568         printf("MCC    |MNC    |LAC    |cause\n");
2569         printf("-------+-------+-------+-------\n");
2570         llist_for_each_entry(temp, &plmn->forbidden_la, entry)
2571                 printf("%03d    |%02d     |0x%04x |#%d\n", temp->mcc, temp->mnc,
2572                         temp->lac, temp->cause);
2573
2574         return 0;
2575 }
2576
2577 /*
2578  * initialization
2579  */
2580
2581 int gsm322_init(struct osmocom_ms *ms)
2582 {
2583         struct gsm322_plmn *plmn = &ms->plmn;
2584         struct gsm322_cellsel *cs = &ms->cellsel;
2585         OSMOCOM_FILE *fp;
2586         char suffix[] = ".ba";
2587         char filename[sizeof(ms->name) + strlen(suffix) + 1];
2588         int i;
2589         struct gsm322_ba_list *ba;
2590         uint8_t buf[4];
2591
2592         LOGP(DPLMN, LOGL_INFO, "init PLMN process\n");
2593         LOGP(DCS, LOGL_INFO, "init Cell Selection process\n");
2594
2595         memset(plmn, 0, sizeof(*plmn));
2596         memset(cs, 0, sizeof(*cs));
2597         plmn->ms = ms;
2598         cs->ms = ms;
2599
2600         /* set initial state */
2601         plmn->state = 0;
2602         cs->state = 0;
2603         plmn->mode = PLMN_MODE_AUTO;
2604
2605         /* init lists */
2606         INIT_LLIST_HEAD(&plmn->event_queue);
2607         INIT_LLIST_HEAD(&cs->event_queue);
2608         INIT_LLIST_HEAD(&plmn->sorted_plmn);
2609         INIT_LLIST_HEAD(&plmn->forbidden_la);
2610         INIT_LLIST_HEAD(&cs->ba_list);
2611
2612         /* set supported frequencies in cell selection list */
2613         for (i = 0; i <= 1023; i++)
2614                 if ((ms->support.freq_map[i >> 3] & (1 << (i & 7))))
2615                         cs->list[i].flags |= GSM322_CS_FLAG_SUPPORT;
2616
2617         /* read BA list */
2618         strcpy(filename, ms->name);
2619         strcat(filename, suffix);
2620         fp = osmocom_fopen(filename, "r");
2621         if (fp) {
2622                 int rc;
2623
2624                 while(!osmocom_feof(fp)) {
2625                         ba = talloc_zero(l23_ctx, struct gsm322_ba_list);
2626                         if (!ba)
2627                                 return -ENOMEM;
2628                         rc = osmocom_fread(buf, 4, 1, fp);
2629                         if (!rc) {
2630                                 talloc_free(ba);
2631                                 break;
2632                         }
2633                         ba->mcc = (buf[0] << 8) | buf[1];
2634                         ba->mnc = (buf[2] << 8) | buf[3];
2635                         rc = osmocom_fread(ba->freq, sizeof(ba->freq), 1, fp);
2636                         if (!rc) {
2637                                 talloc_free(ba);
2638                                 break;
2639                         }
2640                         llist_add_tail(&ba->entry, &cs->ba_list);
2641                         LOGP(DCS, LOGL_INFO, "Read stored BA list (mcc=%d "
2642                                 "mnc=%d  %s, %s)\n", ba->mcc, ba->mnc,
2643                                 gsm_get_mcc(ba->mcc),
2644                                 gsm_get_mnc(ba->mcc, ba->mnc));
2645                 }
2646                 osmocom_fclose(fp);
2647         } else
2648                 LOGP(DCS, LOGL_NOTICE, "No stored BA list\n");
2649
2650         register_signal_handler(SS_L1CTL, &gsm322_l1_signal, NULL);
2651
2652         return 0;
2653 }
2654
2655 int gsm322_exit(struct osmocom_ms *ms)
2656 {
2657         struct gsm322_plmn *plmn = &ms->plmn;
2658         struct gsm322_cellsel *cs = &ms->cellsel;
2659         struct llist_head *lh, *lh2;
2660         struct msgb *msg;
2661         OSMOCOM_FILE *fp;
2662         char suffix[] = ".ba";
2663         char filename[sizeof(ms->name) + strlen(suffix) + 1];
2664         struct gsm322_ba_list *ba;
2665         uint8_t buf[4];
2666
2667         LOGP(DPLMN, LOGL_INFO, "exit PLMN process\n");
2668         LOGP(DCS, LOGL_INFO, "exit Cell Selection process\n");
2669
2670         unregister_signal_handler(SS_L1CTL, &gsm322_l1_signal, NULL);
2671
2672         /* stop cell selection process (if any) */
2673         new_c_state(cs, GSM322_C0_NULL);
2674
2675         /* stop timers */
2676         stop_cs_timer(cs);
2677         stop_plmn_timer(plmn);
2678
2679         /* store BA list */
2680         strcpy(filename, ms->name);
2681         strcat(filename, suffix);
2682         fp = osmocom_fopen(filename, "w");
2683         if (fp) {
2684                 int rc;
2685
2686                 llist_for_each_entry(ba, &cs->ba_list, entry) {
2687                         buf[0] = ba->mcc >> 8;
2688                         buf[1] = ba->mcc & 0xff;
2689                         buf[2] = ba->mnc >> 8;
2690                         buf[3] = ba->mnc & 0xff;
2691                         rc = osmocom_fwrite(buf, 4, 1, fp);
2692                         rc = osmocom_fwrite(ba->freq, sizeof(ba->freq), 1, fp);
2693                         LOGP(DCS, LOGL_INFO, "Write stored BA list (mcc=%d "
2694                                 "mnc=%d  %s, %s)\n", ba->mcc, ba->mnc,
2695                                 gsm_get_mcc(ba->mcc),
2696                                 gsm_get_mnc(ba->mcc, ba->mnc));
2697                 }
2698                 osmocom_fclose(fp);
2699         } else
2700                 LOGP(DCS, LOGL_ERROR, "Failed to write BA list\n");
2701
2702         /* free lists */
2703         while ((msg = msgb_dequeue(&plmn->event_queue)))
2704                 msgb_free(msg);
2705         while ((msg = msgb_dequeue(&cs->event_queue)))
2706                 msgb_free(msg);
2707         llist_for_each_safe(lh, lh2, &plmn->sorted_plmn) {
2708                 llist_del(lh);
2709                 talloc_free(lh);
2710         }
2711         llist_for_each_safe(lh, lh2, &plmn->forbidden_la) {
2712                 llist_del(lh);
2713                 talloc_free(lh);
2714         }
2715         llist_for_each_safe(lh, lh2, &cs->ba_list) {
2716                 llist_del(lh);
2717                 talloc_free(lh);
2718         }
2719         return 0;
2720 }
2721
2722