From: Inga Stotland <inga.stotland@xxxxxxxxx>
---
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 <config.h>
+#endif
+
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#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
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
