Imported Upstream version 5.19upstream/5.19upstream

1 files changed, 144 insertions, 0 deletions

diff --git a/monitor/keys.c b/monitor/keys.c
new file mode 100644
index 00000000..4ccef22c
--- /dev/null
+++ b/monitor/keys.c
@@ -0,0 +1,144 @@
+/*
+ *
+ *  BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2011-2014  Intel Corporation
+ *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
+ *
+ *
+ *  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.
+ *
+ *  You should have received a copy of the GNU Lesser General Public
+ *  License along with this library; if not, write to the Free Software
+ *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <string.h>
+
+#include "src/shared/util.h"
+#include "src/shared/queue.h"
+#include "src/shared/crypto.h"
+
+#include "keys.h"
+
+static const uint8_t empty_key[16] = { 0x00, };
+static const uint8_t empty_addr[6] = { 0x00, };
+
+static struct bt_crypto *crypto;
+
+struct irk_data {
+	uint8_t key[16];
+	uint8_t addr[6];
+	uint8_t addr_type;
+};
+
+static struct queue *irk_list;
+
+void keys_setup(void)
+{
+	crypto = bt_crypto_new();
+
+	irk_list = queue_new();
+}
+
+void keys_cleanup(void)
+{
+	bt_crypto_unref(crypto);
+
+	queue_destroy(irk_list, free);
+}
+
+void keys_update_identity_key(const uint8_t key[16])
+{
+	struct irk_data *irk;
+
+	irk = queue_peek_tail(irk_list);
+	if (irk && !memcmp(irk->key, empty_key, 16)) {
+		memcpy(irk->key, key, 16);
+		return;
+	}
+
+	irk = new0(struct irk_data, 1);
+	if (irk) {
+		memcpy(irk->key, key, 16);
+		if (!queue_push_tail(irk_list, irk))
+			free(irk);
+	}
+}
+
+void keys_update_identity_addr(const uint8_t addr[6], uint8_t addr_type)
+{
+	struct irk_data *irk;
+
+	irk = queue_peek_tail(irk_list);
+	if (irk && !memcmp(irk->addr, empty_addr, 6)) {
+		memcpy(irk->addr, addr, 6);
+		irk->addr_type = addr_type;
+		return;
+	}
+
+	irk = new0(struct irk_data, 1);
+	if (irk) {
+		memcpy(irk->addr, addr, 6);
+		irk->addr_type = addr_type;
+		if (!queue_push_tail(irk_list, irk))
+			free(irk);
+	}
+}
+
+struct resolve_data {
+	bool found;
+	uint8_t addr[6];
+	uint8_t ident[6];
+	uint8_t ident_type;
+};
+
+static void try_resolve_irk(void *data, void *user_data)
+{
+	struct irk_data *irk = data;
+	struct resolve_data *result = user_data;
+	uint8_t local_hash[3];
+
+	if (result->found)
+		return;
+
+	bt_crypto_ah(crypto, irk->key, result->addr + 3, local_hash);
+
+	if (!memcmp(result->addr, local_hash, 3)) {
+		result->found = true;
+		memcpy(result->ident, irk->addr, 6);
+		result->ident_type = irk->addr_type;
+	}
+}
+
+bool keys_resolve_identity(const uint8_t addr[6], uint8_t ident[6],
+							uint8_t *ident_type)
+{
+	struct resolve_data result;
+
+	result.found = false;
+	memcpy(result.addr, addr, 6);
+
+	queue_foreach(irk_list, try_resolve_irk, &result);
+
+	if (result.found) {
+		memcpy(ident, result.ident, 6);
+		*ident_type = result.ident_type;
+		return true;
+	}
+
+	return false;
+}