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