[PATCH BlueZ v2 07/11] meshd: Source code for handling access layer mux/demux

[Brian Gix <brian.gix@intel.com>]

From: Inga Stotland <inga.stotland@intel.com>

---
 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