mesh/net.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2018-2019  Intel Corporation. All rights reserved.
   7  *
   8  *
   9  */
  10
  11 #ifdef HAVE_CONFIG_H
  12 #include <config.h>
  13 #endif
  14
  15 #include <sys/time.h>
  16
  17 #include <ell/ell.h>
  18
  19 #include "mesh/mesh-defs.h"
  20 #include "mesh/util.h"
  21 #include "mesh/crypto.h"
  22 #include "mesh/net-keys.h"
  23 #include "mesh/node.h"
  24 #include "mesh/net.h"
  25 #include "mesh/mesh-io.h"
  26 #include "mesh/friend.h"
  27 #include "mesh/mesh-config.h"
  28 #include "mesh/model.h"
  29 #include "mesh/appkey.h"
  30 #include "mesh/rpl.h"
  31
  32 #define abs_diff(a, b) ((a) > (b) ? (a) - (b) : (b) - (a))
  33
  34 #define IV_IDX_DIFF_RANGE       42
  35
  36 /*#define IV_IDX_UPD_MIN        (5 * 60)        * 5 minute for Testing */
  37 #define IV_IDX_UPD_MIN  (60 * 60 * 96)  /* 96 Hours - per Spec */
  38 #define IV_IDX_UPD_HOLD (IV_IDX_UPD_MIN/2)
  39 #define IV_IDX_UPD_MAX  (IV_IDX_UPD_MIN + IV_IDX_UPD_HOLD)
  40
  41 #define iv_is_updating(net) ((net)->iv_upd_state == IV_UPD_UPDATING)
  42
  43 #define IV_UPDATE_SEQ_TRIGGER 0x800000  /* Half of Seq-Nums expended */
  44
  45 #define SEG_TO  2
  46 #define MSG_TO  60
  47
  48 #define DEFAULT_TRANSMIT_COUNT          1
  49 #define DEFAULT_TRANSMIT_INTERVAL       100
  50
  51 #define SAR_KEY(src, seq0)      ((((uint32_t)(seq0)) << 16) | (src))
  52
  53 #define FAST_CACHE_SIZE 8
  54
  55 enum _relay_advice {
  56         RELAY_NONE,             /* Relay not enabled in node */
  57         RELAY_ALLOWED,          /* Relay enabled, msg not to node's unicast */
  58         RELAY_DISALLOWED,       /* Msg was unicast handled by this node */
  59         RELAY_ALWAYS            /* Relay enabled, msg to a group */
  60 };
  61
  62 enum _iv_upd_state {
  63         /* Allows acceptance of any iv_index secure net beacon */
  64         IV_UPD_INIT,
  65         /* Normal, can transition, accept current or old */
  66         IV_UPD_NORMAL,
  67         /* Updating proc running, we use old, accept old or new */
  68         IV_UPD_UPDATING,
  69         /* Normal, can *not* transition, accept current or old iv_index */
  70         IV_UPD_NORMAL_HOLD,
  71 };
  72
  73 struct net_key {
  74         struct mesh_key_set key_set;
  75         unsigned int beacon_id;
  76         uint8_t key[16];
  77         uint8_t beacon_key[16];
  78         uint8_t network_id[8];
  79 };
  80
  81 struct mesh_subnet {
  82         struct mesh_net *net;
  83         uint16_t idx;
  84         uint32_t net_key_tx;
  85         uint32_t net_key_cur;
  86         uint32_t net_key_upd;
  87         uint8_t key_refresh;
  88         uint8_t kr_phase;
  89 };
  90
  91 struct mesh_net {
  92         struct mesh_io *io;
  93         struct mesh_node *node;
  94         struct mesh_prov *prov;
  95         struct l_queue *app_keys;
  96         unsigned int pkt_id;
  97         unsigned int bea_id;
  98         unsigned int beacon_id;
  99         unsigned int sar_id_next;
 100
 101         bool friend_enable;
 102         bool beacon_enable;
 103         bool proxy_enable;
 104         bool friend_seq;
 105         struct l_timeout *iv_update_timeout;
 106         enum _iv_upd_state iv_upd_state;
 107
 108         bool iv_update;
 109         uint32_t instant; /* Controller Instant of recent Rx */
 110         uint32_t iv_index;
 111         uint32_t seq_num;
 112         uint16_t src_addr;
 113         uint16_t last_addr;
 114         uint16_t tx_interval;
 115         uint16_t tx_cnt;
 116         uint8_t chan; /* Channel of recent Rx */
 117         uint8_t default_ttl;
 118         uint8_t tid;
 119
 120         struct {
 121                 bool enable;
 122                 uint16_t interval;
 123                 uint8_t count;
 124         } relay;
 125
 126         /* Heartbeat info */
 127         struct mesh_net_heartbeat_sub hb_sub;
 128         struct mesh_net_heartbeat_pub hb_pub;
 129         uint16_t features;
 130
 131         struct l_queue *subnets;
 132         struct l_queue *msg_cache;
 133         struct l_queue *replay_cache;
 134         struct l_queue *sar_in;
 135         struct l_queue *sar_out;
 136         struct l_queue *sar_queue;
 137         struct l_queue *frnd_msgs;
 138         struct l_queue *friends;
 139         struct l_queue *negotiations;
 140         struct l_queue *destinations;
 141 };
 142
 143 struct mesh_msg {
 144         uint16_t src;
 145         uint32_t seq;
 146         uint32_t mic;
 147 };
 148
 149 struct mesh_sar {
 150         unsigned int id;
 151         struct l_timeout *seg_timeout;
 152         struct l_timeout *msg_timeout;
 153         uint32_t flags;
 154         uint32_t last_nak;
 155         uint32_t iv_index;
 156         uint32_t seqAuth;
 157         uint16_t seqZero;
 158         uint16_t app_idx;
 159         uint16_t net_idx;
 160         uint16_t src;
 161         uint16_t remote;
 162         uint16_t len;
 163         bool szmic;
 164         bool segmented;
 165         bool frnd;
 166         bool frnd_cred;
 167         uint8_t ttl;
 168         uint8_t last_seg;
 169         uint8_t key_aid;
 170         uint8_t buf[4]; /* Large enough for ACK-Flags and MIC */
 171 };
 172
 173 struct mesh_destination {
 174         uint16_t dst;
 175         uint16_t ref_cnt;
 176 };
 177
 178 struct net_decode {
 179         struct mesh_net *net;
 180         struct mesh_friend *frnd;
 181         struct mesh_key_set *key_set;
 182         uint8_t *packet;
 183         uint32_t iv_index;
 184         uint8_t size;
 185         uint8_t nid;
 186         bool proxy;
 187 };
 188
 189 struct net_queue_data {
 190         struct mesh_io_recv_info *info;
 191         struct mesh_net *net;
 192         const uint8_t *data;
 193         uint8_t *out;
 194         size_t out_size;
 195         enum _relay_advice relay_advice;
 196         uint32_t net_key_id;
 197         uint32_t iv_index;
 198         uint16_t len;
 199         bool seen;
 200 };
 201
 202 struct oneshot_tx {
 203         struct mesh_net *net;
 204         uint16_t interval;
 205         uint8_t cnt;
 206         uint8_t size;
 207         uint8_t packet[30];
 208 };
 209
 210 struct net_beacon_data {
 211         uint32_t net_key_id;
 212         uint32_t ivi;
 213         bool ivu;
 214         bool kr;
 215         bool processed;
 216 };
 217
 218 static struct l_queue *fast_cache;
 219 static struct l_queue *nets;
 220
 221 static void net_rx(void *net_ptr, void *user_data);
 222
 223 static inline struct mesh_subnet *get_primary_subnet(struct mesh_net *net)
 224 {
 225         return l_queue_peek_head(net->subnets);
 226 }
 227
 228 static bool match_key_index(const void *a, const void *b)
 229 {
 230         const struct mesh_subnet *subnet = a;
 231         uint16_t idx = L_PTR_TO_UINT(b);
 232
 233         return subnet->idx == idx;
 234 }
 235
 236 static bool match_key_id(const void *a, const void *b)
 237 {
 238         const struct mesh_subnet *subnet = a;
 239         uint32_t net_key_id = L_PTR_TO_UINT(b);
 240
 241         return (net_key_id == subnet->net_key_cur) ||
 242                                         (net_key_id == subnet->net_key_upd);
 243 }
 244
 245 static bool match_friend_key_id(const void *a, const void *b)
 246 {
 247         const struct mesh_friend *friend = a;
 248         uint32_t net_key_id = L_PTR_TO_UINT(b);
 249
 250         return (net_key_id == friend->net_key_cur) ||
 251                                         (net_key_id == friend->net_key_upd);
 252 }
 253
 254 static void send_hb_publication(void *data)
 255 {
 256         struct mesh_net *net = data;
 257         struct mesh_net_heartbeat_pub *pub = &net->hb_pub;
 258         uint8_t msg[4];
 259         int n = 0;
 260
 261         if (pub->dst == UNASSIGNED_ADDRESS)
 262                 return;
 263
 264         msg[n++] = NET_OP_HEARTBEAT;
 265         msg[n++] = pub->ttl;
 266         l_put_be16(net->features, msg + n);
 267         n += 2;
 268
 269         mesh_net_transport_send(net, 0, 0, mesh_net_get_iv_index(net),
 270                                         pub->ttl, 0, 0, pub->dst, msg, n);
 271 }
 272
 273 static void trigger_heartbeat(struct mesh_net *net, uint16_t feature,
 274                                                                 bool enable)
 275 {
 276         l_debug("HB: %4.4x --> %d", feature, enable);
 277
 278         if (enable) {
 279                 if (net->features & feature)
 280                         return; /* no change */
 281
 282                 net->features |= feature;
 283         } else {
 284                 if (!(net->features & feature))
 285                         return; /* no change */
 286
 287                 net->features &= ~feature;
 288         }
 289
 290         if (!(net->hb_pub.features & feature))
 291                 return; /* no interest in this feature */
 292
 293         l_idle_oneshot(send_hb_publication, net, NULL);
 294 }
 295
 296 static bool match_by_friend(const void *a, const void *b)
 297 {
 298         const struct mesh_friend *frnd = a;
 299         uint16_t dst = L_PTR_TO_UINT(b);
 300
 301         return frnd->lp_addr == dst;
 302 }
 303
 304 static void free_friend_internals(struct mesh_friend *frnd)
 305 {
 306         if (frnd->pkt_cache)
 307                 l_queue_destroy(frnd->pkt_cache, l_free);
 308
 309         l_free(frnd->u.active.grp_list);
 310         frnd->u.active.grp_list = NULL;
 311         frnd->pkt_cache = NULL;
 312
 313         net_key_unref(frnd->net_key_cur);
 314         net_key_unref(frnd->net_key_upd);
 315         frnd->net_key_cur = 0;
 316         frnd->net_key_upd = 0;
 317
 318 }
 319
 320 static void frnd_kr_phase1(void *a, void *b)
 321 {
 322         struct mesh_friend *frnd = a;
 323         uint32_t net_key_id = L_PTR_TO_UINT(b);
 324
 325         frnd->net_key_upd = net_key_frnd_add(net_key_id, frnd->lp_addr,
 326                         frnd->net->src_addr, frnd->lp_cnt, frnd->fn_cnt);
 327 }
 328
 329 static void frnd_kr_phase2(void *a, void *b)
 330 {
 331         struct mesh_friend *frnd = a;
 332
 333         /*
 334          * I think that a Friend should use Old Key as long as possible
 335          * Because a Friend Node will enter Phase 3 before it's LPN.
 336          * Alternatively, the FN could keep the Old Friend Keys until it
 337          * receives it's first Poll using the new keys (?)
 338          */
 339
 340         l_debug("Use Both KeySet %d && %d for %4.4x",
 341                         frnd->net_key_cur, frnd->net_key_upd, frnd->lp_addr);
 342 }
 343
 344 static void frnd_kr_phase3(void *a, void *b)
 345 {
 346         struct mesh_friend *frnd = a;
 347
 348         l_debug("Replace KeySet %d with %d for %4.4x",
 349                         frnd->net_key_cur, frnd->net_key_upd, frnd->lp_addr);
 350         net_key_unref(frnd->net_key_cur);
 351         frnd->net_key_cur = frnd->net_key_upd;
 352         frnd->net_key_upd = 0;
 353 }
 354
 355 /* TODO: add net key idx? For now, use primary net key */
 356 struct mesh_friend *mesh_friend_new(struct mesh_net *net, uint16_t dst,
 357                                         uint8_t ele_cnt, uint8_t frd,
 358                                         uint8_t frw, uint32_t fpt,
 359                                         uint16_t fn_cnt, uint16_t lp_cnt)
 360 {
 361         struct mesh_subnet *subnet;
 362         struct mesh_friend *frnd = l_queue_find(net->friends,
 363                                         match_by_friend, L_UINT_TO_PTR(dst));
 364
 365         if (frnd) {
 366                 /* Kill all timers and empty cache for this friend */
 367                 free_friend_internals(frnd);
 368                 l_timeout_remove(frnd->timeout);
 369                 frnd->timeout = NULL;
 370         } else {
 371                 frnd = l_new(struct mesh_friend, 1);
 372                 l_queue_push_head(net->friends, frnd);
 373         }
 374
 375         /* add _k2 */
 376         frnd->net = net;
 377         frnd->lp_addr = dst;
 378         frnd->frd = frd;
 379         frnd->frw = frw;
 380         frnd->fn_cnt = fn_cnt;
 381         frnd->lp_cnt = lp_cnt;
 382         frnd->poll_timeout = fpt;
 383         frnd->ele_cnt = ele_cnt;
 384         frnd->pkt_cache = l_queue_new();
 385         frnd->net_key_upd = 0;
 386
 387         subnet = get_primary_subnet(net);
 388         /* TODO: the primary key must be present, do we need to add check?. */
 389
 390         frnd->net_key_cur = net_key_frnd_add(subnet->net_key_cur, dst,
 391                                                 net->src_addr, lp_cnt, fn_cnt);
 392
 393         if (!subnet->net_key_upd)
 394                 return frnd;
 395
 396         frnd->net_idx = subnet->idx;
 397         frnd->net_key_upd = net_key_frnd_add(subnet->net_key_upd, dst,
 398                                                 net->src_addr, lp_cnt, fn_cnt);
 399
 400         return frnd;
 401 }
 402
 403 void mesh_friend_free(void *data)
 404 {
 405         struct mesh_friend *frnd = data;
 406
 407         free_friend_internals(frnd);
 408         l_timeout_remove(frnd->timeout);
 409         l_free(frnd);
 410 }
 411
 412 bool mesh_friend_clear(struct mesh_net *net, struct mesh_friend *frnd)
 413 {
 414         bool removed = l_queue_remove(net->friends, frnd);
 415
 416         free_friend_internals(frnd);
 417
 418         return removed;
 419 }
 420
 421 void mesh_friend_sub_add(struct mesh_net *net, uint16_t lpn, uint8_t ele_cnt,
 422                                         uint8_t grp_cnt, const uint8_t *list)
 423 {
 424         uint16_t *new_list;
 425         uint16_t *grp_list;
 426         struct mesh_friend *frnd = l_queue_find(net->friends,
 427                                                         match_by_friend,
 428                                                         L_UINT_TO_PTR(lpn));
 429         if (!frnd)
 430                 return;
 431
 432         new_list = l_malloc((grp_cnt +
 433                                 frnd->u.active.grp_cnt) * sizeof(uint16_t));
 434         grp_list = frnd->u.active.grp_list;
 435
 436         if (grp_list && frnd->u.active.grp_cnt)
 437                 memcpy(new_list, grp_list,
 438                                 frnd->u.active.grp_cnt * sizeof(uint16_t));
 439
 440         memcpy(&new_list[frnd->u.active.grp_cnt], list,
 441                                                 grp_cnt * sizeof(uint16_t));
 442         l_free(grp_list);
 443         frnd->ele_cnt = ele_cnt;
 444         frnd->u.active.grp_list = new_list;
 445         frnd->u.active.grp_cnt += grp_cnt;
 446 }
 447
 448 void mesh_friend_sub_del(struct mesh_net *net, uint16_t lpn, uint8_t cnt,
 449                                                         const uint8_t *del_list)
 450 {
 451         uint16_t *grp_list;
 452         int16_t i, grp_cnt;
 453         size_t cnt16 = cnt * sizeof(uint16_t);
 454         struct mesh_friend *frnd = l_queue_find(net->friends, match_by_friend,
 455                                                         L_UINT_TO_PTR(lpn));
 456         if (!frnd)
 457                 return;
 458
 459         grp_cnt = frnd->u.active.grp_cnt;
 460         grp_list = frnd->u.active.grp_list;
 461
 462         while (cnt-- && grp_cnt) {
 463                 cnt16 -= sizeof(uint16_t);
 464                 for (i = grp_cnt - 1; i >= 0; i--) {
 465                         if (l_get_le16(del_list + cnt16) == grp_list[i]) {
 466                                 grp_cnt--;
 467                                 memcpy(&grp_list[i], &grp_list[i + 1],
 468                                         (grp_cnt - i) * sizeof(uint16_t));
 469                                 break;
 470                         }
 471                 }
 472         }
 473
 474         frnd->u.active.grp_cnt = grp_cnt;
 475
 476         if (!grp_cnt) {
 477                 l_free(frnd->u.active.grp_list);
 478                 frnd->u.active.grp_list = NULL;
 479         }
 480 }
 481
 482 uint32_t mesh_net_next_seq_num(struct mesh_net *net)
 483 {
 484         uint32_t seq = net->seq_num++;
 485
 486         /*
 487          * Cap out-of-range seq_num max value to +1. Out of range
 488          * seq_nums will not be sent as they would violate spec.
 489          * This condition signals a runaway seq_num condition, and
 490          * the node must wait for a completed IV Index update procedure
 491          * before it can send again.
 492          */
 493         if (net->seq_num > SEQ_MASK)
 494                 net->seq_num = SEQ_MASK + 1;
 495
 496         node_set_sequence_number(net->node, net->seq_num);
 497         return seq;
 498 }
 499
 500 static struct mesh_sar *mesh_sar_new(size_t len)
 501 {
 502         size_t size = sizeof(struct mesh_sar) + len;
 503         struct mesh_sar *sar;
 504
 505         sar = l_malloc(size);
 506         memset(sar, 0, size);
 507         return sar;
 508 }
 509
 510 static void mesh_sar_free(void *data)
 511 {
 512         struct mesh_sar *sar = data;
 513
 514         if (!sar)
 515                 return;
 516
 517         l_timeout_remove(sar->seg_timeout);
 518         l_timeout_remove(sar->msg_timeout);
 519         l_free(sar);
 520 }
 521
 522 static void subnet_free(void *data)
 523 {
 524         struct mesh_subnet *subnet = data;
 525
 526         net_key_unref(subnet->net_key_cur);
 527         net_key_unref(subnet->net_key_upd);
 528         l_free(subnet);
 529 }
 530
 531 static struct mesh_subnet *subnet_new(struct mesh_net *net, uint16_t idx)
 532 {
 533         struct mesh_subnet *subnet;
 534
 535         subnet = l_new(struct mesh_subnet, 1);
 536         if (!subnet)
 537                 return NULL;
 538
 539         subnet->net = net;
 540         subnet->idx = idx;
 541         return subnet;
 542 }
 543
 544 static void enable_beacon(void *a, void *b)
 545 {
 546         struct mesh_subnet *subnet = a;
 547         struct mesh_net *net = b;
 548
 549         if (net->beacon_enable)
 550                 net_key_beacon_enable(subnet->net_key_tx);
 551         else
 552                 net_key_beacon_disable(subnet->net_key_tx);
 553 }
 554
 555 static void enqueue_update(void *a, void *b);
 556
 557 static void queue_friend_update(struct mesh_net *net)
 558 {
 559         struct mesh_subnet *subnet;
 560         struct mesh_friend *frnd;
 561         uint8_t flags = 0;
 562
 563         if (l_queue_length(net->friends)) {
 564                 struct mesh_friend_msg update = {
 565                         .src = net->src_addr,
 566                         .iv_index = mesh_net_get_iv_index(net),
 567                         .last_len = 7,
 568                         .ctl = true,
 569                 };
 570
 571                 frnd = l_queue_peek_head(net->friends);
 572                 subnet = l_queue_find(net->subnets, match_key_index,
 573                                                 L_UINT_TO_PTR(frnd->net_idx));
 574
 575                 if (!subnet)
 576                         return;
 577
 578                 if (subnet->kr_phase == KEY_REFRESH_PHASE_TWO)
 579                         flags |= KEY_REFRESH;
 580
 581                 if (net->iv_update)
 582                         flags |= IV_INDEX_UPDATE;
 583
 584                 update.u.one[0].hdr = NET_OP_FRND_UPDATE << OPCODE_HDR_SHIFT;
 585                 update.u.one[0].seq = mesh_net_next_seq_num(net);
 586                 update.u.one[0].data[0] = NET_OP_FRND_UPDATE;
 587                 update.u.one[0].data[1] = flags;
 588                 l_put_be32(net->iv_index, update.u.one[0].data + 2);
 589                 update.u.one[0].data[6] = 0x01; /* More Data */
 590
 591                 l_queue_foreach(net->friends, enqueue_update, &update);
 592         }
 593 }
 594
 595 static void refresh_beacon(void *a, void *b)
 596 {
 597         struct mesh_subnet *subnet = a;
 598         struct mesh_net *net = b;
 599
 600         net_key_beacon_refresh(subnet->net_key_tx, net->iv_index,
 601                 !!(subnet->kr_phase == KEY_REFRESH_PHASE_TWO), net->iv_update);
 602 }
 603
 604 struct mesh_net *mesh_net_new(struct mesh_node *node)
 605 {
 606         struct mesh_net *net;
 607
 608         net = l_new(struct mesh_net, 1);
 609
 610         net->node = node;
 611         net->seq_num = DEFAULT_SEQUENCE_NUMBER;
 612         net->default_ttl = TTL_MASK;
 613
 614         net->tx_cnt = DEFAULT_TRANSMIT_COUNT;
 615         net->tx_interval = DEFAULT_TRANSMIT_INTERVAL;
 616
 617         net->subnets = l_queue_new();
 618         net->msg_cache = l_queue_new();
 619         net->sar_in = l_queue_new();
 620         net->sar_out = l_queue_new();
 621         net->sar_queue = l_queue_new();
 622         net->frnd_msgs = l_queue_new();
 623         net->destinations = l_queue_new();
 624         net->app_keys = l_queue_new();
 625         net->replay_cache = l_queue_new();
 626
 627         if (!nets)
 628                 nets = l_queue_new();
 629
 630         if (!fast_cache)
 631                 fast_cache = l_queue_new();
 632
 633         return net;
 634 }
 635
 636 void mesh_net_free(void *user_data)
 637 {
 638         struct mesh_net *net = user_data;
 639
 640         if (!net)
 641                 return;
 642
 643         l_queue_destroy(net->subnets, subnet_free);
 644         l_queue_destroy(net->msg_cache, l_free);
 645         l_queue_destroy(net->replay_cache, l_free);
 646         l_queue_destroy(net->sar_in, mesh_sar_free);
 647         l_queue_destroy(net->sar_out, mesh_sar_free);
 648         l_queue_destroy(net->sar_queue, mesh_sar_free);
 649         l_queue_destroy(net->frnd_msgs, l_free);
 650         l_queue_destroy(net->friends, mesh_friend_free);
 651         l_queue_destroy(net->negotiations, mesh_friend_free);
 652         l_queue_destroy(net->destinations, l_free);
 653         l_queue_destroy(net->app_keys, appkey_key_free);
 654
 655         l_free(net);
 656 }
 657
 658 void mesh_net_cleanup(void)
 659 {
 660         l_queue_destroy(fast_cache, l_free);
 661         fast_cache = NULL;
 662         l_queue_destroy(nets, mesh_net_free);
 663         nets = NULL;
 664 }
 665
 666 bool mesh_net_set_seq_num(struct mesh_net *net, uint32_t seq)
 667 {
 668         if (!net)
 669                 return false;
 670
 671         net->seq_num = seq;
 672         node_set_sequence_number(net->node, net->seq_num);
 673
 674         return true;
 675 }
 676
 677 bool mesh_net_set_default_ttl(struct mesh_net *net, uint8_t ttl)
 678 {
 679         if (!net)
 680                 return false;
 681
 682         net->default_ttl = ttl;
 683
 684         return true;
 685 }
 686
 687 uint32_t mesh_net_get_seq_num(struct mesh_net *net)
 688 {
 689         if (!net)
 690                 return 0;
 691
 692         return net->seq_num;
 693 }
 694
 695 uint8_t mesh_net_get_default_ttl(struct mesh_net *net)
 696 {
 697         if (!net)
 698                 return 0;
 699
 700         return net->default_ttl;
 701 }
 702
 703 uint16_t mesh_net_get_address(struct mesh_net *net)
 704 {
 705         if (!net)
 706                 return 0;
 707
 708         return net->src_addr;
 709 }
 710
 711 bool mesh_net_register_unicast(struct mesh_net *net,
 712                                         uint16_t address, uint8_t num_ele)
 713 {
 714         if (!net || !IS_UNICAST(address) || !num_ele)
 715                 return false;
 716
 717         net->src_addr = address;
 718         net->last_addr = address + num_ele - 1;
 719         if (net->last_addr < net->src_addr)
 720                 return false;
 721
 722         do {
 723                 mesh_net_dst_reg(net, address);
 724                 address++;
 725                 num_ele--;
 726         } while (num_ele > 0);
 727
 728         return true;
 729 }
 730
 731 bool mesh_net_set_proxy_mode(struct mesh_net *net, bool enable)
 732 {
 733         if (!net)
 734                 return false;
 735
 736         /* No support for proxy yet */
 737         if (enable) {
 738                 l_error("Proxy not supported!");
 739                 return false;
 740         }
 741
 742         trigger_heartbeat(net, FEATURE_PROXY, enable);
 743         return true;
 744 }
 745
 746 bool mesh_net_set_friend_mode(struct mesh_net *net, bool enable)
 747 {
 748         l_debug("mesh_net_set_friend_mode - %d", enable);
 749
 750         if (!net)
 751                 return false;
 752
 753         if (net->friend_enable == enable)
 754                 return true;
 755
 756         if (enable) {
 757                 net->friends = l_queue_new();
 758                 net->negotiations = l_queue_new();
 759         } else {
 760                 l_queue_destroy(net->friends, mesh_friend_free);
 761                 l_queue_destroy(net->negotiations, mesh_friend_free);
 762                 net->friends = net->negotiations = NULL;
 763         }
 764
 765         net->friend_enable = enable;
 766         trigger_heartbeat(net, FEATURE_FRIEND, enable);
 767         return true;
 768 }
 769
 770 bool mesh_net_set_relay_mode(struct mesh_net *net, bool enable,
 771                                 uint8_t cnt, uint8_t interval)
 772 {
 773         if (!net)
 774                 return false;
 775
 776         net->relay.enable = enable;
 777         net->relay.count = cnt;
 778         net->relay.interval = interval;
 779         trigger_heartbeat(net, FEATURE_RELAY, enable);
 780         return true;
 781 }
 782
 783 int mesh_net_get_identity_mode(struct mesh_net *net, uint16_t idx,
 784                                                                 uint8_t *mode)
 785 {
 786         struct mesh_subnet *subnet;
 787
 788         if (!net)
 789                 return MESH_STATUS_UNSPECIFIED_ERROR;
 790
 791         subnet = l_queue_find(net->subnets, match_key_index,
 792                                                         L_UINT_TO_PTR(idx));
 793         if (!subnet)
 794                 return MESH_STATUS_INVALID_NETKEY;
 795
 796         /* Currently, proxy mode is not supported */
 797         *mode = MESH_MODE_UNSUPPORTED;
 798
 799         return MESH_STATUS_SUCCESS;
 800 }
 801
 802 int mesh_net_del_key(struct mesh_net *net, uint16_t idx)
 803 {
 804         struct mesh_subnet *subnet;
 805
 806         if (!net)
 807                 return MESH_STATUS_UNSPECIFIED_ERROR;
 808
 809         subnet = l_queue_find(net->subnets, match_key_index,
 810                                                         L_UINT_TO_PTR(idx));
 811         if (!subnet)
 812                 return MESH_STATUS_SUCCESS;
 813
 814         /* Cannot remove primary key */
 815         if (l_queue_length(net->subnets) <= 1)
 816                 return MESH_STATUS_CANNOT_REMOVE;
 817
 818         /* Delete associated app keys */
 819         appkey_delete_bound_keys(net, idx);
 820
 821         /* Disable hearbeat publication on this subnet */
 822         if (idx == net->hb_pub.net_idx)
 823                 net->hb_pub.dst = UNASSIGNED_ADDRESS;
 824
 825         /* TODO: cancel beacon_enable on this subnet */
 826
 827         l_queue_remove(net->subnets, subnet);
 828         subnet_free(subnet);
 829
 830         if (!mesh_config_net_key_del(node_config_get(net->node), idx))
 831                 return MESH_STATUS_STORAGE_FAIL;
 832
 833         return MESH_STATUS_SUCCESS;
 834 }
 835
 836 static struct mesh_subnet *add_key(struct mesh_net *net, uint16_t idx,
 837                                                         const uint8_t *value)
 838 {
 839         struct mesh_subnet *subnet;
 840
 841         subnet = subnet_new(net, idx);
 842         if (!subnet)
 843                 return NULL;
 844
 845         subnet->net_key_tx = subnet->net_key_cur = net_key_add(value);
 846         if (!subnet->net_key_cur) {
 847                 l_free(subnet);
 848                 return NULL;
 849         }
 850
 851         net_key_beacon_refresh(subnet->net_key_tx, net->iv_index,
 852                                                 false, net->iv_update);
 853
 854         if (net->beacon_enable)
 855                 net_key_beacon_enable(subnet->net_key_tx);
 856
 857         l_queue_push_tail(net->subnets, subnet);
 858
 859         return subnet;
 860 }
 861
 862 /*
 863  * This function is called when Configuration Server Model receives
 864  * a NETKEY_ADD command
 865  */
 866 int mesh_net_add_key(struct mesh_net *net, uint16_t idx, const uint8_t *value)
 867 {
 868         struct mesh_subnet *subnet;
 869
 870         subnet = l_queue_find(net->subnets, match_key_index,
 871                                                         L_UINT_TO_PTR(idx));
 872
 873         if (subnet) {
 874                 if (net_key_confirm(subnet->net_key_cur, value))
 875                         return MESH_STATUS_SUCCESS;
 876                 else
 877                         return MESH_STATUS_IDX_ALREADY_STORED;
 878         }
 879
 880         subnet = add_key(net, idx, value);
 881         if (!subnet)
 882                 return MESH_STATUS_INSUFF_RESOURCES;
 883
 884         if (!mesh_config_net_key_add(node_config_get(net->node), idx, value)) {
 885                 l_queue_remove(net->subnets, subnet);
 886                 subnet_free(subnet);
 887                 return MESH_STATUS_STORAGE_FAIL;
 888         }
 889
 890         return MESH_STATUS_SUCCESS;
 891 }
 892
 893 uint32_t mesh_net_get_iv_index(struct mesh_net *net)
 894 {
 895         if (!net)
 896                 return 0xffffffff;
 897
 898         return net->iv_index - net->iv_update;
 899 }
 900
 901 /* TODO: net key index? */
 902 void mesh_net_get_snb_state(struct mesh_net *net, uint8_t *flags,
 903                                                         uint32_t *iv_index)
 904 {
 905         struct mesh_subnet *subnet;
 906
 907         if (!net || !flags || !iv_index)
 908                 return;
 909
 910         *iv_index = net->iv_index;
 911         *flags = net->iv_update ? IV_INDEX_UPDATE : 0x00;
 912
 913         subnet = get_primary_subnet(net);
 914         if (subnet)
 915                 *flags |= subnet->key_refresh ? KEY_REFRESH : 0x00;
 916 }
 917
 918 bool mesh_net_get_key(struct mesh_net *net, bool new_key, uint16_t idx,
 919                                                         uint32_t *net_key_id)
 920 {
 921         struct mesh_subnet *subnet;
 922
 923         if (!net)
 924                 return false;
 925
 926         subnet = l_queue_find(net->subnets, match_key_index,
 927                                                         L_UINT_TO_PTR(idx));
 928         if (!subnet)
 929                 return false;
 930
 931         if (!new_key) {
 932                 *net_key_id = subnet->net_key_cur;
 933                 return true;
 934         }
 935
 936         if (!subnet->net_key_upd)
 937                 return false;
 938
 939         *net_key_id = subnet->net_key_upd;
 940         return true;
 941 }
 942
 943 bool mesh_net_key_list_get(struct mesh_net *net, uint8_t *buf, uint16_t *size)
 944 {
 945         const struct l_queue_entry *entry;
 946         uint16_t num_keys, req_size, buf_size;
 947         struct mesh_subnet *subnet;
 948
 949         if (!net || !buf || !size)
 950                 return false;
 951
 952         buf_size = *size;
 953
 954         num_keys = l_queue_length(net->subnets);
 955         req_size = (num_keys / 2) * 3 + (num_keys % 2) * 2;
 956
 957         if (buf_size < req_size)
 958                 return false;
 959
 960         *size = req_size;
 961
 962         /* Pack NetKey indices in 3 octets */
 963         for (entry = l_queue_get_entries(net->subnets); num_keys > 1;) {
 964                 uint32_t idx_pair;
 965
 966                 subnet = entry->data;
 967                 idx_pair = subnet->idx;
 968                 idx_pair <<= 12;
 969
 970                 subnet = entry->next->data;
 971                 idx_pair += subnet->idx;
 972
 973                 l_put_le32(idx_pair, buf);
 974                 buf += 3;
 975
 976                 num_keys -= 2;
 977                 entry = entry->next->next;
 978         }
 979
 980         /* If odd number of NetKeys, fill in the end of the buffer */
 981         if (num_keys % 2) {
 982                 subnet = entry->data;
 983                 l_put_le16(subnet->idx, buf);
 984         }
 985
 986         return true;
 987 }
 988
 989 bool mesh_net_get_frnd_seq(struct mesh_net *net)
 990 {
 991         if (!net)
 992                 return false;
 993
 994         return net->friend_seq;
 995 }
 996
 997 void mesh_net_set_frnd_seq(struct mesh_net *net, bool seq)
 998 {
 999         if (!net)
1000                 return;
1001
1002         net->friend_seq = seq;
1003 }
1004
1005 static bool match_cache(const void *a, const void *b)
1006 {
1007         const struct mesh_msg *msg = a;
1008         const struct mesh_msg *tst = b;
1009
1010         if (msg->seq != tst->seq || msg->mic != tst->mic ||
1011                                         msg->src != tst->src)
1012                 return false;
1013
1014         return true;
1015 }
1016
1017 static bool msg_in_cache(struct mesh_net *net, uint16_t src, uint32_t seq,
1018                                                                 uint32_t mic)
1019 {
1020         struct mesh_msg *msg;
1021         struct mesh_msg tst = {
1022                 .src = src,
1023                 .seq = seq,
1024                 .mic = mic,
1025         };
1026
1027         msg = l_queue_remove_if(net->msg_cache, match_cache, &tst);
1028
1029         if (msg) {
1030                 l_debug("Supressing duplicate %4.4x + %6.6x + %8.8x",
1031                                                         src, seq, mic);
1032                 l_queue_push_head(net->msg_cache, msg);
1033                 return true;
1034         }
1035
1036         msg = l_new(struct mesh_msg, 1);
1037         *msg = tst;
1038         l_queue_push_head(net->msg_cache, msg);
1039         l_debug("Add %4.4x + %6.6x + %8.8x", src, seq, mic);
1040
1041         if (l_queue_length(net->msg_cache) > MSG_CACHE_SIZE) {
1042                 msg = l_queue_peek_tail(net->msg_cache);
1043                 /* Remove Tail (oldest msg in cache) */
1044                 l_debug("Remove %4.4x + %6.6x + %8.8x",
1045                                                 msg->src, msg->seq, msg->mic);
1046                 if (l_queue_remove(net->msg_cache, msg))
1047                         l_free(msg);
1048         }
1049
1050         return false;
1051 }
1052
1053 static bool match_sar_seq0(const void *a, const void *b)
1054 {
1055         const struct mesh_sar *sar = a;
1056         uint16_t seqZero = L_PTR_TO_UINT(b);
1057
1058         return sar->seqZero == seqZero;
1059 }
1060
1061 static bool match_sar_remote(const void *a, const void *b)
1062 {
1063         const struct mesh_sar *sar = a;
1064         uint16_t remote = L_PTR_TO_UINT(b);
1065
1066         return sar->remote == remote;
1067 }
1068
1069 static bool match_msg_timeout(const void *a, const void *b)
1070 {
1071         const struct mesh_sar *sar = a;
1072         const struct l_timeout *msg_timeout = b;
1073
1074         return sar->msg_timeout == msg_timeout;
1075 }
1076
1077 static bool match_seg_timeout(const void *a, const void *b)
1078 {
1079         const struct mesh_sar *sar = a;
1080         const struct l_timeout *seg_timeout = b;
1081
1082         return sar->seg_timeout == seg_timeout;
1083 }
1084
1085 static bool match_dest_dst(const void *a, const void *b)
1086 {
1087         const struct mesh_destination *dest = a;
1088         uint16_t dst = L_PTR_TO_UINT(b);
1089
1090         return dst == dest->dst;
1091 }
1092
1093 static bool match_frnd_dst(const void *a, const void *b)
1094 {
1095         const struct mesh_friend *frnd = a;
1096         uint16_t dst = L_PTR_TO_UINT(b);
1097         int16_t i, grp_cnt = frnd->u.active.grp_cnt;
1098         uint16_t *grp_list = frnd->u.active.grp_list;
1099
1100         /*
1101          * Determine if this message is for this friends unicast
1102          * address, and/or one of it's group/virtual addresses
1103          */
1104         if (dst >= frnd->lp_addr && dst < (frnd->lp_addr + frnd->ele_cnt))
1105                 return true;
1106
1107         if (!(dst & 0x8000))
1108                 return false;
1109
1110         for (i = 0; i < grp_cnt; i++) {
1111                 if (dst == grp_list[i])
1112                         return true;
1113         }
1114
1115         return false;
1116 }
1117
1118 static bool is_lpn_friend(struct mesh_net *net, uint16_t addr)
1119 {
1120         void *tst;
1121
1122         tst = l_queue_find(net->friends, match_frnd_dst, L_UINT_TO_PTR(addr));
1123
1124         return tst != NULL;
1125 }
1126
1127 static bool is_us(struct mesh_net *net, uint16_t addr, bool src)
1128 {
1129         void *tst;
1130
1131         if (IS_ALL_NODES(addr))
1132                 return true;
1133
1134         if (addr == FRIENDS_ADDRESS)
1135                 return net->friend_enable;
1136
1137         if (addr == RELAYS_ADDRESS)
1138                 return net->relay.enable;
1139
1140         if (addr == PROXIES_ADDRESS)
1141                 return net->proxy_enable;
1142
1143         if (addr >= net->src_addr && addr <= net->last_addr)
1144                 return true;
1145
1146         tst = l_queue_find(net->destinations, match_dest_dst,
1147                                                         L_UINT_TO_PTR(addr));
1148
1149         if (tst == NULL && !src)
1150                 tst = l_queue_find(net->friends, match_frnd_dst,
1151                                                         L_UINT_TO_PTR(addr));
1152
1153         return tst != NULL;
1154 }
1155
1156 static struct mesh_friend_msg *mesh_friend_msg_new(uint8_t seg_max)
1157 {
1158         struct mesh_friend_msg *frnd_msg;
1159
1160         if (seg_max) {
1161                 size_t size = sizeof(struct mesh_friend_msg) -
1162                                         sizeof(struct mesh_friend_seg_one);
1163
1164                 size += (seg_max + 1) * sizeof(struct mesh_friend_seg_12);
1165                 frnd_msg = l_malloc(size);
1166                 memset(frnd_msg, 0, size);
1167         } else
1168                 frnd_msg = l_new(struct mesh_friend_msg, 1);
1169
1170
1171         return frnd_msg;
1172 }
1173
1174
1175 static bool match_ack(const void *a, const void *b)
1176 {
1177         const struct mesh_friend_msg *old = a;
1178         const struct mesh_friend_msg *rx = b;
1179         uint32_t old_hdr;
1180         uint32_t new_hdr;
1181
1182         /* Determine if old pkt is ACK to same SAR message that new ACK is */
1183         if (!old->ctl || old->src != rx->src)
1184                 return false;
1185
1186         /* Check the quickest items first before digging deeper */
1187         old_hdr = old->u.one[0].hdr & HDR_ACK_MASK;
1188         new_hdr = rx->u.one[0].hdr & HDR_ACK_MASK;
1189
1190         return old_hdr == new_hdr;
1191 }
1192
1193 static void enqueue_friend_pkt(void *a, void *b)
1194 {
1195         struct mesh_friend *frnd = a;
1196         struct mesh_friend_msg *pkt, *rx = b;
1197         size_t size;
1198         int16_t i;
1199
1200         if (rx->done)
1201                 return;
1202
1203         /*
1204          * Determine if this message is for this friends unicast
1205          * address, and/or one of it's group/virtual addresses
1206          */
1207         if (rx->dst >= frnd->lp_addr && (rx->dst - frnd->lp_addr) <
1208                                                         frnd->ele_cnt) {
1209                 rx->done = true;
1210                 goto enqueue;
1211         }
1212
1213         if (!(rx->dst & 0x8000))
1214                 return;
1215
1216         if (!IS_ALL_NODES(rx->dst)) {
1217                 for (i = 0; i < frnd->u.active.grp_cnt; i++) {
1218                         if (rx->dst == frnd->u.active.grp_list[i])
1219                                 goto enqueue;
1220                 }
1221                 return;
1222         }
1223
1224 enqueue:
1225         /* Special handling for Seg Ack -- Only one per message queue */
1226         if (((rx->u.one[0].hdr >> OPCODE_HDR_SHIFT) & OPCODE_MASK) ==
1227                                                 NET_OP_SEG_ACKNOWLEDGE) {
1228                 void *old_head = l_queue_peek_head(frnd->pkt_cache);
1229                 /* Suppress duplicate ACKs */
1230                 do {
1231                         void *old = l_queue_remove_if(frnd->pkt_cache,
1232                                                         match_ack, rx);
1233
1234                         if (!old)
1235                                 break;
1236
1237                         if (old_head == old)
1238                                 /*
1239                                  * If we are discarding head for any
1240                                  * reason, reset FRND SEQ
1241                                  */
1242                                 frnd->u.active.last = frnd->u.active.seq;
1243
1244                         l_free(old);
1245
1246                 } while (true);
1247         }
1248
1249         l_debug("%s for %4.4x from %4.4x ttl: %2.2x (seq: %6.6x) (ctl: %d)",
1250                         __func__, frnd->lp_addr, rx->src, rx->ttl,
1251                         rx->u.one[0].seq, rx->ctl);
1252
1253         if (rx->cnt_in) {
1254                 size = sizeof(struct mesh_friend_msg) -
1255                                 sizeof(struct mesh_friend_seg_one);
1256                 size += (rx->cnt_in + 1) * sizeof(struct mesh_friend_seg_12);
1257         } else
1258                 size = sizeof(struct mesh_friend_msg);
1259
1260         pkt = l_malloc(size);
1261         memcpy(pkt, rx, size);
1262
1263         l_queue_push_tail(frnd->pkt_cache, pkt);
1264
1265         if (l_queue_length(frnd->pkt_cache) > FRND_CACHE_MAX) {
1266                 /*
1267                  * TODO: Guard against popping UPDATE packets
1268                  * (disallowed per spec)
1269                  */
1270                 pkt = l_queue_pop_head(frnd->pkt_cache);
1271                 l_free(pkt);
1272                 frnd->u.active.last = frnd->u.active.seq;
1273         }
1274 }
1275
1276 static void enqueue_update(void *a, void *b)
1277 {
1278         struct mesh_friend *frnd = a;
1279         struct mesh_friend_msg *pkt = b;
1280
1281         pkt->dst = frnd->lp_addr;
1282         pkt->done = false;
1283         enqueue_friend_pkt(frnd, pkt);
1284 }
1285
1286 static uint32_t seq_auth(uint32_t seq, uint16_t seqZero)
1287 {
1288         uint32_t seqAuth = seqZero & SEQ_ZERO_MASK;
1289
1290         seqAuth |= seq & (~SEQ_ZERO_MASK);
1291         if (seqAuth > seq)
1292                 seqAuth -= (SEQ_ZERO_MASK + 1);
1293
1294         return seqAuth;
1295 }
1296
1297 static bool friend_packet_queue(struct mesh_net *net,
1298                                         uint32_t iv_index,
1299                                         bool ctl, uint8_t ttl,
1300                                         uint32_t seq,
1301                                         uint16_t src, uint16_t dst,
1302                                         uint32_t hdr,
1303                                         const uint8_t *data, uint16_t size)
1304 {
1305         struct mesh_friend_msg *frnd_msg;
1306         uint8_t seg_max = SEG_TOTAL(hdr);
1307         bool ret;
1308
1309         if (seg_max && !IS_SEGMENTED(hdr))
1310                 return false;
1311
1312         frnd_msg = mesh_friend_msg_new(seg_max);
1313
1314         if (IS_SEGMENTED(hdr)) {
1315                 uint32_t seqAuth = seq_auth(seq, hdr >> SEQ_ZERO_HDR_SHIFT);
1316                 uint8_t i;
1317
1318                 for (i = 0; i <= seg_max; i++) {
1319                         memcpy(frnd_msg->u.s12[i].data, data, 12);
1320                         frnd_msg->u.s12[i].hdr = hdr;
1321                         frnd_msg->u.s12[i].seq = seqAuth + i;
1322                         data += 12;
1323                         hdr += (1 << SEGO_HDR_SHIFT);
1324                 }
1325
1326                 frnd_msg->cnt_in = seg_max;
1327                 frnd_msg->last_len = size % 12;
1328                 if (!frnd_msg->last_len)
1329                         frnd_msg->last_len = 12;
1330         } else {
1331                 uint8_t opcode = hdr >> OPCODE_HDR_SHIFT;
1332
1333                 if (ctl && opcode != NET_OP_SEG_ACKNOWLEDGE) {
1334
1335                         /* Don't cache Friend Ctl opcodes */
1336                         if (FRND_OPCODE(opcode)) {
1337                                 l_free(frnd_msg);
1338                                 return false;
1339                         }
1340
1341                         memcpy(frnd_msg->u.one[0].data + 1, data, size);
1342                         frnd_msg->last_len = size + 1;
1343                         frnd_msg->u.one[0].data[0] = opcode;
1344                 } else {
1345                         memcpy(frnd_msg->u.one[0].data, data, size);
1346                         frnd_msg->last_len = size;
1347                 }
1348
1349                 frnd_msg->u.one[0].hdr = hdr;
1350                 frnd_msg->u.one[0].seq = seq;
1351         }
1352
1353         frnd_msg->iv_index = iv_index;
1354         frnd_msg->src = src;
1355         frnd_msg->dst = dst;
1356         frnd_msg->ctl = ctl;
1357         frnd_msg->ttl = ttl;
1358
1359         /* Re-Package into Friend Delivery payload */
1360         l_queue_foreach(net->friends, enqueue_friend_pkt, frnd_msg);
1361         ret = frnd_msg->done;
1362
1363         /* TODO Optimization(?): Unicast messages keep this buffer */
1364         l_free(frnd_msg);
1365
1366         return ret;
1367 }
1368
1369 static void friend_ack_rxed(struct mesh_net *net, uint32_t iv_index,
1370                                         uint32_t seq,
1371                                         uint16_t src, uint16_t dst,
1372                                         const uint8_t *pkt)
1373 {
1374         uint32_t hdr = l_get_be32(pkt) &
1375                 ((SEQ_ZERO_MASK << SEQ_ZERO_HDR_SHIFT) | /* Preserve SeqZero */
1376                  ((uint32_t) 0x01 << RELAY_HDR_SHIFT)); /* Preserve Relay bit */
1377         uint32_t flags = l_get_be32(pkt + 3);
1378         struct mesh_friend_msg frnd_ack = {
1379                 .ctl = true,
1380                 .iv_index = iv_index,
1381                 .src = src,
1382                 .dst = dst,
1383                 .last_len = sizeof(flags),
1384                 .u.one[0].seq = seq,
1385                 .done = false,
1386         };
1387
1388         hdr |= NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
1389         frnd_ack.u.one[0].hdr = hdr;
1390         l_put_be32(flags, frnd_ack.u.one[0].data);
1391         l_queue_foreach(net->friends, enqueue_friend_pkt, &frnd_ack);
1392 }
1393
1394 static bool send_seg(struct mesh_net *net, uint8_t cnt, uint16_t interval,
1395                                         struct mesh_sar *msg, uint8_t seg);
1396
1397 static void send_frnd_ack(struct mesh_net *net, uint16_t src, uint16_t dst,
1398                                                 uint32_t hdr, uint32_t flags)
1399 {
1400         uint32_t expected;
1401         uint8_t msg[7];
1402
1403         /* We don't ACK from multicast destinations */
1404         if (src & 0x8000)
1405                 return;
1406
1407         /* Calculate the "Full ACK" mask */
1408         expected = 0xffffffff >> (31 - SEG_TOTAL(hdr));
1409
1410         /* Clear Hdr bits that don't apply to Seg ACK */
1411         hdr &= ~(((uint32_t) 0x01 << SEG_HDR_SHIFT) |
1412                         (OPCODE_MASK << OPCODE_HDR_SHIFT) |
1413                         ((uint32_t) 0x01 << SZMIC_HDR_SHIFT) |
1414                         (SEG_MASK << SEGO_HDR_SHIFT) |
1415                         (SEG_MASK << SEGN_HDR_SHIFT));
1416
1417         hdr |= NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
1418         hdr |= (uint32_t) 0x01 << RELAY_HDR_SHIFT;
1419
1420         /* Clear all unexpected bits */
1421         flags &= expected;
1422
1423         l_put_be32(hdr, msg);
1424         l_put_be32(flags, msg + 3);
1425
1426         l_debug("Send Friend ACK to Segs: %8.8x", flags);
1427
1428         if (is_lpn_friend(net, dst)) {
1429                 /* If we are acking our LPN Friend, queue, don't send */
1430                 friend_ack_rxed(net, mesh_net_get_iv_index(net),
1431                                 mesh_net_next_seq_num(net), 0, dst, msg);
1432         } else {
1433                 mesh_net_transport_send(net, 0, 0,
1434                                 mesh_net_get_iv_index(net), DEFAULT_TTL,
1435                                 0, 0, dst, msg, sizeof(msg));
1436         }
1437 }
1438
1439 static void send_net_ack(struct mesh_net *net, struct mesh_sar *sar,
1440                                                                 uint32_t flags)
1441 {
1442         uint8_t msg[7];
1443         uint32_t hdr;
1444         uint16_t src = sar->src;
1445         uint16_t dst = sar->remote;
1446
1447         /* We don't ACK from multicast destinations */
1448         if (src & 0x8000)
1449                 return;
1450
1451         hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
1452         hdr |= sar->seqZero << SEQ_ZERO_HDR_SHIFT;
1453
1454         if (is_lpn_friend(net, src))
1455                 hdr |= (uint32_t) 0x01 << RELAY_HDR_SHIFT;
1456
1457         l_put_be32(hdr, msg);
1458         l_put_be32(flags, msg + 3);
1459         l_debug("Send%s ACK to Segs: %8.8x", sar->frnd ? " Friend" : "", flags);
1460
1461         if (is_lpn_friend(net, dst)) {
1462                 /* If we are acking our LPN Friend, queue, don't send */
1463                 friend_ack_rxed(net, mesh_net_get_iv_index(net),
1464                                 mesh_net_next_seq_num(net), src, dst, msg);
1465                 return;
1466         }
1467
1468         mesh_net_transport_send(net, 0, sar->net_idx,
1469                                 mesh_net_get_iv_index(net), DEFAULT_TTL,
1470                                 0, src, dst, msg,
1471                                 sizeof(msg));
1472 }
1473
1474 static void inseg_to(struct l_timeout *seg_timeout, void *user_data)
1475 {
1476         struct mesh_net *net = user_data;
1477         struct mesh_sar *sar = l_queue_find(net->sar_in,
1478                                         match_seg_timeout, seg_timeout);
1479
1480         l_timeout_remove(seg_timeout);
1481         if (!sar)
1482                 return;
1483
1484         /* Send NAK */
1485         l_debug("Timeout %p %3.3x", sar, sar->app_idx);
1486         send_net_ack(net, sar, sar->flags);
1487
1488         sar->seg_timeout = l_timeout_create(SEG_TO, inseg_to, net, NULL);
1489 }
1490
1491 static void inmsg_to(struct l_timeout *msg_timeout, void *user_data)
1492 {
1493         struct mesh_net *net = user_data;
1494         struct mesh_sar *sar = l_queue_remove_if(net->sar_in,
1495                         match_msg_timeout, msg_timeout);
1496
1497         l_timeout_remove(msg_timeout);
1498         if (!sar)
1499                 return;
1500
1501         sar->msg_timeout = NULL;
1502         mesh_sar_free(sar);
1503 }
1504
1505 static void outmsg_to(struct l_timeout *msg_timeout, void *user_data)
1506 {
1507         struct mesh_net *net = user_data;
1508         struct mesh_sar *sar = l_queue_remove_if(net->sar_out,
1509                         match_msg_timeout, msg_timeout);
1510
1511         l_timeout_remove(msg_timeout);
1512         if (!sar)
1513                 return;
1514
1515         sar->msg_timeout = NULL;
1516         mesh_sar_free(sar);
1517 }
1518
1519 static void outseg_to(struct l_timeout *seg_timeout, void *user_data);
1520
1521 static void send_queued_sar(struct mesh_net *net, uint16_t dst)
1522 {
1523         struct mesh_sar *sar = l_queue_remove_if(net->sar_queue,
1524                         match_sar_remote, L_UINT_TO_PTR(dst));
1525
1526         if (!sar)
1527                 return;
1528
1529         /* Out to current outgoing, and immediate expire Seg TO */
1530         l_queue_push_head(net->sar_out, sar);
1531         sar->seg_timeout = NULL;
1532         sar->msg_timeout = l_timeout_create(MSG_TO, outmsg_to, net, NULL);
1533         outseg_to(NULL, net);
1534 }
1535
1536 static void ack_received(struct mesh_net *net, bool timeout,
1537                                 uint16_t src, uint16_t dst,
1538                                 uint16_t seq0, uint32_t ack_flag)
1539 {
1540         struct mesh_sar *outgoing;
1541         uint32_t seg_flag = 0x00000001;
1542         uint32_t ack_copy = ack_flag;
1543         uint16_t i;
1544
1545         l_debug("ACK Rxed (%x) (to:%d): %8.8x", seq0, timeout, ack_flag);
1546
1547         outgoing = l_queue_find(net->sar_out, match_sar_seq0,
1548                                                         L_UINT_TO_PTR(seq0));
1549
1550         if (!outgoing) {
1551                 l_debug("Not Found: %4.4x", seq0);
1552                 return;
1553         }
1554
1555         /*
1556          * TODO -- If we receive from different
1557          * SRC than we are sending to, make sure the OBO flag is set
1558          */
1559
1560         if ((!timeout && !ack_flag) ||
1561                         (outgoing->flags & ack_flag) == outgoing->flags) {
1562                 l_debug("ob_sar_removal (%x)", outgoing->flags);
1563
1564                 /* Note: ack_flags == 0x00000000 is a remote Cancel request */
1565
1566                 l_queue_remove(net->sar_out, outgoing);
1567                 send_queued_sar(net, outgoing->remote);
1568                 mesh_sar_free(outgoing);
1569
1570                 return;
1571         }
1572
1573         outgoing->last_nak |= ack_flag;
1574
1575         ack_copy &= outgoing->flags;
1576
1577         for (i = 0; i <= SEG_MAX(true, outgoing->len); i++, seg_flag <<= 1) {
1578                 if (seg_flag & ack_flag) {
1579                         l_debug("Skipping Seg %d of %d",
1580                                         i, SEG_MAX(true, outgoing->len));
1581                         continue;
1582                 }
1583
1584                 ack_copy |= seg_flag;
1585
1586                 l_debug("Resend Seg %d net:%p dst:%x app_idx:%3.3x",
1587                                 i, net, outgoing->remote, outgoing->app_idx);
1588
1589                 send_seg(net, net->tx_cnt, net->tx_interval, outgoing, i);
1590         }
1591
1592         l_timeout_remove(outgoing->seg_timeout);
1593         outgoing->seg_timeout = l_timeout_create(SEG_TO, outseg_to, net, NULL);
1594 }
1595
1596 static void outseg_to(struct l_timeout *seg_timeout, void *user_data)
1597 {
1598         struct mesh_net *net = user_data;
1599         struct mesh_sar *sar = l_queue_find(net->sar_out,
1600                                         match_seg_timeout, seg_timeout);
1601
1602         l_timeout_remove(seg_timeout);
1603         if (!sar)
1604                 return;
1605
1606         sar->seg_timeout = NULL;
1607
1608         /* Re-Send missing segments by faking NACK */
1609         ack_received(net, true, sar->remote, sar->src,
1610                                         sar->seqZero, sar->last_nak);
1611 }
1612
1613 static bool match_replay_cache(const void *a, const void *b)
1614 {
1615         const struct mesh_rpl *rpe = a;
1616         uint16_t src = L_PTR_TO_UINT(b);
1617
1618         return src == rpe->src;
1619 }
1620
1621 static bool clean_old_iv_index(void *a, void *b)
1622 {
1623         struct mesh_rpl *rpe = a;
1624         uint32_t iv_index = L_PTR_TO_UINT(b);
1625
1626         if (iv_index < 2)
1627                 return false;
1628
1629         if (rpe->iv_index < iv_index - 1) {
1630                 l_free(rpe);
1631                 return true;
1632         }
1633
1634         return false;
1635 }
1636
1637 static bool msg_check_replay_cache(struct mesh_net *net, uint16_t src,
1638                                 uint16_t crpl, uint32_t seq, uint32_t iv_index)
1639 {
1640         struct mesh_rpl *rpe;
1641
1642         /* If anything missing reject this message by returning true */
1643         if (!net || !net->node)
1644                 return true;
1645
1646         rpe = l_queue_find(net->replay_cache, match_replay_cache,
1647                                                 L_UINT_TO_PTR(src));
1648
1649         if (rpe) {
1650                 if (iv_index > rpe->iv_index)
1651                         return false;
1652
1653                 /* Return true if (iv_index | seq) too low */
1654                 if (iv_index < rpe->iv_index || seq <= rpe->seq) {
1655                         l_debug("Ignoring replayed packet");
1656                         return true;
1657                 }
1658         } else if (l_queue_length(net->replay_cache) >= crpl) {
1659                 /* SRC not in Replay Cache... see if there is space for it */
1660
1661                 int ret = l_queue_foreach_remove(net->replay_cache,
1662                                 clean_old_iv_index, L_UINT_TO_PTR(iv_index));
1663
1664                 /* Return true if no space could be freed */
1665                 if (!ret) {
1666                         l_debug("Replay cache full");
1667                         return true;
1668                 }
1669         }
1670
1671         return false;
1672 }
1673
1674 static void msg_add_replay_cache(struct mesh_net *net, uint16_t src,
1675                                                 uint32_t seq, uint32_t iv_index)
1676 {
1677         struct mesh_rpl *rpe;
1678
1679         if (!net || !net->replay_cache)
1680                 return;
1681
1682         rpe = l_queue_remove_if(net->replay_cache, match_replay_cache,
1683                                                 L_UINT_TO_PTR(src));
1684
1685         if (!rpe) {
1686                 rpe = l_new(struct mesh_rpl, 1);
1687                 rpe->src = src;
1688         }
1689
1690         rpe->seq = seq;
1691         rpe->iv_index = iv_index;
1692         rpl_put_entry(net->node, src, iv_index, seq);
1693
1694         /* Optimize so that most recent conversations stay earliest in cache */
1695         l_queue_push_head(net->replay_cache, rpe);
1696 }
1697
1698 static bool msg_rxed(struct mesh_net *net, bool frnd, uint32_t iv_index,
1699                                         uint8_t ttl, uint32_t seq,
1700                                         uint16_t net_idx,
1701                                         uint16_t src, uint16_t dst,
1702                                         uint8_t key_aid, bool segmented,
1703                                         bool szmic, uint16_t seqZero,
1704                                         const uint8_t *data, uint16_t size)
1705 {
1706         uint32_t seqAuth = seq_auth(seq, seqZero);
1707         uint16_t crpl;
1708
1709         /* Sanity check seqAuth */
1710         if (seqAuth > seq)
1711                 return false;
1712
1713         /* Save un-decrypted messages for our friends */
1714         if (!frnd && l_queue_length(net->friends)) {
1715                 uint32_t hdr = key_aid << KEY_HDR_SHIFT;
1716                 uint8_t frnd_ttl = ttl;
1717
1718                 /* If not from us, decrement for our hop */
1719                 if (src < net->src_addr || src > net->last_addr) {
1720                         if (frnd_ttl > 1)
1721                                 frnd_ttl--;
1722                         else
1723                                 goto not_for_friend;
1724                 }
1725
1726                 if (szmic || size > 15) {
1727                         hdr |= (uint32_t) 0x01 << SEG_HDR_SHIFT;
1728                         hdr |= szmic << SZMIC_HDR_SHIFT;
1729                         hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT;
1730                         hdr |= SEG_MAX(true, size) << SEGN_HDR_SHIFT;
1731                 }
1732
1733                 if (friend_packet_queue(net, iv_index, false, frnd_ttl,
1734                                         seq, src, dst,
1735                                         hdr, data, size))
1736                         return true;
1737         }
1738
1739 not_for_friend:
1740         if (dst == FRIENDS_ADDRESS && !net->friend_enable)
1741                 return false;
1742
1743         if (dst == RELAYS_ADDRESS && !net->relay.enable)
1744                 return false;
1745
1746         if (dst == PROXIES_ADDRESS && !net->proxy_enable)
1747                 return false;
1748
1749         /* Don't process if already in RPL */
1750         crpl = node_get_crpl(net->node);
1751
1752         if (msg_check_replay_cache(net, src, crpl, seq, iv_index))
1753                 return false;
1754
1755         if (!mesh_model_rx(net->node, szmic, seqAuth, iv_index, net_idx, src,
1756                                                 dst, key_aid, data, size))
1757                 return false;
1758
1759         /* If message has been handled by us, add to RPL */
1760         msg_add_replay_cache(net, src, seq, iv_index);
1761         return true;
1762 }
1763
1764 static uint16_t key_id_to_net_idx(struct mesh_net *net, uint32_t net_key_id)
1765 {
1766         struct mesh_subnet *subnet;
1767         struct mesh_friend *friend;
1768
1769         if (!net)
1770                 return NET_IDX_INVALID;
1771
1772         subnet = l_queue_find(net->subnets, match_key_id,
1773                                                 L_UINT_TO_PTR(net_key_id));
1774
1775         if (subnet)
1776                 return subnet->idx;
1777
1778         friend = l_queue_find(net->friends, match_friend_key_id,
1779                                                 L_UINT_TO_PTR(net_key_id));
1780
1781         if (friend)
1782                 return friend->net_idx;
1783
1784         friend = l_queue_find(net->negotiations, match_friend_key_id,
1785                                                 L_UINT_TO_PTR(net_key_id));
1786
1787         if (friend)
1788                 return friend->net_idx;
1789         else
1790                 return NET_IDX_INVALID;
1791 }
1792
1793 static bool match_frnd_sar_dst(const void *a, const void *b)
1794 {
1795         const struct mesh_friend_msg *frnd_msg = a;
1796         uint16_t dst = L_PTR_TO_UINT(b);
1797
1798         return frnd_msg->dst == dst;
1799 }
1800
1801 static void friend_seg_rxed(struct mesh_net *net,
1802                                 uint32_t iv_index,
1803                                 uint8_t ttl, uint32_t seq,
1804                                 uint16_t src, uint16_t dst, uint32_t hdr,
1805                                 const uint8_t *data, uint8_t size)
1806 {
1807         struct mesh_friend *frnd = NULL;
1808         struct mesh_friend_msg *frnd_msg = NULL;
1809         uint8_t cnt;
1810         uint8_t segN = hdr & 0x1f;
1811         uint8_t segO = ((hdr >> 5) & 0x1f);
1812         uint32_t expected = 0xffffffff >> (31 - segN);
1813         uint32_t this_seg_flag = 0x00000001 << segO;
1814         uint32_t largest = (0xffffffff << segO) & expected;
1815         uint32_t hdr_key =  hdr & HDR_KEY_MASK;
1816
1817         frnd = l_queue_find(net->friends, match_frnd_dst,
1818                         L_UINT_TO_PTR(dst));
1819         if (!frnd)
1820                 return;
1821
1822         if (frnd->u.active.last_hdr == hdr_key) {
1823                 /* We are no longer receiving this msg. Resend final ACK */
1824                 send_frnd_ack(net, dst, src, frnd->u.active.last_hdr,
1825                                                                 0xffffffff);
1826                 return;
1827         }
1828
1829         /* Check if we have a SAR-in-progress that matches incoming segment */
1830         frnd_msg = l_queue_find(net->frnd_msgs, match_frnd_sar_dst,
1831                         L_UINT_TO_PTR(dst));
1832
1833         if (frnd_msg) {
1834                 /* Flush if SZMICN or IV Index has changed */
1835                 if (frnd_msg->iv_index != iv_index)
1836                         frnd_msg->u.s12[0].hdr = 0;
1837
1838                 /* Flush incomplete old SAR message if it doesn't match */
1839                 if ((frnd_msg->u.s12[0].hdr & HDR_KEY_MASK) != hdr_key) {
1840                         l_queue_remove(net->frnd_msgs, frnd_msg);
1841                         l_free(frnd_msg);
1842                         frnd_msg = NULL;
1843                 }
1844         }
1845
1846         if (!frnd_msg) {
1847                 frnd_msg = mesh_friend_msg_new(segN);
1848                 frnd_msg->iv_index = iv_index;
1849                 frnd_msg->src = src;
1850                 frnd_msg->dst = dst;
1851                 frnd_msg->ttl = ttl;
1852                 l_queue_push_tail(net->frnd_msgs, frnd_msg);
1853         } else if (frnd_msg->flags & this_seg_flag) /* Ignore dup segs */
1854                 return;
1855
1856         cnt = frnd_msg->cnt_in;
1857         frnd_msg->flags |= this_seg_flag;
1858
1859         frnd_msg->u.s12[cnt].hdr = hdr;
1860         frnd_msg->u.s12[cnt].seq = seq;
1861         memcpy(frnd_msg->u.s12[cnt].data, data, size);
1862
1863         /* Last segment could be short */
1864         if (segN == segO)
1865                 frnd_msg->last_len = size;
1866
1867         l_debug("RXed Seg %d, Flags %8.8x (cnt: %d)",
1868                                                 segO, frnd_msg->flags, cnt);
1869
1870         /* In reality, if one of these is true, then *both* must be true */
1871         if ((cnt == segN) || (frnd_msg->flags == expected)) {
1872                 l_debug("Full ACK");
1873                 send_frnd_ack(net, dst, src, hdr, frnd_msg->flags);
1874
1875                 if (frnd_msg->ttl > 1) {
1876                         frnd_msg->ttl--;
1877                         /* Add to friends cache  */
1878                         l_queue_foreach(net->friends,
1879                                         enqueue_friend_pkt, frnd_msg);
1880                 }
1881
1882                 /* Remove from "in progress" queue */
1883                 l_queue_remove(net->frnd_msgs, frnd_msg);
1884
1885                 /* TODO Optimization(?): Unicast messages keep this buffer */
1886                 l_free(frnd_msg);
1887                 return;
1888         }
1889
1890         /* Always ACK if this is the largest outstanding segment */
1891         if ((largest & frnd_msg->flags) == largest) {
1892                 l_debug("Partial ACK");
1893                 send_frnd_ack(net, dst, src, hdr, frnd_msg->flags);
1894         }
1895
1896         frnd_msg->cnt_in++;
1897 }
1898
1899 static bool seg_rxed(struct mesh_net *net, bool frnd, uint32_t iv_index,
1900                                         uint8_t ttl, uint32_t seq,
1901                                         uint16_t net_idx,
1902                                         uint16_t src, uint16_t dst,
1903                                         uint8_t key_aid,
1904                                         bool szmic, uint16_t seqZero,
1905                                         uint8_t segO, uint8_t segN,
1906                                         const uint8_t *data, uint8_t size)
1907 {
1908         struct mesh_sar *sar_in = NULL;
1909         uint16_t seg_off = 0;
1910         uint32_t expected, this_seg_flag, largest, seqAuth;
1911         bool reset_seg_to = true;
1912
1913         /*
1914          * DST could receive additional Segments after
1915          * completing due to a lost ACK, so re-ACK and discard
1916          */
1917         sar_in = l_queue_find(net->sar_in, match_sar_remote,
1918                                                 L_UINT_TO_PTR(src));
1919
1920         /* Discard *old* incoming-SAR-in-progress if this segment newer */
1921         seqAuth = seq_auth(seq, seqZero);
1922         if (sar_in && (sar_in->seqAuth != seqAuth ||
1923                                 sar_in->iv_index != iv_index)) {
1924                 bool newer;
1925
1926                 if (iv_index > sar_in->iv_index)
1927                         newer = true;
1928                 else if (iv_index == sar_in->iv_index)
1929                         newer = seqAuth > sar_in->seqAuth;
1930                 else
1931                         newer = false;
1932
1933                 if (newer) {
1934                         /* Cancel Old, start New */
1935                         l_queue_remove(net->sar_in, sar_in);
1936                         mesh_sar_free(sar_in);
1937                         sar_in = NULL;
1938                 } else
1939                         /* Ignore Old */
1940                         return false;
1941         }
1942
1943         expected = 0xffffffff >> (31 - segN);
1944
1945         if (sar_in) {
1946                 l_debug("RXed (old: %04x %06x size:%d) %d of %d",
1947                                         seqZero, seq, size, segO, segN);
1948                 /* Sanity Check--> certain things must match */
1949                 if (SEG_MAX(true, sar_in->len) != segN ||
1950                                 sar_in->key_aid != key_aid)
1951                         return false;
1952
1953                 if (sar_in->flags == expected) {
1954                         /* Re-Send ACK for full msg */
1955                         send_net_ack(net, sar_in, expected);
1956                         return true;
1957                 }
1958         } else {
1959                 uint16_t len = MAX_SEG_TO_LEN(segN);
1960
1961                 l_debug("RXed (new: %04x %06x size: %d len: %d) %d of %d",
1962                                 seqZero, seq, size, len, segO, segN);
1963                 l_debug("Queue Size: %d", l_queue_length(net->sar_in));
1964                 sar_in = mesh_sar_new(len);
1965                 sar_in->seqAuth = seqAuth;
1966                 sar_in->iv_index = iv_index;
1967                 sar_in->src = dst;
1968                 sar_in->remote = src;
1969                 sar_in->seqZero = seqZero;
1970                 sar_in->key_aid = key_aid;
1971                 sar_in->len = len;
1972                 sar_in->last_seg = 0xff;
1973                 sar_in->net_idx = net_idx;
1974                 sar_in->msg_timeout = l_timeout_create(MSG_TO,
1975                                         inmsg_to, net, NULL);
1976
1977                 l_debug("First Seg %4.4x", sar_in->flags);
1978                 l_queue_push_head(net->sar_in, sar_in);
1979         }
1980
1981         seg_off = segO * MAX_SEG_LEN;
1982         memcpy(sar_in->buf + seg_off, data, size);
1983         this_seg_flag = 0x00000001 << segO;
1984
1985         /* Don't reset Seg TO or NAK if we already have this seg */
1986         if (this_seg_flag & sar_in->flags)
1987                 reset_seg_to = false;
1988
1989         sar_in->flags |= this_seg_flag;
1990         sar_in->ttl = ttl;
1991
1992         /* Msg length only definitive on last segment */
1993         if (segO == segN)
1994                 sar_in->len = segN * MAX_SEG_LEN + size;
1995
1996         if (sar_in->flags == expected) {
1997                 /* Got it all */
1998                 send_net_ack(net, sar_in, expected);
1999
2000                 msg_rxed(net, frnd, iv_index, ttl, seq, net_idx,
2001                                 sar_in->remote, dst, key_aid, true, szmic,
2002                                 sar_in->seqZero, sar_in->buf, sar_in->len);
2003
2004                 /* Kill Inter-Seg timeout */
2005                 l_timeout_remove(sar_in->seg_timeout);
2006                 sar_in->seg_timeout = NULL;
2007                 return true;
2008         }
2009
2010         if (reset_seg_to) {
2011                 /* Restart Inter-Seg Timeout */
2012                 l_timeout_remove(sar_in->seg_timeout);
2013
2014                 /* if this is the largest outstanding segment, send NAK now */
2015                 largest = (0xffffffff << segO) & expected;
2016                 if ((largest & sar_in->flags) == largest)
2017                         send_net_ack(net, sar_in, sar_in->flags);
2018
2019                 sar_in->seg_timeout = l_timeout_create(SEG_TO,
2020                                 inseg_to, net, NULL);
2021         } else
2022                 largest = 0;
2023
2024         l_debug("NAK: %d expected:%08x largest:%08x flags:%08x",
2025                         reset_seg_to, expected, largest, sar_in->flags);
2026         return false;
2027 }
2028
2029 static bool ctl_received(struct mesh_net *net, uint32_t net_key_id,
2030                                                 uint32_t iv_index, uint8_t ttl,
2031                                                 uint32_t seq,
2032                                                 uint16_t src, uint16_t dst,
2033                                                 uint8_t opcode, int8_t rssi,
2034                                                 const uint8_t *pkt, uint8_t len)
2035 {
2036         uint8_t msg[12];
2037         uint8_t rsp_ttl = DEFAULT_TTL;
2038         uint8_t n = 0;
2039         uint16_t net_idx;
2040
2041         if (ttl > 1) {
2042                 uint32_t hdr = opcode << OPCODE_HDR_SHIFT;
2043                 uint8_t frnd_ttl = ttl - 1;
2044
2045                 if (friend_packet_queue(net, iv_index, true, frnd_ttl, seq,
2046                                                 src, dst, hdr, pkt, len))
2047                         return true;
2048         }
2049
2050         /* Don't process other peoples Unicast destinations */
2051         if (dst < 0x8000 && (dst < net->src_addr || dst > net->last_addr))
2052                 return false;
2053
2054         switch (opcode) {
2055         default:
2056                 l_error("Unsupported Ctl Opcode: %2.2x", opcode);
2057                 break;
2058
2059         case NET_OP_FRND_POLL:
2060                 if (len != 1 || ttl)
2061                         return false;
2062
2063                 print_packet("Rx-NET_OP_FRND_POLL", pkt, len);
2064                 friend_poll(net, src, !!(pkt[0]), l_queue_find(net->friends,
2065                                         match_by_friend, L_UINT_TO_PTR(src)));
2066                 break;
2067
2068         case NET_OP_FRND_REQUEST:
2069                 if (!net->friend_enable)
2070                         return false;
2071
2072                 if (!IS_ALL_NODES(dst) && dst != FRIENDS_ADDRESS)
2073                         return false;
2074
2075                 if (len != 10 || ttl)
2076                         return false;
2077
2078                 print_packet("Rx-NET_OP_FRND_REQUEST", pkt, len);
2079                 net_idx = key_id_to_net_idx(net, net_key_id);
2080                 friend_request(net, net_idx, src, pkt[0], pkt[1],
2081                                 l_get_be32(pkt + 1) & 0xffffff,
2082                                 l_get_be16(pkt + 5), pkt[7],
2083                                 l_get_be16(pkt + 8), rssi);
2084                 break;
2085
2086         case NET_OP_FRND_CLEAR_CONFIRM:
2087                 if (len != 4)
2088                         return false;
2089
2090                 print_packet("Rx-NET_OP_FRND_CLEAR_CONFIRM", pkt, len);
2091                 friend_clear_confirm(net, src, l_get_be16(pkt),
2092                                                 l_get_be16(pkt + 2));
2093                 break;
2094
2095         case NET_OP_FRND_CLEAR:
2096                 if (len != 4 || dst != net->src_addr)
2097                         return false;
2098
2099                 print_packet("Rx-NET_OP_FRND_CLEAR", pkt, len);
2100                 friend_clear(net, src, l_get_be16(pkt), l_get_be16(pkt + 2),
2101                                 l_queue_find(net->friends, match_by_friend,
2102                                         L_UINT_TO_PTR(l_get_be16(pkt))));
2103                 l_debug("Remaining Friends: %d", l_queue_length(net->friends));
2104                 break;
2105
2106         case NET_OP_PROXY_SUB_ADD:
2107                 if (ttl)
2108                         return false;
2109
2110                 print_packet("Rx-NET_OP_PROXY_SUB_ADD", pkt, len);
2111                 friend_sub_add(net, l_queue_find(net->friends,
2112                                         match_by_friend, L_UINT_TO_PTR(src)),
2113                                 pkt, len);
2114                 break;
2115
2116         case NET_OP_PROXY_SUB_REMOVE:
2117                 if (ttl)
2118                         return false;
2119
2120                 print_packet("Rx-NET_OP_PROXY_SUB_REMOVE", pkt, len);
2121                 friend_sub_del(net, l_queue_find(net->friends, match_by_friend,
2122                                                 L_UINT_TO_PTR(src)), pkt, len);
2123                 break;
2124
2125         case NET_OP_PROXY_SUB_CONFIRM:
2126                 if (ttl)
2127                         return false;
2128
2129                 print_packet("Rx-NET_OP_PROXY_SUB_CONFIRM", pkt, len);
2130                 break;
2131
2132         case NET_OP_HEARTBEAT:
2133                 if (net->hb_sub.enabled && src == net->hb_sub.src) {
2134                         uint8_t hops = pkt[0] - ttl + 1;
2135
2136                         print_packet("Rx-NET_OP_HEARTBEAT", pkt, len);
2137
2138                         if (net->hb_sub.count != 0xffff)
2139                                 net->hb_sub.count++;
2140
2141                         if (net->hb_sub.min_hops > hops)
2142                                 net->hb_sub.min_hops = hops;
2143
2144                         if (net->hb_sub.max_hops < hops)
2145                                 net->hb_sub.max_hops = hops;
2146
2147                         l_debug("HB: cnt:%4.4x min:%2.2x max:%2.2x",
2148                                         net->hb_sub.count, net->hb_sub.min_hops,
2149                                                         net->hb_sub.max_hops);
2150                 }
2151                 break;
2152         }
2153
2154         if (n)
2155                 mesh_net_transport_send(net, 0, 0, mesh_net_get_iv_index(net),
2156                                         rsp_ttl, 0, dst & 0x8000 ? 0 : dst,
2157                                         src, msg, n);
2158
2159         return true;
2160 }
2161
2162 static bool find_fast_hash(const void *a, const void *b)
2163 {
2164         const uint64_t *entry = a;
2165         const uint64_t *test = b;
2166
2167         return *entry == *test;
2168 }
2169
2170 static bool check_fast_cache(uint64_t hash)
2171 {
2172         void *found = l_queue_find(fast_cache, find_fast_hash, &hash);
2173         uint64_t *new_hash;
2174
2175         if (found)
2176                 return false;
2177
2178         if (l_queue_length(fast_cache) >= FAST_CACHE_SIZE)
2179                 new_hash = l_queue_pop_head(fast_cache);
2180         else
2181                 new_hash = l_malloc(sizeof(hash));
2182
2183         *new_hash = hash;
2184         l_queue_push_tail(fast_cache, new_hash);
2185
2186         return true;
2187 }
2188
2189 static bool match_by_dst(const void *a, const void *b)
2190 {
2191         const struct mesh_destination *dest = a;
2192         uint16_t dst = L_PTR_TO_UINT(b);
2193
2194         return dest->dst == dst;
2195 }
2196
2197 static void send_relay_pkt(struct mesh_net *net, uint8_t *data, uint8_t size)
2198 {
2199         uint8_t packet[30];
2200         struct mesh_io *io = net->io;
2201         struct mesh_io_send_info info = {
2202                 .type = MESH_IO_TIMING_TYPE_GENERAL,
2203                 .u.gen.interval = net->relay.interval,
2204                 .u.gen.cnt = net->relay.count,
2205                 .u.gen.min_delay = DEFAULT_MIN_DELAY,
2206                 .u.gen.max_delay = DEFAULT_MAX_DELAY
2207         };
2208
2209         packet[0] = MESH_AD_TYPE_NETWORK;
2210         memcpy(packet + 1, data, size);
2211
2212         mesh_io_send(io, &info, packet, size + 1);
2213 }
2214
2215 static bool simple_match(const void *a, const void *b)
2216 {
2217         return a == b;
2218 }
2219
2220 static void send_msg_pkt_oneshot(void *user_data)
2221 {
2222         struct oneshot_tx *tx = user_data;
2223         struct mesh_net *net;
2224         struct mesh_io_send_info info;
2225         struct net_queue_data net_data = {
2226                 .info = NULL,
2227                 .data = tx->packet + 1,
2228                 .len = tx->size - 1,
2229                 .relay_advice = RELAY_NONE,
2230         };
2231
2232         /* Send to local nodes first */
2233         l_queue_foreach(nets, net_rx, &net_data);
2234
2235         /* Make sure specific network still valid */
2236         net = l_queue_find(nets, simple_match, tx->net);
2237
2238         if (!net || net_data.relay_advice == RELAY_DISALLOWED) {
2239                 l_free(tx);
2240                 return;
2241         }
2242
2243         tx->packet[0] = MESH_AD_TYPE_NETWORK;
2244         info.type = MESH_IO_TIMING_TYPE_GENERAL;
2245         info.u.gen.interval = tx->interval;
2246         info.u.gen.cnt = tx->cnt;
2247         info.u.gen.min_delay = DEFAULT_MIN_DELAY;
2248         /* No extra randomization when sending regular mesh messages */
2249         info.u.gen.max_delay = DEFAULT_MIN_DELAY;
2250
2251         mesh_io_send(net->io, &info, tx->packet, tx->size);
2252         l_free(tx);
2253 }
2254
2255 static void send_msg_pkt(struct mesh_net *net, uint8_t cnt, uint16_t interval,
2256                                                 uint8_t *packet, uint8_t size)
2257 {
2258         struct oneshot_tx *tx = l_new(struct oneshot_tx, 1);
2259
2260         tx->net = net;
2261         tx->interval = interval;
2262         tx->cnt = cnt;
2263         tx->size = size;
2264         memcpy(tx->packet, packet, size);
2265
2266         l_idle_oneshot(send_msg_pkt_oneshot, tx, NULL);
2267 }
2268
2269 static enum _relay_advice packet_received(void *user_data,
2270                                 uint32_t net_key_id, uint32_t iv_index,
2271                                 const void *data, uint8_t size, int8_t rssi)
2272 {
2273         struct mesh_net *net = user_data;
2274         const uint8_t *msg = data;
2275         uint8_t app_msg_len;
2276         uint8_t net_ttl, key_aid, net_segO, net_segN, net_opcode;
2277         uint32_t net_seq, cache_cookie;
2278         uint16_t net_src, net_dst, net_seqZero;
2279         uint16_t net_idx;
2280         uint8_t packet[31];
2281         bool net_ctl, net_segmented, net_szmic, net_relay;
2282
2283         memcpy(packet + 2, data, size);
2284
2285         net_idx = key_id_to_net_idx(net, net_key_id);
2286         if (net_idx == NET_IDX_INVALID)
2287                 return RELAY_NONE;
2288
2289         print_packet("RX: Network [clr] :", packet + 2, size);
2290
2291         if (!mesh_crypto_packet_parse(packet + 2, size, &net_ctl, &net_ttl,
2292                                         &net_seq, &net_src, &net_dst,
2293                                         &cache_cookie, &net_opcode,
2294                                         &net_segmented, &key_aid, &net_szmic,
2295                                         &net_relay, &net_seqZero, &net_segO,
2296                                         &net_segN, &msg, &app_msg_len)) {
2297                 l_error("Failed to parse packet content");
2298                 return RELAY_NONE;
2299         }
2300
2301         if (net_dst == 0) {
2302                 l_error("illegal parms: DST: %4.4x Ctl: %d TTL: %2.2x",
2303                                                 net_dst, net_ctl, net_ttl);
2304                 return RELAY_NONE;
2305         }
2306
2307         /* Ignore if we originally sent this */
2308         if (is_us(net, net_src, true))
2309                 return RELAY_NONE;
2310
2311         /*
2312          * As a Relay, suppress repeats of last N packets that pass through
2313          * The "cache_cookie" should be unique part of App message.
2314          */
2315         if (msg_in_cache(net, net_src, net_seq, cache_cookie))
2316                 return RELAY_NONE;
2317
2318         l_debug("RX: Network %04x -> %04x : TTL 0x%02x : IV : %8.8x SEQ 0x%06x",
2319                         net_src, net_dst, net_ttl, iv_index, net_seq);
2320
2321         if (is_us(net, net_dst, false) ||
2322                         (net_ctl && net_opcode == NET_OP_HEARTBEAT)) {
2323
2324                 l_debug("RX: App 0x%04x -> 0x%04x : TTL 0x%02x : SEQ 0x%06x",
2325                                         net_src, net_dst, net_ttl, net_seq);
2326
2327                 if (net_ctl) {
2328                         l_debug("CTL - %4.4x RX", net_seqZero);
2329                         if (net_opcode == NET_OP_SEG_ACKNOWLEDGE) {
2330                                 /* Illegal to send ACK to non-Unicast Addr */
2331                                 if (net_dst & 0x8000)
2332                                         return RELAY_NONE;
2333
2334                                 /* Pedantic check for correct size */
2335                                 if (app_msg_len != 7)
2336                                         return RELAY_NONE;
2337
2338                                 /* If this is an ACK to our friend queue-only */
2339                                 if (is_lpn_friend(net, net_dst))
2340                                         friend_ack_rxed(net, iv_index, net_seq,
2341                                                         net_src, net_dst, msg);
2342                                 else
2343                                         ack_received(net, false,
2344                                                         net_src, net_dst,
2345                                                         net_seqZero,
2346                                                         l_get_be32(msg + 3));
2347                         } else {
2348                                 ctl_received(net, net_key_id, iv_index, net_ttl,
2349                                                 net_seq, net_src, net_dst,
2350                                                 net_opcode, rssi, msg,
2351                                                                 app_msg_len);
2352                         }
2353                 } else if (net_segmented) {
2354                         /*
2355                          * If we accept SAR packets to non-Unicast, then
2356                          * Friend Sar at least needs to be Unicast Only
2357                          */
2358                         if (is_lpn_friend(net, net_dst) &&
2359                                                         !(net_dst & 0x8000)) {
2360                                 /*
2361                                  * Check TTL >= 2 before accepting segments
2362                                  * for Friends
2363                                  */
2364                                 if (net_ttl >= 2) {
2365                                         friend_seg_rxed(net, iv_index, net_ttl,
2366                                                 net_seq, net_src, net_dst,
2367                                                 l_get_be32(packet + 2 + 9),
2368                                                 msg, app_msg_len);
2369                                 }
2370                         } else {
2371                                 seg_rxed(net, NULL, iv_index, net_ttl,
2372                                                 net_seq, net_idx, net_src,
2373                                                 net_dst, key_aid, net_szmic,
2374                                                 net_seqZero, net_segO, net_segN,
2375                                                 msg, app_msg_len);
2376                         }
2377
2378                 } else {
2379                         msg_rxed(net, NULL, iv_index, net_ttl, net_seq, net_idx,
2380                                         net_src, net_dst, key_aid, false,
2381                                         false, net_seq & SEQ_ZERO_MASK, msg,
2382                                         app_msg_len);
2383                 }
2384
2385                 /* If this is one of our Unicast addresses, disallow relay */
2386                 if (IS_UNICAST(net_dst))
2387                         return RELAY_DISALLOWED;
2388         }
2389
2390         /* If relay not enable, or no more hops allowed */
2391         if (!net->relay.enable || net_ttl < 0x02)
2392                 return RELAY_NONE;
2393
2394         /* Group or Virtual destinations should *always* be relayed */
2395         if (IS_GROUP(net_dst) || IS_VIRTUAL(net_dst))
2396                 return RELAY_ALWAYS;
2397
2398         /* Unicast destinations for other nodes *may* be relayed */
2399         else if (IS_UNICAST(net_dst))
2400                 return RELAY_ALLOWED;
2401
2402         /* Otherwise, do not make a relay decision */
2403         else
2404                 return RELAY_NONE;
2405 }
2406
2407 static void net_rx(void *net_ptr, void *user_data)
2408 {
2409         struct net_queue_data *data = user_data;
2410         struct mesh_net *net = net_ptr;
2411         enum _relay_advice relay_advice;
2412         uint8_t *out;
2413         size_t out_size;
2414         uint32_t net_key_id;
2415         int8_t rssi = 0;
2416         bool ivi_net = !!(net->iv_index & 1);
2417         bool ivi_pkt = !!(data->data[0] & 0x80);
2418
2419         /* if IVI flag differs, use previous IV Index */
2420         uint32_t iv_index = net->iv_index - (ivi_pkt ^ ivi_net);
2421
2422         net_key_id = net_key_decrypt(iv_index, data->data, data->len,
2423                                                         &out, &out_size);
2424
2425         if (!net_key_id)
2426                 return;
2427
2428         if (!data->seen) {
2429                 data->seen = true;
2430                 print_packet("RX: Network [enc] :", data->data, data->len);
2431         }
2432
2433         if (data->info) {
2434                 net->instant = data->info->instant;
2435                 net->chan = data->info->chan;
2436                 rssi = data->info->rssi;
2437         }
2438
2439         relay_advice = packet_received(net, net_key_id, iv_index, out, out_size,
2440                                                                         rssi);
2441         if (relay_advice > data->relay_advice) {
2442                 data->iv_index = iv_index;
2443                 data->relay_advice = relay_advice;
2444                 data->net_key_id = net_key_id;
2445                 data->net = net;
2446                 data->out = out;
2447                 data->out_size = out_size;
2448         }
2449 }
2450
2451 static void net_msg_recv(void *user_data, struct mesh_io_recv_info *info,
2452                                         const uint8_t *data, uint16_t len)
2453 {
2454         uint64_t hash;
2455         bool isNew;
2456         struct net_queue_data net_data = {
2457                 .info = info,
2458                 .data = data + 1,
2459                 .len = len - 1,
2460                 .relay_advice = RELAY_NONE,
2461                 .seen = false,
2462         };
2463
2464         if (len < 9)
2465                 return;
2466
2467         hash = l_get_le64(data + 1);
2468
2469         /* Only process packet once per reception */
2470         isNew = check_fast_cache(hash);
2471         if (!isNew)
2472                 return;
2473
2474         l_queue_foreach(nets, net_rx, &net_data);
2475
2476         if (net_data.relay_advice == RELAY_ALWAYS ||
2477                         net_data.relay_advice == RELAY_ALLOWED) {
2478                 uint8_t ttl = net_data.out[1] & TTL_MASK;
2479
2480                 net_data.out[1] &=  ~TTL_MASK;
2481                 net_data.out[1] |= ttl - 1;
2482                 net_key_encrypt(net_data.net_key_id, net_data.iv_index,
2483                                         net_data.out, net_data.out_size);
2484                 send_relay_pkt(net_data.net, net_data.out, net_data.out_size);
2485         }
2486 }
2487
2488 static void iv_upd_to(struct l_timeout *upd_timeout, void *user_data)
2489 {
2490         struct mesh_net *net = user_data;
2491
2492         switch (net->iv_upd_state) {
2493         case IV_UPD_UPDATING:
2494                 if (l_queue_length(net->sar_out) ||
2495                                         l_queue_length(net->sar_queue)) {
2496                         l_debug("don't leave IV Update until sar_out empty");
2497                         l_timeout_modify(net->iv_update_timeout, 10);
2498                         break;
2499                 }
2500
2501                 l_debug("iv_upd_state = IV_UPD_NORMAL_HOLD");
2502                 net->iv_upd_state = IV_UPD_NORMAL_HOLD;
2503                 l_timeout_modify(net->iv_update_timeout, IV_IDX_UPD_MIN);
2504
2505                 if (net->iv_update)
2506                         mesh_net_set_seq_num(net, 0);
2507
2508                 net->iv_update = false;
2509                 mesh_config_write_iv_index(node_config_get(net->node),
2510                                                         net->iv_index, false);
2511                 l_queue_foreach(net->subnets, refresh_beacon, net);
2512                 queue_friend_update(net);
2513                 l_queue_clear(net->msg_cache, l_free);
2514                 break;
2515
2516         case IV_UPD_INIT:
2517         case IV_UPD_NORMAL_HOLD:
2518         case IV_UPD_NORMAL:
2519                 l_timeout_remove(upd_timeout);
2520                 net->iv_update_timeout = NULL;
2521                 l_debug("iv_upd_state = IV_UPD_NORMAL");
2522                 net->iv_upd_state = IV_UPD_NORMAL;
2523
2524                 if (net->iv_update)
2525                         mesh_net_set_seq_num(net, 0);
2526
2527                 net->iv_update = false;
2528
2529                 if (net->seq_num > IV_UPDATE_SEQ_TRIGGER)
2530                         mesh_net_iv_index_update(net);
2531                 break;
2532         }
2533 }
2534
2535
2536 static int key_refresh_phase_two(struct mesh_net *net, uint16_t idx)
2537 {
2538         struct mesh_subnet *subnet;
2539
2540         if (!net)
2541                 return MESH_STATUS_UNSPECIFIED_ERROR;
2542
2543         subnet = l_queue_find(net->subnets, match_key_index,
2544                                                         L_UINT_TO_PTR(idx));
2545
2546         if (!subnet || !subnet->net_key_upd)
2547                 return MESH_STATUS_INVALID_NETKEY;
2548
2549         l_debug("Key refresh procedure phase 2: start using new net TX keys");
2550
2551         if (subnet->kr_phase == KEY_REFRESH_PHASE_TWO)
2552                 return MESH_STATUS_SUCCESS;
2553
2554         subnet->key_refresh = 1;
2555         subnet->net_key_tx = subnet->net_key_upd;
2556         /*
2557          * TODO: Provisioner may need to stay in phase three until
2558          * it hears beacons from all the nodes
2559          */
2560         subnet->kr_phase = KEY_REFRESH_PHASE_TWO;
2561         refresh_beacon(subnet, net);
2562         queue_friend_update(net);
2563
2564         l_queue_foreach(net->friends, frnd_kr_phase2, net);
2565
2566         if (!mesh_config_net_key_set_phase(node_config_get(net->node), idx,
2567                                                         KEY_REFRESH_PHASE_TWO))
2568                 return MESH_STATUS_STORAGE_FAIL;
2569
2570         return MESH_STATUS_SUCCESS;
2571 }
2572
2573 static int key_refresh_finish(struct mesh_net *net, uint16_t idx)
2574 {
2575         struct mesh_subnet *subnet;
2576
2577         if (!net)
2578                 return MESH_STATUS_UNSPECIFIED_ERROR;
2579
2580         subnet = l_queue_find(net->subnets, match_key_index,
2581                                                         L_UINT_TO_PTR(idx));
2582         if (!subnet || !subnet->net_key_upd)
2583                 return MESH_STATUS_INVALID_NETKEY;
2584
2585         if (subnet->kr_phase == KEY_REFRESH_PHASE_NONE)
2586                 return MESH_STATUS_SUCCESS;
2587
2588         l_debug("Key refresh phase 3: use new keys only, discard old ones");
2589
2590         /* Switch to using new keys, discard old ones */
2591         net_key_unref(subnet->net_key_cur);
2592         subnet->net_key_tx = subnet->net_key_cur = subnet->net_key_upd;
2593         subnet->net_key_upd = 0;
2594         subnet->key_refresh = 0;
2595         subnet->kr_phase = KEY_REFRESH_PHASE_NONE;
2596         refresh_beacon(subnet, net);
2597         queue_friend_update(net);
2598
2599         l_queue_foreach(net->friends, frnd_kr_phase3, net);
2600
2601         appkey_finalize(net, idx);
2602
2603         if (!mesh_config_net_key_set_phase(node_config_get(net->node), idx,
2604                                                         KEY_REFRESH_PHASE_NONE))
2605                 return MESH_STATUS_STORAGE_FAIL;
2606
2607         return MESH_STATUS_SUCCESS;
2608 }
2609
2610 static bool update_kr_state(struct mesh_subnet *subnet, bool kr, uint32_t id)
2611 {
2612         /* Figure out the key refresh phase */
2613         if (kr) {
2614                 if (id == subnet->net_key_upd) {
2615                         l_debug("Beacon based KR phase 2 change");
2616                         return (key_refresh_phase_two(subnet->net, subnet->idx)
2617                                                         == MESH_STATUS_SUCCESS);
2618                 }
2619         } else {
2620                 if (id == subnet->net_key_upd) {
2621                         l_debug("Beacon based KR phase 3 change");
2622                         return (key_refresh_finish(subnet->net, subnet->idx)
2623                                                         == MESH_STATUS_SUCCESS);
2624                 }
2625         }
2626
2627         return false;
2628 }
2629
2630 static bool update_iv_ivu_state(struct mesh_net *net, uint32_t iv_index,
2631                                                                 bool ivu)
2632 {
2633         if ((iv_index - ivu) > (net->iv_index - net->iv_update)) {
2634                 /* Don't accept IV_Index changes when performing SAR Out */
2635                 if (l_queue_length(net->sar_out))
2636                         return false;
2637         }
2638
2639         /* If first beacon seen, accept without judgement */
2640         if (net->iv_upd_state == IV_UPD_INIT) {
2641                 if (ivu) {
2642                         /* Ignore beacons with IVU if IV already updated */
2643                         if (iv_index == net->iv_index && !net->iv_update)
2644                                 return false;
2645
2646                         /*
2647                          * Other devices will be accepting old or new iv_index,
2648                          * but we don't know how far through update they are.
2649                          * Starting permissive state will allow us maximum
2650                          * (96 hours) to resync
2651                          */
2652                         l_debug("iv_upd_state = IV_UPD_UPDATING");
2653                         net->iv_upd_state = IV_UPD_UPDATING;
2654                         net->iv_update_timeout = l_timeout_create(
2655                                         IV_IDX_UPD_MIN, iv_upd_to, net, NULL);
2656                 } else {
2657                         l_debug("iv_upd_state = IV_UPD_NORMAL");
2658                         net->iv_upd_state = IV_UPD_NORMAL;
2659                 }
2660         } else if (ivu) {
2661                 /* Ignore beacons with IVU if they come too soon */
2662                 if (!net->iv_update &&
2663                                 net->iv_upd_state == IV_UPD_NORMAL_HOLD) {
2664                         l_error("Update attempted too soon");
2665                         return false;
2666                 }
2667
2668                 /* Ignore beacons with IVU if IV already updated */
2669                 if (iv_index == net->iv_index)
2670                         return false;
2671
2672                 if (!net->iv_update) {
2673                         l_debug("iv_upd_state = IV_UPD_UPDATING");
2674                         net->iv_upd_state = IV_UPD_UPDATING;
2675                         net->iv_update_timeout = l_timeout_create(
2676                                         IV_IDX_UPD_MIN, iv_upd_to, net, NULL);
2677                 }
2678         } else if (net->iv_update) {
2679                 l_error("IVU clear attempted too soon");
2680                 return false;
2681         }
2682
2683         if ((iv_index - ivu) > (net->iv_index - net->iv_update))
2684                 mesh_net_set_seq_num(net, 0);
2685
2686         if (ivu != net->iv_update || iv_index != net->iv_index) {
2687                 struct mesh_config *cfg = node_config_get(net->node);
2688
2689                 mesh_config_write_iv_index(cfg, iv_index, ivu);
2690
2691                 /* Cleanup Replay Protection List NVM */
2692                 rpl_update(net->node, iv_index);
2693         }
2694
2695         node_property_changed(net->node, "IVIndex");
2696
2697         net->iv_index = iv_index;
2698         net->iv_update = ivu;
2699         return true;
2700 }
2701
2702 static void process_beacon(void *net_ptr, void *user_data)
2703 {
2704         bool updated = false;
2705         struct mesh_net *net = net_ptr;
2706         struct net_beacon_data *beacon_data = user_data;
2707         uint32_t ivi;
2708         bool ivu, kr, local_kr;
2709         struct mesh_subnet *subnet, *primary_subnet;
2710
2711         ivi = beacon_data->ivi;
2712
2713         /* Ignore out-of-range IV_Index for this network */
2714         if ((net->iv_index + IV_IDX_DIFF_RANGE < ivi) || (ivi < net->iv_index))
2715                 return;
2716
2717         /* Ignore beacons not in this universe */
2718         subnet = l_queue_find(net->subnets, match_key_id,
2719                                         L_UINT_TO_PTR(beacon_data->net_key_id));
2720
2721         if (!subnet)
2722                 return;
2723
2724         /*
2725          * @MshPRFv1.0.1 section 3.10.5: IV Update procedure
2726          * If this node is a member of a primary subnet and receives a Secure
2727          * Network beacon on a secondary subnet with an IV Index greater than
2728          * the last known IV Index of the primary subnet, the Secure Network
2729          * beacon shall be ignored.
2730          */
2731         primary_subnet = get_primary_subnet(net);
2732         if (primary_subnet && subnet != primary_subnet && ivi > net->iv_index)
2733                 return;
2734
2735         /* Get IVU and KR boolean bits from beacon */
2736         ivu = beacon_data->ivu;
2737         kr = beacon_data->kr;
2738         local_kr = !!(subnet->kr_phase == KEY_REFRESH_PHASE_TWO);
2739
2740         /* We have officially *seen* this beacon now */
2741         beacon_data->processed = true;
2742
2743         /*
2744          * Ignore the beacon if it doesn't change anything, unless we're
2745          * doing IV Recovery
2746          */
2747         if (net->iv_upd_state == IV_UPD_INIT || ivi != net->iv_index ||
2748                                                         ivu != net->iv_update)
2749                 updated |= update_iv_ivu_state(net, ivi, ivu);
2750
2751         if (kr != local_kr)
2752                 updated |= update_kr_state(subnet, kr, beacon_data->net_key_id);
2753
2754         if (updated)
2755                 net_key_beacon_refresh(subnet->net_key_tx, net->iv_index,
2756                                 !!(subnet->kr_phase == KEY_REFRESH_PHASE_TWO),
2757                                                                 net->iv_update);
2758 }
2759
2760 static void beacon_recv(void *user_data, struct mesh_io_recv_info *info,
2761                                         const uint8_t *data, uint16_t len)
2762 {
2763         struct net_beacon_data beacon_data = {
2764                 .processed = false,
2765         };
2766
2767         if (len != 23 || data[1] != 0x01)
2768                 return;
2769
2770         /* Ignore Network IDs unknown to this daemon */
2771         beacon_data.net_key_id = net_key_network_id(data + 3);
2772         if (!beacon_data.net_key_id)
2773                 return;
2774
2775         /* Get data bits from beacon */
2776         beacon_data.ivu = !!(data[2] & 0x02);
2777         beacon_data.kr = !!(data[2] & 0x01);
2778         beacon_data.ivi = l_get_be32(data + 11);
2779
2780         /* Validate beacon before accepting */
2781         if (!net_key_snb_check(beacon_data.net_key_id, beacon_data.ivi,
2782                                         beacon_data.kr, beacon_data.ivu,
2783                                         l_get_be64(data + 15))) {
2784                 l_error("mesh_crypto_beacon verify failed");
2785                 return;
2786         }
2787
2788         l_queue_foreach(nets, process_beacon, &beacon_data);
2789
2790         if (beacon_data.processed)
2791                 net_key_beacon_seen(beacon_data.net_key_id);
2792 }
2793
2794 void net_local_beacon(uint32_t net_key_id, uint8_t *beacon)
2795 {
2796         struct net_beacon_data beacon_data = {
2797                 .net_key_id = net_key_id,
2798                 .ivu = !!(beacon[2] & 0x02),
2799                 .kr = !!(beacon[2] & 0x01),
2800                 .ivi = l_get_be32(beacon + 11),
2801         };
2802
2803         /* Deliver locally generated beacons to all nodes */
2804         l_queue_foreach(nets, process_beacon, &beacon_data);
2805 }
2806
2807 bool mesh_net_set_beacon_mode(struct mesh_net *net, bool enable)
2808 {
2809         if (!net)
2810                 return false;
2811
2812         if (net->beacon_enable == enable)
2813                 return true;
2814
2815         net->beacon_enable = enable;
2816
2817         if (enable)
2818                 l_queue_foreach(net->subnets, refresh_beacon, net);
2819
2820         l_queue_foreach(net->subnets, enable_beacon, net);
2821         queue_friend_update(net);
2822
2823         return true;
2824 }
2825
2826 /* This function is called when network keys are restored from storage. */
2827 bool mesh_net_set_key(struct mesh_net *net, uint16_t idx, const uint8_t *key,
2828                                         const uint8_t *new_key, uint8_t phase)
2829 {
2830         struct mesh_subnet *subnet;
2831
2832         /* Current key must be always present */
2833         if (!key)
2834                 return false;
2835
2836         /* If key refresh is in progress, a new key must be present */
2837         if (phase != KEY_REFRESH_PHASE_NONE && !new_key)
2838                 return false;
2839
2840         /* Check if the subnet with the specified index already exists */
2841         subnet = l_queue_find(net->subnets, match_key_index,
2842                                                         L_UINT_TO_PTR(idx));
2843         if (subnet)
2844                 return false;
2845
2846         subnet = add_key(net, idx, key);
2847         if (!subnet)
2848                 return false;
2849
2850         if (new_key && phase)
2851                 subnet->net_key_upd = net_key_add(new_key);
2852
2853         /* Preserve key refresh state to generate secure beacon flags*/
2854         if (phase == KEY_REFRESH_PHASE_TWO) {
2855                 subnet->key_refresh = 1;
2856                 subnet->net_key_tx = subnet->net_key_upd;
2857
2858                 if (net->beacon_enable) {
2859                         /* Switch beaconing key */
2860                         net_key_beacon_disable(subnet->net_key_cur);
2861                         net_key_beacon_enable(subnet->net_key_upd);
2862                 }
2863         }
2864
2865         subnet->kr_phase = phase;
2866
2867         net_key_beacon_refresh(subnet->net_key_tx, net->iv_index,
2868                 !!(subnet->kr_phase == KEY_REFRESH_PHASE_TWO), net->iv_update);
2869
2870
2871         return true;
2872 }
2873
2874 bool mesh_net_attach(struct mesh_net *net, struct mesh_io *io)
2875 {
2876         bool first;
2877
2878         if (!net)
2879                 return false;
2880
2881         first = l_queue_isempty(nets);
2882         if (first) {
2883                 uint8_t snb[] = {MESH_AD_TYPE_BEACON, 0x01};
2884                 uint8_t pkt[] = {MESH_AD_TYPE_NETWORK};
2885
2886                 if (!nets)
2887                         nets = l_queue_new();
2888
2889                 if (!fast_cache)
2890                         fast_cache = l_queue_new();
2891
2892                 mesh_io_register_recv_cb(io, snb, sizeof(snb),
2893                                                         beacon_recv, NULL);
2894                 mesh_io_register_recv_cb(io, pkt, sizeof(pkt),
2895                                                         net_msg_recv, NULL);
2896         }
2897
2898         if (l_queue_find(nets, simple_match, net))
2899                 return false;
2900
2901         l_queue_push_head(nets, net);
2902
2903         net->io = io;
2904
2905         return true;
2906 }
2907
2908 struct mesh_io *mesh_net_detach(struct mesh_net *net)
2909 {
2910         uint8_t snb[] = {MESH_AD_TYPE_BEACON, 0x01};
2911         uint8_t pkt[] = {MESH_AD_TYPE_NETWORK};
2912         struct mesh_io *io;
2913         uint8_t type = 0;
2914
2915         if (!net || !net->io)
2916                 return NULL;
2917
2918         io = net->io;
2919
2920         mesh_io_send_cancel(net->io, &type, 1);
2921
2922         /* Only deregister io if this is the last network detached.*/
2923         if (l_queue_length(nets) < 2) {
2924                 mesh_io_deregister_recv_cb(io, snb, sizeof(snb));
2925                 mesh_io_deregister_recv_cb(io, pkt, sizeof(pkt));
2926         }
2927
2928         net->io = NULL;
2929         l_queue_remove(nets, net);
2930
2931         return io;
2932 }
2933
2934 bool mesh_net_iv_index_update(struct mesh_net *net)
2935 {
2936         if (net->iv_upd_state != IV_UPD_NORMAL)
2937                 return false;
2938
2939         l_debug("iv_upd_state = IV_UPD_UPDATING");
2940         l_queue_clear(net->msg_cache, l_free);
2941
2942         if (!mesh_config_write_iv_index(node_config_get(net->node),
2943                                                 net->iv_index + 1, true))
2944                 return false;
2945
2946         net->iv_upd_state = IV_UPD_UPDATING;
2947         net->iv_index++;
2948         net->iv_update = true;
2949         l_queue_foreach(net->subnets, refresh_beacon, net);
2950         queue_friend_update(net);
2951         net->iv_update_timeout = l_timeout_create(
2952                         IV_IDX_UPD_MIN,
2953                         iv_upd_to, net, NULL);
2954
2955         return true;
2956 }
2957
2958
2959 bool mesh_net_dst_reg(struct mesh_net *net, uint16_t dst)
2960 {
2961         struct mesh_destination *dest = l_queue_find(net->destinations,
2962                                         match_by_dst, L_UINT_TO_PTR(dst));
2963
2964         if (IS_UNASSIGNED(dst) || IS_ALL_NODES(dst))
2965                 return false;
2966
2967         if (!dest) {
2968                 dest = l_new(struct mesh_destination, 1);
2969
2970                 if (dst < 0x8000)
2971                         l_queue_push_head(net->destinations, dest);
2972                 else
2973                         l_queue_push_tail(net->destinations, dest);
2974         }
2975
2976         dest->dst = dst;
2977         dest->ref_cnt++;
2978
2979         return true;
2980 }
2981
2982 bool mesh_net_dst_unreg(struct mesh_net *net, uint16_t dst)
2983 {
2984         struct mesh_destination *dest = l_queue_find(net->destinations,
2985                                         match_by_dst, L_UINT_TO_PTR(dst));
2986
2987         if (!dest)
2988                 return false;
2989
2990         if (dest->ref_cnt)
2991                 dest->ref_cnt--;
2992
2993         if (dest->ref_cnt)
2994                 return true;
2995
2996         l_queue_remove(net->destinations, dest);
2997
2998         l_free(dest);
2999         return true;
3000 }
3001
3002 static bool send_seg(struct mesh_net *net, uint8_t cnt, uint16_t interval,
3003                                         struct mesh_sar *msg, uint8_t segO)
3004 {
3005         struct mesh_subnet *subnet;
3006         uint8_t seg_len;
3007         uint8_t gatt_data[30];
3008         uint8_t *packet = gatt_data;
3009         uint8_t packet_len;
3010         uint8_t segN = SEG_MAX(msg->segmented, msg->len);
3011         uint16_t seg_off = SEG_OFF(segO);
3012         uint32_t seq_num;
3013
3014         if (msg->segmented) {
3015                 /* Send each segment on unique seq_num */
3016                 seq_num = mesh_net_next_seq_num(net);
3017
3018                 if (msg->len - seg_off > SEG_OFF(1))
3019                         seg_len = SEG_OFF(1);
3020                 else
3021                         seg_len = msg->len - seg_off;
3022         } else {
3023                 /* Send on same seq_num used for Access Layer */
3024                 seq_num = msg->seqAuth;
3025                 seg_len = msg->len;
3026         }
3027
3028         /* Start IV Update procedure when we hit our trigger point */
3029         if (!msg->frnd && net->seq_num > IV_UPDATE_SEQ_TRIGGER)
3030                 mesh_net_iv_index_update(net);
3031
3032         l_debug("segN %d segment %d seg_off %d", segN, segO, seg_off);
3033
3034         /* TODO: Are we RXing on an LPN's behalf? Then set RLY bit */
3035         if (!mesh_crypto_packet_build(false, msg->ttl, seq_num, msg->src,
3036                                         msg->remote, 0, msg->segmented,
3037                                         msg->key_aid, msg->szmic, false,
3038                                         msg->seqZero, segO, segN,
3039                                         msg->buf + seg_off, seg_len,
3040                                         packet + 1, &packet_len)) {
3041                 l_error("Failed to build packet");
3042                 return false;
3043         }
3044
3045         print_packet("Clr-Net Tx", packet + 1, packet_len);
3046
3047         subnet = l_queue_find(net->subnets, match_key_index,
3048                                                 L_UINT_TO_PTR(msg->net_idx));
3049         if (!subnet)
3050                 return false;
3051
3052         if (!net_key_encrypt(subnet->net_key_tx, msg->iv_index, packet + 1,
3053                                                                 packet_len)) {
3054                 l_error("Failed to encode packet");
3055                 return false;
3056         }
3057
3058         send_msg_pkt(net, cnt, interval, packet, packet_len + 1);
3059
3060         msg->last_seg = segO;
3061
3062         return true;
3063 }
3064
3065 void mesh_net_send_seg(struct mesh_net *net, uint32_t net_key_id,
3066                         uint32_t iv_index, uint8_t ttl, uint32_t seq,
3067                         uint16_t src, uint16_t dst, uint32_t hdr,
3068                         const void *seg, uint16_t seg_len)
3069 {
3070         uint8_t packet[30];
3071         uint8_t packet_len;
3072         bool segmented = !!((hdr >> SEG_HDR_SHIFT) & true);
3073         uint8_t key_aid = (hdr >> KEY_HDR_SHIFT) & KEY_ID_MASK;
3074         bool szmic = !!((hdr >> SZMIC_HDR_SHIFT) & true);
3075         uint16_t seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) & SEQ_ZERO_MASK;
3076         uint8_t segO = (hdr >> SEGO_HDR_SHIFT) & SEG_MASK;
3077         uint8_t segN = (hdr >> SEGN_HDR_SHIFT) & SEG_MASK;
3078
3079         /*
3080          * MshPRFv1.0.1 section 3.4.5.2, Interface output filter:
3081          * If TTL is set to 1, message shall be dropped.
3082          */
3083         if (ttl == 1)
3084                 return;
3085
3086         /* TODO: Only used for current POLLed segments to LPNs */
3087
3088         l_debug("SEQ: %6.6x", seq + segO);
3089         l_debug("SEQ0: %6.6x", seq);
3090         l_debug("segO: %d", segO);
3091
3092         if (!mesh_crypto_packet_build(false, ttl, seq, src, dst, 0,
3093                                         segmented, key_aid, szmic, false,
3094                                         seqZero, segO, segN, seg, seg_len,
3095                                         packet + 1, &packet_len)) {
3096                 l_error("Failed to build packet");
3097                 return;
3098         }
3099
3100         if (!net_key_encrypt(net_key_id, iv_index, packet + 1, packet_len)) {
3101                 l_error("Failed to encode packet");
3102                 return;
3103         }
3104
3105         send_msg_pkt(net, net->tx_cnt, net->tx_interval, packet,
3106                                                                 packet_len + 1);
3107
3108         l_debug("TX: Friend Seg-%d %04x -> %04x : len %u) : TTL %d : SEQ %06x",
3109                                         segO, src, dst, packet_len, ttl, seq);
3110
3111         print_packet("TX: Friend", packet + 1, packet_len);
3112 }
3113
3114 bool mesh_net_app_send(struct mesh_net *net, bool frnd_cred, uint16_t src,
3115                                 uint16_t dst, uint8_t key_aid, uint16_t net_idx,
3116                                 uint8_t ttl, uint8_t cnt, uint16_t interval,
3117                                 uint32_t seq, uint32_t iv_index, bool segmented,
3118                                 bool szmic, const void *msg, uint16_t msg_len)
3119 {
3120         struct mesh_sar *payload = NULL;
3121         uint8_t seg, seg_max;
3122         bool result;
3123
3124         if (!net || msg_len > 384)
3125                 return false;
3126
3127         if (!src)
3128                 src = net->src_addr;
3129
3130         if (!src || !dst)
3131                 return false;
3132
3133         if (ttl == DEFAULT_TTL)
3134                 ttl = net->default_ttl;
3135
3136         /* Long and sizmic messages *require* segmenting */
3137         segmented |= szmic;
3138         seg_max = SEG_MAX(segmented, msg_len);
3139         segmented |= !!(seg_max);
3140
3141         /* First enqueue to any Friends and internal models */
3142         result = msg_rxed(net, false, iv_index, ttl, seq, net_idx, src, dst,
3143                                 key_aid, segmented, szmic, seq & SEQ_ZERO_MASK,
3144                                 msg, msg_len);
3145
3146         /*
3147          * If addressed to a unicast address and successfully enqueued,
3148          * or delivered to one of our Unicast addresses we are done
3149          */
3150         if ((result && IS_UNICAST(dst)) || src == dst ||
3151                         (dst >= net->src_addr && dst <= net->last_addr))
3152                 return true;
3153
3154         /*
3155          * MshPRFv1.0.1 section 3.4.5.2, Interface output filter:
3156          * If TTL is set to 1, message shall be dropped.
3157          */
3158         if (ttl == 1)
3159                 return true;
3160
3161         /* Setup OTA Network send */
3162         payload = mesh_sar_new(msg_len);
3163         memcpy(payload->buf, msg, msg_len);
3164         payload->len = msg_len;
3165         payload->src = src;
3166         payload->remote = dst;
3167         payload->ttl = ttl;
3168         payload->szmic = szmic;
3169         payload->frnd_cred = frnd_cred;
3170         payload->key_aid = key_aid;
3171         payload->net_idx = net_idx;
3172         payload->iv_index = mesh_net_get_iv_index(net);
3173         payload->seqAuth = seq;
3174         payload->segmented = segmented;
3175
3176         if (segmented) {
3177                 payload->flags = 0xffffffff >> (31 - seg_max);
3178                 payload->seqZero = seq & SEQ_ZERO_MASK;
3179                 payload->id = ++net->sar_id_next;
3180
3181                 /* Single thread SAR messages to same Unicast DST */
3182                 if (l_queue_find(net->sar_out, match_sar_remote,
3183                                                         L_UINT_TO_PTR(dst))) {
3184                         /* Delay sending Outbound SAR unless prior
3185                          * SAR to same DST has completed */
3186
3187                         l_debug("OB-Queued SeqZero: %4.4x", payload->seqZero);
3188                         l_queue_push_tail(net->sar_queue, payload);
3189                         return true;
3190                 }
3191         }
3192
3193         result = true;
3194
3195         if (!IS_UNICAST(dst) && segmented) {
3196                 int i;
3197
3198                 for (i = 0; i < 4; i++) {
3199                         for (seg = 0; seg <= seg_max && result; seg++)
3200                                 result = send_seg(net, cnt, interval, payload,
3201                                                                         seg);
3202                 }
3203         } else {
3204                 for (seg = 0; seg <= seg_max && result; seg++)
3205                         result = send_seg(net, cnt, interval, payload, seg);
3206         }
3207
3208         /* Reliable: Cache; Unreliable: Flush*/
3209         if (result && segmented && IS_UNICAST(dst)) {
3210                 l_queue_push_head(net->sar_out, payload);
3211                 payload->seg_timeout =
3212                         l_timeout_create(SEG_TO, outseg_to, net, NULL);
3213                 payload->msg_timeout =
3214                         l_timeout_create(MSG_TO, outmsg_to, net, NULL);
3215                 payload->id = ++net->sar_id_next;
3216         } else
3217                 mesh_sar_free(payload);
3218
3219         return result;
3220 }
3221
3222 void mesh_net_ack_send(struct mesh_net *net, uint32_t net_key_id,
3223                         uint32_t iv_index, uint8_t ttl, uint32_t seq,
3224                         uint16_t src, uint16_t dst, bool rly, uint16_t seqZero,
3225                         uint32_t ack_flags)
3226 {
3227         uint32_t hdr;
3228         uint8_t data[7];
3229         uint8_t pkt_len;
3230         uint8_t pkt[30];
3231
3232         /*
3233          * MshPRFv1.0.1 section 3.4.5.2, Interface output filter:
3234          * If TTL is set to 1, message shall be dropped.
3235          */
3236         if (ttl == 1)
3237                 return;
3238
3239         hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
3240         hdr |= rly << RELAY_HDR_SHIFT;
3241         hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT;
3242         l_put_be32(hdr, data);
3243         l_put_be32(ack_flags, data + 3);
3244
3245         /* Not Segmented, no Key ID associated, no segO or segN */
3246         if (!mesh_crypto_packet_build(true, ttl, seq, src, dst,
3247                                         NET_OP_SEG_ACKNOWLEDGE, false, 0, false,
3248                                         rly, seqZero, 0, 0, data + 1, 6,
3249                                         pkt + 1, &pkt_len))
3250                 return;
3251
3252         if (!net_key_id) {
3253                 struct mesh_subnet *subnet = get_primary_subnet(net);
3254
3255                 net_key_id = subnet->net_key_tx;
3256         }
3257
3258         if (!net_key_encrypt(net_key_id, iv_index, pkt + 1, pkt_len)) {
3259                 l_error("Failed to encode packet");
3260                 return;
3261         }
3262
3263         send_msg_pkt(net, net->tx_cnt, net->tx_interval, pkt, pkt_len + 1);
3264
3265         l_debug("TX: Friend ACK %04x -> %04x : len %u : TTL %d : SEQ %06x",
3266                                         src, dst, pkt_len, ttl, seq);
3267         print_packet("TX: Friend ACK", pkt + 1, pkt_len);
3268 }
3269
3270 void mesh_net_transport_send(struct mesh_net *net, uint32_t net_key_id,
3271                                 uint16_t net_idx, uint32_t iv_index,
3272                                 uint8_t ttl, uint32_t seq, uint16_t src,
3273                                 uint16_t dst, const uint8_t *msg,
3274                                 uint16_t msg_len)
3275 {
3276         uint32_t use_seq = seq;
3277         uint8_t pkt_len;
3278         uint8_t pkt[30];
3279         bool result = false;
3280
3281         if (!net->src_addr)
3282                 return;
3283
3284         if (!src)
3285                 src = net->src_addr;
3286
3287         if (src == dst)
3288                 return;
3289
3290         if (ttl == DEFAULT_TTL)
3291                 ttl = net->default_ttl;
3292
3293         /* Range check the Opcode and msg length*/
3294         if (*msg & 0xc0 || (9 + msg_len + 8 > 29))
3295                 return;
3296
3297         /*
3298          * MshPRFv1.0.1 section 3.4.5.2, Interface output filter:
3299          * If TTL is set to 1, message shall be dropped.
3300          */
3301         if (ttl == 1)
3302                 return;
3303
3304         /* Enqueue for Friend if forwardable and from us */
3305         if (!net_key_id && src >= net->src_addr && src <= net->last_addr) {
3306                 uint32_t hdr = msg[0] << OPCODE_HDR_SHIFT;
3307                 uint8_t frnd_ttl = ttl;
3308
3309                 if (friend_packet_queue(net, iv_index, true, frnd_ttl,
3310                                         mesh_net_next_seq_num(net), src, dst,
3311                                         hdr, msg + 1, msg_len - 1))
3312                         return;
3313         }
3314
3315         /* Deliver to Local entities if applicable */
3316         if (!(dst & 0x8000) && src >= net->src_addr && src <= net->last_addr)
3317                 result = ctl_received(net, net_key_id, iv_index, ttl,
3318                                         mesh_net_next_seq_num(net), src, dst,
3319                                         msg[0], 0, msg + 1, msg_len - 1);
3320
3321         if (!net_key_id) {
3322                 struct mesh_subnet *subnet = l_queue_find(net->subnets,
3323                                 match_key_index, L_UINT_TO_PTR(net_idx));
3324                 if (!subnet)
3325                         return;
3326
3327                 net_key_id = subnet->net_key_tx;
3328                 use_seq = mesh_net_next_seq_num(net);
3329
3330                 if (result || (dst >= net->src_addr && dst <= net->last_addr))
3331                         return;
3332         }
3333
3334         if (!mesh_crypto_packet_build(true, ttl, use_seq, src, dst, msg[0],
3335                                 false, 0, false, false, 0, 0, 0, msg + 1,
3336                                 msg_len - 1, pkt + 1, &pkt_len))
3337                 return;
3338
3339         if (!net_key_encrypt(net_key_id, iv_index, pkt + 1, pkt_len)) {
3340                 l_error("Failed to encode packet");
3341                 return;
3342         }
3343
3344         if (!(IS_UNASSIGNED(dst)))
3345                 send_msg_pkt(net, net->tx_cnt, net->tx_interval, pkt,
3346                                                                 pkt_len + 1);
3347 }
3348
3349 int mesh_net_key_refresh_phase_set(struct mesh_net *net, uint16_t idx,
3350                                                         uint8_t transition)
3351 {
3352         switch (transition) {
3353         case KEY_REFRESH_TRANS_TWO:
3354                 return key_refresh_phase_two(net, idx);
3355
3356         case KEY_REFRESH_TRANS_THREE:
3357                 return key_refresh_finish(net, idx);
3358
3359         default:
3360                 return MESH_STATUS_UNSPECIFIED_ERROR;
3361         }
3362 }
3363
3364 int mesh_net_key_refresh_phase_get(struct mesh_net *net, uint16_t idx,
3365                                                                 uint8_t *phase)
3366 {
3367         struct mesh_subnet *subnet;
3368
3369         if (!net)
3370                 return MESH_STATUS_UNSPECIFIED_ERROR;
3371
3372         subnet = l_queue_find(net->subnets, match_key_index,
3373                                                         L_UINT_TO_PTR(idx));
3374         if (!subnet)
3375                 return MESH_STATUS_INVALID_NETKEY;
3376
3377         *phase = subnet->kr_phase;
3378         return MESH_STATUS_SUCCESS;
3379 }
3380
3381 /*
3382  * This function is called when Configuration Server Model receives
3383  * a NETKEY_UPDATE command
3384  */
3385 int mesh_net_update_key(struct mesh_net *net, uint16_t idx,
3386                                                         const uint8_t *value)
3387 {
3388         struct mesh_subnet *subnet;
3389
3390         if (!net)
3391                 return MESH_STATUS_UNSPECIFIED_ERROR;
3392
3393         subnet = l_queue_find(net->subnets, match_key_index,
3394                                                         L_UINT_TO_PTR(idx));
3395
3396         if (!subnet)
3397                 return MESH_STATUS_INVALID_NETKEY;
3398
3399         /* Check if the key has been already successfully updated */
3400         if (subnet->kr_phase == KEY_REFRESH_PHASE_ONE &&
3401                                 net_key_confirm(subnet->net_key_upd, value))
3402                 return MESH_STATUS_SUCCESS;
3403
3404         if (subnet->kr_phase != KEY_REFRESH_PHASE_NONE)
3405                 return MESH_STATUS_CANNOT_UPDATE;
3406
3407         if (subnet->net_key_upd) {
3408                 net_key_unref(subnet->net_key_upd);
3409                 l_debug("Warning: overwriting new keys");
3410         }
3411
3412         /* Preserve starting data */
3413         subnet->net_key_upd = net_key_add(value);
3414
3415         if (!subnet->net_key_upd) {
3416                 l_error("Failed to start key refresh phase one");
3417                 return MESH_STATUS_CANNOT_UPDATE;
3418         }
3419
3420         /* If we are a Friend-Node, generate all our new keys */
3421         l_queue_foreach(net->friends, frnd_kr_phase1,
3422                                         L_UINT_TO_PTR(subnet->net_key_upd));
3423
3424         l_debug("key refresh phase 1: Key ID %d", subnet->net_key_upd);
3425
3426         if (!mesh_config_net_key_update(node_config_get(net->node), idx, value))
3427                 return MESH_STATUS_STORAGE_FAIL;
3428
3429         subnet->kr_phase = KEY_REFRESH_PHASE_ONE;
3430
3431         return MESH_STATUS_SUCCESS;
3432 }
3433
3434 struct mesh_net_heartbeat_sub *mesh_net_get_heartbeat_sub(struct mesh_net *net)
3435 {
3436         return &net->hb_sub;
3437 }
3438
3439 struct mesh_net_heartbeat_pub *mesh_net_get_heartbeat_pub(struct mesh_net *net)
3440 {
3441         return &net->hb_pub;
3442 }
3443
3444 void mesh_net_set_iv_index(struct mesh_net *net, uint32_t index, bool update)
3445 {
3446         net->iv_index = index;
3447         net->iv_update = update;
3448 }
3449
3450 uint16_t mesh_net_get_primary_idx(struct mesh_net *net)
3451 {
3452         struct mesh_subnet *subnet;
3453
3454         if (!net)
3455                 return NET_IDX_INVALID;
3456
3457         subnet = get_primary_subnet(net);
3458         if (!subnet)
3459                 return NET_IDX_INVALID;
3460
3461         return subnet->idx;
3462 }
3463
3464 uint32_t mesh_net_friend_timeout(struct mesh_net *net, uint16_t addr)
3465 {
3466         struct mesh_friend *frnd = l_queue_find(net->friends, match_by_friend,
3467                                                         L_UINT_TO_PTR(addr));
3468
3469         if (!frnd)
3470                 return 0;
3471         else
3472                 return frnd->poll_timeout;
3473 }
3474
3475 struct l_queue *mesh_net_get_friends(struct mesh_net *net)
3476 {
3477         if (net)
3478                 return net->friends;
3479
3480         return NULL;
3481 }
3482
3483 struct l_queue *mesh_net_get_negotiations(struct mesh_net *net)
3484 {
3485         if (net)
3486                 return net->negotiations;
3487
3488         return NULL;
3489 }
3490
3491 struct mesh_node *mesh_net_node_get(struct mesh_net *net)
3492 {
3493         return  net->node;
3494 }
3495
3496 struct l_queue *mesh_net_get_app_keys(struct mesh_net *net)
3497 {
3498         if (!net)
3499                 return NULL;
3500
3501         if (!net->app_keys)
3502                 net->app_keys = l_queue_new();
3503
3504         return net->app_keys;
3505 }
3506
3507 bool mesh_net_have_key(struct mesh_net *net, uint16_t idx)
3508 {
3509         if (!net)
3510                 return false;
3511
3512         return (l_queue_find(net->subnets, match_key_index,
3513                                                 L_UINT_TO_PTR(idx)) != NULL);
3514 }
3515
3516 bool mesh_net_is_local_address(struct mesh_net *net, uint16_t src,
3517                                                                 uint16_t count)
3518 {
3519         const uint16_t last = src + count - 1;
3520         if (!net)
3521                 return false;
3522
3523         return (src >= net->src_addr && src <= net->last_addr) &&
3524                         (last >= net->src_addr && last <= net->last_addr);
3525 }
3526
3527 void mesh_net_transmit_params_set(struct mesh_net *net, uint8_t count,
3528                                                         uint16_t interval)
3529 {
3530         if (!net)
3531                 return;
3532
3533         net->tx_interval = interval;
3534         net->tx_cnt = count;
3535 }
3536
3537 void mesh_net_transmit_params_get(struct mesh_net *net, uint8_t *count,
3538                                                         uint16_t *interval)
3539 {
3540         if (!net)
3541                 return;
3542
3543         *interval = net->tx_interval;
3544         *count = net->tx_cnt;
3545 }
3546
3547 struct mesh_io *mesh_net_get_io(struct mesh_net *net)
3548 {
3549         if (!net)
3550                 return NULL;
3551
3552         return net->io;
3553 }
3554
3555 struct mesh_prov *mesh_net_get_prov(struct mesh_net *net)
3556 {
3557         if (!net)
3558                 return NULL;
3559
3560         return net->prov;
3561 }
3562
3563 void mesh_net_set_prov(struct mesh_net *net, struct mesh_prov *prov)
3564 {
3565         if (!net)
3566                 return;
3567
3568         net->prov = prov;
3569 }
3570
3571 static void refresh_instant(void *a, void *b)
3572 {
3573         struct mesh_subnet *subnet = a;
3574         struct mesh_net *net = b;
3575         uint32_t instant = net_key_beacon_last_seen(subnet->net_key_tx);
3576
3577         if (net->instant < instant)
3578                 net->instant = instant;
3579 }
3580
3581 uint32_t mesh_net_get_instant(struct mesh_net *net)
3582 {
3583         if (!net)
3584                 return 0;
3585
3586         l_queue_foreach(net->subnets, refresh_instant, net);
3587
3588         return net->instant;
3589 }
3590
3591 static void hb_sub_timeout_func(struct l_timeout *timeout, void *user_data)
3592 {
3593         struct mesh_net *net = user_data;
3594         struct mesh_net_heartbeat_sub *sub = &net->hb_sub;
3595
3596         l_debug("HB Subscription Ended");
3597         l_timeout_remove(sub->timer);
3598         sub->timer = NULL;
3599         sub->enabled = false;
3600 }
3601
3602 static uint32_t log_to_uint32(uint8_t log)
3603 {
3604         if (!log)
3605                 return 0x0000;
3606
3607         return (1 << (log - 1));
3608 }
3609
3610 int mesh_net_set_heartbeat_sub(struct mesh_net *net, uint16_t src, uint16_t dst,
3611                                                         uint8_t period_log)
3612 {
3613         struct mesh_net_heartbeat_sub *sub = &net->hb_sub;
3614         struct timeval time_now;
3615
3616         if (!net)
3617                 return MESH_STATUS_UNSPECIFIED_ERROR;
3618
3619         /* Check if the subscription should be disabled */
3620         if (IS_UNASSIGNED(src) || IS_UNASSIGNED(dst) || !period_log) {
3621                 if (IS_GROUP(sub->dst))
3622                         mesh_net_dst_unreg(net, sub->dst);
3623
3624                 /* Preserve collected data, but disable */
3625                 sub->enabled = false;
3626                 sub->dst = UNASSIGNED_ADDRESS;
3627                 sub->src = UNASSIGNED_ADDRESS;
3628                 sub->period = 0;
3629
3630         } else {
3631                 if (sub->dst != dst) {
3632                         if (IS_GROUP(sub->dst))
3633                                 mesh_net_dst_unreg(net, sub->dst);
3634
3635                         if (IS_GROUP(dst))
3636                                 mesh_net_dst_reg(net, dst);
3637                 }
3638
3639                 sub->enabled = true;
3640                 sub->src = src;
3641                 sub->dst = dst;
3642                 sub->count = 0;
3643                 sub->period = log_to_uint32(period_log);
3644                 sub->min_hops = 0x7f;
3645                 sub->max_hops = 0x00;
3646                 gettimeofday(&time_now, NULL);
3647                 sub->start = time_now.tv_sec;
3648         }
3649
3650         /* TODO: Save to node config */
3651
3652         if (!sub->enabled) {
3653                 l_timeout_remove(sub->timer);
3654                 sub->timer = NULL;
3655                 return MESH_STATUS_SUCCESS;
3656         }
3657
3658         if (!sub->timer)
3659                 sub->timer = l_timeout_create(sub->period, hb_sub_timeout_func,
3660                                                                 net, NULL);
3661         else
3662                 l_timeout_modify(sub->timer, sub->period);
3663
3664         return MESH_STATUS_SUCCESS;
3665 }
3666
3667 static void hb_pub_timeout_func(struct l_timeout *timeout, void *user_data)
3668 {
3669         struct mesh_net *net = user_data;
3670         struct mesh_net_heartbeat_pub *pub = &net->hb_pub;
3671
3672         send_hb_publication(net);
3673
3674         if (pub->count != 0xffff)
3675                 pub->count--;
3676
3677         if (pub->count > 0)
3678                 l_timeout_modify(pub->timer, pub->period);
3679         else {
3680                 l_timeout_remove(pub->timer);
3681                 pub->timer = NULL;
3682         }
3683 }
3684
3685 static void update_hb_pub_timer(struct mesh_net *net,
3686                                         struct mesh_net_heartbeat_pub *pub)
3687 {
3688         if (IS_UNASSIGNED(pub->dst) || pub->count == 0) {
3689                 l_timeout_remove(pub->timer);
3690                 pub->timer = NULL;
3691                 return;
3692         }
3693
3694         if (!pub->timer)
3695                 pub->timer = l_timeout_create(pub->period,
3696                                         hb_pub_timeout_func, net, NULL);
3697         else
3698                 l_timeout_modify(pub->timer, pub->period);
3699 }
3700
3701 int mesh_net_set_heartbeat_pub(struct mesh_net *net, uint16_t dst,
3702                                 uint16_t features, uint16_t idx, uint8_t ttl,
3703                                 uint8_t count_log, uint8_t period_log)
3704 {
3705         struct mesh_subnet *subnet;
3706         struct mesh_net_heartbeat_pub *pub = &net->hb_pub;
3707
3708         if (!net)
3709                 return MESH_STATUS_UNSPECIFIED_ERROR;
3710
3711         subnet = l_queue_find(net->subnets, match_key_index,
3712                                                         L_UINT_TO_PTR(idx));
3713         if (!subnet)
3714                 return MESH_STATUS_INVALID_NETKEY;
3715
3716         pub->dst = dst;
3717
3718         if (pub->dst == UNASSIGNED_ADDRESS) {
3719                 pub->count = 0;
3720                 pub->period = 0;
3721                 pub->ttl = 0;
3722         } else {
3723                 pub->count = (count_log != 0xff) ?
3724                                         log_to_uint32(count_log) : 0xffff;
3725                 pub->period = log_to_uint32(period_log);
3726         }
3727
3728         pub->ttl = ttl;
3729         pub->features = features;
3730         pub->net_idx = idx;
3731         update_hb_pub_timer(net, pub);
3732
3733         /* TODO: Save to node config */
3734         return MESH_STATUS_SUCCESS;
3735 }
3736
3737 bool mesh_net_load_rpl(struct mesh_net *net)
3738 {
3739         return rpl_get_list(net->node, net->replay_cache);
3740 }