* [PATCH BlueZ v7 01/14] mesh: Shared private meshd interfaces
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 02/14] mesh: Mesh crypto support Brian Gix
` (13 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/crypto.h | 163 ++++++++++++++++++++
mesh/display.h | 28 ++++
mesh/friend.h | 57 +++++++
mesh/mesh-io-api.h | 58 ++++++++
mesh/mesh-io-generic.h | 20 +++
mesh/mesh-io.h | 99 +++++++++++++
mesh/net.h | 392 +++++++++++++++++++++++++++++++++++++++++++++++++
mesh/prov.h | 162 ++++++++++++++++++++
mesh/provision.h | 30 ++++
9 files changed, 1009 insertions(+)
create mode 100644 mesh/crypto.h
create mode 100644 mesh/display.h
create mode 100644 mesh/friend.h
create mode 100644 mesh/mesh-io-api.h
create mode 100644 mesh/mesh-io-generic.h
create mode 100644 mesh/mesh-io.h
create mode 100644 mesh/net.h
create mode 100644 mesh/prov.h
create mode 100644 mesh/provision.h
diff --git a/mesh/crypto.h b/mesh/crypto.h
new file mode 100644
index 000000000..ffd312231
--- /dev/null
+++ b/mesh/crypto.h
@@ -0,0 +1,163 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+bool mesh_crypto_aes_ccm_encrypt(const uint8_t nonce[13], const uint8_t key[16],
+ const uint8_t *aad, uint16_t aad_len,
+ const uint8_t *msg, uint16_t msg_len,
+ uint8_t *out_msg,
+ void *out_mic, size_t mic_size);
+bool mesh_crypto_aes_ccm_decrypt(const uint8_t nonce[13], const uint8_t key[16],
+ const uint8_t *aad, uint16_t aad_len,
+ const uint8_t *enc_msg, uint16_t enc_msg_len,
+ uint8_t *out_msg,
+ void *out_mic, size_t mic_size);
+bool mesh_aes_ecb_one(const uint8_t key[16],
+ const uint8_t plaintext[16], uint8_t encrypted[16]);
+bool mesh_crypto_nkik(const uint8_t network_key[16], uint8_t identity_key[16]);
+bool mesh_crypto_nkbk(const uint8_t network_key[16], uint8_t beacon_key[16]);
+bool mesh_crypto_nkpk(const uint8_t network_key[16], uint8_t proxy_key[16]);
+bool mesh_crypto_identity(const uint8_t net_key[16], uint16_t addr,
+ uint8_t id[16]);
+bool mesh_crypto_beacon_cmac(const uint8_t encryption_key[16],
+ const uint8_t network_id[16],
+ uint32_t iv_index, bool kr,
+ bool iu, uint64_t *cmac);
+bool mesh_crypto_network_nonce(bool frnd, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint32_t iv_index,
+ uint8_t nonce[13]);
+bool mesh_crypto_network_encrypt(bool ctl, uint8_t ttl,
+ uint32_t seq, uint16_t src,
+ uint32_t iv_index,
+ const uint8_t net_key[16],
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *net_mic);
+bool mesh_crypto_network_decrypt(bool frnd, uint8_t ttl,
+ uint32_t seq, uint16_t src,
+ uint32_t iv_index,
+ const uint8_t net_key[16],
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *net_mic, size_t mic_size);
+bool mesh_crypto_application_nonce(uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ bool aszmic, uint8_t nonce[13]);
+bool mesh_crypto_device_nonce(uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ bool aszmic, uint8_t nonce[13]);
+bool mesh_crypto_application_encrypt(uint8_t akf, uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ const uint8_t app_key[16],
+ const uint8_t *aad, uint8_t aad_len,
+ const uint8_t *msg, uint8_t msg_len,
+ uint8_t *out,
+ void *app_mic, size_t mic_size);
+bool mesh_crypto_application_decrypt(uint8_t akf, uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ const uint8_t app_key[16],
+ const uint8_t *aad, uint8_t aad_len,
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *app_mic, size_t mic_size);
+bool mesh_crypto_device_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t device_key[16]);
+bool mesh_crypto_virtual_addr(const uint8_t virtual_label[16],
+ uint16_t *v_addr);
+bool mesh_crypto_nonce(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t nonce[13]);
+bool mesh_crypto_k1(const uint8_t ikm[16], const uint8_t salt[16],
+ const void *info, size_t info_len, uint8_t okm[16]);
+bool mesh_crypto_k2(const uint8_t n[16], const uint8_t *p, size_t p_len,
+ uint8_t net_id[1],
+ uint8_t enc_key[16],
+ uint8_t priv_key[16]);
+bool mesh_crypto_k3(const uint8_t n[16], uint8_t out64[8]);
+bool mesh_crypto_k4(const uint8_t a[16], uint8_t out5[1]);
+bool mesh_crypto_s1(const void *info, size_t len, uint8_t salt[16]);
+bool mesh_crypto_prov_prov_salt(const uint8_t conf_salt[16],
+ const uint8_t prov_rand[16],
+ const uint8_t dev_rand[16],
+ uint8_t prov_salt[16]);
+bool mesh_crypto_prov_conf_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t conf_key[16]);
+bool mesh_crypto_session_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t session_key[16]);
+bool mesh_crypto_privacy_counter(uint32_t iv_index,
+ const uint8_t *payload,
+ uint8_t privacy_counter[16]);
+bool mesh_crypto_network_obfuscate(const uint8_t privacy_key[16],
+ const uint8_t privacy_counter[16],
+ bool ctl, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint8_t *out);
+bool mesh_crypto_network_clarify(const uint8_t privacy_key[16],
+ const uint8_t privacy_counter[16],
+ const uint8_t net_hdr[6],
+ bool *ctl, uint8_t *ttl,
+ uint32_t *seq, uint16_t *src);
+
+bool mesh_crypto_packet_build(bool ctl, uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint8_t opcode,
+ bool segmented, uint8_t key_id,
+ bool szmic, bool relay, uint16_t seqZero,
+ uint8_t segO, uint8_t segN,
+ const uint8_t *payload, uint8_t payload_len,
+ uint8_t *packet, uint8_t *packet_len);
+bool mesh_crypto_packet_parse(const uint8_t *packet, uint8_t packet_len,
+ bool *ctl, uint8_t *ttl, uint32_t *seq,
+ uint16_t *src, uint16_t *dst,
+ uint32_t *cookie, uint8_t *opcode,
+ bool *segmented, uint8_t *key_id,
+ bool *szmic, bool *relay, uint16_t *seqZero,
+ uint8_t *segO, uint8_t *segN,
+ const uint8_t **payload, uint8_t *payload_len);
+bool mesh_crypto_payload_encrypt(uint8_t *aad, const uint8_t *payload,
+ uint8_t *out, uint16_t payload_len,
+ uint16_t src, uint16_t dst, uint8_t key_id,
+ uint32_t seq_num, uint32_t iv_index,
+ bool aszmic,
+ const uint8_t application_key[16]);
+bool mesh_crypto_payload_decrypt(uint8_t *aad, uint16_t aad_len,
+ const uint8_t *payload, uint16_t payload_len,
+ bool szmict,
+ uint16_t src, uint16_t dst, uint8_t key_id,
+ uint32_t seq_num, uint32_t iv_index,
+ uint8_t *out,
+ const uint8_t application_key[16]);
+bool mesh_crypto_packet_encode(uint8_t *packet, uint8_t packet_len,
+ const uint8_t network_key[16],
+ uint32_t iv_index,
+ const uint8_t privacy_key[16]);
+bool mesh_crypto_packet_decode(const uint8_t *packet, uint8_t packet_len,
+ bool proxy, uint8_t *out, uint32_t iv_index,
+ const uint8_t network_key[16],
+ const uint8_t privacy_key[16]);
+bool mesh_crypto_packet_label(uint8_t *packet, uint8_t packet_len,
+ uint16_t iv_index, uint8_t network_id);
+
+uint8_t mesh_crypto_compute_fcs(const uint8_t *packet, uint8_t packet_len);
+bool mesh_crypto_check_fcs(const uint8_t *packet, uint8_t packet_len,
+ uint8_t received_fcs);
+bool mesh_crypto_aes_cmac(const uint8_t key[16], const uint8_t *msg,
+ size_t msg_len, uint8_t res[16]);
diff --git a/mesh/display.h b/mesh/display.h
new file mode 100644
index 000000000..f5d1f7f79
--- /dev/null
+++ b/mesh/display.h
@@ -0,0 +1,28 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+#define COLOR_OFF "\x1B[0m"
+#define COLOR_RED "\x1B[0;91m"
+#define COLOR_GREEN "\x1B[0;92m"
+#define COLOR_YELLOW "\x1B[0;93m"
+#define COLOR_BLUE "\x1B[0;94m"
+
+unsigned int num_columns(void);
+
+void print_packet(const char *label, const void *data, uint16_t size);
diff --git a/mesh/friend.h b/mesh/friend.h
new file mode 100644
index 000000000..1fa6ec92a
--- /dev/null
+++ b/mesh/friend.h
@@ -0,0 +1,57 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+#define OP_FRND_REQUEST 0x8040
+#define OP_FRND_INQUIRY 0x8041
+#define OP_FRND_CONFIRM 0x8042
+#define OP_FRND_SUB_LIST_ADD 0x8043
+#define OP_FRND_SUB_LIST_CONFIRM 0x8044
+#define OP_FRND_SUB_LIST_REMOVE 0x8045
+#define OP_FRND_NEGOTIATE 0x8046
+#define OP_FRND_CLEAR 0x8047
+
+void friend_poll(struct mesh_net *net, uint16_t src, bool seq,
+ struct mesh_friend *frnd);
+void friend_request(struct mesh_net *net, uint16_t src, uint8_t minReq,
+ uint8_t delay, uint32_t timeout, uint16_t prev,
+ uint8_t num_elements, uint16_t cntr, int8_t rssi);
+void friend_clear_confirm(struct mesh_net *net, uint16_t src, uint16_t lpn,
+ uint16_t lpnCounter);
+void friend_clear(struct mesh_net *net, uint16_t src, uint16_t lpn,
+ uint16_t lpnCounter, struct mesh_friend *frnd);
+void friend_sub_add(struct mesh_net *net, struct mesh_friend *frnd,
+ const uint8_t *pkt, uint8_t len);
+void friend_sub_del(struct mesh_net *net, struct mesh_friend *frnd,
+ const uint8_t *pkt, uint8_t len);
+void mesh_friend_relay_init(struct mesh_net *net, uint16_t addr);
+
+/* Low-Power-Node role */
+void frnd_sub_add(struct mesh_net *net, uint32_t parms[7]);
+void frnd_sub_del(struct mesh_net *net, uint32_t parms[7]);
+void frnd_poll(struct mesh_net *net, bool retry);
+void frnd_clear(struct mesh_net *net);
+void frnd_ack_poll(struct mesh_net *net);
+void frnd_poll_cancel(struct mesh_net *net);
+void frnd_request_friend(struct mesh_net *net, uint8_t cache,
+ uint8_t offer_delay, uint8_t delay, uint32_t timeout);
+void frnd_offer(struct mesh_net *net, uint16_t src, uint8_t window,
+ uint8_t cache, uint8_t sub_list_size,
+ int8_t r_rssi, int8_t l_rssi, uint16_t fn_cnt);
+void frnd_key_refresh(struct mesh_net *net, uint8_t phase);
+struct mesh_key_set *frnd_get_key(struct mesh_net *net);
diff --git a/mesh/mesh-io-api.h b/mesh/mesh-io-api.h
new file mode 100644
index 000000000..f69fceeb2
--- /dev/null
+++ b/mesh/mesh-io-api.h
@@ -0,0 +1,58 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+struct mesh_io_private;
+
+typedef bool (*mesh_io_init_t)(uint16_t index, struct mesh_io *io);
+typedef bool (*mesh_io_destroy_t)(struct mesh_io *io);
+typedef bool (*mesh_io_caps_t)(struct mesh_io *io, struct mesh_io_caps *caps);
+typedef bool (*mesh_io_send_t)(struct mesh_io *io,
+ struct mesh_io_send_info *info,
+ const uint8_t *data, uint16_t len);
+typedef bool (*mesh_io_register_t)(struct mesh_io *io, uint8_t filter_id,
+ mesh_io_recv_func_t cb, void *user_data);
+typedef bool (*mesh_io_deregister_t)(struct mesh_io *io, uint8_t filter_id);
+typedef bool (*mesh_io_filter_set_t)(struct mesh_io *io,
+ uint8_t filter_id, const uint8_t *data, uint8_t len,
+ mesh_io_status_func_t callback, void *user_data);
+typedef bool (*mesh_io_tx_cancel_t)(struct mesh_io *io, uint8_t *pattern,
+ uint8_t len);
+
+struct mesh_io_api {
+ mesh_io_init_t init;
+ mesh_io_destroy_t destroy;
+ mesh_io_caps_t caps;
+ mesh_io_send_t send;
+ mesh_io_register_t reg;
+ mesh_io_deregister_t dereg;
+ mesh_io_filter_set_t set;
+ mesh_io_tx_cancel_t cancel;
+};
+
+struct mesh_io {
+ enum mesh_io_type type;
+ uint16_t index;
+ const struct mesh_io_api *api;
+ struct mesh_io_private *pvt;
+};
+
+struct mesh_io_table {
+ enum mesh_io_type type;
+ const struct mesh_io_api *api;
+};
diff --git a/mesh/mesh-io-generic.h b/mesh/mesh-io-generic.h
new file mode 100644
index 000000000..4bf4d5cb7
--- /dev/null
+++ b/mesh/mesh-io-generic.h
@@ -0,0 +1,20 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+extern const struct mesh_io_api mesh_io_generic;
diff --git a/mesh/mesh-io.h b/mesh/mesh-io.h
new file mode 100644
index 000000000..754f6129c
--- /dev/null
+++ b/mesh/mesh-io.h
@@ -0,0 +1,99 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+struct mesh_io;
+
+#define MESH_IO_FILTER_BEACON 1
+#define MESH_IO_FILTER_PROV 2
+#define MESH_IO_FILTER_NET 3
+
+#define MESH_IO_TX_COUNT_UNLIMITED 0
+
+enum mesh_io_type {
+ MESH_IO_TYPE_NONE = 0,
+ MESH_IO_TYPE_GENERIC
+};
+
+enum mesh_io_timing_type {
+ MESH_IO_TIMING_TYPE_GENERAL = 1,
+ MESH_IO_TIMING_TYPE_POLL,
+ MESH_IO_TIMING_TYPE_POLL_RSP
+};
+
+struct mesh_io_recv_info {
+ uint32_t instant;
+ uint8_t chan;
+ int8_t rssi;
+};
+
+struct mesh_io_send_info {
+ enum mesh_io_timing_type type;
+ union {
+ struct {
+ uint16_t interval;
+ uint8_t cnt;
+ uint8_t min_delay;
+ uint8_t max_delay;
+ } gen;
+
+ struct {
+ uint16_t scan_duration;
+ uint8_t scan_delay;
+ uint8_t filter_ids[2];
+ uint8_t min_delay;
+ uint8_t max_delay;
+ } poll;
+
+ struct {
+ uint32_t instant;
+ uint8_t delay;
+ } poll_rsp;
+
+ } u;
+};
+
+struct mesh_io_caps {
+ uint8_t max_num_filters;
+ uint8_t window_accuracy;
+};
+
+typedef void (*mesh_io_recv_func_t)(void *user_data,
+ struct mesh_io_recv_info *info,
+ const uint8_t *data, uint16_t len);
+
+typedef void (*mesh_io_status_func_t)(void *user_data, int status,
+ uint8_t filter_id);
+
+struct mesh_io *mesh_io_new(uint16_t index, enum mesh_io_type type);
+void mesh_io_destroy(struct mesh_io *io);
+
+bool mesh_io_get_caps(struct mesh_io *io, struct mesh_io_caps *caps);
+
+bool mesh_io_register_recv_cb(struct mesh_io *io, uint8_t filter_id,
+ mesh_io_recv_func_t cb, void *user_data);
+
+bool mesh_io_deregister_recv_cb(struct mesh_io *io, uint8_t filter_id);
+
+bool mesh_set_filter(struct mesh_io *io, uint8_t filter_id,
+ const uint8_t *data, uint8_t len,
+ mesh_io_status_func_t cb, void *user_data);
+
+bool mesh_io_send(struct mesh_io *io, struct mesh_io_send_info *info,
+ const uint8_t *data, uint16_t len);
+bool mesh_io_send_cancel(struct mesh_io *io, uint8_t *pattern, uint8_t len);
diff --git a/mesh/net.h b/mesh/net.h
new file mode 100644
index 000000000..e48380314
--- /dev/null
+++ b/mesh/net.h
@@ -0,0 +1,392 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#ifndef __packed
+#define __packed __attribute__((packed))
+#endif
+
+struct mesh_io;
+struct mesh_node;
+
+#define DEV_ID 0
+
+#define UNUSED_KEY_IDX 0xffff
+
+#define APP_ID_DEV 0
+#define APP_ID_ANY ((unsigned int) -1)
+#define NET_ID_ANY (APP_ID_ANY - 1)
+
+#define CTL 0x80
+#define TTL_MASK 0x7f
+#define SEQ_MASK 0xffffff
+
+#define CREDFLAG_MASK 0x1000
+#define APP_IDX_MASK 0x0fff
+#define APP_IDX_DEV 0x7fff
+#define APP_IDX_ANY 0x8000
+#define APP_IDX_NET 0xffff
+
+#define NET_IDX_INVALID 0xffff
+#define NET_NID_INVALID 0xff
+
+#define KEY_CACHE_SIZE 64
+#define FRND_CACHE_MAX 32
+
+#define MAX_UNSEG_LEN 15 /* msg_len == 11 + sizeof(MIC) */
+#define MAX_SEG_LEN 12 /* UnSeg length - 3 octets overhead */
+#define SEG_MAX(len) (((len) <= MAX_UNSEG_LEN) ? 0 : \
+ (((len) - 1) / MAX_SEG_LEN))
+#define SEG_OFF(seg) ((seg) * MAX_SEG_LEN)
+#define MAX_SEG_TO_LEN(seg) ((seg) ? SEG_OFF((seg) + 1) : MAX_UNSEG_LEN)
+
+#define SEGMENTED 0x80
+#define UNSEGMENTED 0x00
+#define SEG_HDR_SHIFT 31
+#define IS_SEGMENTED(hdr) (!!((hdr) & (true << SEG_HDR_SHIFT)))
+
+#define KEY_ID_MASK 0x7f
+#define KEY_AID_MASK 0x3f
+#define KEY_ID_AKF 0x40
+#define KEY_AID_SHIFT 0
+#define AKF_HDR_SHIFT 30
+#define KEY_HDR_SHIFT 24
+#define HAS_APP_KEY(hdr) (!!((hdr) & (true << AKF_HDR_SHIFT)))
+
+#define OPCODE_MASK 0x7f
+#define OPCODE_HDR_SHIFT 24
+#define RELAY 0x80
+#define RELAY_HDR_SHIFT 23
+#define SZMIC 0x80
+#define SZMIC_HDR_SHIFT 23
+#define SEQ_ZERO_MASK 0x1fff
+#define SEQ_ZERO_HDR_SHIFT 10
+#define IS_RELAYED(hdr) (!!((hdr) & (true << RELAY_HDR_SHIFT)))
+#define HAS_MIC64(hdr) (!!((hdr) & (true << SZMIC_HDR_SHIFT)))
+
+#define SEG_MASK 0x1f
+#define SEGO_HDR_SHIFT 5
+#define SEGN_HDR_SHIFT 0
+#define SEG_TOTAL(hdr) (((hdr) >> SEGN_HDR_SHIFT) & SEG_MASK)
+
+/* Mask of Hdr bits which must be constant over entire incoming SAR message */
+/* (SEG || AKF || AID || SZMIC || SeqZero || SegN) */
+#define HDR_KEY_MASK ((true << SEG_HDR_SHIFT) | \
+ (KEY_ID_MASK << KEY_HDR_SHIFT) | \
+ (true << SZMIC_HDR_SHIFT) | \
+ (SEQ_ZERO_MASK << SEQ_ZERO_HDR_SHIFT) | \
+ (SEG_MASK << SEGN_HDR_SHIFT))
+
+#define HDR_ACK_MASK ((OPCODE_MASK << OPCODE_HDR_SHIFT) | \
+ (SEQ_ZERO_MASK << SEQ_ZERO_HDR_SHIFT))
+
+
+
+#define MSG_CACHE_SIZE 70
+#define REPLAY_CACHE_SIZE 10
+
+/* Proxy Configuration Opcodes */
+#define PROXY_OP_SET_FILTER_TYPE 0x00
+#define PROXY_OP_FILTER_ADD 0x01
+#define PROXY_OP_FILTER_DEL 0x02
+#define PROXY_OP_FILTER_STATUS 0x03
+
+/* Proxy Filter Defines */
+#define PROXY_FILTER_WHITELIST 0x00
+#define PROXY_FILTER_BLACKLIST 0x01
+
+/* Network Tranport Opcodes */
+#define NET_OP_SEG_ACKNOWLEDGE 0x00
+#define NET_OP_FRND_POLL 0x01
+#define NET_OP_FRND_UPDATE 0x02
+#define NET_OP_FRND_REQUEST 0x03
+#define NET_OP_FRND_OFFER 0x04
+#define NET_OP_FRND_CLEAR 0x05
+#define NET_OP_FRND_CLEAR_CONFIRM 0x06
+
+#define NET_OP_PROXY_SUB_ADD 0x07
+#define NET_OP_PROXY_SUB_REMOVE 0x08
+#define NET_OP_PROXY_SUB_CONFIRM 0x09
+#define NET_OP_HEARTBEAT 0x0a
+
+#define FRND_OPCODE(x) \
+ ((x) >= NET_OP_FRND_POLL && (x) <= NET_OP_FRND_CLEAR_CONFIRM)
+
+struct mesh_net_addr_range {
+ uint16_t low;
+ uint16_t high;
+ uint16_t next;
+};
+
+struct mesh_net_prov_caps {
+ uint8_t num_ele;
+ uint16_t algorithms;
+ uint8_t pub_type;
+ uint8_t static_type;
+ uint8_t output_size;
+ uint16_t output_action;
+ uint8_t input_size;
+ uint16_t input_action;
+} __packed;
+
+struct mesh_net_heartbeat {
+ struct l_timeout *pub_timer;
+ struct l_timeout *sub_timer;
+ struct timeval sub_time;
+ bool sub_enabled;
+ uint32_t pub_period;
+ uint32_t sub_period;
+ uint32_t sub_start;
+ uint16_t pub_dst;
+ uint16_t pub_count;
+ uint16_t pub_features;
+ uint16_t features;
+ uint16_t pub_net_idx;
+ uint16_t sub_src;
+ uint16_t sub_dst;
+ uint16_t sub_count;
+ uint8_t pub_ttl;
+ uint8_t sub_min_hops;
+ uint8_t sub_max_hops;
+};
+
+struct mesh_key_set {
+ bool frnd;
+ uint8_t nid;
+ uint8_t enc_key[16];
+ uint8_t privacy_key[16];
+};
+
+struct mesh_friend {
+ struct mesh_net *net;
+ struct l_queue *pkt_cache;
+ struct l_timeout *timeout;
+ void *pkt;
+ uint16_t *grp_list;
+ uint32_t poll_timeout;
+ uint32_t last_hdr;
+ uint16_t dst; /* Primary Element unicast addr */
+ uint16_t fn_cnt;
+ uint16_t lp_cnt;
+ int16_t grp_cnt;
+ struct mesh_key_set key_set;
+ struct mesh_key_set new_key_set;
+ uint8_t ele_cnt;
+ uint8_t frd;
+ uint8_t frw;
+ bool seq;
+ bool last;
+};
+
+struct mesh_frnd_pkt {
+ uint32_t iv_index;
+ uint32_t seq;
+ uint16_t src;
+ uint16_t dst;
+ uint16_t size;
+ uint8_t segN;
+ uint8_t segO;
+ uint8_t ttl;
+ uint8_t tc;
+ bool szmict;
+ union {
+ struct {
+ uint8_t key_id;
+ } m;
+ struct {
+ uint16_t seq0;
+ } a;
+ struct {
+ uint8_t opcode;
+ } c;
+ } u;
+ uint8_t data[];
+};
+
+struct mesh_friend_seg_one {
+ uint32_t hdr;
+ uint32_t seq;
+ bool sent;
+ bool md;
+ uint8_t data[15];
+};
+
+struct mesh_friend_seg_12 {
+ uint32_t hdr;
+ uint32_t seq;
+ bool sent;
+ bool md;
+ uint8_t data[12];
+};
+
+struct mesh_friend_msg {
+ uint32_t iv_index;
+ uint32_t flags;
+ uint16_t src;
+ uint16_t dst;
+ uint8_t ttl;
+ uint8_t cnt_in;
+ uint8_t cnt_out;
+ uint8_t last_len;
+ bool done;
+ bool ctl;
+ union {
+ struct mesh_friend_seg_one one[1]; /* Single segment */
+ struct mesh_friend_seg_12 s12[0]; /* Array of segments */
+ } u;
+};
+
+typedef void (*mesh_status_func_t)(void *user_data, bool result);
+typedef void (*mesh_net_status_func_t)(uint16_t remote, uint8_t status,
+ void *data, uint16_t size,
+ void *user_data);
+
+struct mesh_net *mesh_net_new(uint16_t index);
+struct mesh_net *mesh_net_ref(struct mesh_net *net);
+void mesh_net_unref(struct mesh_net *net);
+void mesh_net_flush_msg_queues(struct mesh_net *net);
+void mesh_net_set_iv_index(struct mesh_net *net, uint32_t index, bool update);
+bool mesh_net_iv_index_update(struct mesh_net *net);
+bool mesh_net_set_seq_num(struct mesh_net *net, uint32_t number);
+uint32_t mesh_net_get_seq_num(struct mesh_net *net);
+uint32_t mesh_net_next_seq_num(struct mesh_net *net);
+bool mesh_net_set_default_ttl(struct mesh_net *net, uint8_t ttl);
+uint8_t mesh_net_get_default_ttl(struct mesh_net *net);
+bool mesh_net_get_frnd_seq(struct mesh_net *net);
+void mesh_net_set_frnd_seq(struct mesh_net *net, bool seq);
+uint16_t mesh_net_get_address(struct mesh_net *net);
+bool mesh_net_register_unicast(struct mesh_net *net,
+ uint16_t unicast, uint8_t num_ele);
+bool mesh_net_set_friend(struct mesh_net *net, uint16_t friend_addr);
+uint16_t mesh_net_get_friend(struct mesh_net *net);
+uint8_t mesh_net_get_num_ele(struct mesh_net *net);
+bool mesh_net_set_beacon_mode(struct mesh_net *net, bool enable);
+bool mesh_net_set_proxy_mode(struct mesh_net *net, bool enable);
+bool mesh_net_set_relay_mode(struct mesh_net *net, bool enable, uint8_t cnt,
+ uint8_t interval);
+bool mesh_net_set_friend_mode(struct mesh_net *net, bool enable);
+bool mesh_net_add_keyset(struct mesh_net *net, struct mesh_key_set *key_set);
+bool mesh_net_remove_keyset(struct mesh_net *net, struct mesh_key_set *key_set);
+int mesh_net_del_key(struct mesh_net *net, uint16_t net_idx);
+int mesh_net_add_key(struct mesh_net *net, bool update,
+ uint16_t net_idx, const void *key);
+uint32_t mesh_net_get_iv_index(struct mesh_net *net);
+void mesh_net_get_snb_state(struct mesh_net *net,
+ uint8_t *flags, uint32_t *iv_index);
+bool mesh_net_get_key(struct mesh_net *net, bool new_key, uint16_t idx,
+ uint8_t key[16]);
+bool mesh_net_attach(struct mesh_net *net, struct mesh_io *io);
+struct mesh_io *mesh_net_detach(struct mesh_net *net);
+struct l_queue *mesh_net_get_app_keys(struct mesh_net *net);
+
+bool mesh_net_flush(struct mesh_net *net);
+void mesh_net_transport_send(struct mesh_net *net, struct mesh_key_set *key_set,
+ bool fast, uint32_t iv_index, uint8_t ttl,
+ uint32_t seq, uint16_t src, uint16_t dst,
+ const uint8_t *msg, uint16_t msg_len);
+
+unsigned int mesh_net_app_send(struct mesh_net *net, bool frnd_cred,
+ uint16_t src, uint16_t dst, uint8_t key_id,
+ uint8_t ttl, uint32_t seq, uint32_t iv_index,
+ bool szmic, const void *msg, uint16_t msg_len,
+ mesh_net_status_func_t status_func,
+ void *user_data);
+void mesh_net_app_send_cancel(struct mesh_net *net, unsigned int id);
+void mesh_net_ack_send(struct mesh_net *net, struct mesh_key_set *key_set,
+ uint32_t iv_index, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint16_t dst, bool rly,
+ uint16_t seqZero, uint32_t ack_flags);
+struct mesh_net_prov_caps *mesh_net_prov_caps_get(struct mesh_net *net);
+uint8_t *mesh_net_priv_key_get(struct mesh_net *net);
+bool mesh_net_priv_key_set(struct mesh_net *net, uint8_t key[32]);
+uint8_t *mesh_net_prov_rand(struct mesh_net *net);
+uint16_t mesh_net_prov_uni(struct mesh_net *net, uint8_t ele_cnt);
+bool mesh_net_id_uuid_set(struct mesh_net *net, uint8_t uuid[16]);
+uint8_t *mesh_net_test_addr(struct mesh_net *net);
+int mesh_net_get_identity_mode(struct mesh_net *net, uint16_t idx,
+ uint8_t *mode);
+char *mesh_net_id_name(struct mesh_net *net);
+bool mesh_net_test_mode(struct mesh_net *net);
+bool mesh_net_dst_reg(struct mesh_net *net, uint16_t dst);
+bool mesh_net_dst_unreg(struct mesh_net *net, uint16_t dst);
+struct mesh_friend *mesh_friend_new(struct mesh_net *net, uint16_t dst,
+ uint8_t ele_cnt, uint8_t frd,
+ uint8_t frw, uint32_t fpt,
+ uint16_t fn_cnt, uint16_t lp_cnt);
+void mesh_friend_free(void *frnd);
+bool mesh_friend_clear(struct mesh_net *net, struct mesh_friend *frnd);
+void mesh_friend_sub_add(struct mesh_net *net, uint16_t lpn, uint8_t ele_cnt,
+ uint8_t grp_cnt,
+ const uint8_t *list);
+void mesh_friend_sub_del(struct mesh_net *net, uint16_t lpn, uint8_t cnt,
+ const uint8_t *del_list);
+uint8_t mesh_net_key_refresh_phase_set(struct mesh_net *net, uint16_t net_idx,
+ uint8_t transition);
+uint8_t mesh_net_key_refresh_phase_get(struct mesh_net *net, uint16_t net_idx,
+ uint8_t *phase);
+int mesh_net_kr_phase_one(struct mesh_net *net, uint16_t net_idx,
+ const uint8_t *key);
+int mesh_net_key_refresh_phase_two(struct mesh_net *net, uint16_t net_idx);
+int mesh_net_key_refresh_finish(struct mesh_net *net, uint16_t net_idx);
+void mesh_net_send_seg(struct mesh_net *net, struct mesh_key_set *key_set,
+ uint32_t iv_index, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint16_t dst, uint32_t hdr,
+ const void *seg, uint16_t seg_len);
+uint16_t mesh_net_get_features(struct mesh_net *net);
+struct mesh_net_heartbeat *mesh_net_heartbeat_get(struct mesh_net *net);
+void mesh_net_heartbeat_init(struct mesh_net *net);
+void mesh_net_heartbeat_send(struct mesh_net *net);
+void mesh_net_uni_range_set(struct mesh_net *net,
+ struct mesh_net_addr_range *range);
+struct mesh_net_addr_range mesh_net_uni_range_get(struct mesh_net *net);
+void mesh_net_provisioner_mode_set(struct mesh_net *net, bool mode);
+bool mesh_net_provisioner_mode_get(struct mesh_net *net);
+bool mesh_net_key_list_get(struct mesh_net *net, uint8_t *buf, uint16_t *count);
+uint16_t mesh_net_get_primary_idx(struct mesh_net *net);
+void mesh_net_sub_list_add(struct mesh_net *net, uint16_t addr);
+void mesh_net_sub_list_del(struct mesh_net *net, uint16_t addr);
+uint32_t mesh_net_friend_timeout(struct mesh_net *net, uint16_t addr);
+struct mesh_io *mesh_net_get_io(struct mesh_net *net);
+bool mesh_net_local_node_set(struct mesh_net *net, struct mesh_node *node,
+ bool provisioner);
+struct mesh_node *mesh_net_local_node_get(struct mesh_net *net);
+bool mesh_net_set_crpl(struct mesh_net *net, uint16_t crpl);
+uint16_t mesh_net_get_crpl(struct mesh_net *net);
+bool mesh_net_have_key(struct mesh_net *net, uint16_t net_idx);
+bool mesh_net_jconfig_set(struct mesh_net *net, void *jconfig);
+void *mesh_net_jconfig_get(struct mesh_net *net);
+bool mesh_net_cfg_file_set(struct mesh_net *net, const char *cfg);
+bool mesh_net_cfg_file_get(struct mesh_net *net, const char **cfg);
+bool mesh_net_is_local_address(struct mesh_net *net, uint16_t addr);
+void mesh_net_set_window_accuracy(struct mesh_net *net, uint8_t accuracy);
+void mesh_net_transmit_params_set(struct mesh_net *net, uint8_t count,
+ uint16_t interval);
+void mesh_net_transmit_params_get(struct mesh_net *net, uint8_t *count,
+ uint16_t *interval);
+struct mesh_prov *mesh_net_get_prov(struct mesh_net *net);
+void mesh_net_set_prov(struct mesh_net *net, struct mesh_prov *prov);
+void mesh_net_provisioned_set(struct mesh_net *net, bool provisioned);
+bool mesh_net_provisioned_get(struct mesh_net *net);
+bool mesh_net_provisioned_new(struct mesh_net *net, uint8_t device_key[16],
+ uint16_t net_idx, uint8_t net_key[16],
+ uint16_t unicast, uint16_t snb_flags,
+ uint32_t iv_index, mesh_status_func_t cb,
+ void *user_data);
diff --git a/mesh/prov.h b/mesh/prov.h
new file mode 100644
index 000000000..09fe6c3cd
--- /dev/null
+++ b/mesh/prov.h
@@ -0,0 +1,162 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+#ifndef __packed
+#define __packed __attribute__((packed))
+#endif
+
+struct mesh_net;
+struct mesh_dev;
+
+enum mesh_trans {
+ MESH_TRANS_IDLE,
+ MESH_TRANS_TX,
+ MESH_TRANS_RX,
+};
+
+enum mesh_bearer {
+ MESH_BEARER_IDLE,
+ MESH_BEARER_ADV,
+};
+
+enum mesh_prov_mode {
+ MESH_PROV_MODE_NONE,
+ MESH_PROV_MODE_INITIATOR,
+ MESH_PROV_MODE_GATT_ACCEPTOR,
+ MESH_PROV_MODE_ADV_ACCEPTOR,
+ MESH_PROV_MODE_GATT_CLIENT,
+ MESH_PROV_MODE_MESH_SERVER,
+ MESH_PROV_MODE_MESH_CLIENT,
+ MESH_PROV_MODE_MESH_GATT_CLIENT,
+};
+
+struct mesh_prov;
+typedef void (*mesh_prov_open_func_t)(struct mesh_prov *prov);
+typedef void (*mesh_prov_close_func_t)(struct mesh_prov *prov, uint8_t reason);
+typedef void (*mesh_prov_send_func_t)(bool success, struct mesh_prov *prov);
+typedef void (*mesh_prov_receive_func_t)(const void *data, uint16_t size,
+ struct mesh_prov *prov);
+
+struct prov_invite {
+ uint8_t attention;
+} __packed;
+
+struct prov_start {
+ uint8_t algorithm;
+ uint8_t pub_key;
+ uint8_t auth_method;
+ uint8_t auth_action;
+ uint8_t auth_size;
+} __packed;
+
+struct conf_input {
+ struct prov_invite invite;
+ struct mesh_net_prov_caps caps;
+ struct prov_start start;
+ uint8_t prv_pub_key[64];
+ uint8_t dev_pub_key[64];
+} __packed;
+
+struct mesh_prov {
+ int ref_count;
+ struct mesh_dev *dev;
+ struct mesh_net *net;
+ enum mesh_prov_mode mode;
+ enum mesh_trans trans;
+ enum mesh_bearer bearer;
+ uint8_t uuid[16];
+ uint8_t caps[12];
+
+ uint32_t conn_id;
+ uint16_t net_idx;
+ uint16_t remote;
+ uint16_t addr;
+ uint16_t expected_len;
+ uint16_t packet_len;
+ uint8_t local_msg_num;
+ uint8_t peer_msg_num;
+ uint8_t last_peer_msg_num;
+ uint8_t got_segs;
+ uint8_t expected_segs;
+ uint8_t expected_fcs;
+ uint8_t packet_buf[80];
+ uint8_t peer_buf[80];
+ struct timeval tx_start;
+ struct l_timeout *tx_timeout;
+
+ /* Provisioning credentials and crypto material */
+ struct conf_input conf_inputs;
+ uint8_t dev_key[16];
+ uint8_t conf_salt[16];
+ uint8_t s_key[16];
+ uint8_t s_nonce[13];
+ uint8_t conf_key[16];
+ uint8_t conf[16];
+ uint8_t r_conf[16];
+ uint8_t rand_auth[32];
+ uint8_t prov_salt[16];
+ uint8_t secret[32];
+ uint8_t r_public[64];
+ uint8_t l_public[64];
+ /* End Provisioning credentials and crypto material */
+
+ mesh_prov_open_func_t open_callback;
+ mesh_prov_close_func_t close_callback;
+ mesh_prov_receive_func_t receive_callback;
+ void *receive_data;
+ mesh_prov_send_func_t send_callback;
+ void *send_data;
+};
+
+struct mesh_prov *mesh_prov_new(struct mesh_net *net, uint16_t remote);
+
+struct mesh_prov *mesh_prov_ref(struct mesh_prov *prov);
+void mesh_prov_unref(struct mesh_prov *prov);
+
+bool mesh_prov_gatt_client(struct mesh_prov *prov, struct mesh_dev *dev,
+ uint8_t uuid[16],
+ mesh_prov_open_func_t open_callback,
+ mesh_prov_close_func_t close_callback,
+ mesh_prov_receive_func_t recv_callback,
+ void *user_data);
+
+bool mesh_prov_listen(struct mesh_net *net, uint8_t uuid[16], uint8_t caps[12],
+ mesh_prov_open_func_t open_callback,
+ mesh_prov_close_func_t close_callback,
+ mesh_prov_receive_func_t recv_callback,
+ void *user_data);
+
+bool mesh_prov_connect(struct mesh_prov *prov, struct mesh_dev *dev,
+ uint16_t net_idx, uint8_t uuid[16],
+ mesh_prov_open_func_t open_callback,
+ mesh_prov_close_func_t close_callback,
+ mesh_prov_receive_func_t recv_callback,
+ void *user_data);
+
+unsigned int mesh_prov_send(struct mesh_prov *prov,
+ const void *data, uint16_t size,
+ mesh_prov_send_func_t send_callback,
+ void *user_data);
+bool mesh_prov_cancel(struct mesh_prov *prov, unsigned int id);
+
+bool mesh_prov_close(struct mesh_prov *prov, uint8_t reason);
+void mesh_prov_set_addr(struct mesh_prov *prov, uint16_t addr);
+uint16_t mesh_prov_get_addr(struct mesh_prov *prov);
+void mesh_prov_set_idx(struct mesh_prov *prov, uint16_t net_idx);
+uint16_t mesh_prov_get_idx(struct mesh_prov *prov);
diff --git a/mesh/provision.h b/mesh/provision.h
new file mode 100644
index 000000000..0c59bf037
--- /dev/null
+++ b/mesh/provision.h
@@ -0,0 +1,30 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+struct mesh_prov;
+struct l_queue;
+
+void initiator_prov_open(struct mesh_prov *prov);
+void initiator_prov_close(struct mesh_prov *prov, uint8_t reason);
+void initiator_prov_receive(const void *pkt, uint16_t size,
+ struct mesh_prov *prov);
+void acceptor_prov_open(struct mesh_prov *prov);
+void acceptor_prov_close(struct mesh_prov *prov, uint8_t reason);
+void acceptor_prov_receive(const void *pkt, uint16_t size,
+ struct mesh_prov *prov);
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 02/14] mesh: Mesh crypto support
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 01/14] mesh: Shared private meshd interfaces Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 03/14] mesh: Infrastructure for Mesh daemon Brian Gix
` (12 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/crypto.c | 1545 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 1545 insertions(+)
create mode 100644 mesh/crypto.c
diff --git a/mesh/crypto.c b/mesh/crypto.c
new file mode 100644
index 000000000..dda867924
--- /dev/null
+++ b/mesh/crypto.c
@@ -0,0 +1,1545 @@
+/*
+ *
+ * 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 <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <ell/ell.h>
+
+#include <linux/if_alg.h>
+
+#ifndef SOL_ALG
+#define SOL_ALG 279
+#endif
+
+#ifndef ALG_SET_AEAD_AUTHSIZE
+#define ALG_SET_AEAD_AUTHSIZE 5
+#endif
+
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/crypto.h"
+
+/* Multiply used Zero array */
+static const uint8_t zero[16] = { 0, };
+
+static int alg_new(int fd, const void *keyval, socklen_t keylen,
+ size_t mic_size)
+{
+ if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0)
+ return -1;
+
+ if (mic_size &&
+ setsockopt(fd, SOL_ALG, ALG_SET_AEAD_AUTHSIZE,
+ NULL, mic_size) < 0)
+ return -1;
+
+ /* FIXME: This should use accept4() with SOCK_CLOEXEC */
+ return accept(fd, NULL, 0);
+}
+
+static bool alg_encrypt(int fd, const void *inbuf, size_t inlen,
+ void *outbuf, size_t outlen)
+{
+ __u32 alg_op = ALG_OP_ENCRYPT;
+ char cbuf[CMSG_SPACE(sizeof(alg_op))];
+ struct cmsghdr *cmsg;
+ struct msghdr msg;
+ struct iovec iov;
+ ssize_t len;
+
+ memset(cbuf, 0, sizeof(cbuf));
+ memset(&msg, 0, sizeof(msg));
+
+ msg.msg_control = cbuf;
+ msg.msg_controllen = sizeof(cbuf);
+
+ cmsg = CMSG_FIRSTHDR(&msg);
+ cmsg->cmsg_level = SOL_ALG;
+ cmsg->cmsg_type = ALG_SET_OP;
+ cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op));
+ memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op));
+
+ iov.iov_base = (void *) inbuf;
+ iov.iov_len = inlen;
+
+ msg.msg_iov = &iov;
+ msg.msg_iovlen = 1;
+
+ len = sendmsg(fd, &msg, 0);
+ if (len < 0)
+ return false;
+
+ len = read(fd, outbuf, outlen);
+ if (len < 0)
+ return false;
+
+ return true;
+}
+
+static int aes_ecb_setup(const uint8_t key[16])
+{
+ struct sockaddr_alg salg;
+ int fd, nfd;
+
+ fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+ if (fd < 0)
+ return -1;
+
+ memset(&salg, 0, sizeof(salg));
+ salg.salg_family = AF_ALG;
+ strcpy((char *) salg.salg_type, "skcipher");
+ strcpy((char *) salg.salg_name, "ecb(aes)");
+
+ if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ nfd = alg_new(fd, key, 16, 0);
+
+ close(fd);
+
+ return nfd;
+}
+
+static bool aes_ecb(int fd, const uint8_t plaintext[16], uint8_t encrypted[16])
+{
+ return alg_encrypt(fd, plaintext, 16, encrypted, 16);
+}
+
+static void aes_ecb_destroy(int fd)
+{
+ close(fd);
+}
+
+static bool aes_ecb_one(const uint8_t key[16],
+ const uint8_t plaintext[16], uint8_t encrypted[16])
+{
+ bool result;
+ int fd;
+
+ fd = aes_ecb_setup(key);
+ if (fd < 0)
+ return false;
+
+ result = aes_ecb(fd, plaintext, encrypted);
+
+ aes_ecb_destroy(fd);
+
+ return result;
+}
+
+bool mesh_aes_ecb_one(const uint8_t key[16],
+ const uint8_t plaintext[16], uint8_t encrypted[16])
+{
+ return aes_ecb_one(key, plaintext, encrypted);
+}
+
+/* Maximum message length that can be passed to aes_cmac */
+#define CMAC_MSG_MAX (64 + 64 + 17)
+
+static int aes_cmac_setup(const uint8_t key[16])
+{
+ struct sockaddr_alg salg;
+ int fd, nfd;
+
+ fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0);
+ if (fd < 0)
+ return -1;
+
+ memset(&salg, 0, sizeof(salg));
+ salg.salg_family = AF_ALG;
+ strcpy((char *) salg.salg_type, "hash");
+ strcpy((char *) salg.salg_name, "cmac(aes)");
+
+ if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ nfd = alg_new(fd, key, 16, 0);
+
+ close(fd);
+
+ return nfd;
+}
+
+static bool aes_cmac(int fd, const uint8_t *msg,
+ size_t msg_len, uint8_t res[16])
+{
+ ssize_t len;
+
+ if (msg_len > CMAC_MSG_MAX)
+ return false;
+
+ len = send(fd, msg, msg_len, 0);
+ if (len < 0)
+ return false;
+
+ len = read(fd, res, 16);
+ if (len < 0)
+ return false;
+
+ return true;
+}
+
+static void aes_cmac_destroy(int fd)
+{
+ close(fd);
+}
+
+static int aes_cmac_N_start(const uint8_t N[16])
+{
+ int fd;
+
+ fd = aes_cmac_setup(N);
+ return fd;
+}
+
+static bool aes_cmac_one(const uint8_t key[16], const void *msg,
+ size_t msg_len, uint8_t res[16])
+{
+ bool result;
+ int fd;
+
+ fd = aes_cmac_setup(key);
+ if (fd < 0)
+ return false;
+
+ result = aes_cmac(fd, msg, msg_len, res);
+
+ aes_cmac_destroy(fd);
+
+ return result;
+}
+
+bool mesh_crypto_aes_cmac(const uint8_t key[16], const uint8_t *msg,
+ size_t msg_len, uint8_t res[16])
+{
+ return aes_cmac_one(key, msg, msg_len, res);
+}
+
+bool mesh_crypto_aes_ccm_encrypt(const uint8_t nonce[13], const uint8_t key[16],
+ const uint8_t *aad, uint16_t aad_len,
+ const uint8_t *msg, uint16_t msg_len,
+ uint8_t *out_msg,
+ void *out_mic, size_t mic_size)
+{
+ uint8_t pmsg[16], cmic[16], cmsg[16];
+ uint8_t mic[16], Xn[16];
+ uint16_t blk_cnt, last_blk;
+ bool result;
+ size_t i, j;
+ int fd;
+
+ /* Mesh limits AAD length to 16 */
+ if (aad_len > 16)
+ return false;
+
+ fd = aes_ecb_setup(key);
+ if (fd < 0)
+ return false;
+
+ /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(0x0000, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmic);
+ if (!result)
+ goto done;
+
+ /* X_0 = e(AppKey, 0x09 || nonce || length) */
+ if (mic_size == 8)
+ pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
+ else
+ pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
+
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(msg_len, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* If AAD is being used to authenticate, include it here */
+ if (aad_len) {
+ l_put_be16(aad_len, pmsg);
+
+ for (i = 0; i < sizeof(uint16_t); i++)
+ pmsg[i] = Xn[i] ^ pmsg[i];
+
+ j = 0;
+ aad_len += sizeof(uint16_t);
+ while (aad_len > 16) {
+ do {
+ pmsg[i] = Xn[i] ^ aad[j];
+ i++, j++;
+ } while (i < 16);
+
+ aad_len -= 16;
+ i = 0;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ for (i = 0; i < aad_len; i++, j++)
+ pmsg[i] = Xn[i] ^ aad[j];
+
+ for (i = aad_len; i < 16; i++)
+ pmsg[i] = Xn[i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ last_blk = msg_len % 16;
+ blk_cnt = (msg_len + 15) / 16;
+ if (!last_blk)
+ last_blk = 16;
+
+ for (j = 0; j < blk_cnt; j++) {
+ if (j + 1 == blk_cnt) {
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < last_blk; i++)
+ pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
+ for (i = last_blk; i < 16; i++)
+ pmsg[i] = Xn[i] ^ 0x00;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* MIC = C_mic ^ X_1 */
+ for (i = 0; i < sizeof(mic); i++)
+ mic[i] = cmic[i] ^ Xn[i];
+
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ if (out_msg) {
+ /* Encrypted = Payload[0-15] ^ C_1 */
+ for (i = 0; i < last_blk; i++)
+ out_msg[(j * 16) + i] =
+ msg[(j * 16) + i] ^ cmsg[i];
+
+ }
+ } else {
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < 16; i++)
+ pmsg[i] = Xn[i] ^ msg[(j * 16) + i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ if (out_msg) {
+ /* Encrypted = Payload[0-15] ^ C_N */
+ for (i = 0; i < 16; i++)
+ out_msg[(j * 16) + i] =
+ msg[(j * 16) + i] ^ cmsg[i];
+ }
+
+ }
+ }
+
+ if (out_msg)
+ memcpy(out_msg + msg_len, mic, mic_size);
+
+ if (out_mic) {
+ switch (mic_size) {
+ case 4:
+ *(uint32_t *)out_mic = l_get_be32(mic);
+ break;
+ case 8:
+ *(uint64_t *)out_mic = l_get_be64(mic);
+ break;
+ default:
+ /* Unsupported MIC size */
+ result = false;
+ }
+ }
+
+done:
+ aes_ecb_destroy(fd);
+
+ return result;
+}
+
+bool mesh_crypto_aes_ccm_decrypt(const uint8_t nonce[13], const uint8_t key[16],
+ const uint8_t *aad, uint16_t aad_len,
+ const uint8_t *enc_msg, uint16_t enc_msg_len,
+ uint8_t *out_msg,
+ void *out_mic, size_t mic_size)
+{
+ uint8_t msg[16], pmsg[16], cmic[16], cmsg[16], Xn[16];
+ uint8_t mic[16];
+ uint16_t msg_len = enc_msg_len - mic_size;
+ uint16_t last_blk, blk_cnt;
+ bool result;
+ size_t i, j;
+ int fd;
+
+ if (enc_msg_len < 5 || aad_len > 16)
+ return false;
+
+ fd = aes_ecb_setup(key);
+ if (fd < 0)
+ return false;
+
+ /* C_mic = e(AppKey, 0x01 || nonce || 0x0000) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(0x0000, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmic);
+ if (!result)
+ goto done;
+
+ /* X_0 = e(AppKey, 0x09 || nonce || length) */
+ if (mic_size == 8)
+ pmsg[0] = 0x19 | (aad_len ? 0x40 : 0x00);
+ else
+ pmsg[0] = 0x09 | (aad_len ? 0x40 : 0x00);
+
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(msg_len, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* If AAD is being used to authenticate, include it here */
+ if (aad_len) {
+ l_put_be16(aad_len, pmsg);
+
+ for (i = 0; i < sizeof(uint16_t); i++)
+ pmsg[i] = Xn[i] ^ pmsg[i];
+
+ j = 0;
+ aad_len += sizeof(uint16_t);
+ while (aad_len > 16) {
+ do {
+ pmsg[i] = Xn[i] ^ aad[j];
+ i++, j++;
+ } while (i < 16);
+
+ aad_len -= 16;
+ i = 0;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ for (i = 0; i < aad_len; i++, j++)
+ pmsg[i] = Xn[i] ^ aad[j];
+
+ for (i = aad_len; i < 16; i++)
+ pmsg[i] = Xn[i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+
+ last_blk = msg_len % 16;
+ blk_cnt = (msg_len + 15) / 16;
+ if (!last_blk)
+ last_blk = 16;
+
+ for (j = 0; j < blk_cnt; j++) {
+ if (j + 1 == blk_cnt) {
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ /* Encrypted = Payload[0-15] ^ C_1 */
+ for (i = 0; i < last_blk; i++)
+ msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
+
+ if (out_msg)
+ memcpy(out_msg + (j * 16), msg, last_blk);
+
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < last_blk; i++)
+ pmsg[i] = Xn[i] ^ msg[i];
+ for (i = last_blk; i < 16; i++)
+ pmsg[i] = Xn[i] ^ 0x00;
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+
+ /* MIC = C_mic ^ X_1 */
+ for (i = 0; i < sizeof(mic); i++)
+ mic[i] = cmic[i] ^ Xn[i];
+ } else {
+ /* C_1 = e(AppKey, 0x01 || nonce || 0x0001) */
+ pmsg[0] = 0x01;
+ memcpy(pmsg + 1, nonce, 13);
+ l_put_be16(j + 1, pmsg + 14);
+
+ result = aes_ecb(fd, pmsg, cmsg);
+ if (!result)
+ goto done;
+
+ /* Encrypted = Payload[0-15] ^ C_1 */
+ for (i = 0; i < 16; i++)
+ msg[i] = enc_msg[(j * 16) + i] ^ cmsg[i];
+
+ if (out_msg)
+ memcpy(out_msg + (j * 16), msg, 16);
+
+ /* X_1 = e(AppKey, X_0 ^ Payload[0-15]) */
+ for (i = 0; i < 16; i++)
+ pmsg[i] = Xn[i] ^ msg[i];
+
+ result = aes_ecb(fd, pmsg, Xn);
+ if (!result)
+ goto done;
+ }
+ }
+
+ if (out_mic) {
+ switch (mic_size) {
+ case 4:
+ *(uint32_t *)out_mic = l_get_be32(mic);
+ break;
+ case 8:
+ *(uint64_t *)out_mic = l_get_be64(mic);
+ break;
+ default:
+ /* Unsupported MIC size */
+ result = false;
+ }
+ }
+
+done:
+ aes_ecb_destroy(fd);
+
+ return result;
+}
+
+bool mesh_crypto_k1(const uint8_t ikm[16], const uint8_t salt[16],
+ const void *info, size_t info_len, uint8_t okm[16])
+{
+ uint8_t res[16];
+
+ if (!aes_cmac_one(salt, ikm, 16, res))
+ return false;
+
+ return aes_cmac_one(res, info, info_len, okm);
+}
+
+bool mesh_crypto_k2(const uint8_t n[16], const uint8_t *p, size_t p_len,
+ uint8_t net_id[1],
+ uint8_t enc_key[16],
+ uint8_t priv_key[16])
+{
+ int fd;
+ uint8_t output[16];
+ uint8_t t[16];
+ uint8_t *stage;
+ bool success = false;
+
+ stage = l_malloc(sizeof(output) + p_len + 1);
+ if (!stage)
+ return false;
+
+ if (!mesh_crypto_s1("smk2", 4, stage))
+ goto fail;
+
+ if (!aes_cmac_one(stage, n, 16, t))
+ goto fail;
+
+ fd = aes_cmac_N_start(t);
+ if (fd < 0)
+ goto fail;
+
+ memcpy(stage, p, p_len);
+ stage[p_len] = 1;
+
+ if (!aes_cmac(fd, stage, p_len + 1, output))
+ goto done;
+
+ net_id[0] = output[15] & 0x7f;
+
+ memcpy(stage, output, 16);
+ memcpy(stage + 16, p, p_len);
+ stage[p_len + 16] = 2;
+
+ if (!aes_cmac(fd, stage, p_len + 16 + 1, output))
+ goto done;
+
+ memcpy(enc_key, output, 16);
+
+ memcpy(stage, output, 16);
+ memcpy(stage + 16, p, p_len);
+ stage[p_len + 16] = 3;
+
+ if (!aes_cmac(fd, stage, p_len + 16 + 1, output))
+ goto done;
+
+ memcpy(priv_key, output, 16);
+ success = true;
+
+done:
+ aes_cmac_destroy(fd);
+fail:
+ l_free(stage);
+
+ return success;
+}
+
+static bool crypto_128(const uint8_t n[16], const char *s, uint8_t out128[16])
+{
+ const uint8_t id128[] = { 'i', 'd', '1', '2', '8', 0x01 };
+ uint8_t salt[16];
+
+ if (!mesh_crypto_s1(s, 4, salt))
+ return false;
+
+ return mesh_crypto_k1(n, salt, id128, sizeof(id128), out128);
+}
+
+bool mesh_crypto_nkik(const uint8_t n[16], uint8_t identity_key[16])
+{
+ return crypto_128(n, "nkik", identity_key);
+}
+
+bool mesh_crypto_identity(const uint8_t net_key[16], uint16_t addr,
+ uint8_t id[16])
+{
+ uint8_t id_key[16];
+ uint8_t tmp[16];
+
+ if (!mesh_crypto_nkik(net_key, id_key))
+ return false;
+
+ if (!l_get_be64(id + 8))
+ l_getrandom(id + 8, 8);
+
+ memset(tmp, 0, sizeof(tmp));
+ memcpy(tmp + 6, id + 8, 8);
+ l_put_be16(addr, tmp + 14);
+
+ if (!aes_ecb_one(id_key, tmp, tmp))
+ return false;
+
+ memcpy(id, tmp + 8, 8);
+ return true;
+}
+
+bool mesh_crypto_nkbk(const uint8_t n[16], uint8_t beacon_key[16])
+{
+ return crypto_128(n, "nkbk", beacon_key);
+}
+
+bool mesh_crypto_nkpk(const uint8_t n[16], uint8_t proxy_key[16])
+{
+ return crypto_128(n, "nkpk", proxy_key);
+}
+
+bool mesh_crypto_k3(const uint8_t n[16], uint8_t out64[8])
+{
+ const uint8_t id64[] = { 'i', 'd', '6', '4', 0x01 };
+ uint8_t tmp[16];
+ uint8_t t[16];
+
+ if (!mesh_crypto_s1("smk3", 4, tmp))
+ return false;
+
+ if (!aes_cmac_one(tmp, n, 16, t))
+ return false;
+
+ if (!aes_cmac_one(t, id64, sizeof(id64), tmp))
+ return false;
+
+ memcpy(out64, tmp + 8, 8);
+
+ return true;
+}
+
+bool mesh_crypto_k4(const uint8_t a[16], uint8_t out6[1])
+{
+ const uint8_t id6[] = { 'i', 'd', '6', 0x01 };
+ uint8_t tmp[16];
+ uint8_t t[16];
+
+ if (!mesh_crypto_s1("smk4", 4, tmp))
+ return false;
+
+ if (!aes_cmac_one(tmp, a, 16, t))
+ return false;
+
+ if (!aes_cmac_one(t, id6, sizeof(id6), tmp))
+ return false;
+
+ out6[0] = tmp[15] & 0x3f;
+ return true;
+}
+
+bool mesh_crypto_beacon_cmac(const uint8_t encryption_key[16],
+ const uint8_t network_id[8],
+ uint32_t iv_index, bool kr, bool iu,
+ uint64_t *cmac)
+{
+ uint8_t msg[13], tmp[16];
+
+ if (!cmac)
+ return false;
+
+ msg[0] = kr ? 0x01 : 0x00;
+ msg[0] |= iu ? 0x02 : 0x00;
+ memcpy(msg + 1, network_id, 8);
+ l_put_be32(iv_index, msg + 9);
+
+ if (!aes_cmac_one(encryption_key, msg, 13, tmp))
+ return false;
+
+ *cmac = l_get_be64(tmp);
+
+ return true;
+}
+
+bool mesh_crypto_network_nonce(bool ctl, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint32_t iv_index,
+ uint8_t nonce[13])
+{
+ nonce[0] = 0;
+ nonce[1] = (ttl & TTL_MASK) | (ctl ? CTL : 0x00);
+ nonce[2] = (seq >> 16) & 0xff;
+ nonce[3] = (seq >> 8) & 0xff;
+ nonce[4] = seq & 0xff;
+
+ /* SRC */
+ l_put_be16(src, nonce + 5);
+
+ l_put_be16(0, nonce + 7);
+
+ /* IV Index */
+ l_put_be32(iv_index, nonce + 9);
+
+ return true;
+}
+
+bool mesh_crypto_network_encrypt(bool ctl, uint8_t ttl,
+ uint32_t seq, uint16_t src,
+ uint32_t iv_index,
+ const uint8_t net_key[16],
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *net_mic)
+{
+ uint8_t nonce[13];
+
+ if (!mesh_crypto_network_nonce(ctl, ttl, seq, src, iv_index, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_encrypt(nonce, net_key, NULL, 0, enc_msg,
+ enc_msg_len, out, net_mic,
+ ctl ? 8 : 4);
+}
+
+bool mesh_crypto_network_decrypt(bool ctl, uint8_t ttl,
+ uint32_t seq, uint16_t src,
+ uint32_t iv_index,
+ const uint8_t net_key[16],
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *net_mic, size_t mic_size)
+{
+ uint8_t nonce[13];
+
+ if (!mesh_crypto_network_nonce(ctl, ttl, seq, src, iv_index, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_decrypt(nonce, net_key, NULL, 0,
+ enc_msg, enc_msg_len, out,
+ net_mic, mic_size);
+}
+
+bool mesh_crypto_application_nonce(uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ bool aszmic, uint8_t nonce[13])
+{
+ nonce[0] = 0x01;
+ nonce[1] = aszmic ? 0x80 : 0x00;
+ nonce[2] = (seq & 0x00ff0000) >> 16;
+ nonce[3] = (seq & 0x0000ff00) >> 8;
+ nonce[4] = (seq & 0x000000ff);
+ nonce[5] = (src & 0xff00) >> 8;
+ nonce[6] = (src & 0x00ff);
+ nonce[7] = (dst & 0xff00) >> 8;
+ nonce[8] = (dst & 0x00ff);
+ l_put_be32(iv_index, nonce + 9);
+
+ return true;
+}
+
+bool mesh_crypto_device_nonce(uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ bool aszmic, uint8_t nonce[13])
+{
+ nonce[0] = 0x02;
+ nonce[1] = aszmic ? 0x80 : 0x00;
+ nonce[2] = (seq & 0x00ff0000) >> 16;
+ nonce[3] = (seq & 0x0000ff00) >> 8;
+ nonce[4] = (seq & 0x000000ff);
+ nonce[5] = (src & 0xff00) >> 8;
+ nonce[6] = (src & 0x00ff);
+ nonce[7] = (dst & 0xff00) >> 8;
+ nonce[8] = (dst & 0x00ff);
+ l_put_be32(iv_index, nonce + 9);
+
+ return true;
+}
+
+bool mesh_crypto_application_encrypt(uint8_t key_id, uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ const uint8_t app_key[16],
+ const uint8_t *aad, uint8_t aad_len,
+ const uint8_t *msg, uint8_t msg_len,
+ uint8_t *out,
+ void *app_mic, size_t mic_size)
+{
+ uint8_t nonce[13];
+ bool aszmic = (mic_size == 8) ? true : false;
+
+ if (!key_id && !mesh_crypto_device_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ if (key_id && !mesh_crypto_application_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_encrypt(nonce, app_key, aad, aad_len,
+ msg, msg_len,
+ out, app_mic, mic_size);
+}
+
+bool mesh_crypto_application_decrypt(uint8_t key_id, uint32_t seq, uint16_t src,
+ uint16_t dst, uint32_t iv_index,
+ const uint8_t app_key[16],
+ const uint8_t *aad, uint8_t aad_len,
+ const uint8_t *enc_msg, uint8_t enc_msg_len,
+ uint8_t *out, void *app_mic, size_t mic_size)
+{
+ uint8_t nonce[13];
+ bool aszmic = (mic_size == 8) ? true : false;
+
+ if (!key_id && !mesh_crypto_device_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ if (key_id && !mesh_crypto_application_nonce(seq, src, dst,
+ iv_index, aszmic, nonce))
+ return false;
+
+ return mesh_crypto_aes_ccm_decrypt(nonce, app_key,
+ aad, aad_len, enc_msg,
+ enc_msg_len, out,
+ app_mic, mic_size);
+}
+
+bool mesh_crypto_session_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t session_key[16])
+{
+ const uint8_t prsk[4] = "prsk";
+
+ if (!aes_cmac_one(salt, secret, 32, session_key))
+ return false;
+
+ return aes_cmac_one(session_key, prsk, 4, session_key);
+}
+
+bool mesh_crypto_nonce(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t nonce[13])
+{
+ const uint8_t prsn[4] = "prsn";
+ uint8_t tmp[16];
+ bool result;
+
+ if (!aes_cmac_one(salt, secret, 32, tmp))
+ return false;
+
+ result = aes_cmac_one(tmp, prsn, 4, tmp);
+
+ if (result)
+ memcpy(nonce, tmp + 3, 13);
+
+ return result;
+}
+
+bool mesh_crypto_s1(const void *info, size_t len, uint8_t salt[16])
+{
+ return aes_cmac_one(zero, info, len, salt);
+}
+
+bool mesh_crypto_prov_prov_salt(const uint8_t conf_salt[16],
+ const uint8_t prov_rand[16],
+ const uint8_t dev_rand[16],
+ uint8_t prov_salt[16])
+{
+ uint8_t tmp[16 * 3];
+
+ memcpy(tmp, conf_salt, 16);
+ memcpy(tmp + 16, prov_rand, 16);
+ memcpy(tmp + 32, dev_rand, 16);
+
+ return aes_cmac_one(zero, tmp, sizeof(tmp), prov_salt);
+}
+
+bool mesh_crypto_prov_conf_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t conf_key[16])
+{
+ const uint8_t prck[4] = "prck";
+
+ if (!aes_cmac_one(salt, secret, 32, conf_key))
+ return false;
+
+ return aes_cmac_one(conf_key, prck, 4, conf_key);
+}
+
+bool mesh_crypto_device_key(const uint8_t secret[32],
+ const uint8_t salt[16],
+ uint8_t device_key[16])
+{
+ const uint8_t prdk[4] = "prdk";
+
+ if (!aes_cmac_one(salt, secret, 32, device_key))
+ return false;
+
+ return aes_cmac_one(device_key, prdk, 4, device_key);
+}
+
+bool mesh_crypto_virtual_addr(const uint8_t virtual_label[16],
+ uint16_t *addr)
+{
+ uint8_t tmp[16];
+
+ if (!mesh_crypto_s1("vtad", 4, tmp))
+ return false;
+
+ if (!addr || !aes_cmac_one(tmp, virtual_label, 16, tmp))
+ return false;
+
+ *addr = (l_get_be16(tmp + 14) & 0x3fff) | 0x8000;
+
+ return true;
+}
+
+bool mesh_crypto_privacy_counter(uint32_t iv_index,
+ const uint8_t *payload,
+ uint8_t privacy_counter[16])
+{
+ memset(privacy_counter, 0, 5);
+ l_put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, payload, 7);
+
+ return true;
+}
+
+bool mesh_crypto_network_obfuscate(const uint8_t privacy_key[16],
+ const uint8_t privacy_counter[16],
+ bool ctl, uint8_t ttl, uint32_t seq,
+ uint16_t src, uint8_t *out)
+{
+ uint8_t ecb[16], tmp[16];
+ int i;
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, ecb))
+ return false;
+
+ tmp[0] = ((!!ctl) << 7) | (ttl & TTL_MASK);
+ tmp[1] = (seq & 0xff0000) >> 16;
+ tmp[2] = (seq & 0x00ff00) >> 8;
+ tmp[3] = (seq & 0x0000ff);
+ tmp[4] = (src & 0xff00) >> 8;
+ tmp[5] = (src & 0x00ff);
+
+ if (out) {
+ for (i = 0; i < 6; i++)
+ out[i] = ecb[i] ^ tmp[i];
+ }
+
+ return true;
+}
+
+bool mesh_crypto_network_clarify(const uint8_t privacy_key[16],
+ const uint8_t privacy_counter[16],
+ const uint8_t net_hdr[6],
+ bool *ctl, uint8_t *ttl,
+ uint32_t *seq, uint16_t *src)
+{
+ uint8_t ecb[16], tmp[6];
+ int i;
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, ecb))
+ return false;
+
+ for (i = 0; i < 6; i++)
+ tmp[i] = ecb[i] ^ net_hdr[i];
+
+ if (ctl)
+ *ctl = !!(tmp[0] & CTL);
+
+ if (ttl)
+ *ttl = tmp[0] & TTL_MASK;
+
+ if (seq)
+ *seq = l_get_be32(tmp) & SEQ_MASK;
+
+ if (src)
+ *src = l_get_be16(tmp + 4);
+
+ return true;
+}
+
+bool mesh_crypto_packet_build(bool ctl, uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint8_t opcode,
+ bool segmented, uint8_t key_id,
+ bool szmic, bool relay, uint16_t seqZero,
+ uint8_t segO, uint8_t segN,
+ const uint8_t *payload, uint8_t payload_len,
+ uint8_t *packet, uint8_t *packet_len)
+{
+ uint32_t hdr;
+ size_t n;
+
+ l_put_be32(seq, packet + 1);
+ packet[1] = (ctl ? CTL : 0) | (ttl & TTL_MASK);
+
+ l_put_be16(src, packet + 5);
+ l_put_be16(dst, packet + 7);
+ n = 9;
+
+ if (!ctl) {
+ hdr = segmented << SEG_HDR_SHIFT;
+ hdr |= (key_id & KEY_ID_MASK) << KEY_HDR_SHIFT;
+ if (segmented) {
+ hdr |= szmic << SZMIC_HDR_SHIFT;
+ hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT;
+ hdr |= (segO & SEG_MASK) << SEGO_HDR_SHIFT;
+ hdr |= (segN & SEG_MASK) << SEGN_HDR_SHIFT;
+ }
+ l_put_be32(hdr, packet + n);
+
+ /* Only first octet is valid for unsegmented messages */
+ if (segmented)
+ n += 4;
+ else
+ n += 1;
+
+ memcpy(packet + n, payload, payload_len);
+
+ l_put_be32(0x00000000, packet + payload_len + n);
+ if (packet_len)
+ *packet_len = payload_len + n + 4;
+ } else {
+ if ((opcode & OPCODE_MASK) != opcode)
+ return false;
+
+ hdr = opcode << KEY_HDR_SHIFT;
+ l_put_be32(hdr, packet + n);
+ n += 1;
+
+ memcpy(packet + n, payload, payload_len);
+ n += payload_len;
+
+ l_put_be64(0x0000000000000000, packet + n);
+ if (packet_len)
+ *packet_len = n + 8;
+ }
+
+
+ return true;
+}
+
+bool mesh_crypto_packet_parse(const uint8_t *packet, uint8_t packet_len,
+ bool *ctl, uint8_t *ttl, uint32_t *seq,
+ uint16_t *src, uint16_t *dst,
+ uint32_t *cookie, uint8_t *opcode,
+ bool *segmented, uint8_t *key_id,
+ bool *szmic, bool *relay, uint16_t *seqZero,
+ uint8_t *segO, uint8_t *segN,
+ const uint8_t **payload, uint8_t *payload_len)
+{
+ uint32_t hdr;
+ uint16_t this_dst;
+ bool is_segmented;
+
+ if (packet_len < 14)
+ return false;
+
+ this_dst = l_get_be16(packet + 7);
+
+ /* Try to keep bits in the order they exist within the packet */
+ if (ctl)
+ *ctl = !!(packet[1] & CTL);
+
+ if (ttl)
+ *ttl = packet[1] & TTL_MASK;
+
+ if (seq)
+ *seq = l_get_be32(packet + 1) & SEQ_MASK;
+
+ if (src)
+ *src = l_get_be16(packet + 5);
+
+ if (dst)
+ *dst = this_dst;
+
+ hdr = l_get_be32(packet + 9);
+
+ is_segmented = !!((hdr >> SEG_HDR_SHIFT) & true);
+ if (segmented)
+ *segmented = is_segmented;
+
+ if (packet[1] & CTL) {
+ uint8_t this_opcode = packet[9] & OPCODE_MASK;
+
+ if (cookie)
+ *cookie = l_get_be32(packet + 9);
+
+ if (opcode)
+ *opcode = this_opcode;
+
+ if (this_dst && this_opcode == NET_OP_SEG_ACKNOWLEDGE) {
+ if (relay)
+ *relay = !!((hdr >> RELAY_HDR_SHIFT) & true);
+
+ if (seqZero)
+ *seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) &
+ SEQ_ZERO_MASK;
+
+ if (payload)
+ *payload = packet + 9;
+
+ if (payload_len)
+ *payload_len = packet_len - 9 - 8;
+ } else {
+ if (payload)
+ *payload = packet + 10;
+
+ if (payload_len)
+ *payload_len = packet_len - 10 - 8;
+ }
+ } else {
+ if (cookie)
+ *cookie = l_get_be32(packet + packet_len - 8);
+
+ if (key_id)
+ *key_id = (hdr >> KEY_HDR_SHIFT) & KEY_ID_MASK;
+
+ if (is_segmented) {
+ if (szmic)
+ *szmic = !!((hdr >> SZMIC_HDR_SHIFT) & true);
+
+ if (seqZero)
+ *seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) &
+ SEQ_ZERO_MASK;
+
+ if (segO)
+ *segO = (hdr >> SEGO_HDR_SHIFT) & SEG_MASK;
+
+ if (segN)
+ *segN = (hdr >> SEGN_HDR_SHIFT) & SEG_MASK;
+
+ if (payload)
+ *payload = packet + 13;
+
+ if (payload_len)
+ *payload_len = packet_len - 13 - 4;
+ } else {
+ if (payload)
+ *payload = packet + 10;
+
+ if (payload_len)
+ *payload_len = packet_len - 10 - 4;
+ }
+ }
+
+ return true;
+}
+
+bool mesh_crypto_payload_encrypt(uint8_t *aad, const uint8_t *payload,
+ uint8_t *out, uint16_t payload_len,
+ uint16_t src, uint16_t dst, uint8_t key_id,
+ uint32_t seq_num, uint32_t iv_index,
+ bool aszmic,
+ const uint8_t application_key[16])
+{
+ uint8_t application_nonce[13] = { 0x01, };
+
+ if (payload_len < 1)
+ return false;
+
+ /* Key_ID == 0 means the Device Key is being used */
+ if (!key_id)
+ application_nonce[0] = 0x02;
+
+ /* Seq Num */
+ l_put_be32(seq_num, application_nonce + 1);
+
+ /* ASZMIC */
+ application_nonce[1] |= aszmic ? 0x80 : 0x00;
+
+ /* SRC */
+ l_put_be16(src, application_nonce + 5);
+
+ /* DST */
+ l_put_be16(dst, application_nonce + 7);
+
+ /* IV Index */
+ l_put_be32(iv_index, application_nonce + 9);
+
+ if (!mesh_crypto_aes_ccm_encrypt(application_nonce, application_key,
+ aad, aad ? 16 : 0,
+ payload, payload_len,
+ out, NULL,
+ aszmic ? 8 : 4))
+ return false;
+
+ return true;
+}
+
+bool mesh_crypto_payload_decrypt(uint8_t *aad, uint16_t aad_len,
+ const uint8_t *payload, uint16_t payload_len,
+ bool szmict,
+ uint16_t src, uint16_t dst,
+ uint8_t key_id, uint32_t seq_num,
+ uint32_t iv_index, uint8_t *out,
+ const uint8_t app_key[16])
+{
+ uint8_t app_nonce[13] = { 0x01, };
+ uint32_t mic32;
+ uint64_t mic64;
+
+ if (payload_len < 5 || !out)
+ return false;
+
+ /* Key_ID == 0 means the Device Key is being used */
+ if (!key_id)
+ app_nonce[0] = 0x02;
+
+ /* Seq Num */
+ l_put_be32(seq_num, app_nonce + 1);
+
+ /* ASZMIC */
+ app_nonce[1] |= szmict ? 0x80 : 0x00;
+
+ /* SRC */
+ l_put_be16(src, app_nonce + 5);
+
+ /* DST */
+ l_put_be16(dst, app_nonce + 7);
+
+ /* IV Index */
+ l_put_be32(iv_index, app_nonce + 9);
+
+ memcpy(out, payload, payload_len);
+
+ if (szmict) {
+ if (!mesh_crypto_aes_ccm_decrypt(app_nonce, app_key,
+ aad, aad_len,
+ payload, payload_len,
+ out, &mic64, sizeof(mic64)))
+ return false;
+
+ mic64 ^= l_get_be64(out + payload_len - 8);
+ l_put_be64(mic64, out + payload_len - 8);
+
+ if (mic64)
+ return false;
+ } else {
+ if (!mesh_crypto_aes_ccm_decrypt(app_nonce, app_key,
+ aad, aad_len,
+ payload, payload_len,
+ out, &mic32, sizeof(mic32)))
+ return false;
+
+ mic32 ^= l_get_be32(out + payload_len - 4);
+ l_put_be32(mic32, out + payload_len - 4);
+
+ if (mic32)
+ return false;
+ }
+
+ return true;
+}
+
+bool mesh_crypto_packet_encode(uint8_t *packet, uint8_t packet_len,
+ const uint8_t network_key[16],
+ uint32_t iv_index,
+ const uint8_t privacy_key[16])
+{
+ uint8_t network_nonce[13] = { 0x00, 0x00 };
+ uint8_t privacy_counter[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, };
+ uint8_t tmp[16];
+ int i;
+
+ if (packet_len < 14)
+ return false;
+
+ /* Detect Proxy packet by CTL == true && DST == 0x0000 */
+ if ((packet[1] & CTL) && l_get_be16(packet + 7) == 0)
+ network_nonce[0] = 0x03; /* Proxy Nonce */
+ else
+ /* CTL + TTL */
+ network_nonce[1] = packet[1];
+
+ /* Seq Num */
+ network_nonce[2] = packet[2];
+ network_nonce[3] = packet[3];
+ network_nonce[4] = packet[4];
+
+ /* SRC */
+ network_nonce[5] = packet[5];
+ network_nonce[6] = packet[6];
+
+ /* DST not available */
+ network_nonce[7] = 0;
+ network_nonce[8] = 0;
+
+ /* IV Index */
+ l_put_be32(iv_index, network_nonce + 9);
+
+ /* Check for Long net-MIC */
+ if (packet[1] & CTL) {
+ if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key,
+ NULL, 0,
+ packet + 7, packet_len - 7 - 8,
+ packet + 7, NULL, 8))
+ return false;
+ } else {
+ if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key,
+ NULL, 0,
+ packet + 7, packet_len - 7 - 4,
+ packet + 7, NULL, 4))
+ return false;
+ }
+
+ l_put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, packet + 7, 7);
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, tmp))
+ return false;
+
+ for (i = 0; i < 6; i++)
+ packet[1 + i] ^= tmp[i];
+
+ return true;
+}
+
+bool mesh_crypto_packet_decode(const uint8_t *packet, uint8_t packet_len,
+ bool proxy, uint8_t *out, uint32_t iv_index,
+ const uint8_t network_key[16],
+ const uint8_t privacy_key[16])
+{
+ uint8_t privacy_counter[16] = { 0x00, 0x00, 0x00, 0x00, 0x00, };
+ uint8_t network_nonce[13] = { 0x00, 0x00, };
+ uint8_t tmp[16];
+ uint16_t src;
+ int i;
+
+ if (packet_len < 14)
+ return false;
+
+ l_put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, packet + 7, 7);
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, tmp))
+ return false;
+
+ memcpy(out, packet, packet_len);
+ for (i = 0; i < 6; i++)
+ out[1 + i] ^= tmp[i];
+
+ src = l_get_be16(out + 5);
+
+ /* Pre-check SRC address for illegal values */
+ if (!src || src >= 0x8000)
+ return false;
+
+ /* Detect Proxy packet by CTL == true && proxy == true */
+ if ((out[1] & CTL) && proxy)
+ network_nonce[0] = 0x03; /* Proxy Nonce */
+ else
+ /* CTL + TTL */
+ network_nonce[1] = out[1];
+
+ /* Seq Num */
+ network_nonce[2] = out[2];
+ network_nonce[3] = out[3];
+ network_nonce[4] = out[4];
+
+ /* SRC */
+ network_nonce[5] = out[5];
+ network_nonce[6] = out[6];
+
+ /* DST not available */
+ network_nonce[7] = 0;
+ network_nonce[8] = 0;
+
+ /* IV Index */
+ l_put_be32(iv_index, network_nonce + 9);
+
+ /* Check for Long MIC */
+ if (out[1] & CTL) {
+ uint64_t mic;
+
+ if (!mesh_crypto_aes_ccm_decrypt(network_nonce, network_key,
+ NULL, 0, packet + 7, packet_len - 7,
+ out + 7, &mic, sizeof(mic)))
+ return false;
+
+ mic ^= l_get_be64(out + packet_len - 8);
+ l_put_be64(mic, out + packet_len - 8);
+
+ if (mic)
+ return false;
+ } else {
+ uint32_t mic;
+
+ if (!mesh_crypto_aes_ccm_decrypt(network_nonce, network_key,
+ NULL, 0, packet + 7, packet_len - 7,
+ out + 7, &mic, sizeof(mic)))
+ return false;
+
+ mic ^= l_get_be32(out + packet_len - 4);
+ l_put_be32(mic, out + packet_len - 4);
+
+ if (mic)
+ return false;
+ }
+
+ return true;
+}
+
+bool mesh_crypto_packet_label(uint8_t *packet, uint8_t packet_len,
+ uint16_t iv_index, uint8_t network_id)
+{
+ packet[0] = (iv_index & 0x0001) << 7 | (network_id & 0x7f);
+
+ return true;
+}
+
+/* reversed, 8-bit, poly=0x07 */
+static const uint8_t crc_table[256] = {
+ 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75,
+ 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b,
+ 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69,
+ 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67,
+
+ 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d,
+ 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43,
+ 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51,
+ 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f,
+
+ 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05,
+ 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b,
+ 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19,
+ 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17,
+
+ 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d,
+ 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33,
+ 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21,
+ 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f,
+
+ 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95,
+ 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b,
+ 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89,
+ 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87,
+
+ 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad,
+ 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3,
+ 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1,
+ 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf,
+
+ 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5,
+ 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb,
+ 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9,
+ 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7,
+
+ 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd,
+ 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3,
+ 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1,
+ 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf
+};
+
+uint8_t mesh_crypto_compute_fcs(const uint8_t *packet, uint8_t packet_len)
+{
+ uint8_t fcs = 0xff;
+ int i;
+
+ for (i = 0; i < packet_len; i++)
+ fcs = crc_table[fcs ^ packet[i]];
+
+ return 0xff - fcs;
+}
+
+bool mesh_crypto_check_fcs(const uint8_t *packet, uint8_t packet_len,
+ uint8_t received_fcs)
+{
+ uint8_t fcs = 0xff;
+ int i;
+
+ for (i = 0; i < packet_len; i++)
+ fcs = crc_table[fcs ^ packet[i]];
+
+ fcs = crc_table[fcs ^ received_fcs];
+
+ return fcs == 0xcf;
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 03/14] mesh: Infrastructure for Mesh daemon
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 01/14] mesh: Shared private meshd interfaces Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 02/14] mesh: Mesh crypto support Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 04/14] mesh: Initial Mesh Friendship support Brian Gix
` (11 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/display.c | 64 +++++
mesh/mesh-io-generic.c | 660 +++++++++++++++++++++++++++++++++++++++++++++++++
mesh/mesh-io.c | 188 ++++++++++++++
3 files changed, 912 insertions(+)
create mode 100644 mesh/display.c
create mode 100644 mesh/mesh-io-generic.c
create mode 100644 mesh/mesh-io.c
diff --git a/mesh/display.c b/mesh/display.c
new file mode 100644
index 000000000..8238ae849
--- /dev/null
+++ b/mesh/display.c
@@ -0,0 +1,64 @@
+/*
+ *
+ * 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.
+ *
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <termios.h>
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "display.h"
+
+static unsigned int cached_num_columns;
+
+unsigned int num_columns(void)
+{
+ struct winsize ws;
+
+ if (cached_num_columns > 0)
+ return cached_num_columns;
+
+ if (ioctl(STDOUT_FILENO, TIOCGWINSZ, &ws) < 0 || ws.ws_col == 0)
+ cached_num_columns = 80;
+ else
+ cached_num_columns = ws.ws_col;
+
+ return cached_num_columns;
+}
+
+void print_packet(const char *label, const void *data, uint16_t size)
+{
+ struct timeval pkt_time;
+
+ gettimeofday(&pkt_time, NULL);
+
+ if (size > 0) {
+ char *str;
+
+ str = l_util_hexstring(data, size);
+ l_debug("%05d.%03d %s: %s",
+ (uint32_t) pkt_time.tv_sec % 100000,
+ (uint32_t) pkt_time.tv_usec/1000, label, str);
+ l_free(str);
+ } else
+ l_debug("%05d.%03d %s: empty",
+ (uint32_t) pkt_time.tv_sec % 100000,
+ (uint32_t) pkt_time.tv_usec/1000, label);
+}
diff --git a/mesh/mesh-io-generic.c b/mesh/mesh-io-generic.c
new file mode 100644
index 000000000..52514e280
--- /dev/null
+++ b/mesh/mesh-io-generic.c
@@ -0,0 +1,660 @@
+/*
+ *
+ * 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 "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "monitor/bt.h"
+#include "src/shared/hci.h"
+
+#include "mesh/display.h"
+#include "mesh/mesh-io.h"
+#include "mesh/mesh-io-api.h"
+
+#include "mesh/mesh-io-generic.h"
+
+struct mesh_io_private {
+ uint16_t index;
+ struct bt_hci *hci;
+ struct l_timeout *tx_timeout;
+ struct l_queue *rx_regs;
+ struct l_queue *tx_pkts;
+ uint8_t filters[3]; /* Simple filtering on AD type only */
+ bool sending;
+ struct tx_pkt *tx;
+ uint16_t interval;
+};
+
+struct pvt_rx_reg {
+ uint8_t filter_id;
+ mesh_io_recv_func_t cb;
+ void *user_data;
+};
+
+struct process_data {
+ struct mesh_io_private *pvt;
+ const uint8_t *data;
+ uint8_t len;
+ struct mesh_io_recv_info info;
+};
+
+struct tx_pkt {
+ struct mesh_io_send_info info;
+ bool delete;
+ uint8_t len;
+ uint8_t pkt[30];
+};
+
+struct tx_pattern {
+ const uint8_t *data;
+ uint8_t len;
+};
+
+static uint32_t get_instant(void)
+{
+ struct timeval tm;
+ uint32_t instant;
+
+ gettimeofday(&tm, NULL);
+ instant = tm.tv_sec * 1000;
+ instant += tm.tv_usec / 1000;
+
+ return instant;
+}
+
+static uint32_t instant_remaining_ms(uint32_t instant)
+{
+ instant -= get_instant();
+ return instant;
+}
+
+static void process_rx_callbacks(void *v_rx, void *v_reg)
+{
+ struct pvt_rx_reg *rx_reg = v_rx;
+ struct process_data *rx = v_reg;
+ uint8_t ad_type;
+
+ ad_type = rx->pvt->filters[rx_reg->filter_id - 1];
+
+ if (rx->data[0] == ad_type && rx_reg->cb)
+ rx_reg->cb(rx_reg->user_data, &rx->info, rx->data, rx->len);
+}
+
+static void process_rx(struct mesh_io_private *pvt, int8_t rssi,
+ uint32_t instant,
+ const uint8_t *data, uint8_t len)
+{
+ struct process_data rx = {
+ .pvt = pvt,
+ .data = data,
+ .len = len,
+ .info.instant = instant,
+ .info.chan = 7,
+ .info.rssi = rssi,
+ };
+
+ l_queue_foreach(pvt->rx_regs, process_rx_callbacks, &rx);
+}
+
+static void event_adv_report(struct mesh_io *io, const void *buf, uint8_t size)
+{
+ const struct bt_hci_evt_le_adv_report *evt = buf;
+ const uint8_t *adv;
+ uint32_t instant;
+ uint8_t adv_len;
+ uint16_t len = 0;
+ int8_t rssi;
+
+ if (evt->event_type != 0x03)
+ return;
+
+ if (evt->addr_type != BDADDR_LE_PUBLIC &&
+ evt->addr_type != BDADDR_LE_RANDOM)
+ return;
+
+ instant = get_instant();
+ adv = evt->data;
+ adv_len = evt->data_len;
+
+ /* rssi is just beyond last byte of data */
+ rssi = (int8_t) adv[adv_len];
+
+ while (len < adv_len - 1) {
+ uint8_t field_len = adv[0];
+
+ /* Check for the end of advertising data */
+ if (field_len == 0)
+ break;
+
+ len += field_len + 1;
+
+ /* Do not continue data parsing if got incorrect length */
+ if (len > adv_len)
+ break;
+
+ /* TODO: Create an Instant to use */
+ process_rx(io->pvt, rssi, instant, adv + 1, adv[0]);
+
+ adv += field_len + 1;
+ }
+}
+
+static void event_callback(const void *buf, uint8_t size, void *user_data)
+{
+ uint8_t event = l_get_u8(buf);
+ struct mesh_io *io = user_data;
+
+ switch (event) {
+ case BT_HCI_EVT_LE_ADV_REPORT:
+ event_adv_report(io, buf + 1, size - 1);
+ break;
+
+ default:
+ l_info("Other Meta Evt - %d", event);
+ }
+}
+
+static bool dev_init(uint16_t index, struct mesh_io *io)
+{
+ struct mesh_io_private *tmp;
+
+ if (!io || io->pvt)
+ return false;
+
+ tmp = l_new(struct mesh_io_private, 1);
+
+ if (tmp == NULL)
+ return false;
+
+ tmp->rx_regs = l_queue_new();
+ tmp->tx_pkts = l_queue_new();
+ if (!tmp->rx_regs || !tmp->tx_pkts)
+ goto fail;
+
+ tmp->hci = bt_hci_new_user_channel(index);
+ if (!tmp->hci)
+ goto fail;
+
+ bt_hci_register(tmp->hci, BT_HCI_EVT_LE_META_EVENT,
+ event_callback, io, NULL);
+
+ io->pvt = tmp;
+ return true;
+
+fail:
+ l_queue_destroy(tmp->rx_regs, l_free);
+ l_queue_destroy(tmp->tx_pkts, l_free);
+ l_free(tmp);
+ return false;
+}
+
+static bool dev_destroy(struct mesh_io *io)
+{
+ struct mesh_io_private *pvt = io->pvt;
+
+ if (!pvt)
+ return true;
+
+ bt_hci_unref(pvt->hci);
+ l_timeout_remove(pvt->tx_timeout);
+ l_queue_destroy(pvt->rx_regs, l_free);
+ l_queue_destroy(pvt->tx_pkts, l_free);
+ l_free(pvt);
+ io->pvt = NULL;
+
+ return true;
+}
+
+static bool dev_caps(struct mesh_io *io, struct mesh_io_caps *caps)
+{
+ struct mesh_io_private *pvt = io->pvt;
+
+ if (!pvt || !caps)
+ return false;
+
+ caps->max_num_filters = sizeof(pvt->filters);
+ caps->window_accuracy = 50;
+
+ return true;
+}
+
+static void send_cancel_done(const void *buf, uint8_t size,
+ void *user_data)
+{
+ struct mesh_io_private *pvt = user_data;
+ struct bt_hci_cmd_le_set_random_address cmd;
+
+ if (!pvt)
+ return;
+
+ pvt->sending = false;
+
+ /* At end of any burst of ADVs, change random address */
+ l_getrandom(cmd.addr, 6);
+ cmd.addr[5] |= 0xc0;
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_RANDOM_ADDRESS,
+ &cmd, sizeof(cmd), NULL, NULL, NULL);
+}
+
+static void send_cancel(struct mesh_io_private *pvt)
+{
+ struct bt_hci_cmd_le_set_adv_enable cmd;
+
+ if (!pvt)
+ return;
+
+ if (!pvt->sending) {
+ send_cancel_done(NULL, 0, pvt);
+ return;
+ }
+
+ cmd.enable = 0x00; /* Disable advertising */
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
+ &cmd, sizeof(cmd),
+ send_cancel_done, pvt, NULL);
+}
+
+static void set_send_adv_enable(const void *buf, uint8_t size,
+ void *user_data)
+{
+ struct mesh_io_private *pvt = user_data;
+ struct bt_hci_cmd_le_set_adv_enable cmd;
+
+ if (!pvt)
+ return;
+
+ pvt->sending = true;
+ cmd.enable = 0x01; /* Enable advertising */
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
+ &cmd, sizeof(cmd), NULL, NULL, NULL);
+}
+
+static void set_send_adv_data(const void *buf, uint8_t size,
+ void *user_data)
+{
+ struct mesh_io_private *pvt = user_data;
+ struct tx_pkt *tx;
+ struct bt_hci_cmd_le_set_adv_data cmd;
+
+ if (!pvt || !pvt->tx)
+ return;
+
+ tx = pvt->tx;
+ if (tx->len >= sizeof(cmd.data))
+ goto done;
+
+ memset(&cmd, 0, sizeof(cmd));
+
+ cmd.len = tx->len + 1;
+ cmd.data[0] = tx->len;
+ memcpy(cmd.data + 1, tx->pkt, tx->len);
+
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_DATA,
+ &cmd, sizeof(cmd),
+ set_send_adv_enable, pvt, NULL);
+done:
+ if (tx->delete)
+ l_free(tx);
+
+ pvt->tx = NULL;
+}
+
+static void set_send_adv_params(const void *buf, uint8_t size,
+ void *user_data)
+{
+ struct mesh_io_private *pvt = user_data;
+ struct bt_hci_cmd_le_set_adv_parameters cmd;
+ uint16_t hci_interval;
+
+ if (!pvt)
+ return;
+
+ hci_interval = (pvt->interval * 16) / 10;
+ cmd.min_interval = L_CPU_TO_LE16(hci_interval);
+ cmd.max_interval = L_CPU_TO_LE16(hci_interval);
+ cmd.type = 0x03; /* ADV_NONCONN_IND */
+ cmd.own_addr_type = 0x01; /* ADDR_TYPE_RANDOM */
+ cmd.direct_addr_type = 0x00;
+ memset(cmd.direct_addr, 0, 6);
+ cmd.channel_map = 0x07;
+ cmd.filter_policy = 0x03;
+
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_PARAMETERS,
+ &cmd, sizeof(cmd),
+ set_send_adv_data, pvt, NULL);
+}
+
+static void send_pkt(struct mesh_io_private *pvt, struct tx_pkt *tx,
+ uint16_t interval)
+{
+ struct bt_hci_cmd_le_set_adv_enable cmd;
+
+ pvt->tx = tx;
+ pvt->interval = interval;
+
+ if (!pvt->sending) {
+ set_send_adv_params(NULL, 0, pvt);
+ return;
+ }
+
+ cmd.enable = 0x00; /* Disable advertising */
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_ADV_ENABLE,
+ &cmd, sizeof(cmd),
+ set_send_adv_params, pvt, NULL);
+}
+
+static void tx_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_io_private *pvt = user_data;
+ struct tx_pkt *tx;
+ uint16_t ms;
+ uint8_t count;
+
+ if (!pvt)
+ return;
+
+ tx = l_queue_pop_head(pvt->tx_pkts);
+ if (!tx) {
+ l_timeout_remove(timeout);
+ pvt->tx_timeout = NULL;
+ send_cancel(pvt);
+ return;
+ }
+
+ if (tx->info.type == MESH_IO_TIMING_TYPE_GENERAL) {
+ ms = tx->info.u.gen.interval;
+ count = tx->info.u.gen.cnt;
+ if (count != MESH_IO_TX_COUNT_UNLIMITED)
+ tx->info.u.gen.cnt--;
+ } else {
+ ms = 25;
+ count = 1;
+ }
+
+ tx->delete = !!(count == 1);
+
+ send_pkt(pvt, tx, ms);
+
+ if (count == 1) {
+ /* send_pkt will delete when done */
+ tx = l_queue_peek_head(pvt->tx_pkts);
+ if (tx && tx->info.type == MESH_IO_TIMING_TYPE_POLL_RSP) {
+ ms = instant_remaining_ms(tx->info.u.poll_rsp.instant +
+ tx->info.u.poll_rsp.delay);
+ }
+ } else
+ l_queue_push_tail(pvt->tx_pkts, tx);
+
+ if (timeout) {
+ pvt->tx_timeout = timeout;
+ l_timeout_modify_ms(timeout, ms);
+ } else
+ pvt->tx_timeout = l_timeout_create_ms(ms, tx_timeout,
+ pvt, NULL);
+}
+
+static void tx_worker(void *user_data)
+{
+ struct mesh_io_private *pvt = user_data;
+ struct tx_pkt *tx;
+ uint32_t delay;
+
+ tx = l_queue_peek_head(pvt->tx_pkts);
+ if (!tx)
+ return;
+
+ switch (tx->info.type) {
+ case MESH_IO_TIMING_TYPE_GENERAL:
+ if (tx->info.u.gen.min_delay == tx->info.u.gen.max_delay)
+ delay = tx->info.u.gen.min_delay;
+ else {
+ l_getrandom(&delay, sizeof(delay));
+ delay %= tx->info.u.gen.max_delay -
+ tx->info.u.gen.min_delay;
+ delay += tx->info.u.gen.min_delay;
+ }
+ break;
+
+ case MESH_IO_TIMING_TYPE_POLL:
+ if (tx->info.u.poll.min_delay == tx->info.u.poll.max_delay)
+ delay = tx->info.u.poll.min_delay;
+ else {
+ l_getrandom(&delay, sizeof(delay));
+ delay %= tx->info.u.poll.max_delay -
+ tx->info.u.poll.min_delay;
+ delay += tx->info.u.poll.min_delay;
+ }
+ break;
+
+ case MESH_IO_TIMING_TYPE_POLL_RSP:
+ /* Delay until Instant + Delay */
+ delay = instant_remaining_ms(tx->info.u.poll_rsp.instant +
+ tx->info.u.poll_rsp.delay);
+ if (delay > 255)
+ delay = 0;
+ break;
+
+ default:
+ return;
+ }
+
+ if (!delay)
+ tx_timeout(pvt->tx_timeout, pvt);
+ else if (pvt->tx_timeout)
+ l_timeout_modify_ms(pvt->tx_timeout, delay);
+ else
+ pvt->tx_timeout = l_timeout_create_ms(delay, tx_timeout,
+ pvt, NULL);
+}
+
+static bool send_tx(struct mesh_io *io, struct mesh_io_send_info *info,
+ const uint8_t *data, uint16_t len)
+{
+ struct mesh_io_private *pvt = io->pvt;
+ struct tx_pkt *tx;
+ bool sending = false;
+
+ if (!info || !data || !len || len > sizeof(tx->pkt))
+ return false;
+
+
+ tx = l_new(struct tx_pkt, 1);
+ if (!tx)
+ return false;
+
+ memcpy(&tx->info, info, sizeof(tx->info));
+ memcpy(&tx->pkt, data, len);
+ tx->len = len;
+
+ if (info->type == MESH_IO_TIMING_TYPE_POLL_RSP)
+ l_queue_push_head(pvt->tx_pkts, tx);
+ else {
+ sending = !l_queue_isempty(pvt->tx_pkts);
+ l_queue_push_tail(pvt->tx_pkts, tx);
+ }
+
+ if (!sending) {
+ l_timeout_remove(pvt->tx_timeout);
+ pvt->tx_timeout = NULL;
+ l_idle_oneshot(tx_worker, pvt, NULL);
+ }
+
+ return true;
+}
+
+static bool find_by_ad_type(const void *a, const void *b)
+{
+ const struct tx_pkt *tx = a;
+ uint8_t ad_type = L_PTR_TO_UINT(b);
+
+ return !ad_type || ad_type == tx->pkt[0];
+}
+
+static bool find_by_pattern(const void *a, const void *b)
+{
+ const struct tx_pkt *tx = a;
+ const struct tx_pattern *pattern = b;
+
+ if (tx->len < pattern->len)
+ return false;
+
+ return (!memcmp(tx->pkt, pattern->data, pattern->len));
+}
+
+static bool tx_cancel(struct mesh_io *io, uint8_t *data, uint8_t len)
+{
+ struct mesh_io_private *pvt = io->pvt;
+ struct tx_pkt *tx;
+
+ if (!data)
+ return false;
+
+ if (len == 1) {
+ do {
+ tx = l_queue_remove_if(pvt->tx_pkts, find_by_ad_type,
+ L_UINT_TO_PTR(data[0]));
+ l_free(tx);
+ } while (tx);
+ } else {
+ struct tx_pattern pattern = {
+ .data = data,
+ .len = len
+ };
+
+ do {
+ tx = l_queue_remove_if(pvt->tx_pkts, find_by_pattern,
+ &pattern);
+ l_free(tx);
+ } while (tx);
+ }
+
+ if (l_queue_isempty(pvt->tx_pkts)) {
+ send_cancel(pvt);
+ l_timeout_remove(pvt->tx_timeout);
+ pvt->tx_timeout = NULL;
+ }
+
+ return true;
+}
+
+static bool find_by_filter_id(const void *a, const void *b)
+{
+ const struct pvt_rx_reg *rx_reg = a;
+ uint8_t filter_id = L_PTR_TO_UINT(b);
+
+ return rx_reg->filter_id == filter_id;
+}
+
+static bool recv_register(struct mesh_io *io, uint8_t filter_id,
+ mesh_io_recv_func_t cb, void *user_data)
+{
+ struct bt_hci_cmd_le_set_scan_enable cmd;
+ struct mesh_io_private *pvt = io->pvt;
+ struct pvt_rx_reg *rx_reg;
+ bool scanning;
+
+ l_info("%s %d", __func__, filter_id);
+ if (!cb || !filter_id || filter_id > sizeof(pvt->filters))
+ return false;
+
+ rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter_id,
+ L_UINT_TO_PTR(filter_id));
+
+ if (!rx_reg) {
+ rx_reg = l_new(struct pvt_rx_reg, 1);
+ if (!rx_reg)
+ return false;
+ }
+
+ rx_reg->filter_id = filter_id;
+ rx_reg->cb = cb;
+ rx_reg->user_data = user_data;
+
+ scanning = !l_queue_isempty(pvt->rx_regs);
+
+ l_queue_push_head(pvt->rx_regs, rx_reg);
+
+ if (!scanning) {
+ cmd.enable = 0x01; /* Enable scanning */
+ cmd.filter_dup = 0x00; /* Report duplicates */
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+ &cmd, sizeof(cmd), NULL, NULL, NULL);
+ }
+
+ return true;
+}
+
+static bool recv_deregister(struct mesh_io *io, uint8_t filter_id)
+{
+ struct bt_hci_cmd_le_set_scan_enable cmd;
+ struct mesh_io_private *pvt = io->pvt;
+
+ struct pvt_rx_reg *rx_reg;
+
+ rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter_id,
+ L_UINT_TO_PTR(filter_id));
+
+ if (rx_reg)
+ l_free(rx_reg);
+
+ if (l_queue_isempty(pvt->rx_regs)) {
+ cmd.enable = 0x00; /* Disable scanning */
+ cmd.filter_dup = 0x00; /* Report duplicates */
+ bt_hci_send(pvt->hci, BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+ &cmd, sizeof(cmd), NULL, NULL, NULL);
+
+ }
+
+ return true;
+}
+
+static bool filter_set(struct mesh_io *io,
+ uint8_t filter_id, const uint8_t *data, uint8_t len,
+ mesh_io_status_func_t callback, void *user_data)
+{
+ struct mesh_io_private *pvt = io->pvt;
+
+ l_info("%s id: %d, --> %2.2x", __func__, filter_id, data[0]);
+ if (!data || !len || !filter_id || filter_id > sizeof(pvt->filters))
+ return false;
+
+ pvt->filters[filter_id - 1] = data[0];
+
+ /* TODO: Delayed Call to successful status */
+
+ return true;
+}
+
+const struct mesh_io_api mesh_io_generic = {
+ .init = dev_init,
+ .destroy = dev_destroy,
+ .caps = dev_caps,
+ .send = send_tx,
+ .reg = recv_register,
+ .dereg = recv_deregister,
+ .set = filter_set,
+ .cancel = tx_cancel,
+};
diff --git a/mesh/mesh-io.c b/mesh/mesh-io.c
new file mode 100644
index 000000000..5cdfdc5ff
--- /dev/null
+++ b/mesh/mesh-io.c
@@ -0,0 +1,188 @@
+/*
+ *
+ * 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 <ell/ell.h>
+
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh-io.h"
+#include "mesh/mesh-io-api.h"
+
+/* List of Mesh-IO Type headers */
+#include "mesh/mesh-io-generic.h"
+
+/* List of Supported Mesh-IO Types */
+static const struct mesh_io_table table[] = {
+ {MESH_IO_TYPE_GENERIC, &mesh_io_generic}
+};
+
+static struct l_queue *io_list;
+
+static bool match_by_io(const void *a, const void *b)
+{
+ return a == b;
+}
+
+static bool match_by_index(const void *a, const void *b)
+{
+ const struct mesh_io *io = a;
+
+ return io->index == L_PTR_TO_UINT(b);
+}
+
+struct mesh_io *mesh_io_new(uint16_t index, enum mesh_io_type type)
+{
+ const struct mesh_io_api *api = NULL;
+ struct mesh_io *io;
+ uint16_t i;
+
+ l_info("%s %d\n", __func__, type);
+
+ for (i = 0; i < L_ARRAY_SIZE(table); i++) {
+ if (table[i].type == type) {
+ api = table[i].api;
+ break;
+ }
+ }
+
+ io = l_queue_find(io_list, match_by_index, L_UINT_TO_PTR(index));
+
+ if (!api || !api->init || io)
+ return NULL;
+
+ io = l_new(struct mesh_io, 1);
+
+ if (!io)
+ return NULL;
+
+ io->type = type;
+ io->index = index;
+ io->api = api;
+ if (!api->init(index, io))
+ goto fail;
+
+ if (!io_list)
+ io_list = l_queue_new();
+
+ if (api->set) {
+ uint8_t pkt = MESH_AD_TYPE_NETWORK;
+ uint8_t bec = MESH_AD_TYPE_BEACON;
+ uint8_t prv = MESH_AD_TYPE_PROVISION;
+
+ api->set(io, 1, &bec, 1, NULL, NULL);
+ api->set(io, 2, &prv, 1, NULL, NULL);
+ api->set(io, 3, &pkt, 1, NULL, NULL);
+ }
+
+ if (l_queue_push_head(io_list, io))
+ return io;
+
+fail:
+ if (api->destroy)
+ api->destroy(io);
+
+ l_free(io);
+ return NULL;
+}
+
+void mesh_io_destroy(struct mesh_io *io)
+{
+ io = l_queue_remove_if(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->destroy)
+ io->api->destroy(io);
+
+ l_free(io);
+
+ if (l_queue_isempty(io_list)) {
+ l_queue_destroy(io_list, NULL);
+ io_list = NULL;
+ }
+}
+
+bool mesh_io_get_caps(struct mesh_io *io, struct mesh_io_caps *caps)
+{
+ io = l_queue_find(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->caps)
+ return io->api->caps(io, caps);
+
+ return false;
+}
+
+bool mesh_io_register_recv_cb(struct mesh_io *io, uint8_t filter_id,
+ mesh_io_recv_func_t cb, void *user_data)
+{
+ io = l_queue_find(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->reg)
+ return io->api->reg(io, filter_id, cb, user_data);
+
+ return false;
+}
+
+bool mesh_io_deregister_recv_cb(struct mesh_io *io, uint8_t filter_id)
+{
+ io = l_queue_find(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->dereg)
+ return io->api->dereg(io, filter_id);
+
+ return false;
+}
+
+bool mesh_set_filter(struct mesh_io *io, uint8_t filter_id,
+ const uint8_t *data, uint8_t len,
+ mesh_io_status_func_t cb, void *user_data)
+{
+ io = l_queue_find(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->set)
+ return io->api->set(io, filter_id, data, len, cb, user_data);
+
+ return false;
+}
+
+bool mesh_io_send(struct mesh_io *io, struct mesh_io_send_info *info,
+ const uint8_t *data, uint16_t len)
+{
+ io = l_queue_find(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->send)
+ return io->api->send(io, info, data, len);
+
+ return false;
+}
+
+bool mesh_io_send_cancel(struct mesh_io *io, uint8_t *pattern, uint8_t len)
+{
+ io = l_queue_find(io_list, match_by_io, io);
+
+ if (io && io->api && io->api->cancel)
+ return io->api->cancel(io, pattern, len);
+
+ return false;
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 04/14] mesh: Initial Mesh Friendship support
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (2 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 03/14] mesh: Infrastructure for Mesh daemon Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 05/14] mesh: Provisioning logic for mesh Brian Gix
` (10 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/friend.c | 1116 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 1116 insertions(+)
create mode 100644 mesh/friend.c
diff --git a/mesh/friend.c b/mesh/friend.c
new file mode 100644
index 000000000..5a4088ef1
--- /dev/null
+++ b/mesh/friend.c
@@ -0,0 +1,1116 @@
+/*
+ *
+ * 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 <unistd.h>
+#include <stdio.h>
+#include <sys/time.h>
+#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/model.h"
+#include "mesh/display.h"
+
+#include "mesh/friend.h"
+
+#define MAX_FRND_GROUPS 20
+#define FRND_RELAY_WINDOW 250 /* 250 ms */
+#define FRND_CACHE_SIZE 16
+#define FRND_SUB_LIST_SIZE 8
+
+#define RESPONSE_DELAY (100 - 12) /* 100 ms - 12ms hw delay */
+#define MIN_RESP_DELAY 10 /* 10 ms */
+#define MAX_RESP_DELAY 255 /* 255 ms */
+
+/* Absolute maximum time to wait for LPN to choose us. */
+#define RESPONSE_POLL_DELAY 1300 /* 1.300 s */
+
+static uint8_t frnd_relay_window = FRND_RELAY_WINDOW;
+static uint8_t frnd_cache_size = FRND_CACHE_SIZE;
+static uint8_t frnd_sublist_size = FRND_SUB_LIST_SIZE;
+
+struct frnd_negotiation {
+ struct l_timeout *timeout;
+ struct mesh_net *net;
+ struct mesh_key_set key_set;
+ uint32_t poll_timeout;
+ uint16_t low_power_node;
+ uint16_t old_relay;
+ uint8_t num_ele;
+ uint8_t lp_cnt;
+ uint8_t fn_cnt;
+ uint8_t wrfrw;
+ uint8_t receive_delay;
+ int8_t rssi;
+ bool clearing;
+};
+
+static struct l_queue *frnd_negotiations;
+static uint16_t counter;
+
+static void response_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct frnd_negotiation *neg = user_data;
+
+ /* LPN did not choose us */
+ l_info("Did not win negotiation for %4.4x", neg->low_power_node);
+
+ mesh_net_remove_keyset(neg->net, &neg->key_set);
+ l_queue_remove(frnd_negotiations, neg);
+ l_timeout_remove(timeout);
+ l_free(neg);
+}
+
+static void response_delay(struct l_timeout *timeout, void *user_data)
+{
+ struct frnd_negotiation *neg = user_data;
+ uint16_t net_idx = mesh_net_get_primary_idx(neg->net);
+ uint8_t key[16];
+ uint8_t p[9] = { 1 };
+ uint8_t msg[8];
+ uint16_t n = 0;
+ bool res;
+
+ l_timeout_remove(timeout);
+
+ /* Create key Set for this offer */
+ l_put_be16(neg->low_power_node, p + 1);
+ l_put_be16(mesh_net_get_address(neg->net), p + 3);
+ l_put_be16(neg->lp_cnt, p + 5);
+ l_put_be16(counter, p + 7);
+ res = mesh_net_get_key(neg->net, false, net_idx, key);
+ if (!res)
+ goto cleanup;
+
+ print_packet("Friend Key P =", p, 9);
+ res = mesh_crypto_k2(key, p, sizeof(p), &neg->key_set.nid,
+ neg->key_set.enc_key, neg->key_set.privacy_key);
+ if (!res)
+ goto cleanup;
+
+ print_packet("NID =", &neg->key_set.nid, 1);
+ print_packet("ENC_KEY =", neg->key_set.enc_key, 16);
+ print_packet("PRIV_KEY =", neg->key_set.privacy_key, 16);
+
+ neg->fn_cnt = counter++;
+ neg->key_set.frnd = true;
+ mesh_net_add_keyset(neg->net, &neg->key_set);
+
+ msg[n++] = NET_OP_FRND_OFFER;
+ msg[n++] = frnd_relay_window;
+ msg[n++] = frnd_cache_size;
+ msg[n++] = frnd_sublist_size;
+ msg[n++] = neg->rssi;
+ l_put_be16(neg->fn_cnt, msg + n);
+ n += 2;
+ print_packet("Tx-NET_OP_FRND_OFFER", msg, n);
+ mesh_net_transport_send(neg->net, NULL, true,
+ mesh_net_get_iv_index(neg->net), 0,
+ 0, 0, neg->low_power_node,
+ msg, n);
+
+ /* Offer expires in 1.3 seconds, which is the max time for LPN to
+ * receive all offers, 1 second to make decision, and a little extra
+ */
+ neg->timeout = l_timeout_create_ms(1000 + MAX_RESP_DELAY,
+ response_timeout, neg, NULL);
+
+ return;
+
+cleanup:
+ mesh_net_remove_keyset(neg->net, &neg->key_set);
+ l_queue_remove(frnd_negotiations, neg);
+ l_free(neg);
+}
+
+static uint8_t cache_size(uint8_t power)
+{
+ return 1 << power;
+}
+
+static bool match_by_lpn(const void *a, const void *b)
+{
+ const struct frnd_negotiation *neg = a;
+ uint16_t lpn = L_PTR_TO_UINT(b);
+
+ return neg->low_power_node == lpn;
+}
+
+static bool match_by_dst(const void *a, const void *b)
+{
+ const struct mesh_friend *frnd = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+
+ return frnd->dst == dst;
+}
+
+/* Scaling factors in 1/10 ms */
+static const int32_t scaling[] = {
+ 10,
+ 15,
+ 20,
+ 15,
+};
+
+void friend_request(struct mesh_net *net, uint16_t src,
+ uint8_t minReq, uint8_t delay, uint32_t timeout,
+ uint16_t prev, uint8_t num_ele, uint16_t cntr,
+ int8_t rssi)
+{
+ struct frnd_negotiation *neg;
+ uint8_t rssiScale = (minReq >> 5) & 3;
+ uint8_t winScale = (minReq >> 3) & 3;
+ uint8_t minCache = (minReq >> 0) & 7;
+ int32_t rsp_delay;
+
+ l_info("RSSI of Request: %d dbm", rssi);
+ l_info("Delay: %d ms", delay);
+ l_info("Poll Timeout of Request: %d ms", timeout * 100);
+ l_info("Previous Friend: %4.4x", prev);
+ l_info("Num Elem: %2.2x", num_ele);
+ l_info("Cache Requested: %d", cache_size(minCache));
+ l_info("Cache to offer: %d", frnd_cache_size);
+
+ /* Determine our own suitability before
+ * deciding to participate in negotiation
+ */
+ if (minCache == 0 || num_ele == 0)
+ return;
+
+ if (delay < 0x0A)
+ return;
+
+ if (timeout < 0x00000A || timeout > 0x34BBFF)
+ return;
+
+ if (cache_size(minCache) > frnd_cache_size)
+ return;
+
+ if (frnd_negotiations == NULL)
+ frnd_negotiations = l_queue_new();
+
+ /* TODO: Check RSSI, and then start Negotiation if appropriate */
+
+ /* We are participating in this Negotiation */
+ neg = l_new(struct frnd_negotiation, 1);
+ l_queue_push_head(frnd_negotiations, neg);
+
+ neg->net = net;
+ neg->low_power_node = src;
+ neg->lp_cnt = cntr;
+ neg->rssi = rssi;
+ neg->receive_delay = delay;
+ neg->poll_timeout = timeout;
+ neg->old_relay = prev;
+ neg->num_ele = num_ele;
+
+ /* RSSI (Negative Factor, larger values == less time)
+ * Scaling factor 0-3 == multiplier of 1.0 - 2.5
+ * Minimum factor of 1. Bit 1 adds additional factor
+ * of 1, bit zero and additional 0.5
+ */
+ rsp_delay = -(rssi * scaling[rssiScale]);
+ l_info("RSSI Factor: %d ms", rsp_delay / 10);
+
+ /* Relay Window (Positive Factor, larger values == more time)
+ * Scaling factor 0-3 == multiplier of 1.0 - 2.5
+ * Minimum factor of 1. Bit 1 adds additional factor
+ * of 1, bit zero and additional 0.5
+ */
+ rsp_delay += frnd_relay_window * scaling[winScale];
+ l_info("Win Size Factor: %d ms",
+ (frnd_relay_window * scaling[winScale]) / 10);
+
+ /* Normalize to ms */
+ rsp_delay /= 10;
+
+ /* Range limits are 10-255 ms */
+ if (rsp_delay < MIN_RESP_DELAY)
+ rsp_delay = MIN_RESP_DELAY;
+ else if (rsp_delay > MAX_RESP_DELAY)
+ rsp_delay = MAX_RESP_DELAY;
+
+ l_info("Total Response Delay: %d ms", rsp_delay);
+
+ /* Add in 100ms delay before start of "Offer Period" */
+ rsp_delay += RESPONSE_DELAY;
+
+ neg->timeout = l_timeout_create_ms(rsp_delay,
+ response_delay, neg, NULL);
+}
+
+static struct l_queue *retired_lpns;
+
+void friend_clear_confirm(struct mesh_net *net, uint16_t src,
+ uint16_t lpn, uint16_t lpnCounter)
+{
+ struct frnd_negotiation *neg = l_queue_remove_if(frnd_negotiations,
+ match_by_lpn, L_UINT_TO_PTR(lpn));
+
+ l_info("Friend Clear confirmed %4.4x (cnt %4.4x)", lpn, lpnCounter);
+
+ if (!neg)
+ return;
+
+ l_timeout_remove(neg->timeout);
+ l_queue_remove(frnd_negotiations, neg);
+ l_free(neg);
+}
+
+static void friend_poll_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_friend *frnd = user_data;
+
+ if (mesh_friend_clear(frnd->net, frnd))
+ l_info("Friend Poll Timeout %4.4x", frnd->dst);
+
+ l_timeout_remove(frnd->timeout);
+ frnd->timeout = NULL;
+
+ /* Friend may be in either Network or Retired list, so try both */
+ l_queue_remove(retired_lpns, frnd);
+ mesh_friend_free(frnd);
+}
+
+void friend_clear(struct mesh_net *net, uint16_t src, uint16_t lpn,
+ uint16_t lpnCounter, struct mesh_friend *frnd)
+{
+ uint8_t msg[5] = { NET_OP_FRND_CLEAR_CONFIRM };
+ bool removed = false;
+ uint16_t lpnDelta;
+
+ if (frnd) {
+ lpnDelta = lpnCounter - frnd->lp_cnt;
+
+ /* Ignore old Friend Clear commands */
+ if (lpnDelta > 0x100)
+ return;
+
+ /* Move friend from Network list to Retired list */
+ removed = mesh_friend_clear(net, frnd);
+ if (removed) {
+ struct mesh_friend *old;
+ struct frnd_negotiation *neg = l_queue_remove_if(
+ frnd_negotiations,
+ match_by_lpn,
+ L_UINT_TO_PTR(frnd->dst));
+
+ /* Cancel any negotiations or clears */
+ if (neg) {
+ l_timeout_remove(neg->timeout);
+ l_free(neg);
+ }
+
+ /* Create Retired LPN list if needed */
+ if (retired_lpns == NULL)
+ retired_lpns = l_queue_new();
+
+ /* Find any duplicates */
+ old = l_queue_find(retired_lpns, match_by_dst,
+ L_UINT_TO_PTR(lpn));
+
+ /* Force time-out of old friendship */
+ if (old)
+ friend_poll_timeout(old->timeout, old);
+
+ /* Retire this LPN (keeps timeout running) */
+ l_queue_push_tail(retired_lpns, frnd);
+ }
+ } else {
+ frnd = l_queue_find(retired_lpns, match_by_dst,
+ L_UINT_TO_PTR(lpn));
+ if (!frnd)
+ return;
+
+ lpnDelta = lpnCounter - frnd->lp_cnt;
+
+ /* Ignore old Friend Clear commands */
+ if (!lpnDelta || (lpnDelta > 0x100))
+ return;
+ }
+
+ l_info("Friend Cleared %4.4x (%4.4x)", lpn, lpnCounter);
+
+ l_put_be16(lpn, msg + 1);
+ l_put_be16(lpnCounter, msg + 3);
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), DEFAULT_TTL,
+ 0, 0, src,
+ msg, sizeof(msg));
+}
+
+static void clear_retry(struct l_timeout *timeout, void *user_data)
+{
+ struct frnd_negotiation *neg = user_data;
+ uint8_t msg[5] = { NET_OP_FRND_CLEAR };
+ uint32_t secs = 1 << neg->receive_delay;
+
+
+ l_put_be16(neg->low_power_node, msg + 1);
+ l_put_be16(neg->lp_cnt, msg + 3);
+ mesh_net_transport_send(neg->net, NULL, false,
+ mesh_net_get_iv_index(neg->net), DEFAULT_TTL,
+ 0, 0, neg->old_relay,
+ msg, sizeof(msg));
+
+ if (secs && ((secs << 1) < neg->poll_timeout/10)) {
+ neg->receive_delay++;
+ l_info("Try FRND_CLR again in %d seconds (total timeout %d)",
+ secs, neg->poll_timeout/10);
+ l_timeout_modify(neg->timeout, secs);
+ } else {
+ l_info("FRND_CLR timed out %d", secs);
+ l_timeout_remove(timeout);
+ l_queue_remove(frnd_negotiations, neg);
+ l_free(neg);
+ }
+}
+
+static void friend_delay_rsp(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_friend *frnd = user_data;
+ struct mesh_friend_msg *pkt = frnd->pkt;
+ struct mesh_net *net = frnd->net;
+ uint32_t net_seq, iv_index;
+ uint8_t upd[7] = { NET_OP_FRND_UPDATE };
+
+ l_timeout_remove(timeout);
+
+ if (pkt == NULL)
+ goto update;
+
+ if (pkt->ctl) {
+ /* Make sure we don't change the bit-sense of MD,
+ * once it has been set because that would cause
+ * a "Dirty Nonce" security violation
+ */
+ if (((pkt->u.one[0].hdr >> OPCODE_HDR_SHIFT) & OPCODE_MASK) ==
+ NET_OP_SEG_ACKNOWLEDGE) {
+ bool rly = !!((pkt->u.one[0].hdr >> RELAY_HDR_SHIFT) &
+ true);
+ uint16_t seqZero = pkt->u.one[0].hdr >>
+ SEQ_ZERO_HDR_SHIFT;
+
+ seqZero &= SEQ_ZERO_MASK;
+
+ l_info("Fwd ACK pkt %6.6x-%8.8x",
+ pkt->u.one[0].seq,
+ pkt->iv_index);
+
+ pkt->u.one[0].sent = true;
+ mesh_net_ack_send(net, &frnd->key_set,
+ pkt->iv_index, pkt->ttl,
+ pkt->u.one[0].seq, pkt->src, pkt->dst,
+ rly, seqZero,
+ l_get_be32(pkt->u.one[0].data));
+
+
+ } else {
+ l_info("Fwd CTL pkt %6.6x-%8.8x",
+ pkt->u.one[0].seq,
+ pkt->iv_index);
+
+ print_packet("Frnd-CTL",
+ pkt->u.one[0].data, pkt->last_len);
+
+ pkt->u.one[0].sent = true;
+ mesh_net_transport_send(net, &frnd->key_set, false,
+ pkt->iv_index, pkt->ttl,
+ pkt->u.one[0].seq, pkt->src, pkt->dst,
+ pkt->u.one[0].data, pkt->last_len);
+ }
+ } else {
+ /* If segments after this one, then More Data must be TRUE */
+ uint8_t len;
+
+ if (pkt->cnt_out < pkt->cnt_in)
+ len = sizeof(pkt->u.s12[0].data);
+ else
+ len = pkt->last_len;
+
+ l_info("Fwd FRND pkt %6.6x",
+ pkt->u.s12[pkt->cnt_out].seq);
+
+ print_packet("Frnd-Msg", pkt->u.s12[pkt->cnt_out].data, len);
+
+ pkt->u.s12[pkt->cnt_out].sent = true;
+ mesh_net_send_seg(net, &frnd->key_set,
+ pkt->iv_index,
+ pkt->ttl,
+ pkt->u.s12[pkt->cnt_out].seq,
+ pkt->src, pkt->dst,
+ pkt->u.s12[pkt->cnt_out].hdr,
+ pkt->u.s12[pkt->cnt_out].data, len);
+ }
+
+ return;
+
+update:
+ // No More Data -- send Update message with md = false
+ net_seq = mesh_net_get_seq_num(net);
+ l_info("Fwd FRND UPDATE %6.6x with MD == 0", net_seq);
+
+ frnd->last = frnd->seq;
+ mesh_net_get_snb_state(net, upd + 1, &iv_index);
+ l_put_be32(iv_index, upd + 2);
+ upd[6] = false; // Queue is Empty
+ print_packet("Update", upd, sizeof(upd));
+ mesh_net_transport_send(net, &frnd->key_set, false,
+ mesh_net_get_iv_index(net), 0,
+ net_seq, 0, frnd->dst,
+ upd, sizeof(upd));
+ mesh_net_next_seq_num(net);
+}
+
+
+void friend_poll(struct mesh_net *net, uint16_t src, bool seq,
+ struct mesh_friend *frnd)
+{
+ struct frnd_negotiation *neg;
+ struct mesh_friend_msg *pkt;
+ bool md;
+
+ neg = l_queue_find(frnd_negotiations, match_by_lpn, L_UINT_TO_PTR(src));
+ if (neg && !neg->clearing) {
+ uint8_t msg[5] = { NET_OP_FRND_CLEAR };
+
+ l_info("Won negotiation for %4.4x", neg->low_power_node);
+
+ /* This call will clean-up and replace if already friends */
+ frnd = mesh_friend_new(net, src, neg->num_ele,
+ neg->receive_delay,
+ neg->wrfrw,
+ neg->poll_timeout,
+ neg->fn_cnt, neg->lp_cnt);
+
+ frnd->timeout = l_timeout_create_ms(
+ frnd->poll_timeout * 100,
+ friend_poll_timeout, frnd, NULL);
+
+ l_timeout_remove(neg->timeout);
+ mesh_net_remove_keyset(neg->net, &neg->key_set);
+
+ if (neg->old_relay == 0 ||
+ neg->old_relay == mesh_net_get_address(net)) {
+ l_queue_remove(frnd_negotiations, neg);
+ l_free(neg);
+ } else {
+ neg->clearing = true;
+ l_put_be16(neg->low_power_node, msg + 1);
+ l_put_be16(neg->lp_cnt, msg + 3);
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), DEFAULT_TTL,
+ 0, 0, neg->old_relay,
+ msg, sizeof(msg));
+
+ /* Reuse receive_delay as a shift counter to
+ * time-out FRIEND_CLEAR
+ */
+ neg->receive_delay = 1;
+ neg->timeout = l_timeout_create(1, clear_retry,
+ neg, NULL);
+ }
+ }
+
+ if (!frnd)
+ return;
+
+ /* Reset Poll Timeout */
+ l_timeout_modify_ms(frnd->timeout, frnd->poll_timeout * 100);
+
+ if (!l_queue_length(frnd->pkt_cache))
+ goto update;
+
+ if (frnd->seq != frnd->last && frnd->seq != seq) {
+ pkt = l_queue_peek_head(frnd->pkt_cache);
+ if (pkt->cnt_out < pkt->cnt_in) {
+ pkt->cnt_out++;
+ } else {
+ pkt = l_queue_pop_head(frnd->pkt_cache);
+ l_free(pkt);
+ }
+ }
+
+ pkt = l_queue_peek_head(frnd->pkt_cache);
+
+ if (!pkt)
+ goto update;
+
+ frnd->seq = seq;
+ frnd->last = !seq;
+ md = !!(l_queue_length(frnd->pkt_cache) > 1);
+
+ if (pkt->ctl) {
+ /* Make sure we don't change the bit-sense of MD,
+ * once it has been set because that would cause
+ * a "Dirty Nonce" security violation
+ */
+ if (!(pkt->u.one[0].sent))
+ pkt->u.one[0].md = md;
+ } else {
+ /* If segments after this one, then More Data must be TRUE */
+ if (pkt->cnt_out < pkt->cnt_in)
+ md = true;
+
+ /* Make sure we don't change the bit-sense of MD, once
+ * it has been set because that would cause a
+ * "Dirty Nonce" security violation
+ */
+ if (!(pkt->u.s12[pkt->cnt_out].sent))
+ pkt->u.s12[pkt->cnt_out].md = md;
+ }
+ frnd->pkt = pkt;
+ l_timeout_create_ms(frnd->frd, friend_delay_rsp, frnd, NULL);
+
+ return;
+
+update:
+ frnd->pkt = NULL;
+ l_timeout_create_ms(frnd->frd, friend_delay_rsp, frnd, NULL);
+}
+
+void friend_sub_add(struct mesh_net *net, struct mesh_friend *frnd,
+ const uint8_t *pkt, uint8_t len)
+{
+ uint16_t *new_list;
+ uint32_t net_seq;
+ uint8_t plen = len;
+ uint8_t msg[] = { NET_OP_PROXY_SUB_CONFIRM, 0 };
+
+ if (!frnd || MAX_FRND_GROUPS < frnd->grp_cnt + (len/2))
+ return;
+
+ msg[1] = *pkt++;
+ plen--;
+
+ /* Sanity Check Values, abort if any illegal */
+ while (plen >= 2) {
+ plen -= 2;
+ if (l_get_be16(pkt + plen) < 0x8000)
+ return;
+ }
+
+ new_list = l_malloc(frnd->grp_cnt * sizeof(uint16_t) + len);
+ if (frnd->grp_list)
+ memcpy(new_list, frnd->grp_list,
+ frnd->grp_cnt * sizeof(uint16_t));
+
+ while (len >= 2) {
+ new_list[frnd->grp_cnt++] = l_get_be16(pkt);
+ pkt += 2;
+ len -= 2;
+ }
+
+ l_free(frnd->grp_list);
+ frnd->grp_list = new_list;
+
+ print_packet("Tx-NET_OP_PROXY_SUB_CONFIRM", msg, sizeof(msg));
+ net_seq = mesh_net_get_seq_num(net);
+ mesh_net_transport_send(net, &frnd->key_set, false,
+ mesh_net_get_iv_index(net), 0,
+ net_seq, 0, frnd->dst,
+ msg, sizeof(msg));
+ mesh_net_next_seq_num(net);
+}
+
+void friend_sub_del(struct mesh_net *net, struct mesh_friend *frnd,
+ const uint8_t *pkt, uint8_t len)
+{
+ uint32_t net_seq;
+ uint8_t msg[] = { NET_OP_PROXY_SUB_CONFIRM, 0 };
+ int i;
+
+ if (!frnd)
+ return;
+
+ msg[1] = *pkt++;
+ len--;
+
+ while (len >= 2) {
+ uint16_t grp = l_get_be16(pkt);
+
+ for (i = frnd->grp_cnt - 1; i >= 0; i--) {
+ if (frnd->grp_list[i] == grp) {
+ frnd->grp_cnt--;
+ memcpy(&frnd->grp_list[i],
+ &frnd->grp_list[i + 1],
+ (frnd->grp_cnt - i) * 2);
+ break;
+ }
+ }
+ len -= 2;
+ pkt += 2;
+ }
+
+ print_packet("Tx-NET_OP_PROXY_SUB_CONFIRM", msg, sizeof(msg));
+ net_seq = mesh_net_get_seq_num(net);
+ mesh_net_transport_send(net, &frnd->key_set, false,
+ mesh_net_get_iv_index(net), 0,
+ net_seq, 0, frnd->dst,
+ msg, sizeof(msg));
+ mesh_net_next_seq_num(net);
+}
+
+/* Low-Power-Node role */
+struct frnd_offers {
+ uint16_t fn_cnt;
+ uint16_t src;
+ uint8_t window;
+ uint8_t cache;
+ uint8_t sub_list_size;
+ int8_t local_rssi;
+ int8_t remote_rssi;
+};
+
+#define MAX_POLL_RETRIES 5
+static bool quick_pick;
+static uint8_t poll_cnt;
+static struct l_queue *offers;
+static uint16_t old_friend;
+static uint16_t fn_cnt, cnt = 0xffff;
+static uint32_t poll_period_ms;
+static struct l_timeout *poll_retry_to;
+static struct l_timeout *poll_period_to;
+static struct mesh_key_set lpn_set;
+static struct mesh_key_set new_lpn_set;
+
+void frnd_offer(struct mesh_net *net, uint16_t src, uint8_t window,
+ uint8_t cache, uint8_t sub_list_size,
+ int8_t r_rssi, int8_t l_rssi, uint16_t fn_cnt)
+{
+ struct frnd_offers *offer;
+
+ l_info("RSSI of Offer: %d dbm", l_rssi);
+
+ /* Ignore RFU window value 0 */
+ if (window == 0)
+ return;
+
+ if (mesh_net_get_friend(net))
+ return;
+
+ if (quick_pick) {
+ if (mesh_net_set_friend(net, src)) {
+ old_friend = src;
+ frnd_poll(net, false);
+ }
+ return;
+ }
+
+ offer = l_new(struct frnd_offers, 1);
+ offer->src = src;
+ offer->window = window;
+ offer->cache = cache;
+ offer->sub_list_size = sub_list_size;
+ offer->local_rssi = l_rssi;
+ offer->remote_rssi = r_rssi;
+ offer->fn_cnt = fn_cnt;
+
+ l_queue_push_tail(offers, offer);
+}
+
+static void frnd_poll_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+
+ frnd_poll(net, true);
+}
+
+static void frnd_negotiated_to(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+
+ l_info("frnd_negotiated_to");
+ if (!mesh_net_get_friend(net)) {
+ l_timeout_remove(poll_period_to);
+ poll_period_to = NULL;
+ return;
+ }
+
+ if (!poll_retry_to)
+ frnd_poll(net, false);
+}
+
+void frnd_poll_cancel(struct mesh_net *net)
+{
+ l_timeout_remove(poll_retry_to);
+ poll_retry_to = NULL;
+}
+
+void frnd_poll(struct mesh_net *net, bool retry)
+{
+ struct mesh_key_set *key_set = &lpn_set;
+ uint32_t net_seq;
+ uint8_t msg[2] = { NET_OP_FRND_POLL };
+ bool seq = mesh_net_get_frnd_seq(net);
+
+ /* Check if we are in Phase 2 of Key Refresh */
+ if (new_lpn_set.nid != 0xff) {
+ uint8_t phase;
+ uint16_t net_idx = mesh_net_get_primary_idx(net);
+ uint8_t status =
+ mesh_net_key_refresh_phase_get(net, net_idx, &phase);
+
+ if (status == MESH_STATUS_SUCCESS &&
+ phase == KEY_REFRESH_PHASE_TWO)
+ key_set = &new_lpn_set;
+ }
+
+ if (!retry) {
+ poll_cnt = MAX_POLL_RETRIES;
+ seq = !seq;
+ mesh_net_set_frnd_seq(net, seq);
+ } else if (!(poll_cnt--)) {
+ l_info("Lost Friendship with %4.4x", old_friend);
+ l_timeout_remove(poll_period_to);
+ poll_period_to = NULL;
+ frnd_poll_cancel(net);
+ mesh_net_remove_keyset(net, &lpn_set);
+ mesh_net_remove_keyset(net, &new_lpn_set);
+ mesh_net_set_friend(net, 0);
+ return;
+ }
+
+ if (poll_retry_to)
+ l_timeout_remove(poll_retry_to);
+
+ l_info("TX-FRIEND POLL %d", seq);
+ msg[1] = seq;
+ net_seq = mesh_net_get_seq_num(net);
+ mesh_net_transport_send(net, key_set, true,
+ mesh_net_get_iv_index(net), 0,
+ net_seq, 0, mesh_net_get_friend(net),
+ msg, sizeof(msg));
+ mesh_net_next_seq_num(net);
+ poll_retry_to = l_timeout_create_ms(1000, frnd_poll_timeout, net, NULL);
+
+ /* Reset Poll Period for next "Wake Up" */
+ if (poll_period_to)
+ l_timeout_modify_ms(poll_period_to, poll_period_ms);
+ else
+ poll_period_to = l_timeout_create_ms(poll_period_ms,
+ frnd_negotiated_to, net, NULL);
+}
+
+void frnd_ack_poll(struct mesh_net *net)
+{
+ /* Start new POLL, but only if not already Polling */
+ if (poll_retry_to == NULL)
+ frnd_poll(net, false);
+}
+
+static void req_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct frnd_offers *best;
+ struct frnd_offers *offer = l_queue_pop_head(offers);
+ uint8_t p[9] = { 1 };
+ uint8_t key[16];
+ bool res;
+
+ l_timeout_remove(timeout);
+
+ best = offer;
+ while (offer) {
+ /* Screen out clearly inferior RSSI friends first */
+ if (offer->local_rssi < -40 && offer->remote_rssi < -40) {
+ if (best->local_rssi + 20 < offer->local_rssi ||
+ best->remote_rssi + 20 < offer->remote_rssi) {
+
+ l_free(best);
+ best = offer;
+ offer = l_queue_pop_head(offers);
+ continue;
+ }
+ }
+
+ /* Otherwise use best Windows, with Cache size as tie breaker */
+ if (best->window > offer->window ||
+ (best->window == offer->window &&
+ best->cache < offer->cache)) {
+ l_free(best);
+ best = offer;
+ } else if (best != offer)
+ l_free(offer);
+
+ offer = l_queue_pop_head(offers);
+ }
+
+ mesh_net_remove_keyset(net, &lpn_set);
+ mesh_net_remove_keyset(net, &new_lpn_set);
+ if (mesh_net_get_friend(net)) {
+ l_free(best);
+ return;
+ } else if (!best) {
+ l_info("No Offers Received");
+ return;
+ }
+
+ fn_cnt = best->fn_cnt;
+ l_put_be16(mesh_net_get_address(net), p + 1);
+ l_put_be16(best->src, p + 3);
+ l_put_be16(cnt, p + 5);
+ l_put_be16(best->fn_cnt, p + 7);
+ print_packet("Friend Key P =", p, 9);
+ res = mesh_net_get_key(net, false, mesh_net_get_primary_idx(net), key);
+ if (!res)
+ return;
+
+ res = mesh_crypto_k2(key, p, sizeof(p), &lpn_set.nid,
+ lpn_set.enc_key, lpn_set.privacy_key);
+ if (!res)
+ return;
+
+ print_packet("Cur-NID", &lpn_set.nid, 1);
+ print_packet("Cur-ENC_KEY", lpn_set.enc_key, 16);
+ print_packet("Cur-PRIV_KEY", lpn_set.privacy_key, 16);
+
+ mesh_net_add_keyset(net, &lpn_set);
+
+ res = mesh_net_get_key(net, true, mesh_net_get_primary_idx(net), key);
+
+ if (res)
+ res = mesh_crypto_k2(key, p, sizeof(p), &new_lpn_set.nid,
+ new_lpn_set.enc_key, new_lpn_set.privacy_key);
+ if (!res) {
+ new_lpn_set.nid = 0xff;
+ goto old_keys_only;
+ }
+
+ print_packet("New-NID", &new_lpn_set.nid, 1);
+ print_packet("New-ENC_KEY", new_lpn_set.enc_key, 16);
+ print_packet("New-PRIV_KEY", new_lpn_set.privacy_key, 16);
+
+ mesh_net_add_keyset(net, &new_lpn_set);
+
+old_keys_only:
+
+ l_info("Winning offer %4.4x RSSI: %ddb Window: %dms Cache sz: %d",
+ best->src, best->local_rssi,
+ best->window, best->cache);
+
+ if (mesh_net_set_friend(net, best->src)) {
+ old_friend = best->src;
+ mesh_net_set_frnd_seq(net, true);
+ frnd_poll(net, false);
+ }
+
+ l_free(best);
+}
+
+void frnd_clear(struct mesh_net *net)
+{
+ uint8_t msg[12];
+ uint8_t n = 0;
+ uint16_t frnd_addr = mesh_net_get_friend(net);
+ uint16_t my_addr = mesh_net_get_address(net);
+
+ msg[n++] = NET_OP_FRND_CLEAR;
+ l_put_be16(my_addr, msg + n);
+ n += 2;
+ l_put_be16(cnt, msg + n);
+ n += 2;
+
+ mesh_net_remove_keyset(net, &lpn_set);
+ mesh_net_remove_keyset(net, &new_lpn_set);
+ mesh_net_set_friend(net, 0);
+
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), 0,
+ 0, 0, frnd_addr,
+ msg, n);
+}
+
+void frnd_request_friend(struct mesh_net *net, uint8_t cache,
+ uint8_t offer_delay, uint8_t delay, uint32_t timeout)
+{
+ uint8_t msg[12];
+ uint8_t n = 0;
+
+ if (offers == NULL)
+ offers = l_queue_new();
+
+ msg[n++] = NET_OP_FRND_REQUEST;
+ msg[n] = cache & 0x07; // MinRequirements - Cache
+ msg[n++] |= (offer_delay & 0x0f) << 3; // Offer Delay
+ poll_period_ms = (timeout * 300) / 4; // 3/4 of the time in ms
+ l_put_be32(timeout, msg + n); // PollTimeout
+ msg[n++] = delay; // ReceiveDelay
+ n += 3;
+ l_put_be16(old_friend, msg + n); // PreviousAddress
+ n += 2;
+ msg[n++] = mesh_net_get_num_ele(net); // NumElements
+ l_put_be16(cnt + 1, msg + n); // Next counter
+ n += 2;
+ print_packet("Tx-NET_OP_FRND_REQUEST", msg, n);
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), 0,
+ 0, 0, FRIENDS_ADDRESS,
+ msg, n);
+ l_timeout_create_ms(1000, req_timeout, net, NULL); // 1000 ms
+ mesh_net_set_friend(net, 0);
+ cnt++;
+}
+
+static uint8_t trans_id;
+void frnd_sub_add(struct mesh_net *net, uint32_t parms[7])
+{
+ struct mesh_key_set *key_set = &lpn_set;
+ uint32_t net_seq;
+ uint8_t msg[15] = { NET_OP_PROXY_SUB_ADD };
+ uint8_t i, n = 1;
+
+ /* Check if we are in Phase 2 of Key Refresh */
+ if (new_lpn_set.nid != 0xff) {
+ uint8_t phase;
+ uint16_t net_idx = mesh_net_get_primary_idx(net);
+ uint8_t status = mesh_net_key_refresh_phase_get(net,
+ net_idx, &phase);
+
+ if (status == MESH_STATUS_SUCCESS &&
+ phase == KEY_REFRESH_PHASE_TWO)
+ key_set = &new_lpn_set;
+ }
+
+ msg[n++] = ++trans_id;
+ for (i = 0; i < 7; i++) {
+ if (parms[i] < 0x8000 || parms[i] > 0xffff)
+ break;
+
+ l_put_be16(parms[i], msg + n);
+ n += 2;
+ }
+
+ net_seq = mesh_net_get_seq_num(net);
+ print_packet("Friend Sub Add", msg, n);
+ mesh_net_transport_send(net, key_set, false,
+ mesh_net_get_iv_index(net), 0,
+ net_seq, 0, mesh_net_get_friend(net),
+ msg, n);
+ mesh_net_next_seq_num(net);
+}
+
+void frnd_sub_del(struct mesh_net *net, uint32_t parms[7])
+{
+ struct mesh_key_set *key_set = &lpn_set;
+ uint32_t net_seq;
+ uint8_t msg[15] = { NET_OP_PROXY_SUB_REMOVE };
+ uint8_t i, n = 1;
+
+ /* Check if we are in Phase 2 of Key Refresh */
+ if (new_lpn_set.nid != 0xff) {
+ uint8_t phase;
+ uint16_t net_idx = mesh_net_get_primary_idx(net);
+ uint8_t status = mesh_net_key_refresh_phase_get(net,
+ net_idx, &phase);
+
+ if (status == MESH_STATUS_SUCCESS &&
+ phase == KEY_REFRESH_PHASE_TWO)
+ key_set = &new_lpn_set;
+ }
+
+ msg[n++] = ++trans_id;
+ for (i = 0; i < 7; i++) {
+ if (parms[i] < 0x8000 || parms[i] > 0xffff)
+ break;
+
+ l_put_be16(parms[i], msg + n);
+ n += 2;
+ }
+
+ net_seq = mesh_net_get_seq_num(net);
+ print_packet("Friend Sub Del", msg, n);
+ mesh_net_transport_send(net, key_set, false,
+ mesh_net_get_iv_index(net), 0,
+ net_seq, 0, mesh_net_get_friend(net),
+ msg, n);
+ mesh_net_next_seq_num(net);
+}
+
+void frnd_key_refresh(struct mesh_net *net, uint8_t phase)
+{
+ uint16_t net_idx = mesh_net_get_primary_idx(net);
+ uint8_t p[9] = { 1 };
+ uint8_t key[16];
+
+ switch (phase) {
+ default:
+ case 0:
+ case 3:
+ if (new_lpn_set.nid != 0xff) {
+ l_info("LPN Retiring KeySet %2.2x", lpn_set.nid);
+ lpn_set = new_lpn_set;
+ new_lpn_set.nid = 0xff;
+ mesh_net_remove_keyset(net, &new_lpn_set);
+ }
+ return;
+
+ case 1:
+ mesh_net_remove_keyset(net, &new_lpn_set);
+ if (!mesh_net_get_key(net, true, net_idx, key)) {
+ new_lpn_set.nid = 0xff;
+ return;
+ }
+
+ l_put_be16(mesh_net_get_address(net), p + 1);
+ l_put_be16(mesh_net_get_friend(net), p + 3);
+ l_put_be16(cnt, p + 5);
+ l_put_be16(fn_cnt, p + 7);
+ print_packet("Friend Key P =", p, 9);
+
+ if (!mesh_crypto_k2(key, p, sizeof(p), &new_lpn_set.nid,
+ new_lpn_set.enc_key,
+ new_lpn_set.privacy_key)) {
+ new_lpn_set.nid = 0xff;
+ return;
+ }
+
+ print_packet("New-NID", &new_lpn_set.nid, 1);
+ print_packet("New-ENC_KEY", new_lpn_set.enc_key, 16);
+ print_packet("New-PRIV_KEY", new_lpn_set.privacy_key, 16);
+
+ mesh_net_add_keyset(net, &new_lpn_set);
+ return;
+
+ case 2:
+ /* Should we do anything here? Maybe not */
+ return;
+ }
+}
+
+struct mesh_key_set *frnd_get_key(struct mesh_net *net)
+{
+ uint8_t idx = mesh_net_get_primary_idx(net);
+ uint8_t phase = 0;
+
+ mesh_net_key_refresh_phase_get(net, idx, &phase);
+
+ if (phase == 2)
+ return &new_lpn_set;
+ else
+ return &lpn_set;
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 05/14] mesh: Provisioning logic for mesh
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (3 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 04/14] mesh: Initial Mesh Friendship support Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 06/14] mesh: Upper and Lower mesh transport Brian Gix
` (9 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/prov.c | 722 ++++++++++++++++++++++++++++++++++
mesh/provision.c | 1159 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 files changed, 1881 insertions(+)
create mode 100644 mesh/prov.c
create mode 100644 mesh/provision.c
diff --git a/mesh/prov.c b/mesh/prov.c
new file mode 100644
index 000000000..45ced404c
--- /dev/null
+++ b/mesh/prov.c
@@ -0,0 +1,722 @@
+/*
+ *
+ * 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 "mesh/mesh-defs.h"
+
+#include "mesh/mesh-io.h"
+#include "mesh/display.h"
+#include "mesh/crypto.h"
+#include "mesh/net.h"
+#include "mesh/prov.h"
+
+#define PB_ADV_MTU 24
+
+#define DEFAULT_CONN_ID 0x00000000
+#define DEFAULT_PROV_MSG_NUM 0x00
+#define DEFAULT_DEV_MSG_NUM 0x80
+
+#define TX_TIMEOUT 30
+
+struct mesh_prov *mesh_prov_new(struct mesh_net *net, uint16_t remote)
+{
+ struct mesh_prov *prov;
+
+ prov = l_new(struct mesh_prov, 1);
+
+ prov->remote = remote;
+ prov->net = net;
+
+ return mesh_prov_ref(prov);
+}
+
+struct mesh_prov *mesh_prov_ref(struct mesh_prov *prov)
+{
+ if (!prov)
+ return NULL;
+
+ __sync_fetch_and_add(&prov->ref_count, 1);
+
+ return prov;
+}
+
+void mesh_prov_unref(struct mesh_prov *prov)
+{
+ struct mesh_io *io;
+ uint8_t type;
+
+ if (!prov)
+ return;
+
+ if (__sync_sub_and_fetch(&prov->ref_count, 1))
+ return;
+
+ io = mesh_net_get_io(prov->net);
+ type = MESH_AD_TYPE_BEACON;
+ mesh_io_send_cancel(io, &type, 1);
+ type = MESH_AD_TYPE_PROVISION;
+ mesh_io_send_cancel(io, &type, 1);
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_PROV);
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON);
+ l_timeout_remove(prov->tx_timeout);
+
+
+ l_info("Freed Prov Data");
+ l_free(prov);
+}
+
+static void packet_received(struct mesh_prov *prov, const void *data,
+ uint16_t size, uint8_t fcs)
+{
+ if (prov->receive_callback)
+ prov->receive_callback(data, size, prov);
+}
+
+static void send_open_req(struct mesh_prov *prov)
+{
+ struct mesh_io *io;
+ uint8_t open_req[23] = { MESH_AD_TYPE_PROVISION };
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = 50,
+ .u.gen.cnt = 1,
+ .u.gen.min_delay = 0,
+ .u.gen.max_delay = 0,
+ };
+
+ if (!prov)
+ return;
+
+ io = mesh_net_get_io(prov->net);
+ if (!io)
+ return;
+
+ l_put_be32(prov->conn_id, open_req + 1);
+ open_req[1 + 4] = prov->local_msg_num = 0;
+ open_req[1 + 4 + 1] = 0x03; /* OPEN_REQ */
+ memcpy(open_req + 1 + 4 + 1 + 1, prov->uuid, 16);
+
+ /* print_packet("PB-TX", open_req + 1, sizeof(open_req) - 1); */
+ mesh_io_send_cancel(io, open_req, 1);
+ mesh_io_send(io, &info, open_req, sizeof(open_req));
+}
+
+static void send_open_cfm(struct mesh_prov *prov)
+{
+ struct mesh_io *io;
+ uint8_t open_cfm[7] = { MESH_AD_TYPE_PROVISION };
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = 50,
+ .u.gen.cnt = 1,
+ .u.gen.min_delay = 0,
+ .u.gen.max_delay = 0,
+ };
+
+ if (!prov)
+ return;
+
+ io = mesh_net_get_io(prov->net);
+ if (!io)
+ return;
+
+ l_put_be32(prov->conn_id, open_cfm + 1);
+ open_cfm[1 + 4] = 0;
+ open_cfm[1 + 4 + 1] = 0x07; /* OPEN_CFM */
+
+ /* print_packet("PB-TX", open_cfm + 1, sizeof(open_cfm) - 1); */
+
+ mesh_io_send_cancel(io, open_cfm, 1);
+ mesh_io_send(io, &info, open_cfm, sizeof(open_cfm));
+}
+
+static void send_close_ind(struct mesh_prov *prov, uint8_t reason)
+{
+ uint8_t buf[8] = { MESH_AD_TYPE_PROVISION };
+ struct mesh_io *io;
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = 50,
+ .u.gen.cnt = 3,
+ .u.gen.min_delay = 0,
+ .u.gen.max_delay = 0,
+ };
+
+ if (!prov)
+ return;
+
+ if (prov->bearer == MESH_BEARER_ADV) {
+ io = mesh_net_get_io(prov->net);
+ if (!io)
+ return;
+
+ l_put_be32(prov->conn_id, buf + 1);
+ buf[5] = 0;
+ buf[6] = 0x0B; /* CLOSE_IND */
+ buf[7] = reason;
+
+ /* print_packet("PB-TX", buf + 1, sizeof(buf) - 1); */
+
+ mesh_io_send_cancel(io, buf, 1);
+ mesh_io_send(io, &info, buf, sizeof(buf));
+ }
+
+ prov->bearer = MESH_BEARER_IDLE;
+}
+
+static void tx_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_prov *prov = user_data;
+ uint8_t cancel[] = { MESH_AD_TYPE_PROVISION };
+ struct mesh_io *io;
+
+ if (!prov)
+ return;
+
+ l_timeout_remove(prov->tx_timeout);
+ prov->tx_timeout = NULL;
+
+ io = mesh_net_get_io(prov->net);
+ if (!io)
+ return;
+
+ mesh_io_send_cancel(io, cancel, sizeof(cancel));
+
+ l_info("TX timeout");
+ mesh_prov_close(prov, 1);
+}
+
+static void send_adv_segs(struct mesh_prov *prov)
+{
+ struct mesh_io *io = mesh_net_get_io(prov->net);
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = 50,
+ .u.gen.cnt = MESH_IO_TX_COUNT_UNLIMITED,
+ .u.gen.min_delay = 0,
+ .u.gen.max_delay = 0,
+ };
+ const void *data = prov->packet_buf;
+ uint16_t size = prov->packet_len;
+ uint16_t init_size;
+ uint8_t buf[1 + PB_ADV_MTU + 5] = { MESH_AD_TYPE_PROVISION };
+ uint8_t max_seg;
+ uint8_t consumed;
+ int i;
+
+ if (!size)
+ return;
+
+ mesh_io_send_cancel(io, buf, 1);
+
+ l_put_be32(prov->conn_id, buf + 1);
+ buf[1 + 4] = prov->local_msg_num;
+
+ if (size > PB_ADV_MTU - 4) {
+ max_seg = 1 +
+ (((size - (PB_ADV_MTU - 4)) - 1) / (PB_ADV_MTU - 1));
+ init_size = PB_ADV_MTU - 4;
+ } else {
+ max_seg = 0;
+ init_size = size;
+ }
+
+ /* print_packet("FULL-TX", data, size); */
+
+ l_debug("Sending %u fragments for %u octets", max_seg + 1, size);
+
+ buf[1 + 4 + 1] = max_seg << 2;
+ l_put_be16(size, buf + 1 + 4 + 1 + 1);
+ buf[9] = mesh_crypto_compute_fcs(data, size);
+ memcpy(buf + 1 + 4 + 1 + 1 + 2 + 1, data, init_size);
+
+ l_debug("max_seg: %2.2x", max_seg);
+ l_debug("size: %2.2x, CRC: %2.2x", size, buf[9]);
+
+ /* print_packet("PB-TX", buf + 1, init_size + 9); */
+ mesh_io_send(io, &info, buf, init_size + 10);
+
+ consumed = init_size;
+
+ for (i = 1; i <= max_seg; i++) {
+ uint8_t seg_size; /* Amount of payload data being sent */
+
+ if (size - consumed > PB_ADV_MTU - 1)
+ seg_size = PB_ADV_MTU - 1;
+ else
+ seg_size = size - consumed;
+
+ buf[6] = (i << 2) | 0x02;
+ memcpy(buf + 7, data + consumed, seg_size);
+
+ /* print_packet("PB-TX", buf + 1, seg_size + 6); */
+
+ mesh_io_send(io, &info, buf, seg_size + 7);
+
+ consumed += seg_size;
+ }
+}
+
+static void send_adv_msg(struct mesh_prov *prov, const void *data,
+ uint16_t size)
+{
+ l_timeout_remove(prov->tx_timeout);
+ prov->tx_timeout = l_timeout_create(TX_TIMEOUT, tx_timeout, prov, NULL);
+
+ memcpy(prov->packet_buf, data, size);
+ prov->packet_len = size;
+
+ send_adv_segs(prov);
+}
+
+static void send_ack(struct mesh_prov *prov)
+{
+ struct mesh_io *io = mesh_net_get_io(prov->net);
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = 50,
+ .u.gen.cnt = 1,
+ .u.gen.min_delay = 0,
+ .u.gen.max_delay = 0,
+ };
+ uint8_t ack[7] = { MESH_AD_TYPE_PROVISION };
+
+ l_put_be32(prov->conn_id, ack + 1);
+ ack[1 + 4] = prov->last_peer_msg_num;
+ ack[1 + 4 + 1] = 0x01; /* ACK */
+
+ /* print_packet("ADV-ACK", ack + 1, sizeof(ack) - 1); */
+ mesh_io_send(io, &info, ack, sizeof(ack));
+}
+
+static void adv_data_pkt(uint8_t type, const void *pkt, uint8_t size,
+ void *user_data)
+{
+ const uint8_t *data = pkt;
+ struct mesh_prov *prov = user_data;
+ uint16_t offset = 0;
+
+ if ((type & 0x03) == 0x00) {
+ uint8_t last_seg = type >> 2;
+
+ prov->expected_len = l_get_be16(data);
+ prov->expected_fcs = l_get_u8(data + 2);
+
+ /* print_packet("Pkt", pkt, size); */
+ data += 3;
+ size -= 3;
+
+ prov->trans = MESH_TRANS_RX;
+
+ if (prov->expected_len > sizeof(prov->peer_buf)) {
+ l_info("Incoming pkt exceeds storage %d > %ld",
+ prov->expected_len, sizeof(prov->peer_buf));
+ return;
+ } else if (last_seg == 0)
+ prov->trans = MESH_TRANS_IDLE;
+
+ prov->expected_segs = 0xff >> (7 - last_seg);
+ prov->got_segs |= 1;
+ memcpy(prov->peer_buf, data, size);
+
+ } else if ((type & 0x03) == 0x02) {
+ offset = (PB_ADV_MTU - 4) + ((type >> 2) - 1) *
+ (PB_ADV_MTU - 1);
+
+ if (offset + size > prov->expected_len) {
+ l_info("Incoming pkt exceeds agreed len %d + %d > %d",
+ offset, size, prov->expected_len);
+ return;
+ }
+
+ prov->trans = MESH_TRANS_RX;
+
+ l_debug("Processing fragment %u", type & 0x3f);
+
+ prov->got_segs |= 1 << (type >> 2);
+ memcpy(prov->peer_buf + offset, data, size);
+
+ } else if (type == 0x01) {
+ if (prov->send_callback) {
+ void *data = prov->send_data;
+ mesh_prov_send_func_t cb = prov->send_callback;
+
+ prov->trans = MESH_TRANS_IDLE;
+ prov->send_callback = NULL;
+ prov->send_data = NULL;
+
+ cb(true, data);
+ }
+ return;
+ } else
+ return;
+
+ if (prov->got_segs != prov->expected_segs)
+ return;
+
+ /* Validate RXed packet and pass up to Provisioning */
+ if (!mesh_crypto_check_fcs(prov->peer_buf,
+ prov->expected_len,
+ prov->expected_fcs)) {
+ l_debug("Invalid FCS");
+ return;
+ }
+
+ prov->last_peer_msg_num = prov->peer_msg_num;
+ send_ack(prov);
+
+ prov->trans = MESH_TRANS_IDLE;
+
+ packet_received(prov, prov->peer_buf,
+ prov->expected_len, prov->expected_fcs);
+
+ /* Reset Re-Assembly for next packet */
+ prov->expected_len = sizeof(prov->peer_buf);
+ prov->expected_fcs = 0;
+ prov->expected_segs = 0;
+ prov->got_segs = 0;
+
+}
+
+static void adv_bearer_packet(void *user_data, struct mesh_io_recv_info *info,
+ const uint8_t *pkt, uint16_t len)
+{
+ struct mesh_prov *prov = user_data;
+ uint32_t conn_id;
+ uint8_t msg_num;
+ uint8_t type;
+
+ if (len < 6) {
+ l_info(" Too short packet");
+ return;
+ }
+
+ conn_id = l_get_be32(pkt + 1);
+ msg_num = l_get_u8(pkt + 1 + 4);
+ type = l_get_u8(pkt + 1 + 4 + 1);
+
+ /*if (prov->conn_id == conn_id) print_packet("ADV-RX", pkt, len); */
+
+ if (prov->conn_id != DEFAULT_CONN_ID) {
+ if (prov->conn_id != conn_id) {
+ l_debug("rxed unknown conn_id: %8.8x != %8.8x",
+ conn_id, prov->conn_id);
+ return;
+ }
+ } else if (type != 0x03)
+ return;
+
+ /* print_packet("PB-ADV-RX", pkt, len); */
+
+ /* Normalize pkt to start of PROV pkt payload */
+ pkt += 7;
+ len -= 7;
+
+ if (type == 0x07) { /* OPEN_CFM */
+ if (conn_id != prov->conn_id)
+ return;
+
+ if (msg_num != prov->local_msg_num)
+ return;
+
+ l_info("Link open confirmed");
+
+ prov->bearer = MESH_BEARER_ADV;
+ if (prov->open_callback)
+ prov->open_callback(prov->receive_data);
+ } else if (type == 0x01) {
+ if (conn_id != prov->conn_id)
+ return;
+
+ if (msg_num != prov->local_msg_num)
+ return;
+
+ l_debug("Got ACK %d", msg_num);
+ adv_data_pkt(type, pkt, len, user_data);
+ } else if (type == 0x03) {
+ /*
+ * Ignore if:
+ * 1. We are already provisioning
+ * 2. We are not advertising that we are unprovisioned
+ * 3. Open request not addressed to us
+ */
+ if (prov->conn_id != DEFAULT_CONN_ID &&
+ prov->conn_id != conn_id)
+ return;
+
+ if (prov->local_msg_num != (DEFAULT_DEV_MSG_NUM - 1))
+ return;
+
+ if (memcmp(pkt, prov->uuid, 16))
+ return;
+
+ l_info("Link open request");
+
+ prov->last_peer_msg_num = 0xFF;
+ prov->bearer = MESH_BEARER_ADV;
+ if (prov->open_callback && prov->conn_id == DEFAULT_CONN_ID)
+ prov->open_callback(prov->receive_data);
+
+ prov->conn_id = conn_id;
+ prov->peer_msg_num = msg_num;
+ send_open_cfm(prov);
+ } else if (type == 0x0B) {
+ if (prov->conn_id != conn_id)
+ return;
+
+ prov->conn_id = DEFAULT_CONN_ID;
+ prov->local_msg_num = 0xFF;
+ prov->peer_msg_num = 0xFF;
+ prov->last_peer_msg_num = 0xFF;
+
+ l_timeout_remove(prov->tx_timeout);
+ prov->tx_timeout = NULL;
+
+ l_info("Link closed notification: %2.2x", pkt[0]);
+
+ if (prov->close_callback)
+ prov->close_callback(prov->receive_data, pkt[0]);
+ } else if ((type & 0x03) == 0x00) {
+ if (prov->conn_id != conn_id)
+ return;
+
+ if (msg_num == prov->last_peer_msg_num) {
+ send_ack(prov);
+ return;
+ }
+
+ prov->peer_msg_num = msg_num;
+
+ l_debug("Processing Data with %u fragments,%d octets",
+ type >> 2, l_get_be16(pkt));
+ adv_data_pkt(type, pkt, len, user_data);
+
+ } else if ((type & 0x03) == 0x02) {
+ if (prov->conn_id != conn_id)
+ return;
+
+ if (msg_num == prov->last_peer_msg_num) {
+ send_ack(prov);
+ return;
+ }
+
+ prov->peer_msg_num = msg_num;
+
+ l_debug("Processing fragment %u", type >> 2);
+ adv_data_pkt(type, pkt, len, user_data);
+ }
+}
+
+static void beacon_packet(void *user_data, struct mesh_io_recv_info *info,
+ const uint8_t *pkt, uint16_t len)
+{
+ struct mesh_prov *prov = user_data;
+ struct mesh_io *io;
+
+ pkt++;
+ len--;
+
+ if (len < 19)
+ return;
+
+ if (!pkt[0])
+ print_packet("UnProv-BEACON-RX", pkt, len);
+
+ /* Ignore devices not matching UUID */
+ if (pkt[0] || memcmp(pkt + 1, prov->uuid, 16))
+ return;
+
+ io = mesh_net_get_io(prov->net);
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON);
+
+ if ((prov->conn_id != DEFAULT_CONN_ID) ||
+ (prov->bearer != MESH_BEARER_IDLE)) {
+ l_info("PB-ADV: Already Provisioning");
+ return;
+ }
+
+ l_getrandom(&prov->conn_id, sizeof(prov->conn_id));
+ prov->bearer = MESH_BEARER_ADV;
+ send_open_req(prov);
+}
+
+static bool mesh_prov_enable(struct mesh_prov *prov, enum mesh_prov_mode mode,
+ uint8_t uuid[16])
+{
+ const uint8_t pb_adv_data[] = { MESH_AD_TYPE_BEACON, 0 };
+ uint8_t adv_data[62];
+ uint8_t adv_len, type;
+ struct mesh_io *io;
+ struct mesh_io_send_info tx_info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = 1000, /* ms */
+ .u.gen.cnt = 0, /* 0 == Infinite */
+ .u.gen.min_delay = 0, /* no delay */
+ .u.gen.max_delay = 0, /* no delay */
+ };
+
+ if (!prov || !prov->net)
+ return false;
+
+
+ prov->mode = mode;
+ memcpy(prov->uuid, uuid, 16);
+ prov->conn_id = DEFAULT_CONN_ID;
+ io = mesh_net_get_io(prov->net);
+
+ switch (mode) {
+ case MESH_PROV_MODE_NONE:
+ break;
+ case MESH_PROV_MODE_INITIATOR:
+ print_packet("Searching for uuid", uuid, 16);
+ prov->local_msg_num = DEFAULT_PROV_MSG_NUM;
+ prov->peer_msg_num = DEFAULT_DEV_MSG_NUM;
+ mesh_io_register_recv_cb(io, MESH_IO_FILTER_PROV,
+ adv_bearer_packet, prov);
+ mesh_io_register_recv_cb(io, MESH_IO_FILTER_BEACON,
+ beacon_packet, prov);
+ break;
+
+ case MESH_PROV_MODE_ADV_ACCEPTOR:
+ prov->local_msg_num = DEFAULT_DEV_MSG_NUM - 1;
+ prov->peer_msg_num = DEFAULT_PROV_MSG_NUM;
+
+ print_packet("Beaconing as unProvisioned uuid", uuid, 16);
+ adv_len = sizeof(pb_adv_data);
+ memcpy(adv_data, pb_adv_data, adv_len);
+ memcpy(adv_data + adv_len, uuid, 16);
+ adv_len += 16;
+ adv_len += 2;
+ mesh_io_register_recv_cb(io, MESH_IO_FILTER_PROV,
+ adv_bearer_packet, prov);
+ type = MESH_AD_TYPE_BEACON;
+ mesh_io_send_cancel(io, &type, 1);
+ mesh_io_send(io, &tx_info, adv_data, adv_len);
+ break;
+
+ case MESH_PROV_MODE_GATT_CLIENT:
+ case MESH_PROV_MODE_MESH_GATT_CLIENT:
+ case MESH_PROV_MODE_GATT_ACCEPTOR:
+ case MESH_PROV_MODE_MESH_SERVER:
+ case MESH_PROV_MODE_MESH_CLIENT:
+ default:
+ l_error("Unimplemented Prov Mode: %d", mode);
+ break;
+ }
+
+ return true;
+}
+
+bool mesh_prov_listen(struct mesh_net *net, uint8_t uuid[16], uint8_t caps[12],
+ mesh_prov_open_func_t open_callback,
+ mesh_prov_close_func_t close_callback,
+ mesh_prov_receive_func_t recv_callback,
+ void *user_data)
+{
+ struct mesh_prov *prov = mesh_net_get_prov(net);
+
+ if (!prov) {
+ prov = mesh_prov_new(net, 0);
+ if (!prov)
+ return false;
+
+ mesh_net_set_prov(net, prov);
+ }
+
+ prov->open_callback = open_callback;
+ prov->close_callback = close_callback;
+ prov->receive_callback = recv_callback;
+ prov->receive_data = prov; /* TODO: retink the callback placement */
+ memcpy(prov->caps, caps, sizeof(prov->caps));
+
+ prov->trans = MESH_TRANS_IDLE;
+
+
+ return mesh_prov_enable(prov, MESH_PROV_MODE_ADV_ACCEPTOR, uuid);
+}
+
+unsigned int mesh_prov_send(struct mesh_prov *prov,
+ const void *ptr, uint16_t size,
+ mesh_prov_send_func_t send_callback,
+ void *user_data)
+{
+ const uint8_t *data = ptr;
+
+ if (!prov)
+ return 0;
+
+ if (prov->trans != MESH_TRANS_IDLE)
+ return 0;
+
+ if (prov->remote) {
+ /* TODO -- PB-Remote */
+ } else {
+ prov->send_callback = send_callback;
+ prov->send_data = user_data;
+ prov->trans = MESH_TRANS_TX;
+ prov->local_msg_num++;
+ send_adv_msg(prov, data, size);
+ }
+
+ return 1;
+}
+
+bool mesh_prov_close(struct mesh_prov *prov, uint8_t reason)
+{
+ if (!prov)
+ return false;
+
+ prov->local_msg_num = 0;
+ send_close_ind(prov, reason);
+
+ prov->conn_id = DEFAULT_CONN_ID;
+ prov->local_msg_num = 0xFF;
+ prov->peer_msg_num = 0xFF;
+ prov->last_peer_msg_num = 0xFF;
+
+ if (prov->tx_timeout) {
+ l_timeout_remove(prov->tx_timeout);
+
+ /* If timing out, give Close indication 1 second of
+ * provisioning timing to get final Close indication out
+ */
+ prov->tx_timeout = l_timeout_create(1, tx_timeout, prov, NULL);
+ }
+
+ if (prov->close_callback)
+ prov->close_callback(prov->receive_data, reason);
+
+ return false;
+}
+
+void mesh_prov_set_addr(struct mesh_prov *prov, uint16_t addr)
+{
+ prov->addr = addr;
+}
+
+uint16_t mesh_prov_get_idx(struct mesh_prov *prov)
+{
+ return prov->net_idx;
+}
diff --git a/mesh/provision.c b/mesh/provision.c
new file mode 100644
index 000000000..c7cb5f160
--- /dev/null
+++ b/mesh/provision.c
@@ -0,0 +1,1159 @@
+/*
+ *
+ * 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/select.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <ctype.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <getopt.h>
+#include <time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+#include "src/shared/ecc.h"
+
+#include "mesh/display.h"
+#include "mesh/crypto.h"
+#include "mesh/net.h"
+#include "mesh/prov.h"
+#include "mesh/provision.h"
+#include "mesh/node.h"
+
+#define PROV_INVITE 0x00
+#define PROV_CAPS 0x01
+#define PROV_START 0x02
+#define PROV_PUB_KEY 0x03
+#define PROV_INP_CMPLT 0x04
+#define PROV_CONFIRM 0x05
+#define PROV_RANDOM 0x06
+#define PROV_DATA 0x07
+#define PROV_COMPLETE 0x08
+#define PROV_FAILED 0x09
+
+#define PROV_ERR_INVALID_PDU 0x01
+#define PROV_ERR_INVALID_FORMAT 0x02
+#define PROV_ERR_UNEXPECTED_PDU 0x03
+#define PROV_ERR_CONFIRM_FAILED 0x04
+#define PROV_ERR_INSUF_RESOURCE 0x05
+#define PROV_ERR_DECRYPT_FAILED 0x06
+#define PROV_ERR_UNEXPECTED_ERR 0x07
+#define PROV_ERR_CANT_ASSIGN_ADDR 0x08
+
+/* Expected Provisioning PDU sizes */
+static const uint16_t expected_pdu_size[] = {
+ 1 + 1, /* PROV_INVITE */
+ 1 + 1 + 2 + 1 + 1 + 1 + 2 + 1 + 2, /* PROV_CAPS */
+ 1 + 1 + 1 + 1 + 1 + 1, /* PROV_START */
+ 1 + 64, /* PROV_PUB_KEY */
+ 1, /* PROV_INP_CMPLT */
+ 1 + 16, /* PROV_CONFIRM */
+ 1 + 16, /* PROV_RANDOM */
+ 1 + 16 + 2 + 1 + 4 + 2 + 8, /* PROV_DATA */
+ 1, /* PROV_COMPLETE */
+ 1 + 1, /* PROV_FAILED */
+};
+
+static enum {
+ PUB_KEY_TYPE_ephemeral,
+ PUB_KEY_TYPE_available,
+} pub_key_type = PUB_KEY_TYPE_ephemeral;
+
+static enum {
+ AUTH_TYPE_3a,
+ AUTH_TYPE_3b,
+ AUTH_TYPE_3c,
+} prov_auth_type = AUTH_TYPE_3c;
+
+enum {
+ INT_PROV_IDLE,
+ INT_PROV_INVITE_SENT,
+ INT_PROV_INVITE_ACKED,
+ INT_PROV_START_SENT,
+ INT_PROV_START_ACKED,
+ INT_PROV_KEY_SENT,
+ INT_PROV_KEY_ACKED,
+ INT_PROV_CONF_SENT,
+ INT_PROV_CONF_ACKED,
+ INT_PROV_RAND_SENT,
+ INT_PROV_RAND_ACKED,
+ INT_PROV_DATA_SENT,
+ INT_PROV_DATA_ACKED,
+} int_prov_state = INT_PROV_IDLE;
+
+enum {
+ ACP_PROV_IDLE,
+ ACP_PROV_CAPS_SENT,
+ ACP_PROV_CAPS_ACKED,
+ ACP_PROV_KEY_SENT,
+ ACP_PROV_KEY_ACKED,
+ ACP_PROV_INP_CMPLT_SENT,
+ ACP_PROV_INP_CMPLT_ACKED,
+ ACP_PROV_CONF_SENT,
+ ACP_PROV_CONF_ACKED,
+ ACP_PROV_RAND_SENT,
+ ACP_PROV_RAND_ACKED,
+ ACP_PROV_CMPLT_SENT,
+ ACP_PROV_FAIL_SENT,
+} acp_prov_state = ACP_PROV_IDLE;
+
+static uint8_t prov_expected;
+static int8_t prov_last = -1;
+
+static void int_prov_send_cmplt(bool success, struct mesh_prov *prov);
+static void acp_prov_send_cmplt(bool success, struct mesh_prov *prov);
+
+static void swap_u256_bytes(uint8_t *u256)
+{
+ int i;
+
+ /* End-to-End byte reflection of 32 octet buffer */
+ for (i = 0; i < 16; i++) {
+ u256[i] ^= u256[31 - i];
+ u256[31 - i] ^= u256[i];
+ u256[i] ^= u256[31 - i];
+ }
+}
+
+static uint8_t u16_highest_bit(uint16_t mask)
+{
+ uint8_t cnt = 0;
+
+ if (!mask)
+ return 0xff;
+
+ while (mask & 0xfffe) {
+ cnt++;
+ mask >>= 1;
+ }
+
+ return cnt;
+}
+
+static void send_prov_start(struct mesh_prov *prov)
+{
+ struct mesh_net *net = prov->net;
+ struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+ uint8_t prov_start[6] = { PROV_START };
+
+ memset(prov_start + 1, 0, 1 + 1 + 1 + 1 + 1);
+ if (!(caps->algorithms & 0x0001)) {
+ /* We only support FIPS P-256 Elliptic Curve */
+ l_error("Unrecognized Algorithm %4.4x", caps->algorithms);
+ return;
+ }
+
+ if (caps->pub_type) {
+ /* Prov Step 2b: New device exposed PublicKey OOB */
+ prov_start[2] = 0x01;
+ pub_key_type = PUB_KEY_TYPE_available;
+ } else {
+ pub_key_type = PUB_KEY_TYPE_ephemeral;
+ }
+
+ if (caps->output_size &&
+ caps->output_action) {
+ /* Prov Step 3a: Output OOB used */
+ prov_start[3] = 0x02;
+ prov_start[4] = u16_highest_bit(caps->output_action);
+ prov_start[5] = caps->output_size > 8 ?
+ 8 : caps->output_size;
+
+ prov_auth_type = AUTH_TYPE_3a;
+
+ } else if (caps->input_size &&
+ caps->input_action) {
+ /* Prov Step 3b: Input OOB used */
+ prov_start[3] = 0x03;
+ prov_start[4] = u16_highest_bit(caps->input_action);
+ prov_start[5] = caps->input_size > 8 ?
+ 8 : caps->input_size;
+
+ prov_auth_type = AUTH_TYPE_3b;
+
+ } else {
+ if (caps->static_type)
+ prov_start[3] = 0x01;
+
+ /* Prov Step 3c: Static OOB used (or no OOB available) */
+ prov_auth_type = AUTH_TYPE_3c;
+ }
+
+ memcpy(&prov->conf_inputs.start, prov_start + 1,
+ sizeof(prov->conf_inputs.start));
+
+ int_prov_state = INT_PROV_START_SENT;
+ if (pub_key_type == PUB_KEY_TYPE_ephemeral)
+ prov_expected = PROV_PUB_KEY;
+ else if (prov_auth_type == AUTH_TYPE_3b)
+ prov_expected = PROV_INP_CMPLT;
+ else
+ prov_expected = PROV_CONFIRM;
+
+ mesh_prov_send(prov, prov_start, 6,
+ int_prov_send_cmplt, prov);
+
+}
+
+static void calculate_secrets(struct mesh_prov *prov, bool initiator)
+{
+ struct mesh_net *net = prov->net;
+ uint8_t *priv_key = mesh_net_priv_key_get(net);
+ bool test_mode = mesh_net_test_mode(net);
+ uint8_t tmp[64];
+
+ if (initiator) {
+ memcpy(prov->conf_inputs.prv_pub_key,
+ prov->l_public, sizeof(prov->l_public));
+ memcpy(prov->conf_inputs.dev_pub_key,
+ prov->r_public, sizeof(prov->r_public));
+ } else {
+ memcpy(prov->conf_inputs.prv_pub_key,
+ prov->r_public, sizeof(prov->r_public));
+ memcpy(prov->conf_inputs.dev_pub_key,
+ prov->l_public, sizeof(prov->l_public));
+ }
+
+ /* Convert to Mesh byte order */
+ memcpy(tmp, prov->r_public, 64);
+ swap_u256_bytes(tmp);
+ swap_u256_bytes(tmp + 32);
+
+ ecdh_shared_secret(tmp, priv_key, prov->secret);
+
+ /* Convert to Mesh byte order */
+ swap_u256_bytes(prov->secret);
+
+ mesh_crypto_s1(&prov->conf_inputs,
+ sizeof(prov->conf_inputs), prov->conf_salt);
+
+
+ mesh_crypto_prov_conf_key(prov->secret, prov->conf_salt,
+ prov->conf_key);
+
+ if (test_mode) {
+ print_packet("PublicKeyRemote", prov->r_public, 64);
+ print_packet("PublicKeyLocal", prov->l_public, 64);
+ print_packet("PrivateKeyLocal", priv_key, 32);
+ print_packet("ConfirmationInputs", &prov->conf_inputs,
+ sizeof(prov->conf_inputs));
+ print_packet("ECDHSecret", prov->secret,
+ sizeof(prov->secret));
+ print_packet("ConfirmationSalt", prov->conf_salt, 16);
+ print_packet("ConfirmationKey", prov->conf_key,
+ sizeof(prov->conf_key));
+ }
+}
+
+static void send_prov_key(struct mesh_prov *prov,
+ mesh_prov_send_func_t send_callback)
+{
+ uint8_t send_pub_key[65] = { PROV_PUB_KEY };
+
+ memcpy(send_pub_key + 1, prov->l_public, 64);
+ mesh_prov_send(prov, send_pub_key, 65,
+ send_callback, prov);
+}
+
+static void send_prov_data(struct mesh_prov *prov)
+{
+ struct mesh_net *net = prov->net;
+ struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+ uint64_t mic;
+ uint32_t iv_index;
+ uint8_t snb_flags;
+ uint16_t net_idx = mesh_prov_get_idx(prov);
+ uint8_t prov_data[1 + 16 + 2 + 1 + 4 + 2 + sizeof(mic)] = { PROV_DATA };
+ uint16_t uni_addr = mesh_net_prov_uni(net, caps->num_ele);
+ bool test_mode = mesh_net_test_mode(net);
+
+ /* Calculate Provisioning Data */
+ prov_expected = PROV_COMPLETE;
+ mesh_net_get_snb_state(net, &snb_flags, &iv_index);
+
+ mesh_net_get_key(net, !!(snb_flags & 0x01), net_idx, prov_data + 1);
+ l_put_be16(net_idx, prov_data + 1 + 16);
+ l_put_u8(snb_flags, prov_data + 1 + 16 + 2);
+ l_put_be32(iv_index, prov_data + 1 + 16 + 2 + 1);
+ l_put_be16(uni_addr, prov_data + 1 + 16 + 2 + 1 + 4);
+
+ if (test_mode)
+ print_packet("Data", prov_data + 1, 16 + 2 + 1 + 4 + 2);
+
+ mesh_crypto_device_key(prov->secret, prov->prov_salt, prov->dev_key);
+ if (test_mode) {
+ print_packet("DevKey", prov->dev_key, 16);
+ print_packet("NetworkKey", prov_data + 1, 16);
+ print_packet("NetworkKey Index", prov_data + 1 + 16, 2);
+ print_packet("SNB Flags", prov_data + 1 + 16 + 2, 1);
+ print_packet("IVindex", prov_data + 1 + 16 + 2 + 1, 4);
+ print_packet("Unicast Addr", prov_data + 1 + 16 + 2 + 1 + 4, 2);
+ }
+
+ mesh_crypto_aes_ccm_encrypt(prov->s_nonce, prov->s_key,
+ NULL, 0,
+ &prov_data[1],
+ sizeof(prov_data) - 1 - sizeof(mic),
+ &prov_data[1],
+ &mic, sizeof(mic));
+ if (test_mode)
+ print_packet("DataEncrypted + mic", prov_data + 1,
+ sizeof(prov_data) - 1);
+
+ int_prov_state = INT_PROV_DATA_SENT;
+ mesh_prov_send(prov, prov_data, sizeof(prov_data),
+ int_prov_send_cmplt, prov);
+ mesh_prov_set_addr(prov, uni_addr);
+}
+
+static void send_prov_conf(struct mesh_prov *prov,
+ mesh_prov_send_func_t send_callback)
+{
+ struct mesh_net *net = prov->net;
+ uint8_t *test_rand = mesh_net_prov_rand(net);
+ uint8_t prov_conf[1 + sizeof(prov->conf)] = { PROV_CONFIRM };
+ bool test_mode = mesh_net_test_mode(net);
+
+ if (test_mode && test_rand[0])
+ memcpy(prov->rand_auth, test_rand, 16);
+ else
+ l_getrandom(prov->rand_auth, 16);
+
+ /* Calculate Confirmation */
+ mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+ sizeof(prov->rand_auth), prov->conf);
+
+ /* Marshal Confirmation */
+ memcpy(prov_conf + 1, prov->conf, sizeof(prov->conf));
+
+ if (test_mode) {
+ print_packet("ConfirmationKey", prov->conf_key,
+ sizeof(prov->conf_key));
+ print_packet("RandomAuthValue", prov->rand_auth,
+ sizeof(prov->rand_auth));
+ print_packet("Sending Confirmation", prov->conf,
+ sizeof(prov->conf));
+ }
+
+ mesh_prov_send(prov, prov_conf, sizeof(prov_conf),
+ send_callback, prov);
+}
+
+static void send_prov_rand(struct mesh_prov *prov,
+ mesh_prov_send_func_t send_callback)
+{
+ struct mesh_net *net = prov->net;
+ uint8_t prov_rand[17] = { PROV_RANDOM };
+ bool test_mode = mesh_net_test_mode(net);
+
+ /* Marshal Random */
+ memcpy(prov_rand + 1, prov->rand_auth, 16);
+
+ if (test_mode)
+ print_packet("Sending Random", prov->rand_auth, 16);
+
+ mesh_prov_send(prov, prov_rand, sizeof(prov_rand),
+ send_callback, prov);
+}
+
+enum inputType {
+ INP_key,
+ INP_dec,
+ INP_text,
+};
+
+struct input_data {
+ struct mesh_prov *prov;
+ enum inputType type;
+ bool initiator;
+ void *dest;
+ void *user_data;
+ union {
+ struct {
+ uint8_t idx;
+ char data[129];
+ } key;
+ struct {
+ uint64_t value;
+ } dec;
+ struct {
+ uint8_t idx;
+ char str[16];
+ } text;
+ } u;
+};
+
+static void collectInput(struct mesh_prov *prov, char *prompt,
+ enum inputType type, bool initiator,
+ void *dest, void *user_data)
+{
+ struct input_data *inp = l_new(struct input_data, 1);
+
+ inp->prov = prov;
+ inp->type = type;
+ inp->dest = dest;
+ inp->initiator = initiator;
+ inp->user_data = user_data;
+
+ if (prompt)
+ l_info("%s", prompt);
+
+ /* TODO: Request agent get OOB data */
+}
+
+static uint32_t digit_mod(uint8_t power)
+{
+ uint32_t ret = 1;
+
+ while (power--)
+ ret *= 10;
+
+ return ret;
+}
+
+static char *key_type(uint8_t type)
+{
+ switch (type) {
+ case 0x01:
+ return "QR-Code";
+ case 0x02:
+ return "Barcode";
+ case 0x03:
+ return "NFC Tag";
+ case 0x04:
+ return "Printed Number";
+ default:
+ return "unknown Source";
+ }
+}
+
+static void int_prov_send_cmplt(bool success, struct mesh_prov *prov)
+{
+ struct mesh_net *net = prov->net;
+ struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+
+ l_debug("Provision sending complete");
+
+ switch (int_prov_state) {
+ case INT_PROV_INVITE_SENT:
+ int_prov_state = INT_PROV_INVITE_ACKED;
+ if (acp_prov_state == ACP_PROV_CAPS_SENT)
+ send_prov_start(prov);
+ break;
+ case INT_PROV_START_SENT:
+ int_prov_state = INT_PROV_START_ACKED;
+ if (pub_key_type == PUB_KEY_TYPE_ephemeral) {
+ int_prov_state = INT_PROV_KEY_SENT;
+ send_prov_key(prov, int_prov_send_cmplt);
+ } else {
+ collectInput(prov, NULL, INP_key, true,
+ prov->r_public, prov);
+ l_info("\n\nEnter key from %s:\n",
+ key_type(caps->pub_type));
+ }
+ break;
+ case INT_PROV_KEY_SENT:
+ int_prov_state = INT_PROV_KEY_ACKED;
+ if (pub_key_type == PUB_KEY_TYPE_ephemeral) {
+ prov_expected = PROV_PUB_KEY;
+ break;
+ }
+
+ /* Start Step 3 */
+ memset(prov->rand_auth + 16, 0, 16);
+ if (prov_auth_type == AUTH_TYPE_3a)
+ collectInput(prov,
+ "\n\nEnter prompted number from device:",
+ INP_dec, true,
+ prov->rand_auth + 32 - sizeof(uint32_t),
+ prov);
+
+ else if (prov_auth_type == AUTH_TYPE_3b) {
+ uint32_t oob_key;
+
+ l_getrandom(&oob_key, sizeof(uint32_t));
+ oob_key %= digit_mod(caps->input_size);
+ l_put_be32(oob_key,
+ prov->rand_auth + 32 -
+ sizeof(uint32_t));
+ l_info("\n\nEnter %d on Device\n", oob_key);
+ prov_expected = PROV_INP_CMPLT;
+
+ } else if (caps->static_type) {
+ collectInput(prov, NULL, INP_text, true,
+ prov->rand_auth + 16, prov);
+ l_info("\n\nstatic OOB str from %s:\n",
+ key_type(caps->static_type));
+
+ } else {
+ int_prov_state = INT_PROV_CONF_SENT;
+ send_prov_conf(prov, int_prov_send_cmplt);
+ }
+
+ break;
+ case INT_PROV_CONF_SENT:
+ int_prov_state = INT_PROV_CONF_ACKED;
+ if (acp_prov_state == ACP_PROV_CONF_SENT) {
+ int_prov_state = INT_PROV_RAND_SENT;
+ prov_expected = PROV_RANDOM;
+ send_prov_rand(prov, int_prov_send_cmplt);
+ }
+ break;
+ case INT_PROV_RAND_SENT:
+ int_prov_state = INT_PROV_RAND_ACKED;
+ if (acp_prov_state == ACP_PROV_RAND_SENT)
+ send_prov_data(prov);
+ break;
+ case INT_PROV_DATA_SENT:
+ int_prov_state = INT_PROV_DATA_ACKED;
+ break;
+ default:
+ case INT_PROV_INVITE_ACKED:
+ case INT_PROV_START_ACKED:
+ case INT_PROV_KEY_ACKED:
+ case INT_PROV_CONF_ACKED:
+ case INT_PROV_RAND_ACKED:
+ case INT_PROV_DATA_ACKED:
+ case INT_PROV_IDLE:
+ break;
+ }
+}
+
+void initiator_prov_open(struct mesh_prov *prov)
+{
+ uint8_t invite[] = { PROV_INVITE, 30 };
+ uint8_t *priv_key;
+
+ l_info("Provisioning link opened");
+
+ priv_key = mesh_net_priv_key_get(prov->net);
+ ecc_make_key(prov->l_public, priv_key);
+
+ int_prov_state = INT_PROV_INVITE_SENT;
+ prov_expected = PROV_CAPS;
+ prov_last = -1;
+ prov->conf_inputs.invite.attention = invite[1];
+ mesh_prov_send(prov, invite, sizeof(invite),
+ int_prov_send_cmplt, prov);
+}
+
+void initiator_prov_close(struct mesh_prov *prov, uint8_t reason)
+{
+ struct mesh_net *net = prov->net;
+ uint32_t iv_index;
+ uint8_t snb_flags;
+
+ l_info("Provisioning link closed");
+
+ /* Get the provisioned node's composition data*/
+ if (reason == 0) {
+ mesh_net_get_snb_state(net, &snb_flags, &iv_index);
+
+ l_info("Save provisioner's DB");
+ }
+}
+
+void initiator_prov_receive(const void *pkt, uint16_t size,
+ struct mesh_prov *prov)
+{
+ struct mesh_net *net = prov->net;
+ struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+ bool test_mode = mesh_net_test_mode(net);
+ const uint8_t *data = pkt;
+ uint8_t tmp[16];
+ uint8_t type = *data++;
+ uint8_t err = 0;
+
+
+ l_debug("Provisioning packet received type: %2.2x (%u octets)",
+ type, size);
+
+ if (type == prov_last) {
+ l_error("Ignore repeated %2.2x packet", type);
+ return;
+ } else if ((type > prov_expected || type < prov_last) &&
+ type != PROV_FAILED) {
+ l_error("Expected %2.2x, Got:%2.2x", prov_expected, type);
+ err = PROV_ERR_UNEXPECTED_PDU;
+ goto failure;
+ }
+
+ if (type >= L_ARRAY_SIZE(expected_pdu_size) ||
+ size != expected_pdu_size[type]) {
+ l_error("Expected PDU size %d, Got %d (type: %2.2x)",
+ expected_pdu_size[type], size, type);
+ err = PROV_ERR_INVALID_FORMAT;
+ goto failure;
+ }
+
+ prov_last = type;
+
+ switch (type) {
+ case PROV_CAPS: /* Capabilities */
+ int_prov_state = INT_PROV_INVITE_ACKED;
+ acp_prov_state = ACP_PROV_CAPS_SENT;
+ caps->num_ele = data[0];
+ if (test_mode)
+ l_info("Got Num Ele %d", data[0]);
+
+ caps->algorithms = l_get_be16(data + 1);
+ if (test_mode)
+ l_info("Got alg %d", caps->algorithms);
+
+ caps->pub_type = data[3];
+ if (test_mode)
+ l_info("Got pub_type %d", data[3]);
+
+ caps->static_type = data[4];
+ if (test_mode)
+ l_info("Got static_type %d", data[4]);
+
+ caps->output_size = data[5];
+ if (test_mode)
+ l_info("Got output_size %d", data[5]);
+
+ caps->output_action = l_get_be16(data + 6);
+ if (test_mode)
+ l_info("Got output_action %d", l_get_be16(data + 6));
+
+ caps->input_size = data[8];
+ if (test_mode)
+ l_info("Got input_size %d", data[8]);
+
+ caps->input_action = l_get_be16(data + 9);
+ if (test_mode)
+ l_info("Got input_action %d", l_get_be16(data + 9));
+
+ if (caps->algorithms != 0x0001) {
+ l_error("Unsupported Algorithm");
+ err = PROV_ERR_INVALID_FORMAT;
+ goto failure;
+ }
+
+ memcpy(&prov->conf_inputs.caps, data, 11);
+
+ if (int_prov_state == INT_PROV_INVITE_ACKED)
+ send_prov_start(prov);
+ break;
+
+ case PROV_PUB_KEY: /* Public Key */
+ int_prov_state = INT_PROV_KEY_ACKED;
+ acp_prov_state = ACP_PROV_KEY_SENT;
+ memcpy(prov->r_public, data, 64);
+ calculate_secrets(prov, true);
+ prov_expected = PROV_CONFIRM;
+
+ memset(prov->rand_auth + 16, 0, 16);
+ if (prov_auth_type == AUTH_TYPE_3a) {
+ collectInput(prov,
+ "\n\nEnter number from device:",
+ INP_dec, true,
+ prov->rand_auth + 32 - sizeof(uint32_t),
+ prov);
+
+ } else if (prov_auth_type == AUTH_TYPE_3b) {
+
+ uint32_t oob_key;
+
+ l_getrandom(&oob_key, sizeof(uint32_t));
+ oob_key %= digit_mod(caps->input_size);
+ l_put_be32(oob_key,
+ prov->rand_auth + 32 -
+ sizeof(uint32_t));
+ l_info("\n\nEnter %d on Device\n", oob_key);
+ prov_expected = PROV_INP_CMPLT;
+
+ } else if (caps->static_type) {
+ collectInput(prov, NULL, INP_dec, true,
+ prov->rand_auth + 16, prov);
+ l_info("\n\nstatic OOB str from %s:\n",
+ key_type(caps->static_type));
+
+ } else
+ send_prov_conf(prov, int_prov_send_cmplt);
+ break;
+
+ case PROV_INP_CMPLT: /* Provisioning Input Complete */
+ acp_prov_state = ACP_PROV_INP_CMPLT_SENT;
+ prov_expected = PROV_CONFIRM;
+ send_prov_conf(prov, int_prov_send_cmplt);
+ break;
+
+ case PROV_CONFIRM: /* Confirmation */
+ int_prov_state = INT_PROV_CONF_ACKED;
+ acp_prov_state = ACP_PROV_CONF_SENT;
+ /* RXed Device Confirmation */
+ memcpy(prov->conf, data, sizeof(prov->conf));
+ if (test_mode)
+ print_packet("ConfirmationDevice", prov->conf,
+ sizeof(prov->conf));
+
+ if (int_prov_state == INT_PROV_CONF_ACKED) {
+ prov_expected = PROV_RANDOM;
+ send_prov_rand(prov, int_prov_send_cmplt);
+ }
+ break;
+
+ case PROV_RANDOM: /* Random */
+ int_prov_state = INT_PROV_RAND_ACKED;
+ acp_prov_state = ACP_PROV_RAND_SENT;
+
+ /* Calculate SessionKey while the data is fresh */
+ mesh_crypto_prov_prov_salt(prov->conf_salt,
+ prov->rand_auth, data,
+ prov->prov_salt);
+ mesh_crypto_session_key(prov->secret, prov->prov_salt,
+ prov->s_key);
+ mesh_crypto_nonce(prov->secret, prov->prov_salt, prov->s_nonce);
+ if (test_mode) {
+ print_packet("SessionKey", prov->s_key,
+ sizeof(prov->s_key));
+ print_packet("Nonce", prov->s_nonce,
+ sizeof(prov->s_nonce));
+ }
+
+ /* RXed Device Confirmation */
+ memcpy(prov->rand_auth, data, sizeof(prov->conf));
+ if (test_mode)
+ print_packet("RandomDevice", prov->rand_auth, 16);
+
+ mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+ sizeof(prov->rand_auth), tmp);
+
+ if (memcmp(tmp, prov->conf, sizeof(prov->conf))) {
+ l_error("Provisioning Failed-Confirm compare)");
+ err = PROV_ERR_CONFIRM_FAILED;
+ goto failure;
+ }
+
+ if (int_prov_state == INT_PROV_RAND_ACKED) {
+ prov_expected = PROV_COMPLETE;
+ send_prov_data(prov);
+ }
+ break;
+
+ case PROV_COMPLETE: /* Complete */
+ l_info("Provisioning Complete");
+ int_prov_state = INT_PROV_IDLE;
+ mesh_prov_close(prov, 0);
+ break;
+
+ case PROV_FAILED: /* Failed */
+ l_error("Provisioning Failed (reason: %d)", data[0]);
+ err = data[0];
+ goto failure;
+
+ default:
+ l_error("Unknown Pkt %2.2x", type);
+ err = PROV_ERR_UNEXPECTED_PDU;
+ goto failure;
+ }
+
+ return;
+
+failure:
+ int_prov_state = INT_PROV_IDLE;
+ mesh_prov_close(prov, err);
+}
+
+static void acp_prov_send_cmplt(bool success, struct mesh_prov *prov)
+{
+ l_debug("Provision sending complete");
+
+ switch (acp_prov_state) {
+ case ACP_PROV_CAPS_SENT:
+ acp_prov_state = ACP_PROV_CAPS_ACKED;
+ if (int_prov_state == INT_PROV_KEY_SENT) {
+ acp_prov_state = ACP_PROV_KEY_SENT;
+ prov_expected = PROV_CONFIRM;
+ send_prov_key(prov, acp_prov_send_cmplt);
+ }
+ break;
+ case ACP_PROV_KEY_SENT:
+ acp_prov_state = ACP_PROV_KEY_ACKED;
+ if (int_prov_state == INT_PROV_CONF_SENT) {
+ acp_prov_state = ACP_PROV_CONF_SENT;
+ prov_expected = PROV_RANDOM;
+ send_prov_conf(prov, acp_prov_send_cmplt);
+ }
+ break;
+ case ACP_PROV_INP_CMPLT_SENT:
+ acp_prov_state = ACP_PROV_INP_CMPLT_ACKED;
+ break;
+ case ACP_PROV_CONF_SENT:
+ acp_prov_state = ACP_PROV_CONF_ACKED;
+ if (int_prov_state == INT_PROV_RAND_SENT) {
+ acp_prov_state = ACP_PROV_RAND_SENT;
+ prov_expected = PROV_DATA;
+ send_prov_rand(prov, acp_prov_send_cmplt);
+ }
+ break;
+ case ACP_PROV_RAND_SENT:
+ acp_prov_state = ACP_PROV_RAND_ACKED;
+ break;
+ case ACP_PROV_CMPLT_SENT:
+ acp_prov_state = ACP_PROV_IDLE;
+ mesh_net_provisioned_set(prov->net, true);
+ default:
+ case ACP_PROV_IDLE:
+ case ACP_PROV_CAPS_ACKED:
+ case ACP_PROV_KEY_ACKED:
+ case ACP_PROV_INP_CMPLT_ACKED:
+ case ACP_PROV_CONF_ACKED:
+ case ACP_PROV_RAND_ACKED:
+ case ACP_PROV_FAIL_SENT:
+ break;
+ }
+}
+
+void acceptor_prov_open(struct mesh_prov *prov)
+{
+ uint8_t *priv_key;
+
+ l_info("Provisioning link opened");
+
+ priv_key = mesh_net_priv_key_get(prov->net);
+ ecc_make_key(prov->l_public, priv_key);
+
+ prov_expected = PROV_INVITE;
+ prov_last = -1;
+}
+
+void acceptor_prov_close(struct mesh_prov *prov, uint8_t reason)
+{
+ l_info("Provisioning link closed");
+ mesh_prov_unref(prov);
+}
+
+static void prov_store_cfm(void *user_data, bool result)
+{
+ struct mesh_prov *prov = user_data;
+ uint8_t out[2];
+
+ if (result) {
+ acp_prov_state = ACP_PROV_CMPLT_SENT;
+ out[0] = PROV_COMPLETE;
+ mesh_prov_send(prov, out, 1,
+ acp_prov_send_cmplt,
+ prov);
+ } else {
+ acp_prov_state = ACP_PROV_FAIL_SENT;
+ out[0] = PROV_FAILED;
+ out[1] = PROV_ERR_INSUF_RESOURCE;
+ mesh_prov_send(prov, out, 2, NULL, NULL);
+ }
+}
+
+void acceptor_prov_receive(const void *pkt, uint16_t size,
+ struct mesh_prov *prov)
+{
+ struct mesh_net *net = prov->net;
+ struct mesh_net_prov_caps *caps = mesh_net_prov_caps_get(net);
+ uint8_t *priv_key = mesh_net_priv_key_get(net);
+ bool test_mode = mesh_net_test_mode(net);
+ bool ret;
+ const uint8_t *data = pkt;
+ uint8_t type = *data++;
+ uint8_t out[129];
+ uint8_t tmp[16];
+ uint8_t rand_dev[16];
+ uint64_t rx_mic, decode_mic;
+
+ l_debug("Provisioning packet received type: %2.2x (%u octets)",
+ type, size);
+
+ if (type == prov_last) {
+ l_error("Ignore repeated %2.2x packet", type);
+ return;
+ } else if (type > prov_expected || type < prov_last) {
+ l_error("Expected %2.2x, Got:%2.2x", prov_expected, type);
+ out[1] = PROV_ERR_UNEXPECTED_PDU;
+ goto failure;
+ }
+
+ if (type >= L_ARRAY_SIZE(expected_pdu_size) ||
+ size != expected_pdu_size[type]) {
+ l_error("Expected PDU size %d, Got %d (type: %2.2x)",
+ size, expected_pdu_size[type], type);
+ out[1] = PROV_ERR_INVALID_FORMAT;
+ goto failure;
+ }
+
+ prov_last = type;
+
+ switch (type) {
+ case PROV_INVITE: /* Prov Invite */
+ int_prov_state = INT_PROV_INVITE_SENT;
+ /* Prov Capabilities */
+ out[0] = PROV_CAPS;
+ out[1] = caps->num_ele;
+ l_put_be16(caps->algorithms, out + 2);
+ out[4] = caps->pub_type;
+ out[5] = caps->static_type;
+ out[6] = caps->output_size;
+ l_put_be16(caps->output_action, out + 7);
+ out[9] = caps->input_size;
+ l_put_be16(caps->input_action, out + 10);
+
+ prov->conf_inputs.invite.attention = data[0];
+ memcpy(&prov->conf_inputs.caps, out + 1,
+ sizeof(prov->conf_inputs.caps));
+
+ acp_prov_state = ACP_PROV_CAPS_SENT;
+ prov_expected = PROV_START;
+ mesh_prov_send(prov, out, sizeof(*caps) + 1,
+ acp_prov_send_cmplt, prov);
+ break;
+
+ case PROV_START: /* Prov Start */
+ if (data[0]) {
+ /* Only Algorithm 0x00 supported */
+ l_error("Invalid Algorithm: %2.2x", data[0]);
+ out[1] = PROV_ERR_INVALID_FORMAT;
+ goto failure;
+ }
+
+ acp_prov_state = ACP_PROV_CAPS_ACKED;
+ int_prov_state = INT_PROV_START_SENT;
+ prov_expected = PROV_PUB_KEY;
+ memcpy(&prov->conf_inputs.start, data,
+ sizeof(prov->conf_inputs.start));
+ if (data[1] == 1 && caps->pub_type) {
+ pub_key_type = PUB_KEY_TYPE_available;
+ ecc_make_key(prov->l_public, priv_key);
+ } else if (data[1] == 0) {
+ pub_key_type = PUB_KEY_TYPE_ephemeral;
+ /* Use Ephemeral Key */
+ l_getrandom(priv_key, 32);
+ ecc_make_key(prov->l_public, priv_key);
+ } else {
+ out[1] = PROV_ERR_INVALID_FORMAT;
+ goto failure;
+ }
+
+ swap_u256_bytes(prov->l_public);
+ swap_u256_bytes(prov->l_public + 32);
+
+ switch (data[2]) {
+ default:
+ out[1] = PROV_ERR_INVALID_FORMAT;
+ goto failure;
+
+ case 0x00:
+ case 0x01:
+ prov_auth_type = AUTH_TYPE_3c;
+ break;
+
+ case 0x02:
+ prov_auth_type = AUTH_TYPE_3a;
+ caps->output_action = 1 << data[3];
+ caps->output_size = data[4];
+ break;
+
+ case 0x03:
+ prov_auth_type = AUTH_TYPE_3b;
+ caps->input_action = 1 << data[3];
+ caps->input_size = data[4];
+ break;
+ }
+ break;
+
+ case PROV_PUB_KEY: /* Public Key */
+ int_prov_state = INT_PROV_KEY_SENT;
+ prov_expected = PROV_CONFIRM;
+ /* Save Key */
+ memcpy(prov->r_public, data, 64);
+ calculate_secrets(prov, false);
+
+ if (pub_key_type == PUB_KEY_TYPE_ephemeral) {
+ acp_prov_state = ACP_PROV_KEY_SENT;
+ send_prov_key(prov, acp_prov_send_cmplt);
+ }
+
+ /* Start Step 3 */
+ memset(prov->rand_auth + 16, 0, 16);
+ if (prov_auth_type == AUTH_TYPE_3a) {
+ uint32_t oob_key;
+
+ l_getrandom(&oob_key, sizeof(uint32_t));
+ oob_key %= digit_mod(caps->output_size);
+ l_put_be32(oob_key,
+ prov->rand_auth + 32 - sizeof(uint32_t));
+ l_info("\n\nEnter %d on Provisioner\n",
+ oob_key);
+
+ } else if (prov_auth_type == AUTH_TYPE_3b) {
+ if (caps->input_action == (1 << 3)) {
+ /* TODO: Collect Text Input data */
+ ;
+ } else {
+ /* TODO: Collect Decimal Input data */
+ ;
+ }
+
+ } else {
+ if (caps->static_type) {
+ /* TODO: Collect Static Input data */
+ /* (If needed) */
+ ;
+ }
+ }
+
+ break;
+
+ case PROV_CONFIRM: /* Confirmation */
+ int_prov_state = INT_PROV_CONF_SENT;
+ acp_prov_state = ACP_PROV_KEY_ACKED;
+ /* RXed Provision Confirmation */
+ memcpy(prov->r_conf, data, sizeof(prov->r_conf));
+ if (test_mode)
+ print_packet("ConfirmationProvisioner",
+ prov->r_conf,
+ sizeof(prov->r_conf));
+
+ if (acp_prov_state == ACP_PROV_KEY_ACKED) {
+ prov_expected = PROV_RANDOM;
+ send_prov_conf(prov, acp_prov_send_cmplt);
+ }
+ break;
+
+ case PROV_RANDOM: /* Random */
+ int_prov_state = INT_PROV_RAND_SENT;
+ acp_prov_state = ACP_PROV_CONF_ACKED;
+
+ /* Calculate Session key while the data is fresh */
+ mesh_crypto_prov_prov_salt(prov->conf_salt, data,
+ prov->rand_auth,
+ prov->prov_salt);
+ mesh_crypto_session_key(prov->secret, prov->prov_salt,
+ prov->s_key);
+ mesh_crypto_nonce(prov->secret, prov->prov_salt, prov->s_nonce);
+
+ if (test_mode) {
+ print_packet("SessionKey", prov->s_key,
+ sizeof(prov->s_key));
+ print_packet("Nonce", prov->s_nonce,
+ sizeof(prov->s_nonce));
+ }
+
+
+ /* Save Local Random data to send after verification */
+ memcpy(rand_dev, prov->rand_auth, 16);
+ /* RXed Provisioner Confirmation */
+ memcpy(prov->rand_auth, data, 16);
+ if (test_mode)
+ print_packet("RandomProvisioner", prov->rand_auth, 16);
+
+ mesh_crypto_aes_cmac(prov->conf_key, prov->rand_auth,
+ sizeof(prov->rand_auth), tmp);
+
+ if (memcmp(tmp, prov->r_conf,
+ sizeof(prov->r_conf))) {
+ l_error("Provisioning Failed-Confirm compare");
+ out[1] = PROV_ERR_CONFIRM_FAILED;
+ goto failure;
+ }
+
+
+ memcpy(prov->rand_auth, rand_dev, 16);
+ if (acp_prov_state == ACP_PROV_CONF_ACKED) {
+ prov_expected = PROV_DATA;
+ send_prov_rand(prov, acp_prov_send_cmplt);
+ }
+ break;
+
+ case PROV_DATA: /* Provisioning Data */
+ int_prov_state = INT_PROV_DATA_SENT;
+ acp_prov_state = ACP_PROV_RAND_ACKED;
+ if (test_mode) {
+ print_packet("DataEncrypted + mic", data, size - 1);
+ print_packet("Rxed-mic", data + 16 + 2 + 1 + 4 + 2, 8);
+ }
+
+ rx_mic = l_get_be64(data + 16 + 2 + 1 + 4 + 2);
+ mesh_crypto_aes_ccm_decrypt(prov->s_nonce, prov->s_key,
+ NULL, 0,
+ data, size - 1, out + 1,
+ &decode_mic, sizeof(decode_mic));
+
+ if (test_mode) {
+ print_packet("Data", out + 1, 16 + 2 + 1 + 4 + 2);
+ l_info("Calc-mic: %16.16lx", decode_mic);
+ }
+
+ if (rx_mic == decode_mic) {
+ mesh_crypto_device_key(prov->secret,
+ prov->prov_salt,
+ prov->dev_key);
+ if (test_mode) {
+ print_packet("DevKey", prov->dev_key, 16);
+ print_packet("NetworkKey", out + 1, 16);
+ print_packet("NetworkKey Index",
+ out + 1 + 16, 2);
+ print_packet("SNB Flags",
+ out + 1 + 16 + 2, 1);
+ print_packet("IVindex",
+ out + 1 + 16 + 2 + 1, 4);
+ print_packet("Unicast Addr",
+ out + 1 + 16 + 2 + 1 + 4, 2);
+ }
+
+ /* Set Provisioned Data */
+ ret = mesh_net_provisioned_new(prov->net,
+ prov->dev_key,
+ l_get_be16(out + 17),
+ out + 1,
+ l_get_be16(out + 24),
+ out[19],
+ l_get_be32(out + 20),
+ prov_store_cfm, prov);
+
+ if (!ret) {
+ out[1] = PROV_ERR_INSUF_RESOURCE;
+ goto failure;
+ }
+ } else {
+ l_error("Provisioning Failed-MIC compare");
+ out[1] = PROV_ERR_DECRYPT_FAILED;
+ goto failure;
+ }
+ break;
+
+ default:
+ l_error("Unknown Pkt %2.2x", type);
+ out[1] = PROV_ERR_UNEXPECTED_PDU;
+ goto failure;
+ }
+
+ return;
+
+failure:
+ acp_prov_state = ACP_PROV_FAIL_SENT;
+ out[0] = PROV_FAILED;
+ mesh_prov_send(prov, out, 2, acp_prov_send_cmplt, prov);
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 06/14] mesh: Upper and Lower mesh transport
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (4 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 05/14] mesh: Provisioning logic for mesh Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 07/14] mesh: Add Accessors to Transport layer data Brian Gix
` (8 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/net.c | 3639 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 3639 insertions(+)
create mode 100644 mesh/net.c
diff --git a/mesh/net.c b/mesh/net.c
new file mode 100644
index 000000000..0dafa1780
--- /dev/null
+++ b/mesh/net.c
@@ -0,0 +1,3639 @@
+/*
+ *
+ * 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 <stdio.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+#include "mesh/util.h"
+
+#include "mesh/display.h"
+#include "mesh/crypto.h"
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/mesh-io.h"
+#include "mesh/friend.h"
+#include "mesh/storage.h"
+#include "mesh/model.h"
+#include "mesh/appkey.h"
+#include "mesh/prov.h"
+#include "mesh/provision.h"
+
+#define abs_diff(a, b) ((a) > (b) ? (a) - (b) : (b) - (a))
+
+#define IV_IDX_DIFF_RANGE 42
+
+/* #define IV_IDX_UPD_MIN (60) 1 minute for Testing */
+#define IV_IDX_UPD_MIN (60 * 60 * 96) /* 96 Hours - per Spec */
+#define IV_IDX_UPD_HOLD (IV_IDX_UPD_MIN/2)
+#define IV_IDX_UPD_MAX (IV_IDX_UPD_MIN + IV_IDX_UPD_HOLD)
+
+#define iv_is_updating(net) ((net)->iv_upd_state == IV_UPD_UPDATING)
+
+#define IV_UPDATE_SEQ_TRIGGER 0x800000 /* Half of Seq-Nums expended */
+
+#define SEG_TO 2
+#define MSG_TO 60
+
+#define DEFAULT_MIN_DELAY 0
+#define DEFAULT_MAX_DELAY 25
+
+#define DEFAULT_TRANSMIT_COUNT 1
+#define DEFAULT_TRANSMIT_INTERVAL 100
+
+#define BEACON_TYPE_SNB 0x01
+
+#define BEACON_INTERVAL_MIN 10
+#define BEACON_INTERVAL_MAX 600
+
+#define SAR_KEY(src, seq0) ((((uint32_t)(seq0)) << 16) | (src))
+
+enum _iv_upd_state {
+ /* Allows acceptance of any iv_index secure net beacon */
+ IV_UPD_INIT,
+ /* Normal, can transition, accept current or old */
+ IV_UPD_NORMAL,
+ /* Updating proc running, we use old, accept old or new */
+ IV_UPD_UPDATING,
+ /* Normal, can *not* transition, accept current or old iv_index */
+ IV_UPD_NORMAL_HOLD,
+};
+
+struct net_key {
+ struct mesh_key_set key_set;
+ unsigned int beacon_id;
+ uint8_t key[16];
+ uint8_t beacon_key[16];
+ uint8_t network_id[8];
+};
+
+struct mesh_beacon {
+ struct l_timeout *timeout;
+ uint32_t ts;
+ uint16_t observe_period;
+ uint16_t observed;
+ uint16_t expected;
+ uint8_t half_period;
+ uint8_t beacon[23];
+};
+
+struct mesh_subnet {
+ struct mesh_net *net;
+ uint16_t idx;
+ struct net_key *tx;
+ struct net_key current;
+ struct net_key updated;
+ struct mesh_beacon snb;
+ uint8_t key_refresh;
+ uint8_t kr_phase;
+};
+
+struct mesh_net {
+ int ref_count;
+ struct mesh_io *io;
+ struct mesh_node *local_node;
+ struct mesh_prov *prov;
+ void *jconfig_local;
+ const char *cfg_file;
+ struct l_queue *app_keys;
+ unsigned int pkt_id;
+ unsigned int bea_id;
+ unsigned int beacon_id;
+ unsigned int key_id_next;
+ unsigned int sar_id_next;
+
+ bool friend_enable;
+ bool beacon_enable;
+ bool proxy_enable;
+ bool provisioner;
+ bool provisioned;
+ bool friend_seq;
+ struct l_timeout *iv_update_timeout;
+ enum _iv_upd_state iv_upd_state;
+
+ bool iv_update;
+ uint32_t instant; /* Controller Instant of recent Rx */
+ uint32_t iv_index;
+ uint32_t seq_num;
+ uint32_t cached_seq_num;
+ uint16_t crpl;
+ uint16_t src_addr;
+ uint16_t last_addr;
+ uint16_t friend_addr;
+ uint16_t tx_interval;
+ uint16_t tx_cnt;
+ uint8_t chan; /* Channel of recent Rx */
+ uint8_t default_ttl;
+ uint8_t tid;
+ uint8_t window_accuracy;
+
+ struct {
+ bool enable;
+ uint16_t interval;
+ uint8_t count;
+ } relay;
+
+ struct mesh_net_heartbeat heartbeat;
+
+ struct l_queue *subnets;
+ struct l_queue *msg_cache;
+ struct l_queue *sar_in;
+ struct l_queue *sar_out;
+ struct l_queue *frnd_msgs;
+ struct l_queue *friends;
+ struct l_queue *destinations;
+ struct l_queue *fast_cache;
+ struct l_queue *key_sets;
+
+ uint8_t prov_priv_key[32];
+
+ /* Unprovisioned Identity */
+ char id_name[20];
+ uint8_t id_uuid[16];
+
+ /* Provisioner: unicast address range */
+ struct mesh_net_addr_range prov_uni_addr;
+
+ /* Test Data */
+ uint8_t prov_rand[16];
+ uint8_t test_bd_addr[6];
+ struct mesh_net_prov_caps prov_caps;
+ bool test_mode;
+};
+
+struct mesh_msg {
+ uint16_t src;
+ uint32_t seq;
+ uint32_t mic;
+};
+
+struct mesh_sar {
+ unsigned int id;
+ struct l_timeout *seg_timeout;
+ struct l_timeout *msg_timeout;
+ mesh_net_status_func_t status_func;
+ void *user_data;
+ uint32_t flags;
+ uint32_t last_nak;
+ uint32_t iv_index;
+ uint32_t seqAuth;
+ uint16_t seqZero;
+ uint16_t app_idx;
+ uint16_t src;
+ uint16_t remote;
+ uint16_t len;
+ bool szmic;
+ bool frnd;
+ bool frnd_cred;
+ uint8_t ttl;
+ uint8_t last_seg;
+ uint8_t key_id;
+ uint8_t buf[4]; /* Large enough for ACK-Flags and MIC */
+};
+
+struct mesh_destination {
+ uint16_t dst;
+ uint16_t ref_cnt;
+};
+
+struct msg_rx {
+ const uint8_t *data;
+ uint32_t iv_index;
+ uint32_t seq;
+ uint16_t src;
+ uint16_t dst;
+ uint16_t size;
+ uint8_t tc;
+ bool done;
+ bool szmic;
+ union {
+ struct {
+ uint16_t app_idx;
+ uint8_t key_id;
+ } m;
+ struct {
+ uint16_t seq0;
+ } a;
+ struct {
+ uint8_t opcode;
+ } c;
+ } u;
+};
+
+struct net_decode {
+ struct mesh_net *net;
+ struct mesh_friend *frnd;
+ struct mesh_key_set *key_set;
+ uint8_t *packet;
+ uint32_t iv_index;
+ uint8_t size;
+ uint8_t nid;
+ bool proxy;
+};
+
+static inline struct mesh_subnet *get_primary_subnet(struct mesh_net *net)
+{
+ return l_queue_peek_head(net->subnets);
+}
+
+static bool match_key_index(const void *a, const void *b)
+{
+ const struct mesh_subnet *subnet = a;
+ uint16_t idx = L_PTR_TO_UINT(b);
+
+ return subnet->idx == idx;
+}
+
+static bool match_key_set(const void *a, const void *b)
+{
+ const struct mesh_subnet *subnet = a;
+ const struct mesh_key_set *key_set = b;
+
+ return (key_set == &subnet->current.key_set) ||
+ (key_set == &subnet->updated.key_set);
+}
+
+static bool match_network_id(const void *a, const void *b)
+{
+ const struct mesh_subnet *subnet = a;
+ const uint8_t *network_id = b;
+
+ return ((memcmp(subnet->current.network_id, network_id, 8) == 0) ||
+ (memcmp(subnet->updated.network_id, network_id, 8) == 0));
+}
+
+static void idle_mesh_heartbeat_send(void *net)
+{
+ mesh_net_heartbeat_send(net);
+}
+
+static void trigger_heartbeat(struct mesh_net *net, uint16_t feature,
+ bool in_use)
+{
+ struct mesh_net_heartbeat *hb = &net->heartbeat;
+
+ l_info("%s: %4.4x --> %d", __func__, feature, in_use);
+ if (in_use) {
+ if (net->heartbeat.features & feature)
+ return; /* no change */
+
+ hb->features |= feature;
+ } else {
+ if (!(hb->features & feature))
+ return; /* no change */
+
+ hb->features &= ~feature;
+ }
+
+ if (!(hb->pub_features & feature))
+ return; /* not interested in this feature */
+
+ l_idle_oneshot(idle_mesh_heartbeat_send, net, NULL);
+
+}
+
+static bool match_by_friend(const void *a, const void *b)
+{
+ const struct mesh_friend *frnd = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+
+ return frnd->dst == dst;
+}
+
+static void free_friend_internals(struct mesh_friend *frnd)
+{
+ if (frnd->pkt_cache)
+ l_queue_destroy(frnd->pkt_cache, l_free);
+
+ if (frnd->grp_list)
+ l_free(frnd->grp_list);
+
+ frnd->pkt_cache = NULL;
+ frnd->grp_list = NULL;
+ mesh_net_remove_keyset(frnd->net, &frnd->key_set);
+ mesh_net_remove_keyset(frnd->net, &frnd->new_key_set);
+}
+
+static void frnd_kr_phase1(void *a, void *b)
+{
+ struct mesh_friend *frnd = a;
+ const uint8_t *key = b;
+ uint8_t p[9] = {0x01};
+
+ l_put_be16(frnd->dst, p + 1);
+ l_put_be16(frnd->net->src_addr, p + 3);
+ l_put_be16(frnd->lp_cnt, p + 5);
+ l_put_be16(frnd->fn_cnt, p + 7);
+
+ mesh_crypto_k2(key, p, sizeof(p), &frnd->new_key_set.nid,
+ frnd->new_key_set.enc_key,
+ frnd->new_key_set.privacy_key);
+
+ mesh_net_add_keyset(frnd->net, &frnd->new_key_set);
+ l_info("Add New KeySet %2.2x for %4.4x",
+ frnd->new_key_set.nid, frnd->dst);
+ l_info("Outgoing with %2.2x", frnd->key_set.nid);
+}
+
+static void frnd_kr_phase2(void *a, void *b)
+{
+ struct mesh_friend *frnd = a;
+
+ /*
+ * I think that a Friend should use Old Key as long as possible
+ * Because a Friend Node will enter Phase 3 before it's LPN.
+ * Alternatively, the FN could keep the Old Friend Keys until it
+ * receives it's first Poll using the new keys (?)
+ */
+
+ l_info("Use Both KeySet %2.2x && %2.2x for %4.4x",
+ frnd->key_set.nid, frnd->new_key_set.nid, frnd->dst);
+}
+
+static void frnd_kr_phase3(void *a, void *b)
+{
+ struct mesh_friend *frnd = a;
+ struct mesh_net *net = b;
+
+ l_info("Replace KeySet %2.2x with %2.2x for %4.4x",
+ frnd->key_set.nid, frnd->new_key_set.nid, frnd->dst);
+ frnd->key_set = frnd->new_key_set;
+ mesh_net_remove_keyset(net, &frnd->new_key_set);
+ frnd->new_key_set.nid = 0xff;
+}
+
+/* TODO: add net key idx? For now, use primary net key */
+struct mesh_friend *mesh_friend_new(struct mesh_net *net, uint16_t dst,
+ uint8_t ele_cnt, uint8_t frd,
+ uint8_t frw, uint32_t fpt,
+ uint16_t fn_cnt, uint16_t lp_cnt)
+{
+ struct mesh_subnet *subnet;
+ uint8_t p[9] = {0x01};
+ struct mesh_friend *frnd = l_queue_find(net->friends,
+ match_by_friend, L_UINT_TO_PTR(dst));
+
+ if (frnd) {
+ /* Kill all timers and empty cache for this friend */
+ free_friend_internals(frnd);
+ l_timeout_remove(frnd->timeout);
+ frnd->timeout = NULL;
+ } else {
+ frnd = l_new(struct mesh_friend, 1);
+ l_queue_push_head(net->friends, frnd);
+ }
+
+ /* add _k2 */
+ frnd->net = net;
+ frnd->dst = dst;
+ frnd->frd = frd;
+ frnd->frw = frw;
+ frnd->fn_cnt = fn_cnt;
+ frnd->lp_cnt = lp_cnt;
+ frnd->poll_timeout = fpt;
+ frnd->ele_cnt = ele_cnt;
+ frnd->pkt_cache = l_queue_new();
+ frnd->new_key_set.nid = NET_NID_INVALID;
+
+ l_put_be16(dst, p + 1);
+ l_put_be16(net->src_addr, p + 3);
+ l_put_be16(lp_cnt, p + 5);
+ l_put_be16(fn_cnt, p + 7);
+
+ subnet = get_primary_subnet(net);
+ /* TODO: the primary key must be present, do we need to add check?. */
+
+ mesh_crypto_k2(subnet->current.key, p, sizeof(p),
+ &frnd->key_set.nid,
+ frnd->key_set.enc_key,
+ frnd->key_set.privacy_key);
+
+ frnd->key_set.frnd = true;
+ mesh_net_add_keyset(net, &frnd->key_set);
+
+ if (subnet->updated.key_set.nid == NET_NID_INVALID)
+ return frnd;
+
+ mesh_crypto_k2(subnet->updated.key, p, sizeof(p),
+ &frnd->new_key_set.nid,
+ frnd->new_key_set.enc_key,
+ frnd->new_key_set.privacy_key);
+ frnd->new_key_set.frnd = true;
+ mesh_net_add_keyset(net, &frnd->new_key_set);
+
+ return frnd;
+}
+
+void mesh_friend_free(void *data)
+{
+ struct mesh_friend *frnd = data;
+
+ free_friend_internals(frnd);
+ l_timeout_remove(frnd->timeout);
+ l_free(frnd);
+}
+
+bool mesh_friend_clear(struct mesh_net *net, struct mesh_friend *frnd)
+{
+ bool removed = l_queue_remove(net->friends, frnd);
+
+ free_friend_internals(frnd);
+
+ return removed;
+}
+
+bool mesh_net_add_keyset(struct mesh_net *net, struct mesh_key_set *key_set)
+{
+ if (!net)
+ return false;
+
+ l_info("Add KEY_SET %2.2x (%d) %p",
+ key_set->nid, key_set->frnd, key_set);
+ return l_queue_push_tail(net->key_sets, key_set);
+}
+
+bool mesh_net_remove_keyset(struct mesh_net *net, struct mesh_key_set *key_set)
+{
+ if (!net || !net->key_sets)
+ return false;
+
+ l_info("DEL KEY_SET %2.2x (%d) %p",
+ key_set->nid, key_set->frnd, key_set);
+ return l_queue_remove(net->key_sets, key_set);
+}
+
+void mesh_friend_sub_add(struct mesh_net *net, uint16_t lpn, uint8_t ele_cnt,
+ uint8_t grp_cnt,
+ const uint8_t *list)
+{
+ uint16_t *new_list;
+ uint16_t *grp_list;
+ struct mesh_friend *frnd = l_queue_find(net->friends,
+ match_by_friend,
+ L_UINT_TO_PTR(lpn));
+ if (!frnd)
+ return;
+
+ new_list = l_malloc((grp_cnt + frnd->grp_cnt) * sizeof(uint16_t));
+ grp_list = frnd->grp_list;
+
+ if (grp_list && frnd->grp_cnt)
+ memcpy(new_list, grp_list, frnd->grp_cnt * sizeof(uint16_t));
+
+ memcpy(&new_list[frnd->grp_cnt], list, grp_cnt * sizeof(uint16_t));
+ l_free(grp_list);
+ frnd->ele_cnt = ele_cnt;
+ frnd->grp_list = new_list;
+ frnd->grp_cnt += grp_cnt;
+}
+
+void mesh_friend_sub_del(struct mesh_net *net, uint16_t lpn,
+ uint8_t cnt,
+ const uint8_t *del_list)
+{
+ uint16_t *grp_list;
+ int16_t i, grp_cnt;
+ size_t cnt16 = cnt * sizeof(uint16_t);
+ struct mesh_friend *frnd = l_queue_find(net->friends,
+ match_by_friend,
+ L_UINT_TO_PTR(lpn));
+ if (!frnd)
+ return;
+
+ grp_cnt = frnd->grp_cnt;
+ grp_list = frnd->grp_list;
+
+ while (cnt-- && grp_cnt) {
+ cnt16 -= sizeof(uint16_t);
+ for (i = grp_cnt - 1; i >= 0; i--) {
+ if (l_get_le16(del_list + cnt16) == grp_list[i]) {
+ grp_cnt--;
+ memcpy(&grp_list[i], &grp_list[i + 1],
+ (grp_cnt - i) * sizeof(uint16_t));
+ break;
+ }
+ }
+ }
+
+ frnd->grp_cnt = grp_cnt;
+
+ if (!grp_cnt) {
+ l_free(frnd->grp_list);
+ frnd->grp_list = NULL;
+ }
+}
+
+uint32_t mesh_net_next_seq_num(struct mesh_net *net)
+{
+ uint32_t seq = net->seq_num;
+
+ net->seq_num++;
+
+ /* Periodically store advanced sequence number */
+ if (net->seq_num + MIN_SEQ_TRIGGER >= net->cached_seq_num) {
+ net->cached_seq_num = net->seq_num +
+ node_seq_cache(net->local_node);
+ node_set_sequence_number(net->local_node, net->cached_seq_num);
+ }
+
+ return seq;
+}
+
+static struct mesh_sar *mesh_sar_new(size_t len)
+{
+ size_t size = sizeof(struct mesh_sar) + len;
+ struct mesh_sar *sar;
+
+ sar = l_malloc(size);
+
+ memset(sar, 0, size);
+
+ return sar;
+}
+
+static void mesh_sar_free(void *data)
+{
+ struct mesh_sar *sar = data;
+
+ if (!sar)
+ return;
+
+ l_timeout_remove(sar->seg_timeout);
+ l_timeout_remove(sar->msg_timeout);
+ l_free(sar);
+}
+
+static void mesh_msg_free(void *data)
+{
+ struct mesh_msg *msg = data;
+
+ l_free(msg);
+}
+
+static void lpn_process_beacon(void *user_data, const void *data, uint8_t size,
+ int8_t rssi);
+
+static struct mesh_subnet *subnet_new(struct mesh_net *net, uint16_t idx)
+{
+ struct mesh_subnet *subnet;
+
+ subnet = l_new(struct mesh_subnet, 1);
+ if (!subnet)
+ return NULL;
+
+ subnet->net = net;
+ subnet->idx = idx;
+ subnet->tx = &subnet->current;
+ subnet->updated.key_set.nid = NET_NID_INVALID;
+ subnet->snb.beacon[0] = MESH_AD_TYPE_BEACON;
+ return subnet;
+}
+
+static bool create_keys(struct mesh_net *net, struct net_key *keys,
+ const uint8_t *net_key)
+{
+ uint8_t nid[1];
+ uint8_t enc_key[16];
+ uint8_t privacy_key[16];
+ uint8_t network_id[8];
+ uint8_t p[] = {0};
+
+ if (!mesh_crypto_k2(net_key, p, sizeof(p),
+ nid, enc_key, privacy_key))
+ return false;
+
+ if (!mesh_crypto_k3(net_key, network_id))
+ return false;
+
+ if (!mesh_crypto_nkbk(net_key, keys->beacon_key))
+ return false;
+
+ keys->key_set.frnd = false;
+ keys->key_set.nid = nid[0];
+ memcpy(keys->key_set.enc_key, enc_key, 16);
+ memcpy(keys->key_set.privacy_key, privacy_key, 16);
+ memcpy(keys->network_id, network_id, 8);
+ memcpy(keys->key, net_key, 16);
+ return true;
+}
+
+static bool create_secure_beacon(struct mesh_net *net,
+ struct mesh_subnet *subnet,
+ uint8_t *beacon_data, uint8_t size)
+{
+ uint64_t cmac;
+
+ if (size < 22)
+ return false;
+
+ beacon_data[0] = BEACON_TYPE_SNB;
+ beacon_data[1] = 0;
+
+ if (subnet->key_refresh)
+ beacon_data[1] |= 0x01;
+
+ if (iv_is_updating(net))
+ beacon_data[1] |= 0x02;
+
+ memcpy(beacon_data + 2, subnet->tx->network_id, 8);
+ l_put_be32(net->iv_index, beacon_data + 10);
+
+ if (!mesh_crypto_beacon_cmac(subnet->tx->beacon_key,
+ subnet->tx->network_id,
+ net->iv_index, subnet->key_refresh,
+ iv_is_updating(net), &cmac))
+ return false;
+
+ l_put_be64(cmac, beacon_data + 14);
+
+ return true;
+}
+
+static void send_network_beacon(struct mesh_subnet *subnet,
+ struct mesh_net *net)
+{
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = net->tx_interval,
+ .u.gen.cnt = 1,
+ .u.gen.min_delay = DEFAULT_MIN_DELAY,
+ .u.gen.max_delay = DEFAULT_MAX_DELAY
+ };
+
+ l_info("Send SNB on network %3.3x", subnet->idx);
+ mesh_io_send(net->io, &info, subnet->snb.beacon,
+ sizeof(subnet->snb.beacon));
+}
+
+static void network_beacon_timeout(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_subnet *subnet = user_data;
+ uint32_t interval;
+
+ send_network_beacon(subnet, subnet->net);
+
+ if (!subnet->snb.half_period) {
+ l_debug("beacon TO period %d, observed %d, expected %d",
+ subnet->snb.observe_period,
+ subnet->snb.observed,
+ subnet->snb.expected);
+ interval = subnet->snb.observe_period *
+ (subnet->snb.observed + 1) / subnet->snb.expected;
+ subnet->snb.observe_period = interval * 2;
+ subnet->snb.expected = subnet->snb.observe_period / 10;
+ subnet->snb.observed = 0;
+ } else
+ interval = subnet->snb.observe_period / 2;
+
+ if (interval < BEACON_INTERVAL_MIN)
+ interval = BEACON_INTERVAL_MIN;
+
+ if (interval > BEACON_INTERVAL_MAX)
+ interval = BEACON_INTERVAL_MAX;
+
+ subnet->snb.ts = get_timestamp_secs();
+ subnet->snb.half_period ^= 1;
+ l_timeout_modify(timeout, interval);
+}
+
+static void start_network_beacon(void *a, void *b)
+{
+ struct mesh_subnet *subnet = a;
+ struct mesh_net *net = b;
+
+ if (!net->beacon_enable) {
+ if (subnet->snb.timeout)
+ l_timeout_remove(subnet->snb.timeout);
+ subnet->snb.timeout = NULL;
+ return;
+ }
+
+ /* If timeout is active, let it run it's course */
+ if (subnet->snb.timeout)
+ return;
+
+ send_network_beacon(subnet, subnet->net);
+
+ subnet->snb.ts = get_timestamp_secs();
+ subnet->snb.expected = 2;
+ subnet->snb.observed = 0;
+ subnet->snb.half_period = 1;
+ subnet->snb.observe_period = BEACON_INTERVAL_MIN * 2;
+
+ subnet->snb.timeout = l_timeout_create(BEACON_INTERVAL_MIN,
+ network_beacon_timeout, subnet, NULL);
+}
+
+struct mesh_net *mesh_net_new(uint16_t index)
+{
+ struct mesh_net *net;
+
+ net = l_new(struct mesh_net, 1);
+
+ if (!net)
+ return NULL;
+
+ net->pkt_id = 0;
+ net->bea_id = 0;
+ net->key_id_next = 0;
+
+ net->beacon_enable = true;
+ net->proxy_enable = false;
+ net->relay.enable = false;
+
+ net->seq_num = 0x000000;
+ net->src_addr = 0x0000;
+ net->default_ttl = 0x00;
+
+ net->provisioner = false;
+
+ net->test_mode = false;
+ memset(&net->prov_caps, 0, sizeof(net->prov_caps));
+ net->prov_caps.algorithms = 1;
+
+ net->tx_cnt = DEFAULT_TRANSMIT_COUNT;
+ net->tx_interval = DEFAULT_TRANSMIT_INTERVAL;
+
+ net->subnets = l_queue_new();
+ net->key_sets = l_queue_new();
+ net->fast_cache = l_queue_new();
+ net->msg_cache = l_queue_new();
+ net->sar_in = l_queue_new();
+ net->sar_out = l_queue_new();
+ net->frnd_msgs = l_queue_new();
+ net->friends = l_queue_new();
+ net->destinations = l_queue_new();
+ net->app_keys = l_queue_new();
+
+ memset(&net->heartbeat, 0, sizeof(net->heartbeat));
+
+ return mesh_net_ref(net);
+}
+
+struct mesh_net *mesh_net_ref(struct mesh_net *net)
+{
+ if (!net)
+ return NULL;
+
+ __sync_fetch_and_add(&net->ref_count, 1);
+
+ return net;
+}
+
+void mesh_net_unref(struct mesh_net *net)
+{
+ if (!net)
+ return;
+
+ if (__sync_sub_and_fetch(&net->ref_count, 1))
+ return;
+
+ /* key_sets are not allocated to this queue. Only Borrowed */
+ l_queue_destroy(net->key_sets, NULL);
+ net->key_sets = NULL;
+
+ l_queue_destroy(net->subnets, l_free);
+ l_queue_destroy(net->fast_cache, mesh_msg_free);
+ l_queue_destroy(net->msg_cache, mesh_msg_free);
+ l_queue_destroy(net->sar_in, mesh_sar_free);
+ l_queue_destroy(net->sar_out, mesh_sar_free);
+ l_queue_destroy(net->frnd_msgs, l_free);
+ l_queue_destroy(net->friends, mesh_friend_free);
+ l_queue_destroy(net->destinations, l_free);
+ l_queue_destroy(net->app_keys, appkey_key_free);
+
+ l_free(net);
+}
+
+int mesh_net_del_key(struct mesh_net *net, uint16_t idx)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ /* Cannot remove primary key */
+ if (l_queue_length(net->subnets) <= 1)
+ return MESH_STATUS_CANNOT_REMOVE;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+ if (!subnet)
+ return MESH_STATUS_CANNOT_REMOVE;
+
+ /* Delete associated app keys */
+ appkey_delete_bound_keys(net, idx);
+
+ /* Disable hearbeat publication on this subnet */
+ if (idx == net->heartbeat.pub_net_idx)
+ net->heartbeat.pub_dst = UNASSIGNED_ADDRESS;
+
+ mesh_net_remove_keyset(net, &subnet->current.key_set);
+ mesh_net_remove_keyset(net, &subnet->updated.key_set);
+
+ /* TODO: cancel beacon_enable on this subnet */
+
+ l_queue_remove(net->subnets, subnet);
+ if (!storage_local_net_key_del(net, idx))
+ return MESH_STATUS_STORAGE_FAIL;
+
+ return MESH_STATUS_SUCCESS;
+}
+
+int mesh_net_add_key(struct mesh_net *net, bool update, uint16_t idx,
+ const void *value)
+{
+ int status;
+ struct mesh_subnet *subnet;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+
+ if (update) {
+ if (subnet && subnet->kr_phase == KEY_REFRESH_PHASE_NONE) {
+ l_info("Start key refresh");
+ status = mesh_net_kr_phase_one(net, idx, value);
+ if (status == MESH_STATUS_SUCCESS &&
+ !storage_local_net_key_add(net, idx,
+ value, KEY_REFRESH_PHASE_ONE))
+ return MESH_STATUS_STORAGE_FAIL;
+ } else
+ return MESH_STATUS_CANNOT_UPDATE;
+ }
+
+ if (subnet)
+ return memcmp(subnet->current.key, value, 16) ?
+ MESH_STATUS_IDX_ALREADY_STORED : MESH_STATUS_SUCCESS;
+
+ subnet = subnet_new(net, idx);
+ if (!subnet)
+ return MESH_STATUS_INSUFF_RESOURCES;
+
+ if (!create_keys(net, &subnet->current, value) ||
+ !mesh_net_add_keyset(net, &subnet->current.key_set)) {
+ l_free(subnet);
+ return MESH_STATUS_INSUFF_RESOURCES;
+ }
+
+ if (!create_secure_beacon(net, subnet, &subnet->snb.beacon[1], 22) ||
+ !l_queue_push_tail(net->subnets, subnet)) {
+ mesh_net_remove_keyset(net, &subnet->current.key_set);
+ l_free(subnet);
+ return MESH_STATUS_INSUFF_RESOURCES;
+ }
+
+ if (!storage_local_net_key_add(net, idx, value,
+ KEY_REFRESH_PHASE_NONE)) {
+ l_queue_remove(net->subnets, subnet);
+ mesh_net_remove_keyset(net, &subnet->current.key_set);
+ l_free(subnet);
+ return MESH_STATUS_STORAGE_FAIL;
+ }
+
+ start_network_beacon(subnet, net);
+
+ return MESH_STATUS_SUCCESS;
+}
+
+void mesh_net_flush_msg_queues(struct mesh_net *net)
+{
+ l_queue_clear(net->msg_cache, mesh_msg_free);
+ l_queue_clear(net->fast_cache, mesh_msg_free);
+}
+
+static bool match_cache(const void *a, const void *b)
+{
+ const struct mesh_msg *msg = a;
+ const struct mesh_msg *tst = b;
+
+ if (msg->seq != tst->seq || msg->mic != tst->mic ||
+ msg->src != tst->src)
+ return false;
+
+ return true;
+}
+
+static bool msg_in_cache(struct mesh_net *net, uint16_t src, uint32_t seq,
+ uint32_t mic)
+{
+ struct mesh_msg *msg;
+ struct mesh_msg tst = {
+ .src = src,
+ .seq = seq,
+ .mic = mic,
+ };
+
+ msg = l_queue_remove_if(net->msg_cache, match_cache, &tst);
+
+ if (msg) {
+ l_debug("Supressing duplicate %4.4x + %6.6x + %8.8x",
+ src, seq, mic);
+ l_queue_push_head(net->msg_cache, msg);
+ return true;
+ }
+
+ msg = l_new(struct mesh_msg, 1);
+ *msg = tst;
+ l_queue_push_head(net->msg_cache, msg);
+ l_debug("Add %4.4x + %6.6x + %8.8x", src, seq, mic);
+
+ if (l_queue_length(net->msg_cache) > MSG_CACHE_SIZE) {
+ msg = l_queue_peek_tail(net->msg_cache);
+ /* Remove Tail (oldest msg in cache) */
+ l_debug("Remove %4.4x + %6.6x + %8.8x",
+ msg->src, msg->seq, msg->mic);
+ if (l_queue_remove(net->msg_cache, msg))
+ l_free(msg);
+ }
+
+ return false;
+}
+
+static bool match_sar_seq0(const void *a, const void *b)
+{
+ const struct mesh_sar *sar = a;
+ uint16_t seqZero = L_PTR_TO_UINT(b);
+
+ return sar->seqZero == seqZero;
+}
+
+static bool match_sar_remote(const void *a, const void *b)
+{
+ const struct mesh_sar *sar = a;
+ uint16_t remote = L_PTR_TO_UINT(b);
+
+ return sar->remote == remote;
+}
+
+static bool match_msg_timeout(const void *a, const void *b)
+{
+ const struct mesh_sar *sar = a;
+ const struct l_timeout *msg_timeout = b;
+
+ return sar->msg_timeout == msg_timeout;
+}
+
+static bool match_sar_id(const void *a, const void *b)
+{
+ const struct mesh_sar *sar = a;
+ unsigned int id = L_PTR_TO_UINT(b);
+
+ return sar->id == id;
+}
+
+static bool match_seg_timeout(const void *a, const void *b)
+{
+ const struct mesh_sar *sar = a;
+ const struct l_timeout *seg_timeout = b;
+
+ return sar->seg_timeout == seg_timeout;
+}
+
+static bool match_dest_dst(const void *a, const void *b)
+{
+ const struct mesh_destination *dest = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+
+ return dst == dest->dst;
+}
+
+static bool match_frnd_dst(const void *a, const void *b)
+{
+ const struct mesh_friend *frnd = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+ int16_t i, grp_cnt = frnd->grp_cnt;
+ uint16_t *grp_list = frnd->grp_list;
+
+ /*
+ * Determine if this message is for this friends unicast
+ * address, and/or one of it's group/virtual addresses
+ */
+ if (dst >= frnd->dst && dst < (frnd->dst + frnd->ele_cnt))
+ return true;
+
+ if (!(dst & 0x8000))
+ return false;
+
+ for (i = 0; i < grp_cnt; i++) {
+ if (dst == grp_list[i])
+ return true;
+ }
+
+ return false;
+}
+
+static bool is_lpn_friend(struct mesh_net *net, uint16_t addr, bool frnd)
+{
+ void *tst;
+
+ if (!frnd)
+ return false;
+
+ tst = l_queue_find(net->friends, match_frnd_dst, L_UINT_TO_PTR(addr));
+
+ return tst != NULL;
+}
+
+static bool is_us(struct mesh_net *net, uint16_t addr, bool src)
+{
+ void *tst;
+
+ if (IS_ALL_NODES(addr))
+ return true;
+
+ if (addr == FRIENDS_ADDRESS)
+ return net->friend_enable;
+
+ if (addr == RELAYS_ADDRESS)
+ return net->relay.enable;
+
+ if (addr == PROXIES_ADDRESS)
+ return net->proxy_enable;
+
+ if (addr >= net->src_addr && addr <= net->last_addr)
+ return true;
+
+ tst = l_queue_find(net->destinations, match_dest_dst,
+ L_UINT_TO_PTR(addr));
+
+ if (tst == NULL && !src)
+ tst = l_queue_find(net->friends, match_frnd_dst,
+ L_UINT_TO_PTR(addr));
+
+ return tst != NULL;
+}
+
+static struct mesh_friend_msg *mesh_friend_msg_new(uint8_t seg_max)
+{
+ struct mesh_friend_msg *frnd_msg;
+
+ if (seg_max) {
+ size_t size = sizeof(struct mesh_friend_msg) -
+ sizeof(struct mesh_friend_seg_one);
+
+ size += (seg_max + 1) * sizeof(struct mesh_friend_seg_12);
+ frnd_msg = (struct mesh_friend_msg *) l_new(uint8_t, size);
+ } else
+ frnd_msg = l_new(struct mesh_friend_msg, 1);
+
+
+ return frnd_msg;
+}
+
+
+static bool match_ack(const void *a, const void *b)
+{
+ const struct mesh_friend_msg *old = a;
+ const struct mesh_friend_msg *rx = b;
+ uint32_t old_hdr;
+ uint32_t new_hdr;
+
+ /* Determine if old pkt is ACK to same SAR message that new ACK is */
+ if (!old->ctl || old->src != rx->src)
+ return false;
+
+ /* Check the quickest items first before digging deeper */
+ old_hdr = old->u.one[0].hdr & HDR_ACK_MASK;
+ new_hdr = rx->u.one[0].hdr & HDR_ACK_MASK;
+
+ return old_hdr == new_hdr;
+}
+
+static void enqueue_friend_pkt(void *a, void *b)
+{
+ struct mesh_friend *frnd = a;
+ struct mesh_friend_msg *pkt, *rx = b;
+ size_t size;
+ int16_t i;
+
+ if (rx->done)
+ return;
+
+ /*
+ * Determine if this message is for this friends unicast
+ * address, and/or one of it's group/virtual addresses
+ */
+ if (rx->dst >= frnd->dst && (rx->dst - frnd->dst) < frnd->ele_cnt) {
+ rx->done = true;
+ goto enqueue;
+ }
+
+ if (!(rx->dst & 0x8000))
+ return;
+
+ if (!IS_ALL_NODES(rx->dst)) {
+ for (i = 0; i < frnd->grp_cnt; i++) {
+ if (rx->dst == frnd->grp_list[i])
+ goto enqueue;
+ }
+ return;
+ }
+
+enqueue:
+ /* Special handling for Seg Ack -- Only one per message queue */
+ if (((rx->u.one[0].hdr >> OPCODE_HDR_SHIFT) & OPCODE_MASK) ==
+ NET_OP_SEG_ACKNOWLEDGE) {
+ void *old_head = l_queue_peek_head(frnd->pkt_cache);
+ /* Suppress duplicate ACKs */
+ do {
+ void *old = l_queue_remove_if(frnd->pkt_cache,
+ match_ack, rx);
+
+ if (old) {
+ if (old_head == old) {
+ /*
+ * If we are discarding head for any
+ * reason, reset FRND SEQ
+ */
+ frnd->last = frnd->seq;
+ }
+
+ l_free(old);
+ } else
+ break;
+
+ } while (true);
+ }
+
+ l_debug("%s for %4.4x from %4.4x ttl: %2.2x (seq: %6.6x) (ctl: %d)",
+ __func__, frnd->dst, rx->src, rx->ttl,
+ rx->u.one[0].seq, rx->ctl);
+
+ if (rx->cnt_in) {
+ size = sizeof(struct mesh_friend_msg) -
+ sizeof(struct mesh_friend_seg_one);
+ size += (rx->cnt_in + 1) * sizeof(struct mesh_friend_seg_12);
+ } else
+ size = sizeof(struct mesh_friend_msg);
+
+ pkt = l_malloc(size);
+ memcpy(pkt, rx, size);
+
+ l_queue_push_tail(frnd->pkt_cache, pkt);
+
+ if (l_queue_length(frnd->pkt_cache) > FRND_CACHE_MAX) {
+ /*
+ * TODO: Guard against popping UPDATE packets
+ * (disallowed per spec)
+ */
+ pkt = l_queue_pop_head(frnd->pkt_cache);
+ l_free(pkt);
+ frnd->last = frnd->seq;
+ }
+}
+
+static void enqueue_update(void *a, void *b)
+{
+ struct mesh_friend *frnd = a;
+ struct mesh_friend_msg *pkt = b;
+
+ pkt->dst = frnd->dst;
+ pkt->done = false;
+ enqueue_friend_pkt(frnd, pkt);
+}
+
+static uint32_t seq_auth(uint32_t seq, uint16_t seqZero)
+{
+ uint32_t seqAuth = seqZero & SEQ_ZERO_MASK;
+
+ seqAuth |= seq & (~SEQ_ZERO_MASK);
+ if (seqAuth > seq)
+ seqAuth -= (SEQ_ZERO_MASK + 1);
+
+ return seqAuth;
+}
+
+static bool friend_packet_queue(struct mesh_net *net,
+ uint32_t iv_index,
+ bool ctl, uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint32_t hdr,
+ const uint8_t *data, uint16_t size)
+{
+ struct mesh_friend_msg *frnd_msg;
+ uint8_t seg_max = SEG_TOTAL(hdr);
+ bool ret;
+
+ if (seg_max && !IS_SEGMENTED(hdr))
+ return false;
+
+ frnd_msg = mesh_friend_msg_new(seg_max);
+
+ if (IS_SEGMENTED(hdr)) {
+ uint32_t seqAuth = seq_auth(seq, hdr >> SEQ_ZERO_HDR_SHIFT);
+ uint8_t i;
+
+ for (i = 0; i <= seg_max; i++) {
+ memcpy(frnd_msg->u.s12[i].data, data, 12);
+ frnd_msg->u.s12[i].hdr = hdr;
+ frnd_msg->u.s12[i].seq = seqAuth + i;
+ data += 12;
+ hdr += (1 << SEGO_HDR_SHIFT);
+ }
+ frnd_msg->u.s12[seg_max].seq = seq;
+ frnd_msg->cnt_in = seg_max;
+ frnd_msg->last_len = size % 12;
+ if (!frnd_msg->last_len)
+ frnd_msg->last_len = 12;
+ } else {
+ uint8_t opcode = hdr >> OPCODE_HDR_SHIFT;
+
+ if (ctl && opcode != NET_OP_SEG_ACKNOWLEDGE) {
+
+ /* Don't cache Friend Ctl opcodes */
+ if (FRND_OPCODE(opcode)) {
+ l_free(frnd_msg);
+ return false;
+ }
+
+ memcpy(frnd_msg->u.one[0].data + 1, data, size);
+ frnd_msg->last_len = size + 1;
+ frnd_msg->u.one[0].data[0] = opcode;
+ } else {
+ memcpy(frnd_msg->u.one[0].data, data, size);
+ frnd_msg->last_len = size;
+ }
+ frnd_msg->u.one[0].hdr = hdr;
+ frnd_msg->u.one[0].seq = seq;
+ }
+
+ frnd_msg->iv_index = iv_index;
+ frnd_msg->src = src;
+ frnd_msg->dst = dst;
+ frnd_msg->ctl = ctl;
+ frnd_msg->ttl = ttl;
+
+ /* Re-Package into Friend Delivery payload */
+ l_queue_foreach(net->friends, enqueue_friend_pkt, frnd_msg);
+ ret = frnd_msg->done;
+
+ /* TODO Optimization(?): Unicast messages keep this buffer */
+ l_free(frnd_msg);
+
+ return ret;
+}
+
+static void friend_ack_rxed(struct mesh_net *net, uint32_t iv_index,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ const uint8_t *pkt)
+{
+ uint32_t hdr = l_get_be32(pkt) &
+ ((SEQ_ZERO_MASK << SEQ_ZERO_HDR_SHIFT) | /* Preserve SeqZero */
+ (true << RELAY_HDR_SHIFT)); /* Preserve Relay bit */
+ uint32_t flags = l_get_be32(pkt + 3);
+ struct mesh_friend_msg frnd_ack = {
+ .ctl = true,
+ .iv_index = iv_index,
+ .src = src,
+ .dst = dst,
+ .last_len = sizeof(flags),
+ .u.one[0].seq = seq,
+ .done = false,
+ };
+
+ hdr |= NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
+ frnd_ack.u.one[0].hdr = hdr;
+ l_put_be32(flags, frnd_ack.u.one[0].data);
+ l_queue_foreach(net->friends, enqueue_friend_pkt, &frnd_ack);
+}
+
+static bool send_seg(struct mesh_net *net, struct mesh_sar *msg, uint8_t seg);
+
+static void send_frnd_ack(struct mesh_net *net, uint16_t src, uint16_t dst,
+ uint32_t hdr, uint32_t flags)
+{
+ uint32_t expected;
+ uint8_t msg[7];
+
+ /* We don't ACK from multicast destinations */
+ if (src & 0x8000)
+ return;
+
+ /* Calculate the "Full ACK" mask */
+ expected = 0xffffffff >> (31 - SEG_TOTAL(hdr));
+
+ /* Clear Hdr bits that don't apply to Seg ACK */
+ hdr &= ~((true << SEG_HDR_SHIFT) |
+ (OPCODE_MASK << OPCODE_HDR_SHIFT) |
+ (true << SZMIC_HDR_SHIFT) |
+ (SEG_MASK << SEGO_HDR_SHIFT) |
+ (SEG_MASK << SEGN_HDR_SHIFT));
+
+ hdr |= NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
+ hdr |= true << RELAY_HDR_SHIFT;
+
+ /* Clear all unexpected bits */
+ flags &= expected;
+
+ l_put_be32(hdr, msg);
+ l_put_be32(flags, msg + 3);
+
+ l_info("Send Friend ACK to Segs: %8.8x", flags);
+
+ if (is_lpn_friend(net, dst, true)) {
+ /* If we are acking our LPN Friend, queue, don't send */
+ friend_ack_rxed(net, mesh_net_get_iv_index(net),
+ mesh_net_next_seq_num(net), 0, dst, msg);
+ } else {
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), DEFAULT_TTL,
+ 0, 0, dst, msg, sizeof(msg));
+ }
+}
+
+static void send_net_ack(struct mesh_net *net, struct mesh_sar *sar,
+ uint32_t flags)
+{
+ uint8_t msg[7];
+ uint32_t hdr;
+ uint16_t src = sar->src;
+ uint16_t dst = sar->remote;
+
+ /* We don't ACK from multicast destinations */
+ if (src & 0x8000)
+ return;
+
+ /* We don't ACK segments as a Low Power Node */
+ if (net->friend_addr)
+ return;
+
+ hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
+ hdr |= sar->seqZero << SEQ_ZERO_HDR_SHIFT;
+
+ if (is_lpn_friend(net, src, true))
+ hdr |= true << RELAY_HDR_SHIFT;
+
+ l_put_be32(hdr, msg);
+ l_put_be32(flags, msg + 3);
+ l_info("Send%s ACK to Segs: %8.8x", sar->frnd ? " Friend" : "", flags);
+
+ if (is_lpn_friend(net, dst, true)) {
+ /* If we are acking our LPN Friend, queue, don't send */
+ friend_ack_rxed(net, mesh_net_get_iv_index(net),
+ mesh_net_next_seq_num(net), src, dst, msg);
+ return;
+ }
+
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), DEFAULT_TTL,
+ 0, src, dst, msg, sizeof(msg));
+}
+
+static void inseg_to(struct l_timeout *seg_timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_sar *sar = l_queue_find(net->sar_in,
+ match_seg_timeout, seg_timeout);
+
+ l_timeout_remove(seg_timeout);
+ if (!sar)
+ return;
+
+ /* Send NAK */
+ l_info("Timeout %p %3.3x", sar, sar->app_idx);
+ send_net_ack(net, sar, sar->flags);
+
+ sar->seg_timeout = l_timeout_create(SEG_TO, inseg_to, net, NULL);
+}
+
+static void inmsg_to(struct l_timeout *msg_timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_sar *sar = l_queue_remove_if(net->sar_in,
+ match_msg_timeout, msg_timeout);
+
+ l_timeout_remove(msg_timeout);
+ if (!sar)
+ return;
+
+ sar->msg_timeout = NULL;
+
+ /* print_packet("Incoming SAR Timeout", sar->buf, sar->len); */
+ mesh_sar_free(sar);
+}
+
+static void outmsg_to(struct l_timeout *msg_timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_sar *sar = l_queue_remove_if(net->sar_out,
+ match_msg_timeout, msg_timeout);
+
+ l_timeout_remove(msg_timeout);
+ if (!sar)
+ return;
+
+ sar->msg_timeout = NULL;
+
+ if (sar->status_func)
+ sar->status_func(sar->remote, 1,
+ sar->buf, sar->len - 4,
+ sar->user_data);
+
+ /* print_packet("Outgoing SAR Timeout", sar->buf, sar->len); */
+ mesh_sar_free(sar);
+}
+
+static void outseg_to(struct l_timeout *seg_timeout, void *user_data);
+static void ack_received(struct mesh_net *net, bool timeout,
+ uint16_t src, uint16_t dst,
+ uint16_t seq0, uint32_t ack_flag)
+{
+ struct mesh_sar *outgoing;
+ uint32_t seg_flag = 0x00000001;
+ uint32_t ack_copy = ack_flag;
+ uint16_t i;
+
+ l_info("ACK Rxed (%x) (to:%d): %8.8x", seq0, timeout, ack_flag);
+
+ outgoing = l_queue_find(net->sar_out, match_sar_seq0,
+ L_UINT_TO_PTR(seq0));
+
+ if (!outgoing) {
+ l_info("Not Found: %4.4x", seq0);
+ return;
+ }
+
+ /*
+ * TODO -- If we receive from different
+ * SRC than we are sending to, make sure the OBO flag is set
+ */
+
+ if ((!timeout && !ack_flag) ||
+ (outgoing->flags & ack_flag) == outgoing->flags) {
+ l_debug("ob_sar_removal (%x)", outgoing->flags);
+
+ /* Note: ack_flags == 0x00000000 is a remote Cancel request */
+ if (outgoing->status_func)
+ outgoing->status_func(src, ack_flag ? 0 : 1,
+ outgoing->buf,
+ outgoing->len - 4, outgoing->user_data);
+
+ l_queue_remove(net->sar_out, outgoing);
+ mesh_sar_free(outgoing);
+
+ return;
+ }
+
+ outgoing->last_nak |= ack_flag;
+
+ ack_copy &= outgoing->flags;
+
+ for (i = 0; i <= SEG_MAX(outgoing->len); i++, seg_flag <<= 1) {
+ if (seg_flag & ack_flag) {
+ l_debug("Skipping Seg %d of %d",
+ i, SEG_MAX(outgoing->len));
+ continue;
+ }
+
+ ack_copy |= seg_flag;
+
+ l_info("Resend Seg %d net:%p dst:%x app_idx:%3.3x",
+ i, net, outgoing->remote, outgoing->app_idx);
+
+ send_seg(net, outgoing, i);
+ }
+
+ l_timeout_remove(outgoing->seg_timeout);
+ outgoing->seg_timeout = l_timeout_create(SEG_TO, outseg_to, net, NULL);
+}
+
+static void outack_to(struct l_timeout *seg_timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_sar *sar = l_queue_find(net->sar_out,
+ match_seg_timeout, seg_timeout);
+
+ l_timeout_remove(seg_timeout);
+ if (!sar)
+ return;
+
+ sar->seg_timeout = NULL;
+
+ /* Re-Send missing segments by faking NAK */
+ ack_received(net, true, sar->remote, sar->src,
+ sar->seqZero, sar->last_nak);
+}
+
+static void outseg_to(struct l_timeout *seg_timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_sar *sar = l_queue_find(net->sar_out,
+ match_seg_timeout, seg_timeout);
+
+ l_timeout_remove(seg_timeout);
+ if (!sar)
+ return;
+
+ sar->seg_timeout = NULL;
+
+ if (net->friend_addr) {
+ /* We are LPN -- Poll for ACK */
+ frnd_ack_poll(net);
+ sar->seg_timeout = l_timeout_create(SEG_TO,
+ outack_to, net, NULL);
+ } else {
+ /* Re-Send missing segments by faking NACK */
+ ack_received(net, true, sar->remote, sar->src,
+ sar->seqZero, sar->last_nak);
+ }
+}
+
+static bool msg_rxed(struct mesh_net *net, bool frnd,
+ uint32_t iv_index,
+ uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint8_t key_id,
+ bool szmic, uint16_t seqZero,
+ const uint8_t *data, uint16_t size)
+{
+ uint32_t seqAuth = seq_auth(seq, seqZero);
+
+ /* Sanity check seqAuth */
+ if (seqAuth > seq)
+ return false;
+
+ /* Save un-decrypted messages for our friends */
+ if (!frnd && l_queue_length(net->friends)) {
+ uint32_t hdr = key_id << KEY_HDR_SHIFT;
+ uint8_t frnd_ttl = ttl;
+
+ /* If not from us, decrement for our hop */
+ if (src < net->src_addr || src > net->last_addr) {
+ if (frnd_ttl > 1)
+ frnd_ttl--;
+ else
+ goto not_for_friend;
+ }
+
+ if (szmic || size > 15) {
+ hdr |= true << SEG_HDR_SHIFT;
+ hdr |= szmic << SZMIC_HDR_SHIFT;
+ hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT;
+ hdr |= SEG_MAX(size) << SEGN_HDR_SHIFT;
+ }
+
+ if (friend_packet_queue(net, iv_index, false, frnd_ttl,
+ seq, src, dst,
+ hdr, data, size))
+ return true;
+ }
+
+not_for_friend:
+ return mesh_model_rx(net, szmic, seqAuth, seq, iv_index,
+ ttl, src, dst, key_id, data, size);
+}
+
+static bool match_frnd_sar_dst(const void *a, const void *b)
+{
+ const struct mesh_friend_msg *frnd_msg = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+
+ return frnd_msg->dst == dst;
+}
+
+static void friend_seg_rxed(struct mesh_net *net,
+ uint32_t iv_index,
+ uint8_t ttl, uint32_t seq,
+ uint16_t src, uint16_t dst, uint32_t hdr,
+ const uint8_t *data, uint8_t size)
+{
+ struct mesh_friend *frnd = NULL;
+ struct mesh_friend_msg *frnd_msg = NULL;
+ uint8_t cnt;
+ uint8_t segN = hdr & 0x1f;
+ uint8_t segO = ((hdr >> 5) & 0x1f);
+ uint32_t expected = 0xffffffff >> (31 - segN);
+ uint32_t this_seg_flag = 0x00000001 << segO;
+ uint32_t largest = (0xffffffff << segO) & expected;
+ uint32_t hdr_key = hdr & HDR_KEY_MASK;
+
+ frnd = l_queue_find(net->friends, match_frnd_dst,
+ L_UINT_TO_PTR(dst));
+ if (!frnd)
+ return;
+
+ if (frnd->last_hdr == hdr_key) {
+ /* We are no longer receiving this msg. Resend final ACK */
+ send_frnd_ack(net, dst, src, frnd->last_hdr, 0xffffffff);
+ return;
+ }
+
+ /* Check if we have a SAR-in-progress that matches incoming segment */
+ frnd_msg = l_queue_find(net->frnd_msgs, match_frnd_sar_dst,
+ L_UINT_TO_PTR(dst));
+
+ if (frnd_msg) {
+ /* Flush if SZMICN or IV Index has changed */
+ if (frnd_msg->iv_index != iv_index)
+ frnd_msg->u.s12[0].hdr = 0;
+
+ /* Flush incomplete old SAR message if it doesn't match */
+ if ((frnd_msg->u.s12[0].hdr & HDR_KEY_MASK) != hdr_key) {
+ l_queue_remove(net->frnd_msgs, frnd_msg);
+ l_free(frnd_msg);
+ frnd_msg = NULL;
+ }
+ }
+
+ if (!frnd_msg) {
+ frnd_msg = mesh_friend_msg_new(segN);
+ frnd_msg->iv_index = iv_index;
+ frnd_msg->src = src;
+ frnd_msg->dst = dst;
+ frnd_msg->ttl = ttl;
+ l_queue_push_tail(net->frnd_msgs, frnd_msg);
+ } else if (frnd_msg->flags & this_seg_flag) /* Ignore dup segs */
+ return;
+
+ cnt = frnd_msg->cnt_in;
+ frnd_msg->flags |= this_seg_flag;
+
+ frnd_msg->u.s12[cnt].hdr = hdr;
+ frnd_msg->u.s12[cnt].seq = seq;
+ memcpy(frnd_msg->u.s12[cnt].data, data, size);
+
+ /* Last segment could be short */
+ if (segN == segO)
+ frnd_msg->last_len = size;
+
+ l_info("RXed Seg %d, Flags %8.8x (cnt: %d)",
+ segO, frnd_msg->flags, cnt);
+
+ /* In reality, if one of these is true, then *both* must be true */
+ if ((cnt == segN) || (frnd_msg->flags == expected)) {
+ l_info("Full ACK");
+ send_frnd_ack(net, dst, src, hdr, frnd_msg->flags);
+
+ if (frnd_msg->ttl > 1) {
+ frnd_msg->ttl--;
+ /* Add to friends cache */
+ l_queue_foreach(net->friends,
+ enqueue_friend_pkt, frnd_msg);
+ }
+
+ /* Remove from "in progress" queue */
+ l_queue_remove(net->frnd_msgs, frnd_msg);
+
+ /* TODO Optimization(?): Unicast messages keep this buffer */
+ l_free(frnd_msg);
+ return;
+ }
+
+ /* Always ACK if this is the largest outstanding segment */
+ if ((largest & frnd_msg->flags) == largest) {
+ l_info("Partial ACK");
+ send_frnd_ack(net, dst, src, hdr, frnd_msg->flags);
+ }
+
+ frnd_msg->cnt_in++;
+}
+
+static bool seg_rxed(struct mesh_net *net, bool frnd,
+ uint32_t iv_index,
+ uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint8_t key_id,
+ bool szmic, uint16_t seqZero,
+ uint8_t segO, uint8_t segN,
+ const uint8_t *data, uint8_t size)
+{
+ struct mesh_sar *sar_in = NULL;
+ uint16_t seg_off = 0;
+ uint32_t expected, this_seg_flag, largest, seqAuth;
+ bool reset_seg_to = true;
+
+ /*
+ * DST could receive additional Segments after
+ * completing due to a lost ACK, so re-ACK and discard
+ */
+ sar_in = l_queue_find(net->sar_in, match_sar_remote,
+ L_UINT_TO_PTR(src));
+
+ /* Discard *old* incoming-SAR-in-progress if this segment newer */
+ seqAuth = seq_auth(seq, seqZero);
+ if (sar_in && (sar_in->seqAuth != seqAuth ||
+ sar_in->iv_index != iv_index)) {
+ bool newer;
+
+ if (iv_index > sar_in->iv_index)
+ newer = true;
+ else if (iv_index == sar_in->iv_index)
+ newer = seqAuth > sar_in->seqAuth;
+ else
+ newer = false;
+
+ if (newer) {
+ /* Cancel Old, start New */
+ l_queue_remove(net->sar_in, sar_in);
+ mesh_sar_free(sar_in);
+ sar_in = NULL;
+ } else
+ /* Ignore Old */
+ return false;
+ }
+
+ expected = 0xffffffff >> (31 - segN);
+
+ if (sar_in) {
+ l_info("RXed (old: %04x %06x size:%d) %d of %d",
+ seqZero, seq, size, segO, segN);
+ /* Sanity Check--> certain things must match */
+ if (SEG_MAX(sar_in->len) != segN ||
+ sar_in->key_id != key_id)
+ return false;
+
+ if (sar_in->flags == expected) {
+ /* Re-Send ACK for full msg */
+ if (!net->friend_addr)
+ send_net_ack(net, sar_in, expected);
+ return true;
+ }
+ } else {
+ uint16_t len = MAX_SEG_TO_LEN(segN);
+
+ l_info("RXed (new: %04x %06x size: %d len: %d) %d of %d",
+ seqZero, seq, size, len, segO, segN);
+ l_debug("Queue Size: %d", l_queue_length(net->sar_in));
+ sar_in = mesh_sar_new(len);
+ sar_in->seqAuth = seqAuth;
+ sar_in->iv_index = iv_index;
+ sar_in->src = dst;
+ sar_in->remote = src;
+ sar_in->seqZero = seqZero;
+ sar_in->key_id = key_id;
+ sar_in->len = len;
+ sar_in->last_seg = 0xff;
+ if (!net->friend_addr)
+ sar_in->msg_timeout = l_timeout_create(MSG_TO,
+ inmsg_to, net, NULL);
+
+ l_debug("First Seg %4.4x", sar_in->flags);
+ l_queue_push_head(net->sar_in, sar_in);
+ }
+ /* print_packet("Seg", data, size); */
+
+ seg_off = segO * MAX_SEG_LEN;
+ memcpy(sar_in->buf + seg_off, data, size);
+ this_seg_flag = 0x00000001 << segO;
+
+ /* Don't reset Seg TO or NAK if we already have this seg */
+ if (this_seg_flag & sar_in->flags)
+ reset_seg_to = false;
+
+ sar_in->flags |= this_seg_flag;
+ sar_in->ttl = ttl;
+
+ l_debug("Have Frags %4.4x", sar_in->flags);
+
+ /* Msg length only definitive on last segment */
+ if (segO == segN)
+ sar_in->len = segN * MAX_SEG_LEN + size;
+
+ if (sar_in->flags == expected) {
+ /* Got it all */
+ if (!net->friend_addr)
+ send_net_ack(net, sar_in, expected);
+
+ msg_rxed(net, frnd,
+ iv_index,
+ ttl,
+ seq,
+ sar_in->remote, dst,
+ key_id,
+ szmic, sar_in->seqZero,
+ sar_in->buf, sar_in->len);
+
+ /* Kill Inter-Seg timeout */
+ l_timeout_remove(sar_in->seg_timeout);
+ sar_in->seg_timeout = NULL;
+ return true;
+
+ } else if (reset_seg_to) {
+ /* Restart Inter-Seg Timeout */
+ l_timeout_remove(sar_in->seg_timeout);
+
+ /* if this is the largest outstanding segment, send NAK now */
+ if (!net->friend_addr) {
+ largest = (0xffffffff << segO) & expected;
+ if ((largest & sar_in->flags) == largest)
+ send_net_ack(net, sar_in, sar_in->flags);
+
+ sar_in->seg_timeout = l_timeout_create(SEG_TO,
+ inseg_to, net, NULL);
+ }
+ }
+
+ l_debug("NAK: %d expected:%08x largest:%08x flags:%08x",
+ reset_seg_to, expected, largest, sar_in->flags);
+ return false;
+}
+
+static bool ctl_received(struct mesh_net *net, bool frnd, uint32_t iv_index,
+ uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint8_t opcode, int8_t rssi,
+ const uint8_t *pkt, uint8_t len)
+{
+ uint8_t msg[12];
+ uint8_t rsp_ttl = DEFAULT_TTL;
+ uint8_t n = 0;
+
+ if (!frnd && ttl > 1) {
+ uint32_t hdr = opcode << OPCODE_HDR_SHIFT;
+ uint8_t frnd_ttl = ttl - 1;
+
+ if (friend_packet_queue(net, iv_index,
+ true, frnd_ttl,
+ seq,
+ src, dst,
+ hdr,
+ pkt, len))
+ return true;
+ }
+
+ /* Don't process other peoples Unicast destinations */
+ if (dst < 0x8000 && (dst < net->src_addr || dst > net->last_addr))
+ return false;
+
+ switch (opcode) {
+ default:
+ l_error("Unsupported Ctl Opcode: %2.2x", opcode);
+ break;
+
+ case NET_OP_FRND_POLL:
+ if (len != 1 || ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_FRND_POLL", pkt, len);
+ friend_poll(net, src, !!(pkt[0]),
+ l_queue_find(net->friends,
+ match_by_friend,
+ L_UINT_TO_PTR(src)));
+ break;
+
+ case NET_OP_FRND_UPDATE:
+ if (ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_FRND_UPDATE", pkt, len);
+ lpn_process_beacon(net, pkt, len, 0);
+ break;
+
+ case NET_OP_FRND_REQUEST:
+ if (!net->friend_enable)
+ return false;
+
+ if (!IS_ALL_NODES(dst) && dst != FRIENDS_ADDRESS)
+ return false;
+
+ if (len != 10 || ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_FRND_REQUEST", pkt, len);
+ friend_request(net, src, pkt[0], pkt[1],
+ l_get_be32(pkt + 1) & 0xffffff,
+ l_get_be16(pkt + 5), pkt[7],
+ l_get_be16(pkt + 8), rssi);
+ break;
+
+ case NET_OP_FRND_OFFER:
+ if (len != 6 || ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_FRND_OFFER", pkt, len);
+ frnd_offer(net, src, pkt[0], pkt[1], pkt[2],
+ (int8_t) pkt[3], rssi, l_get_be16(pkt + 4));
+ break;
+
+ case NET_OP_FRND_CLEAR_CONFIRM:
+ if (len != 4)
+ return false;
+
+ print_packet("Rx-NET_OP_FRND_CLEAR_CONFIRM", pkt, len);
+ friend_clear_confirm(net, src, l_get_be16(pkt),
+ l_get_be16(pkt + 2));
+ break;
+
+ case NET_OP_FRND_CLEAR:
+ if (len != 4 || dst != net->src_addr)
+ return false;
+
+ print_packet("Rx-NET_OP_FRND_CLEAR", pkt, len);
+ friend_clear(net, src, l_get_be16(pkt), l_get_be16(pkt + 2),
+ l_queue_find(net->friends,
+ match_by_friend,
+ L_UINT_TO_PTR(l_get_be16(pkt))));
+ l_info("Remaining Friends: %d", l_queue_length(net->friends));
+ break;
+
+ case NET_OP_PROXY_SUB_ADD:
+ if (ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_PROXY_SUB_ADD", pkt, len);
+ friend_sub_add(net, l_queue_find(net->friends,
+ match_by_friend, L_UINT_TO_PTR(src)),
+ pkt, len);
+ break;
+
+ case NET_OP_PROXY_SUB_REMOVE:
+ if (ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_PROXY_SUB_REMOVE", pkt, len);
+ friend_sub_del(net, l_queue_find(net->friends,
+ match_by_friend, L_UINT_TO_PTR(src)),
+ pkt, len);
+ break;
+
+ case NET_OP_PROXY_SUB_CONFIRM:
+ if (ttl)
+ return false;
+
+ print_packet("Rx-NET_OP_PROXY_SUB_CONFIRM", pkt, len);
+ break;
+
+ case NET_OP_HEARTBEAT:
+ if (net->heartbeat.sub_enabled &&
+ src == net->heartbeat.sub_src) {
+ uint8_t hops = pkt[0] - ttl + 1;
+
+ print_packet("Rx-NET_OP_HEARTBEAT", pkt, len);
+
+ if (net->heartbeat.sub_count != 0xffff)
+ net->heartbeat.sub_count++;
+
+ if (net->heartbeat.sub_min_hops > hops)
+ net->heartbeat.sub_min_hops = hops;
+
+ if (net->heartbeat.sub_max_hops < hops)
+ net->heartbeat.sub_max_hops = hops;
+
+ l_info("HB: cnt:%4.4x min:%2.2x max:%2.2x",
+ net->heartbeat.sub_count,
+ net->heartbeat.sub_min_hops,
+ net->heartbeat.sub_max_hops);
+ }
+ break;
+ }
+
+ if (n) {
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), rsp_ttl,
+ 0, dst & 0x8000 ? 0 : dst, src,
+ msg, n);
+ }
+
+ return true;
+}
+
+static bool find_fast_hash(const void *a, const void *b)
+{
+ const uint64_t *entry = a;
+ const uint64_t *test = b;
+
+ return *entry == *test;
+}
+
+static void *check_fast_cache(struct mesh_net *net, uint64_t hash)
+{
+ void *found = l_queue_find(net->fast_cache, find_fast_hash, &hash);
+ uint64_t *new_hash;
+
+ if (found)
+ return NULL;
+
+ if (l_queue_length(net->fast_cache) >= 8)
+ new_hash = l_queue_pop_head(net->fast_cache);
+ else
+ new_hash = l_malloc(sizeof(hash));
+
+ *new_hash = hash;
+ l_queue_push_tail(net->fast_cache, new_hash);
+
+ return new_hash;
+}
+
+static bool match_keyset(const void *a, const void *b)
+{
+ const struct mesh_friend *frnd = a;
+ const struct mesh_key_set *key_set = b;
+
+ return (key_set == &frnd->key_set) || (key_set == &frnd->new_key_set);
+}
+
+static void try_decode(void *a, void *b)
+{
+ struct mesh_key_set *key_set = a;
+ struct net_decode *decode = b;
+ uint8_t tmp[29];
+ bool status;
+
+ if (decode->key_set || key_set->nid != decode->nid)
+ return;
+
+ status = mesh_crypto_packet_decode(decode->packet, decode->size,
+ decode->proxy, tmp, decode->iv_index,
+ key_set->enc_key, key_set->privacy_key);
+
+ if (!status)
+ return;
+
+ memcpy(decode->packet, tmp, decode->size);
+ decode->key_set = key_set;
+ if (key_set->frnd)
+ decode->frnd = l_queue_find(decode->net->friends,
+ match_keyset, key_set);
+ else
+ decode->frnd = NULL;
+}
+
+static struct mesh_key_set *net_packet_decode(struct mesh_net *net,
+ uint32_t iv_index, uint8_t nid,
+ struct mesh_friend **frnd,
+ bool proxy,
+ uint8_t *packet, uint8_t size)
+{
+ struct net_decode decode = {
+ .net = net,
+ .key_set = NULL,
+ .nid = nid,
+ .iv_index = iv_index,
+ .packet = packet,
+ .size = size,
+ .proxy = proxy,
+ };
+
+ l_queue_foreach(net->key_sets, try_decode, &decode);
+
+ if (decode.key_set != NULL) {
+ *frnd = decode.frnd;
+ return decode.key_set;
+ }
+ return NULL;
+}
+
+static bool match_key_nid(const void *a, const void *b)
+{
+ const struct mesh_key_set *key_set = a;
+ uint8_t nid = L_PTR_TO_UINT(b);
+
+ return key_set->nid == nid;
+}
+
+static bool match_by_dst(const void *a, const void *b)
+{
+ const struct mesh_destination *dest = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+
+ return dest->dst == dst;
+}
+
+static void send_relay_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size)
+{
+ struct mesh_io *io = net->io;
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = net->relay.interval,
+ .u.gen.cnt = net->relay.count,
+ .u.gen.min_delay = DEFAULT_MIN_DELAY,
+ .u.gen.max_delay = DEFAULT_MAX_DELAY
+ };
+
+ packet[0] = MESH_AD_TYPE_NETWORK;
+
+ mesh_io_send(io, &info, packet, size);
+}
+
+static void send_msg_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size)
+{
+ struct mesh_io *io = net->io;
+ struct mesh_io_send_info info = {
+ .type = MESH_IO_TIMING_TYPE_GENERAL,
+ .u.gen.interval = net->tx_interval,
+ .u.gen.cnt = net->tx_cnt,
+ .u.gen.min_delay = DEFAULT_MIN_DELAY,
+ /* No extra randomization when sending regular mesh messages */
+ .u.gen.max_delay = DEFAULT_MIN_DELAY
+ };
+
+ packet[0] = MESH_AD_TYPE_NETWORK;
+
+ mesh_io_send(io, &info, packet, size);
+}
+
+static void packet_received(void *user_data, const void *data, uint8_t size,
+ int8_t rssi)
+{
+ struct mesh_net *net = user_data;
+ uint32_t iv_index;
+ uint8_t iv_flag;
+ uint8_t nid;
+ const uint8_t *msg = data;
+ uint8_t app_msg_len;
+ uint8_t net_ttl, net_key_id, net_segO, net_segN, net_opcode;
+ uint32_t net_seq, cache_cookie;
+ uint16_t net_src, net_dst, net_seqZero;
+ uint8_t packet[31];
+ bool net_ctl, net_segmented, net_szmic, net_relay;
+ struct mesh_friend *net_frnd;
+ bool drop = false;
+ uint64_t hash, *isNew = NULL;
+ struct mesh_key_set *keys;
+
+ nid = msg[0] & 0x7f;
+
+ /* Ignore unrecognized NIDs */
+ if (!(l_queue_find(net->key_sets, match_key_nid, L_UINT_TO_PTR(nid)))) {
+ /* print_packet("Nope", data, size); */
+ return;
+ }
+
+ iv_flag = msg[0] >> 7;
+ iv_index = net->iv_index;
+ l_debug("%s iv_index %d NID: %2.2x", __func__, iv_index, nid);
+
+ if (sizeof(uint16_t) <= sizeof(void *)) {
+ /* Add in additional cache to allow us to
+ * avoid decrypting duplicatesr
+ * Fast 64 bit Hash, Network MIC doesn't matter
+ * With 64 bit hash, false pos chance is 1 in 1.8 * 10^19
+ */
+ hash = l_get_le64(msg + 1) ^ l_get_le64(msg + 9);
+ isNew = check_fast_cache(net, hash);
+ if (!isNew)
+ return;
+
+ l_debug("New");
+ }
+
+ memcpy(packet + 2, data, size);
+
+ if (iv_index && (iv_index & 0x01) != iv_flag)
+ iv_index--;
+
+ /* Tester--Drop 90% of packets */
+ /* l_getrandom(&iv_flag, 1); */
+ /* if (iv_flag%10<9) drop = true; */
+
+ if (!drop)
+ print_packet("RX: Network [enc] :", data, size);
+
+ keys = net_packet_decode(net, iv_index, nid, &net_frnd, false,
+ packet + 2, size);
+ if (keys == NULL) {
+ l_debug("Failed to decode packet");
+ /* Remove fast-cache-hash */
+ l_queue_remove(net->fast_cache, isNew);
+ l_free(isNew);
+ return;
+ }
+
+ if (!drop)
+ print_packet("RX: Network [clr] :", packet + 2, size);
+
+ if (!mesh_crypto_packet_parse(packet + 2, size,
+ &net_ctl, &net_ttl,
+ &net_seq,
+ &net_src, &net_dst,
+ &cache_cookie,
+ &net_opcode,
+ &net_segmented,
+ &net_key_id,
+ &net_szmic, &net_relay, &net_seqZero,
+ &net_segO, &net_segN,
+ &msg, &app_msg_len)) {
+ l_error("Failed to parse packet content");
+ return;
+ }
+
+ /* Ignore incoming packets if we are LPN and frnd bit not set */
+ if (net->friend_addr) {
+ struct mesh_subnet *subnet;
+
+ subnet = l_queue_find(net->subnets, match_key_set, keys);
+ if (subnet)
+ return;
+
+ /* If the queue is empty, stop polling */
+ if (net_ctl && net_opcode == NET_OP_FRND_UPDATE && !msg[5])
+ frnd_poll_cancel(net);
+ else
+ frnd_poll(net, false);
+
+ } else if (net_dst == 0) {
+ l_error("illegal parms: DST: %4.4x Ctl: %d TTL: %2.2x",
+ net_dst, net_ctl, net_ttl);
+ return;
+ }
+
+ /* Ignore if we originally sent this */
+ if (is_us(net, net_src, true))
+ return;
+
+ if (drop) {
+ l_info("Dropping SEQ 0x%06x", net_seq);
+ return;
+ }
+
+ l_debug("check %08x", cache_cookie);
+
+ /* As a Relay, suppress repeats of last N packets that pass through */
+ /* The "cache_cookie" should be unique part of App message */
+ if (msg_in_cache(net, net_src, net_seq, cache_cookie))
+ return;
+
+ l_debug("RX: Network %04x -> %04x : TTL 0x%02x : IV : %8.8x SEQ 0x%06x",
+ net_src, net_dst, net_ttl, iv_index, net_seq);
+
+ if (is_us(net, net_dst, false) ||
+ is_lpn_friend(net, net_src, !!(net_frnd)) ||
+ (net_ctl && net_opcode == NET_OP_HEARTBEAT)) {
+
+ l_info("RX: App 0x%04x -> 0x%04x : TTL 0x%02x : SEQ 0x%06x",
+ net_src, net_dst, net_ttl, net_seq);
+
+ l_debug("seq:%x seq0:%x", net_seq, net_seqZero);
+ if (net_ctl) {
+ l_debug("CTL - %4.4x RX", net_seqZero);
+ if (net_opcode == NET_OP_SEG_ACKNOWLEDGE) {
+ /* Illegal to send ACK to non-Unicast Addr */
+ if (net_dst & 0x8000)
+ return;
+
+ /* print_packet("Got ACK", msg, app_msg_len); */
+ /* Pedantic check for correct size */
+ if (app_msg_len != 7)
+ return;
+
+ /* If this is an ACK to our friend queue-only */
+ if (is_lpn_friend(net, net_dst, true))
+ friend_ack_rxed(net, iv_index, net_seq,
+ net_src, net_dst,
+ msg);
+ else
+ ack_received(net, false,
+ net_src, net_dst,
+ net_seqZero,
+ l_get_be32(msg + 3));
+ } else {
+ ctl_received(net, !!(net_frnd), iv_index,
+ net_ttl, net_seq, net_src,
+ net_dst, net_opcode, rssi,
+ msg, app_msg_len);
+ }
+ } else if (net_segmented) {
+ /* If we accept SAR packets to non-Unicast, then
+ * Friend Sar at least needs to be Unicast Only
+ */
+ if (is_lpn_friend(net, net_dst, true) &&
+ !(net_dst & 0x8000)) {
+ /* Check TTL >= 2 before accepting segments
+ * for Friends
+ */
+ if (net_ttl >= 2) {
+ friend_seg_rxed(net, iv_index,
+ net_ttl, net_seq,
+ net_src, net_dst,
+ l_get_be32(packet + 2 + 9),
+ msg, app_msg_len);
+ }
+ } else {
+ seg_rxed(net, net_frnd,
+ iv_index,
+ net_ttl,
+ net_seq,
+ net_src, net_dst,
+ net_key_id,
+ net_szmic, net_seqZero,
+ net_segO, net_segN,
+ msg, app_msg_len);
+ }
+
+ } else {
+ msg_rxed(net, net_frnd,
+ iv_index,
+ net_ttl,
+ net_seq,
+ net_src, net_dst,
+ net_key_id,
+ false, net_seq & SEQ_ZERO_MASK,
+ msg, app_msg_len);
+ }
+
+ if (!!(net_frnd))
+ l_info("Ask for more data!");
+
+ /* If this is one of our Unicast addresses, don't relay */
+ if (net_dst <= 0x7fff)
+ return;
+ }
+
+ if (!net->relay.enable || net_ttl < 0x02 || net_frnd)
+ return;
+
+ packet[2 + 1] = (packet[2 + 1] & ~TTL_MASK) | (net_ttl - 1);
+
+ if (!mesh_crypto_packet_encode(packet + 2, size, keys->enc_key,
+ iv_index, keys->privacy_key)) {
+ l_error("Failed to encode relay packet");
+ return;
+ }
+
+ if (net->relay.enable)
+ send_relay_pkt(net, packet, size + 1);
+}
+
+static void net_msg_recv(void *user_data, struct mesh_io_recv_info *info,
+ const uint8_t *data, uint16_t len)
+{
+ struct mesh_net *net = user_data;
+ int8_t rssi = 0;
+
+ if (len <= 2 || !net)
+ return;
+
+ if (info) {
+ net->instant = info->instant;
+ net->chan = info->chan;
+ rssi = info->rssi;
+ }
+
+ packet_received(user_data, data + 1, len - 1, rssi);
+}
+
+static void set_network_beacon(void *a, void *b)
+{
+ struct mesh_subnet *subnet = a;
+ struct mesh_net *net = b;
+ uint8_t beacon_data[22];
+
+ if (!create_secure_beacon(net, subnet, beacon_data,
+ sizeof(beacon_data)))
+ return;
+
+ if (memcmp(&subnet->snb.beacon[1], beacon_data,
+ sizeof(beacon_data)) == 0)
+ return;
+
+ memcpy(&subnet->snb.beacon[1], beacon_data, sizeof(beacon_data));
+
+ if (net->beacon_enable && !net->friend_addr) {
+ print_packet("Set My Beacon to",
+ beacon_data, sizeof(beacon_data));
+ start_network_beacon(subnet, net);
+ }
+
+ if (l_queue_length(net->friends)) {
+ struct mesh_friend_msg update = {
+ .src = net->src_addr,
+ .iv_index = mesh_net_get_iv_index(net),
+ .last_len = 7,
+ .ctl = true,
+ };
+
+ update.u.one[0].hdr = NET_OP_FRND_UPDATE << OPCODE_HDR_SHIFT;
+ update.u.one[0].seq = mesh_net_next_seq_num(net);
+ update.u.one[0].data[0] = NET_OP_FRND_UPDATE;
+ update.u.one[0].data[1] = beacon_data[3];
+ l_put_be32(net->iv_index, update.u.one[0].data + 2);
+ update.u.one[0].data[6] = 0x01; /* More Data */
+ /* print_packet("Frnd-Beacon-SRC",
+ * beacon_data, sizeof(beacon_data));
+ */
+ /* print_packet("Frnd-Update", update.u.one[0].data, 6); */
+
+ l_queue_foreach(net->friends, enqueue_update, &update);
+ }
+}
+
+static void iv_upd_to(struct l_timeout *upd_timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+
+ switch (net->iv_upd_state) {
+ case IV_UPD_UPDATING:
+ if (l_queue_length(net->sar_out)) {
+ l_info("don't leave IV Update until sar_out empty");
+ l_timeout_modify(net->iv_update_timeout, 10);
+ break;
+ }
+
+ l_info("iv_upd_state = IV_UPD_NORMAL_HOLD");
+ net->iv_upd_state = IV_UPD_NORMAL_HOLD;
+ l_timeout_modify(net->iv_update_timeout, IV_IDX_UPD_MIN);
+ mesh_net_set_seq_num(net, 0);
+ l_queue_foreach(net->subnets, set_network_beacon, net);
+ mesh_net_flush_msg_queues(net);
+ break;
+
+ case IV_UPD_INIT:
+ case IV_UPD_NORMAL_HOLD:
+ case IV_UPD_NORMAL:
+ l_timeout_remove(upd_timeout);
+ net->iv_update_timeout = NULL;
+ l_info("iv_upd_state = IV_UPD_NORMAL");
+ net->iv_upd_state = IV_UPD_NORMAL;
+ if (net->seq_num > IV_UPDATE_SEQ_TRIGGER)
+ mesh_net_iv_index_update(net);
+ break;
+ }
+}
+
+static void update_iv_kr_state(struct mesh_subnet *subnet, uint32_t iv_index,
+ bool iv_update, bool kr_transition,
+ bool rxed_key_refresh, bool lpn)
+{
+ struct mesh_net *net = subnet->net;
+ uint8_t local_kr;
+ uint32_t local_iv_index;
+ bool local_iv_update;
+
+ /* Save original settings to avoid resetting same values,
+ * and secure beacon timer
+ */
+ local_iv_index = net->iv_index;
+ local_kr = subnet->key_refresh;
+ local_iv_update = iv_is_updating(net);
+
+ if (iv_index != local_iv_index || kr_transition)
+ l_info("SNB-RX: %8.8x - Key Refresh: %d IV Update: %d",
+ iv_index, rxed_key_refresh, iv_update);
+
+ if (iv_update && (net->iv_upd_state > IV_UPD_UPDATING)) {
+ if (iv_index != net->iv_index)
+ l_error("Update attempted to0 soon (Normal < MIN)");
+
+ return;
+ }
+
+ if (net->iv_upd_state == IV_UPD_INIT) {
+ if (iv_index > net->iv_index)
+ mesh_net_set_seq_num(net, 0);
+ net->iv_index = iv_index;
+
+ if (iv_update) {
+ /* Other devices will be accepting old or new iv_index,
+ * but we don't know how far through update they are.
+ * Starting permissive state will allow us maximum
+ * (96 hours) to resync
+ */
+ l_info("iv_upd_state = IV_UPD_UPDATING");
+ net->iv_upd_state = IV_UPD_UPDATING;
+ net->iv_update_timeout = l_timeout_create(
+ IV_IDX_UPD_MIN, iv_upd_to, net, NULL);
+ } else {
+ l_info("iv_upd_state = IV_UPD_NORMAL");
+ net->iv_upd_state = IV_UPD_NORMAL;
+ }
+
+ storage_local_set_iv_index(net, iv_index, net->iv_upd_state);
+
+ /* Figure out the key refresh phase */
+ if (kr_transition) {
+ if (rxed_key_refresh)
+ mesh_net_key_refresh_phase_two(net,
+ subnet->idx);
+ else
+ mesh_net_key_refresh_finish(net, subnet->idx);
+ }
+
+ if (!lpn)
+ set_network_beacon(subnet, net);
+
+ return;
+ }
+
+ if (iv_update && !iv_is_updating(net)) {
+ l_info("iv_upd_state = IV_UPD_UPDATING");
+ net->iv_upd_state = IV_UPD_UPDATING;
+ net->iv_update_timeout = l_timeout_create(IV_IDX_UPD_MIN,
+ iv_upd_to, net, NULL);
+ storage_local_set_iv_index(net, iv_index, net->iv_upd_state);
+ } else if (iv_update && iv_index != net->iv_index) {
+ l_error("Update attempted too soon (iv idx already updated)");
+ return;
+ }
+
+ if (iv_index != local_iv_index || kr_transition)
+ l_info("IVindex 0x%8.8x / Key Refresh update received",
+ iv_index);
+
+ if (iv_index > net->iv_index) {
+ l_queue_clear(net->msg_cache, mesh_msg_free);
+ net->iv_index = iv_index;
+ storage_local_set_iv_index(net, iv_index, net->iv_upd_state);
+ }
+
+ /* Figure out the key refresh phase */
+ if (kr_transition) {
+ if (rxed_key_refresh)
+ mesh_net_key_refresh_phase_two(net, subnet->idx);
+ else
+ mesh_net_key_refresh_finish(net, subnet->idx);
+ }
+
+ if (!lpn)
+ return;
+
+ if (local_kr != subnet->key_refresh ||
+ local_iv_index != net->iv_index ||
+ local_iv_update != iv_is_updating(net))
+ set_network_beacon(subnet, net);
+}
+
+static void process_beacon(void *user_data, const void *data,
+ uint8_t size, int8_t rssi)
+{
+ struct mesh_net *net = user_data;
+ const uint8_t *buf = data;
+ uint32_t iv_index;
+ uint64_t cmac;
+ bool iv_update, rxed_iv_update, rxed_key_refresh;
+ struct mesh_subnet *subnet;
+ struct net_key *keys;
+ bool kr_transition = false;
+
+ if (size != 22 || buf[0] != 0x01)
+ return;
+
+ /* print_packet("Secure Net Beacon RXed", data, size); */
+ rxed_key_refresh = (buf[1] & 0x01) == 0x01;
+ rxed_iv_update = iv_update = (buf[1] & 0x02) == 0x02;
+ iv_index = l_get_be32(buf + 10);
+
+ l_debug("KR: %d -- IVU: %d -- IV: %8.8x",
+ rxed_key_refresh, rxed_iv_update, iv_index);
+
+ /* Inhibit recognizing iv_update true-->false
+ * if we have outbound SAR messages in flight
+ */
+ if (l_queue_length(net->sar_out)) {
+ if (!iv_update && iv_update != iv_is_updating(net))
+ iv_update = true;
+ }
+
+ subnet = l_queue_find(net->subnets, match_network_id, buf + 2);
+ if (!subnet)
+ return;
+
+ /* Check if Key Refresh flag value is different from
+ * the locally stored one
+ */
+ if (rxed_key_refresh != subnet->key_refresh)
+ kr_transition = true;
+
+ /* If the local node is a provisioner or there are no new keys,
+ * ignore KR beacon setting
+ */
+ if (net->provisioner)
+ kr_transition = false;
+ else if (subnet->updated.key_set.nid == NET_NID_INVALID)
+ kr_transition = false;
+ /* If beacon's key refresh bit is not set and the beacon is encoded
+ * with the "new" network key, this signals transition from
+ * key refresh procedure to normal operation
+ */
+ else if (!rxed_key_refresh &&
+ !memcmp(subnet->updated.network_id, buf + 2, 8))
+ kr_transition = true;
+
+ if ((net->iv_index + IV_IDX_DIFF_RANGE < iv_index) ||
+ (iv_index < net->iv_index)) {
+ l_info("iv index outside range");
+ return;
+ }
+
+ /* Don't bother going further if nothing has changed */
+ if (!memcmp(&subnet->snb.beacon[1], buf, size)) {
+ subnet->snb.observed++;
+ return;
+ }
+
+ if (!rxed_key_refresh && !subnet->key_refresh && !kr_transition)
+ keys = &subnet->current;
+ else if (subnet->updated.key_set.nid != NET_NID_INVALID)
+ keys = &subnet->updated;
+ else
+ return;
+
+ if (memcmp(keys->network_id, buf + 2, 8))
+ return;
+
+ /* Any behavioral changes must pass CMAC test */
+ if (!mesh_crypto_beacon_cmac(keys->beacon_key, keys->network_id,
+ iv_index, rxed_key_refresh,
+ rxed_iv_update, &cmac)) {
+ l_error("mesh_crypto_beacon_cmac failed");
+ return;
+ }
+
+ if (cmac != l_get_be64(buf + 14)) {
+ l_error("cmac compare failed %16.16lx != %16.16lx",
+ cmac, l_get_be64(buf + 14));
+ return;
+ }
+
+ if (iv_index == net->iv_index &&
+ iv_is_updating(net) == iv_update && !kr_transition) {
+ l_info("No change: IV index %4.4x, rxed KR = %d ",
+ iv_index, rxed_key_refresh);
+ if (net->iv_upd_state == IV_UPD_INIT) {
+ l_info("iv_upd_state = IV_UPD_NORMAL");
+ net->iv_upd_state = IV_UPD_NORMAL;
+ }
+
+ subnet->snb.observed++;
+ return;
+ }
+
+ subnet->snb.observed++;
+
+ update_iv_kr_state(subnet, iv_index, iv_update, kr_transition,
+ rxed_key_refresh, false);
+}
+
+static void lpn_process_beacon(void *user_data, const void *data,
+ uint8_t size, int8_t rssi)
+{
+ struct mesh_net *net = user_data;
+ const uint8_t *buf = data;
+ uint32_t iv_index;
+ bool iv_update, rxed_key_refresh;
+ struct mesh_subnet *subnet;
+ bool kr_transition = false;
+
+ /* print_packet("lpn: Secure Net Beacon RXed", data, size); */
+ rxed_key_refresh = (buf[0] & 0x01) == 0x01;
+ iv_index = l_get_be32(buf + 1);
+
+ l_debug("KR: %d -- IVU: %d -- IV: %8.8x",
+ rxed_key_refresh, iv_index);
+
+ /* Inhibit recognizing iv_update true-->false if we have outbound
+ * SAR messages in flight
+ */
+ if (l_queue_length(net->sar_out)) {
+ if (!iv_update && iv_update != iv_is_updating(net))
+ iv_update = true;
+ }
+
+ /* TODO: figure out actual network index (i.e., friendship subnet) */
+ subnet = get_primary_subnet(net);
+ if (!subnet)
+ return;
+
+ /* Check if Key Refresh flag value is different from
+ * the locally stored one
+ */
+ if (rxed_key_refresh != subnet->key_refresh)
+ kr_transition = true;
+
+ /* If the local node is a provisioner or there are no new keys,
+ * ignore KR beacon setting
+ */
+ if (subnet->updated.key_set.nid == NET_NID_INVALID)
+ kr_transition = false;
+
+ if ((net->iv_index + IV_IDX_DIFF_RANGE < iv_index) ||
+ (iv_index < net->iv_index)) {
+ l_info("iv index outside range");
+ return;
+ }
+
+ /* Don't bother going further if nothing has changed */
+ if (!kr_transition && iv_index == net->iv_index &&
+ iv_update == iv_is_updating(net) &&
+ net->iv_upd_state != IV_UPD_INIT)
+ return;
+
+ if (iv_index == net->iv_index &&
+ iv_is_updating(net) == iv_update && !kr_transition) {
+ l_info("No change: IV index %4.4x, rxed KR = %d ",
+ iv_index, rxed_key_refresh);
+ if (net->iv_upd_state == IV_UPD_INIT) {
+ l_info("iv_upd_state = IV_UPD_NORMAL");
+ net->iv_upd_state = IV_UPD_NORMAL;
+ }
+ return;
+ }
+
+ update_iv_kr_state(subnet, iv_index, iv_update, kr_transition,
+ rxed_key_refresh, true);
+}
+
+static void beacon_recv(void *user_data, struct mesh_io_recv_info *info,
+ const uint8_t *data, uint16_t len)
+{
+ struct mesh_net *net = user_data;
+ int8_t rssi = 0;
+
+ if (len <= 2 || !net)
+ return;
+
+ if (info) {
+ net->instant = info->instant;
+ net->chan = info->chan;
+ rssi = info->rssi;
+ }
+
+ process_beacon(user_data, data + 1, len - 1, rssi);
+}
+
+bool mesh_net_set_beacon_mode(struct mesh_net *net, bool enable)
+{
+ if (!net || !IS_UNASSIGNED(net->friend_addr))
+ return false;
+
+ if (net->beacon_enable != enable) {
+ uint8_t type = MESH_AD_TYPE_BEACON;
+
+ net->beacon_enable = enable;
+
+ if (!enable)
+ mesh_io_send_cancel(net->io, &type, 1);
+
+ l_queue_foreach(net->subnets, start_network_beacon, net);
+ }
+
+ return true;
+}
+
+
+bool mesh_net_attach(struct mesh_net *net, struct mesh_io *io)
+{
+ if (!net)
+ return false;
+
+ net->io = io;
+
+ return true;
+}
+
+struct mesh_io *mesh_net_detach(struct mesh_net *net)
+{
+ struct mesh_io *io;
+ uint8_t type = 0;
+
+ if (!net)
+ return NULL;
+
+ io = net->io;
+
+ mesh_io_send_cancel(net->io, &type, 1);
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON);
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_NET);
+
+ net->io = NULL;
+
+ return io;
+}
+
+bool mesh_net_iv_index_update(struct mesh_net *net)
+{
+ if (net->iv_upd_state != IV_UPD_NORMAL)
+ return false;
+
+ l_info("iv_upd_state = IV_UPD_UPDATING");
+ mesh_net_flush_msg_queues(net);
+ net->iv_upd_state = IV_UPD_UPDATING;
+ net->iv_index++;
+ if (!storage_local_set_iv_index(net, net->iv_index, IV_UPD_UPDATING))
+ return false;
+
+ l_queue_foreach(net->subnets, set_network_beacon, net);
+ net->iv_update_timeout = l_timeout_create(
+ IV_IDX_UPD_MIN,
+ iv_upd_to, net, NULL);
+
+ return true;
+}
+
+
+
+void mesh_net_sub_list_add(struct mesh_net *net, uint16_t addr)
+{
+ uint8_t msg[11] = { PROXY_OP_FILTER_ADD };
+ uint8_t n = 1;
+
+ l_put_be16(addr, msg + n);
+ n += 2;
+
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), 0,
+ 0, 0, 0, msg, n);
+}
+
+void mesh_net_sub_list_del(struct mesh_net *net, uint16_t addr)
+{
+ uint8_t msg[11] = { PROXY_OP_FILTER_DEL };
+ uint8_t n = 1;
+
+ l_put_be16(addr, msg + n);
+ n += 2;
+
+ mesh_net_transport_send(net, NULL, false,
+ mesh_net_get_iv_index(net), 0,
+ 0, 0, 0, msg, n);
+}
+
+bool mesh_net_dst_reg(struct mesh_net *net, uint16_t dst)
+{
+ struct mesh_destination *dest = l_queue_find(net->destinations,
+ match_by_dst, L_UINT_TO_PTR(dst));
+
+ if (IS_UNASSIGNED(dst) || IS_ALL_NODES(dst))
+ return false;
+
+ if (!dest) {
+ dest = l_new(struct mesh_destination, 1);
+
+ if (dst < 0x8000)
+ l_queue_push_head(net->destinations, dest);
+ else
+ l_queue_push_tail(net->destinations, dest);
+
+ /* If LPN, and Group/Virtual, add to Subscription List */
+ if (net->friend_addr) {
+ /* TODO: Fix this garbage */
+ uint32_t u32_dst[7] = {dst, 0xffffffff};
+
+ frnd_sub_add(net, u32_dst);
+ }
+ }
+
+ dest->dst = dst;
+ dest->ref_cnt++;
+
+ return true;
+}
+
+bool mesh_net_dst_unreg(struct mesh_net *net, uint16_t dst)
+{
+ struct mesh_destination *dest = l_queue_find(net->destinations,
+ match_by_dst, L_UINT_TO_PTR(dst));
+
+ if (!dest)
+ return false;
+
+ if (dest->ref_cnt)
+ dest->ref_cnt--;
+
+ if (dest->ref_cnt)
+ return true;
+
+ /* TODO: If LPN, and Group/Virtual, remove from Subscription List */
+ if (net->friend_addr) {
+ /* TODO: Fix this garbage */
+ uint32_t u32_dst[7] = {dst, 0xffffffff};
+
+ frnd_sub_del(net, u32_dst);
+ }
+
+ l_queue_remove(net->destinations, dest);
+
+ l_free(dest);
+ return true;
+}
+
+bool mesh_net_flush(struct mesh_net *net)
+{
+ if (!net)
+ return false;
+
+ /* TODO mesh-io Flush */
+ return true;
+}
+
+/* TODO: add net key index */
+static bool send_seg(struct mesh_net *net, struct mesh_sar *msg, uint8_t segO)
+{
+ uint8_t seg_len;
+ uint8_t gatt_data[30];
+ uint8_t *packet = gatt_data;
+ uint8_t packet_len;
+ uint8_t segN = SEG_MAX(msg->len);
+ uint16_t seg_off = SEG_OFF(segO);
+ struct mesh_key_set *key_set = NULL;
+ uint32_t seq_num = mesh_net_next_seq_num(net);
+
+ if (segN) {
+ if (msg->len - seg_off > SEG_OFF(1))
+ seg_len = SEG_OFF(1);
+ else
+ seg_len = msg->len - seg_off;
+ } else {
+ seg_len = msg->len;
+ }
+
+ /* Start IV Update procedure when we hit our trigger point */
+ if (!msg->frnd && net->seq_num > IV_UPDATE_SEQ_TRIGGER)
+ mesh_net_iv_index_update(net);
+
+ l_debug("segN %d segment %d seg_off %d", segN, segO, seg_off);
+ /* print_packet("Sending", msg->buf + seg_off, seg_len); */
+ {
+ /* TODO: Are we RXing on an LPN's behalf? Then set RLY bit */
+
+ if (!mesh_crypto_packet_build(false, msg->ttl,
+ seq_num,
+ msg->src, msg->remote,
+ 0,
+ segN ? true : false, msg->key_id,
+ msg->szmic, false, msg->seqZero,
+ segO, segN,
+ msg->buf + seg_off, seg_len,
+ packet + 1, &packet_len)) {
+ l_error("Failed to build packet");
+ return false;
+ }
+ }
+ print_packet("Clr-Net Tx", packet + 1, packet_len);
+
+ if (msg->frnd_cred && net->friend_addr)
+ key_set = frnd_get_key(net);
+
+ if (key_set == NULL) {
+ struct mesh_subnet *subnet = get_primary_subnet(net);
+
+ key_set = &subnet->tx->key_set;
+ }
+
+ if (!mesh_crypto_packet_encode(packet + 1, packet_len, key_set->enc_key,
+ msg->iv_index, key_set->privacy_key)) {
+ l_error("Failed to encode packet");
+ return false;
+ }
+
+ print_packet("Step 3", packet + 1, packet_len);
+ if (!mesh_crypto_packet_label(packet + 1, packet_len,
+ msg->iv_index, key_set->nid)) {
+ l_error("Failed to label packet");
+ return false;
+ }
+ /* print_packet("Step 4", packet + 1, packet_len); */
+
+ {
+ char *str;
+
+ send_msg_pkt(net, packet, packet_len + 1);
+
+ str = l_util_hexstring(packet + 1, packet_len);
+ l_info("TX: Network %04x -> %04x : %s (%u) : TTL %d : SEQ %06x",
+ msg->src, msg->remote, str,
+ packet_len, msg->ttl,
+ msg->frnd ? msg->seqAuth + segO : seq_num);
+ l_free(str);
+ }
+
+ msg->last_seg = segO;
+
+ return true;
+}
+
+void mesh_net_send_seg(struct mesh_net *net, struct mesh_key_set *key_set,
+ uint32_t iv_index,
+ uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ uint32_t hdr,
+ const void *seg, uint16_t seg_len)
+{
+ char *str;
+ uint8_t packet[30];
+ uint8_t packet_len;
+ bool segmented = !!((hdr >> SEG_HDR_SHIFT) & true);
+ uint8_t key_id = (hdr >> KEY_HDR_SHIFT) & KEY_ID_MASK;
+ bool szmic = !!((hdr >> SZMIC_HDR_SHIFT) & true);
+ uint16_t seqZero = (hdr >> SEQ_ZERO_HDR_SHIFT) & SEQ_ZERO_MASK;
+ uint8_t segO = (hdr >> SEGO_HDR_SHIFT) & SEG_MASK;
+ uint8_t segN = (hdr >> SEGN_HDR_SHIFT) & SEG_MASK;
+
+ /* TODO: Only used for current POLLed segments to LPNs */
+
+ l_debug("SEQ: %6.6x", seq + segO);
+ l_debug("SEQ0: %6.6x", seq);
+ l_debug("segO: %d", segO);
+
+ if (!mesh_crypto_packet_build(false, ttl,
+ seq,
+ src, dst,
+ 0,
+ segmented, key_id,
+ szmic, false, seqZero,
+ segO, segN,
+ seg, seg_len,
+ packet + 1, &packet_len)) {
+ l_error("Failed to build packet");
+ return;
+ }
+
+ if (!mesh_crypto_packet_encode(packet + 1, packet_len, key_set->enc_key,
+ iv_index, key_set->privacy_key)) {
+ l_error("Failed to encode packet");
+ return;
+ }
+
+ /* print_packet("Step 3", packet + 0, packet_len); */
+ if (!mesh_crypto_packet_label(packet + 1, packet_len, iv_index,
+ key_set->nid)) {
+ l_error("Failed to label packet");
+ return;
+ }
+ /* print_packet("Step 4", packet + 1, packet_len); */
+
+ send_msg_pkt(net, packet, packet_len + 1);
+
+ str = l_util_hexstring(packet + 1, packet_len);
+ l_info("TX: Friend Seg-%d %04x -> %04x : %s (%u) : TTL %d : SEQ %06x",
+ segO, src, dst, str, packet_len, ttl, seq);
+ l_free(str);
+}
+
+unsigned int mesh_net_app_send(struct mesh_net *net, bool frnd_cred,
+ uint16_t src, uint16_t dst,
+ uint8_t key_id, uint8_t ttl,
+ uint32_t seq, uint32_t iv_index,
+ bool szmic,
+ const void *msg, uint16_t msg_len,
+ mesh_net_status_func_t status_func,
+ void *user_data)
+{
+ struct mesh_sar *payload = NULL;
+ uint8_t seg, seg_max;
+ unsigned int ret = 0;
+ bool result;
+
+ if (!net || msg_len > 384)
+ return 0;
+
+ if (!src)
+ src = net->src_addr;
+
+ if (!src || !dst)
+ return 0;
+
+ if (ttl == 0xff)
+ ttl = net->default_ttl;
+
+ seg_max = SEG_MAX(msg_len);
+
+ /* First enqueue to any Friends and internal models */
+ result = msg_rxed(net, false,
+ iv_index,
+ ttl,
+ seq + seg_max,
+ src, dst,
+ key_id,
+ szmic, seq & SEQ_ZERO_MASK,
+ msg, msg_len);
+
+ /* If successfully enqued or delivered
+ * to Unicast address, we are done
+ */
+ if (result || src == dst ||
+ (dst >= net->src_addr && dst <= net->last_addr)) {
+ /* Adjust our seq_num for "virtual" delivery */
+ net->seq_num += seg_max;
+ mesh_net_next_seq_num(net);
+ return 0;
+ }
+
+ /* If Segmented, Cancel any OB segmented message to same DST */
+ if (seg_max) {
+ payload = l_queue_remove_if(net->sar_out, match_sar_remote,
+ L_UINT_TO_PTR(dst));
+ mesh_sar_free(payload);
+ }
+
+ /* Setup OTA Network send */
+ payload = mesh_sar_new(msg_len);
+ memcpy(payload->buf, msg, msg_len);
+ payload->len = msg_len;
+ payload->src = src;
+ payload->remote = dst;
+ payload->ttl = ttl;
+ payload->szmic = szmic;
+ payload->frnd_cred = frnd_cred;
+ payload->key_id = key_id;
+ if (seg_max) {
+ payload->flags = 0xffffffff >> (31 - seg_max);
+ payload->seqZero = seq & SEQ_ZERO_MASK;
+ }
+
+ payload->iv_index = mesh_net_get_iv_index(net);
+ payload->seqAuth = net->seq_num;
+
+ result = true;
+ if (!IS_UNICAST(dst) && seg_max) {
+ for (int i = 0; i < 4; i++) {
+ for (seg = 0; seg <= seg_max && result; seg++)
+ result = send_seg(net, payload, seg);
+ }
+ } else {
+ for (seg = 0; seg <= seg_max && result; seg++)
+ result = send_seg(net, payload, seg);
+ }
+
+ /* Reliable: Cache; Unreliable: Flush*/
+ if (result && seg_max && IS_UNICAST(dst)) {
+ l_queue_push_head(net->sar_out, payload);
+ payload->seg_timeout =
+ l_timeout_create(SEG_TO, outseg_to, net, NULL);
+ payload->msg_timeout =
+ l_timeout_create(MSG_TO, outmsg_to, net, NULL);
+ payload->status_func = status_func;
+ payload->user_data = user_data;
+ ret = payload->id = ++net->sar_id_next;
+ } else
+ mesh_sar_free(payload);
+
+ return ret;
+}
+
+void mesh_net_app_send_cancel(struct mesh_net *net, unsigned int id)
+{
+ struct mesh_sar *sar = l_queue_remove_if(net->sar_out, match_sar_id,
+ L_UINT_TO_PTR(id));
+
+ if (sar) {
+ l_info("Canceling OB %d", id);
+ if (sar->status_func)
+ sar->status_func(sar->remote, 2,
+ sar->buf, sar->len - 4,
+ sar->user_data);
+ }
+ mesh_sar_free(sar);
+}
+
+/* TODO: add net key index */
+void mesh_net_ack_send(struct mesh_net *net, struct mesh_key_set *key_set,
+ uint32_t iv_index,
+ uint8_t ttl,
+ uint32_t seq,
+ uint16_t src, uint16_t dst,
+ bool rly, uint16_t seqZero,
+ uint32_t ack_flags)
+{
+ uint32_t hdr;
+ uint8_t data[7];
+ uint8_t pkt_len;
+ uint8_t pkt[30];
+ char *str;
+
+ hdr = NET_OP_SEG_ACKNOWLEDGE << OPCODE_HDR_SHIFT;
+ hdr |= rly << RELAY_HDR_SHIFT;
+ hdr |= (seqZero & SEQ_ZERO_MASK) << SEQ_ZERO_HDR_SHIFT;
+ l_put_be32(hdr, data);
+ l_put_be32(ack_flags, data + 3);
+ if (!mesh_crypto_packet_build(true, ttl,
+ seq,
+ src, dst,
+ NET_OP_SEG_ACKNOWLEDGE,
+ false, /* Not Segmented */
+ 0, /* No Key ID associated */
+ false, rly, seqZero,
+ 0, 0, /* no segO or segN */
+ data + 1, 6,
+ pkt + 1, &pkt_len)) {
+ return;
+ }
+
+ if (key_set == NULL) {
+ struct mesh_subnet *subnet = get_primary_subnet(net);
+
+ key_set = &subnet->tx->key_set;
+ }
+
+ if (!mesh_crypto_packet_encode(pkt + 1, pkt_len, key_set->enc_key,
+ iv_index, key_set->privacy_key)) {
+ l_error("Failed to encode packet");
+ return;
+ }
+
+ /* print_packet("Step 3", pkt, pkt_len); */
+ if (!mesh_crypto_packet_label(pkt + 1, pkt_len,
+ iv_index, key_set->nid)) {
+ l_error("Failed to label packet");
+ return;
+ }
+
+ /* print_packet("Step 4", pkt, pkt_len); */
+ send_msg_pkt(net, pkt, pkt_len + 1);
+
+ str = l_util_hexstring(pkt + 1, pkt_len);
+ l_info("TX: Friend ACK %04x -> %04x : %s (%u) : TTL %d : SEQ %06x",
+ src, dst, str, pkt_len,
+ ttl, seq);
+ l_free(str);
+}
+
+/* TODO: add net key index */
+void mesh_net_transport_send(struct mesh_net *net, struct mesh_key_set *key_set,
+ bool fast, uint32_t iv_index, uint8_t ttl,
+ uint32_t seq, uint16_t src, uint16_t dst,
+ const uint8_t *msg, uint16_t msg_len)
+{
+ uint32_t use_seq = seq;
+ uint8_t pkt_len;
+ uint8_t pkt[30];
+ bool result = false;
+
+ if (!net->src_addr)
+ return;
+
+ if (!src)
+ src = net->src_addr;
+
+ if (src == dst)
+ return;
+
+ if (ttl == 0xff)
+ ttl = net->default_ttl;
+
+ /* Range check the Opcode and msg length*/
+ if (*msg & 0xc0 || (9 + msg_len + 8 > 29))
+ return;
+
+ /* Enqueue for Friend if forwardable and from us */
+ if (!(key_set) && src >= net->src_addr && src <= net->last_addr) {
+ uint32_t hdr = msg[0] << OPCODE_HDR_SHIFT;
+ uint8_t frnd_ttl = ttl;
+
+ if (friend_packet_queue(net, iv_index,
+ true, frnd_ttl,
+ mesh_net_next_seq_num(net),
+ src, dst,
+ hdr,
+ msg + 1, msg_len - 1)) {
+ return;
+ }
+ }
+
+ /* Deliver to Local entities if applicable */
+ if (!(dst & 0x8000) && src >= net->src_addr && src <= net->last_addr) {
+ result = ctl_received(net, !!(key_set),
+ iv_index, ttl,
+ mesh_net_next_seq_num(net),
+ src, dst,
+ msg[0], 0, msg + 1, msg_len - 1);
+ }
+
+ if (key_set == NULL) {
+ struct mesh_subnet *subnet = get_primary_subnet(net);
+
+ key_set = &subnet->tx->key_set;
+ use_seq = mesh_net_next_seq_num(net);
+
+ if (result || (dst >= net->src_addr && dst <= net->last_addr))
+ return;
+ }
+
+ if (!mesh_crypto_packet_build(true, ttl,
+ use_seq,
+ src, dst,
+ msg[0],
+ false, 0,
+ false, false, 0,
+ 0, 0,
+ msg + 1, msg_len - 1,
+ pkt + 1, &pkt_len))
+ return;
+
+ /* print_packet("Step 2", pkt + 1, pkt_len); */
+
+ if (!mesh_crypto_packet_encode(pkt + 1, pkt_len, key_set->enc_key,
+ iv_index, key_set->privacy_key)) {
+ l_error("Failed to encode pkt");
+ return;
+ }
+
+ /* print_packet("Step 3", pkt + 1, pkt_len); */
+ if (!mesh_crypto_packet_label(pkt, pkt_len, iv_index,
+ key_set->nid)) {
+ l_error("Failed to label pkt");
+ return;
+ }
+
+ /* print_packet("Step 4", pkt + 1, pkt_len); */
+
+ if (dst != 0) {
+ char *str;
+
+ send_msg_pkt(net, pkt, pkt_len + 1);
+
+ str = l_util_hexstring(pkt + 1, pkt_len);
+ l_info("TX: Network %04x -> %04x : %s (%u) : TTL %d : SEQ %06x",
+ src, dst, str, pkt_len,
+ ttl, use_seq);
+ l_free(str);
+ }
+}
+
+uint8_t mesh_net_key_refresh_phase_set(struct mesh_net *net, uint16_t idx,
+ uint8_t transition)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+ if (!subnet)
+ return MESH_STATUS_INVALID_NETKEY;
+
+ if (transition == subnet->kr_phase)
+ return MESH_STATUS_SUCCESS;
+
+ if ((transition != 2 && transition != 3) ||
+ transition < subnet->kr_phase)
+ return MESH_STATUS_CANNOT_SET;
+
+ switch (transition) {
+ case 2:
+ if (mesh_net_key_refresh_phase_two(net, idx)
+ != MESH_STATUS_SUCCESS)
+ return MESH_STATUS_CANNOT_SET;
+ break;
+ case 3:
+ if (mesh_net_key_refresh_finish(net, idx)
+ != MESH_STATUS_SUCCESS)
+ return MESH_STATUS_CANNOT_SET;
+ break;
+ default:
+ return MESH_STATUS_CANNOT_SET;
+ }
+ return MESH_STATUS_SUCCESS;
+}
+
+uint8_t mesh_net_key_refresh_phase_get(struct mesh_net *net, uint16_t idx,
+ uint8_t *phase)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+ if (!subnet)
+ return MESH_STATUS_INVALID_NETKEY;
+
+ *phase = subnet->kr_phase;
+ return MESH_STATUS_SUCCESS;
+}
+
+int mesh_net_kr_phase_one(struct mesh_net *net, uint16_t idx,
+ const uint8_t *value)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+ if (!subnet)
+ return MESH_STATUS_CANNOT_UPDATE;
+
+ if (subnet->updated.key_set.nid != NET_NID_INVALID)
+ l_info("Warning: overwriting new keys");
+
+ /* Preserve starting data */
+ subnet->updated = subnet->current;
+
+ /* Generate new keys */
+ if (!create_keys(net, &subnet->updated, value)) {
+ subnet->updated.key_set.nid = NET_NID_INVALID;
+ l_error("Failed to start key refresh phase one");
+ return MESH_STATUS_CANNOT_UPDATE;
+ }
+
+ /* If we are an LPN, generate our keys here */
+ if (net->friend_addr)
+ frnd_key_refresh(net, 1);
+ else
+ /* If we are a Friend-Node, generate all our new keys */
+ l_queue_foreach(net->friends, frnd_kr_phase1, (void *)value);
+
+ l_info("key refresh phase 1: NID 0x%2x", subnet->updated.key_set.nid);
+
+ mesh_net_add_keyset(net, &subnet->updated.key_set);
+ subnet->kr_phase = KEY_REFRESH_PHASE_ONE;
+
+ return MESH_STATUS_SUCCESS;
+}
+
+int mesh_net_key_refresh_phase_two(struct mesh_net *net, uint16_t idx)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+
+ if (!subnet || subnet->updated.key_set.nid == NET_NID_INVALID)
+ return MESH_STATUS_INVALID_NETKEY;
+
+ l_info("Key refresh procedure phase 2: start using new net TX keys");
+ subnet->key_refresh = 1;
+ subnet->tx = &subnet->updated;
+ /* TODO: Provisioner may need to stay in phase three until
+ * it hears beacons from all the nodes
+ */
+ subnet->kr_phase = KEY_REFRESH_PHASE_TWO;
+ set_network_beacon(subnet, net);
+
+ if (net->friend_addr)
+ frnd_key_refresh(net, 2);
+ else
+ l_queue_foreach(net->friends, frnd_kr_phase2, net);
+
+ return MESH_STATUS_SUCCESS;
+}
+
+int mesh_net_key_refresh_finish(struct mesh_net *net, uint16_t idx)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+
+ if (!subnet || subnet->updated.key_set.nid == NET_NID_INVALID)
+ return MESH_STATUS_INVALID_NETKEY;
+
+ if (subnet->kr_phase == KEY_REFRESH_PHASE_NONE)
+ return MESH_STATUS_SUCCESS;
+
+ l_info("Key refresh phase 3: use new keys only, discard old ones");
+
+ /* Switch to using new keys, discard old ones */
+ subnet->current = subnet->updated;
+ subnet->tx = &subnet->current;
+ subnet->updated.key_set.nid = NET_NID_INVALID;
+ mesh_net_remove_keyset(net, &subnet->updated.key_set);
+ subnet->key_refresh = 0;
+ subnet->kr_phase = KEY_REFRESH_PHASE_NONE;
+ set_network_beacon(subnet, net);
+
+ if (net->friend_addr)
+ frnd_key_refresh(net, 3);
+ else
+ l_queue_foreach(net->friends, frnd_kr_phase3, net);
+
+ return MESH_STATUS_SUCCESS;
+}
+
+void mesh_net_heartbeat_send(struct mesh_net *net)
+{
+ struct mesh_net_heartbeat *hb = &net->heartbeat;
+ uint8_t msg[4];
+ int n = 0;
+
+ if (hb->pub_dst == UNASSIGNED_ADDRESS)
+ return;
+
+ msg[n++] = NET_OP_HEARTBEAT;
+ msg[n++] = hb->pub_ttl;
+ l_put_be16(hb->features, msg + n);
+ n += 2;
+
+ mesh_net_transport_send(net, NULL, false, mesh_net_get_iv_index(net),
+ hb->pub_ttl, 0, 0, hb->pub_dst, msg, n);
+}
+
+void mesh_net_heartbeat_init(struct mesh_net *net)
+{
+ struct mesh_net_heartbeat *hb = &net->heartbeat;
+
+ memset(hb, 0, sizeof(struct mesh_net_heartbeat));
+ hb->sub_min_hops = 0xff;
+ hb->features = mesh_net_get_features(net);
+}
+
+uint32_t mesh_net_friend_timeout(struct mesh_net *net, uint16_t addr)
+{
+ struct mesh_friend *frnd = l_queue_find(net->friends, match_by_friend,
+ L_UINT_TO_PTR(addr));
+
+ if (!frnd)
+ return 0;
+ else
+ return frnd->poll_timeout;
+}
+
+bool mesh_net_local_node_set(struct mesh_net *net, struct mesh_node *node,
+ bool provisioner)
+{
+ if (net->local_node) {
+ l_info("Local node already registered");
+ return false;
+ }
+
+ net->local_node = node;
+ net->provisioner = provisioner;
+
+ return true;
+}
+
+bool mesh_net_provisioned_new(struct mesh_net *net, uint8_t device_key[16],
+ uint16_t net_idx, uint8_t net_key[16],
+ uint16_t unicast, uint16_t snb_flags,
+ uint32_t iv_index, mesh_status_func_t cb,
+ void *user_data)
+{
+ if (net->provisioned || !net->local_node)
+ return false;
+
+ if (!node_set_primary(net->local_node, unicast) ||
+ !(mesh_net_register_unicast(net, unicast,
+ mesh_net_get_num_ele(net))))
+ return false;
+
+ if (!node_set_device_key(net->local_node, device_key))
+ return false;
+
+ net->iv_index = iv_index;
+ net->iv_update = ((snb_flags & 0x02) != 0);
+ if (!storage_local_set_iv_index(net, iv_index, net->iv_update))
+ return false;
+
+ if (mesh_net_add_key(net, false, net_idx, net_key) !=
+ MESH_STATUS_SUCCESS)
+ return false;
+
+ if ((snb_flags & 0x01) &&
+ (mesh_net_add_key(net, true, net_idx, net_key) !=
+ MESH_STATUS_SUCCESS)) {
+ l_queue_clear(net->subnets, l_free);
+ return false;
+ }
+
+ return storage_save_new_config(net, net->cfg_file, cb, user_data);
+
+}
+
+void mesh_net_provisioned_set(struct mesh_net *net, bool provisioned)
+{
+ struct mesh_io *io;
+
+ if (!net)
+ return;
+
+ net->provisioned = provisioned;
+ io = net->io;
+
+ if (provisioned) {
+ mesh_io_register_recv_cb(io, MESH_IO_FILTER_BEACON,
+ beacon_recv, net);
+ mesh_io_register_recv_cb(io, MESH_IO_FILTER_NET,
+ net_msg_recv, net);
+ } else {
+ uint8_t *uuid = node_uuid_get(net->local_node);
+
+ if (!uuid)
+ return;
+
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_BEACON);
+ mesh_io_deregister_recv_cb(io, MESH_IO_FILTER_NET);
+
+ mesh_prov_listen(net, uuid, (uint8_t *) &net->prov_caps,
+ acceptor_prov_open,
+ acceptor_prov_close,
+ acceptor_prov_receive, net);
+ }
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 07/14] mesh: Add Accessors to Transport layer data
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (5 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 06/14] mesh: Upper and Lower mesh transport Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 08/14] mesh: Header files for mesh access layer and utilities Brian Gix
` (7 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/net.c | 549 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 549 insertions(+)
diff --git a/mesh/net.c b/mesh/net.c
index 0dafa1780..544f9efa5 100644
--- a/mesh/net.c
+++ b/mesh/net.c
@@ -828,6 +828,226 @@ void mesh_net_unref(struct mesh_net *net)
l_free(net);
}
+bool mesh_net_set_seq_num(struct mesh_net *net, uint32_t number)
+{
+ if (!net)
+ return false;
+
+ net->cached_seq_num = net->seq_num = number;
+
+ return true;
+}
+
+bool mesh_net_set_default_ttl(struct mesh_net *net, uint8_t ttl)
+{
+ if (!net)
+ return false;
+
+ net->default_ttl = ttl;
+
+ return true;
+}
+
+uint32_t mesh_net_get_seq_num(struct mesh_net *net)
+{
+ if (!net)
+ return 0;
+
+ return net->seq_num;
+}
+
+uint8_t mesh_net_get_default_ttl(struct mesh_net *net)
+{
+ if (!net)
+ return 0;
+
+ return net->default_ttl;
+}
+
+uint16_t mesh_net_get_address(struct mesh_net *net)
+{
+ if (!net)
+ return 0;
+
+ return net->src_addr;
+}
+
+bool mesh_net_register_unicast(struct mesh_net *net,
+ uint16_t address, uint8_t num_ele)
+{
+ if (!net || !IS_UNICAST(address) || !num_ele)
+ return false;
+
+ l_info("mesh_net_set_address: 0x%x", address);
+ net->src_addr = address;
+ net->last_addr = address + num_ele - 1;
+ if (net->last_addr < net->src_addr)
+ return false;
+
+ do {
+ mesh_net_dst_reg(net, address);
+ address++;
+ num_ele--;
+ } while (num_ele > 0);
+
+ return true;
+}
+
+uint8_t mesh_net_get_num_ele(struct mesh_net *net)
+{
+ if (!net)
+ return 0;
+
+ return net->last_addr - net->src_addr + 1;
+}
+
+bool mesh_net_set_proxy_mode(struct mesh_net *net, bool enable)
+{
+ if (!net)
+ return false;
+
+ /* No support for proxy yet */
+ if (enable) {
+ l_error("Proxy not supported!");
+ return false;
+ }
+
+ trigger_heartbeat(net, FEATURE_PROXY, enable);
+ return true;
+}
+
+bool mesh_net_set_friend_mode(struct mesh_net *net, bool enable)
+{
+ if (!net)
+ return false;
+
+ if (net->friend_enable && !enable)
+ l_queue_clear(net->friends, mesh_friend_free);
+
+ net->friend_enable = enable;
+ trigger_heartbeat(net, FEATURE_FRIEND, enable);
+ return true;
+}
+
+bool mesh_net_set_relay_mode(struct mesh_net *net, bool enable,
+ uint8_t cnt, uint8_t interval)
+{
+ if (!net)
+ return false;
+
+ net->relay.enable = enable;
+ net->relay.count = cnt;
+ net->relay.interval = interval;
+ trigger_heartbeat(net, FEATURE_RELAY, enable);
+ return true;
+}
+
+struct mesh_net_prov_caps *mesh_net_prov_caps_get(struct mesh_net *net)
+{
+ if (net)
+ return &net->prov_caps;
+
+ return NULL;
+}
+
+char *mesh_net_id_name(struct mesh_net *net)
+{
+ if (net && net->id_name[0])
+ return net->id_name;
+
+ return NULL;
+}
+
+bool mesh_net_id_uuid_set(struct mesh_net *net, uint8_t uuid[16])
+{
+ if (!net)
+ return false;
+
+ memcpy(net->id_uuid, uuid, 16);
+
+ return true;
+}
+
+uint8_t *mesh_net_priv_key_get(struct mesh_net *net)
+{
+ if (net)
+ return net->prov_priv_key;
+
+ return NULL;
+}
+
+bool mesh_net_priv_key_set(struct mesh_net *net, uint8_t key[32])
+{
+ if (!net)
+ return false;
+
+ memcpy(net->prov_priv_key, key, 32);
+ return true;
+}
+
+uint8_t *mesh_net_test_addr(struct mesh_net *net)
+{
+ const uint8_t zero_addr[] = {0, 0, 0, 0, 0, 0};
+
+ if (net && memcmp(net->test_bd_addr, zero_addr, 6))
+ return net->test_bd_addr;
+
+ return NULL;
+}
+
+uint8_t *mesh_net_prov_rand(struct mesh_net *net)
+{
+ if (net)
+ return net->prov_rand;
+
+ return NULL;
+}
+
+uint16_t mesh_net_prov_uni(struct mesh_net *net, uint8_t ele_cnt)
+{
+ uint16_t uni;
+ uint16_t next;
+
+ if (!net)
+ return 0;
+
+ next = net->prov_uni_addr.next + ele_cnt;
+ if (next > 0x8000 || next > net->prov_uni_addr.high)
+ return UNASSIGNED_ADDRESS;
+
+ uni = net->prov_uni_addr.next;
+ net->prov_uni_addr.next = next;
+
+ return uni;
+}
+
+bool mesh_net_test_mode(struct mesh_net *net)
+{
+ if (net)
+ return net->test_mode;
+
+ return false;
+}
+
+int mesh_net_get_identity_mode(struct mesh_net *net, uint16_t idx,
+ uint8_t *mode)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return MESH_STATUS_UNSPECIFIED_ERROR;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+ if (!subnet)
+ return MESH_STATUS_INVALID_NETKEY;
+
+ /* Currently, proxy mode is not supported */
+ *mode = MESH_MODE_UNSUPPORTED;
+
+ return MESH_STATUS_SUCCESS;
+}
+
int mesh_net_del_key(struct mesh_net *net, uint16_t idx)
{
struct mesh_subnet *subnet;
@@ -924,6 +1144,98 @@ void mesh_net_flush_msg_queues(struct mesh_net *net)
l_queue_clear(net->fast_cache, mesh_msg_free);
}
+uint32_t mesh_net_get_iv_index(struct mesh_net *net)
+{
+ if (!net)
+ return 0xffffffff;
+
+ return net->iv_index - (iv_is_updating(net) ? 1 : 0);
+}
+
+/* TODO: net key index? */
+void mesh_net_get_snb_state(struct mesh_net *net, uint8_t *flags,
+ uint32_t *iv_index)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net || !flags || !iv_index)
+ return;
+
+ *iv_index = net->iv_index;
+ *flags = (net->iv_upd_state == IV_UPD_UPDATING) ? 0x02 : 0x00;
+
+ subnet = get_primary_subnet(net);
+ if (subnet)
+ *flags |= subnet->key_refresh ? 0x01 : 0x00;
+}
+
+bool mesh_net_get_key(struct mesh_net *net, bool new_key, uint16_t idx,
+ uint8_t key_buf[16])
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return false;
+
+ subnet = l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx));
+ if (!subnet)
+ return false;
+
+ if (!new_key) {
+ memcpy(key_buf, subnet->current.key, 16);
+ return true;
+ }
+
+ if (subnet->updated.key_set.nid == NET_NID_INVALID)
+ return false;
+
+ memcpy(key_buf, subnet->updated.key, 16);
+ return true;
+}
+
+bool mesh_net_key_list_get(struct mesh_net *net, uint8_t *buf, uint16_t *size)
+{
+ const struct l_queue_entry *entry;
+ uint16_t n, buf_size;
+
+ if (!net || !buf || !size)
+ return false;
+
+ buf_size = *size;
+ if (buf_size < l_queue_length(net->subnets) * 2)
+ return false;
+
+ n = 0;
+ entry = l_queue_get_entries(net->subnets);
+
+ for (; entry; entry = entry->next) {
+ struct mesh_subnet *subnet = entry->data;
+
+ l_put_le16(subnet->idx, buf);
+ n += 2;
+ }
+
+ *size = n;
+ return true;
+}
+
+bool mesh_net_get_frnd_seq(struct mesh_net *net)
+{
+ if (!net)
+ return false;
+
+ return net->friend_seq;
+}
+
+void mesh_net_set_frnd_seq(struct mesh_net *net, bool seq)
+{
+ if (!net)
+ return;
+
+ net->friend_seq = seq;
+}
+
static bool match_cache(const void *a, const void *b)
{
const struct mesh_msg *msg = a;
@@ -2865,6 +3177,34 @@ void mesh_net_sub_list_del(struct mesh_net *net, uint16_t addr)
0, 0, 0, msg, n);
}
+/* TODO: change to use net index */
+bool mesh_net_set_friend(struct mesh_net *net, uint16_t friend_addr)
+{
+ if (!net)
+ return false;
+
+ net->bea_id = 0;
+
+ l_info("Set Frnd addr: %4.4x", friend_addr);
+ if (!friend_addr)
+ trigger_heartbeat(net, FEATURE_LPN, false);
+ else
+ trigger_heartbeat(net, FEATURE_LPN, true);
+
+ net->friend_addr = friend_addr;
+
+ set_network_beacon(get_primary_subnet(net), net);
+ return true;
+}
+
+uint16_t mesh_net_get_friend(struct mesh_net *net)
+{
+ if (!net)
+ return 0;
+
+ return net->friend_addr;
+}
+
bool mesh_net_dst_reg(struct mesh_net *net, uint16_t dst)
{
struct mesh_destination *dest = l_queue_find(net->destinations,
@@ -3518,6 +3858,27 @@ int mesh_net_key_refresh_finish(struct mesh_net *net, uint16_t idx)
return MESH_STATUS_SUCCESS;
}
+uint16_t mesh_net_get_features(struct mesh_net *net)
+{
+ uint16_t features = 0;
+
+ if (net->relay.enable)
+ features |= FEATURE_RELAY;
+ if (net->proxy_enable)
+ features |= FEATURE_PROXY;
+ if (!l_queue_isempty(net->friends))
+ features |= FEATURE_FRIEND;
+ if (net->friend_addr != UNASSIGNED_ADDRESS)
+ features |= FEATURE_LPN;
+
+ return features;
+}
+
+struct mesh_net_heartbeat *mesh_net_heartbeat_get(struct mesh_net *net)
+{
+ return &net->heartbeat;
+}
+
void mesh_net_heartbeat_send(struct mesh_net *net)
{
struct mesh_net_heartbeat *hb = &net->heartbeat;
@@ -3545,6 +3906,49 @@ void mesh_net_heartbeat_init(struct mesh_net *net)
hb->features = mesh_net_get_features(net);
}
+void mesh_net_uni_range_set(struct mesh_net *net,
+ struct mesh_net_addr_range *range)
+{
+ net->prov_uni_addr.low = range->low;
+ net->prov_uni_addr.high = range->high;
+ net->prov_uni_addr.next = range->next;
+}
+
+struct mesh_net_addr_range mesh_net_uni_range_get(struct mesh_net *net)
+{
+ return net->prov_uni_addr;
+}
+
+void mesh_net_set_iv_index(struct mesh_net *net, uint32_t index, bool update)
+{
+ net->iv_index = index;
+ net->iv_update = update;
+}
+
+void mesh_net_provisioner_mode_set(struct mesh_net *net, bool mode)
+{
+ net->provisioner = mode;
+}
+
+bool mesh_net_provisioner_mode_get(struct mesh_net *net)
+{
+ return net->provisioner;
+}
+
+uint16_t mesh_net_get_primary_idx(struct mesh_net *net)
+{
+ struct mesh_subnet *subnet;
+
+ if (!net)
+ return NET_IDX_INVALID;
+
+ subnet = get_primary_subnet(net);
+ if (!subnet)
+ return NET_IDX_INVALID;
+
+ return subnet->idx;
+}
+
uint32_t mesh_net_friend_timeout(struct mesh_net *net, uint16_t addr)
{
struct mesh_friend *frnd = l_queue_find(net->friends, match_by_friend,
@@ -3570,6 +3974,143 @@ bool mesh_net_local_node_set(struct mesh_net *net, struct mesh_node *node,
return true;
}
+struct mesh_node *mesh_net_local_node_get(struct mesh_net *net)
+{
+ return net->local_node;
+}
+
+bool mesh_net_set_crpl(struct mesh_net *net, uint16_t crpl)
+{
+ if (!net)
+ return false;
+
+ net->crpl = crpl;
+ return true;
+}
+
+uint16_t mesh_net_get_crpl(struct mesh_net *net)
+{
+ if (!net)
+ return 0;
+
+ return net->crpl;
+}
+
+struct l_queue *mesh_net_get_app_keys(struct mesh_net *net)
+{
+ if (!net)
+ return NULL;
+
+ if (!net->app_keys)
+ net->app_keys = l_queue_new();
+
+ return net->app_keys;
+}
+
+bool mesh_net_have_key(struct mesh_net *net, uint16_t idx)
+{
+ if (!net)
+ return false;
+
+ return (l_queue_find(net->subnets, match_key_index,
+ L_UINT_TO_PTR(idx)) != NULL);
+}
+
+bool mesh_net_jconfig_set(struct mesh_net *net, void *jconfig)
+{
+ if (!net)
+ return false;
+
+ net->jconfig_local = jconfig;
+ return true;
+}
+
+void *mesh_net_jconfig_get(struct mesh_net *net)
+{
+ if (!net)
+ return NULL;
+
+ return net->jconfig_local;
+}
+
+bool mesh_net_cfg_file_set(struct mesh_net *net, const char *cfg)
+{
+ if (!net)
+ return false;
+
+ net->cfg_file = cfg;
+ return true;
+}
+
+bool mesh_net_cfg_file_get(struct mesh_net *net, const char **cfg)
+{
+ if (!net)
+ return false;
+
+ *cfg = net->cfg_file;
+ return true;
+}
+
+bool mesh_net_is_local_address(struct mesh_net *net, uint16_t addr)
+{
+ if (!net)
+ return false;
+
+ return (addr >= net->src_addr && addr <= net->last_addr);
+}
+
+void mesh_net_set_window_accuracy(struct mesh_net *net, uint8_t accuracy)
+{
+ if (!net)
+ return;
+
+ net->window_accuracy = accuracy;
+}
+
+void mesh_net_transmit_params_set(struct mesh_net *net, uint8_t count,
+ uint16_t interval)
+{
+ if (!net)
+ return;
+
+ net->tx_interval = interval;
+ net->tx_cnt = count;
+}
+
+void mesh_net_transmit_params_get(struct mesh_net *net, uint8_t *count,
+ uint16_t *interval)
+{
+ if (!net)
+ return;
+
+ *interval = net->tx_interval;
+ *count = net->tx_cnt;
+}
+
+struct mesh_io *mesh_net_get_io(struct mesh_net *net)
+{
+ if (!net)
+ return NULL;
+
+ return net->io;
+}
+
+struct mesh_prov *mesh_net_get_prov(struct mesh_net *net)
+{
+ if (!net)
+ return NULL;
+
+ return net->prov;
+}
+
+void mesh_net_set_prov(struct mesh_net *net, struct mesh_prov *prov)
+{
+ if (!net)
+ return;
+
+ net->prov = prov;
+}
+
bool mesh_net_provisioned_new(struct mesh_net *net, uint8_t device_key[16],
uint16_t net_idx, uint8_t net_key[16],
uint16_t unicast, uint16_t snb_flags,
@@ -3637,3 +4178,11 @@ void mesh_net_provisioned_set(struct mesh_net *net, bool provisioned)
acceptor_prov_receive, net);
}
}
+
+bool mesh_net_provisioned_get(struct mesh_net *net)
+{
+ if (!net)
+ return false;
+
+ return net->provisioned;
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 08/14] mesh: Header files for mesh access layer and utilities
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (6 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 07/14] mesh: Add Accessors to Transport layer data Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 09/14] mesh: Source " Brian Gix
` (6 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
This adds initial implementation of Mesh access layer functionality
---
mesh/agent.h | 42 ++++++++++++++++
mesh/appkey.h | 43 ++++++++++++++++
mesh/cfgmod.h | 98 +++++++++++++++++++++++++++++++++++++
mesh/mesh-defs.h | 84 ++++++++++++++++++++++++++++++++
mesh/mesh.h | 32 ++++++++++++
mesh/model.h | 146 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
mesh/node.h | 80 ++++++++++++++++++++++++++++++
mesh/storage.h | 51 +++++++++++++++++++
mesh/util.h | 24 +++++++++
9 files changed, 600 insertions(+)
create mode 100644 mesh/agent.h
create mode 100644 mesh/appkey.h
create mode 100644 mesh/cfgmod.h
create mode 100644 mesh/mesh-defs.h
create mode 100644 mesh/mesh.h
create mode 100644 mesh/model.h
create mode 100644 mesh/node.h
create mode 100644 mesh/storage.h
create mode 100644 mesh/util.h
diff --git a/mesh/agent.h b/mesh/agent.h
new file mode 100644
index 000000000..6fb475691
--- /dev/null
+++ b/mesh/agent.h
@@ -0,0 +1,42 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017 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.
+ *
+ *
+ */
+
+#define MAX_HEXADECIMAL_OOB_LEN 128
+#define DECIMAL_OOB_LEN 4
+#define MAX_ASCII_OOB_LEN 16
+
+enum oob_type {
+ NONE,
+ HEXADECIMAL,
+ DECIMAL,
+ ASCII,
+ OUTPUT,
+} oob_type_t;
+
+typedef void (*agent_input_cb)(enum oob_type type, void *input, uint16_t len,
+ void *user_data);
+bool agent_input_request(enum oob_type type, uint16_t max_len,
+ agent_input_cb cb, void *user_data);
+
+bool agent_output_request(const char *str);
+void agent_output_request_cancel(void);
+bool agent_completion(void);
+bool agent_input(const char *input);
+void agent_release(void);
diff --git a/mesh/appkey.h b/mesh/appkey.h
new file mode 100644
index 000000000..8fce3dcd0
--- /dev/null
+++ b/mesh/appkey.h
@@ -0,0 +1,43 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+/* TODO: get this number from configuration */
+#define MAX_APP_KEYS 32
+
+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);
+void appkey_key_free(void *data);
+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);
+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);
+const uint8_t *appkey_get_key(struct mesh_net *net, uint16_t app_idx,
+ uint8_t *key_id);
+bool appkey_have_key(struct mesh_net *net, uint16_t app_idx);
+int appkey_key_add(struct mesh_net *net, uint16_t net_idx, uint16_t app_idx,
+ const uint8_t *new_key, bool update);
+int appkey_key_delete(struct mesh_net *net, uint16_t net_idx, uint16_t app_idx);
+void appkey_delete_bound_keys(struct mesh_net *net, uint16_t net_idx);
+uint8_t appkey_list(struct mesh_net *net, uint16_t net_idx, uint8_t *buf,
+ uint16_t buf_size, uint16_t *size);
diff --git a/mesh/cfgmod.h b/mesh/cfgmod.h
new file mode 100644
index 000000000..bedb0c6f6
--- /dev/null
+++ b/mesh/cfgmod.h
@@ -0,0 +1,98 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+#define CONFIG_SRV_MODEL (VENDOR_ID_MASK | 0x0000)
+#define CONFIG_CLI_MODEL (VENDOR_ID_MASK | 0x0001)
+
+/* New List */
+#define OP_APPKEY_ADD 0x00
+#define OP_APPKEY_DELETE 0x8000
+#define OP_APPKEY_GET 0x8001
+#define OP_APPKEY_LIST 0x8002
+#define OP_APPKEY_STATUS 0x8003
+#define OP_APPKEY_UPDATE 0x01
+#define OP_DEV_COMP_GET 0x8008
+#define OP_DEV_COMP_STATUS 0x02
+#define OP_CONFIG_BEACON_GET 0x8009
+#define OP_CONFIG_BEACON_SET 0x800A
+#define OP_CONFIG_BEACON_STATUS 0x800B
+#define OP_CONFIG_DEFAULT_TTL_GET 0x800C
+#define OP_CONFIG_DEFAULT_TTL_SET 0x800D
+#define OP_CONFIG_DEFAULT_TTL_STATUS 0x800E
+#define OP_CONFIG_FRIEND_GET 0x800F
+#define OP_CONFIG_FRIEND_SET 0x8010
+#define OP_CONFIG_FRIEND_STATUS 0x8011
+#define OP_CONFIG_PROXY_GET 0x8012
+#define OP_CONFIG_PROXY_SET 0x8013
+#define OP_CONFIG_PROXY_STATUS 0x8014
+#define OP_CONFIG_KEY_REFRESH_PHASE_GET 0x8015
+#define OP_CONFIG_KEY_REFRESH_PHASE_SET 0x8016
+#define OP_CONFIG_KEY_REFRESH_PHASE_STATUS 0x8017
+#define OP_CONFIG_MODEL_PUB_GET 0x8018
+#define OP_CONFIG_MODEL_PUB_SET 0x03
+#define OP_CONFIG_MODEL_PUB_STATUS 0x8019
+#define OP_CONFIG_MODEL_PUB_VIRT_SET 0x801A
+#define OP_CONFIG_MODEL_SUB_ADD 0x801B
+#define OP_CONFIG_MODEL_SUB_DELETE 0x801C
+#define OP_CONFIG_MODEL_SUB_DELETE_ALL 0x801D
+#define OP_CONFIG_MODEL_SUB_OVERWRITE 0x801E
+#define OP_CONFIG_MODEL_SUB_STATUS 0x801F
+#define OP_CONFIG_MODEL_SUB_VIRT_ADD 0x8020
+#define OP_CONFIG_MODEL_SUB_VIRT_DELETE 0x8021
+#define OP_CONFIG_MODEL_SUB_VIRT_OVERWRITE 0x8022
+#define OP_CONFIG_NETWORK_TRANSMIT_GET 0x8023
+#define OP_CONFIG_NETWORK_TRANSMIT_SET 0x8024
+#define OP_CONFIG_NETWORK_TRANSMIT_STATUS 0x8025
+#define OP_CONFIG_RELAY_GET 0x8026
+#define OP_CONFIG_RELAY_SET 0x8027
+#define OP_CONFIG_RELAY_STATUS 0x8028
+#define OP_CONFIG_MODEL_SUB_GET 0x8029
+#define OP_CONFIG_MODEL_SUB_LIST 0x802A
+#define OP_CONFIG_VEND_MODEL_SUB_GET 0x802B
+#define OP_CONFIG_VEND_MODEL_SUB_LIST 0x802C
+#define OP_CONFIG_POLL_TIMEOUT_LIST 0x802D
+#define OP_CONFIG_POLL_TIMEOUT_STATUS 0x802E
+/* Health opcodes in health-mod.h */
+#define OP_CONFIG_HEARTBEAT_PUB_GET 0x8038
+#define OP_CONFIG_HEARTBEAT_PUB_SET 0x8039
+#define OP_CONFIG_HEARTBEAT_PUB_STATUS 0x06
+#define OP_CONFIG_HEARTBEAT_SUB_GET 0x803A
+#define OP_CONFIG_HEARTBEAT_SUB_SET 0x803B
+#define OP_CONFIG_HEARTBEAT_SUB_STATUS 0x803C
+#define OP_MODEL_APP_BIND 0x803D
+#define OP_MODEL_APP_STATUS 0x803E
+#define OP_MODEL_APP_UNBIND 0x803F
+#define OP_NETKEY_ADD 0x8040
+#define OP_NETKEY_DELETE 0x8041
+#define OP_NETKEY_GET 0x8042
+#define OP_NETKEY_LIST 0x8043
+#define OP_NETKEY_STATUS 0x8044
+#define OP_NETKEY_UPDATE 0x8045
+#define OP_NODE_IDENTITY_GET 0x8046
+#define OP_NODE_IDENTITY_SET 0x8047
+#define OP_NODE_IDENTITY_STATUS 0x8048
+#define OP_NODE_RESET 0x8049
+#define OP_NODE_RESET_STATUS 0x804A
+#define OP_MODEL_APP_GET 0x804B
+#define OP_MODEL_APP_LIST 0x804C
+#define OP_VEND_MODEL_APP_GET 0x804C
+#define OP_VEND_MODEL_APP_LIST 0x804E
+
+void mesh_config_srv_init(struct mesh_net *net, uint8_t ele_idx);
diff --git a/mesh/mesh-defs.h b/mesh/mesh-defs.h
new file mode 100644
index 000000000..d40fc43e8
--- /dev/null
+++ b/mesh/mesh-defs.h
@@ -0,0 +1,84 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+#define MESH_AD_TYPE_PROVISION 0x29
+#define MESH_AD_TYPE_NETWORK 0x2A
+#define MESH_AD_TYPE_BEACON 0x2B
+
+#define FEATURE_RELAY 1
+#define FEATURE_PROXY 2
+#define FEATURE_FRIEND 4
+#define FEATURE_LPN 8
+
+#define MESH_MODE_DISABLED 0
+#define MESH_MODE_ENABLED 1
+#define MESH_MODE_UNSUPPORTED 2
+
+#define KEY_REFRESH_PHASE_NONE 0x00
+#define KEY_REFRESH_PHASE_ONE 0x01
+#define KEY_REFRESH_PHASE_TWO 0x02
+#define KEY_REFRESH_PHASE_THREE 0x03
+
+#define DEFAULT_TTL 0xff
+
+/* Supported algorithms for provisioning */
+#define ALG_FIPS_256_ECC 0x0001
+
+/* Input OOB action bit flags */
+#define OOB_IN_PUSH 0x0001
+#define OOB_IN_TWIST 0x0002
+#define OOB_IN_NUMBER 0x0004
+#define OOB_IN_ALPHA 0x0008
+
+/* Output OOB action bit flags */
+#define OOB_OUT_BLINK 0x0001
+#define OOB_OUT_BEEP 0x0002
+#define OOB_OUT_VIBRATE 0x0004
+#define OOB_OUT_NUMBER 0x0008
+#define OOB_OUT_ALPHA 0x0010
+
+#define UNASSIGNED_ADDRESS 0x0000
+#define PROXIES_ADDRESS 0xfffc
+#define FRIENDS_ADDRESS 0xfffd
+#define RELAYS_ADDRESS 0xfffe
+#define ALL_NODES_ADDRESS 0xffff
+#define VIRTUAL_ADDRESS_LOW 0x8000
+#define VIRTUAL_ADDRESS_HIGH 0xbfff
+#define GROUP_ADDRESS_LOW 0xc000
+#define GROUP_ADDRESS_HIGH 0xff00
+
+#define NODE_IDENTITY_STOPPED 0x00
+#define NODE_IDENTITY_RUNNING 0x01
+#define NODE_IDENTITY_NOT_SUPPORTED 0x02
+
+#define PRIMARY_ELE_IDX 0x00
+
+#define VENDOR_ID_MASK 0xffff0000
+
+#define MAX_KEY_IDX 0x0fff
+
+#define IS_UNASSIGNED(x) ((x) == UNASSIGNED_ADDRESS)
+#define IS_UNICAST(x) (((x) > UNASSIGNED_ADDRESS) && \
+ ((x) < VIRTUAL_ADDRESS_LOW))
+#define IS_VIRTUAL(x) (((x) >= VIRTUAL_ADDRESS_LOW) && \
+ ((x) <= VIRTUAL_ADDRESS_HIGH))
+#define IS_GROUP(x) (((x) >= GROUP_ADDRESS_LOW) && \
+ ((x) <= GROUP_ADDRESS_HIGH))
+#define IS_ALL_NODES(x) ((x) == ALL_NODES_ADDRESS)
diff --git a/mesh/mesh.h b/mesh/mesh.h
new file mode 100644
index 000000000..7cd1e6158
--- /dev/null
+++ b/mesh/mesh.h
@@ -0,0 +1,32 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+struct bt_mesh;
+struct mesh_net;
+
+struct bt_mesh *mesh_create(uint16_t index);
+struct bt_mesh *mesh_ref(struct bt_mesh *mesh);
+void mesh_unref(struct bt_mesh *mesh);
+bool mesh_load_config(struct bt_mesh *mesh, const char *in_config_name);
+bool mesh_set_output(struct bt_mesh *mesh, const char *out_config_name);
+const char *mesh_status_str(uint8_t err);
+
+/* Command line testing */
+struct mesh_net *mesh_get_net(struct bt_mesh *mesh);
diff --git a/mesh/model.h b/mesh/model.h
new file mode 100644
index 000000000..3a41bd722
--- /dev/null
+++ b/mesh/model.h
@@ -0,0 +1,146 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+#include <ell/ell.h>
+
+struct mesh_model;
+
+#define OP_UNRELIABLE 0x0100
+
+#define MAX_BINDINGS 10
+#define MAX_GRP_PER_MOD 10
+
+#define VIRTUAL_BASE 0x10000
+
+#define MESH_MAX_ACCESS_PAYLOAD 380
+
+#define MESH_STATUS_SUCCESS 0x00
+#define MESH_STATUS_INVALID_ADDRESS 0x01
+#define MESH_STATUS_INVALID_MODEL 0x02
+#define MESH_STATUS_INVALID_APPKEY 0x03
+#define MESH_STATUS_INVALID_NETKEY 0x04
+#define MESH_STATUS_INSUFF_RESOURCES 0x05
+#define MESH_STATUS_IDX_ALREADY_STORED 0x06
+#define MESH_STATUS_INVALID_PUB_PARAM 0x07
+#define MESH_STATUS_NOT_SUB_MOD 0x08
+#define MESH_STATUS_STORAGE_FAIL 0x09
+#define MESH_STATUS_FEATURE_NO_SUPPORT 0x0a
+#define MESH_STATUS_CANNOT_UPDATE 0x0b
+#define MESH_STATUS_CANNOT_REMOVE 0x0c
+#define MESH_STATUS_CANNOT_BIND 0x0d
+#define MESH_STATUS_UNABLE_CHANGE_STATE 0x0e
+#define MESH_STATUS_CANNOT_SET 0x0f
+#define MESH_STATUS_UNSPECIFIED_ERROR 0x10
+#define MESH_STATUS_INVALID_BINDING 0x11
+
+#define OP_MODEL_TEST 0x8000fffe
+#define OP_MODEL_INVALID 0x8000ffff
+
+#define USE_PUB_VALUE 0x00
+
+#define ACTION_ADD 1
+#define ACTION_UPDATE 2
+#define ACTION_DELETE 3
+
+struct mesh_model_pub {
+ uint32_t addr;
+ uint16_t idx;
+ uint8_t ttl;
+ uint8_t credential;
+ uint8_t period;
+ uint8_t retransmit;
+};
+
+typedef void (*mesh_model_unregister)(void *user_data);
+typedef bool (*mesh_model_recv_cb)(uint16_t src, uint32_t dst, uint16_t unicast,
+ uint16_t app_idx, const uint8_t *data,
+ uint16_t len, uint8_t ttl,
+ const void *user_data);
+typedef int (*mesh_model_bind_cb)(uint16_t app_idx, int action);
+typedef int (*mesh_model_pub_cb)(struct mesh_model_pub *pub);
+typedef int (*mesh_model_sub_cb)(uint16_t sub_addr, int action);
+
+struct mesh_model_ops {
+ mesh_model_unregister unregister;
+ mesh_model_recv_cb recv;
+ mesh_model_bind_cb bind;
+ mesh_model_pub_cb pub;
+ mesh_model_sub_cb sub;
+};
+
+struct mesh_model *mesh_model_new(uint8_t ele_idx, uint32_t id, bool vendor);
+void mesh_model_free(void *data);
+uint32_t mesh_model_get_model_id(const struct mesh_model *model);
+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);
+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);
+struct mesh_model_pub *mesh_model_pub_get(struct mesh_net *net, uint8_t ele_idx,
+ uint32_t mod_id, int *status);
+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);
+struct mesh_model *mesh_model_init(struct mesh_net *net, uint8_t ele_idx,
+ struct mesh_db_model *db_mod);
+
+int mesh_model_binding_add(struct mesh_net *net, uint16_t addr, uint32_t id,
+ uint16_t app_idx);
+int mesh_model_binding_del(struct mesh_net *net, uint16_t addr, uint32_t id,
+ uint16_t idx);
+int mesh_model_get_bindings(struct mesh_net *net, uint16_t addr, uint32_t id,
+ uint8_t *buf, uint16_t buf_len, uint16_t *size);
+int mesh_model_sub_add(struct mesh_net *net, uint16_t addr, uint32_t id,
+ const uint8_t *grp, bool b_virt,
+ uint16_t *dst);
+int mesh_model_sub_del(struct mesh_net *net, uint16_t addr, uint32_t id,
+ const uint8_t *grp, bool b_virt,
+ uint16_t *dst);
+int mesh_model_sub_del_all(struct mesh_net *net, uint16_t addr, uint32_t id);
+int mesh_model_sub_ovr(struct mesh_net *net, uint16_t addr, uint32_t id,
+ const uint8_t *grp, bool b_virt,
+ uint16_t *dst);
+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);
+uint16_t mesh_model_cfg_blk(uint8_t *pkt);
+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);
+
+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);
+
+void mesh_model_app_key_generate_new(struct mesh_net *net, uint16_t net_idx);
+void mesh_model_app_key_delete(struct mesh_net *net, struct l_queue *models,
+ uint16_t idx);
+struct l_queue *mesh_model_get_appkeys(struct mesh_net *net);
+void *mesh_model_get_local_node_data(struct mesh_net *net);
+void mesh_model_add_virtual(struct mesh_net *net, const uint8_t *v);
+void mesh_model_del_virtual(struct mesh_net *net, uint32_t va24);
+void mesh_model_list_virtual(struct mesh_net *net);
+uint16_t mesh_model_opcode_set(uint32_t opcode, uint8_t *buf);
+bool mesh_model_opcode_get(const uint8_t *buf, uint16_t size,
+ uint32_t *opcode, uint16_t *n);
diff --git a/mesh/node.h b/mesh/node.h
new file mode 100644
index 000000000..f417fe503
--- /dev/null
+++ b/mesh/node.h
@@ -0,0 +1,80 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+#include "mesh/mesh-db.h"
+
+struct mesh_net;
+struct mesh_node;
+
+/* To prevent local node JSON cache thrashing, minimum update times */
+#define MIN_SEQ_TRIGGER 32
+#define MIN_SEQ_CACHE (2*MIN_SEQ_TRIGGER)
+#define MIN_SEQ_CACHE_TIME (5*60)
+
+struct mesh_node *node_new(void);
+void node_free(struct mesh_node *node);
+uint8_t *node_uuid_get(struct mesh_node *node);
+struct mesh_node *node_find_by_addr(uint16_t addr);
+struct mesh_node *node_find_by_uuid(uint8_t uuid[16]);
+bool node_is_provisioned(struct mesh_node *node);
+bool node_app_key_delete(struct mesh_net *net, uint16_t addr,
+ uint16_t net_idx, uint16_t idx);
+bool node_net_key_delete(struct mesh_node *node, uint16_t index);
+bool node_set_primary(struct mesh_node *node, uint16_t unicast);
+uint16_t node_get_primary(struct mesh_node *node);
+uint16_t node_get_primary_net_idx(struct mesh_node *node);
+bool node_set_device_key(struct mesh_node *node, uint8_t key[16]);
+const uint8_t *node_get_device_key(struct mesh_node *node);
+void node_set_num_elements(struct mesh_node *node, uint8_t num_ele);
+uint8_t node_get_num_elements(struct mesh_node *node);
+bool node_parse_composition(struct mesh_node *node, uint8_t *buf, uint16_t len);
+struct l_queue *node_get_net_keys(struct mesh_node *node);
+struct l_queue *node_get_app_keys(struct mesh_node *node);
+bool node_add_binding(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id, uint16_t app_idx);
+bool node_del_binding(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t model_id, uint16_t app_idx);
+uint8_t node_default_ttl_get(struct mesh_node *node);
+bool node_default_ttl_set(struct mesh_node *node, uint8_t ttl);
+bool node_set_sequence_number(struct mesh_node *node, uint32_t seq);
+uint32_t node_get_sequence_number(struct mesh_node *node);
+int node_get_element_idx(struct mesh_node *node, uint16_t ele_addr);
+struct l_queue *node_get_element_models(struct mesh_node *node, uint8_t ele_idx,
+ int *status);
+struct mesh_model *node_get_model(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t id, int *status);
+uint16_t node_get_crpl(struct mesh_node *node);
+struct mesh_node *node_create_from_storage(struct mesh_net *net,
+ struct mesh_db_node *db_node,
+ bool local);
+uint16_t node_generate_comp(struct mesh_node *node, uint8_t *buf, uint16_t sz);
+uint8_t node_lpn_mode_get(struct mesh_node *node);
+bool node_relay_mode_set(struct mesh_node *node, bool enable, uint8_t cnt,
+ uint16_t interval);
+uint8_t node_relay_mode_get(struct mesh_node *node, uint8_t *cnt,
+ uint16_t *interval);
+bool node_proxy_mode_set(struct mesh_node *node, bool enable);
+uint8_t node_proxy_mode_get(struct mesh_node *node);
+bool node_beacon_mode_set(struct mesh_node *node, bool enable);
+uint8_t node_beacon_mode_get(struct mesh_node *node);
+bool node_friend_mode_set(struct mesh_node *node, bool enable);
+uint8_t node_friend_mode_get(struct mesh_node *node);
+uint32_t node_seq_cache(struct mesh_node *node);
+void node_cleanup(struct mesh_net *net);
diff --git a/mesh/storage.h b/mesh/storage.h
new file mode 100644
index 000000000..341932dba
--- /dev/null
+++ b/mesh/storage.h
@@ -0,0 +1,51 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2017-2918 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.
+ *
+ *
+ */
+
+struct mesh_net;
+
+bool storage_parse_config(struct mesh_net *net, const char *config_name);
+bool storage_save_config(struct mesh_net *net, const char *config_name,
+ bool no_wait, mesh_status_func_t cb, void *user_data);
+bool storage_save_new_config(struct mesh_net *net, const char *config_name,
+ mesh_status_func_t cb, void *user_data);
+void storage_release(struct mesh_net *net);
+
+bool storage_model_bind(struct mesh_net *net, uint16_t addr, uint32_t id,
+ uint16_t app_idx, bool unbind);
+
+bool storage_local_set_ttl(struct mesh_net *net, uint8_t ttl);
+bool storage_local_set_relay(struct mesh_net *net, bool enable, uint8_t count,
+ uint8_t interval);
+bool storage_local_set_transmit_params(struct mesh_net *net, uint8_t count,
+ uint8_t interval);
+bool storage_local_set_mode(struct mesh_net *net, uint8_t mode,
+ const char *mode_name);
+bool storage_local_net_key_add(struct mesh_net *net, uint16_t net_idx,
+ const uint8_t key[16], int phase);
+bool storage_local_net_key_del(struct mesh_net *net, uint16_t net_idx);
+bool storage_local_app_key_add(struct mesh_net *net, uint16_t net_idx,
+ uint16_t app_idx, const uint8_t key[16], bool update);
+bool storage_local_app_key_del(struct mesh_net *net, uint16_t net_idx,
+ uint16_t app_idx);
+bool storage_local_write_sequence_number(struct mesh_net *net, uint32_t seq);
+bool storage_local_set_iv_index(struct mesh_net *net, uint32_t iv_index,
+ bool update);
+bool storage_local_set_device_key(struct mesh_net *net, uint8_t dev_key[16]);
+bool storage_local_set_unicast(struct mesh_net *net, uint16_t unicast);
diff --git a/mesh/util.h b/mesh/util.h
new file mode 100644
index 000000000..61110104a
--- /dev/null
+++ b/mesh/util.h
@@ -0,0 +1,24 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+uint32_t get_timestamp_secs(void);
+bool str2hex(const char *str, uint16_t in_len, uint8_t *out,
+ uint16_t out_len);
+size_t hex2str(uint8_t *in, size_t in_len, char *out, size_t out_len);
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 09/14] mesh: Source files for mesh access layer and utilities
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (7 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 08/14] mesh: Header files for mesh access layer and utilities Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:58 ` [PATCH BlueZ v7 10/14] mesh: Source code for handling access layer mux/demux Brian Gix
` (5 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
This adds initial implementation of BT Mesh access layer
functionality plus utilities.
---
mesh/agent.c | 229 ++++++++++++++++
mesh/appkey.c | 536 ++++++++++++++++++++++++++++++++++++
mesh/btmesh.c | 176 ++++++++++++
mesh/main.c | 174 ++++++++++++
mesh/mesh.c | 184 +++++++++++++
mesh/node.c | 851 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
mesh/storage.c | 672 +++++++++++++++++++++++++++++++++++++++++++++
mesh/util.c | 71 +++++
8 files changed, 2893 insertions(+)
create mode 100644 mesh/agent.c
create mode 100644 mesh/appkey.c
create mode 100644 mesh/btmesh.c
create mode 100644 mesh/main.c
create mode 100644 mesh/mesh.c
create mode 100644 mesh/node.c
create mode 100644 mesh/storage.c
create mode 100644 mesh/util.c
diff --git a/mesh/agent.c b/mesh/agent.c
new file mode 100644
index 000000000..1da10b7bd
--- /dev/null
+++ b/mesh/agent.c
@@ -0,0 +1,229 @@
+/*
+ *
+ * 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 <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include <ell/ell.h>
+
+#include "src/shared/shell.h"
+
+#include "mesh/util.h"
+#include "mesh/agent.h"
+
+struct input_request {
+ enum oob_type type;
+ uint16_t len;
+ agent_input_cb cb;
+ void *user_data;
+};
+
+static struct input_request pending_request = {NONE, 0, NULL, NULL};
+
+bool agent_completion(void)
+{
+ if (pending_request.type == NONE)
+ return false;
+
+ return true;
+}
+
+static void reset_input_request(void)
+{
+ pending_request.type = NONE;
+ pending_request.len = 0;
+ pending_request.cb = NULL;
+ pending_request.user_data = NULL;
+}
+
+static void try_again(void)
+{
+ static int try_count;
+ enum oob_type type = pending_request.type;
+
+ if (try_count == 2) {
+ reset_input_request();
+ try_count = 0;
+ return;
+ }
+
+ pending_request.type = NONE;
+ agent_input_request(type, pending_request.len, pending_request.cb,
+ pending_request.user_data);
+
+ try_count++;
+}
+
+static void response_hexadecimal(const char *input, void *user_data)
+{
+ uint8_t buf[MAX_HEXADECIMAL_OOB_LEN];
+
+ if (!str2hex(input, strlen(input), buf, pending_request.len)) {
+ bt_shell_printf("Incorrect input: expecting %d hex octets\n",
+ pending_request.len);
+ try_again();
+ return;
+ }
+
+ if (pending_request.cb)
+ pending_request.cb(HEXADECIMAL, buf, pending_request.len,
+ pending_request.user_data);
+
+ reset_input_request();
+}
+
+static void response_decimal(const char *input, void *user_data)
+{
+ uint8_t buf[DECIMAL_OOB_LEN];
+
+ if (strlen(input) > pending_request.len) {
+ bt_shell_printf("Bad input: expected no more than %d digits\n",
+ pending_request.len);
+ try_again();
+ return;
+ }
+
+ l_put_be32(atoi(input), buf);
+
+ if (pending_request.cb)
+ pending_request.cb(DECIMAL, buf, DECIMAL_OOB_LEN,
+ pending_request.user_data);
+
+ reset_input_request();
+}
+
+static void response_ascii(const char *input, void *user_data)
+{
+ if (pending_request.cb)
+ pending_request.cb(ASCII, (uint8_t *) input, strlen(input),
+ pending_request.user_data);
+
+ reset_input_request();
+}
+
+static bool request_hexadecimal(uint16_t len)
+{
+ if (len > MAX_HEXADECIMAL_OOB_LEN)
+ return false;
+
+ bt_shell_printf("Request hexadecimal key (hex %d octets)\n", len);
+ bt_shell_prompt_input("mesh", "Enter key (hex number):",
+ response_hexadecimal, NULL);
+
+ return true;
+}
+
+static uint32_t power_ten(uint8_t power)
+{
+ uint32_t ret = 1;
+
+ while (power--)
+ ret *= 10;
+
+ return ret;
+}
+
+static bool request_decimal(uint16_t len)
+{
+ bt_shell_printf("Request decimal key (0 - %d)\n", power_ten(len) - 1);
+ bt_shell_prompt_input("mesh", "Enter Numeric key:", response_decimal,
+ NULL);
+
+ return true;
+}
+
+static bool request_ascii(uint16_t len)
+{
+ if (len > MAX_ASCII_OOB_LEN)
+ return false;
+
+ bt_shell_printf("Request ASCII key (max characters %d)\n", len);
+ bt_shell_prompt_input("mesh", "Enter key (ascii string):",
+ response_ascii, NULL);
+
+ return true;
+}
+
+bool agent_input_request(enum oob_type type, uint16_t max_len,
+ agent_input_cb cb, void *user_data)
+{
+ bool result;
+
+ if (pending_request.type != NONE)
+ return false;
+
+ switch (type) {
+ case HEXADECIMAL:
+ result = request_hexadecimal(max_len);
+ break;
+ case DECIMAL:
+ result = request_decimal(max_len);
+ break;
+ case ASCII:
+ result = request_ascii(max_len);
+ break;
+ case NONE:
+ case OUTPUT:
+ default:
+ return false;
+ };
+
+ if (result) {
+ pending_request.type = type;
+ pending_request.len = max_len;
+ pending_request.cb = cb;
+ pending_request.user_data = user_data;
+
+ return true;
+ }
+
+ return false;
+}
+
+static void response_output(const char *input, void *user_data)
+{
+ reset_input_request();
+}
+
+bool agent_output_request(const char *str)
+{
+ if (pending_request.type != NONE)
+ return false;
+
+ pending_request.type = OUTPUT;
+ bt_shell_prompt_input("mesh", str, response_output, NULL);
+ return true;
+}
+
+void agent_output_request_cancel(void)
+{
+ if (pending_request.type != OUTPUT)
+ return;
+
+ pending_request.type = NONE;
+ bt_shell_release_prompt("");
+}
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;
+}
diff --git a/mesh/btmesh.c b/mesh/btmesh.c
new file mode 100644
index 000000000..c312d85db
--- /dev/null
+++ b/mesh/btmesh.c
@@ -0,0 +1,176 @@
+/*
+ *
+ * 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 <ctype.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <ell/ell.h>
+
+#include "src/shared/shell.h"
+#include "src/shared/mainloop.h"
+
+#include "mesh/mesh.h"
+#include "mesh/net.h"
+
+#define PROMPT COLOR_BLUE "[btmesh]" COLOR_OFF "# "
+
+static struct bt_mesh *mesh;
+
+static const struct option main_options[] = {
+ { "index", 1, 0, 'i' },
+ { "config", 1, 0, 'c' },
+ { "save", 1, 0, 's' },
+ { 0, 0, 0, 0 }
+};
+
+static const char *index_option;
+static const char *config_option;
+static const char *save_option;
+
+static const char **optargs[] = {
+ &index_option,
+ &config_option,
+ &save_option,
+};
+
+static const char *help[] = {
+ "Specify adapter index",
+ "Specify input configuration file",
+ "Specify output configuration file"
+};
+
+static const struct bt_shell_opt opt = {
+ .options = main_options,
+ .optno = sizeof(main_options) / sizeof(struct option),
+ .optstr = "i:c:s:",
+ .optarg = optargs,
+ .help = help,
+};
+
+static int get_arg_on_off(int argc, char *argv[])
+{
+ if (!strcmp(argv[1], "on") || !strcmp(argv[1], "yes"))
+ return 1;
+
+ if (!strcmp(argv[1], "off") || !strcmp(argv[1], "no"))
+ return 0;
+
+ bt_shell_printf("Invalid argument %s\n", argv[1]);
+ return -1;
+}
+
+static void cmd_beacon(int argc, char *argv[])
+{
+ bool res;
+ int enable;
+
+ enable = get_arg_on_off(argc, argv);
+ if (enable < 0)
+ return;
+
+ res = mesh_net_set_beacon_mode(mesh_get_net(mesh), enable);
+ if (res)
+ bt_shell_printf("Local beacon mode is %s\n",
+ enable > 0 ? "enabled" : "disabled");
+ else
+ bt_shell_printf("Failed to set local beacon mode to %s\n",
+ enable > 0 ? "enabled" : "disabled");
+}
+
+static const struct bt_shell_menu main_menu = {
+ .name = "main",
+ .entries = {
+ { "beacon", "<enable>", cmd_beacon, "Enable/disable beaconing"},
+ { } },
+};
+
+static int get_index(const char *arg)
+{
+ if (strlen(arg) > 3 && !strncasecmp(arg, "hci", 3))
+ return atoi(&arg[3]);
+ else
+ return atoi(arg);
+}
+
+static void ell_event(int fd, uint32_t events, void *user_data)
+{
+ int timeout = l_main_prepare();
+
+ l_main_iterate(timeout);
+}
+
+int main(int argc, char *argv[])
+{
+ int index;
+ int fd;
+ int status;
+
+ l_log_set_stderr();
+ l_debug_enable("*");
+
+ if (!l_main_init())
+ return -1;
+
+ bt_shell_init(argc, argv, &opt);
+ bt_shell_set_menu(&main_menu);
+
+ if (!index_option) {
+ bt_shell_usage();
+ return 0;
+ }
+
+ if (config_option)
+ l_info("Reading local configuration from %s\n", config_option);
+
+ if (save_option)
+ l_info("Saving local configuration to %s\n", save_option);
+
+ bt_shell_set_prompt(PROMPT);
+
+ index = get_index(index_option);
+
+ l_info("Starting mesh on hci%d\n", index);
+
+ mesh = mesh_create(index);
+ if (!mesh || !mesh_load_config(mesh, config_option)) {
+ l_info("Failed to create mesh\n");
+ bt_shell_cleanup();
+ return EXIT_FAILURE;
+ }
+
+ if (save_option)
+ mesh_set_output(mesh, save_option);
+
+ fd = l_main_get_epoll_fd();
+ mainloop_add_fd(fd, EPOLLIN, ell_event, NULL, NULL);
+
+ status = bt_shell_attach(fileno(stdin));
+ bt_shell_run();
+
+ mesh_unref(mesh);
+ l_main_exit();
+
+ return status;
+}
diff --git a/mesh/main.c b/mesh/main.c
new file mode 100644
index 000000000..8c03f51eb
--- /dev/null
+++ b/mesh/main.c
@@ -0,0 +1,174 @@
+/*
+ *
+ * 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 <getopt.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <ctype.h>
+
+#include <sys/stat.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh.h"
+#include "mesh/net.h"
+#include "mesh/storage.h"
+
+static const struct option main_options[] = {
+ { "index", required_argument, NULL, 'i' },
+ { "config", optional_argument, NULL, 'c' },
+ { "nodetach", no_argument, NULL, 'n' },
+ { "debug", no_argument, NULL, 'd' },
+ { "help", no_argument, NULL, 'h' },
+ { }
+};
+
+static void usage(void)
+{
+ l_info("");
+ l_info("Usage:\n"
+ "\tmeshd [options]\n");
+ l_info("Options:\n"
+ "\t--index <hcinum> Use specified controller\n"
+ "\t--config Configuration file\n"
+ "\t--nodetach Run in foreground\n"
+ "\t--debug Enable debug output\n"
+ "\t--help Show %s information\n", __func__);
+}
+
+static void signal_handler(struct l_signal *signal, uint32_t signo,
+ void *user_data)
+{
+ static bool terminated;
+
+ switch (signo) {
+ case SIGINT:
+ case SIGTERM:
+ if (terminated)
+ return;
+ l_info("Terminating");
+ l_main_quit();
+ terminated = true;
+ break;
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ int status;
+ bool detached = true;
+ struct l_signal *signal;
+ sigset_t mask;
+ struct bt_mesh *mesh = NULL;
+ const char *config_file = NULL;
+
+ if (!l_main_init())
+ return -1;
+
+ l_log_set_stderr();
+
+ for (;;) {
+ int opt;
+ const char *str;
+
+ opt = getopt_long(argc, argv, "i:c:ndh", main_options, NULL);
+ if (opt < 0)
+ break;
+
+ switch (opt) {
+ case 'i':
+ if (strlen(optarg) > 3 && !strncmp(optarg, "hci", 3))
+ str = optarg + 3;
+ else
+ str = optarg;
+ if (!isdigit(*str)) {
+ l_error("Invalid controller index value");
+ status = EXIT_FAILURE;
+ goto done;
+ }
+
+ mesh = mesh_create(atoi(str));
+ if (!mesh) {
+ l_error("Failed to initialize mesh");
+ status = EXIT_FAILURE;
+ goto done;
+ }
+
+ break;
+ case 'n':
+ detached = false;
+ break;
+ case 'd':
+ l_debug_enable("*");
+ break;
+ case 'c':
+ config_file = optarg;
+ break;
+ case 'h':
+ usage();
+ status = EXIT_SUCCESS;
+ goto done;
+ default:
+ usage();
+ status = EXIT_FAILURE;
+ goto done;
+ }
+ }
+
+ if (!mesh) {
+ usage();
+ status = EXIT_FAILURE;
+ goto done;
+ }
+
+ if (!mesh_load_config(mesh, config_file)) {
+ l_error("Failed to load mesh configuration: %s", config_file);
+ status = EXIT_FAILURE;
+ goto done;
+ }
+
+ sigemptyset(&mask);
+ sigaddset(&mask, SIGINT);
+ sigaddset(&mask, SIGTERM);
+ signal = l_signal_create(&mask, signal_handler, NULL, NULL);
+
+ umask(0077);
+
+ if (detached) {
+ if (daemon(0, 0)) {
+ perror("Failed to start meshd daemon");
+ status = EXIT_FAILURE;
+ goto done;
+ }
+ }
+
+ status = l_main_run();
+
+ l_signal_remove(signal);
+
+done:
+ mesh_unref(mesh);
+ l_main_exit();
+
+ return status;
+}
diff --git a/mesh/mesh.c b/mesh/mesh.c
new file mode 100644
index 000000000..a6f733f5c
--- /dev/null
+++ b/mesh/mesh.c
@@ -0,0 +1,184 @@
+/*
+ *
+ * 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 <time.h>
+#include <ell/ell.h>
+
+#include "lib/bluetooth.h"
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh-io.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/storage.h"
+#include "mesh/cfgmod.h"
+#include "mesh/model.h"
+#include "mesh/mesh.h"
+
+struct scan_filter {
+ uint8_t id;
+ const char *pattern;
+};
+
+struct bt_mesh {
+ struct mesh_net *net;
+ int ref_count;
+ struct l_queue *filters;
+ uint8_t max_filters;
+};
+
+static void save_exit_config(struct bt_mesh *mesh)
+{
+ const char *cfg_filename;
+
+ if (!mesh_net_cfg_file_get(mesh->net, &cfg_filename) || !cfg_filename)
+ return;
+
+ /* Preserve the last sequence number before saving configuration */
+ storage_local_write_sequence_number(mesh->net,
+ mesh_net_get_seq_num(mesh->net));
+
+ if (storage_save_config(mesh->net, cfg_filename, true, NULL, NULL))
+ l_info("Saved final configuration to %s", cfg_filename);
+}
+
+struct bt_mesh *mesh_create(uint16_t index)
+{
+ struct bt_mesh *mesh;
+ struct mesh_io *io;
+ struct mesh_io_caps caps;
+
+ mesh = l_new(struct bt_mesh, 1);
+ if (!mesh)
+ return NULL;
+
+ mesh->net = mesh_net_new(index);
+ if (!mesh->net) {
+ l_free(mesh);
+ return NULL;
+ }
+
+ io = mesh_io_new(index, MESH_IO_TYPE_GENERIC);
+ if (!io) {
+ mesh_net_unref(mesh->net);
+ l_free(mesh);
+ return NULL;
+ }
+
+ mesh_io_get_caps(io, &caps);
+ mesh->max_filters = caps.max_num_filters;
+
+ mesh_net_attach(mesh->net, io);
+ mesh_net_set_window_accuracy(mesh->net, caps.window_accuracy);
+
+ return mesh_ref(mesh);
+}
+
+struct bt_mesh *mesh_ref(struct bt_mesh *mesh)
+{
+ if (!mesh)
+ return NULL;
+
+ __sync_fetch_and_add(&mesh->ref_count, 1);
+
+ return mesh;
+}
+
+void mesh_unref(struct bt_mesh *mesh)
+{
+ struct mesh_io *io;
+
+ if (!mesh)
+ return;
+
+ if (__sync_sub_and_fetch(&mesh->ref_count, 1))
+ return;
+
+ if (mesh_net_provisioned_get(mesh->net))
+ save_exit_config(mesh);
+
+ node_cleanup(mesh->net);
+
+ storage_release(mesh->net);
+ io = mesh_net_detach(mesh->net);
+ if (io)
+ mesh_io_destroy(io);
+
+ mesh_net_unref(mesh->net);
+ l_free(mesh);
+}
+
+bool mesh_load_config(struct bt_mesh *mesh, const char *in_config_name)
+{
+ if (!storage_parse_config(mesh->net, in_config_name))
+ return false;
+
+ /* Register foundational models */
+ mesh_config_srv_init(mesh->net, PRIMARY_ELE_IDX);
+
+ return true;
+}
+
+bool mesh_set_output(struct bt_mesh *mesh, const char *config_name)
+{
+ if (!config_name)
+ return false;
+
+ return mesh_net_cfg_file_set(mesh->net, config_name);
+}
+
+const char *mesh_status_str(uint8_t err)
+{
+ switch (err) {
+ case MESH_STATUS_SUCCESS: return "Success";
+ case MESH_STATUS_INVALID_ADDRESS: return "Invalid Address";
+ case MESH_STATUS_INVALID_MODEL: return "Invalid Model";
+ case MESH_STATUS_INVALID_APPKEY: return "Invalid AppKey";
+ case MESH_STATUS_INVALID_NETKEY: return "Invalid NetKey";
+ case MESH_STATUS_INSUFF_RESOURCES: return "Insufficient Resources";
+ case MESH_STATUS_IDX_ALREADY_STORED: return "Key Idx Already Stored";
+ case MESH_STATUS_INVALID_PUB_PARAM: return "Invalid Publish Parameters";
+ case MESH_STATUS_NOT_SUB_MOD: return "Not a Subscribe Model";
+ case MESH_STATUS_STORAGE_FAIL: return "Storage Failure";
+ case MESH_STATUS_FEATURE_NO_SUPPORT: return "Feature Not Supported";
+ case MESH_STATUS_CANNOT_UPDATE: return "Cannot Update";
+ case MESH_STATUS_CANNOT_REMOVE: return "Cannot Remove";
+ case MESH_STATUS_CANNOT_BIND: return "Cannot bind";
+ case MESH_STATUS_UNABLE_CHANGE_STATE: return "Unable to change state";
+ case MESH_STATUS_CANNOT_SET: return "Cannot set";
+ case MESH_STATUS_UNSPECIFIED_ERROR: return "Unspecified error";
+ case MESH_STATUS_INVALID_BINDING: return "Invalid Binding";
+
+ default: return "Unknown";
+ }
+}
+
+struct mesh_net *mesh_get_net(struct bt_mesh *mesh)
+{
+ if (!mesh)
+ return NULL;
+
+ return mesh->net;
+}
diff --git a/mesh/node.c b/mesh/node.c
new file mode 100644
index 000000000..501ab8eea
--- /dev/null
+++ b/mesh/node.c
@@ -0,0 +1,851 @@
+/*
+ *
+ * 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 <sys/time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/net.h"
+#include "mesh/node.h"
+#include "mesh/storage.h"
+#include "mesh/appkey.h"
+#include "mesh/model.h"
+
+#define MIN_COMP_SIZE 14
+
+struct node_element {
+ struct l_queue *models;
+ uint16_t location;
+ uint8_t idx;
+};
+
+struct node_composition {
+ uint16_t cid;
+ uint16_t pid;
+ uint16_t vid;
+ uint16_t crpl;
+};
+
+struct mesh_node {
+ struct mesh_net *net;
+ struct l_queue *net_keys;
+ struct l_queue *app_keys;
+ struct l_queue *elements;
+ time_t upd_sec;
+ uint32_t seq_number;
+ uint32_t seq_min_cache;
+ uint16_t primary;
+ uint16_t num_ele;
+ uint8_t dev_uuid[16];
+ uint8_t dev_key[16];
+ uint8_t ttl;
+ bool provisioner;
+ struct node_composition *comp;
+ struct {
+ uint16_t interval;
+ uint8_t cnt;
+ uint8_t mode;
+ } relay;
+ uint8_t lpn;
+ uint8_t proxy;
+ uint8_t friend;
+ uint8_t beacon;
+};
+
+static struct l_queue *nodes;
+
+static bool match_node_unicast(const void *a, const void *b)
+{
+ const struct mesh_node *node = a;
+ uint16_t dst = L_PTR_TO_UINT(b);
+
+ return (dst >= node->primary &&
+ dst <= (node->primary + node->num_ele - 1));
+}
+
+static bool match_device_uuid(const void *a, const void *b)
+{
+ const struct mesh_node *node = a;
+ const uint8_t *uuid = b;
+
+ return (memcmp(node->dev_uuid, uuid, 16) == 0);
+}
+
+static bool match_element_idx(const void *a, const void *b)
+{
+ const struct node_element *element = a;
+ uint32_t index = L_PTR_TO_UINT(b);
+
+ return (element->idx == index);
+}
+
+static bool match_key_idx(const void *a, const void *b)
+{
+ return (L_PTR_TO_UINT(a) == L_PTR_TO_UINT(b));
+}
+
+static bool match_model_id(const void *a, const void *b)
+{
+ const struct mesh_model *model = a;
+ uint32_t id = L_PTR_TO_UINT(b);
+
+ return (mesh_model_get_model_id(model) == id);
+}
+
+struct mesh_node *node_find_by_addr(uint16_t addr)
+{
+ if (!IS_UNICAST(addr))
+ return NULL;
+
+ return l_queue_find(nodes, match_node_unicast, L_UINT_TO_PTR(addr));
+}
+
+struct mesh_node *node_find_by_uuid(uint8_t uuid[16])
+{
+ return l_queue_find(nodes, match_device_uuid, uuid);
+}
+
+uint8_t *node_uuid_get(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ return node->dev_uuid;
+}
+
+struct mesh_node *node_new(void)
+{
+ struct mesh_node *node;
+
+ node = l_new(struct mesh_node, 1);
+
+ if (!node)
+ return NULL;
+
+ l_queue_push_tail(nodes, node);
+
+ return node;
+}
+
+static void element_free(void *data)
+{
+ struct node_element *element = data;
+
+ l_queue_destroy(element->models, mesh_model_free);
+ l_free(element);
+}
+
+static void free_node_resources(void *data)
+{
+ struct mesh_node *node = data;
+
+ l_queue_destroy(node->net_keys, NULL);
+ l_queue_destroy(node->app_keys, NULL);
+ l_queue_destroy(node->elements, element_free);
+ l_free(node->comp);
+
+ if (node->net)
+ mesh_net_unref(node->net);
+
+ l_free(node);
+}
+
+void node_free(struct mesh_node *node)
+{
+ if (!node)
+ return;
+ l_queue_remove(nodes, node);
+ free_node_resources(node);
+}
+
+static bool add_models(struct mesh_net *net, struct node_element *ele,
+ struct mesh_db_element *db_ele)
+{
+ const struct l_queue_entry *entry;
+
+ if (!ele->models)
+ ele->models = l_queue_new();
+ if (!ele->models)
+ return false;
+
+ entry = l_queue_get_entries(db_ele->models);
+ for (; entry; entry = entry->next) {
+ struct mesh_model *mod;
+ struct mesh_db_model *db_mod;
+
+ db_mod = entry->data;
+ mod = mesh_model_init(net, ele->idx, db_mod);
+ if (!mod)
+ return false;
+
+ l_queue_push_tail(ele->models, mod);
+ }
+
+ return true;
+}
+
+static bool add_element(struct mesh_node *node, struct mesh_db_element *db_ele)
+{
+ struct node_element *ele;
+
+ ele = l_new(struct node_element, 1);
+ if (!ele)
+ return false;
+
+ ele->idx = db_ele->index;
+ ele->location = db_ele->location;
+
+ if (!db_ele->models || !add_models(node->net, ele, db_ele))
+ return false;
+
+ l_queue_push_tail(node->elements, ele);
+ return true;
+}
+
+static bool add_elements(struct mesh_node *node, struct mesh_db_node *db_node)
+{
+ const struct l_queue_entry *entry;
+
+ if (!node->elements)
+ node->elements = l_queue_new();
+
+ if (!node->elements)
+ return false;
+
+ entry = l_queue_get_entries(db_node->elements);
+ for (; entry; entry = entry->next)
+ if (!add_element(node, entry->data))
+ return false;
+
+ return true;
+}
+
+struct mesh_node *node_create_from_storage(struct mesh_net *net,
+ struct mesh_db_node *db_node,
+ bool local)
+{
+ struct mesh_node *node;
+ unsigned int num_ele;
+
+ if (local && !net)
+ return NULL;
+
+ node = node_new();
+ if (!node)
+ return NULL;
+
+ node->comp = l_new(struct node_composition, 1);
+ if (!node->comp) {
+ node_free(node);
+ return NULL;
+ }
+
+ node->comp->cid = db_node->cid;
+ node->comp->pid = db_node->pid;
+ node->comp->vid = db_node->vid;
+ node->comp->crpl = db_node->crpl;
+ node->lpn = db_node->modes.lpn;
+
+ node->proxy = db_node->modes.proxy;
+ node->lpn = db_node->modes.lpn;
+ node->friend = db_node->modes.friend;
+ node->relay.mode = db_node->modes.relay.state;
+ node->relay.cnt = db_node->modes.relay.cnt;
+ node->relay.interval = db_node->modes.relay.interval;
+ node->beacon = db_node->modes.beacon;
+
+ l_info("relay %2.2x, proxy %2.2x, lpn %2.2x, friend %2.2x",
+ node->relay.mode, node->proxy, node->friend, node->lpn);
+ node->ttl = db_node->ttl;
+ node->seq_number = db_node->seq_number;
+
+ num_ele = l_queue_length(db_node->elements);
+ if (num_ele > 0xff) {
+ node_free(node);
+ return NULL;
+ }
+
+ node->num_ele = num_ele;
+ if (num_ele != 0 && !add_elements(node, db_node)) {
+ node_free(node);
+ return NULL;
+ }
+
+ node->primary = db_node->unicast;
+
+ memcpy(node->dev_uuid, db_node->uuid, 16);
+
+ if (local)
+ node->net = mesh_net_ref(net);
+
+ return node;
+}
+
+void node_cleanup(struct mesh_net *net)
+{
+ struct mesh_node *node;
+
+ if (!net)
+ return;
+ node = mesh_net_local_node_get(net);
+ if (node)
+ node_free(node);
+
+ l_queue_destroy(nodes, free_node_resources);
+
+}
+
+bool node_is_provisioned(struct mesh_node *node)
+{
+ return (!IS_UNASSIGNED(node->primary));
+}
+
+bool node_net_key_delete(struct mesh_node *node, uint16_t idx)
+{
+ if (!node)
+ return false;
+
+ if (!l_queue_find(node->net_keys, match_key_idx, L_UINT_TO_PTR(idx)))
+ return false;
+
+ l_queue_remove(node->net_keys, L_UINT_TO_PTR(idx));
+ /* TODO: remove all associated app keys and bindings */
+ return true;
+}
+
+bool node_app_key_delete(struct mesh_net *net, uint16_t addr,
+ uint16_t net_idx, uint16_t app_idx)
+{
+ struct mesh_node *node;
+ uint32_t index;
+ const struct l_queue_entry *entry;
+
+ node = node_find_by_addr(addr);
+ if (!node)
+ return false;
+
+ index = (net_idx << 16) + app_idx;
+
+ if (!l_queue_find(node->app_keys, match_key_idx, L_UINT_TO_PTR(index)))
+ return false;
+
+ l_queue_remove(node->app_keys, L_UINT_TO_PTR(index));
+
+ storage_local_app_key_del(net, net_idx, app_idx);
+
+ entry = l_queue_get_entries(node->elements);
+ for (; entry; entry = entry->next) {
+ struct node_element *ele = entry->data;
+
+ mesh_model_app_key_delete(net, ele->models, app_idx);
+ }
+
+ return true;
+}
+
+bool node_set_primary(struct mesh_node *node, uint16_t unicast)
+{
+ if (!node)
+ return false;
+
+ node->primary = unicast;
+
+ /* If local node, save to storage */
+ if (node->net)
+ return storage_local_set_unicast(node->net, unicast);
+
+ /* TODO: for provisioner, store remote node info */
+ return true;
+}
+
+uint16_t node_get_primary(struct mesh_node *node)
+{
+ if (!node)
+ return UNASSIGNED_ADDRESS;
+ else
+ return node->primary;
+}
+
+bool node_set_device_key(struct mesh_node *node, uint8_t key[16])
+
+{
+ if (!node || !key)
+ return false;
+
+ memcpy(node->dev_key, key, 16);
+
+ /* If local node, save to storage */
+ if (node->net)
+ return storage_local_set_device_key(node->net, key);
+
+ /* TODO: for provisioner, store remote node info */
+ return true;
+}
+
+const uint8_t *node_get_device_key(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ else
+ return node->dev_key;
+}
+
+uint8_t node_get_num_elements(struct mesh_node *node)
+{
+ return node->num_ele;
+}
+
+struct l_queue *node_get_net_keys(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ else
+ return node->net_keys;
+}
+
+struct l_queue *node_get_app_keys(struct mesh_node *node)
+{
+ if (!node)
+ return NULL;
+ else
+ return node->app_keys;
+}
+
+struct l_queue *node_get_element_models(struct mesh_node *node,
+ uint8_t ele_idx, int *status)
+{
+ struct node_element *ele;
+
+ if (!node) {
+ if (status)
+ *status = MESH_STATUS_INVALID_ADDRESS;
+ return NULL;
+ }
+
+ ele = l_queue_find(node->elements, match_element_idx,
+ L_UINT_TO_PTR(ele_idx));
+ if (!ele) {
+ if (status)
+ *status = MESH_STATUS_INVALID_ADDRESS;
+ return NULL;
+ }
+
+ if (status)
+ *status = MESH_STATUS_SUCCESS;
+
+ return ele->models;
+}
+
+struct mesh_model *node_get_model(struct mesh_node *node, uint8_t ele_idx,
+ uint32_t id, int *status)
+{
+ struct l_queue *models;
+ struct mesh_model *model;
+
+ if (!node) {
+ if (status)
+ *status = MESH_STATUS_INVALID_ADDRESS;
+ return NULL;
+ }
+
+ models = node_get_element_models(node, ele_idx, status);
+ if (!models)
+ return NULL;
+
+ model = l_queue_find(models, match_model_id, L_UINT_TO_PTR(id));
+
+ if (status)
+ *status = (model) ? MESH_STATUS_SUCCESS :
+ MESH_STATUS_INVALID_MODEL;
+
+ return model;
+}
+
+uint8_t node_default_ttl_get(struct mesh_node *node)
+{
+ if (!node)
+ return DEFAULT_TTL;
+ return node->ttl;
+}
+
+bool node_default_ttl_set(struct mesh_node *node, uint8_t ttl)
+{
+ bool res, is_local;
+
+ if (!node)
+ return false;
+
+ is_local = (node->net && mesh_net_local_node_get(node->net) == node) ?
+ true : false;
+
+ res = storage_local_set_ttl(node->net, ttl);
+
+ if (res) {
+ node->ttl = ttl;
+ if (is_local)
+ mesh_net_set_default_ttl(node->net, ttl);
+ }
+
+ return res;
+}
+
+bool node_set_sequence_number(struct mesh_node *node, uint32_t seq)
+{
+ bool is_local;
+ struct timeval write_time;
+
+
+ if (!node)
+ return false;
+
+ node->seq_number = seq;
+
+ is_local = (node->net && mesh_net_local_node_get(node->net) == node) ?
+ true : false;
+
+ if (!is_local)
+ return true;
+
+ /*
+ * Holistically determine worst case 5 minute sequence consumption
+ * so that we typically (once we reach a steady state) rewrite the
+ * local node file with a new seq cache value no more than once every
+ * five minutes (or more)
+ */
+ gettimeofday(&write_time, NULL);
+ if (node->upd_sec) {
+ uint32_t elapsed = write_time.tv_sec - node->upd_sec;
+
+ if (elapsed < MIN_SEQ_CACHE_TIME) {
+ uint32_t ideal = node->seq_min_cache;
+
+ l_info("Old Seq Cache: %d", node->seq_min_cache);
+
+ ideal *= (MIN_SEQ_CACHE_TIME / elapsed);
+
+ if (ideal > node->seq_min_cache + MIN_SEQ_CACHE)
+ node->seq_min_cache = ideal;
+ else
+ node->seq_min_cache += MIN_SEQ_CACHE;
+
+ l_info("New Seq Cache: %d", node->seq_min_cache);
+ }
+ }
+
+ node->upd_sec = write_time.tv_sec;
+
+ l_info("Storage-Write");
+ return storage_local_write_sequence_number(node->net, seq);
+}
+
+uint32_t node_get_sequence_number(struct mesh_node *node)
+{
+ if (!node)
+ return 0xffffffff;
+
+ return node->seq_number;
+}
+
+uint32_t node_seq_cache(struct mesh_node *node)
+{
+ if (node->seq_min_cache < MIN_SEQ_CACHE)
+ node->seq_min_cache = MIN_SEQ_CACHE;
+
+ return node->seq_min_cache;
+}
+
+int node_get_element_idx(struct mesh_node *node, uint16_t ele_addr)
+{
+ uint16_t addr;
+ uint8_t num_ele;
+
+ if (!node)
+ return -1;
+
+ num_ele = node_get_num_elements(node);
+ if (!num_ele)
+ return -2;
+
+ addr = node_get_primary(node);
+
+ if (ele_addr < addr || ele_addr >= addr + num_ele)
+ return -3;
+ else
+ return ele_addr - addr;
+}
+
+uint16_t node_get_crpl(struct mesh_node *node)
+{
+ if (!node)
+ return 0;
+
+ return node->comp->crpl;
+}
+
+uint8_t node_relay_mode_get(struct mesh_node *node, uint8_t *count,
+ uint16_t *interval)
+{
+ if (!node) {
+ *count = 0;
+ *interval = 0;
+ return MESH_MODE_DISABLED;
+ }
+
+ *count = node->relay.cnt;
+ *interval = node->relay.interval;
+ return node->relay.mode;
+}
+
+uint8_t node_lpn_mode_get(struct mesh_node *node)
+{
+ if (!node)
+ return MESH_MODE_DISABLED;
+
+ return node->lpn;
+}
+
+bool node_relay_mode_set(struct mesh_node *node, bool enable, uint8_t cnt,
+ uint16_t interval)
+{
+ bool res, is_local;
+
+ if (!node || node->relay.mode == MESH_MODE_UNSUPPORTED)
+ return false;
+
+ is_local = (node->net && mesh_net_local_node_get(node->net) == node) ?
+ true : false;
+
+ res = storage_local_set_relay(node->net, enable, cnt, interval);
+
+ if (res) {
+ node->relay.mode = enable ? MESH_MODE_ENABLED :
+ MESH_MODE_DISABLED;
+ node->relay.cnt = cnt;
+ node->relay.interval = interval;
+ if (is_local)
+ mesh_net_set_relay_mode(node->net, enable, cnt,
+ interval);
+ }
+
+ return res;
+}
+
+bool node_proxy_mode_set(struct mesh_node *node, bool enable)
+{
+ bool res, is_local;
+ uint8_t proxy;
+
+ if (!node || node->proxy == MESH_MODE_UNSUPPORTED)
+ return false;
+
+ is_local = (node->net && mesh_net_local_node_get(node->net) == node) ?
+ true : false;
+
+ proxy = enable ? MESH_MODE_ENABLED : MESH_MODE_DISABLED;
+ res = storage_local_set_mode(node->net, proxy, "proxy");
+
+ if (res) {
+ node->proxy = proxy;
+ if (is_local)
+ mesh_net_set_proxy_mode(node->net, enable);
+ }
+
+ return res;
+}
+
+uint8_t node_proxy_mode_get(struct mesh_node *node)
+{
+ if (!node)
+ return MESH_MODE_DISABLED;
+
+ return node->proxy;
+}
+
+bool node_beacon_mode_set(struct mesh_node *node, bool enable)
+{
+ bool res, is_local;
+ uint8_t beacon;
+
+ if (!node)
+ return false;
+
+ is_local = (node->net && mesh_net_local_node_get(node->net) == node) ?
+ true : false;
+
+ beacon = enable ? MESH_MODE_ENABLED : MESH_MODE_DISABLED;
+ res = storage_local_set_mode(node->net, beacon, "beacon");
+
+ if (res) {
+ node->beacon = beacon;
+ if (is_local)
+ mesh_net_set_beacon_mode(node->net, enable);
+ }
+
+ return res;
+}
+
+uint8_t node_beacon_mode_get(struct mesh_node *node)
+{
+ if (!node)
+ return MESH_MODE_DISABLED;
+
+ return node->beacon;
+}
+
+bool node_friend_mode_set(struct mesh_node *node, bool enable)
+{
+ bool res, is_local;
+ uint8_t friend;
+
+ if (!node || node->friend == MESH_MODE_UNSUPPORTED)
+ return false;
+
+ is_local = (node->net && mesh_net_local_node_get(node->net) == node) ?
+ true : false;
+
+ friend = enable ? MESH_MODE_ENABLED : MESH_MODE_DISABLED;
+ res = storage_local_set_mode(node->net, friend, "friend");
+
+ if (res) {
+ node->friend = friend;
+ if (is_local)
+ mesh_net_set_friend_mode(node->net, enable);
+ }
+
+ return res;
+}
+
+uint8_t node_friend_mode_get(struct mesh_node *node)
+{
+ if (!node)
+ return MESH_MODE_DISABLED;
+
+ return node->friend;
+}
+
+uint16_t node_generate_comp(struct mesh_node *node, uint8_t *buf, uint16_t sz)
+{
+ uint16_t n, features;
+ const struct l_queue_entry *ele_entry;
+
+ if (!node || !node->comp || sz < MIN_COMP_SIZE)
+ return 0;
+
+ n = 0;
+
+ l_put_le16(node->comp->cid, buf + n);
+ n += 2;
+ l_put_le16(node->comp->pid, buf + n);
+ n += 2;
+ l_put_le16(node->comp->vid, buf + n);
+ n += 2;
+ l_put_le16(node->comp->crpl, buf + n);
+ n += 2;
+
+ features = 0;
+
+ if (node->relay.mode != MESH_MODE_UNSUPPORTED)
+ features |= FEATURE_RELAY;
+ if (node->proxy != MESH_MODE_UNSUPPORTED)
+ features |= FEATURE_PROXY;
+ if (node->friend != MESH_MODE_UNSUPPORTED)
+ features |= FEATURE_FRIEND;
+ if (node->lpn != MESH_MODE_UNSUPPORTED)
+ features |= FEATURE_LPN;
+
+ l_put_le16(features, buf + n);
+ n += 2;
+
+ ele_entry = l_queue_get_entries(node->elements);
+ for (; ele_entry; ele_entry = ele_entry->next) {
+ struct node_element *ele = ele_entry->data;
+ const struct l_queue_entry *mod_entry;
+ uint8_t num_s = 0, num_v = 0;
+ uint8_t *mod_buf;
+
+ /* At least fit location and zeros for number of models */
+ if ((n + 4) > sz)
+ return n;
+ l_info("ele->location %d", ele->location);
+ l_put_le16(ele->location, buf + n);
+ n += 2;
+
+ /* Store models IDs, store num_s and num_v later */
+ mod_buf = buf + n;
+ n += 2;
+
+ /* Get SIG models */
+ mod_entry = l_queue_get_entries(ele->models);
+ for (; mod_entry; mod_entry = mod_entry->next) {
+ struct mesh_model *mod = mod_entry->data;
+ uint32_t mod_id;
+
+ mod_id = mesh_model_get_model_id(
+ (const struct mesh_model *) mod);
+
+ if ((mod_id >> 16) == 0xffff) {
+ if (n + 2 > sz)
+ goto element_done;
+
+ l_put_le16((uint16_t) (mod_id & 0xffff),
+ buf + n);
+ n += 2;
+ num_s++;
+ }
+ }
+
+ /* Get vendor models */
+ mod_entry = l_queue_get_entries(ele->models);
+ for (; mod_entry; mod_entry = mod_entry->next) {
+ struct mesh_model *mod = mod_entry->data;
+ uint32_t mod_id;
+ uint16_t vendor;
+
+ mod_id = mesh_model_get_model_id(
+ (const struct mesh_model *) mod);
+
+ vendor = (uint16_t) (mod_id >> 16);
+ if (vendor != 0xffff) {
+ if (n + 4 > sz)
+ goto element_done;
+
+ l_put_le16(vendor, buf + n);
+ n += 2;
+ l_put_le16((uint16_t) (mod_id & 0xffff),
+ buf + n);
+ n += 2;
+ num_v++;
+ }
+
+ }
+
+element_done:
+ mod_buf[0] = num_s;
+ mod_buf[1] = num_v;
+
+ }
+
+ return n;
+}
diff --git a/mesh/storage.c b/mesh/storage.c
new file mode 100644
index 000000000..85fa81dda
--- /dev/null
+++ b/mesh/storage.c
@@ -0,0 +1,672 @@
+/*
+ *
+ * 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 <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include <json-c/json.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+
+#include "mesh/net.h"
+#include "mesh/appkey.h"
+#include "mesh/model.h"
+#include "mesh/storage.h"
+
+/*
+ * TODO: figure out naming convention to store alternative nodes
+ * Mesh storage dir wil be in configure.ac
+ */
+#define DEVICE_COMPOSITION_FILE "../config/composition.json"
+#define NODE_CONGIGURATION_FILE "../config/configuration.json"
+
+static bool read_local_node_cb(struct mesh_db_node *db_node, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_node *node;
+ uint32_t seq_number;
+ uint16_t crpl;
+ uint8_t ttl, mode, cnt, num_ele;
+ uint16_t unicast, interval;
+ uint8_t *uuid;
+
+ if (!net)
+ return false;
+
+ node = node_create_from_storage(net, db_node, true);
+ if (!node)
+ return false;
+
+ mesh_net_local_node_set(net, node, db_node->provisioner);
+ seq_number = node_get_sequence_number(node);
+ mesh_net_set_seq_num(net, seq_number);
+ ttl = node_default_ttl_get(node);
+ mesh_net_set_default_ttl(net, ttl);
+ crpl = node_get_crpl(node);
+ mesh_net_set_crpl(net, crpl);
+
+ mode = node_proxy_mode_get(node);
+ if (mode == MESH_MODE_ENABLED || mode == MESH_MODE_DISABLED)
+ mesh_net_set_proxy_mode(net, mode == MESH_MODE_ENABLED);
+
+ mode = node_friend_mode_get(node);
+ if (mode == MESH_MODE_ENABLED || mode == MESH_MODE_DISABLED)
+ mesh_net_set_friend_mode(net, mode == MESH_MODE_ENABLED);
+
+ mode = node_relay_mode_get(node, &cnt, &interval);
+ if (mode == MESH_MODE_ENABLED || mode == MESH_MODE_DISABLED)
+ mesh_net_set_relay_mode(net, mode == MESH_MODE_ENABLED, cnt,
+ interval);
+
+ mode = node_beacon_mode_get(node);
+ if (mode == MESH_MODE_ENABLED || mode == MESH_MODE_DISABLED)
+ mesh_net_set_beacon_mode(net, mode == MESH_MODE_ENABLED);
+
+ unicast = db_node->unicast;
+ num_ele = node_get_num_elements(node);
+
+ if (!IS_UNASSIGNED(unicast) &&
+ !mesh_net_register_unicast(net, unicast, num_ele))
+ return false;
+
+ uuid = node_uuid_get(node);
+ if (uuid)
+ mesh_net_id_uuid_set(net, uuid);
+ return true;
+}
+
+static bool read_net_keys_cb(uint16_t idx, uint8_t *key, uint8_t *new_key,
+ int phase, void *user_data)
+{
+ struct mesh_net *net = user_data;
+
+ if (!net)
+ return false;
+
+ if (mesh_net_add_key(net, false, idx, key) != MESH_STATUS_SUCCESS)
+ return false;
+ /* TODO: handle restoring key refresh phase and new keys */
+
+ return true;
+}
+
+static bool read_app_keys_cb(uint16_t net_idx, uint16_t app_idx, uint8_t *key,
+ uint8_t *new_key, void *user_data)
+{
+ struct mesh_net *net = user_data;
+
+ if (!net)
+ return false;
+
+ return appkey_key_init(net, net_idx, app_idx, key, new_key);
+}
+
+static bool parse_local_node(struct mesh_net *net, json_object *jnode)
+{
+ bool bvalue;
+ uint32_t iv_index;
+ uint8_t key_buf[16];
+ uint8_t cnt;
+ uint16_t interval;
+
+ if (mesh_db_read_iv_index(jnode, &iv_index, &bvalue))
+ mesh_net_set_iv_index(net, iv_index, bvalue);
+
+ if (mesh_db_read_net_transmit(jnode, &cnt, &interval))
+ mesh_net_transmit_params_set(net, cnt, interval);
+
+ /* Node composition/configuration info */
+ if (!mesh_db_read_node(jnode, read_local_node_cb, net))
+ return false;
+
+ if (!mesh_db_read_net_keys(jnode, read_net_keys_cb, net))
+ return false;
+
+ /* TODO: use the actual "primary" network index for this node */
+ if (mesh_db_read_device_key(jnode, key_buf) &&
+ !node_set_device_key(mesh_net_local_node_get(net), key_buf))
+ return false;
+
+ mesh_db_read_app_keys(jnode, read_app_keys_cb, net);
+
+ return true;
+}
+
+static bool read_unprov_device_cb(struct mesh_db_node *db_node, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_node *node;
+ uint16_t crpl;
+ uint8_t *uuid;
+
+ if (!net)
+ return false;
+
+ node = node_create_from_storage(net, db_node, true);
+
+ if (!node)
+ return false;
+
+ mesh_net_local_node_set(net, node, db_node->provisioner);
+ crpl = node_get_crpl(node);
+ mesh_net_set_crpl(net, crpl);
+
+ uuid = node_uuid_get(node);
+ if (uuid)
+ mesh_net_id_uuid_set(net, uuid);
+
+ return true;
+}
+
+static bool parse_unprovisioned_device(struct mesh_net *net, json_object *jnode)
+{
+ struct mesh_db_prov prov;
+ struct mesh_net_prov_caps *caps;
+ struct mesh_node *node;
+
+ /* Node composition/configuration info */
+ if (!mesh_db_read_unprovisioned_device(jnode,
+ read_unprov_device_cb, net))
+ return false;
+
+ if (!mesh_db_read_prov_info(jnode, &prov))
+ return false;
+
+ caps = mesh_net_prov_caps_get(net);
+ if (!caps)
+ return false;
+
+ node = mesh_net_local_node_get(net);
+ if (!node)
+ return false;
+
+ caps->num_ele = node_get_num_elements(node);
+ l_put_le16(prov.algorithm, &caps->algorithms);
+ caps->pub_type = prov.pub_type;
+ caps->static_type = prov.static_type;
+ caps->output_size = prov.output_oob.size;
+ l_put_le16(prov.output_oob.actions, &caps->output_action);
+ caps->input_size = prov.input_oob.size;
+ l_put_le16(prov.input_oob.actions, &caps->input_action);
+
+ return mesh_net_priv_key_set(net, prov.priv_key);
+}
+
+static bool parse_config(struct mesh_net *net, const char *config_name,
+ bool unprovisioned)
+{
+ int fd;
+ char *str;
+ const char *out;
+ struct stat st;
+ ssize_t sz;
+ json_object *jnode = NULL;
+ bool result = false;
+
+ if (!config_name)
+ return false;
+
+ fd = open(config_name, O_RDONLY);
+ if (!fd)
+ return false;
+
+ if (fstat(fd, &st) == -1) {
+ close(fd);
+ return false;
+ }
+
+ str = (char *) l_new(char, st.st_size + 1);
+ if (!str) {
+ close(fd);
+ return false;
+ }
+
+ sz = read(fd, str, st.st_size);
+ if (sz != st.st_size) {
+ l_error("Failed to read configuration file");
+ goto done;
+ }
+
+ jnode = json_tokener_parse(str);
+ if (!jnode)
+ goto done;
+
+ mesh_net_jconfig_set(net, jnode);
+
+ if (!unprovisioned)
+ result = parse_local_node(net, jnode);
+ else
+ result = parse_unprovisioned_device(net, jnode);
+
+ if (!result) {
+ storage_release(net);
+ goto done;
+ }
+
+ mesh_net_cfg_file_get(net, &out);
+ if (!out)
+ mesh_net_cfg_file_set(net, !unprovisioned ?
+ config_name : NODE_CONGIGURATION_FILE);
+done:
+ close(fd);
+ if (str)
+ l_free(str);
+
+ return result;
+}
+
+bool storage_parse_config(struct mesh_net *net, const char *config_name)
+{
+ bool result = false;
+ bool unprovisioned = !config_name;
+
+ if (unprovisioned) {
+ result = parse_config(net, DEVICE_COMPOSITION_FILE, true);
+ goto done;
+ }
+
+ result = parse_config(net, config_name, false);
+
+ if (!result) {
+ char *bak = (char *) l_malloc(strlen(config_name) + 5);
+
+ if (!bak)
+ goto done;
+
+ /* Fall-back to Backup version */
+ strncpy(bak, config_name, strlen(config_name) + 1);
+ bak = strncat(bak, ".bak", 5);
+
+ remove(config_name);
+ rename(bak, config_name);
+
+ result = parse_config(net, config_name, false);
+
+ l_free(bak);
+ }
+
+ /* If configuration read fails, try as unprovisioned device */
+ if (!result) {
+ l_info("Parse configuration failed, trying unprovisioned");
+ unprovisioned = true;
+ result = parse_config(net, DEVICE_COMPOSITION_FILE, true);
+ }
+
+done:
+ if (result)
+ mesh_net_provisioned_set(net, !unprovisioned);
+
+ return result;
+}
+
+bool storage_local_set_ttl(struct mesh_net *net, uint8_t ttl)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_int(jnode, "defaultTTL", ttl);
+}
+
+bool storage_local_set_relay(struct mesh_net *net, bool enable,
+ uint8_t count, uint8_t interval)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_relay_mode(jnode, enable, count, interval);
+}
+
+bool storage_local_set_transmit_params(struct mesh_net *net, uint8_t count,
+ uint8_t interval)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_net_transmit(jnode, count, interval);
+}
+
+bool storage_local_set_mode(struct mesh_net *net, uint8_t mode,
+ const char *mode_name)
+{
+ json_object *jnode;
+
+ if (!net || !mode_name)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_mode(jnode, mode_name, mode);
+}
+
+bool storage_model_bind(struct mesh_net *net, uint16_t addr, uint32_t mod_id,
+ uint16_t app_idx, bool unbind)
+{
+ json_object *jnode;
+ bool is_local;
+
+ if (!net)
+ return false;
+
+ is_local = mesh_net_is_local_address(net, addr);
+ if (is_local) {
+ int ele_idx;
+ bool is_vendor = (mod_id > 0xffff);
+
+ ele_idx = node_get_element_idx(mesh_net_local_node_get(net),
+ addr);
+ if (ele_idx < 0)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ if (unbind)
+ return mesh_db_model_binding_del(jnode, ele_idx,
+ is_vendor, mod_id, app_idx);
+ else
+ return mesh_db_model_binding_add(jnode, ele_idx,
+ is_vendor, mod_id, app_idx);
+ }
+
+ /* TODO: write remote node bindings to provisioner DB */
+ return false;
+}
+
+bool storage_local_app_key_add(struct mesh_net *net, uint16_t net_idx,
+ uint16_t app_idx, const uint8_t key[16], bool update)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_app_key_add(jnode, net_idx, app_idx, key, update);
+}
+
+bool storage_local_app_key_del(struct mesh_net *net, uint16_t net_idx,
+ uint16_t app_idx)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_app_key_del(jnode, net_idx, app_idx);
+
+}
+
+bool storage_local_net_key_add(struct mesh_net *net, uint16_t net_idx,
+ const uint8_t key[16], int phase)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_net_key_add(jnode, net_idx, key, phase);
+}
+
+bool storage_local_net_key_del(struct mesh_net *net, uint16_t net_idx)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_net_key_del(jnode, net_idx);
+}
+
+bool storage_local_set_iv_index(struct mesh_net *net, uint32_t iv_index,
+ bool update)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_iv_index(jnode, iv_index, update);
+}
+
+bool storage_local_set_device_key(struct mesh_net *net, uint8_t dev_key[16])
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_device_key(jnode, dev_key);
+}
+
+bool storage_local_set_unicast(struct mesh_net *net, uint16_t unicast)
+{
+ json_object *jnode;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ return mesh_db_write_uint16_hex(jnode, "unicastAddress", unicast);
+}
+
+bool storage_local_write_sequence_number(struct mesh_net *net, uint32_t seq)
+{
+ json_object *jnode;
+ const char *cfg_file;
+ bool result;
+
+ if (!net)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ result = mesh_db_write_int(jnode, "sequenceNumber", seq);
+ if (!result)
+ return false;
+
+ result = mesh_net_cfg_file_get(net, &cfg_file);
+ if (result && cfg_file)
+ result = storage_save_config(net, cfg_file, false, NULL, NULL);
+
+ return result;
+}
+
+static bool save_config(struct mesh_net *net, const char *config_name)
+{
+ FILE *outfile;
+ const char *str;
+ json_object *jnode;
+ bool result = false;
+
+ if (!net || !config_name)
+ return false;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ outfile = fopen(config_name, "w");
+ if (!outfile) {
+ l_error("Failed to save configuration to %s", config_name);
+ return false;
+ }
+
+ str = json_object_to_json_string_ext(jnode, JSON_C_TO_STRING_PRETTY);
+
+ if (fwrite(str, sizeof(char), strlen(str), outfile) < strlen(str))
+ l_warn("Incomplete write of mesh configuration");
+ else
+ result = true;
+
+ fclose(outfile);
+
+ return result;
+}
+
+struct write_info {
+ const char *config_name;
+ struct mesh_net *net;
+ void *user_data;
+ mesh_status_func_t cb;
+};
+
+static void idle_save_config(void *user_data)
+{
+ struct write_info *info = user_data;
+ char *tmp = (char *) l_malloc(strlen(info->config_name) + 5);
+ char *bak = (char *) l_malloc(strlen(info->config_name) + 5);
+ bool result = false;
+
+ if (!tmp || !bak)
+ goto done;
+
+ strncpy(tmp, info->config_name, strlen(info->config_name) + 1);
+ strncpy(bak, info->config_name, strlen(info->config_name) + 1);
+ tmp = strncat(tmp, ".tmp", 5);
+ bak = strncat(bak, ".bak", 5);
+ remove(tmp);
+
+ l_debug("Storage-Wrote");
+ result = save_config(info->net, tmp);
+
+ if (result) {
+ remove(bak);
+ rename(info->config_name, bak);
+ rename(tmp, info->config_name);
+ }
+
+ remove(tmp);
+done:
+ l_free(tmp);
+ l_free(bak);
+
+ if (info->cb)
+ info->cb(info->user_data, result);
+
+ l_free(info);
+}
+
+bool storage_save_config(struct mesh_net *net, const char *config_name,
+ bool no_wait, mesh_status_func_t cb, void *user_data)
+{
+ struct write_info *info;
+
+ info = l_new(struct write_info, 1);
+ if (!info)
+ return false;
+
+ info->net = net;
+ info->config_name = config_name;
+ info->cb = cb;
+ info->user_data = user_data;
+
+ if (no_wait)
+ idle_save_config(info);
+ l_idle_oneshot(idle_save_config, info, NULL);
+
+ return true;
+}
+
+bool storage_save_new_config(struct mesh_net *net, const char *config_name,
+ mesh_status_func_t cb, void *user_data)
+{
+ json_object *jnode;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (!jnode)
+ return false;
+
+ mesh_db_remove_property(jnode, "provision");
+
+ return storage_save_config(net, config_name, false, cb, user_data);
+}
+
+void storage_release(struct mesh_net *net)
+{
+ json_object *jnode;
+
+ jnode = mesh_net_jconfig_get(net);
+ if (jnode)
+ json_object_put(jnode);
+
+ mesh_net_jconfig_set(net, NULL);
+}
diff --git a/mesh/util.c b/mesh/util.c
new file mode 100644
index 000000000..5d771431c
--- /dev/null
+++ b/mesh/util.c
@@ -0,0 +1,71 @@
+/*
+ *
+ * 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 <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <time.h>
+
+#include "mesh/util.h"
+
+uint32_t get_timestamp_secs(void)
+{
+ struct timespec ts;
+
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ return ts.tv_sec;
+}
+
+bool str2hex(const char *str, uint16_t in_len, uint8_t *out,
+ uint16_t out_len)
+{
+ uint16_t i;
+
+ if (in_len < out_len * 2)
+ return false;
+
+ for (i = 0; i < out_len; i++) {
+ if (sscanf(&str[i * 2], "%02hhx", &out[i]) != 1)
+ return false;
+ }
+
+ return true;
+}
+
+size_t hex2str(uint8_t *in, size_t in_len, char *out, size_t out_len)
+{
+ static const char hexdigits[] = "0123456789abcdef";
+ size_t i;
+
+ if (in_len * 2 > (out_len - 1))
+ return 0;
+
+ for (i = 0; i < in_len; i++) {
+ out[i * 2] = hexdigits[in[i] >> 4];
+ out[i * 2 + 1] = hexdigits[in[i] & 0xf];
+ }
+
+ out[in_len * 2] = '\0';
+ return i;
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 10/14] mesh: Source code for handling access layer mux/demux
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (8 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 09/14] mesh: Source " Brian Gix
@ 2018-07-14 19:58 ` Brian Gix
2018-07-14 19:59 ` [PATCH BlueZ v7 11/14] mesh: Mesh config server model Brian Gix
` (4 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:58 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
---
mesh/model.c | 1274 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 1274 insertions(+)
create mode 100644 mesh/model.c
diff --git a/mesh/model.c b/mesh/model.c
new file mode 100644
index 000000000..d50817843
--- /dev/null
+++ b/mesh/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 "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/crypto.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/appkey.h"
+#include "mesh/model.h"
+#include "mesh/display.h"
+#include "mesh/cfgmod.h"
+#include "mesh/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.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 11/14] mesh: Mesh config server model
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (9 preceding siblings ...)
2018-07-14 19:58 ` [PATCH BlueZ v7 10/14] mesh: Source code for handling access layer mux/demux Brian Gix
@ 2018-07-14 19:59 ` Brian Gix
2018-07-14 19:59 ` [PATCH BlueZ v7 12/14] mesh: Read and write mesh configuration in JSON format Brian Gix
` (3 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:59 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
---
mesh/cfgmod-server.c | 1194 ++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 1194 insertions(+)
create mode 100644 mesh/cfgmod-server.c
diff --git a/mesh/cfgmod-server.c b/mesh/cfgmod-server.c
new file mode 100644
index 000000000..e982c79b3
--- /dev/null
+++ b/mesh/cfgmod-server.c
@@ -0,0 +1,1194 @@
+/*
+ *
+ * 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 <unistd.h>
+#include <stdio.h>
+#include <sys/time.h>
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+
+#include "mesh/mesh.h"
+#include "mesh/node.h"
+#include "mesh/net.h"
+#include "mesh/appkey.h"
+#include "mesh/model.h"
+#include "mesh/storage.h"
+
+#include "mesh/cfgmod.h"
+
+#define CFG_MAX_MSG_LEN 380
+
+static void send_pub_status(struct mesh_net *net, uint16_t src, uint16_t dst,
+ uint8_t status, uint16_t ele_addr, uint16_t pub_addr,
+ uint32_t mod_id, uint16_t idx, bool cred_flag,
+ uint8_t ttl, uint8_t period, uint8_t retransmit)
+{
+ uint8_t msg[16];
+ size_t n;
+
+ n = mesh_model_opcode_set(OP_CONFIG_MODEL_PUB_STATUS, msg);
+ msg[n++] = status;
+ l_put_le16(ele_addr, msg + n);
+ n += 2;
+ l_put_le16(pub_addr, msg + n);
+ n += 2;
+ idx |= cred_flag ? CREDFLAG_MASK : 0;
+ l_put_le16(idx, msg + n);
+ n += 2;
+ msg[n++] = ttl;
+ msg[n++] = period;
+ msg[n++] = retransmit;
+ if (mod_id < 0x10000) {
+ l_put_le16(mod_id, msg + n);
+ n += 2;
+ } else {
+ l_put_le16(mod_id >> 16, msg + n);
+ n += 2;
+ l_put_le16(mod_id, msg + n);
+ n += 2;
+ }
+
+ mesh_model_send(net, CONFIG_SRV_MODEL,
+ dst, src,
+ APP_IDX_DEV, DEFAULT_TTL, msg, n);
+}
+
+static bool config_pub_get(struct mesh_net *net, uint16_t src, uint16_t dst,
+ const uint8_t *pkt, uint16_t size)
+{
+ uint32_t mod_id;
+ uint16_t ele_addr;
+ int ele_idx;
+ struct mesh_model_pub *pub;
+ int status;
+
+ if (size == 4)
+ mod_id = l_get_le16(pkt + 2);
+ else if (size == 6) {
+ mod_id = l_get_le16(pkt + 2) << 16;
+ mod_id |= l_get_le16(pkt + 4);
+ } else
+ return false;
+
+ ele_addr = l_get_le16(pkt);
+ ele_idx = node_get_element_idx(mesh_net_local_node_get(net), ele_addr);
+
+ if (ele_idx >= 0)
+ pub = mesh_model_pub_get(net, ele_idx, mod_id, &status);
+ else
+ status = MESH_STATUS_INVALID_ADDRESS;
+
+ if (pub && status == MESH_STATUS_SUCCESS)
+ send_pub_status(net, src, dst, status, ele_addr, pub->addr,
+ mod_id, pub->idx, pub->credential, pub->ttl,
+ pub->period, pub->retransmit);
+ else
+ send_pub_status(net, src, dst, status, ele_addr, 0, mod_id,
+ 0, 0, 0, 0, 0);
+ return true;
+}
+
+static bool config_pub_set(struct mesh_net *net, uint16_t src, uint16_t dst,
+ const uint8_t *pkt, uint16_t size,
+ bool unreliable)
+{
+ uint32_t mod_id;
+ uint16_t ele_addr, idx, ota = 0;
+ const uint8_t *pub_addr;
+ uint16_t test_addr;
+ uint8_t ttl, period;
+ uint8_t retransmit;
+ int status;
+ bool cred_flag, b_virt = false;
+
+ switch (size) {
+ default:
+ return false;
+
+ case 11:
+ idx = l_get_le16(pkt + 4);
+ ttl = pkt[6];
+ period = pkt[7];
+ retransmit = pkt[8];
+ mod_id = l_get_le16(pkt + 9);
+ break;
+
+ case 13:
+ idx = l_get_le16(pkt + 4);
+ ttl = pkt[6];
+ period = pkt[7];
+ retransmit = pkt[8];
+ mod_id = l_get_le16(pkt + 9) << 16;
+ mod_id |= l_get_le16(pkt + 11);
+ break;
+
+ case 25:
+ b_virt = true;
+ idx = l_get_le16(pkt + 18);
+ ttl = pkt[20];
+ period = pkt[21];
+ retransmit = pkt[22];
+ mod_id = l_get_le16(pkt + 23);
+ break;
+
+ case 27:
+ b_virt = true;
+ idx = l_get_le16(pkt + 18);
+ ttl = pkt[20];
+ period = pkt[21];
+ retransmit = pkt[22];
+ mod_id = l_get_le16(pkt + 23) << 16;
+ mod_id |= l_get_le16(pkt + 25);
+ break;
+ }
+ ele_addr = l_get_le16(pkt);
+ pub_addr = pkt + 2;
+
+ /* Doesn't accept out-of-range TTLs */
+ if (ttl > TTL_MASK && ttl != DEFAULT_TTL)
+ return false;
+
+ /* Get cred_flag */
+ cred_flag = !!(CREDFLAG_MASK & idx);
+
+ /* Ignore non-IDX bits */
+ idx &= APP_IDX_MASK;
+
+ /* Doesn't accept virtual seeming addresses */
+ test_addr = l_get_le16(pub_addr);
+ if (!b_virt && test_addr > 0x7fff && test_addr < 0xc000)
+ return false;
+
+ status = mesh_model_pub_set(net, ele_addr, mod_id, pub_addr, idx,
+ cred_flag, ttl, period, retransmit,
+ b_virt, &ota);
+
+ l_info("pub_set: status %d, ea %4.4x, ota: %4.4x, mod: %x, idx: %3.3x",
+ status, ele_addr, ota, mod_id, idx);
+
+ if (IS_UNASSIGNED(ota) && !b_virt)
+ ttl = period = idx = 0;
+
+ if (status >= 0 && !unreliable)
+ send_pub_status(net, src, dst, status, ele_addr, ota,
+ mod_id, idx, cred_flag, ttl, period,
+ retransmit);
+ return true;
+}
+
+static void send_sub_status(struct mesh_net *net, uint16_t src, uint16_t dst,
+ uint8_t status, uint16_t ele_addr,
+ uint16_t addr, uint32_t mod)
+{
+ uint8_t msg[12];
+ int n = mesh_model_opcode_set(OP_CONFIG_MODEL_SUB_STATUS, msg);
+
+ msg[n++] = status;
+ l_put_le16(ele_addr, msg + n);
+ n += 2;
+ l_put_le16(addr, msg + n);
+ n += 2;
+ if (mod >= 0x10000) {
+ l_put_le16(mod >> 16, msg + n);
+ l_put_le16(mod, msg + n + 2);
+ n += 4;
+ } else {
+ l_put_le16(mod, msg + n);
+ n += 2;
+ }
+
+ mesh_model_send(net, CONFIG_SRV_MODEL, dst, src, APP_IDX_DEV,
+ DEFAULT_TTL, msg, n);
+}
+
+static bool config_sub_get(struct mesh_net *net, uint16_t src, uint16_t dst,
+ const uint8_t *pkt, uint16_t size)
+{
+ uint16_t ele_addr;
+ uint32_t mod_id;
+ uint16_t n = 0;
+ int ret = 0;
+ uint8_t *status;
+ uint16_t buf_size;
+ uint8_t msg[5 + sizeof(uint16_t) * MAX_GRP_PER_MOD];
+
+ /* Incoming message has already been size-checked */
+ ele_addr = l_get_le16(pkt);
+
+ switch (size) {
+ default:
+ l_debug("Bad Len Cfg_Pub_Set: %d", size);
+ return false;
+
+ case 4:
+ mod_id = l_get_le16(pkt + 2);
+ n = mesh_model_opcode_set(OP_CONFIG_MODEL_SUB_LIST, msg);
+ status = msg + n;
+ msg[n++] = 0;
+ l_put_le16(ele_addr, msg + n);
+ n += 2;
+ l_put_le16(mod_id, msg + n);
+ n += 2;
+ break;
+
+ case 6:
+ mod_id = l_get_le16(pkt + 2) << 16;
+ mod_id |= l_get_le16(pkt + 4);
+ n = mesh_model_opcode_set(OP_CONFIG_VEND_MODEL_SUB_LIST, msg);
+ status = msg + n;
+ msg[n++] = 0;
+ l_put_le16(ele_addr, msg + n);
+ n += 2;
+ l_put_le16(mod_id >> 16, msg + n);
+ n += 2;
+ l_put_le16(mod_id, msg + n);
+ n += 2;
+ break;
+ }
+
+ buf_size = sizeof(uint16_t) * MAX_GRP_PER_MOD;
+ ret = mesh_model_sub_get(net, ele_addr, mod_id, msg + n, buf_size,
+ &size);
+
+ if (!ret)
+ n += size;
+ else if (ret > 0)
+ *status = ret;
+
+ mesh_model_send(net, CONFIG_SRV_MODEL,
+ dst, src, APP_IDX_DEV,
+ DEFAULT_TTL, msg, n);
+ return true;
+}
+
+static void config_sub_set(struct mesh_net *net, uint16_t src, uint16_t dst,
+ const uint8_t *pkt, uint16_t size,
+ bool virt, uint32_t opcode)
+{
+ uint16_t grp, ele_addr;
+ bool unreliable = !!(opcode & OP_UNRELIABLE);
+ uint32_t mod_id, func;
+ const uint8_t *addr = NULL;
+ int status = 0;
+
+ switch (size) {
+ default:
+ l_error("Bad Len Cfg_Sub_Set: %d", size);
+ return;
+ case 4:
+ if (opcode != OP_CONFIG_MODEL_SUB_DELETE_ALL)
+ return;
+ mod_id = l_get_le16(pkt + 2);
+ break;
+ case 6:
+ if (virt)
+ return;
+ if (opcode != OP_CONFIG_MODEL_SUB_DELETE_ALL)
+ mod_id = l_get_le16(pkt + 4);
+ else {
+ mod_id = l_get_le16(pkt + 2) << 16;
+ mod_id |= l_get_le16(pkt + 4);
+ }
+ break;
+ case 8:
+ if (virt)
+ return;
+ mod_id = l_get_le16(pkt + 4) << 16;
+ mod_id |= l_get_le16(pkt + 6);
+ break;
+ case 20:
+ if (!virt)
+ return;
+ mod_id = l_get_le16(pkt + 18);
+ break;
+ case 22:
+ if (!virt)
+ return;
+ mod_id = l_get_le16(pkt + 18) << 16;
+ mod_id |= l_get_le16(pkt + 20);
+ break;
+ }
+ ele_addr = l_get_le16(pkt);
+
+ if (opcode != OP_CONFIG_MODEL_SUB_DELETE_ALL) {
+ addr = pkt + 2;
+ grp = l_get_le16(addr);
+ } else
+ grp = UNASSIGNED_ADDRESS;
+
+ func = opcode & ~OP_UNRELIABLE;
+ switch (func) {
+ default:
+ l_info("Bad opcode: %x", func);
+ return;
+
+ case OP_CONFIG_MODEL_SUB_DELETE_ALL:
+ status = mesh_model_sub_del_all(net, ele_addr, mod_id);
+ break;
+
+ case OP_CONFIG_MODEL_SUB_VIRT_OVERWRITE:
+ grp = UNASSIGNED_ADDRESS;
+ /* Fall Through */
+ case OP_CONFIG_MODEL_SUB_OVERWRITE:
+ status = mesh_model_sub_ovr(net, ele_addr, mod_id,
+ addr, virt, &grp);
+ break;
+ case OP_CONFIG_MODEL_SUB_VIRT_ADD:
+ grp = UNASSIGNED_ADDRESS;
+ /* Fall Through */
+ case OP_CONFIG_MODEL_SUB_ADD:
+ status = mesh_model_sub_add(net, ele_addr, mod_id,
+ addr, virt, &grp);
+ break;
+ case OP_CONFIG_MODEL_SUB_VIRT_DELETE:
+ grp = UNASSIGNED_ADDRESS;
+ /* Fall Through */
+ case OP_CONFIG_MODEL_SUB_DELETE:
+ status = mesh_model_sub_del(net, ele_addr, mod_id,
+ addr, virt, &grp);
+ break;
+ }
+
+ if (!unreliable && status >= 0)
+ send_sub_status(net, src, dst, status, ele_addr, grp, mod_id);
+
+}
+
+static void send_model_app_status(struct mesh_net *net, uint16_t src,
+ uint16_t dst, uint8_t status,
+ uint16_t addr, uint32_t id,
+ uint16_t idx)
+{
+ uint8_t msg[12];
+ size_t n = mesh_model_opcode_set(OP_MODEL_APP_STATUS, msg);
+
+ msg[n++] = status;
+ l_put_le16(addr, msg + n);
+ n += 2;
+ l_put_le16(idx, msg + n);
+ n += 2;
+ if (id > 0xffff) {
+ l_put_le16(id >> 16, msg + n);
+ n += 2;
+ }
+ l_put_le16(id, msg + n);
+ n += 2;
+
+ mesh_model_send(net, CONFIG_SRV_MODEL, dst, src, APP_IDX_DEV,
+ DEFAULT_TTL, msg, n);
+}
+
+static void model_app_list(struct mesh_net *net, uint16_t src, uint16_t dst,
+ const uint8_t *pkt, uint16_t size)
+{
+ uint16_t ele_addr;
+ uint32_t mod_id = 0xffff;
+ uint8_t *msg = NULL;
+ uint8_t *status;
+ uint16_t n, buf_size;
+ int result;
+
+ buf_size = MAX_BINDINGS * sizeof(uint16_t);
+ msg = l_malloc(7 + buf_size);
+ if (!msg)
+ return;
+
+ ele_addr = l_get_le16(pkt);
+
+ switch (size) {
+ default:
+ l_free(msg);
+ return;
+ case 4:
+ n = mesh_model_opcode_set(OP_MODEL_APP_LIST, msg);
+ status = msg + n;
+ mod_id = l_get_le16(pkt + 2);
+ l_put_le16(ele_addr, msg + 1 + n);
+ l_put_le16(mod_id, msg + 3 + n);
+ n += 5;
+ break;
+ case 6:
+ n = mesh_model_opcode_set(OP_VEND_MODEL_APP_LIST, msg);
+ status = msg + n;
+ mod_id = l_get_le16(pkt + 2) << 16;
+ mod_id |= l_get_le16(pkt + 4);
+
+ l_put_le16(ele_addr, msg + 1 + n);
+ l_put_le16(mod_id >> 16, msg + 3 + n);
+ l_put_le16(mod_id, msg + 5 + n);
+ n += 7;
+ break;
+ }
+
+
+ result = mesh_model_get_bindings(net, ele_addr, mod_id, msg + n,
+ buf_size, &size);
+ n += size;
+
+ if (result >= 0) {
+ *status = result;
+ mesh_model_send(net, CONFIG_SRV_MODEL, dst, src, APP_IDX_DEV,
+ DEFAULT_TTL, msg, n);
+ }
+
+ l_free(msg);
+}
+
+static bool model_app_bind(struct mesh_net *net, uint16_t src, uint16_t dst,
+ const uint8_t *pkt, uint16_t size,
+ bool unbind)
+{
+ uint16_t ele_addr;
+ uint32_t mod_id;
+ uint16_t idx;
+ int result;
+
+ switch (size) {
+ default:
+ return false;
+
+ case 6:
+ mod_id = l_get_le16(pkt + 4);
+ break;
+ case 8:
+ mod_id = l_get_le16(pkt + 4) << 16;
+ mod_id |= l_get_le16(pkt + 6);
+ break;
+ }
+
+ ele_addr = l_get_le16(pkt);
+ idx = l_get_le16(pkt + 2);
+
+ if (idx > 0xfff)
+ return false;
+
+ if (unbind)
+ result = mesh_model_binding_del(net, ele_addr, mod_id, idx);
+ else
+ result = mesh_model_binding_add(net, ele_addr, mod_id, idx);
+
+ send_model_app_status(net, src, dst, result, ele_addr, mod_id, idx);
+
+ return true;
+}
+
+static void hb_pub_timeout_func(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net);
+
+ mesh_net_heartbeat_send(net);
+
+ if (hb->pub_count != 0xffff)
+ hb->pub_count--;
+ if (hb->pub_count > 0)
+ l_timeout_modify(hb->pub_timer, hb->pub_period);
+ else {
+ l_timeout_remove(hb->pub_timer);
+ hb->pub_timer = NULL;
+ }
+ l_debug("%d left", hb->pub_count);
+}
+
+static void update_hb_pub_timer(struct mesh_net *net,
+ struct mesh_net_heartbeat *hb)
+{
+ if (IS_UNASSIGNED(hb->pub_dst) || hb->pub_count == 0) {
+ l_timeout_remove(hb->pub_timer);
+ hb->pub_timer = NULL;
+ return;
+ }
+
+ if (!hb->pub_timer)
+ hb->pub_timer = l_timeout_create(hb->pub_period,
+ hb_pub_timeout_func, net, NULL);
+ else
+ l_timeout_modify(hb->pub_timer, hb->pub_period);
+}
+
+static void hb_sub_timeout_func(struct l_timeout *timeout, void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net);
+
+ l_info("HB Subscription Ended");
+ l_timeout_remove(hb->sub_timer);
+ hb->sub_timer = NULL;
+ hb->sub_enabled = false;
+}
+
+static uint8_t uint32_to_log(uint32_t value)
+{
+ uint32_t val = 1;
+ uint8_t ret = 1;
+
+ if (!value)
+ return 0;
+ else if (value > 0x10000)
+ return 0xff;
+
+ while (val < value) {
+ val <<= 1;
+ ret++;
+ }
+
+ return ret;
+}
+
+static uint32_t log_to_uint32(uint8_t log, uint8_t offset)
+{
+ if (!log)
+ return 0x0000;
+ else if (log > 0x11)
+ return 0xffff;
+ else
+ return (1 << (log - offset));
+}
+
+
+static int hb_subscription_set(struct mesh_net *net, uint16_t src,
+ uint16_t dst, uint8_t period_log)
+{
+ struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net);
+ struct timeval time_now;
+
+ /* SRC must be Unicast, DST can be any legal address except Virtual */
+ if ((!IS_UNASSIGNED(src) && !IS_UNICAST(src)) || IS_VIRTUAL(dst))
+ return -1;
+
+ /* Check if the subscription should be disabled */
+ if (IS_UNASSIGNED(src) || IS_UNASSIGNED(dst)) {
+ if (IS_GROUP(hb->sub_dst))
+ mesh_net_dst_unreg(net, hb->sub_dst);
+
+ l_timeout_remove(hb->sub_timer);
+ hb->sub_timer = NULL;
+ hb->sub_enabled = false;
+ hb->sub_dst = UNASSIGNED_ADDRESS;
+ hb->sub_src = UNASSIGNED_ADDRESS;
+ hb->sub_count = 0;
+ hb->sub_period = 0;
+ hb->sub_min_hops = 0;
+ hb->sub_max_hops = 0;
+ return MESH_STATUS_SUCCESS;
+ } else if (!period_log && src == hb->sub_src && dst == hb->sub_dst) {
+ /* Preserve collected data, but disable */
+ l_timeout_remove(hb->sub_timer);
+ hb->sub_timer = NULL;
+ hb->sub_enabled = false;
+ hb->sub_period = 0;
+ return MESH_STATUS_SUCCESS;
+ }
+
+ if (hb->sub_dst != dst) {
+ if (IS_GROUP(hb->sub_dst))
+ mesh_net_dst_unreg(net, hb->sub_dst);
+ if (IS_GROUP(dst))
+ mesh_net_dst_reg(net, dst);
+ }
+
+ hb->sub_enabled = !!period_log;
+ hb->sub_src = src;
+ hb->sub_dst = dst;
+ hb->sub_count = 0;
+ hb->sub_period = log_to_uint32(period_log, 1);
+ hb->sub_min_hops = 0x00;
+ hb->sub_max_hops = 0x00;
+
+ gettimeofday(&time_now, NULL);
+ hb->sub_start = time_now.tv_sec;
+
+ if (!hb->sub_enabled) {
+ l_timeout_remove(hb->sub_timer);
+ hb->sub_timer = NULL;
+ return MESH_STATUS_SUCCESS;
+ }
+
+ hb->sub_min_hops = 0xff;
+
+ if (!hb->sub_timer)
+ hb->sub_timer = l_timeout_create(hb->sub_period,
+ hb_sub_timeout_func, net, NULL);
+ else
+ l_timeout_modify(hb->sub_timer, hb->sub_period);
+
+ return MESH_STATUS_SUCCESS;
+}
+
+static void node_reset(struct l_timeout *timeout, void *user_data)
+{
+ l_info("Node Reset");
+ l_timeout_remove(timeout);
+ l_main_quit();
+}
+
+static bool cfg_srv_pkt(uint16_t src, uint32_t dst,
+ uint16_t unicast, uint16_t idx,
+ const uint8_t *data, uint16_t size,
+ uint8_t ttl, const void *user_data)
+{
+ struct mesh_net *net = (struct mesh_net *) user_data;
+ const uint8_t *pkt = data;
+ struct timeval time_now;
+ uint32_t opcode, tmp32;
+ int b_res = MESH_STATUS_SUCCESS;
+ uint8_t msg[11];
+ uint8_t *long_msg = NULL;
+ struct mesh_net_heartbeat *hb;
+ uint16_t net_idx, app_idx;
+ uint8_t state, status;
+ uint8_t phase;
+ bool virt = false;
+ uint8_t count;
+ uint16_t interval;
+ struct mesh_node *node;
+ uint16_t n;
+
+ if (idx != APP_IDX_DEV)
+ return false;
+
+ if (mesh_model_opcode_get(pkt, size, &opcode, &n)) {
+ size -= n;
+ pkt += n;
+ } else
+ return false;
+
+ hb = mesh_net_heartbeat_get(net);
+ l_debug("CONFIG-SRV-opcode 0x%x size %u idx %3.3x", opcode, size, idx);
+
+ node = mesh_net_local_node_get(net);
+ n = 0;
+
+ switch (opcode) {
+ default:
+ return false;
+
+ case OP_DEV_COMP_GET:
+ if (size != 1)
+ return false;
+
+ /* Only page 0 is currently supported */
+ if (pkt[0] != 0) {
+ l_info("Unsupported page number %d", pkt[0]);
+ l_info("Returning page number 0");
+ }
+ long_msg = l_malloc(CFG_MAX_MSG_LEN);
+ n = mesh_model_opcode_set(OP_DEV_COMP_STATUS, long_msg);
+ long_msg[n++] = 0;
+ n += node_generate_comp(node, long_msg + n,
+ CFG_MAX_MSG_LEN - n);
+
+ break;
+
+ case OP_CONFIG_DEFAULT_TTL_SET:
+ if (size != 1 || pkt[0] > TTL_MASK || pkt[0] == 1)
+ return true;
+
+ if (pkt[0] <= TTL_MASK)
+ node_default_ttl_set(node, pkt[0]);
+ /* Fall Through */
+
+ case OP_CONFIG_DEFAULT_TTL_GET:
+ l_info("Get/Set Default TTL");
+
+ n = mesh_model_opcode_set(OP_CONFIG_DEFAULT_TTL_STATUS, msg);
+ msg[n++] = node_default_ttl_get(node);
+ break;
+
+ case OP_CONFIG_MODEL_PUB_VIRT_SET:
+ if (size != 25 && size != 27)
+ return true;
+
+ config_pub_set(net, src, unicast, pkt, size,
+ !!(opcode & OP_UNRELIABLE));
+ break;
+
+ case OP_CONFIG_MODEL_PUB_SET:
+ if (size != 11 && size != 13)
+ return true;
+
+ config_pub_set(net, src, unicast, pkt, size,
+ !!(opcode & OP_UNRELIABLE));
+ break;
+
+ case OP_CONFIG_MODEL_PUB_GET:
+ config_pub_get(net, src, unicast, pkt, size);
+ break;
+
+ case OP_CONFIG_VEND_MODEL_SUB_GET:
+ if (size != 6)
+ return true;
+ config_sub_get(net, src, unicast, pkt, size);
+ break;
+
+ case OP_CONFIG_MODEL_SUB_GET:
+ if (size != 4)
+ return true;
+ config_sub_get(net, src, unicast, pkt, size);
+ break;
+
+ case OP_CONFIG_MODEL_SUB_VIRT_OVERWRITE:
+ case OP_CONFIG_MODEL_SUB_VIRT_DELETE:
+ case OP_CONFIG_MODEL_SUB_VIRT_ADD:
+ virt = true;
+ /* Fall Through */
+ case OP_CONFIG_MODEL_SUB_OVERWRITE:
+ case OP_CONFIG_MODEL_SUB_DELETE:
+ case OP_CONFIG_MODEL_SUB_ADD:
+ case OP_CONFIG_MODEL_SUB_DELETE_ALL:
+ config_sub_set(net, src, unicast, pkt, size, virt, opcode);
+ break;
+
+ case OP_CONFIG_RELAY_SET:
+ if (size != 2 || pkt[0] > 0x01)
+ return true;
+
+ count = (pkt[1] >> 5) + 1;
+ interval = ((pkt[1] & 0x1f) + 1) * 10;
+ node_relay_mode_set(node, !!pkt[0], pkt[1]>>5,
+ pkt[1] & 0x1f);
+ /* Fall Through */
+
+ case OP_CONFIG_RELAY_GET:
+ n = mesh_model_opcode_set(OP_CONFIG_RELAY_STATUS, msg);
+
+ msg[n++] = node_relay_mode_get(node, &count, &interval);
+ msg[n++] = ((count - 1) << 5) + ((interval/10 - 1) & 0x1f);
+
+ l_info("Get/Set Relay Config (%d)", msg[n-1]);
+ break;
+
+ case OP_CONFIG_NETWORK_TRANSMIT_SET:
+ if (size != 1)
+ return true;
+
+ count = (pkt[0] >> 5) + 1;
+ interval = ((pkt[0] & 0x1f) + 1) * 10;
+ if (storage_local_set_transmit_params(net, count, interval))
+ mesh_net_transmit_params_set(net, count, interval);
+ /* Fall Through */
+
+ case OP_CONFIG_NETWORK_TRANSMIT_GET:
+ n = mesh_model_opcode_set(OP_CONFIG_NETWORK_TRANSMIT_STATUS,
+ msg);
+ mesh_net_transmit_params_get(net, &count, &interval);
+ msg[n++] = ((count - 1) << 5) + ((interval/10 - 1) & 0x1f);
+
+ l_info("Get/Set Network Transmit Config");
+ break;
+
+ case OP_CONFIG_PROXY_SET:
+ if (size != 1 || pkt[0] > 0x01)
+ return true;
+
+ node_proxy_mode_set(node, !!pkt[0]);
+ /* Fall Through */
+
+ case OP_CONFIG_PROXY_GET:
+ n = mesh_model_opcode_set(OP_CONFIG_PROXY_STATUS, msg);
+
+ msg[n++] = node_proxy_mode_get(node);
+ l_info("Get/Set Config Proxy (%d)", msg[n-1]);
+ break;
+
+ case OP_NODE_IDENTITY_SET:
+ if (size != 3 || pkt[2] > 0x01)
+ return true;
+
+ net_idx = l_get_le16(pkt);
+ if (net_idx > 0xfff)
+ return true;
+
+ /*
+ * Currently no support for proxy: node identity not supported
+ */
+
+ /* Fall Through */
+
+ case OP_NODE_IDENTITY_GET:
+ if (size < 2)
+ return true;
+
+ net_idx = l_get_le16(pkt);
+ if (net_idx > 0xfff)
+ return true;
+
+ n = mesh_model_opcode_set(OP_NODE_IDENTITY_STATUS, msg);
+
+ status = mesh_net_get_identity_mode(net, net_idx, &state);
+
+ msg[n++] = status;
+
+ l_put_le16(net_idx, msg + n);
+ n += 2;
+
+ msg[n++] = state;
+ l_info("Get/Set Config Identity (%d)", state);
+ break;
+
+ case OP_CONFIG_BEACON_SET:
+ if (size != 1 || pkt[0] > 0x01)
+ return true;
+
+ node_beacon_mode_set(node, !!pkt[0]);
+ /* Fall Through */
+
+ case OP_CONFIG_BEACON_GET:
+ n = mesh_model_opcode_set(OP_CONFIG_BEACON_STATUS, msg);
+
+ msg[n++] = node_beacon_mode_get(node);
+ l_info("Get/Set Config Beacon (%d)", msg[n-1]);
+ break;
+
+ case OP_CONFIG_FRIEND_SET:
+ if (size != 1 || pkt[0] > 0x01)
+ return true;
+
+ node_friend_mode_set(node, !!pkt[0]);
+ /* Fall Through */
+
+ case OP_CONFIG_FRIEND_GET:
+
+ n = mesh_model_opcode_set(OP_CONFIG_FRIEND_STATUS, msg);
+
+ msg[n++] = node_friend_mode_get(node);
+ l_info("Get/Set Friend (%d)", msg[n-1]);
+ break;
+
+ case OP_CONFIG_KEY_REFRESH_PHASE_SET:
+ if (size != 3 || pkt[2] > 0x03)
+ return true;
+
+ b_res = mesh_net_key_refresh_phase_set(net, l_get_le16(pkt),
+ pkt[2]);
+ size = 2;
+ /* Fall Through */
+
+ case OP_CONFIG_KEY_REFRESH_PHASE_GET:
+ if (size != 2)
+ return true;
+
+ net_idx = l_get_le16(pkt);
+
+ n = mesh_model_opcode_set(OP_CONFIG_KEY_REFRESH_PHASE_STATUS,
+ msg);
+
+ /* State: 0x00-0x03 phase of key refresh */
+ status = mesh_net_key_refresh_phase_get(net, net_idx,
+ &phase);
+ if (status != MESH_STATUS_SUCCESS) {
+ b_res = status;
+ phase = KEY_REFRESH_PHASE_NONE;
+ }
+
+ msg[n++] = b_res;
+ l_put_le16(net_idx, msg + n);
+ n += 2;
+ msg[n++] = phase;
+
+ l_info("Get/Set Key Refresh State (%d)", msg[n-1]);
+ break;
+
+ case OP_APPKEY_ADD:
+ case OP_APPKEY_UPDATE:
+ if (size != 19)
+ return true;
+
+ net_idx = l_get_le16(pkt) & 0xfff;
+ app_idx = l_get_le16(pkt + 1) >> 4;
+ b_res = appkey_key_add(net, net_idx, app_idx, pkt + 3,
+ opcode == OP_APPKEY_UPDATE);
+
+ l_info("Add/Update AppKey %s: Net_Idx %3.3x, App_Idx %3.3x",
+ (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail",
+ net_idx, app_idx);
+
+
+ n = mesh_model_opcode_set(OP_APPKEY_STATUS, msg);
+
+ msg[n++] = b_res;
+ msg[n++] = pkt[0];
+ msg[n++] = pkt[1];
+ msg[n++] = pkt[2];
+ break;
+
+ case OP_APPKEY_DELETE:
+ if (size != 3)
+ return
+ true;
+
+ net_idx = l_get_le16(pkt) & 0xfff;
+ app_idx = l_get_le16(pkt + 1) >> 4;
+ b_res = appkey_key_delete(net, net_idx, app_idx);
+ if (b_res == MESH_STATUS_SUCCESS)
+ node_app_key_delete(net, dst, net_idx, app_idx);
+ l_info("Delete AppKey %s Net_Idx %3.3x to App_Idx %3.3x",
+ (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail",
+ net_idx, app_idx);
+
+ n = mesh_model_opcode_set(OP_APPKEY_STATUS, msg);
+ msg[n++] = b_res;
+ msg[n++] = pkt[0];
+ msg[n++] = pkt[1];
+ msg[n++] = pkt[2];
+ break;
+
+ case OP_APPKEY_GET:
+ if (size != 2)
+ return true;
+ net_idx = l_get_le16(pkt);
+
+ long_msg = l_malloc(CFG_MAX_MSG_LEN);
+ n = mesh_model_opcode_set(OP_APPKEY_LIST, long_msg);
+
+ status = appkey_list(net, net_idx, long_msg + n + 3,
+ CFG_MAX_MSG_LEN - n - 3, &size);
+
+ long_msg[n] = status;
+ l_put_le16(net_idx, long_msg + n + 1);
+ n += (size + 3);
+ break;
+
+ case OP_NETKEY_ADD:
+ case OP_NETKEY_UPDATE:
+ if (size != 18)
+ return true;
+
+ b_res = mesh_net_add_key(net, opcode == OP_NETKEY_UPDATE,
+ l_get_le16(pkt), pkt + 2);
+
+ l_info("NetKey Add/Update %s",
+ (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail");
+
+ n = mesh_model_opcode_set(OP_NETKEY_STATUS, msg);
+ msg[n++] = b_res;
+ l_put_le16(l_get_le16(pkt), msg + n);
+ n += 2;
+ break;
+
+ case OP_NETKEY_DELETE:
+ if (size != 2)
+ return true;
+
+ b_res = mesh_net_del_key(net, l_get_le16(pkt));
+
+ l_info("NetKey delete %s",
+ (b_res == MESH_STATUS_SUCCESS) ? "success" : "fail");
+
+ n = mesh_model_opcode_set(OP_NETKEY_STATUS, msg);
+ msg[n++] = b_res;
+ l_put_le16(l_get_le16(pkt), msg + n);
+ n += 2;
+ break;
+
+ case OP_NETKEY_GET:
+ long_msg = l_malloc(CFG_MAX_MSG_LEN);
+ n = mesh_model_opcode_set(OP_NETKEY_LIST, long_msg);
+ size = CFG_MAX_MSG_LEN - n;
+
+ if (mesh_net_key_list_get(net, long_msg + n, &size))
+ n += size;
+ else
+ n = 0;
+ break;
+
+ case OP_MODEL_APP_BIND:
+ case OP_MODEL_APP_UNBIND:
+ model_app_bind(net, src, unicast, pkt, size,
+ opcode != OP_MODEL_APP_BIND);
+ break;
+
+ case OP_VEND_MODEL_APP_GET:
+ if (size != 6)
+ return true;
+ model_app_list(net, src, unicast, pkt, size);
+ break;
+
+ case OP_MODEL_APP_GET:
+ if (size != 4)
+ return true;
+ model_app_list(net, src, unicast, pkt, size);
+ break;
+
+ case OP_CONFIG_HEARTBEAT_PUB_SET:
+ l_info("OP_CONFIG_HEARTBEAT_PUB_SET");
+ if (size != 9) {
+ l_info("bad size %d", size);
+ return true;
+ }
+ if (pkt[2] > 0x11 || pkt[3] > 0x10 || pkt[4] > 0x7f)
+ return true;
+ else if (IS_VIRTUAL(l_get_le16(pkt)))
+ b_res = MESH_STATUS_INVALID_ADDRESS;
+ else if (l_get_le16(pkt + 7) != mesh_net_get_primary_idx(net))
+ /* Future work: check for valid subnets */
+ b_res = MESH_STATUS_INVALID_NETKEY;
+
+ n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_PUB_STATUS,
+ msg);
+ msg[n++] = b_res;
+
+ memcpy(&msg[n], pkt, 9);
+
+ /* Ignore RFU bits in features */
+ l_put_le16(l_get_le16(pkt + 5) & 0xf, &msg[n + 5]);
+
+ /* Add octet count to status */
+ n += 9;
+
+ if (b_res != MESH_STATUS_SUCCESS)
+ break;
+
+ hb->pub_dst = l_get_le16(pkt);
+ if (hb->pub_dst == UNASSIGNED_ADDRESS ||
+ pkt[2] == 0 || pkt[3] == 0) {
+ /*
+ * We might still have a pub_dst here in case
+ * we need it for State Change heartbeat
+ */
+ hb->pub_count = 0;
+ hb->pub_period = 0;
+ } else {
+ hb->pub_count = (pkt[2] != 0xff) ?
+ log_to_uint32(pkt[2], 1) : 0xffff;
+ hb->pub_period = log_to_uint32(pkt[3], 1);
+ }
+
+ hb->pub_ttl = pkt[4];
+ hb->pub_features = l_get_le16(pkt + 5) & 0xf;
+ hb->pub_net_idx = l_get_le16(pkt + 7);
+ update_hb_pub_timer(net, hb);
+
+ break;
+
+ case OP_CONFIG_HEARTBEAT_PUB_GET:
+ n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_PUB_STATUS, msg);
+ msg[n++] = b_res;
+ l_put_le16(hb->pub_dst, msg + n);
+ n += 2;
+ msg[n++] = uint32_to_log(hb->pub_count);
+ msg[n++] = uint32_to_log(hb->pub_period);
+ msg[n++] = hb->pub_ttl;
+ l_put_le16(hb->pub_features, msg + n);
+ n += 2;
+ l_put_le16(hb->pub_net_idx, msg + n);
+ n += 2;
+ break;
+
+ case OP_CONFIG_HEARTBEAT_SUB_SET:
+ if (size != 5)
+ return true;
+
+ l_info("Set Sub Period (Log %2.2x) %d sec",
+ pkt[4], log_to_uint32(pkt[4], 1));
+
+ b_res = hb_subscription_set(net, l_get_le16(pkt),
+ l_get_le16(pkt + 2),
+ pkt[4]);
+ if (b_res < 0)
+ return true;
+
+ /* Fall through */
+
+ case OP_CONFIG_HEARTBEAT_SUB_GET:
+ gettimeofday(&time_now, NULL);
+ time_now.tv_sec -= hb->sub_start;
+
+ if (time_now.tv_sec >= hb->sub_period)
+ time_now.tv_sec = 0;
+ else
+ time_now.tv_sec = hb->sub_period - time_now.tv_sec;
+
+ l_info("Sub Period (Log %2.2x) %d sec",
+ uint32_to_log(time_now.tv_sec),
+ (int) time_now.tv_sec);
+
+ n = mesh_model_opcode_set(OP_CONFIG_HEARTBEAT_SUB_STATUS, msg);
+ msg[n++] = b_res;
+ l_put_le16(hb->sub_src, msg + n);
+ n += 2;
+ l_put_le16(hb->sub_dst, msg + n);
+ n += 2;
+ msg[n++] = uint32_to_log(time_now.tv_sec);
+ msg[n++] = uint32_to_log(hb->sub_count);
+ msg[n++] = hb->sub_count ? hb->sub_min_hops : 0;
+ msg[n++] = hb->sub_max_hops;
+ break;
+
+ case OP_CONFIG_POLL_TIMEOUT_LIST:
+ if (size != 2 || l_get_le16(pkt) == 0 ||
+ l_get_le16(pkt) > 0x7fff)
+ return true;
+
+ n = mesh_model_opcode_set(OP_CONFIG_POLL_TIMEOUT_STATUS, msg);
+ l_put_le16(l_get_le16(pkt), msg + n);
+ n += 2;
+ tmp32 = mesh_net_friend_timeout(net, l_get_le16(pkt));
+ msg[n++] = tmp32;
+ msg[n++] = tmp32 >> 8;
+ msg[n++] = tmp32 >> 16;
+ break;
+
+ case OP_NODE_RESET:
+ n = mesh_model_opcode_set(OP_NODE_RESET_STATUS, msg);
+ l_timeout_create(1, node_reset, net, NULL);
+ break;
+ }
+
+ if (n) {
+ /* print_packet("App Tx", long_msg ? long_msg : msg, n); */
+ mesh_model_send(net, CONFIG_SRV_MODEL,
+ unicast, src,
+ APP_IDX_DEV, DEFAULT_TTL,
+ long_msg ? long_msg : msg, n);
+ }
+ l_free(long_msg);
+
+ return true;
+}
+
+static void cfgmod_srv_unregister(void *user_data)
+{
+ struct mesh_net *net = user_data;
+ struct mesh_net_heartbeat *hb = mesh_net_heartbeat_get(net);
+
+ l_timeout_remove(hb->pub_timer);
+ l_timeout_remove(hb->sub_timer);
+ hb->pub_timer = hb->sub_timer = NULL;
+}
+
+static const struct mesh_model_ops ops = {
+ .unregister = cfgmod_srv_unregister,
+ .recv = cfg_srv_pkt,
+ .bind = NULL,
+ .sub = NULL,
+ .pub = NULL
+};
+
+void mesh_config_srv_init(struct mesh_net *net, uint8_t ele_idx)
+{
+ l_debug("%2.2x", ele_idx);
+ mesh_model_register(net, ele_idx, CONFIG_SRV_MODEL, &ops, net);
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 12/14] mesh: Read and write mesh configuration in JSON format
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (10 preceding siblings ...)
2018-07-14 19:59 ` [PATCH BlueZ v7 11/14] mesh: Mesh config server model Brian Gix
@ 2018-07-14 19:59 ` Brian Gix
2018-07-14 19:59 ` [PATCH BlueZ v7 13/14] mesh: Sample device composition " Brian Gix
` (2 subsequent siblings)
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:59 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
This adds implementation for parsing and writing Mesh configuration
into JSON format. Also, parse stored unprovisioned device composiotion.
---
mesh/mesh-db.c | 1360 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
mesh/mesh-db.h | 144 ++++++
2 files changed, 1504 insertions(+)
create mode 100644 mesh/mesh-db.c
create mode 100644 mesh/mesh-db.h
diff --git a/mesh/mesh-db.c b/mesh/mesh-db.c
new file mode 100644
index 000000000..7bddab010
--- /dev/null
+++ b/mesh/mesh-db.c
@@ -0,0 +1,1360 @@
+/*
+ *
+ * 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 <errno.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <ell/ell.h>
+
+#include "mesh/mesh-defs.h"
+#include "mesh/util.h"
+
+#include "mesh/mesh-db.h"
+
+#define CHECK_KEY_IDX_RANGE(x) (((x) >= 0) && ((x) <= 4095))
+
+static bool get_int(json_object *jobj, const char *keyword, int *value)
+{
+ json_object *jvalue;
+
+ if (!json_object_object_get_ex(jobj, keyword, &jvalue))
+ return false;
+
+ *value = json_object_get_int(jvalue);
+ if (errno == EINVAL)
+ return false;
+
+ return true;
+}
+
+static bool add_key(json_object *jobject, const char *desc,
+ const uint8_t key[16])
+{
+ json_object *jstring;
+ char hexstr[33];
+
+ hex2str((uint8_t *) key, 16, hexstr, 33);
+ jstring = json_object_new_string(hexstr);
+ if (!jstring)
+ return false;
+
+ json_object_object_add(jobject, desc, jstring);
+ return true;
+}
+
+static json_object *get_element_model(json_object *jnode, int ele_idx,
+ uint32_t mod_id, bool vendor)
+{
+ json_object *jelements, *jelement, *jmodels;
+ int i, num_mods;
+ size_t len;
+ char buf[9];
+
+ if (!vendor)
+ snprintf(buf, 5, "%4.4x", (uint16_t)mod_id);
+ else
+ snprintf(buf, 9, "%8.8x", mod_id);
+
+ json_object_object_get_ex(jnode, "elements", &jelements);
+ if (!jelements)
+ return NULL;
+
+ jelement = json_object_array_get_idx(jelements, ele_idx);
+ if (!jelement)
+ return NULL;
+
+ json_object_object_get_ex(jelement, "models", &jmodels);
+ if (!jmodels)
+ return NULL;
+
+ num_mods = json_object_array_length(jmodels);
+ if (!num_mods)
+ return NULL;
+
+ if (!vendor) {
+ snprintf(buf, 5, "%4.4x", mod_id);
+ len = 4;
+ } else {
+ snprintf(buf, 9, "%8.8x", mod_id);
+ len = 8;
+ }
+
+ for (i = 0; i < num_mods; ++i) {
+ json_object *jmodel, *jvalue;
+ char *str;
+
+ jmodel = json_object_array_get_idx(jmodels, i);
+ json_object_object_get_ex(jmodel, "modelId", &jvalue);
+ if (!jvalue)
+ return NULL;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str)
+ return NULL;
+
+ if (!strncmp(str, buf, len))
+ return jmodel;
+ }
+
+ return NULL;
+}
+
+static bool jarray_has_string(json_object *jarray, char *str, size_t len)
+{
+ int i, sz = json_object_array_length(jarray);
+
+ for (i = 0; i < sz; ++i) {
+ json_object *jentry;
+ char *str_entry;
+
+ jentry = json_object_array_get_idx(jarray, i);
+ str_entry = (char *)json_object_get_string(jentry);
+ if (!str_entry)
+ continue;
+
+ if (!strncmp(str, str_entry, len))
+ return true;
+ }
+
+ return false;
+}
+
+static json_object *jarray_string_del(json_object *jarray, char *str,
+ size_t len)
+{
+ int i, sz = json_object_array_length(jarray);
+ json_object *jarray_new;
+
+ jarray_new = json_object_new_array();
+ if (!jarray_new)
+ return NULL;
+
+ for (i = 0; i < sz; ++i) {
+ json_object *jentry;
+ char *str_entry;
+
+ jentry = json_object_array_get_idx(jarray, i);
+ str_entry = (char *)json_object_get_string(jentry);
+ if (str_entry && !strncmp(str, str_entry, len))
+ continue;
+
+ json_object_array_add(jarray_new, jentry);
+ }
+
+ return jarray_new;
+}
+
+static json_object *get_key_object(json_object *jarray, uint16_t idx)
+{
+ int i, sz = json_object_array_length(jarray);
+
+ for (i = 0; i < sz; ++i) {
+ json_object *jentry, *jvalue;
+ uint32_t jidx;
+
+ jentry = json_object_array_get_idx(jarray, i);
+ if (!json_object_object_get_ex(jentry, "index", &jvalue))
+ return NULL;
+
+ jidx = json_object_get_int(jvalue);
+
+ if (jidx == idx)
+ return jentry;
+ }
+
+ return NULL;
+}
+
+static json_object *jarray_key_del(json_object *jarray, int16_t idx)
+{
+ json_object *jarray_new;
+ int i, sz = json_object_array_length(jarray);
+ char idx_str[5];
+
+ snprintf(idx_str, 5, "%4.4x", idx);
+
+ jarray_new = json_object_new_array();
+ if (!jarray_new)
+ return NULL;
+
+ for (i = 0; i < sz; ++i) {
+ json_object *jentry, *jvalue;
+ char *str;
+
+ jentry = json_object_array_get_idx(jarray, i);
+
+ if (json_object_object_get_ex(jentry, "index", &jvalue)) {
+ str = (char *)json_object_get_string(jvalue);
+ if (str && !strncmp(str, idx_str, 4))
+ continue;
+ }
+
+ json_object_array_add(jarray_new, jentry);
+ }
+
+ return jarray_new;
+}
+
+bool mesh_db_read_iv_index(json_object *jobj, uint32_t *idx, bool *update)
+{
+ int tmp;
+
+ /* IV index */
+ if (!get_int(jobj, "IVindex", &tmp))
+ return false;
+
+ *idx = (uint32_t) tmp;
+
+ if (!get_int(jobj, "IVupdate", &tmp))
+ return false;
+
+ *update = tmp ? true : false;
+
+ return true;
+}
+
+bool mesh_db_read_device_key(json_object *jobj, uint8_t key_buf[16])
+{
+ json_object *jvalue;
+ char *str;
+
+ if (!key_buf)
+ return false;
+
+ if (!json_object_object_get_ex(jobj, "deviceKey", &jvalue) ||
+ !jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), key_buf, 16))
+ return false;
+
+ return true;
+}
+
+bool mesh_db_read_app_keys(json_object *jobj, mesh_db_app_key_cb cb,
+ void *user_data)
+{
+ json_object *jarray;
+ int len;
+ int i;
+
+ if (!cb)
+ return true;
+
+ json_object_object_get_ex(jobj, "appKeys", &jarray);
+ if (!jarray || (json_object_get_type(jarray) != json_type_array))
+ return false;
+
+ len = json_object_array_length(jarray);
+
+ for (i = 0; i < len; ++i) {
+ json_object *jtemp, *jvalue;
+ int app_idx, net_idx;
+ bool key_refresh = false;
+ char *str;
+ uint8_t key[16];
+ uint8_t new_key[16];
+
+ jtemp = json_object_array_get_idx(jarray, i);
+
+ if (!get_int(jtemp, "index", &app_idx))
+ return false;
+
+ if (!CHECK_KEY_IDX_RANGE(app_idx))
+ return false;
+
+ if (!get_int(jtemp, "boundNetKey", &net_idx))
+ return false;
+
+ if (!CHECK_KEY_IDX_RANGE(net_idx))
+ return false;
+
+ json_object_object_get_ex(jtemp, "oldKey", &jvalue);
+ if (jvalue) {
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), key, 16))
+ return false;
+ key_refresh = true;
+ }
+
+ json_object_object_get_ex(jtemp, "key", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), key_refresh ? new_key : key, 16))
+ return false;
+
+ if (!cb((uint16_t)net_idx, (uint16_t) app_idx, key,
+ key_refresh ? new_key : NULL, user_data))
+ return false;
+ }
+
+ return true;
+}
+
+bool mesh_db_read_net_keys(json_object *jobj, mesh_db_net_key_cb cb,
+ void *user_data)
+{
+ json_object *jarray;
+ int len;
+ int i;
+
+ if (!cb)
+ return true;
+
+ json_object_object_get_ex(jobj, "netKeys", &jarray);
+ if (!jarray || (json_object_get_type(jarray) != json_type_array))
+ return false;
+
+ len = json_object_array_length(jarray);
+
+ for (i = 0; i < len; ++i) {
+ json_object *jtemp, *jvalue;
+ int idx;
+ char *str;
+ bool key_refresh = false;
+ int phase;
+ uint8_t key[16];
+ uint8_t new_key[16];
+
+ jtemp = json_object_array_get_idx(jarray, i);
+
+ if (!get_int(jtemp, "index", &idx))
+ return false;
+
+ if (!CHECK_KEY_IDX_RANGE(idx))
+ return false;
+
+ json_object_object_get_ex(jtemp, "oldKey", &jvalue);
+ if (jvalue) {
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), key, 16))
+ return false;
+ key_refresh = true;
+ }
+
+ json_object_object_get_ex(jtemp, "key", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), key_refresh ? new_key : key, 16))
+ return false;
+
+ json_object_object_get_ex(jtemp, "keyRefresh", &jvalue);
+ if (!jvalue)
+ phase = KEY_REFRESH_PHASE_NONE;
+ else
+ phase = json_object_get_int(jvalue);
+
+
+ if (!cb((uint16_t)idx, key, key_refresh ? new_key : NULL, phase,
+ user_data))
+ return false;
+ }
+
+ return true;
+}
+
+bool mesh_db_net_key_add(json_object *jobj, uint16_t idx,
+ const uint8_t key[16], int phase)
+{
+ json_object *jarray, *jentry = NULL, *jstring;
+ char buf[5];
+
+ json_object_object_get_ex(jobj, "netKeys", &jarray);
+ if (!jarray && (phase != KEY_REFRESH_PHASE_NONE))
+ return false;
+
+ if (jarray)
+ jentry = get_key_object(jarray, idx);
+
+ /*
+ * The key entry should exist if the key is updated
+ * (i.e., Key Refresh is underway)
+ */
+ if (!jentry && (phase != KEY_REFRESH_PHASE_NONE))
+ return false;
+
+ if (jentry) {
+ uint8_t buf[16];
+ json_object *jvalue;
+ char *str;
+
+ json_object_object_get_ex(jentry, "key", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), buf, sizeof(buf)))
+ return false;
+
+ /* If the same key, return success */
+ if (memcmp(key, buf, 16) == 0)
+ return true;
+
+ return false;
+ }
+
+ if (phase == KEY_REFRESH_PHASE_NONE) {
+ jentry = json_object_new_object();
+ if (!jentry)
+ goto fail;
+
+ snprintf(buf, 5, "%4.4x", idx);
+ jstring = json_object_new_string(buf);
+ if (!jstring)
+ goto fail;
+
+ json_object_object_add(jentry, "index", jstring);
+
+ snprintf(buf, 5, "%4.4x", idx);
+ jstring = json_object_new_string(buf);
+ if (!jstring)
+ goto fail;
+
+ if (!add_key(jentry, "key", key))
+ goto fail;
+
+ if (!jarray) {
+ jarray = json_object_new_array();
+ if (!jarray)
+ goto fail;
+ json_object_object_add(jobj, "netKeys", jarray);
+ }
+
+ json_object_array_add(jarray, jentry);
+
+ } else {
+
+ if (!json_object_object_get_ex(jentry, "key", &jstring))
+ return false;
+
+ json_object_object_add(jentry, "oldKey", jstring);
+ json_object_object_del(jentry, "key");
+
+ if (!add_key(jentry, "key", key))
+ return false;
+ }
+
+
+ json_object_object_add(jentry, "keyRefresh",
+ json_object_new_int(phase));
+
+ return true;
+fail:
+
+ if (jentry)
+ json_object_put(jentry);
+
+ return false;
+}
+
+bool mesh_db_net_key_del(json_object *jobj, uint16_t idx)
+{
+ json_object *jarray, *jarray_new;
+
+ json_object_object_get_ex(jobj, "netKeys", &jarray);
+ if (!jarray)
+ return true;
+
+ /* Check if matching entry exists */
+ if (!get_key_object(jarray, idx))
+ return true;
+
+ if (json_object_array_length(jarray) == 1) {
+ json_object_object_del(jobj, "netKeys");
+ return true;
+ }
+
+ /*
+ * There is no easy way to delete a value from json array.
+ * Create a new copy without specified element and
+ * then remove old array.
+ */
+ jarray_new = jarray_key_del(jarray, idx);
+ if (!jarray_new)
+ return false;
+
+ json_object_object_del(jobj, "netKeys");
+ json_object_object_add(jobj, "netKeys", jarray_new);
+
+ return true;
+}
+
+bool mesh_db_write_device_key(json_object *jnode, uint8_t *key)
+{
+ return add_key(jnode, "deviceKey", key);
+}
+
+bool mesh_db_app_key_add(json_object *jobj, uint16_t net_idx, uint16_t app_idx,
+ const uint8_t key[16], bool update)
+{
+ json_object *jarray, *jentry = NULL, *jstring = NULL;
+ char buf[5];
+
+ json_object_object_get_ex(jobj, "appKeys", &jarray);
+ if (!jarray && update)
+ return false;
+
+ if (jarray)
+ jentry = get_key_object(jarray, app_idx);
+
+ /* The key entry should exist if the key is updated */
+ if (!jentry && update)
+ return false;
+
+ if (jentry) {
+ uint8_t buf[16];
+ json_object *jvalue;
+ char *str;
+
+ json_object_object_get_ex(jentry, "key", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), buf, sizeof(buf)))
+ return false;
+
+ /* If the same key, return success */
+ if (memcmp(key, buf, 16) == 0)
+ return true;
+
+ return false;
+ }
+
+ if (!update) {
+ jentry = json_object_new_object();
+ if (!jentry)
+ goto fail;
+
+ snprintf(buf, 5, "%4.4x", app_idx);
+ jstring = json_object_new_string(buf);
+ if (!jstring)
+ goto fail;
+
+ json_object_object_add(jentry, "index", jstring);
+
+ snprintf(buf, 5, "%4.4x", net_idx);
+ jstring = json_object_new_string(buf);
+ if (!jstring)
+ goto fail;
+
+ json_object_object_add(jentry, "boundNetKey", jstring);
+
+ if (!add_key(jentry, "key", key))
+ goto fail;
+
+ if (!jarray) {
+ jarray = json_object_new_array();
+ if (!jarray)
+ goto fail;
+ json_object_object_add(jobj, "appKeys", jarray);
+ }
+
+ json_object_array_add(jarray, jentry);
+
+ } else {
+
+ if (!json_object_object_get_ex(jentry, "key", &jstring))
+ return false;
+
+ json_object_object_add(jentry, "oldKey", jstring);
+ json_object_object_del(jentry, "key");
+
+ if (!add_key(jentry, "key", key))
+ return false;
+ }
+
+ return true;
+fail:
+
+ if (jentry)
+ json_object_put(jentry);
+
+ return false;
+}
+
+bool mesh_db_app_key_del(json_object *jobj, uint16_t net_idx, uint16_t idx)
+{
+ json_object *jarray, *jarray_new;
+
+ json_object_object_get_ex(jobj, "appKeys", &jarray);
+ if (!jarray)
+ return true;
+
+ /* Check if matching entry exists */
+ if (!get_key_object(jarray, idx))
+ return true;
+
+ if (json_object_array_length(jarray) == 1) {
+ json_object_object_del(jobj, "appKeys");
+ return true;
+ }
+
+ /*
+ * There is no easy way to delete a value from json array.
+ * Create a new copy without specified element and
+ * then remove old array.
+ */
+ jarray_new = jarray_key_del(jarray, idx);
+ if (!jarray_new)
+ return false;
+
+ json_object_object_del(jobj, "appKeys");
+ json_object_object_add(jobj, "appKeys", jarray_new);
+
+ return true;
+}
+
+bool mesh_db_model_binding_add(json_object *jnode, uint8_t ele_idx, bool vendor,
+ uint32_t mod_id, uint16_t app_idx)
+{
+ json_object *jmodel, *jstring, *jarray;
+ char buf[5];
+
+ jmodel = get_element_model(jnode, ele_idx, mod_id, vendor);
+ if (!jmodel)
+ return false;
+
+ json_object_object_get_ex(jmodel, "bind", &jarray);
+
+ snprintf(buf, 5, "%4.4x", app_idx);
+
+ if (jarray && jarray_has_string(jarray, buf, 4))
+ return true;
+
+ jstring = json_object_new_string(buf);
+ if (!jstring)
+ return false;
+
+ if (!jarray) {
+ jarray = json_object_new_array();
+ if (!jarray) {
+ json_object_put(jstring);
+ return false;
+ }
+ json_object_object_add(jmodel, "bind", jarray);
+ }
+
+ json_object_array_add(jarray, jstring);
+
+ return true;
+}
+
+bool mesh_db_model_binding_del(json_object *jnode, uint8_t ele_idx, bool vendor,
+ uint32_t mod_id, uint16_t app_idx)
+{
+ json_object *jmodel, *jarray, *jarray_new;
+ char buf[5];
+
+ jmodel = get_element_model(jnode, ele_idx, mod_id, vendor);
+ if (!jmodel)
+ return false;
+
+ json_object_object_get_ex(jmodel, "bind", &jarray);
+
+ snprintf(buf, 5, "%4.4x", app_idx);
+
+ if (!jarray || !jarray_has_string(jarray, buf, 4))
+ return true;
+
+ if (json_object_array_length(jarray) == 1) {
+ json_object_object_del(jmodel, "bind");
+ return true;
+ }
+
+ /*
+ * There is no easy way to delete a value from json array.
+ * Create a new copy without specified element and
+ * then remove old array.
+ */
+ jarray_new = jarray_string_del(jarray, buf, 4);
+ if (!jarray_new)
+ return false;
+
+ json_object_object_del(jmodel, "bind");
+ json_object_object_add(jmodel, "bind", jarray_new);
+
+ return true;
+}
+
+static void free_model(void *data)
+{
+ struct mesh_db_model *mod = data;
+
+ l_free(mod->bindings);
+ l_free(mod->subs);
+ l_free(mod->pub);
+ l_free(mod);
+}
+
+static void free_element(void *data)
+{
+ struct mesh_db_element *ele = data;
+
+ l_queue_destroy(ele->models, free_model);
+ l_free(ele);
+}
+
+static bool parse_bindings(json_object *jbindings, struct mesh_db_model *mod)
+{
+ int cnt;
+ int i;
+
+ cnt = json_object_array_length(jbindings);
+ if (cnt > 0xffff)
+ return false;
+
+ mod->num_subs = cnt;
+
+ /* Allow empty bindings list */
+ if (!cnt)
+ return true;
+
+ mod->bindings = l_new(uint16_t, cnt);
+ if (!mod->bindings)
+ return false;
+
+ for (i = 0; i < cnt; ++i) {
+ int idx;
+ json_object *jvalue;
+
+ jvalue = json_object_array_get_idx(jbindings, i);
+ if (!jvalue)
+ return false;
+
+ idx = json_object_get_int(jvalue);
+ if (!CHECK_KEY_IDX_RANGE(idx))
+ return false;
+
+ mod->bindings[i] = (uint16_t) idx;
+ }
+
+ return true;
+}
+
+static bool parse_models(json_object *jmodels, struct mesh_db_element *ele)
+{
+ int i, num_models;
+
+ num_models = json_object_array_length(jmodels);
+ if (!num_models)
+ return true;
+
+ for (i = 0; i < num_models; ++i) {
+ json_object *jmodel, *jarray, *jvalue;
+ struct mesh_db_model *mod;
+ uint32_t id;
+ int len;
+ char *str;
+
+ jmodel = json_object_array_get_idx(jmodels, i);
+ if (!jmodel)
+ goto fail;
+
+ mod = l_new(struct mesh_db_model, 1);
+ if (!ele)
+ goto fail;
+
+ json_object_object_get_ex(jmodel, "modelId", &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+
+ len = strlen(str);
+
+ if (len != 4 && len != 8)
+ goto fail;
+
+ if (len == 4) {
+ if (sscanf(str, "%04x", &id) != 1)
+ goto fail;
+
+ id |= VENDOR_ID_MASK;
+ } else if (len == 8) {
+ if (sscanf(str, "%08x", &id) != 1)
+ goto fail;
+ } else
+ goto fail;
+
+ mod->id = id;
+
+ if (len == 8)
+ mod->vendor = true;
+
+ json_object_object_get_ex(jmodel, "bind", &jarray);
+
+ if (jarray && (json_object_get_type(jmodels) != json_type_array
+ || !parse_bindings(jarray, mod)))
+ goto fail;
+
+ /* TODO add pub/sub */
+ l_queue_push_tail(ele->models, mod);
+ }
+
+ return true;
+
+fail:
+ l_queue_destroy(ele->models, free_model);
+ return false;
+}
+
+static bool parse_elements(json_object *jelements, struct mesh_db_node *node)
+{
+ int i, num_ele;
+
+ num_ele = json_object_array_length(jelements);
+ if (!num_ele)
+ /* Allow "empty" nodes */
+ return true;
+
+ node->elements = l_queue_new();
+ if (!node->elements)
+ return false;
+
+ for (i = 0; i < num_ele; ++i) {
+ json_object *jelement;
+ json_object *jmodels;
+ json_object *jvalue;
+ struct mesh_db_element *ele;
+ int index;
+ char *str;
+
+ jelement = json_object_array_get_idx(jelements, i);
+ if (!jelement)
+ goto fail;
+
+ if (!get_int(jelement, "elementIndex", &index) ||
+ index > num_ele)
+ goto fail;
+
+ ele = l_new(struct mesh_db_element, 1);
+ if (!ele)
+ goto fail;
+
+ ele->index = index;
+ ele->models = l_queue_new();
+ if (!ele->models)
+ goto fail;
+
+ json_object_object_get_ex(jelement, "location", &jvalue);
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &(ele->location)) != 1)
+ goto fail;
+
+ json_object_object_get_ex(jelement, "models", &jmodels);
+
+ if (jmodels && (json_object_get_type(jmodels) != json_type_array
+ || !parse_models(jmodels, ele)))
+ goto fail;
+
+ l_queue_push_tail(node->elements, ele);
+ }
+
+ return true;
+
+fail:
+ l_queue_destroy(node->elements, free_element);
+ node->elements = NULL;
+
+ return false;
+}
+
+static int get_mode(json_object *jvalue)
+{
+ const char *str;
+
+ str = json_object_get_string(jvalue);
+ if (!str)
+ return 0xffffffff;
+
+ if (!strncasecmp(str, "disabled", strlen("disabled")))
+ return MESH_MODE_DISABLED;
+
+ if (!strncasecmp(str, "enabled", strlen("enabled")))
+ return MESH_MODE_ENABLED;
+
+ if (!strncasecmp(str, "unsupported", strlen("unsupported")))
+ return MESH_MODE_UNSUPPORTED;
+
+ return 0xffffffff;
+}
+
+static void parse_features(json_object *jconfig, struct mesh_db_node *node)
+{
+ json_object *jvalue, *jrelay;
+ int mode, count;
+ uint16_t interval;
+
+ json_object_object_get_ex(jconfig, "proxy", &jvalue);
+ if (jvalue) {
+ mode = get_mode(jvalue);
+ if (mode <= MESH_MODE_UNSUPPORTED)
+ node->modes.proxy = mode;
+ }
+
+ json_object_object_get_ex(jconfig, "friend", &jvalue);
+ if (jvalue) {
+ mode = get_mode(jvalue);
+ if (mode <= MESH_MODE_UNSUPPORTED)
+ node->modes.friend = mode;
+ }
+
+ json_object_object_get_ex(jconfig, "lowPower", &jvalue);
+ if (jvalue) {
+ mode = get_mode(jvalue);
+ if (mode <= MESH_MODE_UNSUPPORTED)
+ node->modes.friend = mode;
+ }
+
+ json_object_object_get_ex(jconfig, "beacon", &jvalue);
+ if (jvalue) {
+ mode = get_mode(jvalue);
+ if (mode <= MESH_MODE_ENABLED)
+ node->modes.beacon = mode;
+ }
+
+ json_object_object_get_ex(jconfig, "relay", &jrelay);
+ if (!jrelay)
+ return;
+
+ json_object_object_get_ex(jrelay, "mode", &jvalue);
+ if (jvalue) {
+ mode = get_mode(jvalue);
+ if (mode <= MESH_MODE_UNSUPPORTED)
+ node->modes.relay.state = mode;
+ else
+ return;
+ } else
+ return;
+
+ json_object_object_get_ex(jrelay, "count", &jvalue);
+ if (!jvalue)
+ return;
+
+ /* TODO: check range */
+ count = json_object_get_int(jvalue);
+ node->modes.relay.cnt = count;
+
+ json_object_object_get_ex(jrelay, "interval", &jvalue);
+ if (!jvalue)
+ return;
+
+ /* TODO: check range */
+ interval = json_object_get_int(jvalue);
+ node->modes.relay.interval = interval;
+}
+
+static bool parse_composition(json_object *jcomp, struct mesh_db_node *node)
+{
+ json_object *jvalue;
+ char *str;
+
+ /* All the fields in node composition are mandatory */
+ json_object_object_get_ex(jcomp, "cid", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &node->cid) != 1)
+ return false;
+
+ json_object_object_get_ex(jcomp, "pid", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &node->pid) != 1)
+ return false;
+
+ json_object_object_get_ex(jcomp, "vid", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &node->vid) != 1)
+ return false;
+
+ json_object_object_get_ex(jcomp, "crpl", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &node->crpl) != 1)
+ return false;
+
+ return true;
+}
+
+static uint16_t get_prov_flags(json_object *jarray, uint16_t max_value)
+{
+ int i, cnt;
+ uint16_t result = 0;
+
+ cnt = json_object_array_length(jarray);
+ if (!cnt)
+ return 0;
+
+ for (i = 0; i < cnt; ++i) {
+ json_object *jvalue;
+ int value;
+
+ jvalue = json_object_array_get_idx(jarray, i);
+ value = json_object_get_int(jvalue);
+ if (value > 16)
+ continue;
+
+ if ((1 << value) > max_value)
+ continue;
+
+ result |= (1 << value);
+ }
+
+ return result;
+}
+
+bool mesh_db_read_node(json_object *jnode, mesh_db_node_cb cb, void *user_data)
+{
+ struct mesh_db_node node;
+ json_object *jvalue;
+ char *str;
+
+ if (!cb) {
+ l_info("Node read callback is required");
+ return false;
+ }
+
+ memset(&node, 0, sizeof(node));
+
+ if (!parse_composition(jnode, &node)) {
+ l_info("Failed to parse local node composition");
+ return false;
+ }
+
+ parse_features(jnode, &node);
+
+ json_object_object_get_ex(jnode, "unicastAddress", &jvalue);
+ if (!jvalue) {
+ l_info("Bad config: Unicast address must be present");
+ return false;
+ }
+
+ str = (char *)json_object_get_string(jvalue);
+ if (sscanf(str, "%04hx", &node.unicast) != 1)
+ return false;
+
+ json_object_object_get_ex(jnode, "defaultTTL", &jvalue);
+ if (jvalue) {
+ int ttl = json_object_get_int(jvalue);
+
+ if (ttl < 0 || ttl == 1 || ttl > DEFAULT_TTL)
+ return false;
+ node.ttl = (uint8_t) ttl;
+ }
+
+ json_object_object_get_ex(jnode, "sequenceNumber", &jvalue);
+ if (jvalue)
+ node.seq_number = json_object_get_int(jvalue);
+
+ json_object_object_get_ex(jnode, "elements", &jvalue);
+ if (jvalue && json_object_get_type(jvalue) == json_type_array) {
+ if (!parse_elements(jvalue, &node))
+ return false;
+ }
+
+ return cb(&node, user_data);
+}
+
+bool mesh_db_read_unprovisioned_device(json_object *jnode, mesh_db_node_cb cb,
+ void *user_data)
+{
+ struct mesh_db_node node;
+ json_object *jvalue;
+ char *str;
+
+ if (!cb) {
+ l_info("Device read callback is required");
+ return false;
+ }
+
+ memset(&node, 0, sizeof(node));
+
+ if (!parse_composition(jnode, &node)) {
+ l_info("Failed to parse local device composition");
+ return false;
+ }
+
+ parse_features(jnode, &node);
+
+ json_object_object_get_ex(jnode, "elements", &jvalue);
+ if (jvalue && json_object_get_type(jvalue) == json_type_array) {
+ if (!parse_elements(jvalue, &node))
+ return false;
+ }
+
+ json_object_object_get_ex(jnode, "UUID", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), node.uuid, 16))
+ return false;
+
+ return cb(&node, user_data);
+}
+
+bool mesh_db_read_prov_info(json_object *jnode, struct mesh_db_prov *prov)
+{
+ json_object *jprov, *jarray, *jvalue, *jobj;
+ int value;
+ char *str;
+
+ if (!prov)
+ return false;
+
+ json_object_object_get_ex(jnode, "provision", &jprov);
+ if (!jprov)
+ return false;
+
+ json_object_object_get_ex(jprov, "algorithms", &jarray);
+ if (!jarray || json_object_get_type(jarray) != json_type_array)
+ return false;
+
+ prov->algorithm = get_prov_flags(jarray, ALG_FIPS_256_ECC);
+ if (!prov->algorithm) {
+ l_info("At least one algorithm must be indicated");
+ return false;
+ }
+
+ json_object_object_get_ex(jprov, "outputOOB", &jobj);
+ json_object_object_get_ex(jobj, "size", &jvalue);
+ value = json_object_get_int(jvalue);
+ if (value > 8)
+ return false;
+
+ prov->output_oob.size = (uint8_t) value;
+ json_object_object_get_ex(jobj, "actions", &jarray);
+ if (!jarray || json_object_get_type(jarray) != json_type_array)
+ return false;
+
+ prov->output_oob.actions = get_prov_flags(jarray, OOB_OUT_ALPHA);
+
+ json_object_object_get_ex(jprov, "inputOOB", &jobj);
+ json_object_object_get_ex(jobj, "size", &jvalue);
+ value = json_object_get_int(jvalue);
+ if (value > 8)
+ return false;
+
+ prov->input_oob.size = (uint8_t) value;
+ json_object_object_get_ex(jobj, "actions", &jarray);
+ if (!jarray || json_object_get_type(jarray) != json_type_array)
+ return false;
+
+ prov->input_oob.actions = get_prov_flags(jarray, OOB_IN_ALPHA);
+
+ json_object_object_get_ex(jprov, "publicType", &jvalue);
+ prov->pub_type = (json_object_get_boolean(jvalue)) ? 1 : 0;
+
+ json_object_object_get_ex(jprov, "staticType", &jvalue);
+ prov->static_type = (json_object_get_boolean(jvalue)) ? 1 : 0;
+
+ json_object_object_get_ex(jprov, "privateKey", &jvalue);
+ if (!jvalue)
+ return false;
+
+ str = (char *)json_object_get_string(jvalue);
+ if (!str2hex(str, strlen(str), prov->priv_key, 32))
+ return false;
+
+ return true;
+}
+
+bool mesh_db_write_uint16_hex(json_object *jobj, const char *desc,
+ uint16_t value)
+{
+ json_object *jstring;
+ char buf[5];
+
+ snprintf(buf, 5, "%4.4x", value);
+ jstring = json_object_new_string(buf);
+ if (!jstring)
+ return false;
+
+ json_object_object_add(jobj, desc, jstring);
+ return true;
+}
+
+bool mesh_db_write_int(json_object *jobj, const char *keyword, int value)
+{
+ json_object *jvalue;
+
+ json_object_object_del(jobj, keyword);
+
+ jvalue = json_object_new_int(value);
+ if (!jvalue)
+ return false;
+
+ json_object_object_add(jobj, keyword, jvalue);
+ return true;
+}
+
+bool mesh_db_write_bool(json_object *jobj, const char *keyword, bool value)
+{
+ json_object *jvalue;
+
+ json_object_object_del(jobj, keyword);
+
+ jvalue = json_object_new_boolean(value);
+ if (!jvalue)
+ return false;
+
+ json_object_object_add(jobj, keyword, jvalue);
+ return true;
+}
+
+bool mesh_db_write_mode(json_object *jobj, const char *keyword, int value)
+{
+ json_object *jstring;
+
+ switch (value) {
+ case MESH_MODE_DISABLED:
+ jstring = json_object_new_string("disabled");
+ break;
+ case MESH_MODE_ENABLED:
+ jstring = json_object_new_string("enabled");
+ break;
+ case MESH_MODE_UNSUPPORTED:
+ jstring = json_object_new_string("unsupported");
+ break;
+ default:
+ return false;
+ };
+
+ if (!jstring)
+ return false;
+
+ json_object_object_add(jobj, keyword, jstring);
+
+ return true;
+}
+
+bool mesh_db_write_relay_mode(json_object *jnode, uint8_t mode, uint8_t count,
+ uint16_t interval)
+{
+ json_object *jrelay;
+
+ json_object_object_del(jnode, "relay");
+
+ jrelay = json_object_new_object();
+ if (jrelay)
+ return false;
+
+ if (!mesh_db_write_mode(jrelay, "mode", mode))
+ goto fail;
+
+ if (!mesh_db_write_int(jrelay, "count", count))
+ goto fail;
+
+ if (!mesh_db_write_int(jrelay, "interval", interval))
+ goto fail;
+
+ json_object_object_add(jnode, "relay", jrelay);
+
+ return true;
+fail:
+ json_object_put(jrelay);
+ return false;
+}
+
+bool mesh_db_read_net_transmit(json_object *jobj, uint8_t *cnt,
+ uint16_t *interval)
+{
+ json_object *jretransmit, *jvalue;
+
+ json_object_object_get_ex(jobj, "retransmit", &jretransmit);
+ if (!jretransmit)
+ return false;
+
+ json_object_object_get_ex(jretransmit, "count", &jvalue);
+ if (!jvalue)
+ return false;
+
+ *cnt = (uint8_t) json_object_get_int(jvalue);
+
+ json_object_object_get_ex(jretransmit, "interval", &jvalue);
+ if (!jvalue)
+ return false;
+
+ *interval = (uint16_t) json_object_get_int(jvalue);
+
+ return true;
+}
+
+bool mesh_db_write_net_transmit(json_object *jobj, uint8_t cnt,
+ uint16_t interval)
+{
+ json_object *jretransmit;
+
+ json_object_object_del(jobj, "retransmit");
+
+ jretransmit = json_object_new_object();
+ if (jretransmit)
+ return false;
+
+ if (!mesh_db_write_int(jretransmit, "count", cnt))
+ goto fail;
+
+ if (!mesh_db_write_int(jretransmit, "interval", interval))
+ goto fail;
+
+ json_object_object_add(jobj, "retransmit", jretransmit);
+
+ return true;
+fail:
+ json_object_put(jretransmit);
+ return false;
+
+}
+
+bool mesh_db_write_iv_index(json_object *jobj, uint32_t idx, bool update)
+{
+ int tmp = update ? 1 : 0;
+
+ if (!mesh_db_write_int(jobj, "IVindex", idx))
+ return false;
+
+ if (!mesh_db_write_int(jobj, "IVupdate", tmp))
+ return false;
+
+ return true;
+}
+
+void mesh_db_remove_property(json_object *jobj, const char *desc)
+{
+ json_object_object_del(jobj, desc);
+}
diff --git a/mesh/mesh-db.h b/mesh/mesh-db.h
new file mode 100644
index 000000000..336302f28
--- /dev/null
+++ b/mesh/mesh-db.h
@@ -0,0 +1,144 @@
+/*
+ *
+ * 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.
+ *
+ *
+ */
+
+#include <json-c/json.h>
+
+struct mesh_db_sub {
+ bool virt;
+ union {
+ uint16_t addr;
+ uint8_t virt_addr[16];
+ } src;
+};
+
+struct mesh_db_pub {
+ bool virt;
+ uint16_t addr;
+ uint16_t idx;
+ uint8_t ttl;
+ uint8_t credential;
+ uint8_t period;
+ uint8_t retransmit;
+ uint8_t virt_addr[16];
+};
+
+struct mesh_db_model {
+ struct mesh_db_sub *subs;
+ struct mesh_db_pub *pub;
+ uint16_t *bindings;
+ uint32_t id;
+ bool vendor;
+ uint32_t num_bindings;
+ uint32_t num_subs;
+};
+
+struct mesh_db_element {
+ struct l_queue *models;
+ uint16_t location;
+ uint8_t index;
+};
+
+struct mesh_db_modes {
+ struct {
+ uint16_t interval;
+ uint8_t cnt;
+ uint8_t state;
+ } relay;
+ uint8_t lpn;
+ uint8_t friend;
+ uint8_t proxy;
+ uint8_t beacon;
+};
+
+struct mesh_db_node {
+ bool provisioner;
+ uint32_t seq_number;
+ struct mesh_db_modes modes;
+ uint16_t cid;
+ uint16_t pid;
+ uint16_t vid;
+ uint16_t crpl;
+ uint16_t unicast;
+ uint8_t ttl;
+ struct l_queue *elements;
+ uint8_t uuid[16];
+};
+
+struct mesh_db_prov {
+ uint16_t algorithm;
+ struct {
+ uint16_t actions;
+ uint8_t size;
+ } input_oob;
+ uint8_t pub_type;
+ struct {
+ uint16_t actions;
+ uint8_t size;
+ } output_oob;
+ uint8_t static_type;
+ uint8_t priv_key[32];
+};
+
+typedef bool (*mesh_db_net_key_cb)(uint16_t idx, uint8_t key[16],
+ uint8_t new_key[16], int phase, void *user_data);
+typedef bool (*mesh_db_app_key_cb)(uint16_t idx, uint16_t net_idx,
+ uint8_t key[16], uint8_t new_key[16], void *user_data);
+typedef bool (*mesh_db_node_cb)(struct mesh_db_node *node, void *user_data);
+
+bool mesh_db_read_node(json_object *jobj, mesh_db_node_cb cb, void *user_data);
+bool mesh_db_read_unprovisioned_device(json_object *jnode, mesh_db_node_cb cb,
+ void *user_data);
+bool mesh_db_read_prov_info(json_object *jnode, struct mesh_db_prov *prov);
+bool mesh_db_read_iv_index(json_object *jobj, uint32_t *idx, bool *update);
+bool mesh_db_read_device_key(json_object *jobj, uint8_t key_buf[16]);
+bool mesh_db_read_net_transmit(json_object *jobj, uint8_t *cnt,
+ uint16_t *interval);
+bool mesh_db_write_net_transmit(json_object *jobj, uint8_t cnt,
+ uint16_t interval);
+bool mesh_db_read_net_keys(json_object *jobj, mesh_db_net_key_cb cb,
+ void *user_data);
+bool mesh_db_read_app_keys(json_object *jobj, mesh_db_app_key_cb cb,
+ void *user_data);
+bool mesh_db_write_device_key(json_object *jobj, uint8_t *key);
+bool mesh_db_write_network_key(json_object *jobj, uint16_t idx, uint8_t *key,
+ uint8_t *new_key, int phase);
+bool mesh_db_write_app_key(json_object *jobj, uint16_t net_idx,
+ uint16_t app_idx, uint8_t *key, uint8_t *new_key);
+bool mesh_db_write_int(json_object *jobj, const char *keyword, int value);
+bool mesh_db_write_uint16_hex(json_object *jobj, const char *desc,
+ uint16_t value);
+bool mesh_db_write_bool(json_object *jobj, const char *keyword, bool value);
+bool mesh_db_write_relay_mode(json_object *jnode, uint8_t mode, uint8_t count,
+ uint16_t interval);
+bool mesh_db_write_mode(json_object *jobj, const char *keyword, int value);
+bool mesh_db_model_binding_add(json_object *jnode, uint8_t ele_idx, bool vendor,
+ uint32_t mod_id, uint16_t app_idx);
+bool mesh_db_model_binding_del(json_object *jnode, uint8_t ele_idx, bool vendor,
+ uint32_t mod_id, uint16_t app_idx);
+bool mesh_db_app_key_add(json_object *jnode, uint16_t net_idx, uint16_t app_idx,
+ const uint8_t key[16], bool update);
+bool mesh_db_app_key_del(json_object *jobj, uint16_t net_idx, uint16_t idx);
+bool mesh_db_net_key_add(json_object *jobj, uint16_t net_idx,
+ const uint8_t key[16], int phase);
+bool mesh_db_net_key_del(json_object *jobj, uint16_t net_idx);
+bool mesh_db_write_kr_phase(json_object *jobj, uint16_t net_idx, int phase);
+bool mesh_db_write_address(json_object *jobj, uint16_t address);
+bool mesh_db_write_iv_index(json_object *jobj, uint32_t idx, bool update);
+void mesh_db_remove_property(json_object *jobj, const char *desc);
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 13/14] mesh: Sample device composition in JSON format
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (11 preceding siblings ...)
2018-07-14 19:59 ` [PATCH BlueZ v7 12/14] mesh: Read and write mesh configuration in JSON format Brian Gix
@ 2018-07-14 19:59 ` Brian Gix
2018-07-14 19:59 ` [PATCH BlueZ v7 14/14] Makefile for meshd Brian Gix
2018-08-21 16:45 ` [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Gix, Brian
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:59 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
This is a sample unprovisioned device composition described
in JSON format.
---
mesh/config/composition.json | 44 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 44 insertions(+)
create mode 100644 mesh/config/composition.json
diff --git a/mesh/config/composition.json b/mesh/config/composition.json
new file mode 100644
index 000000000..20c0d0c3a
--- /dev/null
+++ b/mesh/config/composition.json
@@ -0,0 +1,44 @@
+{
+ "$schema":"file:\/\/\/BlueZ\/MeshD\/local_schema\/mesh.jsonschema",
+ "meshName":"BT Mesh sample node",
+ "UUID":"E0ED0F0200000000203C100200000000",
+ "cid":"0002",
+ "pid":"0010",
+ "vid":"0001",
+ "crpl":"000a",
+ "proxy":"unsupported",
+ "friend":"disabled",
+ "lowPower":"disabled",
+ "relay":{
+ "mode":"enabled"
+ },
+ "elements":[
+ {
+ "elementIndex":0,
+ "location":"0001",
+ "models":[
+ {
+ "modelId":"0000"
+ },
+ {
+ "modelId":"1001"
+ }
+ ]
+ }
+ ],
+ "provision": {
+ "privateKey": "729aa0670d72cd6497502ed473502b037e8803b5c60829a5a3caa219505530ba",
+ "algorithms": [ 0 ],
+ "inputOOB": {
+ "size": 8,
+ "actions": [ 2, 3]
+ },
+ "outputOOB": {
+ "size": 8,
+ "actions": [ 3, 4]
+ },
+ "publicType": false,
+ "staticType": false
+ },
+ }
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* [PATCH BlueZ v7 14/14] Makefile for meshd
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (12 preceding siblings ...)
2018-07-14 19:59 ` [PATCH BlueZ v7 13/14] mesh: Sample device composition " Brian Gix
@ 2018-07-14 19:59 ` Brian Gix
2018-08-21 16:45 ` [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Gix, Brian
14 siblings, 0 replies; 16+ messages in thread
From: Brian Gix @ 2018-07-14 19:59 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
---
Makefile.am | 1 +
Makefile.mesh | 41 +++++++++++++++++++++++++++++++++++++++++
2 files changed, 42 insertions(+)
create mode 100644 Makefile.mesh
diff --git a/Makefile.am b/Makefile.am
index 2c9490f5f..0ccf393c6 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -235,6 +235,7 @@ unit_tests =
include Makefile.tools
include Makefile.obexd
include android/Makefile.am
+include Makefile.mesh
if HID2HCI
rulesdir = @UDEV_DIR@/rules.d
diff --git a/Makefile.mesh b/Makefile.mesh
new file mode 100644
index 000000000..a11c68576
--- /dev/null
+++ b/Makefile.mesh
@@ -0,0 +1,41 @@
+if MESH
+mesh_sources = mesh/mesh.h mesh/mesh.c \
+ mesh/mesh-io.h mesh/mesh-io.c \
+ mesh/mesh-io-api.h mesh/bt.h \
+ mesh/mesh-io-generic.h \
+ mesh/mesh-io-generic.c \
+ mesh/storage.h mesh/storage.c \
+ mesh/net.h mesh/net.c \
+ mesh/display.h mesh/display.c \
+ mesh/crypto.h mesh/crypto.c \
+ mesh/friend.h mesh/friend.c \
+ mesh/appkeys.h mesh/appkey.c \
+ mesh/node.h mesh/node.c \
+ mesh/prov.h mesh/prov.c \
+ mesh/provision.h mesh/provision.c \
+ mesh/model.h mesh/model.c \
+ mesh/cfgmod.h mesh/cfgmod-server.c \
+ mesh/mesh-db.h mesh/mesh-db.c \
+ mesh/util.h mesh/util.c \
+ mesh/mesh-defs.h
+libexec_PROGRAMS += mesh/meshd
+
+mesh_meshd_SOURCES = $(mesh_sources) mesh/main.c
+
+mesh_meshd_LDADD = src/shared/ecc.lo src/shared/queue.lo src/shared/io-ell.lo \
+ src/shared/util.lo src/shared/hci.lo \
+ @DBUS_LIBS@ @ELL_LIBS@ -ljson-c
+
+noinst_PROGRAMS += mesh/btmesh
+
+mesh_btmesh_SOURCES = $(mesh_sources) \
+ mesh/agent.h \
+ mesh/agent.c \
+ mesh/btmesh.c
+
+mesh_btmesh_LDADD = src/shared/ecc.lo src/shared/queue.lo src/shared/io-ell.lo \
+ src/shared/util.lo src/shared/hci.lo \
+ src/libshared-mainloop.la \
+ -lreadline @ELL_LIBS@ -ljson-c
+
+endif
--
2.14.4
^ permalink raw reply related [flat|nested] 16+ messages in thread* Re: [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon
2018-07-14 19:58 [PATCH BlueZ v7 00/14] Bluetooth Mesh Daemon Brian Gix
` (13 preceding siblings ...)
2018-07-14 19:59 ` [PATCH BlueZ v7 14/14] Makefile for meshd Brian Gix
@ 2018-08-21 16:45 ` Gix, Brian
14 siblings, 0 replies; 16+ messages in thread
From: Gix, Brian @ 2018-08-21 16:45 UTC (permalink / raw)
To: linux-bluetooth@vger.kernel.org
Cc: marcel@holtmann.org, johan.hedberg@gmail.com, Stotland, Inga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^ permalink raw reply [flat|nested] 16+ messages in thread