mesh/model.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2018-2019  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  */
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include <config.h>
  22 #endif
  23
  24 #include <sys/time.h>
  25 #include <ell/ell.h>
  26 #include <json-c/json.h>
  27
  28 #include "mesh/mesh-defs.h"
  29
  30 #include "mesh/mesh.h"
  31 #include "mesh/crypto.h"
  32 #include "mesh/node.h"
  33 #include "mesh/mesh-db.h"
  34 #include "mesh/net.h"
  35 #include "mesh/appkey.h"
  36 #include "mesh/cfgmod.h"
  37 #include "mesh/storage.h"
  38 #include "mesh/error.h"
  39 #include "mesh/dbus.h"
  40 #include "mesh/util.h"
  41 #include "mesh/model.h"
  42 #include "mesh/keyring.h"
  43
  44 /* Divide and round to ceiling (up) to calculate segment count */
  45 #define CEILDIV(val, div) (((val) + (div) - 1) / (div))
  46
  47 struct mesh_model {
  48         const struct mesh_model_ops *cbs;
  49         void *user_data;
  50         struct l_queue *bindings;
  51         struct l_queue *subs;
  52         struct l_queue *virtuals;
  53         struct mesh_model_pub *pub;
  54         uint32_t id;
  55         uint8_t ele_idx;
  56 };
  57
  58 struct mesh_virtual {
  59         uint32_t id; /* Internal ID of a stored virtual addr, min val 0x10000 */
  60         uint16_t ref_cnt;
  61         uint16_t addr; /* 16-bit virtual address, used in messages */
  62         uint8_t label[16]; /* 128 bit label UUID */
  63 };
  64
  65 /* These struct is used to pass lots of params to l_queue_foreach */
  66 struct mod_forward {
  67         struct mesh_virtual *virt;
  68         const uint8_t *data;
  69         uint16_t src;
  70         uint16_t dst;
  71         uint16_t unicast;
  72         uint16_t idx;
  73         uint16_t size;
  74         uint8_t ttl;
  75         int8_t rssi;
  76         bool szmict;
  77         bool has_dst;
  78         bool done;
  79 };
  80
  81 static struct l_queue *mesh_virtuals;
  82
  83 static uint32_t virt_id_next = VIRTUAL_BASE;
  84 static struct timeval tx_start;
  85
  86 static bool is_internal(uint32_t id)
  87 {
  88         if (id == CONFIG_SRV_MODEL || id == CONFIG_CLI_MODEL)
  89                 return true;
  90
  91         return false;
  92 }
  93
  94 static void unref_virt(void *data)
  95 {
  96         struct mesh_virtual *virt = data;
  97
  98         if (virt->ref_cnt > 0)
  99                 virt->ref_cnt--;
 100
 101         if (virt->ref_cnt)
 102                 return;
 103
 104         l_queue_remove(mesh_virtuals, virt);
 105         l_free(virt);
 106 }
 107
 108 static bool simple_match(const void *a, const void *b)
 109 {
 110         return a == b;
 111 }
 112
 113 static bool has_binding(struct l_queue *bindings, uint16_t idx)
 114 {
 115         const struct l_queue_entry *l;
 116
 117         for (l = l_queue_get_entries(bindings); l; l = l->next) {
 118                 if (L_PTR_TO_UINT(l->data) == idx)
 119                         return true;
 120         }
 121         return false;
 122 }
 123
 124 static bool find_virt_by_id(const void *a, const void *b)
 125 {
 126         const struct mesh_virtual *virt = a;
 127         uint32_t id = L_PTR_TO_UINT(b);
 128
 129         return virt->id == id;
 130 }
 131
 132 static bool find_virt_by_label(const void *a, const void *b)
 133 {
 134         const struct mesh_virtual *virt = a;
 135         const uint8_t *label = b;
 136
 137         return memcmp(virt->label, label, 16) == 0;
 138 }
 139
 140 static bool match_model_id(const void *a, const void *b)
 141 {
 142         const struct mesh_model *model = a;
 143         uint32_t id = L_PTR_TO_UINT(b);
 144
 145         return (mesh_model_get_model_id(model) == id);
 146 }
 147
 148 static struct mesh_model *get_model(struct mesh_node *node, uint8_t ele_idx,
 149                                                 uint32_t id, int *status)
 150 {
 151         struct l_queue *models;
 152         struct mesh_model *model;
 153
 154         models = node_get_element_models(node, ele_idx, status);
 155         if (!models) {
 156                 *status = MESH_STATUS_INVALID_MODEL;
 157                 return NULL;
 158         }
 159
 160         model = l_queue_find(models, match_model_id, L_UINT_TO_PTR(id));
 161
 162         *status = (model) ? MESH_STATUS_SUCCESS : MESH_STATUS_INVALID_MODEL;
 163
 164         return model;
 165 }
 166
 167 static struct mesh_model *find_model(struct mesh_node *node, uint16_t addr,
 168                                                 uint32_t mod_id, int *status)
 169 {
 170         int ele_idx;
 171
 172         ele_idx = node_get_element_idx(node, addr);
 173
 174         if (ele_idx < 0) {
 175                 *status = MESH_STATUS_INVALID_ADDRESS;
 176                 return NULL;
 177         }
 178
 179         return get_model(node, (uint8_t) ele_idx, mod_id, status);
 180 }
 181
 182 static uint32_t pub_period_to_ms(uint8_t pub_period)
 183 {
 184         int n;
 185
 186         n = pub_period >> 2;
 187
 188         switch (pub_period & 0x3) {
 189         default:
 190                 return n * 100;
 191         case 2:
 192                 n *= 10;
 193                 /* Fall Through */
 194         case 1:
 195                 return n * 1000;
 196         case 3:
 197                 return n * 10 * 60 * 1000;
 198         }
 199 }
 200
 201 static struct l_dbus_message *create_config_update_msg(struct mesh_node *node,
 202                                         uint8_t ele_idx, uint32_t id,
 203                                         struct l_dbus_message_builder **builder)
 204 {
 205         struct l_dbus *dbus = dbus_get_bus();
 206         struct l_dbus_message *msg;
 207         const char *owner;
 208         const char *path;
 209         uint16_t model_id;
 210
 211         owner = node_get_owner(node);
 212         path = node_get_element_path(node, ele_idx);
 213         if (!path || !owner)
 214                 return NULL;
 215
 216         l_debug("Send \"UpdateModelConfiguration\"");
 217         msg = l_dbus_message_new_method_call(dbus, owner, path,
 218                                                 MESH_ELEMENT_INTERFACE,
 219                                                 "UpdateModelConfiguration");
 220
 221         *builder = l_dbus_message_builder_new(msg);
 222
 223         model_id = (uint16_t) id;
 224
 225         l_dbus_message_builder_append_basic(*builder, 'q', &model_id);
 226
 227         l_dbus_message_builder_enter_array(*builder, "{sv}");
 228
 229         if ((id & VENDOR_ID_MASK) != VENDOR_ID_MASK) {
 230                 uint16_t vendor = id >> 16;
 231                 dbus_append_dict_entry_basic(*builder, "Vendor", "q", &vendor);
 232         }
 233
 234         return msg;
 235 }
 236
 237 static void config_update_model_pub_period(struct mesh_node *node,
 238                                         uint8_t ele_idx, uint32_t model_id,
 239                                         uint32_t period)
 240 {
 241         struct l_dbus *dbus = dbus_get_bus();
 242         struct l_dbus_message *msg;
 243         struct l_dbus_message_builder *builder;
 244
 245         msg = create_config_update_msg(node, ele_idx, model_id, &builder);
 246         if (!msg)
 247                 return;
 248
 249         dbus_append_dict_entry_basic(builder, "PublicationPeriod", "u",
 250                                                                 &period);
 251
 252         l_dbus_message_builder_leave_array(builder);
 253         l_dbus_message_builder_finalize(builder);
 254         l_dbus_message_builder_destroy(builder);
 255         l_dbus_send(dbus, msg);
 256 }
 257
 258 static void append_dict_uint16_array(struct l_dbus_message_builder *builder,
 259                                         struct l_queue *q, const char *key)
 260 {
 261         const struct l_queue_entry *entry;
 262
 263         l_dbus_message_builder_enter_dict(builder, "sv");
 264         l_dbus_message_builder_append_basic(builder, 's', key);
 265         l_dbus_message_builder_enter_variant(builder, "aq");
 266         l_dbus_message_builder_enter_array(builder, "q");
 267
 268         for (entry = l_queue_get_entries(q); entry; entry = entry->next) {
 269                 uint16_t value = (uint16_t) L_PTR_TO_UINT(entry->data);
 270
 271                 l_dbus_message_builder_append_basic(builder,'q', &value);
 272         }
 273
 274         l_dbus_message_builder_leave_array(builder);
 275         l_dbus_message_builder_leave_variant(builder);
 276         l_dbus_message_builder_leave_dict(builder);
 277 }
 278
 279 static void config_update_model_bindings(struct mesh_node *node,
 280                                                         struct mesh_model *mod)
 281 {
 282         struct l_dbus *dbus = dbus_get_bus();
 283         struct l_dbus_message *msg;
 284         struct l_dbus_message_builder *builder;
 285
 286         msg = create_config_update_msg(node, mod->ele_idx, mod->id,
 287                                                                 &builder);
 288         if (!msg)
 289                 return;
 290
 291         append_dict_uint16_array(builder, mod->bindings, "Bindings");
 292
 293         l_dbus_message_builder_leave_array(builder);
 294         l_dbus_message_builder_finalize(builder);
 295         l_dbus_message_builder_destroy(builder);
 296         l_dbus_send(dbus, msg);
 297 }
 298
 299 static void forward_model(void *a, void *b)
 300 {
 301         struct mesh_model *mod = a;
 302         struct mod_forward *fwd = b;
 303         struct mesh_virtual *virt;
 304         uint32_t dst;
 305         bool result;
 306
 307         l_debug("model %8.8x with idx %3.3x", mod->id, fwd->idx);
 308
 309         if (fwd->idx != APP_IDX_DEV_LOCAL && fwd->idx != APP_IDX_DEV_REMOTE &&
 310                                         !has_binding(mod->bindings, fwd->idx))
 311                 return;
 312
 313         dst = fwd->dst;
 314         if (dst == fwd->unicast || IS_ALL_NODES(dst))
 315                 fwd->has_dst = true;
 316         else if (fwd->virt) {
 317                 virt = l_queue_find(mod->virtuals, simple_match, fwd->virt);
 318
 319                 /* Check that this is not own publication */
 320                 if (mod->pub && (virt && virt->id == mod->pub->addr))
 321                         return;
 322
 323                 if (virt) {
 324                         /*
 325                          * Map Virtual addresses to a usable namespace that
 326                          * prevents us for forwarding a false positive
 327                          * (multiple Virtual Addresses that map to the same
 328                          * 16-bit virtual address identifier)
 329                          */
 330                         fwd->has_dst = true;
 331                         dst = virt->id;
 332                 }
 333         } else {
 334                 if (l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(dst)))
 335                         fwd->has_dst = true;
 336         }
 337
 338         if (!fwd->has_dst)
 339                 return;
 340
 341         /* Return, if this is not a internal model */
 342         if (!mod->cbs)
 343                 return;
 344
 345         result = false;
 346
 347         if (mod->cbs->recv)
 348                 result = mod->cbs->recv(fwd->src, dst, fwd->unicast, fwd->idx,
 349                                 fwd->data, fwd->size, fwd->ttl, mod->user_data);
 350
 351         if (dst == fwd->unicast && result)
 352                 fwd->done = true;
 353 }
 354
 355 static int dev_packet_decrypt(struct mesh_node *node, const uint8_t *data,
 356                                 uint16_t size, bool szmict, uint16_t src,
 357                                 uint16_t dst, uint8_t key_id, uint32_t seq,
 358                                 uint32_t iv_idx, uint8_t *out)
 359 {
 360         uint8_t dev_key[16];
 361         const uint8_t *key;
 362
 363         key = node_get_device_key(node);
 364         if (!key)
 365                 return -1;
 366
 367         if (mesh_crypto_payload_decrypt(NULL, 0, data, size, szmict, src,
 368                                         dst, key_id, seq, iv_idx, out, key))
 369                 return APP_IDX_DEV_LOCAL;
 370
 371         if (!keyring_get_remote_dev_key(node, src, dev_key))
 372                 return -1;
 373
 374         key = dev_key;
 375         if (mesh_crypto_payload_decrypt(NULL, 0, data, size, szmict, src,
 376                                         dst, key_id, seq, iv_idx, out, key))
 377                 return APP_IDX_DEV_REMOTE;
 378
 379         return -1;
 380 }
 381
 382 static int virt_packet_decrypt(struct mesh_net *net, const uint8_t *data,
 383                                 uint16_t size, bool szmict, uint16_t src,
 384                                 uint16_t dst, uint8_t key_id, uint32_t seq,
 385                                 uint32_t iv_idx, uint8_t *out,
 386                                 struct mesh_virtual **decrypt_virt)
 387 {
 388         const struct l_queue_entry *v;
 389
 390         for (v = l_queue_get_entries(mesh_virtuals); v; v = v->next) {
 391                 struct mesh_virtual *virt = v->data;
 392                 int decrypt_idx;
 393
 394                 if (virt->addr != dst)
 395                         continue;
 396
 397                 decrypt_idx = appkey_packet_decrypt(net, szmict, seq,
 398                                                         iv_idx, src, dst,
 399                                                         virt->label, 16, key_id,
 400                                                         data, size, out);
 401
 402                 if (decrypt_idx >= 0) {
 403                         *decrypt_virt = virt;
 404                         return decrypt_idx;
 405                 }
 406         }
 407
 408         return -1;
 409 }
 410
 411 static void cmplt(uint16_t remote, uint8_t status,
 412                                         void *data, uint16_t size,
 413                                         void *user_data)
 414 {
 415         struct timeval tx_end;
 416
 417         if (status)
 418                 l_debug("Tx-->%4.4x (%d octets) Failed (%d)",
 419                                 remote, size, status);
 420         else
 421                 l_debug("Tx-->%4.4x (%d octets) Succeeded", remote, size);
 422
 423         /* print_packet("Sent Data", data, size); */
 424
 425         gettimeofday(&tx_end, NULL);
 426         if (tx_end.tv_sec == tx_start.tv_sec) {
 427                 l_debug("Duration 0.%6.6lu seconds",
 428                                 tx_end.tv_usec - tx_start.tv_usec);
 429         } else {
 430                 if (tx_start.tv_usec > tx_end.tv_usec)
 431                         l_debug("Duration %lu.%6.6lu seconds",
 432                                 tx_end.tv_sec - tx_start.tv_sec - 1,
 433                                 tx_end.tv_usec + 1000000 - tx_start.tv_usec);
 434                 else
 435                         l_debug("Duration %lu.%6.6lu seconds",
 436                                         tx_end.tv_sec - tx_start.tv_sec,
 437                                         tx_end.tv_usec - tx_start.tv_usec);
 438         }
 439 }
 440
 441 static bool msg_send(struct mesh_node *node, bool credential, uint16_t src,
 442                 uint32_t dst, uint8_t key_id, const uint8_t *key,
 443                 uint8_t *label, uint8_t ttl, const void *msg, uint16_t msg_len)
 444 {
 445         bool ret = false;
 446         uint32_t iv_index, seq_num;
 447         uint8_t *out;
 448         bool szmic = false;
 449         uint16_t out_len = msg_len + sizeof(uint32_t);
 450         struct mesh_net *net = node_get_net(node);
 451
 452         /* Use large MIC if it doesn't affect segmentation */
 453         if (msg_len > 11 && msg_len <= 376) {
 454                 if (CEILDIV(out_len, 12) == CEILDIV(out_len + 4, 12)) {
 455                         szmic = true;
 456                         out_len = msg_len + sizeof(uint64_t);
 457                 }
 458         }
 459
 460         out = l_malloc(out_len);
 461
 462         iv_index = mesh_net_get_iv_index(net);
 463
 464         seq_num = mesh_net_get_seq_num(net);
 465         if (!mesh_crypto_payload_encrypt(label, msg, out, msg_len, src, dst,
 466                                 key_id, seq_num, iv_index, szmic, key)) {
 467                 l_error("Failed to Encrypt Payload");
 468                 goto done;
 469         }
 470
 471         /* print_packet("Encrypted with", key, 16); */
 472
 473         ret = mesh_net_app_send(net, credential, src, dst, key_id, ttl,
 474                                         seq_num, iv_index, szmic, out, out_len,
 475                                                                 cmplt, NULL);
 476 done:
 477         l_free(out);
 478         return ret;
 479 }
 480
 481 static void remove_pub(struct mesh_node *node, struct mesh_model *mod)
 482 {
 483         l_free(mod->pub);
 484         mod->pub = NULL;
 485
 486         if (!mod->cbs)
 487                 /* External models */
 488                 config_update_model_pub_period(node, mod->ele_idx, mod->id, 0);
 489         else if (mod->cbs && mod->cbs->pub)
 490                 /* Internal models */
 491                 mod->cbs->pub(NULL);
 492 }
 493
 494 static void model_unbind_idx(struct mesh_node *node, struct mesh_model *mod,
 495                                                                 uint16_t idx)
 496 {
 497         l_queue_remove(mod->bindings, L_UINT_TO_PTR(idx));
 498
 499         if (!mod->cbs)
 500                 /* External model */
 501                 config_update_model_bindings(node, mod);
 502         else if (mod->cbs->bind)
 503                 /* Internal model */
 504                 mod->cbs->bind(idx, ACTION_DELETE);
 505
 506         if (mod->pub && idx != mod->pub->idx)
 507                 return;
 508
 509         /* Remove model publication if the publication key is unbound */
 510         remove_pub(node, mod);
 511 }
 512
 513 static void model_bind_idx(struct mesh_node *node, struct mesh_model *mod,
 514                                                                 uint16_t idx)
 515 {
 516         if (!mod->bindings)
 517                 mod->bindings = l_queue_new();
 518
 519         l_queue_push_tail(mod->bindings, L_UINT_TO_PTR(idx));
 520
 521         l_debug("Add %4.4x to model %8.8x", idx, mod->id);
 522
 523         if (!mod->cbs)
 524                 /* External model */
 525                 config_update_model_bindings(node, mod);
 526         else if (mod->cbs->bind)
 527                 /* Internal model */
 528                 mod->cbs->bind(idx, ACTION_ADD);
 529 }
 530
 531 static int update_binding(struct mesh_node *node, uint16_t addr, uint32_t id,
 532                                                 uint16_t app_idx, bool unbind)
 533 {
 534         int status;
 535         struct mesh_model *mod;
 536         bool is_present;
 537
 538         mod = find_model(node, addr, id, &status);
 539         if (!mod) {
 540                 l_debug("Model not found");
 541                 return status;
 542         }
 543
 544         id = (id >= VENDOR_ID_MASK) ? (id & 0xffff) : id;
 545
 546         if (id == CONFIG_SRV_MODEL || id == CONFIG_CLI_MODEL)
 547                 return MESH_STATUS_INVALID_MODEL;
 548
 549         if (!appkey_have_key(node_get_net(node), app_idx))
 550                 return MESH_STATUS_INVALID_APPKEY;
 551
 552         is_present = has_binding(mod->bindings, app_idx);
 553
 554         if (!is_present && unbind)
 555                 return MESH_STATUS_SUCCESS;
 556
 557         if (is_present && !unbind)
 558                 return MESH_STATUS_SUCCESS;
 559
 560         if (unbind) {
 561                 model_unbind_idx(node, mod, app_idx);
 562
 563                 if (!storage_model_bind(node, addr, id, app_idx, true))
 564                         return MESH_STATUS_STORAGE_FAIL;
 565
 566                 return MESH_STATUS_SUCCESS;
 567         }
 568
 569         if (l_queue_length(mod->bindings) >= MAX_BINDINGS)
 570                 return MESH_STATUS_INSUFF_RESOURCES;
 571
 572         if (!storage_model_bind(node, addr, id, app_idx, false))
 573                 return MESH_STATUS_STORAGE_FAIL;
 574
 575         model_bind_idx(node, mod, app_idx);
 576
 577         return MESH_STATUS_SUCCESS;
 578
 579 }
 580
 581 static struct mesh_virtual *add_virtual(const uint8_t *v)
 582 {
 583         struct mesh_virtual *virt = l_queue_find(mesh_virtuals,
 584                                                 find_virt_by_label, v);
 585
 586         if (virt) {
 587                 virt->ref_cnt++;
 588                 return virt;
 589         }
 590
 591         virt = l_new(struct mesh_virtual, 1);
 592
 593         if (!mesh_crypto_virtual_addr(v, &virt->addr)) {
 594                 l_free(virt);
 595                 return NULL;
 596         }
 597
 598         memcpy(virt->label, v, 16);
 599         virt->ref_cnt = 1;
 600         virt->id = virt_id_next++;
 601         l_queue_push_head(mesh_virtuals, virt);
 602
 603         return virt;
 604 }
 605
 606 static int set_pub(struct mesh_model *mod, const uint8_t *pub_addr,
 607                         uint16_t idx, bool cred_flag, uint8_t ttl,
 608                         uint8_t period, uint8_t retransmit, bool b_virt,
 609                         uint16_t *dst)
 610 {
 611         struct mesh_virtual *virt = NULL;
 612         uint16_t grp;
 613
 614         if (dst) {
 615                 if (b_virt)
 616                         *dst = 0;
 617                 else
 618                         *dst = l_get_le16(pub_addr);
 619         }
 620
 621         if (b_virt) {
 622                 virt = add_virtual(pub_addr);
 623                 if (!virt)
 624                         return MESH_STATUS_STORAGE_FAIL;
 625
 626         }
 627
 628         /* If the old publication address is virtual, remove it from lists */
 629         if (mod->pub && mod->pub->addr >= VIRTUAL_BASE) {
 630                 struct mesh_virtual *old_virt;
 631
 632                 old_virt = l_queue_find(mod->virtuals, find_virt_by_id,
 633                                                 L_UINT_TO_PTR(mod->pub->addr));
 634                 if (old_virt) {
 635                         l_queue_remove(mod->virtuals, old_virt);
 636                         unref_virt(old_virt);
 637                 }
 638         }
 639
 640         mod->pub = l_new(struct mesh_model_pub, 1);
 641
 642         if (b_virt) {
 643                 l_queue_push_head(mod->virtuals, virt);
 644                 grp = virt->addr;
 645                 mod->pub->addr = virt->id;
 646         } else {
 647                 grp = l_get_le16(pub_addr);
 648                 mod->pub->addr = grp;
 649         }
 650
 651         if (dst)
 652                 *dst = grp;
 653
 654         mod->pub->credential = cred_flag;
 655         mod->pub->idx = idx;
 656         mod->pub->ttl = ttl;
 657         mod->pub->period = period;
 658         mod->pub->retransmit = retransmit;
 659
 660         return MESH_STATUS_SUCCESS;
 661 }
 662
 663 static int add_sub(struct mesh_net *net, struct mesh_model *mod,
 664                         const uint8_t *group, bool b_virt, uint16_t *dst)
 665 {
 666         struct mesh_virtual *virt = NULL;
 667         uint16_t grp;
 668
 669         if (b_virt) {
 670                 virt = add_virtual(group);
 671                 if (!virt)
 672                         return MESH_STATUS_STORAGE_FAIL;
 673
 674                 grp = virt->addr;
 675         } else {
 676                 grp = l_get_le16(group);
 677         }
 678
 679         if (dst)
 680                 *dst = grp;
 681
 682         if (!mod->subs)
 683                 mod->subs = l_queue_new();
 684
 685         /* Check if this group already exists */
 686         if (l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(grp))) {
 687                 if (b_virt)
 688                         unref_virt(virt);
 689
 690                 return MESH_STATUS_SUCCESS;
 691         }
 692
 693         if (b_virt)
 694                 l_queue_push_head(mod->virtuals, virt);
 695
 696         l_queue_push_tail(mod->subs, L_UINT_TO_PTR(grp));
 697
 698         l_debug("Added %4.4x", grp);
 699
 700         mesh_net_dst_reg(net, grp);
 701
 702         return MESH_STATUS_SUCCESS;
 703 }
 704
 705 static void send_dev_key_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
 706                                         uint16_t src, uint16_t net_idx,
 707                                         uint16_t size, const uint8_t *data)
 708 {
 709         struct l_dbus *dbus = dbus_get_bus();
 710         struct l_dbus_message *msg;
 711         struct l_dbus_message_builder *builder;
 712         const char *owner;
 713         const char *path;
 714
 715         owner = node_get_owner(node);
 716         path = node_get_element_path(node, ele_idx);
 717         if (!path || !owner)
 718                 return;
 719
 720         l_debug("Send \"DevKeyMessageReceived\"");
 721
 722         msg = l_dbus_message_new_method_call(dbus, owner, path,
 723                                                 MESH_ELEMENT_INTERFACE,
 724                                                 "DevKeyMessageReceived");
 725
 726         builder = l_dbus_message_builder_new(msg);
 727
 728         l_dbus_message_builder_append_basic(builder, 'q', &src);
 729         l_dbus_message_builder_append_basic(builder, 'q', &net_idx);
 730         dbus_append_byte_array(builder, data, size);
 731
 732         l_dbus_message_builder_finalize(builder);
 733         l_dbus_message_builder_destroy(builder);
 734
 735         l_dbus_send(dbus, msg);
 736 }
 737
 738 static void send_msg_rcvd(struct mesh_node *node, uint8_t ele_idx, bool is_sub,
 739                                         uint16_t src, uint16_t key_idx,
 740                                         uint16_t size, const uint8_t *data)
 741 {
 742         struct l_dbus *dbus = dbus_get_bus();
 743         struct l_dbus_message *msg;
 744         struct l_dbus_message_builder *builder;
 745         const char *owner;
 746         const char *path;
 747
 748         owner = node_get_owner(node);
 749         path = node_get_element_path(node, ele_idx);
 750         if (!path || !owner)
 751                 return;
 752
 753         l_debug("Send \"MessageReceived\"");
 754
 755         msg = l_dbus_message_new_method_call(dbus, owner, path,
 756                                 MESH_ELEMENT_INTERFACE, "MessageReceived");
 757
 758         builder = l_dbus_message_builder_new(msg);
 759
 760         l_dbus_message_builder_append_basic(builder, 'q', &src);
 761         l_dbus_message_builder_append_basic(builder, 'q', &key_idx);
 762         l_dbus_message_builder_append_basic(builder, 'b', &is_sub);
 763
 764         dbus_append_byte_array(builder, data, size);
 765
 766         l_dbus_message_builder_finalize(builder);
 767         l_dbus_message_builder_destroy(builder);
 768         l_dbus_send(dbus, msg);
 769 }
 770
 771 bool mesh_model_rx(struct mesh_node *node, bool szmict, uint32_t seq0,
 772                         uint32_t seq, uint32_t iv_index, uint8_t ttl,
 773                         uint16_t src, uint16_t dst, uint8_t key_id,
 774                         const uint8_t *data, uint16_t size)
 775 {
 776         uint8_t *clear_text;
 777         struct mod_forward forward = {
 778                 .src = src,
 779                 .dst = dst,
 780                 .data = NULL,
 781                 .size = size - (szmict ? 8 : 4),
 782                 .ttl = ttl,
 783                 .virt = NULL,
 784         };
 785         struct mesh_net *net = node_get_net(node);
 786         uint8_t num_ele;
 787         int decrypt_idx, i, ele_idx;
 788         uint16_t addr;
 789         struct mesh_virtual *decrypt_virt = NULL;
 790         bool result = false;
 791         bool is_subscription;
 792
 793         l_debug("iv_index %8.8x key_id = %2.2x", iv_index, key_id);
 794         if (!dst)
 795                 return false;
 796
 797         ele_idx = node_get_element_idx(node, dst);
 798
 799         if (dst < 0x8000 && ele_idx < 0)
 800                 /* Unicast and not addressed to us */
 801                 return false;
 802
 803         clear_text = l_malloc(size);
 804         if (!clear_text)
 805                 return false;
 806
 807         forward.data = clear_text;
 808
 809         /*
 810          * The packet needs to be decoded by the correct key which
 811          * is hinted by key_id, but is not necessarily definitive
 812          */
 813         if (key_id == APP_ID_DEV || mesh_net_provisioner_mode_get(net))
 814                 decrypt_idx = dev_packet_decrypt(node, data, size, szmict, src,
 815                                                 dst, key_id, seq0, iv_index,
 816                                                 clear_text);
 817         else if ((dst & 0xc000) == 0x8000)
 818                 decrypt_idx = virt_packet_decrypt(net, data, size, szmict, src,
 819                                                         dst, key_id, seq0,
 820                                                         iv_index, clear_text,
 821                                                         &decrypt_virt);
 822         else
 823                 decrypt_idx = appkey_packet_decrypt(net, szmict, seq0,
 824                                                         iv_index, src, dst,
 825                                                         NULL, 0, key_id, data,
 826                                                         size, clear_text);
 827
 828         if (decrypt_idx < 0) {
 829                 l_error("model.c - Failed to decrypt application payload");
 830                 result = false;
 831                 goto done;
 832         }
 833
 834         /* print_packet("Clr Rx (pre-cache-check)", clear_text, size - 4); */
 835
 836         if (key_id != APP_ID_DEV) {
 837                 uint16_t crpl = node_get_crpl(node);
 838
 839                 if (appkey_msg_in_replay_cache(net, (uint16_t) decrypt_idx, src,
 840                                                         crpl, seq, iv_index)) {
 841                         result = true;
 842                         goto done;
 843                 }
 844         }
 845
 846         print_packet("Clr Rx", clear_text, size - (szmict ? 8 : 4));
 847
 848         forward.virt = decrypt_virt;
 849         forward.idx = decrypt_idx;
 850         num_ele = node_get_num_elements(node);
 851         addr = node_get_primary(node);
 852
 853         if (!num_ele || IS_UNASSIGNED(addr))
 854                 goto done;
 855
 856         is_subscription = !(IS_UNICAST(dst));
 857
 858         for (i = 0; i < num_ele; i++) {
 859                 struct l_queue *models;
 860
 861                 if (!is_subscription && ele_idx != i)
 862                         continue;
 863
 864                 forward.unicast = addr + i;
 865                 forward.has_dst = false;
 866
 867                 models = node_get_element_models(node, i, NULL);
 868
 869                 /* Internal models */
 870                 l_queue_foreach(models, forward_model, &forward);
 871
 872                 /*
 873                  * Cycle through external models if the message has not been
 874                  * handled by internal models
 875                  */
 876                 if (forward.has_dst && !forward.done) {
 877                         if ((decrypt_idx & APP_IDX_MASK) == decrypt_idx)
 878                                 send_msg_rcvd(node, i, is_subscription, src,
 879                                                 forward.idx, forward.size,
 880                                                 forward.data);
 881                         else if (decrypt_idx == APP_IDX_DEV_REMOTE ||
 882                                 (decrypt_idx == APP_IDX_DEV_LOCAL &&
 883                                  mesh_net_is_local_address(net, src)))
 884                                 send_dev_key_msg_rcvd(node, i, src, 0,
 885                                                 forward.size, forward.data);
 886                 }
 887
 888                 /*
 889                  * Either the message has been processed internally or
 890                  * has been passed on to an external model.
 891                  */
 892                 result = forward.has_dst | forward.done;
 893
 894                 /* If the message was to unicast address, we are done */
 895                 if (!is_subscription && ele_idx == i)
 896                         break;
 897         }
 898
 899 done:
 900         l_free(clear_text);
 901         return result;
 902 }
 903
 904 int mesh_model_publish(struct mesh_node *node, uint32_t mod_id,
 905                                 uint16_t src, uint8_t ttl,
 906                                 const void *msg, uint16_t msg_len)
 907 {
 908         struct mesh_net *net = node_get_net(node);
 909         struct mesh_model *mod;
 910         uint32_t target;
 911         uint8_t *label = NULL;
 912         uint16_t dst;
 913         uint8_t key_id;
 914         const uint8_t *key;
 915         bool result;
 916         int status;
 917
 918         /* print_packet("Mod Tx", msg, msg_len); */
 919
 920         if (!net || msg_len > 380)
 921                 return MESH_ERROR_INVALID_ARGS;
 922
 923         /* If SRC is 0, use the Primary Element */
 924         if (src == 0)
 925                 src = mesh_net_get_address(net);
 926
 927         mod = find_model(node, src, mod_id, &status);
 928         if (!mod) {
 929                 l_debug("model %x not found", mod_id);
 930                 return MESH_ERROR_NOT_FOUND;
 931         }
 932
 933         if (!mod->pub) {
 934                 l_debug("publication doesn't exist (model %x)", mod_id);
 935                 return MESH_ERROR_DOES_NOT_EXIST;
 936         }
 937
 938         gettimeofday(&tx_start, NULL);
 939
 940         target = mod->pub->addr;
 941
 942         if (IS_UNASSIGNED(target))
 943                 return MESH_ERROR_DOES_NOT_EXIST;
 944
 945         if (target >= VIRTUAL_BASE) {
 946                 struct mesh_virtual *virt;
 947
 948                 virt = l_queue_find(mesh_virtuals, find_virt_by_id,
 949                                                 L_UINT_TO_PTR(target));
 950                 if (!virt)
 951                         return MESH_ERROR_NOT_FOUND;
 952
 953                 label = virt->label;
 954                 dst = virt->addr;
 955         } else {
 956                 dst = target;
 957         }
 958
 959         l_debug("publish dst=%x", dst);
 960
 961         key = appkey_get_key(net, mod->pub->idx, &key_id);
 962         if (!key) {
 963                 l_debug("no app key for (%x)", mod->pub->idx);
 964                 return false;
 965         }
 966
 967         l_debug("(%x) %p", mod->pub->idx, key);
 968         l_debug("key_id %x", key_id);
 969
 970         result = msg_send(node, mod->pub->credential != 0, src,
 971                                 dst, key_id, key, label, ttl, msg, msg_len);
 972
 973         return result ? MESH_ERROR_NONE : MESH_ERROR_FAILED;
 974
 975 }
 976
 977 bool mesh_model_send(struct mesh_node *node, uint16_t src, uint16_t target,
 978                                         uint16_t app_idx, uint8_t ttl,
 979                                         const void *msg, uint16_t msg_len)
 980 {
 981         uint8_t key_id;
 982         uint8_t dev_key[16];
 983         const uint8_t *key;
 984
 985         /* print_packet("Mod Tx", msg, msg_len); */
 986
 987         /* If SRC is 0, use the Primary Element */
 988         if (src == 0)
 989                 src = node_get_primary(node);
 990
 991         gettimeofday(&tx_start, NULL);
 992
 993         if (IS_UNASSIGNED(target))
 994                 return false;
 995
 996         if (app_idx == APP_IDX_DEV_LOCAL) {
 997                 key = node_get_device_key(node);
 998                 if (!key)
 999                         return false;
1000
1001                 key_id = APP_ID_DEV;
1002         } else if (app_idx == APP_IDX_DEV_REMOTE) {
1003                 if (!keyring_get_remote_dev_key(node, target, dev_key))
1004                         return false;
1005
1006                 key = dev_key;
1007                 key_id = APP_ID_DEV;
1008         } else {
1009                 key = appkey_get_key(node_get_net(node), app_idx, &key_id);
1010                 if (!key) {
1011                         l_debug("no app key for (%x)", app_idx);
1012                         return false;
1013                 }
1014
1015                 l_debug("(%x) %p", app_idx, key);
1016                 l_debug("key_id %x", key_id);
1017         }
1018
1019         return msg_send(node, false, src, target, key_id, key, NULL, ttl,
1020                         msg, msg_len);
1021 }
1022
1023 int mesh_model_pub_set(struct mesh_node *node, uint16_t addr, uint32_t id,
1024                         const uint8_t *pub_addr, uint16_t idx, bool cred_flag,
1025                         uint8_t ttl, uint8_t period, uint8_t retransmit,
1026                         bool b_virt, uint16_t *dst)
1027 {
1028         struct mesh_model *mod;
1029         int status;
1030
1031         mod = find_model(node, addr, id, &status);
1032         if (!mod)
1033                 return status;
1034
1035         if (id == CONFIG_SRV_MODEL || id == CONFIG_CLI_MODEL)
1036                 return MESH_STATUS_INVALID_PUB_PARAM;
1037
1038         if (!appkey_have_key(node_get_net(node), idx))
1039                 return MESH_STATUS_INVALID_APPKEY;
1040
1041         /*
1042          * If the publication address is set to unassigned address value,
1043          * remove the publication
1044          */
1045         if (!b_virt && IS_UNASSIGNED(l_get_le16(pub_addr))) {
1046                 remove_pub(node, mod);
1047                 return MESH_STATUS_SUCCESS;
1048         }
1049
1050         status = set_pub(mod, pub_addr, idx, cred_flag, ttl, period, retransmit,
1051                                                                 b_virt, dst);
1052
1053         if (status != MESH_STATUS_SUCCESS)
1054                 return status;
1055
1056         if (!mod->cbs)
1057                 /* External model */
1058                 config_update_model_pub_period(node, mod->ele_idx, id,
1059                                                 pub_period_to_ms(period));
1060         else
1061                 /* Internal model, call registered callbacks */
1062                 mod->cbs->pub(mod->pub);
1063
1064         return MESH_STATUS_SUCCESS;
1065 }
1066
1067 struct mesh_model_pub *mesh_model_pub_get(struct mesh_node *node, uint16_t addr,
1068                                                 uint32_t mod_id, int *status)
1069 {
1070         struct mesh_model *mod;
1071
1072         mod = find_model(node, addr, mod_id, status);
1073         if (!mod)
1074                 return NULL;
1075
1076         return mod->pub;
1077 }
1078
1079 void mesh_model_free(void *data)
1080 {
1081         struct mesh_model *mod = data;
1082
1083         l_queue_destroy(mod->bindings, NULL);
1084         l_queue_destroy(mod->subs, NULL);
1085         l_queue_destroy(mod->virtuals, unref_virt);
1086         l_free(mod->pub);
1087         l_free(mod);
1088 }
1089
1090 static struct mesh_model *model_new(uint8_t ele_idx, uint32_t id)
1091 {
1092         struct mesh_model *mod = l_new(struct mesh_model, 1);
1093
1094         mod->id = id;
1095         mod->ele_idx = ele_idx;
1096         mod->virtuals = l_queue_new();
1097         return mod;
1098 }
1099
1100 struct mesh_model *mesh_model_new(uint8_t ele_idx, uint16_t id)
1101 {
1102         return model_new(ele_idx, id | VENDOR_ID_MASK);
1103 }
1104
1105 struct mesh_model *mesh_model_vendor_new(uint8_t ele_idx, uint16_t vendor_id,
1106                                                                 uint16_t mod_id)
1107 {
1108         uint32_t id = mod_id | (vendor_id << 16);
1109
1110         return model_new(ele_idx, id);
1111 }
1112
1113 /* Internal models only */
1114 static void restore_model_state(struct mesh_model *mod)
1115 {
1116         const struct mesh_model_ops *cbs;
1117         const struct l_queue_entry *b;
1118
1119         cbs = mod->cbs;
1120         if (!cbs)
1121                 return;
1122
1123         if (!l_queue_isempty(mod->bindings) && cbs->bind) {
1124                 for (b = l_queue_get_entries(mod->bindings); b; b = b->next) {
1125                         if (cbs->bind(L_PTR_TO_UINT(b->data), ACTION_ADD) !=
1126                                                         MESH_STATUS_SUCCESS)
1127                                 break;
1128                 }
1129         }
1130
1131         if (mod->pub && cbs->pub)
1132                 cbs->pub(mod->pub);
1133
1134 }
1135
1136 uint32_t mesh_model_get_model_id(const struct mesh_model *model)
1137 {
1138         return model->id;
1139 }
1140
1141 /* This registers an internal model, i.e. implemented within meshd */
1142 bool mesh_model_register(struct mesh_node *node, uint8_t ele_idx,
1143                                         uint32_t mod_id,
1144                                         const struct mesh_model_ops *cbs,
1145                                         void *user_data)
1146 {
1147         struct mesh_model *mod;
1148         int status;
1149
1150         /* Internal models are always SIG models */
1151         mod_id = VENDOR_ID_MASK | mod_id;
1152
1153         mod = get_model(node, ele_idx, mod_id, &status);
1154         if (!mod)
1155                 return false;
1156
1157         mod->cbs = cbs;
1158         mod->user_data = user_data;
1159
1160         restore_model_state(mod);
1161
1162         return true;
1163 }
1164
1165 void mesh_model_app_key_delete(struct mesh_node *node, struct l_queue *models,
1166                                                         uint16_t app_idx)
1167 {
1168         const struct l_queue_entry *entry = l_queue_get_entries(models);
1169
1170         for (; entry; entry = entry->next) {
1171                 struct mesh_model *model = entry->data;
1172
1173                 model_unbind_idx(node, model, app_idx);
1174         }
1175 }
1176
1177 int mesh_model_binding_del(struct mesh_node *node, uint16_t addr, uint32_t id,
1178                                                 uint16_t app_idx)
1179 {
1180         l_debug("0x%x, 0x%x, %d", addr, id, app_idx);
1181         return update_binding(node, addr, id, app_idx, true);
1182 }
1183
1184 int mesh_model_binding_add(struct mesh_node *node, uint16_t addr, uint32_t id,
1185                                                 uint16_t app_idx)
1186 {
1187         l_debug("0x%x, 0x%x, %d", addr, id, app_idx);
1188         return update_binding(node, addr, id, app_idx, false);
1189 }
1190
1191 int mesh_model_get_bindings(struct mesh_node *node, uint16_t addr, uint32_t id,
1192                                 uint8_t *buf, uint16_t buf_size, uint16_t *size)
1193 {
1194         int status;
1195         struct mesh_model *mod;
1196         const struct l_queue_entry *entry;
1197         uint16_t n;
1198         uint32_t idx_pair;
1199         int i;
1200
1201         mod = find_model(node, addr, id, &status);
1202
1203         if (!mod) {
1204                 *size = 0;
1205                 return status;
1206         }
1207
1208         entry = l_queue_get_entries(mod->bindings);
1209         n = 0;
1210         i = 0;
1211         idx_pair = 0;
1212
1213         for (; entry; entry = entry->next) {
1214                 uint16_t app_idx = (uint16_t) (L_PTR_TO_UINT(entry->data));
1215
1216                 if (!(i & 0x1)) {
1217                         idx_pair = app_idx;
1218                 } else {
1219                         idx_pair <<= 12;
1220                         idx_pair += app_idx;
1221
1222                         /* Unlikely, but check for overflow*/
1223                         if ((n + 3) > buf_size) {
1224                                 l_warn("Binding list too large");
1225                                 goto done;
1226                         }
1227
1228                         l_put_le32(idx_pair, buf);
1229                         buf += 3;
1230                         n += 3;
1231                 }
1232
1233                 i++;
1234         }
1235
1236         /* Process the last app key if present */
1237         if (i & 0x1 && ((n + 2) <= buf_size)) {
1238                 l_put_le16(idx_pair, buf);
1239                 n += 2;
1240         }
1241
1242 done:
1243         *size = n;
1244         return MESH_STATUS_SUCCESS;
1245 }
1246
1247 int mesh_model_sub_get(struct mesh_node *node, uint16_t addr, uint32_t id,
1248                         uint8_t *buf, uint16_t buf_size, uint16_t *size)
1249 {
1250         int status;
1251         int16_t n;
1252         struct mesh_model *mod;
1253         const struct l_queue_entry *entry;
1254
1255         mod = find_model(node, addr, id, &status);
1256         if (!mod)
1257                 return status;
1258
1259         entry = l_queue_get_entries(mod->subs);
1260         *size = 0;
1261         n = 0;
1262
1263         for (; entry; entry = entry->next) {
1264                 if ((n + 2) > buf_size)
1265                         return MESH_STATUS_UNSPECIFIED_ERROR;
1266
1267                 l_put_le16((uint16_t) L_PTR_TO_UINT(entry->data), buf);
1268                 buf += 2;
1269                 n += 2;
1270         }
1271
1272         *size = n;
1273         return MESH_STATUS_SUCCESS;
1274 }
1275
1276 int mesh_model_sub_add(struct mesh_node *node, uint16_t addr, uint32_t id,
1277                         const uint8_t *group, bool b_virt, uint16_t *dst)
1278 {
1279         int status;
1280         struct mesh_model *mod;
1281
1282         mod = find_model(node, addr, id, &status);
1283         if (!mod)
1284                 return status;
1285
1286         return add_sub(node_get_net(node), mod, group, b_virt, dst);
1287 }
1288
1289 int mesh_model_sub_ovr(struct mesh_node *node, uint16_t addr, uint32_t id,
1290                         const uint8_t *group, bool b_virt, uint16_t *dst)
1291 {
1292         int status;
1293         struct l_queue *virtuals, *subs;
1294         struct mesh_virtual *virt;
1295         struct mesh_model *mod;
1296
1297         mod = find_model(node, addr, id, &status);
1298         if (!mod)
1299                 return status;
1300
1301         subs = mod->subs;
1302         virtuals = mod->virtuals;
1303         mod->subs = l_queue_new();
1304         mod->virtuals = l_queue_new();
1305
1306         if (!mod->subs || !mod->virtuals)
1307                 return MESH_STATUS_INSUFF_RESOURCES;
1308
1309         /*
1310          * When overwriting the Subscription List,
1311          * make sure any virtual Publication address is preserved
1312          */
1313         if (mod->pub && mod->pub->addr >= VIRTUAL_BASE) {
1314                 virt = l_queue_find(virtuals, find_virt_by_id,
1315                                 L_UINT_TO_PTR(mod->pub->addr));
1316                 if (virt) {
1317                         virt->ref_cnt++;
1318                         l_queue_push_head(mod->virtuals, virt);
1319                 }
1320         }
1321
1322         status = mesh_model_sub_add(node, addr, id, group, b_virt, dst);
1323
1324         if (status != MESH_STATUS_SUCCESS) {
1325                 /* Adding new group failed, so revert to old lists */
1326                 l_queue_destroy(mod->subs, NULL);
1327                 mod->subs = subs;
1328                 l_queue_destroy(mod->virtuals, unref_virt);
1329                 mod->virtuals = virtuals;
1330         } else {
1331                 const struct l_queue_entry *entry;
1332                 struct mesh_net *net = node_get_net(node);
1333
1334                 entry = l_queue_get_entries(subs);
1335
1336                 for (; entry; entry = entry->next)
1337                         mesh_net_dst_unreg(net,
1338                                         (uint16_t) L_PTR_TO_UINT(entry->data));
1339
1340                 /* Destroy old lists */
1341                 l_queue_destroy(subs, NULL);
1342                 l_queue_destroy(virtuals, unref_virt);
1343         }
1344
1345         return status;
1346 }
1347
1348 int mesh_model_sub_del(struct mesh_node *node, uint16_t addr, uint32_t id,
1349                         const uint8_t *group, bool b_virt, uint16_t *dst)
1350 {
1351         int status;
1352         uint16_t grp;
1353         struct mesh_model *mod;
1354
1355         mod = find_model(node, addr, id, &status);
1356         if (!mod)
1357                 return status;
1358
1359         if (b_virt) {
1360                 struct mesh_virtual *virt;
1361
1362                 virt = l_queue_find(mod->virtuals, find_virt_by_label, group);
1363                 if (virt) {
1364                         l_queue_remove(mod->virtuals, virt);
1365                         grp = virt->addr;
1366                         unref_virt(virt);
1367                 } else {
1368                         if (!mesh_crypto_virtual_addr(group, &grp))
1369                                 return MESH_STATUS_STORAGE_FAIL;
1370                 }
1371         } else {
1372                 grp = l_get_le16(group);
1373         }
1374
1375         *dst = grp;
1376
1377         if (l_queue_remove(mod->subs, L_UINT_TO_PTR(grp)))
1378                 mesh_net_dst_unreg(node_get_net(node), grp);
1379
1380         return MESH_STATUS_SUCCESS;
1381 }
1382
1383 int mesh_model_sub_del_all(struct mesh_node *node, uint16_t addr, uint32_t id)
1384 {
1385         int status;
1386         struct mesh_model *mod;
1387         const struct l_queue_entry *entry;
1388         struct mesh_net *net = node_get_net(node);
1389
1390         mod = find_model(node, addr, id, &status);
1391         if (!mod)
1392                 return status;
1393
1394         entry = l_queue_get_entries(mod->subs);
1395
1396         for (; entry; entry = entry->next)
1397                 mesh_net_dst_unreg(net, (uint16_t) L_PTR_TO_UINT(entry->data));
1398
1399         l_queue_destroy(mod->subs, NULL);
1400         l_queue_destroy(mod->virtuals, unref_virt);
1401         mod->virtuals = l_queue_new();
1402
1403         return MESH_STATUS_SUCCESS;
1404 }
1405
1406 struct mesh_model *mesh_model_setup(struct mesh_node *node, uint8_t ele_idx,
1407                                                                 void *data)
1408 {
1409         struct mesh_db_model *db_mod = data;
1410         struct mesh_model *mod;
1411         struct mesh_net *net;
1412         struct mesh_db_pub *pub = db_mod->pub;
1413         uint32_t i;
1414
1415         if (db_mod->num_bindings > MAX_BINDINGS) {
1416                 l_warn("Binding list too long %u (max %u)",
1417                                         db_mod->num_bindings, MAX_BINDINGS);
1418                 return NULL;
1419         }
1420
1421         mod = model_new(ele_idx, db_mod->vendor ? db_mod->id :
1422                                                 db_mod->id | VENDOR_ID_MASK);
1423
1424         /* Implicitly bind config server model to device key */
1425         if (db_mod->id == CONFIG_SRV_MODEL) {
1426
1427                 if (ele_idx != PRIMARY_ELE_IDX)
1428                         return NULL;
1429
1430                 l_queue_push_head(mod->bindings,
1431                                         L_UINT_TO_PTR(APP_IDX_DEV_LOCAL));
1432                 return mod;
1433         }
1434
1435         if (db_mod->id == CONFIG_CLI_MODEL) {
1436                 l_queue_push_head(mod->bindings,
1437                                         L_UINT_TO_PTR(APP_IDX_DEV_LOCAL));
1438                 return mod;
1439         }
1440
1441         net = node_get_net(node);
1442
1443         /* Add application key bindings if present */
1444         if (db_mod->bindings) {
1445                 mod->bindings = l_queue_new();
1446                 for (i = 0; i < db_mod->num_bindings; i++)
1447                         model_bind_idx(node, mod, db_mod->bindings[i]);
1448         }
1449
1450         /* Add publication if present */
1451         if (pub && (pub->virt || !(IS_UNASSIGNED(pub->addr)))) {
1452                 uint8_t mod_addr[2];
1453                 uint8_t *pub_addr;
1454                 uint8_t retransmit = (pub->count << 5) +
1455                                                 (pub->interval / 50 - 1);
1456
1457                 /* Add publication */
1458                 l_put_le16(pub->addr, &mod_addr);
1459                 pub_addr = pub->virt ? pub->virt_addr : mod_addr;
1460
1461                 if (set_pub(mod, pub_addr, pub->idx, pub->credential, pub->ttl,
1462                         pub->period, retransmit, pub->virt, NULL) !=
1463                                                         MESH_STATUS_SUCCESS) {
1464                         mesh_model_free(mod);
1465                         return NULL;
1466                 }
1467         }
1468
1469         /* Add subscriptions if present */
1470         if (!db_mod->subs)
1471                 return mod;
1472
1473         for (i = 0; i < db_mod->num_subs; i++) {
1474                 uint16_t group;
1475                 uint8_t *src;
1476
1477                 /*
1478                  * To keep calculations for virtual label coherent,
1479                  * convert to little endian.
1480                  */
1481                 l_put_le16(db_mod->subs[i].src.addr, &group);
1482                 src = db_mod->subs[i].virt ? db_mod->subs[i].src.virt_addr :
1483                         (uint8_t *) &group;
1484
1485                 if (add_sub(net, mod, src, db_mod->subs[i].virt, NULL) !=
1486                                                         MESH_STATUS_SUCCESS) {
1487                         mesh_model_free(mod);
1488                         return NULL;
1489                 }
1490         }
1491
1492         return mod;
1493 }
1494
1495 uint16_t mesh_model_opcode_set(uint32_t opcode, uint8_t *buf)
1496 {
1497         if (opcode <= 0x7e) {
1498                 buf[0] = opcode;
1499                 return 1;
1500         }
1501
1502         if (opcode >= 0x8000 && opcode <= 0xbfff) {
1503                 l_put_be16(opcode, buf);
1504                 return 2;
1505         }
1506
1507         if (opcode >= 0xc00000 && opcode <= 0xffffff) {
1508                 buf[0] = (opcode >> 16) & 0xff;
1509                 l_put_be16(opcode, buf + 1);
1510                 return 3;
1511         }
1512
1513         l_debug("Illegal Opcode %x", opcode);
1514         return 0;
1515 }
1516
1517 bool mesh_model_opcode_get(const uint8_t *buf, uint16_t size,
1518                                         uint32_t *opcode, uint16_t *n)
1519 {
1520         if (!n || !opcode || size < 1)
1521                 return false;
1522
1523         switch (buf[0] & 0xc0) {
1524         case 0x00:
1525         case 0x40:
1526                 /* RFU */
1527                 if (buf[0] == 0x7f)
1528                         return false;
1529
1530                 *n = 1;
1531                 *opcode = buf[0];
1532                 break;
1533
1534         case 0x80:
1535                 if (size < 2)
1536                         return false;
1537
1538                 *n = 2;
1539                 *opcode = l_get_be16(buf);
1540                 break;
1541
1542         case 0xc0:
1543                 if (size < 3)
1544                         return false;
1545
1546                 *n = 3;
1547                 *opcode = l_get_be16(buf + 1);
1548                 *opcode |= buf[0] << 16;
1549                 break;
1550
1551         default:
1552                 print_packet("Bad", buf, size);
1553                 return false;
1554         }
1555
1556         return true;
1557 }
1558
1559 void model_build_config(void *model, void *msg_builder)
1560 {
1561         struct l_dbus_message_builder *builder = msg_builder;
1562         struct mesh_model *mod = model;
1563         uint16_t id;
1564
1565         if (is_internal(mod->id))
1566                 return;
1567
1568         if (!l_queue_length(mod->subs) && !l_queue_length(mod->virtuals) &&
1569                                 !mod->pub && !l_queue_length(mod->bindings))
1570                 return;
1571
1572         l_dbus_message_builder_enter_struct(builder, "qa{sv}");
1573
1574         /* Model id */
1575         id = mod->id & 0xffff;
1576         l_dbus_message_builder_append_basic(builder, 'q', &id);
1577
1578         l_dbus_message_builder_enter_array(builder, "{sv}");
1579
1580         /* For vendor models, add vendor id */
1581         if ((mod->id & VENDOR_ID_MASK) != VENDOR_ID_MASK) {
1582                 uint16_t vendor = mod->id >> 16;
1583                 dbus_append_dict_entry_basic(builder, "Vendor", "q", &vendor);
1584         }
1585
1586         /* Model bindings, if present */
1587         if (l_queue_length(mod->bindings))
1588                 append_dict_uint16_array(builder, mod->bindings, "Bindings");
1589
1590         /* Model periodic publication interval, if present */
1591         if (mod->pub) {
1592                 uint32_t period = pub_period_to_ms(mod->pub->period);
1593                 dbus_append_dict_entry_basic(builder, "PublicationPeriod", "u",
1594                                                                 &period);
1595         }
1596
1597         l_dbus_message_builder_leave_array(builder);
1598         l_dbus_message_builder_leave_struct(builder);
1599 }
1600
1601 void mesh_model_init(void)
1602 {
1603         mesh_virtuals = l_queue_new();
1604 }
1605
1606 void mesh_model_cleanup(void)
1607 {
1608         l_queue_destroy(mesh_virtuals, l_free);
1609         mesh_virtuals = NULL;
1610 }