mesh: Source files for mesh access layer and utilities

This adds initial implementation of BT Mesh access layer
functionality plus utilities.

1 files changed, 536 insertions, 0 deletions

diff --git a/mesh/appkey.c b/mesh/appkey.c
new file mode 100644
index 000000000..2ddb1eb80
--- /dev/null
+++ b/mesh/appkey.c
@@ -0,0 +1,536 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2017-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 <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/crypto.h"
+#include "mesh/display.h"
+#include "mesh/model.h"
+#include "mesh/storage.h"
+#include "mesh/appkey.h"
+
+struct mesh_app_key {
+	struct l_queue *replay_cache;
+	uint16_t net_idx;
+	uint16_t app_idx;
+	uint8_t key[16];
+	uint8_t key_id;
+	uint8_t new_key[16];
+	uint8_t new_key_id;
+};
+
+struct mesh_msg {
+	uint32_t iv_index;
+	uint32_t seq;
+	uint16_t src;
+};
+
+struct mod_decrypt {
+	const uint8_t *data;
+	uint8_t *out;
+	struct mesh_app_key *key;
+	uint8_t *virt;
+	uint32_t seq;
+	uint32_t iv_idx;
+	uint16_t src;
+	uint16_t dst;
+	uint16_t idx;
+	uint16_t size;
+	uint16_t virt_size;
+	uint8_t key_id;
+	bool szmict;
+	bool decrypted;
+};
+
+static bool match_key_index(const void *a, const void *b)
+{
+	const struct mesh_app_key *key = a;
+	uint16_t idx = L_PTR_TO_UINT(b);
+
+	return key->app_idx == idx;
+}
+
+static bool match_replay_cache(const void *a, const void *b)
+{
+	const struct mesh_msg *msg = a;
+	uint16_t src = L_PTR_TO_UINT(b);
+
+	return src == msg->src;
+}
+
+static bool clean_old_iv_index(void *a, void *b)
+{
+	struct mesh_msg *msg = a;
+	uint32_t iv_index = L_PTR_TO_UINT(b);
+
+	if (iv_index < 2)
+		return false;
+
+	if (msg->iv_index < iv_index - 1) {
+		l_free(msg);
+		return true;
+	}
+
+	return false;
+}
+
+static void packet_decrypt(void *a, void *b)
+{
+	struct mesh_app_key *key = a;
+	struct mod_decrypt *dec = b;
+
+	l_debug("model.c - app_packet_decrypt");
+	if (dec->decrypted)
+		return;
+
+	if (key->key_id != dec->key_id &&
+			key->new_key_id != dec->key_id)
+		return;
+
+	dec->key = key;
+
+	if (key->key_id == dec->key_id) {
+		dec->decrypted = mesh_crypto_payload_decrypt(dec->virt,
+				dec->virt_size, dec->data, dec->size,
+				dec->szmict, dec->src, dec->dst, dec->key_id,
+				dec->seq, dec->iv_idx, dec->out, key->key);
+		if (dec->decrypted)
+			print_packet("Used App Key", dec->key->key, 16);
+		else
+			print_packet("Failed with App Key", dec->key->key, 16);
+	}
+
+	if (!dec->decrypted && key->new_key_id == dec->key_id) {
+		dec->decrypted = mesh_crypto_payload_decrypt(dec->virt,
+				dec->virt_size, dec->data, dec->size,
+				dec->szmict, dec->src, dec->dst, dec->key_id,
+				dec->seq, dec->iv_idx, dec->out, key->new_key);
+		if (dec->decrypted)
+			print_packet("Used App Key", dec->key->new_key, 16);
+		else
+			print_packet("Failed with App Key",
+							dec->key->new_key, 16);
+	}
+
+	if (dec->decrypted)
+		dec->idx = key->app_idx;
+}
+
+int appkey_packet_decrypt(struct mesh_net *net, bool szmict, uint32_t seq,
+				uint32_t iv_index, uint16_t src,
+				uint16_t dst, uint8_t *virt, uint16_t virt_size,
+				uint8_t key_id, const uint8_t *data,
+				uint16_t data_size, uint8_t *out)
+{
+	struct l_queue *app_keys;
+
+	struct mod_decrypt decrypt = {
+		.src = src,
+		.dst = dst,
+		.seq = seq,
+		.data = data,
+		.out = out,
+		.size = data_size,
+		.key_id = key_id,
+		.iv_idx = iv_index,
+		.virt = virt,
+		.virt_size = virt_size,
+		.szmict = szmict,
+		.decrypted = false,
+	};
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return -1;
+
+	l_queue_foreach(app_keys, packet_decrypt, &decrypt);
+
+	return decrypt.decrypted ? decrypt.idx : -1;
+}
+
+bool appkey_msg_in_replay_cache(struct mesh_net *net, uint16_t idx,
+				uint16_t src, uint16_t crpl, uint32_t seq,
+				uint32_t iv_index)
+{
+	struct mesh_app_key *key;
+	struct mesh_msg *msg;
+	struct l_queue *app_keys;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return false;
+
+	l_debug("Test Replay src: %4.4x seq: %6.6x iv: %8.8x",
+						src, seq, iv_index);
+
+	key = l_queue_find(app_keys, match_key_index, L_UINT_TO_PTR(idx));
+
+	if (!key)
+		return false;
+
+	msg = l_queue_find(key->replay_cache, match_replay_cache,
+						L_UINT_TO_PTR(src));
+
+	if (msg) {
+		if (iv_index > msg->iv_index) {
+			msg->seq = seq;
+			msg->iv_index = iv_index;
+			return false;
+		}
+
+		if (seq < msg->seq) {
+			l_info("Ignoring packet with lower sequence number");
+			return true;
+		}
+
+		if (seq == msg->seq) {
+			l_info("Message already processed (duplicate)");
+			return true;
+		}
+
+		msg->seq = seq;
+
+		return false;
+	}
+
+	l_debug("New Entry for %4.4x", src);
+	if (key->replay_cache == NULL)
+		key->replay_cache = l_queue_new();
+
+	/* Replay Cache is fixed sized */
+	if (l_queue_length(key->replay_cache) >= crpl) {
+		int ret = l_queue_foreach_remove(key->replay_cache,
+				clean_old_iv_index, L_UINT_TO_PTR(iv_index));
+
+		if (!ret)
+			return true;
+	}
+
+	msg = l_new(struct mesh_msg, 1);
+	msg->src = src;
+	msg->seq = seq;
+	msg->iv_index = iv_index;
+	l_queue_push_head(key->replay_cache, msg);
+
+	return false;
+}
+
+static struct mesh_app_key *app_key_new(void)
+{
+	struct mesh_app_key *key = l_new(struct mesh_app_key, 1);
+
+	key->new_key_id = 0xFF;
+	key->replay_cache = l_queue_new();
+	return key;
+}
+
+static bool set_key(struct mesh_app_key *key, uint16_t app_idx,
+			const uint8_t *key_value, bool is_new)
+{
+	uint8_t key_id;
+
+	if (!mesh_crypto_k4(key_value, &key_id))
+		return false;
+
+	key_id = KEY_ID_AKF | (key_id << KEY_AID_SHIFT);
+	if (!is_new)
+		key->key_id = key_id;
+	else
+		key->new_key_id = key_id;
+
+	memcpy(is_new ? key->new_key : key->key, key_value, 16);
+
+	return true;
+}
+
+void appkey_key_free(void *data)
+{
+	struct mesh_app_key *key = data;
+
+	if (!key)
+		return;
+
+	l_queue_destroy(key->replay_cache, l_free);
+	l_free(key);
+}
+
+bool appkey_key_init(struct mesh_net *net, uint16_t net_idx, uint16_t app_idx,
+				uint8_t *key_value, uint8_t *new_key_value)
+{
+	struct mesh_app_key *key;
+	struct l_queue *app_keys;
+
+	if (net_idx > MAX_KEY_IDX || app_idx > MAX_KEY_IDX)
+		return false;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return NULL;
+
+	key = app_key_new();
+	if (!key)
+		return false;
+
+	if (!mesh_net_have_key(net, net_idx))
+		return false;
+
+	key->net_idx = net_idx;
+
+	if (key_value && !set_key(key, app_idx, key_value, false))
+		return false;
+
+	if (new_key_value && !set_key(key, app_idx, new_key_value, true))
+		return false;
+
+	l_queue_push_tail(app_keys, key);
+
+	return true;
+}
+
+const uint8_t *appkey_get_key(struct mesh_net *net, uint16_t app_idx,
+							uint8_t *key_id)
+{
+	struct mesh_app_key *app_key;
+	uint8_t phase;
+	struct l_queue *app_keys;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return NULL;
+
+	app_key = l_queue_find(app_keys, match_key_index,
+							L_UINT_TO_PTR(app_idx));
+	if (!app_key)
+		return NULL;
+
+	if (mesh_net_key_refresh_phase_get(net, app_key->net_idx, &phase) !=
+							MESH_STATUS_SUCCESS)
+		return NULL;
+
+	if (phase != KEY_REFRESH_PHASE_TWO) {
+		*key_id = app_key->key_id;
+		return app_key->key;
+	}
+
+	if (app_key->new_key_id == NET_NID_INVALID)
+		return NULL;
+
+	*key_id = app_key->new_key_id;
+	return app_key->new_key;
+}
+
+bool appkey_have_key(struct mesh_net *net, uint16_t app_idx)
+{
+	struct mesh_app_key *key;
+	struct l_queue *app_keys;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return false;
+
+	key = l_queue_find(app_keys, match_key_index, L_UINT_TO_PTR(app_idx));
+
+	if (!key)
+		return false;
+	else
+		return true;
+}
+
+int appkey_key_add(struct mesh_net *net, uint16_t net_idx, uint16_t app_idx,
+					const uint8_t *new_key, bool update)
+{
+	struct mesh_app_key *key;
+	struct l_queue *app_keys;
+	uint8_t phase = KEY_REFRESH_PHASE_NONE;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return MESH_STATUS_INSUFF_RESOURCES;
+
+	key = l_queue_find(app_keys, match_key_index, L_UINT_TO_PTR(app_idx));
+
+	if (!mesh_net_have_key(net, net_idx) ||
+					(update && key->net_idx != net_idx))
+		return MESH_STATUS_INVALID_NETKEY;
+
+	if (update && !key)
+		return MESH_STATUS_INVALID_APPKEY;
+
+	mesh_net_key_refresh_phase_get(net, net_idx, &phase);
+	if (update && phase != KEY_REFRESH_PHASE_ONE)
+		return MESH_STATUS_CANNOT_UPDATE;
+
+	if (key) {
+		if (memcmp(new_key, key->key, 16) == 0)
+			return MESH_STATUS_SUCCESS;
+
+		if (!update) {
+			l_info("Failed to add key: index already stored %x",
+				(net_idx << 16) | app_idx);
+			return MESH_STATUS_IDX_ALREADY_STORED;
+		}
+	}
+
+	if (!key) {
+		if (l_queue_length(app_keys) <= MAX_APP_KEYS)
+			return MESH_STATUS_INSUFF_RESOURCES;
+
+		key = app_key_new();
+		if (!key)
+			return MESH_STATUS_INSUFF_RESOURCES;
+
+		if (!set_key(key, app_idx, new_key, false)) {
+			appkey_key_free(key);
+			return MESH_STATUS_INSUFF_RESOURCES;
+		}
+
+		if (!storage_local_app_key_add(net, net_idx, app_idx, new_key,
+								false)) {
+			appkey_key_free(key);
+			return MESH_STATUS_STORAGE_FAIL;
+		}
+
+		key->net_idx = net_idx;
+		key->app_idx = app_idx;
+		l_queue_push_tail(app_keys, key);
+	} else {
+		if (!set_key(key, app_idx, new_key, true))
+			return MESH_STATUS_INSUFF_RESOURCES;
+
+		if (!storage_local_app_key_add(net, net_idx, app_idx, new_key,
+								true))
+			return MESH_STATUS_STORAGE_FAIL;
+	}
+
+	l_queue_clear(key->replay_cache, l_free);
+
+	return MESH_STATUS_SUCCESS;
+}
+
+int appkey_key_delete(struct mesh_net *net, uint16_t net_idx,
+							uint16_t app_idx)
+{
+	struct mesh_app_key *key;
+	struct l_queue *app_keys;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return MESH_STATUS_INVALID_APPKEY;
+
+	key = l_queue_find(app_keys, match_key_index, L_UINT_TO_PTR(app_idx));
+
+	if (!key)
+		return MESH_STATUS_INVALID_APPKEY;
+
+	if (key->net_idx != net_idx)
+		return MESH_STATUS_INVALID_NETKEY;
+
+	node_app_key_delete(net, mesh_net_get_address(net), net_idx, app_idx);
+
+	l_queue_remove(app_keys, key);
+	appkey_key_free(key);
+
+	if (!storage_local_app_key_del(net, net_idx, app_idx))
+		return MESH_STATUS_STORAGE_FAIL;
+
+	return MESH_STATUS_SUCCESS;
+}
+
+void appkey_delete_bound_keys(struct mesh_net *net, uint16_t net_idx)
+{
+	const struct l_queue_entry *entry;
+	struct l_queue *app_keys;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys)
+		return;
+
+	entry = l_queue_get_entries(app_keys);
+
+	for (; entry; entry = entry->next) {
+		struct mesh_app_key *key = entry->data;
+
+		appkey_key_delete(net, net_idx, key->app_idx);
+	}
+}
+
+uint8_t appkey_list(struct mesh_net *net, uint16_t net_idx, uint8_t *buf,
+					uint16_t buf_size, uint16_t *size)
+{
+	const struct l_queue_entry *entry;
+	uint32_t idx_pair;
+	int i;
+	uint16_t datalen;
+	struct l_queue *app_keys;
+
+	*size = 0;
+
+	app_keys = mesh_net_get_app_keys(net);
+	if (!app_keys || l_queue_isempty(app_keys))
+		return MESH_STATUS_SUCCESS;
+
+	idx_pair = 0;
+	i = 0;
+	datalen = 0;
+	entry = l_queue_get_entries(app_keys);
+
+	for (; entry; entry = entry->next) {
+		struct mesh_app_key *key = entry->data;
+
+		if (net_idx != key->net_idx)
+			continue;
+
+		if (!(i & 0x1)) {
+			idx_pair = key->app_idx;
+		} else {
+			idx_pair <<= 12;
+			idx_pair += key->app_idx;
+			/* Unlikely, but check for overflow*/
+			if ((datalen + 3) > buf_size) {
+				l_warn("Appkey list too large");
+				goto done;
+			}
+			l_put_le32(idx_pair, buf);
+			buf += 3;
+			datalen += 3;
+		}
+		i++;
+	}
+
+	/* Process the last app key if present */
+	if (i & 0x1 && ((datalen + 2) <= buf_size)) {
+		l_put_le16(idx_pair, buf);
+		datalen += 2;
+	}
+
+done:
+	*size = datalen;
+
+	return MESH_STATUS_SUCCESS;
+}