Return-Path: From: Brian Gix To: Marcel Holtmann , Luiz Augusto von Dentz , linux-bluetooth@vger.kernel.org Cc: Inga Stotland Subject: [PATCH BlueZ v4 10/14] meshd: Source code for handling access layer mux/demux Date: Mon, 30 Apr 2018 14:03:15 -0700 Message-Id: <20180430210319.25137-11-brian.gix@intel.com> In-Reply-To: <20180430210319.25137-1-brian.gix@intel.com> References: <20180430210319.25137-1-brian.gix@intel.com> List-ID: From: Inga Stotland --- meshd/src/model.c | 1274 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1274 insertions(+) create mode 100644 meshd/src/model.c diff --git a/meshd/src/model.c b/meshd/src/model.c new file mode 100644 index 000000000..0b87312c0 --- /dev/null +++ b/meshd/src/model.c @@ -0,0 +1,1274 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2018 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include + +#include "meshd/common/mesh-defs.h" + +#include "meshd/src/mesh.h" +#include "meshd/src/crypto.h" +#include "meshd/src/node.h" +#include "meshd/src/net.h" +#include "meshd/src/appkey.h" +#include "meshd/src/model.h" +#include "meshd/src/display.h" +#include "meshd/src/cfgmod.h" +#include "meshd/src/storage.h" + +struct mesh_model { + const struct mesh_model_ops *cbs; + void *user_data; + struct l_queue *bindings; + struct l_queue *subs; + struct l_queue *virtuals; + struct mesh_model_pub *pub; + uint32_t id; + uint8_t ele_idx; +}; + +struct mesh_virtual { + uint32_t id; /*Identifier of internally stored addr, min val 0x10000 */ + uint16_t ota; + uint16_t ref_cnt; + uint8_t addr[16]; +}; + +/* These struct is used to pass lots of params to l_queue_foreach */ +struct mod_forward { + struct mesh_virtual *virt; + const uint8_t *data; + uint16_t src; + uint16_t dst; + uint16_t unicast; + uint16_t idx; + uint16_t size; + uint8_t ttl; + int8_t rssi; + bool szmict; + bool done; +}; + +static struct l_queue *mesh_virtuals; + +static uint32_t virt_id_next = VIRTUAL_BASE; +static struct timeval tx_start; + +static void unref_virt(void *data) +{ + struct mesh_virtual *virt = data; + + if (virt->ref_cnt > 0) + virt->ref_cnt--; + + if (virt->ref_cnt) + return; + + l_queue_remove(mesh_virtuals, virt); + l_free(virt); +} + +static bool simple_match(const void *a, const void *b) +{ + return a == b; +} + +static bool find_virt_by_id(const void *a, const void *b) +{ + const struct mesh_virtual *virt = a; + uint32_t id = L_PTR_TO_UINT(b); + + return virt->id == id; +} + +static bool find_virt_by_addr(const void *a, const void *b) +{ + const struct mesh_virtual *virt = a; + const uint8_t *addr = b; + + return memcmp(virt->addr, addr, 16) == 0; +} + +static struct mesh_model *find_model(struct mesh_net *net, uint16_t addr, + uint32_t mod_id, int *fail) +{ + int ele_idx; + struct mesh_node *node; + + node = mesh_net_local_node_get(net); + + ele_idx = node_get_element_idx(node, addr); + + if (ele_idx < 0) { + if (fail) + *fail = MESH_STATUS_INVALID_ADDRESS; + return NULL; + } + + return node_get_model(node, (uint8_t) ele_idx, mod_id, fail); +} + +static void forward_model(void *a, void *b) +{ + struct mesh_model *mod = a; + struct mod_forward *fwd = b; + struct mesh_virtual *virt; + uint32_t dst; + bool has_dst = false; + + if (!mod->cbs || !mod->cbs->recv) + return; + + l_debug("model %8.8x with idx %3.3x", mod->id, fwd->idx); + if (fwd->idx != APP_IDX_DEV && + !l_queue_find(mod->bindings, simple_match, + L_UINT_TO_PTR(fwd->idx))) + return; + + dst = fwd->dst; + if (dst == fwd->unicast || IS_ALL_NODES(dst)) + has_dst = true; + else if (fwd->virt) { + virt = l_queue_find(mod->virtuals, simple_match, fwd->virt); + if (virt) { + /* + * Map Virtual addresses to a usable namespace that + * prevents us for forwarding a false positive + * (multiple Virtual Addresses that map to the same + * u16 OTA addr) + */ + has_dst = true; + dst = virt->id; + } + } else { + if (l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(dst))) + has_dst = true; + } + + + if (!has_dst) + return; + + /* + * TODO: models shall be registered with a list of supported opcodes and + * element address. Iterate through the list of opcodes to see if the + * model is an addressee. + * If this is an internal registered model, check for a "bind" callback. + * For an external ("user") model, send D-Bus method (signal?) (TBD) + */ + if (mod->cbs->recv) + mod->cbs->recv(fwd->src, dst, fwd->unicast, fwd->idx, + fwd->data, fwd->size, fwd->ttl, mod->user_data); + + if (dst == fwd->unicast) + fwd->done = true; +} + +static int dev_packet_decrypt(struct mesh_net *net, const uint8_t *data, + uint16_t size, bool szmict, uint16_t src, + uint16_t dst, uint8_t key_id, uint32_t seq, + uint32_t iv_idx, uint8_t *out) +{ + struct mesh_node *node; + const uint8_t *dev_key; + + node = mesh_net_local_node_get(net); + dev_key = node_get_device_key(node); + if (!dev_key) + return false; + + if (mesh_crypto_payload_decrypt(NULL, 0, data, size, szmict, src, + dst, key_id, seq, iv_idx, out, dev_key)) + return APP_IDX_DEV; + + return -1; +} + +static int virt_packet_decrypt(struct mesh_net *net, const uint8_t *data, + uint16_t size, bool szmict, uint16_t src, + uint16_t dst, uint8_t key_id, uint32_t seq, + uint32_t iv_idx, uint8_t *out, + struct mesh_virtual **decrypt_virt) +{ + const struct l_queue_entry *v; + + for (v = l_queue_get_entries(mesh_virtuals); v; v = v->next) { + struct mesh_virtual *virt = v->data; + int decrypt_idx; + + if (virt->ota != dst) + continue; + + decrypt_idx = appkey_packet_decrypt(net, szmict, seq, + iv_idx, src, dst, + virt->addr, 16, key_id, + data, size, out); + + if (decrypt_idx >= 0) { + *decrypt_virt = virt; + return decrypt_idx; + } + } + + return -1; +} + +static void cmplt(uint16_t remote, uint8_t status, + void *data, uint16_t size, + void *user_data) +{ + struct timeval tx_end; + + if (status) + l_info("Tx-->%4.4x (%d octets) Failed (%d)", + remote, size, status); + else + l_info("Tx-->%4.4x (%d octets) Succeeded", remote, size); + + /* print_packet("Sent Data", data, size); */ + + gettimeofday(&tx_end, NULL); + if (tx_end.tv_sec == tx_start.tv_sec) { + l_info("Duration 0.%zu seconds", + tx_end.tv_usec - tx_start.tv_usec); + } else { + if (tx_start.tv_usec > tx_end.tv_usec) + l_info("Duration %zu.%zu seconds", + tx_end.tv_sec - tx_start.tv_sec - 1, + tx_end.tv_usec + 1000000 - tx_start.tv_usec); + else + l_info("Duration %zu.%zu seconds", + tx_end.tv_sec - tx_start.tv_sec, + tx_end.tv_usec - tx_start.tv_usec); + } +} + +static bool pub_frnd_cred(struct mesh_net *net, uint16_t src, uint32_t mod_id) +{ + struct mesh_model *mod = find_model(net, src, mod_id, NULL); + + if (!mod || !mod->pub) + return false; + + return (mod->pub->credential != 0); +} + +static unsigned int msg_send(struct mesh_net *net, uint32_t mod_id, + uint16_t src, uint32_t dst, + uint8_t key_id, const uint8_t *key, + uint8_t *aad, uint8_t ttl, + const void *msg, uint16_t msg_len) +{ + unsigned int ret = 0; + uint32_t iv_index, seq_num; + uint8_t *out; + bool szmic = false; + uint16_t out_len = msg_len + sizeof(uint32_t); + + /* Use large MIC if it doesn't affect segmentation */ + if (msg_len > 11 && msg_len <= 376) { + if ((out_len / 12) == ((out_len + 4) / 12)) { + szmic = true; + out_len = msg_len + sizeof(uint64_t); + } + } + + out = l_malloc(out_len); + + iv_index = mesh_net_get_iv_index(net); + + seq_num = mesh_net_get_seq_num(net); + if (!mesh_crypto_payload_encrypt(aad, msg, out, msg_len, + src, dst, key_id, + seq_num, iv_index, + szmic, key)) { + l_error("Failed to Encrypt Payload"); + goto done; + } + + /* print_packet("Encrypted with", key, 16); */ + + ret = mesh_net_app_send(net, pub_frnd_cred(net, src, mod_id), + src, dst, key_id, ttl, + seq_num, iv_index, + szmic, + out, out_len, + cmplt, NULL); +done: + l_free(out); + return ret; +} + +static void model_unbind_idx(void *a, void *b) +{ + struct mesh_model *mod = a; + uint16_t idx = L_PTR_TO_UINT(b); + + if (idx == mod->pub->idx) { + mod->pub->addr = UNASSIGNED_ADDRESS; + /* + * TODO: callback for internal model or + * D-Bus signal/method "model publication changed" (TBD) + */ + } + + l_queue_remove(mod->bindings, b); + + if (mod->cbs->bind) + mod->cbs->bind(idx, ACTION_DELETE); +} + +static int model_bind_idx(struct mesh_model *mod, uint16_t idx) +{ + if (l_queue_length(mod->bindings) >= MAX_BINDINGS) + return MESH_STATUS_INSUFF_RESOURCES; + + if (!l_queue_push_tail(mod->bindings, L_UINT_TO_PTR(idx))) + return MESH_STATUS_INSUFF_RESOURCES; + + if (mod->cbs->bind) + mod->cbs->bind(idx, ACTION_ADD); + + return MESH_STATUS_SUCCESS; + +} + +static int update_binding(struct mesh_net *net, uint16_t addr, uint32_t id, + uint16_t app_idx, bool unbind) +{ + int fail; + struct mesh_model *mod; + int status; + + mod = find_model(net, addr, id, &fail); + if (!mod) { + l_info("model not found"); + return fail; + } + + if (id == CONFIG_SRV_MODEL || id == CONFIG_CLI_MODEL) + return MESH_STATUS_INVALID_MODEL; + + if (!l_queue_find(mod->bindings, simple_match, L_UINT_TO_PTR(app_idx))) + return MESH_STATUS_CANNOT_BIND; + + if (!appkey_have_key(net, app_idx)) + return MESH_STATUS_INVALID_APPKEY; + + if (unbind) { + model_unbind_idx(mod, &app_idx); + + if (!storage_model_bind(net, addr, id, app_idx, true)) + return MESH_STATUS_STORAGE_FAIL; + + return MESH_STATUS_SUCCESS; + } + + status = model_bind_idx(mod, app_idx); + if (status != MESH_STATUS_SUCCESS) + return status; + + if (!storage_model_bind(net, addr, id, app_idx, false)) { + model_unbind_idx(mod, &app_idx); + return MESH_STATUS_STORAGE_FAIL; + } + + return MESH_STATUS_SUCCESS; + +} + +static int set_pub(struct mesh_model *mod, const uint8_t *mod_addr, + uint16_t idx, bool cred_flag, uint8_t ttl, + uint8_t period, uint8_t retransmit, bool b_virt, + uint16_t *dst) +{ + struct mesh_virtual *virt; + uint16_t grp; + + if (dst) { + if (b_virt) + *dst = 0; + else + *dst = l_get_le16(mod_addr); + } + + if (b_virt) { + if (!mesh_crypto_virtual_addr(mod_addr, &grp)) + return MESH_STATUS_STORAGE_FAIL; + } + + /* If old publication was Virtual, remove it */ + if (mod->pub && mod->pub->addr >= VIRTUAL_BASE) { + virt = l_queue_find(mod->virtuals, find_virt_by_id, + L_UINT_TO_PTR(mod->pub->addr)); + if (virt) { + l_queue_remove(mod->virtuals, virt); + unref_virt(virt); + } + } + + if (b_virt) { + virt = l_queue_find(mesh_virtuals, find_virt_by_addr, mod_addr); + if (!virt) { + virt = l_new(struct mesh_virtual, 1); + virt->id = virt_id_next++; + virt->ota = grp; + memcpy(virt->addr, mod_addr, sizeof(virt->addr)); + l_queue_push_head(mesh_virtuals, virt); + } else { + grp = virt->ota; + } + virt->ref_cnt++; + l_queue_push_head(mod->virtuals, virt); + mod->pub->addr = virt->id; + } else { + grp = l_get_le16(mod_addr); + mod->pub->addr = grp; + } + + if (dst) + *dst = grp; + + if (IS_UNASSIGNED(grp) && mod->pub) { + l_free(mod->pub); + mod->pub = NULL; + /* Remove publication if Pub Addr is 0x0000 */ + } else { + if (!mod->pub) + mod->pub = l_new(struct mesh_model_pub, 1); + if (!mod->pub) + return MESH_STATUS_STORAGE_FAIL; + + mod->pub->credential = cred_flag; + mod->pub->idx = idx; + mod->pub->ttl = ttl; + mod->pub->period = period; + mod->pub->retransmit = retransmit; + } + + return MESH_STATUS_SUCCESS; +} + +static int add_sub(struct mesh_net *net, struct mesh_model *mod, + const uint8_t *group, bool b_virt, uint16_t *dst) +{ + struct mesh_virtual *virt; + uint16_t grp; + + if (b_virt) { + virt = l_queue_find(mesh_virtuals, find_virt_by_addr, group); + if (!virt) { + if (!mesh_crypto_virtual_addr(group, &grp)) + return MESH_STATUS_STORAGE_FAIL; + + virt = l_new(struct mesh_virtual, 1); + virt->id = virt_id_next++; + virt->ota = grp; + memcpy(virt->addr, group, sizeof(virt->addr)); + if (!l_queue_push_head(mesh_virtuals, virt)) + return MESH_STATUS_STORAGE_FAIL; + } else { + grp = virt->ota; + } + virt->ref_cnt++; + l_queue_push_head(mod->virtuals, virt); + } else { + grp = l_get_le16(group); + } + + if (dst) + *dst = grp; + + if (!mod->subs) + mod->subs = l_queue_new(); + if (!mod->subs) + return MESH_STATUS_STORAGE_FAIL; + + if (l_queue_find(mod->subs, simple_match, L_UINT_TO_PTR(grp))) + /* Group already exists */ + return MESH_STATUS_SUCCESS; + + l_queue_push_tail(mod->subs, L_UINT_TO_PTR(grp)); + + l_info("Added %4.4x", grp); + if (net) + mesh_net_dst_reg(net, grp); + + return MESH_STATUS_SUCCESS; +} + +bool mesh_model_rx(struct mesh_net *net, bool szmict, uint32_t seq0, + uint32_t seq, uint32_t iv_index, uint8_t ttl, + uint16_t src, uint16_t dst, uint8_t key_id, + const uint8_t *data, uint16_t size) +{ + uint8_t *clear_text; + struct mod_forward forward = { + .src = src, + .dst = dst, + .data = NULL, + .size = size - (szmict ? 8 : 4), + .ttl = ttl, + .virt = NULL, + .done = false, + }; + + struct mesh_node *node; + uint8_t num_ele; + int decrypt_idx, i, ele_idx; + uint16_t addr; + struct mesh_virtual *decrypt_virt = NULL; + + l_debug("iv_index %8.8x key_id = %2.2x", iv_index, key_id); + if (!dst) + return false; + + node = mesh_net_local_node_get(net); + if (!node) + return false; + + ele_idx = node_get_element_idx(node, dst); + + if (dst < 0x8000 && ele_idx < 0) + /* Unicast and not addressed to us */ + return false; + + + clear_text = l_malloc(size); + if (!clear_text) + return false; + + forward.data = clear_text; + + /* + * The packet needs to be decoded by the correct key which + * is hinted by key_id, but is not necessarily definitive + */ + if (key_id == APP_ID_DEV || mesh_net_provisioner_mode_get(net)) + decrypt_idx = dev_packet_decrypt(net, data, size, szmict, src, + dst, key_id, seq0, iv_index, + clear_text); + else if ((dst & 0xc000) == 0x8000) + decrypt_idx = virt_packet_decrypt(net, data, size, szmict, src, + dst, key_id, seq0, + iv_index, clear_text, + &decrypt_virt); + else + decrypt_idx = appkey_packet_decrypt(net, szmict, seq0, + iv_index, src, dst, + NULL, 0, key_id, data, + size, clear_text); + + if (decrypt_idx < 0) { + l_error("model.c - Failed to decrypt application payload"); + forward.done = false; + goto done; + } + + /* print_packet("Clr Rx (pre-cache-check)", clear_text, size - 4); */ + + if (key_id != APP_ID_DEV) { + uint16_t crpl = mesh_net_get_crpl(net); + + if (appkey_msg_in_replay_cache(net, (uint16_t) decrypt_idx, src, + crpl, seq, iv_index)) { + forward.done = true; + goto done; + } + } + + print_packet("Clr Rx", clear_text, size - (szmict ? 8 : 4)); + + forward.virt = decrypt_virt; + forward.idx = decrypt_idx; + num_ele = node_get_num_elements(node); + addr = node_get_primary(node); + + if (!num_ele || IS_UNASSIGNED(addr)) + goto done; + + for (i = 0; i < num_ele; i++) { + struct l_queue *models; + + if (dst < 0x8000 && ele_idx != i) + continue; + + forward.unicast = addr + i; + models = node_get_element_models(node, i, NULL); + l_queue_foreach(models, forward_model, &forward); + + if (dst < 0x8000 && ele_idx == i) + break; + } +done: + l_free(clear_text); + return forward.done; +} + +unsigned int mesh_model_send(struct mesh_net *net, uint32_t mod_id, + uint16_t src, uint32_t target, + uint16_t app_idx, uint8_t ttl, + const void *msg, uint16_t msg_len) +{ + struct mesh_model *mod; + uint8_t *aad = NULL; + uint16_t dst; + uint8_t key_id; + const uint8_t *key; + + /* print_packet("Mod Tx", msg, msg_len); */ + + if (!net || msg_len > 380) + return 0; + + /* If SRC is 0, use the Primary Element */ + if (src == 0) + src = mesh_net_get_address(net); + + mod = find_model(net, src, mod_id, NULL); + if (!mod) { + l_info("model %x not found", mod_id); + return 0; + } + + gettimeofday(&tx_start, NULL); + + if (target == USE_PUB_VALUE) { + target = mod->pub->addr; + app_idx = mod->pub->idx; + } + + if (IS_UNASSIGNED(target)) + return 0; + + if (target >= VIRTUAL_BASE) { + struct mesh_virtual *virt = l_queue_find(mesh_virtuals, + find_virt_by_id, + L_UINT_TO_PTR(target)); + + if (!virt) + return 0; + + aad = virt->addr; + dst = virt->ota; + } else + dst = target; + + l_debug("dst=%x", dst); + if (app_idx == APP_IDX_DEV && mesh_net_provisioner_mode_get(net)) { + key = node_get_device_key(mesh_net_local_node_get(net)); + } else if (app_idx == APP_IDX_DEV) { + key = node_get_device_key(mesh_net_local_node_get(net)); + if (!key) + return 0; + + l_debug("(%x)", app_idx); + key_id = APP_ID_DEV; + } else { + key = appkey_get_key(net, app_idx, &key_id); + if (!key) { + l_debug("no app key for (%x)", app_idx); + return 0; + } + + l_debug("(%x) %p", app_idx, key); + l_debug("key_id %x", key_id); + } + + return msg_send(net, mod_id, src, dst, key_id, key, aad, ttl, + msg, msg_len); + +} + +int mesh_model_pub_set(struct mesh_net *net, uint16_t addr, uint32_t id, + const uint8_t *mod_addr, uint16_t idx, bool cred_flag, + uint8_t ttl, uint8_t period, uint8_t retransmit, + bool b_virt, uint16_t *dst) +{ + int fail = MESH_STATUS_SUCCESS; + int ele_idx = -1; + struct mesh_model *mod; + struct mesh_node *node; + + node = mesh_net_local_node_get(net); + if (node) + ele_idx = node_get_element_idx(node, addr); + + if (!node || ele_idx < 0) { + fail = MESH_STATUS_INVALID_ADDRESS; + return false; + } + + mod = node_get_model(node, (uint8_t) ele_idx, id, &fail); + if (!mod) + return fail; + + if (id == CONFIG_SRV_MODEL || id == CONFIG_CLI_MODEL) + return MESH_STATUS_INVALID_PUB_PARAM; + + if (!appkey_have_key(net, idx)) + return MESH_STATUS_INVALID_APPKEY; + + return set_pub(mod, mod_addr, idx, cred_flag, ttl, period, retransmit, + b_virt, dst); + /* + * TODO: Add standardized Publication Change notification to model + * definition + */ +} + +struct mesh_model_pub *mesh_model_pub_get(struct mesh_net *net, uint8_t ele_idx, + uint32_t mod_id, int *status) +{ + struct mesh_model *mod; + struct mesh_node *node = mesh_net_local_node_get(net); + + if (!node) { + *status = MESH_STATUS_INVALID_ADDRESS; + return NULL; + } + + mod = node_get_model(node, ele_idx, mod_id, status); + if (!mod) + return NULL; + + return mod->pub; +} + +uint32_t mesh_model_get_model_id(const struct mesh_model *model) +{ + if (!model) + return 0xffffffff; /* TODO: use define */ + return model->id; +} + +void mesh_model_free(void *data) +{ + struct mesh_model *mod = data; + + l_queue_destroy(mod->bindings, NULL); + l_queue_destroy(mod->subs, NULL); + l_queue_destroy(mod->virtuals, unref_virt); + l_free(mod->pub); + l_free(mod); +} + +struct mesh_model *mesh_model_new(uint8_t ele_idx, uint32_t id, bool vendor) +{ + struct mesh_model *mod = l_new(struct mesh_model, 1); + + if (!mod) + return NULL; + + if (vendor) + id |= VENDOR_ID_MASK; + + mod->id = id; + mod->ele_idx = ele_idx; + mod->virtuals = l_queue_new(); + if (!mod->virtuals) { + l_free(mod); + return NULL; + } + return mod; +} + +static void restore_model_state(void *data) +{ + struct mesh_model *mod = data; + const struct mesh_model_ops *cbs; + const struct l_queue_entry *b; + + cbs = mod->cbs; + + if (l_queue_isempty(mod->bindings) || !mod->cbs->bind) { + for (b = l_queue_get_entries(mod->bindings); b; b = b->next) { + if (cbs->bind(L_PTR_TO_UINT(b->data), ACTION_ADD) != + MESH_STATUS_SUCCESS) + break; + } + } + + if (mod->pub && cbs->pub) + cbs->pub(mod->pub); +} + +bool mesh_model_vendor_register(struct mesh_net *net, uint8_t ele_idx, + uint32_t mod_id, + const struct mesh_model_ops *cbs, + void *user_data) +{ + struct mesh_model *mod; + struct mesh_node *node; + + node = mesh_net_local_node_get(net); + if (!node) + return false; + + mod = node_get_model(node, ele_idx, mod_id, NULL); + if (!mod) + return false; + + mod->cbs = cbs; + mod->user_data = user_data; + + l_idle_oneshot(restore_model_state, mod, NULL); + + return true; +} + +bool mesh_model_register(struct mesh_net *net, uint8_t ele_idx, + uint32_t mod_id, + const struct mesh_model_ops *cbs, + void *user_data) +{ + uint32_t id = VENDOR_ID_MASK | mod_id; + + return mesh_model_vendor_register(net, ele_idx, id, cbs, user_data); +} + +void mesh_model_app_key_delete(struct mesh_net *net, struct l_queue *models, + uint16_t app_idx) +{ + l_queue_foreach(models, model_unbind_idx, L_UINT_TO_PTR(app_idx)); +} + +int mesh_model_binding_del(struct mesh_net *net, uint16_t addr, uint32_t id, + uint16_t app_idx) +{ + l_debug("0x%x, 0x%x, %d", addr, id, app_idx); + return update_binding(net, addr, id, app_idx, true); +} + +int mesh_model_binding_add(struct mesh_net *net, uint16_t addr, uint32_t id, + uint16_t app_idx) +{ + l_debug("0x%x, 0x%x, %d", addr, id, app_idx); + return update_binding(net, addr, id, app_idx, false); +} + +int mesh_model_get_bindings(struct mesh_net *net, uint16_t addr, uint32_t id, + uint8_t *buf, uint16_t buf_size, uint16_t *size) +{ + int fail; + struct mesh_model *mod; + const struct l_queue_entry *entry; + uint16_t n; + uint32_t idx_pair; + int i; + + mod = find_model(net, addr, id, &fail); + + if (!mod) { + *size = 0; + return fail; + } + + entry = l_queue_get_entries(mod->bindings); + n = 0; + i = 0; + idx_pair = 0; + + for (; entry; entry = entry->next) { + uint16_t app_idx = (uint16_t) (L_PTR_TO_UINT(entry->data)); + + if (!(i & 0x1)) { + idx_pair = app_idx; + } else { + idx_pair <<= 12; + idx_pair += app_idx; + /* Unlikely, but check for overflow*/ + if ((n + 3) > buf_size) { + l_warn("Binding list too large"); + goto done; + } + l_put_le32(idx_pair, buf); + buf += 3; + n += 3; + } + i++; + } + + /* Process the last app key if present */ + if (i & 0x1 && ((n + 2) <= buf_size)) { + l_put_le16(idx_pair, buf); + n += 2; + } + +done: + *size = n; + + return MESH_STATUS_SUCCESS; +} + +int mesh_model_sub_get(struct mesh_net *net, uint16_t addr, uint32_t id, + uint8_t *buf, uint16_t buf_size, uint16_t *size) +{ + int fail = MESH_STATUS_SUCCESS; + int16_t n; + struct mesh_model *mod; + const struct l_queue_entry *entry; + + mod = find_model(net, addr, id, &fail); + if (!mod) + return fail; + + entry = l_queue_get_entries(mod->subs); + *size = 0; + n = 0; + + for (; entry; entry = entry->next) { + if ((n + 2) > buf_size) + return MESH_STATUS_UNSPECIFIED_ERROR; + + l_put_le16((uint16_t) L_PTR_TO_UINT(entry->data), buf); + buf += 2; + n += 2; + } + + *size = n; + return MESH_STATUS_SUCCESS; +} + +int mesh_model_sub_add(struct mesh_net *net, uint16_t addr, uint32_t id, + const uint8_t *group, bool b_virt, uint16_t *dst) +{ + int fail = MESH_STATUS_SUCCESS; + int ele_idx = -1; + struct mesh_model *mod; + struct mesh_node *node; + + node = mesh_net_local_node_get(net); + if (node) + ele_idx = node_get_element_idx(node, addr); + + if (!node || ele_idx < 0) { + fail = MESH_STATUS_INVALID_ADDRESS; + return false; + } + + mod = node_get_model(node, (uint8_t) ele_idx, id, &fail); + if (!mod) + return fail; + + return add_sub(net, mod, group, b_virt, dst); + /* TODO: communicate to registered models that sub has changed */ +} + +int mesh_model_sub_ovr(struct mesh_net *net, uint16_t addr, uint32_t id, + const uint8_t *group, bool b_virt, uint16_t *dst) +{ + int fail = MESH_STATUS_SUCCESS; + struct l_queue *virtuals, *subs; + struct mesh_virtual *virt; + struct mesh_model *mod; + + mod = find_model(net, addr, id, &fail); + if (!mod) + return fail; + + subs = mod->subs; + virtuals = mod->virtuals; + mod->subs = l_queue_new(); + mod->virtuals = l_queue_new(); + + if (!mod->subs || !mod->virtuals) + return MESH_STATUS_INSUFF_RESOURCES; + + /* + * When overwriting the Subscription List, + * make sure any virtual Publication address is preserved + */ + if (mod->pub && mod->pub->addr >= VIRTUAL_BASE) { + virt = l_queue_find(virtuals, find_virt_by_id, + L_UINT_TO_PTR(mod->pub->addr)); + if (virt) { + virt->ref_cnt++; + l_queue_push_head(mod->virtuals, virt); + } + } + + fail = mesh_model_sub_add(net, addr, id, group, b_virt, dst); + + if (fail) { + /* Adding new group failed, so revert to old list */ + l_queue_destroy(mod->subs, NULL); + mod->subs = subs; + l_queue_destroy(mod->virtuals, unref_virt); + mod->virtuals = virtuals; + } else { + const struct l_queue_entry *entry; + + entry = l_queue_get_entries(subs); + for (; entry; entry = entry->next) + mesh_net_dst_unreg(net, + (uint16_t) L_PTR_TO_UINT(entry->data)); + + l_queue_destroy(subs, NULL); + l_queue_destroy(virtuals, unref_virt); + } + + return fail; +} + +int mesh_model_sub_del(struct mesh_net *net, uint16_t addr, uint32_t id, + const uint8_t *group, bool b_virt, uint16_t *dst) +{ + int fail = MESH_STATUS_SUCCESS; + uint16_t grp; + struct mesh_model *mod; + + mod = find_model(net, addr, id, &fail); + if (!mod) + return fail; + + if (b_virt) { + struct mesh_virtual *virt; + + virt = l_queue_find(mod->virtuals, find_virt_by_addr, group); + if (virt) { + l_queue_remove(mod->virtuals, virt); + grp = virt->ota; + unref_virt(virt); + } else { + if (!mesh_crypto_virtual_addr(group, &grp)) + return MESH_STATUS_STORAGE_FAIL; + } + } else { + grp = l_get_le16(group); + } + + *dst = grp; + + if (l_queue_remove(mod->subs, L_UINT_TO_PTR(grp))) + mesh_net_dst_unreg(net, grp); + + return MESH_STATUS_SUCCESS; +} + +int mesh_model_sub_del_all(struct mesh_net *net, uint16_t addr, uint32_t id) +{ + int fail = MESH_STATUS_SUCCESS; + struct mesh_model *mod; + const struct l_queue_entry *entry; + + mod = find_model(net, addr, id, &fail); + if (!mod) + return fail; + + entry = l_queue_get_entries(mod->subs); + for (; entry; entry = entry->next) + mesh_net_dst_unreg(net, (uint16_t) L_PTR_TO_UINT(entry->data)); + + l_queue_destroy(mod->subs, NULL); + l_queue_destroy(mod->virtuals, unref_virt); + mod->virtuals = l_queue_new(); + + return fail; +} + +struct mesh_model *mesh_model_init(struct mesh_net *net, uint8_t ele_idx, + struct mesh_db_model *db_mod) +{ + struct mesh_model *mod; + uint32_t i; + + mod = mesh_model_new(ele_idx, db_mod->id, db_mod->vendor); + if (!mod) + return NULL; + + /* Implicitly bind config server model to device key */ + if (db_mod->id == CONFIG_SRV_MODEL) { + + if (ele_idx != PRIMARY_ELE_IDX) + return NULL; + + l_queue_push_head(mod->bindings, L_UINT_TO_PTR(APP_IDX_DEV)); + return mod; + } + + if (db_mod->id == CONFIG_CLI_MODEL) { + l_queue_push_head(mod->bindings, L_UINT_TO_PTR(APP_IDX_DEV)); + return mod; + } + + /* Add application key bindings if present */ + if (db_mod->bindings) { + mod->bindings = l_queue_new(); + + if (!mod->bindings) { + mesh_model_free(mod); + return NULL; + } + + for (i = 0; i < db_mod->num_bindings; i++) { + if (!model_bind_idx(mod, db_mod->bindings[i])) { + mesh_model_free(mod); + return NULL; + } + } + } + + /* Add publication if present */ + if (db_mod->pub) { + uint16_t mod_addr; + uint8_t *dst; + + l_put_le16(db_mod->pub->addr, &mod_addr); + dst = db_mod->pub->virt ? db_mod->pub->virt_addr : + (uint8_t *) &mod_addr; + + if (set_pub(mod, dst, db_mod->pub->idx, db_mod->pub->credential, + db_mod->pub->ttl, db_mod->pub->period, + db_mod->pub->retransmit, db_mod->pub->virt, NULL) != + MESH_STATUS_SUCCESS) { + mesh_model_free(mod); + return NULL; + } + } + + /* Add subscriptions if present */ + if (!db_mod->subs) + return mod; + + for (i = 0; i < db_mod->num_subs; i++) { + uint16_t group; + uint8_t *src; + + /* + * To keep calculations for virtual label coherent, + * convert to little endian. + */ + l_put_le16(db_mod->subs[i].src.addr, &group); + src = db_mod->subs[i].virt ? db_mod->subs[i].src.virt_addr : + (uint8_t *) &group; + + if (add_sub(net, mod, src, db_mod->subs[i].virt, NULL) != + MESH_STATUS_SUCCESS) { + mesh_model_free(mod); + return NULL; + } + } + + return mod; +} + +uint16_t mesh_model_opcode_set(uint32_t opcode, uint8_t *buf) +{ + if (opcode <= 0x7e) { + buf[0] = opcode; + return 1; + } + + if (opcode >= 0x8000 && opcode <= 0xbfff) { + l_put_be16(opcode, buf); + return 2; + } + + if (opcode >= 0xc00000 && opcode <= 0xffffff) { + buf[0] = (opcode >> 16) & 0xff; + l_put_be16(opcode, buf + 1); + return 3; + } + + l_info("Illegal Opcode %x", opcode); + return 0; +} + +bool mesh_model_opcode_get(const uint8_t *buf, uint16_t size, + uint32_t *opcode, uint16_t *n) +{ + if (!n || !opcode || size < 1) + return false; + + switch (buf[0] & 0xc0) { + case 0x00: + case 0x40: + /* RFU */ + if (buf[0] == 0x7f) + return false; + + *n = 1; + *opcode = buf[0]; + break; + + case 0x80: + if (size < 2) + return false; + + *n = 2; + *opcode = l_get_be16(buf); + break; + + case 0xc0: + if (size < 3) + return false; + + *n = 3; + *opcode = l_get_be16(buf + 1); + *opcode |= buf[0] << 16; + break; + + default: + print_packet("Bad", buf, size); + return false; + } + + return true; +} + +void mesh_model_add_virtual(struct mesh_net *net, const uint8_t *v) +{ + struct mesh_virtual *virt = l_queue_find(mesh_virtuals, + find_virt_by_addr, v); + + if (virt) { + virt->ref_cnt++; + return; + } + + virt = l_new(struct mesh_virtual, 1); + if (!virt) + return; + + if (!mesh_crypto_virtual_addr(v, &virt->ota)) { + l_free(virt); + return; /* Storage Failure */ + } + + memcpy(virt->addr, v, 16); + virt->ref_cnt++; + virt->id = virt_id_next++; + l_queue_push_head(mesh_virtuals, virt); +} + +void mesh_model_del_virtual(struct mesh_net *net, uint32_t va24) +{ + struct mesh_virtual *virt = l_queue_remove_if(mesh_virtuals, + find_virt_by_id, + L_UINT_TO_PTR(va24)); + + if (virt) + unref_virt(virt); +} -- 2.14.3