---
Makefile.mesh | 1 +
mesh/net_keys.c | 326 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
mesh/net_keys.h | 33 ++++++
3 files changed, 360 insertions(+)
create mode 100644 mesh/net_keys.c
create mode 100644 mesh/net_keys.h
diff --git a/Makefile.mesh b/Makefile.mesh
index e93e68e38..0df7db2be 100644
--- a/Makefile.mesh
+++ b/Makefile.mesh
@@ -1,5 +1,6 @@
if MESH
mesh_sources = mesh/mesh.h mesh/mesh.c \
+ mesh/net_keys.h mesh/net_keys.c \
mesh/mesh-io.h mesh/mesh-io.c \
mesh/mesh-io-api.h \
mesh/mesh-io-generic.h \
diff --git a/mesh/net_keys.c b/mesh/net_keys.c
new file mode 100644
index 000000000..43c8a938b
--- /dev/null
+++ b/mesh/net_keys.c
@@ -0,0 +1,326 @@
+/*
+ *
+ * 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 <stdlib.h>
+#include <ell/ell.h>
+
+#include "mesh/crypto.h"
+#include "mesh/net_keys.h"
+
+#define BEACON_TYPE_SNB 0x01
+#define KEY_REFRESH 0x01
+#define IV_INDEX_UPDATE 0x02
+
+struct net_key {
+ uint32_t id;
+ uint16_t ref_cnt;
+ uint8_t friend_key;
+ uint8_t nid;
+ uint8_t master[16];
+ uint8_t encrypt[16];
+ uint8_t privacy[16];
+ uint8_t beacon[16];
+ uint8_t network[8];
+};
+
+static struct l_queue *keys = NULL;
+static uint32_t last_master_id = 0;
+
+/* To avoid re-decrypting same packet for multiple nodes, cache and check */
+static uint8_t cache_pkt[29];
+static uint8_t cache_plain[29];
+static size_t cache_len;
+static size_t cache_plainlen;
+static uint32_t cache_id;
+static uint32_t cache_iv_index;
+
+static bool match_master(const void *a, const void *b)
+{
+ const struct net_key *key = a;
+
+ return (memcmp(key->master, b, sizeof(key->master)) == 0);
+}
+
+static bool match_id(const void *a, const void *b)
+{
+ const struct net_key *key = a;
+ uint32_t id = L_PTR_TO_UINT(b);
+
+ return id == key->id;
+}
+
+static bool match_network(const void *a, const void *b)
+{
+ const struct net_key *key = a;
+ const uint8_t *network = b;
+
+ return memcmp(key->network, network, sizeof(key->network)) == 0;
+}
+
+/* Key added from Provisioning, NetKey Add or NetKey update */
+uint32_t net_key_add(const uint8_t master[16])
+{
+ struct net_key *key = l_queue_find(keys, match_master, master);
+ uint8_t p[] = {0};
+ bool result;
+
+ if (key) {
+ key->ref_cnt++;
+ return key->id;
+ }
+
+ if (!keys)
+ keys = l_queue_new();
+
+ key = l_new(struct net_key, 1);
+ memcpy(key->master, master, 16);
+ key->ref_cnt++;
+ result = mesh_crypto_k2(master, p, sizeof(p), &key->nid, key->encrypt,
+ key->privacy);
+ if (!result)
+ goto fail;
+
+ result = mesh_crypto_k3(master, key->network);
+ if (!result)
+ goto fail;
+
+ result = mesh_crypto_nkbk(master, key->beacon);
+ if (!result)
+ goto fail;
+
+ key->id = ++last_master_id;
+ l_queue_push_tail(keys, key);
+ return key->id;
+
+fail:
+ l_free(key);
+ return 0;
+}
+
+uint32_t net_key_frnd_add(uint32_t master_id, uint16_t lpn, uint16_t frnd,
+ uint16_t lp_cnt, uint16_t fn_cnt)
+{
+ const struct net_key *key = l_queue_find(keys, match_id,
+ L_UINT_TO_PTR(master_id));
+ struct net_key *frnd_key;
+ uint8_t p[9] = {0x01};
+ bool result;
+
+ if (!key || key->friend_key)
+ return 0;
+
+ frnd_key = l_new(struct net_key, 1);
+
+ l_put_be16(lpn, p + 1);
+ l_put_be16(frnd, p + 3);
+ l_put_be16(lp_cnt, p + 5);
+ l_put_be16(fn_cnt, p + 7);
+
+ result = mesh_crypto_k2(key->master, p, sizeof(p), &frnd_key->nid,
+ frnd_key->encrypt, frnd_key->privacy);
+
+ if (!result) {
+ l_free(frnd_key);
+ return 0;
+ }
+
+ frnd_key->friend_key = true;
+ frnd_key->ref_cnt++;
+ frnd_key->id = ++last_master_id;
+ l_queue_push_head(keys, frnd_key);
+
+ return frnd_key->id;
+}
+
+void net_key_unref(uint32_t id)
+{
+ struct net_key *key = l_queue_find(keys, match_id, L_UINT_TO_PTR(id));
+
+ if (key && key->ref_cnt) {
+ if (--key->ref_cnt == 0) {
+ l_queue_remove(keys, key);
+ l_free(key);
+ }
+ }
+}
+
+bool net_key_confirm(uint32_t id, const uint8_t *master)
+{
+ struct net_key *key = l_queue_find(keys, match_id, L_UINT_TO_PTR(id));
+
+ if (key)
+ return memcmp(key->master, master, sizeof(key->master)) == 0;
+
+ return false;
+}
+
+bool net_key_retrieve(uint32_t id, uint8_t *master)
+{
+ struct net_key *key = l_queue_find(keys, match_id, L_UINT_TO_PTR(id));
+
+ if (key) {
+ memcpy(master, key->master, sizeof(key->master));
+ return true;
+ }
+
+ return false;
+}
+
+static void decrypt_net_pkt(void *a, void *b)
+{
+ const struct net_key *key = a;
+ bool result;
+
+ if (cache_id || !key->ref_cnt || (cache_pkt[0] & 0x7f) != key->nid)
+ return;
+
+ result = mesh_crypto_packet_decode(cache_pkt, cache_len, false,
+ cache_plain, cache_iv_index,
+ key->encrypt, key->privacy);
+
+ if (result) {
+ cache_id = key->id;
+ if (cache_plain[1] & 0x80)
+ cache_plainlen = cache_len - 8;
+ else
+ cache_plainlen = cache_len - 4;
+ }
+}
+
+uint32_t net_key_decrypt(uint32_t iv_index, const uint8_t *pkt, size_t len,
+ uint8_t **plain, size_t *plain_len)
+{
+ bool iv_flag = !!(iv_index & 1);
+ bool iv_pkt = !!(pkt[0] & 0x80);
+
+ if (iv_pkt != iv_flag)
+ iv_index--;
+
+ if (cache_len == len && memcmp(pkt, cache_pkt, len) == 0)
+ goto done;
+
+ cache_id = 0;
+ memcpy(cache_pkt, pkt, len);
+ cache_len = len;
+ cache_iv_index = iv_index;
+
+ /* Try all network keys known to us */
+ l_queue_foreach(keys, decrypt_net_pkt, NULL);
+
+done:
+ if (cache_iv_index != iv_index)
+ return 0;
+
+ if (cache_id) {
+ *plain = cache_plain;
+ *plain_len = cache_plainlen;
+ }
+
+ return cache_id;
+}
+
+bool net_key_encrypt(uint32_t id, uint32_t iv_index, uint8_t *pkt, size_t len)
+{
+ struct net_key *key = l_queue_find(keys, match_id, L_UINT_TO_PTR(id));
+ bool result;
+
+ if (!key)
+ return false;
+
+ result = mesh_crypto_packet_encode(pkt, len, key->encrypt, iv_index,
+ key->privacy);
+
+ if (!result)
+ return false;
+
+ result = mesh_crypto_packet_label(pkt, len, iv_index, key->nid);
+
+ return result;
+}
+
+uint32_t net_key_network_id(const uint8_t network[8])
+{
+ struct net_key *key = l_queue_find(keys, match_network, network);
+
+ if (!key)
+ return 0;
+
+ return key->id;
+}
+
+bool net_key_snb_check(uint32_t id, uint32_t iv_index, bool kr, bool ivu,
+ uint64_t cmac)
+{
+ struct net_key *key = l_queue_find(keys, match_id, L_UINT_TO_PTR(id));
+ uint64_t cmac_check;
+
+ if (!key)
+ return false;
+
+ /* Any behavioral changes must pass CMAC test */
+ if (!mesh_crypto_beacon_cmac(key->beacon, key->network, iv_index, kr,
+ ivu, &cmac_check)) {
+ l_error("mesh_crypto_beacon_cmac failed");
+ return false;
+ }
+
+ if (cmac != cmac_check) {
+ l_error("cmac compare failed %16.16lx != %16.16lx",
+ cmac, cmac_check);
+ return false;
+ }
+
+ return true;
+}
+
+bool net_key_snb_compose(uint32_t id, uint32_t iv_index, bool kr, bool ivu,
+ uint8_t *snb)
+{
+ struct net_key *key = l_queue_find(keys, match_id, L_UINT_TO_PTR(id));
+ uint64_t cmac;
+
+ if (!key)
+ return false;
+
+ /* Any behavioral changes must pass CMAC test */
+ if (!mesh_crypto_beacon_cmac(key->beacon, key->network, iv_index, kr,
+ ivu, &cmac)) {
+ l_error("mesh_crypto_beacon_cmac failed");
+ return false;
+ }
+
+ snb[0] = BEACON_TYPE_SNB;
+ snb[1] = 0;
+
+ if (kr)
+ snb[1] |= KEY_REFRESH;
+
+ if (ivu)
+ snb[1] |= IV_INDEX_UPDATE;
+
+ memcpy(snb + 2, key->network, 8);
+ l_put_be32(iv_index, snb + 10);
+ l_put_be64(cmac, snb + 14);
+
+ return true;
+}
diff --git a/mesh/net_keys.h b/mesh/net_keys.h
new file mode 100644
index 000000000..c752d3991
--- /dev/null
+++ b/mesh/net_keys.h
@@ -0,0 +1,33 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+bool net_key_confirm(uint32_t id, const uint8_t master[16]);
+bool net_key_retrieve(uint32_t id, uint8_t *master);
+uint32_t net_key_add(const uint8_t master[16]);
+uint32_t net_key_frnd_add(uint32_t master_id, uint16_t lpn, uint16_t frnd,
+ uint16_t lp_cnt, uint16_t fn_cnt);
+void net_key_unref(uint32_t id);
+uint32_t net_key_decrypt(uint32_t iv_index, const uint8_t *pkt, size_t len,
+ uint8_t **plain, size_t *plain_len);
+bool net_key_encrypt(uint32_t id, uint32_t iv_index, uint8_t *pkt, size_t len);
+uint32_t net_key_network_id(const uint8_t network[8]);
+bool net_key_snb_check(uint32_t id, uint32_t iv_index, bool kr, bool ivu,
+ uint64_t cmac);
+bool net_key_snb_compose(uint32_t id, uint32_t iv_index, bool kr, bool ivu,
+ uint8_t *snb);
--
2.14.4
