bluez/mesh/appkey.c

/*
 *
 *  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;
}