* [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon
@ 2018-07-06 17:12 Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 01/14] mesh: Shared private meshd interfaces Brian Gix
` (13 more replies)
0 siblings, 14 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
Version 5 of this patch-set conforms to the new layout under a single
flat mesh directory, after the reclassification of meshctl as a tool.
Otherwise:
This is our starting point for creating a stand-alone Mesh Daemon.
The Mesh Daemon (meshd) is intended to eventually co-exist beside the
Bluetooth Daemon (bluetoothd), and either daemon may exist without the
other. The Mesh Daemon will need full control/ownership of a
BT Controller that supports at least Core Spec revision 4.0 or
greater. It works strictly over the Advertising bearer, but we
have plans to extend it to support Mesh Proxy Server over GATT, at
which point it will require support from bluetoothd.
The current meshd, and it's companion Non-Background Command-line
version btmesh, exists fully within user-space, but requires an
HCI_USER socket to the kernel controler, and currently requires root
privledges.
This initial revision is intended to operate as a standard
(non-provisioner) node. It includes the Configuration Server model,
and if it cannot find it's Mesh Configuration, will look for it's
Composition data, and start up as unprovisioned, and accept an
incoming PB-ADV provisioning session.
Two of us (Inga and Brian) have been working on this, and will be
the initial supporters and maintainers, and we gladly invite other
interested parties to participate.
Brian Gix (7):
mesh: Shared private meshd interfaces
mesh: Mesh crypto support
mesh: Infrastructure for Mesh daemon
mesh: Initial Mesh Friendship support
mesh: Provisioning logic for mesh
mesh: Upper and Lower mesh transport
mesh: Add Accessors to Transport layer data
Inga Stotland (7):
mesh: Header files for mesh access layer and utilities
mesh: Source files for mesh access layer and utilities
mesh: Source code for handling access layer mux/demux
mesh: Mesh config server model
mesh: Read and write mesh configuration in JSON format
mesh: Sample device composition in JSON format
Makefile for meshd
Makefile.am | 1 +
Makefile.mesh | 44 +
mesh/agent.c | 229 +++
mesh/agent.h | 42 +
mesh/appkey.c | 536 ++++++
mesh/appkey.h | 43 +
mesh/btmesh.c | 176 ++
mesh/cfgmod-server.c | 1194 ++++++++++++
mesh/cfgmod.h | 98 +
mesh/config/composition.json | 44 +
mesh/crypto.c | 1607 ++++++++++++++++
mesh/crypto.h | 163 ++
mesh/display.c | 67 +
mesh/display.h | 28 +
mesh/friend.c | 1116 +++++++++++
mesh/friend.h | 57 +
mesh/hci.c | 699 +++++++
mesh/hci.h | 56 +
mesh/main.c | 174 ++
mesh/mesh-db.c | 1360 ++++++++++++++
mesh/mesh-db.h | 144 ++
mesh/mesh-defs.h | 84 +
mesh/mesh-io-api.h | 58 +
mesh/mesh-io-generic.c | 660 +++++++
mesh/mesh-io-generic.h | 20 +
mesh/mesh-io.c | 187 ++
mesh/mesh-io.h | 99 +
mesh/mesh.c | 184 ++
mesh/mesh.h | 32 +
mesh/model.c | 1274 +++++++++++++
mesh/model.h | 146 ++
mesh/net.c | 4188 ++++++++++++++++++++++++++++++++++++++++++
mesh/net.h | 392 ++++
mesh/node.c | 851 +++++++++
mesh/node.h | 80 +
mesh/prov.c | 722 ++++++++
mesh/prov.h | 162 ++
mesh/provision.c | 1159 ++++++++++++
mesh/provision.h | 30 +
mesh/storage.c | 672 +++++++
mesh/storage.h | 51 +
mesh/util.c | 71 +
mesh/util.h | 24 +
43 files changed, 19024 insertions(+)
create mode 100644 Makefile.mesh
create mode 100644 mesh/agent.c
create mode 100644 mesh/agent.h
create mode 100644 mesh/appkey.c
create mode 100644 mesh/appkey.h
create mode 100644 mesh/btmesh.c
create mode 100644 mesh/cfgmod-server.c
create mode 100644 mesh/cfgmod.h
create mode 100644 mesh/config/composition.json
create mode 100644 mesh/crypto.c
create mode 100644 mesh/crypto.h
create mode 100644 mesh/display.c
create mode 100644 mesh/display.h
create mode 100644 mesh/friend.c
create mode 100644 mesh/friend.h
create mode 100644 mesh/hci.c
create mode 100644 mesh/hci.h
create mode 100644 mesh/main.c
create mode 100644 mesh/mesh-db.c
create mode 100644 mesh/mesh-db.h
create mode 100644 mesh/mesh-defs.h
create mode 100644 mesh/mesh-io-api.h
create mode 100644 mesh/mesh-io-generic.c
create mode 100644 mesh/mesh-io-generic.h
create mode 100644 mesh/mesh-io.c
create mode 100644 mesh/mesh-io.h
create mode 100644 mesh/mesh.c
create mode 100644 mesh/mesh.h
create mode 100644 mesh/model.c
create mode 100644 mesh/model.h
create mode 100644 mesh/net.c
create mode 100644 mesh/net.h
create mode 100644 mesh/node.c
create mode 100644 mesh/node.h
create mode 100644 mesh/prov.c
create mode 100644 mesh/prov.h
create mode 100644 mesh/provision.c
create mode 100644 mesh/provision.h
create mode 100644 mesh/storage.c
create mode 100644 mesh/storage.h
create mode 100644 mesh/util.c
create mode 100644 mesh/util.h
--
2.14.4
^ permalink raw reply [flat|nested] 21+ messages in thread
* [PATCH BlueZ v5 01/14] mesh: Shared private meshd interfaces
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 02/14] mesh: Mesh crypto support Brian Gix
` (12 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 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/hci.h | 56 +++++++
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 ++++
10 files changed, 1065 insertions(+)
create mode 100644 mesh/crypto.h
create mode 100644 mesh/display.h
create mode 100644 mesh/friend.h
create mode 100644 mesh/hci.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/hci.h b/mesh/hci.h
new file mode 100644
index 000000000..a1362b76a
--- /dev/null
+++ b/mesh/hci.h
@@ -0,0 +1,56 @@
+/*
+ *
+ * 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_hci;
+
+typedef void (*bt_hci_destroy_func_t)(void *user_data);
+
+struct bt_hci *bt_hci_new(int fd);
+struct bt_hci *bt_hci_new_user_channel(uint16_t index);
+struct bt_hci *bt_hci_new_raw_device(uint16_t index);
+
+struct bt_hci *bt_hci_ref(struct bt_hci *hci);
+void bt_hci_unref(struct bt_hci *hci);
+
+bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close);
+
+typedef void (*bt_hci_callback_func_t)(const void *data, uint8_t size,
+ void *user_data);
+
+unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode,
+ const void *data, uint8_t size,
+ bt_hci_callback_func_t callback,
+ void *user_data, bt_hci_destroy_func_t destroy);
+bool bt_hci_cancel(struct bt_hci *hci, unsigned int id);
+bool bt_hci_flush(struct bt_hci *hci);
+
+unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event,
+ bt_hci_callback_func_t callback,
+ void *user_data, bt_hci_destroy_func_t destroy);
+bool bt_hci_unregister(struct bt_hci *hci, unsigned int id);
+
+typedef void (*bt_hci_receive_func_t)(uint16_t handle, uint8_t flags,
+ const void *data, uint16_t size,
+ void *user_data);
+
+bool bt_hci_receive(struct bt_hci *hci, bt_hci_receive_func_t callback,
+ void *user_data, bt_hci_destroy_func_t destroy);
+
+bool bt_hci_write(struct bt_hci *hci, uint16_t handle, uint8_t flags,
+ const void *data, uint16_t size);
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] 21+ messages in thread
* [PATCH BlueZ v5 02/14] mesh: Mesh crypto support
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 01/14] mesh: Shared private meshd interfaces Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 18:10 ` Marcel Holtmann
2018-07-06 17:12 ` [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon Brian Gix
` (11 subsequent siblings)
13 siblings, 1 reply; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/crypto.c | 1607 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 1607 insertions(+)
create mode 100644 mesh/crypto.c
diff --git a/mesh/crypto.c b/mesh/crypto.c
new file mode 100644
index 000000000..c424cf622
--- /dev/null
+++ b/mesh/crypto.c
@@ -0,0 +1,1607 @@
+/*
+ *
+ * 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"
+#include "mesh/display.h"
+
+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) {
+ l_error("key");
+ return -1;
+ }
+
+ if (mic_size &&
+ setsockopt(fd, SOL_ALG, ALG_SET_AEAD_AUTHSIZE,
+ NULL, mic_size) < 0) {
+ l_error("taglen");
+ 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;
+
+ if (aad_len >= 0xff00) {
+ l_error("Unsupported AAD size");
+ 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 == sizeof(uint64_t))
+ 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 sizeof(uint32_t):
+ *(uint32_t *)out_mic = l_get_be32(mic);
+ break;
+ case sizeof(uint64_t):
+ *(uint64_t *)out_mic = l_get_be64(mic);
+ break;
+ default:
+ l_error("Unsupported MIC size");
+ }
+ }
+
+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 >= 0xff00)
+ 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 == sizeof(uint64_t))
+ 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 sizeof(uint32_t):
+ *(uint32_t *)out_mic = l_get_be32(mic);
+ break;
+ case sizeof(uint64_t):
+ *(uint64_t *)out_mic = l_get_be64(mic);
+ break;
+ default:
+ l_error("Unsupported MIC size");
+ }
+ }
+
+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;
+
+ print_packet("K2-N", n, 16);
+ stage = l_malloc(sizeof(output) + p_len + 1);
+ if (stage == NULL)
+ return false;
+
+ if (!mesh_crypto_s1("smk2", 4, stage))
+ goto fail;
+ print_packet("K2-S1(smk2)", stage, 16);
+ print_packet("K2-P", p, p_len);
+
+ if (!aes_cmac_one(stage, n, 16, t))
+ goto fail;
+
+ print_packet("K2-T", t, 16);
+
+ 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;
+
+ print_packet("K2-T1", output, 16);
+
+ 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;
+ print_packet("K2-T2", output, 16);
+
+ 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;
+ print_packet("K2-T3", output, 16);
+
+ 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])
+{
+ 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];
+
+ print_packet("Net_Key", net_key, 16);
+
+ if (!mesh_crypto_nkik(net_key, id_key))
+ return false;
+
+ print_packet("ID_Key", id_key, 16);
+
+ 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);
+
+ print_packet("Nonce", tmp, 16);
+ if (!aes_ecb_one(id_key, tmp, tmp))
+ return false;
+
+ print_packet("result", tmp, 16);
+
+ 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])
+{
+ uint8_t tmp[16];
+ uint8_t t[16];
+ uint8_t id64[] = { 'i', 'd', '6', '4', 0x01 };
+
+ 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])
+{
+ uint8_t tmp[16];
+ uint8_t t[16];
+ uint8_t id6[] = { 'i', 'd', '6', 0x01 };
+
+ 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 ? sizeof(uint64_t) : sizeof(uint32_t));
+}
+
+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 == sizeof(uint64_t)) ? 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 == sizeof(uint64_t)) ? 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])
+{
+ const uint8_t zero[16] = {0};
+
+ 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])
+{
+ const uint8_t zero[16] = {0};
+ 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);
+
+ /* print_packet("AAD", aad, aad ? 16 : 0); */
+ /* print_packet("Nonce", application_nonce, 13); */
+ /* print_packet("Key", application_key, 16); */
+ /* print_packet("Payload[clr]", payload, payload_len); */
+
+ if (!mesh_crypto_aes_ccm_encrypt(application_nonce, application_key,
+ aad, aad ? 16 : 0,
+ payload, payload_len,
+ out, NULL,
+ aszmic ? 8 : 4))
+ return false;
+
+ /* print_packet("Payload[enc]", out, payload_len + (aszmic ? 8 : 4)); */
+
+ 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);
+
+ /* print_packet("AAD", aad, aad_len); */
+ /* print_packet("Nonce", app_nonce, 13); */
+ /* print_packet("Key", app_key, 16); */
+ /* print_packet("Payload[enc]", 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);
+
+ /* print_packet("Payload[clr]", 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);
+
+ /* print_packet("Payload[clr]", 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);
+
+ /* print_packet("Net-Nonce", network_nonce, 13); */
+ /* print_packet("Net-Key", network_key, 16); */
+ /* print_packet("Net-Payload[clr]", packet, packet_len); */
+
+ /* 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, sizeof(uint64_t)))
+ return false;
+ } else {
+ if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key,
+ NULL, 0,
+ packet + 7, packet_len - 7 - 4,
+ packet + 7, NULL, sizeof(uint32_t)))
+ return false;
+ }
+
+ /* print_packet("Net-Payload[enc]", packet, packet_len); */
+
+ l_put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, packet + 7, 7);
+
+ /* print_packet("Priv-Random", privacy_counter, 16); */
+
+ /* print_packet("Priv-Key", privacy_key, 16); */
+
+
+ if (!aes_ecb_one(privacy_key, privacy_counter, tmp))
+ return false;
+
+ for (i = 0; i < 6; i++)
+ packet[1 + i] ^= tmp[i];
+
+ /* print_packet("Net-Private", packet, packet_len); */
+
+ 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;
+
+ /* print_packet("Priv-Key", privacy_key, 16); */
+
+ l_put_be32(iv_index, privacy_counter + 5);
+ memcpy(privacy_counter + 9, packet + 7, 7);
+
+ /* print_packet("Priv-Random", privacy_counter, 16); */
+
+ 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);
+
+ /* print_packet("Net-Nonce", network_nonce, 13); */
+ /* print_packet("Net-Key", network_key, 16); */
+ /* print_packet("Net-Pkt[enc]", out, packet_len); */
+
+ /* 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;
+ }
+
+ /* print_packet("Net-Pkt[clr]", out, packet_len); */
+
+ 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];
+
+ if (fcs != 0xcf)
+ l_error("IOT Warning! CRC %2.2x != 0xcf", fcs);
+
+ return fcs == 0xcf;
+}
--
2.14.4
^ permalink raw reply related [flat|nested] 21+ messages in thread
* [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 01/14] mesh: Shared private meshd interfaces Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 02/14] mesh: Mesh crypto support Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 18:20 ` Marcel Holtmann
2018-07-06 17:12 ` [PATCH BlueZ v5 04/14] mesh: Initial Mesh Friendship support Brian Gix
` (10 subsequent siblings)
13 siblings, 1 reply; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland, Brian Gix
---
mesh/display.c | 67 +++++
mesh/hci.c | 699 +++++++++++++++++++++++++++++++++++++++++++++++++
mesh/mesh-io-generic.c | 660 ++++++++++++++++++++++++++++++++++++++++++++++
mesh/mesh-io.c | 187 +++++++++++++
4 files changed, 1613 insertions(+)
create mode 100644 mesh/display.c
create mode 100644 mesh/hci.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..adf92cae0
--- /dev/null
+++ b/mesh/display.c
@@ -0,0 +1,67 @@
+/*
+ *
+ * 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"
+
+#define likely(x) __builtin_expect(!!(x), 1)
+#define unlikely(x) __builtin_expect(!!(x), 0)
+
+static unsigned int cached_num_columns;
+
+unsigned int num_columns(void)
+{
+ struct winsize ws;
+
+ if (likely(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_info("%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_info("%05d.%03d %s: empty",
+ (uint32_t) pkt_time.tv_sec % 100000,
+ (uint32_t) pkt_time.tv_usec/1000, label);
+}
diff --git a/mesh/hci.c b/mesh/hci.c
new file mode 100644
index 000000000..da6838a55
--- /dev/null
+++ b/mesh/hci.c
@@ -0,0 +1,699 @@
+/*
+ *
+ * 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 <stdlib.h>
+#include <sys/uio.h>
+#include <sys/socket.h>
+#include <ell/ell.h>
+
+#include "monitor/bt.h"
+
+#include "mesh/hci.h"
+
+#define BTPROTO_HCI 1
+struct sockaddr_hci {
+ sa_family_t hci_family;
+ unsigned short hci_dev;
+ unsigned short hci_channel;
+};
+#define HCI_CHANNEL_USER 1
+
+struct bt_hci {
+ int ref_count;
+ struct l_io *io;
+ bool is_stream;
+ bool writer_active;
+ uint8_t num_cmds;
+ uint8_t num_pkts;
+ unsigned int next_cmd_id;
+ unsigned int next_evt_id;
+ struct l_queue *cmd_queue;
+ struct l_queue *rsp_queue;
+ struct l_queue *evt_list;
+ struct l_queue *pkt_queue;
+ bt_hci_receive_func_t receive_callback;
+ bt_hci_destroy_func_t receive_destroy;
+ void *receive_data;
+};
+
+struct cmd {
+ unsigned int id;
+ uint16_t opcode;
+ void *data;
+ uint8_t size;
+ bt_hci_callback_func_t callback;
+ bt_hci_destroy_func_t destroy;
+ void *user_data;
+};
+
+struct evt {
+ unsigned int id;
+ uint8_t event;
+ bt_hci_callback_func_t callback;
+ bt_hci_destroy_func_t destroy;
+ void *user_data;
+};
+
+struct pkt {
+ uint16_t handle;
+ uint8_t flags;
+ void *data;
+ uint16_t size;
+};
+
+static void cmd_free(void *data)
+{
+ struct cmd *cmd = data;
+
+ if (cmd->destroy)
+ cmd->destroy(cmd->user_data);
+
+ l_free(cmd->data);
+ l_free(cmd);
+}
+
+static void evt_free(void *data)
+{
+ struct evt *evt = data;
+
+ if (evt->destroy)
+ evt->destroy(evt->user_data);
+
+ l_free(evt);
+}
+
+static void pkt_free(void *data)
+{
+ struct pkt *pkt = data;
+
+ l_free(pkt->data);
+ l_free(pkt);
+}
+
+static void send_command(struct bt_hci *hci, uint16_t opcode,
+ void *data, uint8_t size)
+{
+ uint8_t type = BT_H4_CMD_PKT;
+ struct bt_hci_cmd_hdr hdr;
+ struct iovec iov[3];
+ int fd, iovcnt;
+
+ hdr.opcode = L_CPU_TO_LE16(opcode);
+ hdr.plen = size;
+
+ iov[0].iov_base = &type;
+ iov[0].iov_len = 1;
+ iov[1].iov_base = &hdr;
+ iov[1].iov_len = sizeof(hdr);
+
+ if (size > 0) {
+ iov[2].iov_base = data;
+ iov[2].iov_len = size;
+ iovcnt = 3;
+ } else
+ iovcnt = 2;
+
+ fd = l_io_get_fd(hci->io);
+ if (fd < 0) {
+ l_error("hci->io Bad");
+ return;
+ }
+
+ if (writev(fd, iov, iovcnt) < 0) {
+ l_error("writev Bad");
+ return;
+ }
+
+ hci->num_cmds--;
+}
+
+static void send_packet(struct bt_hci *hci, uint16_t handle, uint8_t flags,
+ void *data, uint8_t size)
+{
+ uint8_t type = BT_H4_ACL_PKT;
+ struct bt_hci_acl_hdr hdr;
+ struct iovec iov[3];
+ int fd, iovcnt;
+
+ hdr.handle = L_CPU_TO_LE16(flags << 12 | handle);
+ hdr.dlen = L_CPU_TO_LE16(size);
+
+ iov[0].iov_base = &type;
+ iov[0].iov_len = 1;
+ iov[1].iov_base = &hdr;
+ iov[1].iov_len = sizeof(hdr);
+
+ if (size > 0) {
+ iov[2].iov_base = data;
+ iov[2].iov_len = size;
+ iovcnt = 3;
+ } else
+ iovcnt = 2;
+
+ fd = l_io_get_fd(hci->io);
+ if (fd < 0) {
+ l_error("hci->io Bad");
+ return;
+ }
+
+ if (writev(fd, iov, iovcnt) < 0) {
+ l_error("writev Bad");
+ return;
+ }
+
+ hci->num_pkts--;
+}
+
+static bool io_write_callback(struct l_io *io, void *user_data)
+{
+ struct bt_hci *hci = user_data;
+ struct cmd *cmd;
+
+ if (hci->num_cmds > 0)
+ cmd = l_queue_pop_head(hci->cmd_queue);
+ else
+ cmd = NULL;
+
+ if (cmd) {
+ send_command(hci, cmd->opcode, cmd->data, cmd->size);
+ l_queue_push_tail(hci->rsp_queue, cmd);
+ } else {
+ struct pkt *pkt;
+
+ if (hci->num_pkts < 1)
+ goto done;
+
+ pkt = l_queue_pop_head(hci->pkt_queue);
+ if (pkt) {
+ send_packet(hci, pkt->handle, pkt->flags,
+ pkt->data, pkt->size);
+ pkt_free(pkt);
+ }
+ }
+
+ if (!l_queue_isempty(hci->pkt_queue))
+ return true;
+
+done:
+ hci->writer_active = false;
+
+ return false;
+}
+
+static void wakeup_writer(struct bt_hci *hci)
+{
+ if (hci->writer_active)
+ return;
+
+ if (l_queue_isempty(hci->cmd_queue) && l_queue_isempty(hci->pkt_queue))
+ return;
+
+ if (!l_io_set_write_handler(hci->io, io_write_callback, hci, NULL))
+ return;
+
+ hci->writer_active = true;
+}
+
+static bool match_cmd_opcode(const void *a, const void *b)
+{
+ const struct cmd *cmd = a;
+ uint16_t opcode = L_PTR_TO_UINT(b);
+
+ return cmd->opcode == opcode;
+}
+
+static void process_response(struct bt_hci *hci, uint16_t opcode,
+ const void *data, size_t size)
+{
+ struct cmd *cmd;
+
+ if (opcode == BT_HCI_CMD_NOP)
+ goto done;
+
+ cmd = l_queue_remove_if(hci->rsp_queue, match_cmd_opcode,
+ L_UINT_TO_PTR(opcode));
+ if (!cmd)
+ goto done;
+
+ if (cmd->callback)
+ cmd->callback(data, size, cmd->user_data);
+
+ cmd_free(cmd);
+
+done:
+ wakeup_writer(hci);
+}
+
+static void process_notify(void *data, void *user_data)
+{
+ struct bt_hci_evt_hdr *hdr = user_data;
+ struct evt *evt = data;
+
+ if (evt->event == hdr->evt)
+ evt->callback(user_data + sizeof(struct bt_hci_evt_hdr),
+ hdr->plen, evt->user_data);
+}
+
+static void process_event(struct bt_hci *hci, const void *data, size_t size)
+{
+ const struct bt_hci_evt_hdr *hdr = data;
+ const struct bt_hci_evt_cmd_complete *cc;
+ const struct bt_hci_evt_cmd_status *cs;
+ const struct bt_hci_evt_num_completed_packets *cp;
+
+ if (size < sizeof(struct bt_hci_evt_hdr))
+ return;
+
+ data += sizeof(struct bt_hci_evt_hdr);
+ size -= sizeof(struct bt_hci_evt_hdr);
+
+ if (hdr->plen != size)
+ return;
+
+ switch (hdr->evt) {
+ case BT_HCI_EVT_CMD_COMPLETE:
+ if (size < sizeof(*cc))
+ return;
+ cc = data;
+ hci->num_cmds = cc->ncmd;
+ process_response(hci, L_LE16_TO_CPU(cc->opcode),
+ data + sizeof(*cc),
+ size - sizeof(*cc));
+ break;
+
+ case BT_HCI_EVT_CMD_STATUS:
+ if (size < sizeof(*cs))
+ return;
+ cs = data;
+ hci->num_cmds = cs->ncmd;
+ process_response(hci, L_LE16_TO_CPU(cs->opcode),
+ &cs->status, 1);
+ break;
+
+ case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
+ if (size < sizeof(*cp))
+ return;
+ cp = data;
+ l_debug("BT_HCI_EVT_NUM_COMPLETED_PACKETS:%d", cp->count);
+ /* Ignoring handle information for now */
+ hci->num_pkts = cp->count;
+ wakeup_writer(hci);
+ break;
+
+ default:
+ l_queue_foreach(hci->evt_list, process_notify, (void *) hdr);
+ break;
+ }
+}
+
+static void process_acl(struct bt_hci *hci, const void *data, size_t size)
+{
+ const struct bt_hci_acl_hdr *hdr = data;
+ uint16_t handle;
+
+ if (size < sizeof(struct bt_hci_acl_hdr))
+ return;
+
+ data += sizeof(struct bt_hci_acl_hdr);
+ size -= sizeof(struct bt_hci_acl_hdr);
+
+ if (L_LE16_TO_CPU(hdr->dlen) != size)
+ return;
+
+ handle = L_LE16_TO_CPU(hdr->handle);
+
+ if (hci->receive_callback)
+ hci->receive_callback(handle & 0x0fff, handle >> 12,
+ data, size, hci->receive_data);
+}
+
+static bool io_read_callback(struct l_io *io, void *user_data)
+{
+ struct bt_hci *hci = user_data;
+ uint8_t buf[512];
+ ssize_t len;
+ int fd;
+
+ fd = l_io_get_fd(hci->io);
+ if (fd < 0)
+ return false;
+
+ if (hci->is_stream)
+ return false;
+
+ len = read(fd, buf, sizeof(buf));
+ if (len < 0)
+ return false;
+
+ if (len < 1)
+ return true;
+
+ switch (buf[0]) {
+ case BT_H4_EVT_PKT:
+ process_event(hci, buf + 1, len - 1);
+ break;
+ case BT_H4_ACL_PKT:
+ process_acl(hci, buf + 1, len - 1);
+ break;
+ default:
+ l_info("%2.2x-RXed", buf[0]);
+ }
+
+ return true;
+}
+
+static struct bt_hci *create_hci(int fd)
+{
+ struct bt_hci *hci;
+
+ if (fd < 0)
+ return NULL;
+
+ hci = l_new(struct bt_hci, 1);
+
+ hci->io = l_io_new(fd);
+
+ hci->is_stream = true;
+ hci->writer_active = false;
+ hci->num_cmds = 1;
+ hci->num_pkts = 1;
+ hci->next_cmd_id = 1;
+ hci->next_evt_id = 1;
+
+ hci->cmd_queue = l_queue_new();
+ hci->rsp_queue = l_queue_new();
+ hci->evt_list = l_queue_new();
+ hci->pkt_queue = l_queue_new();
+
+ if (!l_io_set_read_handler(hci->io, io_read_callback, hci, NULL)) {
+ l_queue_destroy(hci->pkt_queue, NULL);
+ l_queue_destroy(hci->evt_list, NULL);
+ l_queue_destroy(hci->rsp_queue, NULL);
+ l_queue_destroy(hci->cmd_queue, NULL);
+ l_io_destroy(hci->io);
+ l_free(hci);
+ return NULL;
+ }
+
+ return bt_hci_ref(hci);
+}
+
+struct bt_hci *bt_hci_new(int fd)
+{
+ struct bt_hci *hci;
+
+ hci = create_hci(fd);
+ if (!hci)
+ return NULL;
+
+ return hci;
+}
+
+static int create_socket(uint16_t index, uint16_t channel)
+{
+ struct sockaddr_hci addr;
+ int fd;
+
+ fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
+ BTPROTO_HCI);
+ if (fd < 0)
+ return -1;
+
+ memset(&addr, 0, sizeof(addr));
+ addr.hci_family = AF_BLUETOOTH;
+ addr.hci_dev = index;
+ addr.hci_channel = channel;
+
+ if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+ close(fd);
+ return -1;
+ }
+
+ return fd;
+}
+
+struct bt_hci *bt_hci_new_user_channel(uint16_t index)
+{
+ struct bt_hci *hci;
+ int fd;
+
+ fd = create_socket(index, HCI_CHANNEL_USER);
+ if (fd < 0)
+ return NULL;
+
+ hci = create_hci(fd);
+ if (!hci) {
+ close(fd);
+ return NULL;
+ }
+
+ hci->is_stream = false;
+
+ bt_hci_set_close_on_unref(hci, true);
+
+ return hci;
+}
+
+struct bt_hci *bt_hci_ref(struct bt_hci *hci)
+{
+ if (!hci)
+ return NULL;
+
+ __sync_fetch_and_add(&hci->ref_count, 1);
+
+ return hci;
+}
+
+void bt_hci_unref(struct bt_hci *hci)
+{
+ if (!hci)
+ return;
+
+ if (__sync_sub_and_fetch(&hci->ref_count, 1))
+ return;
+
+ if (hci->receive_destroy)
+ hci->receive_destroy(hci->receive_data);
+
+ l_queue_destroy(hci->pkt_queue, pkt_free);
+ l_queue_destroy(hci->evt_list, evt_free);
+ l_queue_destroy(hci->cmd_queue, cmd_free);
+ l_queue_destroy(hci->rsp_queue, cmd_free);
+
+ l_io_destroy(hci->io);
+
+ l_free(hci);
+}
+
+bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close)
+{
+ if (!hci)
+ return false;
+
+ return l_io_set_close_on_destroy(hci->io, do_close);
+}
+
+unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode,
+ const void *data, uint8_t size,
+ bt_hci_callback_func_t callback,
+ void *user_data, bt_hci_destroy_func_t destroy)
+{
+ struct cmd *cmd;
+
+ if (!hci)
+ return 0;
+
+ cmd = l_new(struct cmd, 1);
+
+ cmd->opcode = opcode;
+ cmd->size = size;
+
+ if (cmd->size > 0)
+ cmd->data = l_memdup(data, cmd->size);
+
+ if (hci->next_cmd_id < 1)
+ hci->next_cmd_id = 1;
+
+ cmd->id = hci->next_cmd_id++;
+
+ cmd->callback = callback;
+ cmd->destroy = destroy;
+ cmd->user_data = user_data;
+
+ if (!l_queue_push_tail(hci->cmd_queue, cmd)) {
+ l_free(cmd->data);
+ l_free(cmd);
+ return 0;
+ }
+
+ wakeup_writer(hci);
+
+ return cmd->id;
+}
+
+static bool match_cmd_id(const void *a, const void *b)
+{
+ const struct cmd *cmd = a;
+ unsigned int id = L_PTR_TO_UINT(b);
+
+ return cmd->id == id;
+}
+
+bool bt_hci_cancel(struct bt_hci *hci, unsigned int id)
+{
+ struct cmd *cmd;
+
+ if (!hci || !id)
+ return false;
+
+ cmd = l_queue_remove_if(hci->cmd_queue, match_cmd_id,
+ L_UINT_TO_PTR(id));
+ if (!cmd) {
+ cmd = l_queue_remove_if(hci->rsp_queue, match_cmd_id,
+ L_UINT_TO_PTR(id));
+ if (!cmd)
+ return false;
+ }
+
+ cmd_free(cmd);
+
+ wakeup_writer(hci);
+
+ return true;
+}
+
+bool bt_hci_flush(struct bt_hci *hci)
+{
+ if (!hci)
+ return false;
+
+ if (hci->writer_active) {
+ l_io_set_write_handler(hci->io, NULL, NULL, NULL);
+ hci->writer_active = false;
+ }
+
+ l_queue_clear(hci->cmd_queue, cmd_free);
+ l_queue_clear(hci->rsp_queue, cmd_free);
+
+ return true;
+}
+
+unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event,
+ bt_hci_callback_func_t callback,
+ void *user_data, bt_hci_destroy_func_t destroy)
+{
+ struct evt *evt;
+
+ if (!hci)
+ return 0;
+
+ evt = l_new(struct evt, 1);
+
+ evt->event = event;
+
+ if (hci->next_evt_id < 1)
+ hci->next_evt_id = 1;
+
+ evt->id = hci->next_evt_id++;
+
+ evt->callback = callback;
+ evt->destroy = destroy;
+ evt->user_data = user_data;
+
+ if (!l_queue_push_tail(hci->evt_list, evt)) {
+ l_free(evt);
+ return 0;
+ }
+
+ return evt->id;
+}
+
+static bool match_evt_id(const void *a, const void *b)
+{
+ const struct evt *evt = a;
+ unsigned int id = L_PTR_TO_UINT(b);
+
+ return evt->id == id;
+}
+
+bool bt_hci_unregister(struct bt_hci *hci, unsigned int id)
+{
+ struct evt *evt;
+
+ if (!hci || !id)
+ return false;
+
+ evt = l_queue_remove_if(hci->evt_list, match_evt_id,
+ L_UINT_TO_PTR(id));
+ if (!evt)
+ return false;
+
+ evt_free(evt);
+
+ return true;
+}
+
+bool bt_hci_receive(struct bt_hci *hci, bt_hci_receive_func_t callback,
+ void *user_data, bt_hci_destroy_func_t destroy)
+{
+ if (!hci)
+ return false;
+
+ if (hci->receive_destroy)
+ hci->receive_destroy(hci->receive_data);
+
+ hci->receive_callback = callback;
+ hci->receive_destroy = destroy;
+ hci->receive_data = user_data;
+
+ return true;
+}
+
+bool bt_hci_write(struct bt_hci *hci, uint16_t handle, uint8_t flags,
+ const void *data, uint16_t size)
+{
+ struct pkt *pkt;
+
+ if (!hci)
+ return false;
+
+ pkt = l_new(struct pkt, 1);
+
+ pkt->handle = handle;
+ pkt->flags = flags;
+ pkt->size = size;
+
+ if (pkt->size > 0)
+ pkt->data = l_memdup(data, pkt->size);
+
+ if (!l_queue_push_tail(hci->pkt_queue, pkt)) {
+ l_free(pkt->data);
+ l_free(pkt);
+ return false;
+ }
+
+ wakeup_writer(hci);
+
+ return true;
+}
diff --git a/mesh/mesh-io-generic.c b/mesh/mesh-io-generic.c
new file mode 100644
index 000000000..7a974cff3
--- /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 "mesh/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..762eb2c6f
--- /dev/null
+++ b/mesh/mesh-io.c
@@ -0,0 +1,187 @@
+/*
+ *
+ * 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 == NULL || api->init == NULL || io != NULL)
+ return NULL;
+
+ io = l_new(struct mesh_io, 1);
+
+ if (io == NULL)
+ return NULL;
+
+ io->type = type;
+ io->index = index;
+ io->api = api;
+ if (!api->init(index, io))
+ goto fail;
+
+ if (io_list == NULL)
+ 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] 21+ messages in thread
* [PATCH BlueZ v5 04/14] mesh: Initial Mesh Friendship support
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (2 preceding siblings ...)
2018-07-06 17:12 ` [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 05/14] mesh: Provisioning logic for mesh Brian Gix
` (9 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 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] 21+ messages in thread
* [PATCH BlueZ v5 05/14] mesh: Provisioning logic for mesh
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (3 preceding siblings ...)
2018-07-06 17:12 ` [PATCH BlueZ v5 04/14] mesh: Initial Mesh Friendship support Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 06/14] mesh: Upper and Lower mesh transport Brian Gix
` (8 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 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] 21+ messages in thread
* [PATCH BlueZ v5 06/14] mesh: Upper and Lower mesh transport
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (4 preceding siblings ...)
2018-07-06 17:12 ` [PATCH BlueZ v5 05/14] mesh: Provisioning logic for mesh Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 07/14] mesh: Add Accessors to Transport layer data Brian Gix
` (7 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 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] 21+ messages in thread
* [PATCH BlueZ v5 07/14] mesh: Add Accessors to Transport layer data
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (5 preceding siblings ...)
2018-07-06 17:12 ` [PATCH BlueZ v5 06/14] mesh: Upper and Lower mesh transport Brian Gix
@ 2018-07-06 17:12 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 08/14] mesh: Header files for mesh access layer and utilities Brian Gix
` (6 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:12 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] 21+ messages in thread
* [PATCH BlueZ v5 08/14] mesh: Header files for mesh access layer and utilities
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (6 preceding siblings ...)
2018-07-06 17:12 ` [PATCH BlueZ v5 07/14] mesh: Add Accessors to Transport layer data Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 09/14] mesh: Source " Brian Gix
` (5 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 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] 21+ messages in thread
* [PATCH BlueZ v5 09/14] mesh: Source files for mesh access layer and utilities
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (7 preceding siblings ...)
2018-07-06 17:13 ` [PATCH BlueZ v5 08/14] mesh: Header files for mesh access layer and utilities Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 10/14] mesh: Source code for handling access layer mux/demux Brian Gix
` (4 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 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] 21+ messages in thread
* [PATCH BlueZ v5 10/14] mesh: Source code for handling access layer mux/demux
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (8 preceding siblings ...)
2018-07-06 17:13 ` [PATCH BlueZ v5 09/14] mesh: Source " Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 11/14] mesh: Mesh config server model Brian Gix
` (3 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 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] 21+ messages in thread
* [PATCH BlueZ v5 11/14] mesh: Mesh config server model
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (9 preceding siblings ...)
2018-07-06 17:13 ` [PATCH BlueZ v5 10/14] mesh: Source code for handling access layer mux/demux Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 12/14] mesh: Read and write mesh configuration in JSON format Brian Gix
` (2 subsequent siblings)
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 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] 21+ messages in thread
* [PATCH BlueZ v5 12/14] mesh: Read and write mesh configuration in JSON format
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (10 preceding siblings ...)
2018-07-06 17:13 ` [PATCH BlueZ v5 11/14] mesh: Mesh config server model Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 13/14] mesh: Sample device composition " Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 14/14] Makefile for meshd Brian Gix
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 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] 21+ messages in thread
* [PATCH BlueZ v5 13/14] mesh: Sample device composition in JSON format
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (11 preceding siblings ...)
2018-07-06 17:13 ` [PATCH BlueZ v5 12/14] mesh: Read and write mesh configuration in JSON format Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 14/14] Makefile for meshd Brian Gix
13 siblings, 0 replies; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 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] 21+ messages in thread
* [PATCH BlueZ v5 14/14] Makefile for meshd
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
` (12 preceding siblings ...)
2018-07-06 17:13 ` [PATCH BlueZ v5 13/14] mesh: Sample device composition " Brian Gix
@ 2018-07-06 17:13 ` Brian Gix
2018-07-06 17:23 ` Marcel Holtmann
13 siblings, 1 reply; 21+ messages in thread
From: Brian Gix @ 2018-07-06 17:13 UTC (permalink / raw)
To: linux-bluetooth; +Cc: marcel, johan.hedberg, inga.stotland
From: Inga Stotland <inga.stotland@intel.com>
---
Makefile.am | 1 +
Makefile.mesh | 44 ++++++++++++++++++++++++++++++++++++++++++++
2 files changed, 45 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..aaf337579
--- /dev/null
+++ b/Makefile.mesh
@@ -0,0 +1,44 @@
+if MESH
+mesh_json_sources = mesh/mesh-db.h mesh/mesh-db.c
+mesh_common_sources = mesh/mesh-defs.h \
+ mesh/util.h mesh/util.c
+
+mesh_sources = $(mesh_common_sources) $(mesh_json_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/hci.h mesh/hci.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
+
+libexec_PROGRAMS += mesh/meshd
+
+mesh_meshd_SOURCES = $(mesh_sources) \
+ mesh/main.c
+
+mesh_meshd_LDADD = /usr/lib64/libell.la src/shared/ecc.lo \
+ @DBUS_LIBS@ -lell -ljson-c -ldl
+
+noinst_PROGRAMS += mesh/btmesh
+
+mesh_btmesh_SOURCES = $(mesh_sources) \
+ mesh/agent.h \
+ mesh/agent.c \
+ mesh/btmesh.c
+
+mesh_btmesh_LDADD = /usr/lib64/libell.la src/shared/ecc.lo \
+ src/libshared-mainloop.la \
+ -lreadline -lell -ljson-c -ldl
+
+endif
--
2.14.4
^ permalink raw reply related [flat|nested] 21+ messages in thread
* Re: [PATCH BlueZ v5 14/14] Makefile for meshd
2018-07-06 17:13 ` [PATCH BlueZ v5 14/14] Makefile for meshd Brian Gix
@ 2018-07-06 17:23 ` Marcel Holtmann
0 siblings, 0 replies; 21+ messages in thread
From: Marcel Holtmann @ 2018-07-06 17:23 UTC (permalink / raw)
To: Brian Gix; +Cc: linux-bluetooth, Johan Hedberg, inga.stotland
Hi Brian,
> ---
> Makefile.am | 1 +
> Makefile.mesh | 44 ++++++++++++++++++++++++++++++++++++++++++++
> 2 files changed, 45 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..aaf337579
> --- /dev/null
> +++ b/Makefile.mesh
> @@ -0,0 +1,44 @@
> +if MESH
> +mesh_json_sources = mesh/mesh-db.h mesh/mesh-db.c
why are these split? Do you plan a version that doesn't require JSON?
> +mesh_common_sources = mesh/mesh-defs.h \
> + mesh/util.h mesh/util.c
> +
> +mesh_sources = $(mesh_common_sources) $(mesh_json_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/hci.h mesh/hci.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
> +
> +libexec_PROGRAMS += mesh/meshd
> +
> +mesh_meshd_SOURCES = $(mesh_sources) \
> + mesh/main.c
Move it on the same line here.
> +
> +mesh_meshd_LDADD = /usr/lib64/libell.la src/shared/ecc.lo \
> + @DBUS_LIBS@ -lell -ljson-c -ldl
The /usr/lib64/libell.la is wrong. It is not portable. @ELL_LIBS@ is needed here. Also what is the -ldl for?
> +
> +noinst_PROGRAMS += mesh/btmesh
> +
> +mesh_btmesh_SOURCES = $(mesh_sources) \
> + mesh/agent.h \
> + mesh/agent.c \
> + mesh/btmesh.c
> +
> +mesh_btmesh_LDADD = /usr/lib64/libell.la src/shared/ecc.lo \
> + src/libshared-mainloop.la \
> + -lreadline -lell -ljson-c -ldl
> +
> +endif
Regards
Marcel
^ permalink raw reply [flat|nested] 21+ messages in thread
* Re: [PATCH BlueZ v5 02/14] mesh: Mesh crypto support
2018-07-06 17:12 ` [PATCH BlueZ v5 02/14] mesh: Mesh crypto support Brian Gix
@ 2018-07-06 18:10 ` Marcel Holtmann
2018-07-08 15:07 ` Gix, Brian
0 siblings, 1 reply; 21+ messages in thread
From: Marcel Holtmann @ 2018-07-06 18:10 UTC (permalink / raw)
To: Brian Gix; +Cc: linux-bluetooth, Johan Hedberg, inga.stotland
Hi Brian,
> ---
> mesh/crypto.c | 1607 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++
> 1 file changed, 1607 insertions(+)
> create mode 100644 mesh/crypto.c
what I am missing is a unit/test-mesh-crypto with the Mesh test vectors.
>
> diff --git a/mesh/crypto.c b/mesh/crypto.c
> new file mode 100644
> index 000000000..c424cf622
> --- /dev/null
> +++ b/mesh/crypto.c
> @@ -0,0 +1,1607 @@
> +/*
> + *
> + * 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"
> +#include "mesh/display.h"
No idea why you would need mesh/display.h here.
> +
> +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) {
> + l_error("key");
> + return -1;
> + }
> +
> + if (mic_size &&
> + setsockopt(fd, SOL_ALG, ALG_SET_AEAD_AUTHSIZE,
> + NULL, mic_size) < 0) {
> + l_error("taglen");
I am not sure we should print errors here.
> + 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;
> +
> + if (aad_len >= 0xff00) {
> + l_error("Unsupported AAD size");
> + return false;
> + }
Same comment for the errors.
In a follow up patch, I would prefer that we move over to use the kernel AES-CMAC support.
> +
> + 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 == sizeof(uint64_t))
> + 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 sizeof(uint32_t):
> + *(uint32_t *)out_mic = l_get_be32(mic);
> + break;
> + case sizeof(uint64_t):
> + *(uint64_t *)out_mic = l_get_be64(mic);
> + break;
> + default:
> + l_error("Unsupported MIC size");
> + }
> + }
> +
> +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 >= 0xff00)
> + 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 == sizeof(uint64_t))
> + 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 sizeof(uint32_t):
> + *(uint32_t *)out_mic = l_get_be32(mic);
> + break;
> + case sizeof(uint64_t):
> + *(uint64_t *)out_mic = l_get_be64(mic);
> + break;
> + default:
> + l_error("Unsupported MIC size");
> + }
> + }
> +
> +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;
> +
> + print_packet("K2-N", n, 16);
> + stage = l_malloc(sizeof(output) + p_len + 1);
> + if (stage == NULL)
> + return false;
I prefer if (!stage) constructs.
> +
> + if (!mesh_crypto_s1("smk2", 4, stage))
> + goto fail;
> + print_packet("K2-S1(smk2)", stage, 16);
> + print_packet("K2-P", p, p_len);
This print_packet is really not acceptable.
> +
> + if (!aes_cmac_one(stage, n, 16, t))
> + goto fail;
> +
> + print_packet("K2-T", t, 16);
> +
> + 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;
> +
> + print_packet("K2-T1", output, 16);
> +
> + 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;
> + print_packet("K2-T2", output, 16);
> +
> + 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;
> + print_packet("K2-T3", output, 16);
> +
> + 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])
> +{
> + 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];
> +
> + print_packet("Net_Key", net_key, 16);
> +
> + if (!mesh_crypto_nkik(net_key, id_key))
> + return false;
> +
> + print_packet("ID_Key", id_key, 16);
> +
> + 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);
> +
> + print_packet("Nonce", tmp, 16);
> + if (!aes_ecb_one(id_key, tmp, tmp))
> + return false;
> +
> + print_packet("result", tmp, 16);
> +
> + 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])
> +{
> + uint8_t tmp[16];
> + uint8_t t[16];
> + uint8_t id64[] = { 'i', 'd', '6', '4', 0x01 };
> +
> + 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])
> +{
> + uint8_t tmp[16];
> + uint8_t t[16];
> + uint8_t id6[] = { 'i', 'd', '6', 0x01 };
These should come first and most likely be declared const.
> +
> + 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 ? sizeof(uint64_t) : sizeof(uint32_t));
> +}
> +
> +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 == sizeof(uint64_t)) ? true : false;
Is this sizeof(uint64_t) construct really a good idea?
> +
> + 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 == sizeof(uint64_t)) ? 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);
Double spaces.
> +
> + if (result)
> + memcpy(nonce, tmp + 3, 13);
> +
> + return result;
> +}
> +
> +bool mesh_crypto_s1(const void *info, size_t len, uint8_t salt[16])
> +{
> + const uint8_t zero[16] = {0};
= { 0, };
> +
> + 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])
> +{
> + const uint8_t zero[16] = {0};
> + 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);
> +
> + /* print_packet("AAD", aad, aad ? 16 : 0); */
> + /* print_packet("Nonce", application_nonce, 13); */
> + /* print_packet("Key", application_key, 16); */
> + /* print_packet("Payload[clr]", payload, payload_len); */
> +
> + if (!mesh_crypto_aes_ccm_encrypt(application_nonce, application_key,
> + aad, aad ? 16 : 0,
> + payload, payload_len,
> + out, NULL,
> + aszmic ? 8 : 4))
> + return false;
> +
> + /* print_packet("Payload[enc]", out, payload_len + (aszmic ? 8 : 4)); */
> +
> + 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);
> +
> + /* print_packet("AAD", aad, aad_len); */
> + /* print_packet("Nonce", app_nonce, 13); */
> + /* print_packet("Key", app_key, 16); */
> + /* print_packet("Payload[enc]", 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);
> +
> + /* print_packet("Payload[clr]", 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);
> +
> + /* print_packet("Payload[clr]", 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);
> +
> + /* print_packet("Net-Nonce", network_nonce, 13); */
> + /* print_packet("Net-Key", network_key, 16); */
> + /* print_packet("Net-Payload[clr]", packet, packet_len); */
> +
> + /* 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, sizeof(uint64_t)))
> + return false;
> + } else {
> + if (!mesh_crypto_aes_ccm_encrypt(network_nonce, network_key,
> + NULL, 0,
> + packet + 7, packet_len - 7 - 4,
> + packet + 7, NULL, sizeof(uint32_t)))
> + return false;
> + }
> +
> + /* print_packet("Net-Payload[enc]", packet, packet_len); */
> +
> + l_put_be32(iv_index, privacy_counter + 5);
> + memcpy(privacy_counter + 9, packet + 7, 7);
> +
> + /* print_packet("Priv-Random", privacy_counter, 16); */
> +
> + /* print_packet("Priv-Key", privacy_key, 16); */
> +
> +
> + if (!aes_ecb_one(privacy_key, privacy_counter, tmp))
> + return false;
> +
> + for (i = 0; i < 6; i++)
> + packet[1 + i] ^= tmp[i];
> +
> + /* print_packet("Net-Private", packet, packet_len); */
> +
> + 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;
> +
> + /* print_packet("Priv-Key", privacy_key, 16); */
> +
> + l_put_be32(iv_index, privacy_counter + 5);
> + memcpy(privacy_counter + 9, packet + 7, 7);
> +
> + /* print_packet("Priv-Random", privacy_counter, 16); */
> +
> + 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);
> +
> + /* print_packet("Net-Nonce", network_nonce, 13); */
> + /* print_packet("Net-Key", network_key, 16); */
> + /* print_packet("Net-Pkt[enc]", out, packet_len); */
> +
> + /* 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;
> + }
> +
> + /* print_packet("Net-Pkt[clr]", out, packet_len); */
> +
> + 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];
> +
> + if (fcs != 0xcf)
> + l_error("IOT Warning! CRC %2.2x != 0xcf", fcs);
This one is bad as well. The caller can print an error if chosen.
Regards
Marcel
^ permalink raw reply [flat|nested] 21+ messages in thread
* Re: [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon
2018-07-06 17:12 ` [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon Brian Gix
@ 2018-07-06 18:20 ` Marcel Holtmann
2018-07-08 15:05 ` Gix, Brian
0 siblings, 1 reply; 21+ messages in thread
From: Marcel Holtmann @ 2018-07-06 18:20 UTC (permalink / raw)
To: Brian Gix; +Cc: linux-bluetooth, Johan Hedberg, inga.stotland
Hi Brian,
> ---
> mesh/display.c | 67 +++++
> mesh/hci.c | 699 +++++++++++++++++++++++++++++++++++++++++++++++++
> mesh/mesh-io-generic.c | 660 ++++++++++++++++++++++++++++++++++++++++++++++
> mesh/mesh-io.c | 187 +++++++++++++
> 4 files changed, 1613 insertions(+)
> create mode 100644 mesh/display.c
> create mode 100644 mesh/hci.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..adf92cae0
> --- /dev/null
> +++ b/mesh/display.c
> @@ -0,0 +1,67 @@
> +/*
> + *
> + * 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"
> +
> +#define likely(x) __builtin_expect(!!(x), 1)
> +#define unlikely(x) __builtin_expect(!!(x), 0)
I highly doubt we will make good use of likely and unlikely. Just remove that. We can measure that at some point, but right now it seems useless. On a side note, I am not convinced that our usage in ELL is actually worth while doing.
> +
> +static unsigned int cached_num_columns;
> +
> +unsigned int num_columns(void)
> +{
> + struct winsize ws;
> +
> + if (likely(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_info("%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_info("%05d.%03d %s: empty",
> + (uint32_t) pkt_time.tv_sec % 100000,
> + (uint32_t) pkt_time.tv_usec/1000, label);
> +}
I have no idea why we are not using util_hexdump() or l_util_hexdump() since they are hooked up properly with debug defines etc.
> diff --git a/mesh/hci.c b/mesh/hci.c
> new file mode 100644
> index 000000000..da6838a55
> --- /dev/null
> +++ b/mesh/hci.c
> @@ -0,0 +1,699 @@
> +/*
> + *
> + * 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 <stdlib.h>
> +#include <sys/uio.h>
> +#include <sys/socket.h>
> +#include <ell/ell.h>
> +
> +#include "monitor/bt.h"
> +
> +#include "mesh/hci.h"
> +
> +#define BTPROTO_HCI 1
> +struct sockaddr_hci {
> + sa_family_t hci_family;
> + unsigned short hci_dev;
> + unsigned short hci_channel;
> +};
> +#define HCI_CHANNEL_USER 1
> +
> +struct bt_hci {
> + int ref_count;
> + struct l_io *io;
> + bool is_stream;
> + bool writer_active;
> + uint8_t num_cmds;
> + uint8_t num_pkts;
> + unsigned int next_cmd_id;
> + unsigned int next_evt_id;
> + struct l_queue *cmd_queue;
> + struct l_queue *rsp_queue;
> + struct l_queue *evt_list;
> + struct l_queue *pkt_queue;
> + bt_hci_receive_func_t receive_callback;
> + bt_hci_destroy_func_t receive_destroy;
> + void *receive_data;
> +};
What is wrong with src/shared/hci.[ch] instead having a copy here?
> +
> +struct cmd {
> + unsigned int id;
> + uint16_t opcode;
> + void *data;
> + uint8_t size;
> + bt_hci_callback_func_t callback;
> + bt_hci_destroy_func_t destroy;
> + void *user_data;
> +};
> +
> +struct evt {
> + unsigned int id;
> + uint8_t event;
> + bt_hci_callback_func_t callback;
> + bt_hci_destroy_func_t destroy;
> + void *user_data;
> +};
> +
> +struct pkt {
> + uint16_t handle;
> + uint8_t flags;
> + void *data;
> + uint16_t size;
> +};
> +
> +static void cmd_free(void *data)
> +{
> + struct cmd *cmd = data;
> +
> + if (cmd->destroy)
> + cmd->destroy(cmd->user_data);
> +
> + l_free(cmd->data);
> + l_free(cmd);
> +}
> +
> +static void evt_free(void *data)
> +{
> + struct evt *evt = data;
> +
> + if (evt->destroy)
> + evt->destroy(evt->user_data);
> +
> + l_free(evt);
> +}
> +
> +static void pkt_free(void *data)
> +{
> + struct pkt *pkt = data;
> +
> + l_free(pkt->data);
> + l_free(pkt);
> +}
> +
> +static void send_command(struct bt_hci *hci, uint16_t opcode,
> + void *data, uint8_t size)
> +{
> + uint8_t type = BT_H4_CMD_PKT;
> + struct bt_hci_cmd_hdr hdr;
> + struct iovec iov[3];
> + int fd, iovcnt;
> +
> + hdr.opcode = L_CPU_TO_LE16(opcode);
> + hdr.plen = size;
> +
> + iov[0].iov_base = &type;
> + iov[0].iov_len = 1;
> + iov[1].iov_base = &hdr;
> + iov[1].iov_len = sizeof(hdr);
> +
> + if (size > 0) {
> + iov[2].iov_base = data;
> + iov[2].iov_len = size;
> + iovcnt = 3;
> + } else
> + iovcnt = 2;
> +
> + fd = l_io_get_fd(hci->io);
> + if (fd < 0) {
> + l_error("hci->io Bad");
> + return;
> + }
> +
> + if (writev(fd, iov, iovcnt) < 0) {
> + l_error("writev Bad");
> + return;
> + }
> +
> + hci->num_cmds--;
> +}
> +
> +static void send_packet(struct bt_hci *hci, uint16_t handle, uint8_t flags,
> + void *data, uint8_t size)
> +{
> + uint8_t type = BT_H4_ACL_PKT;
> + struct bt_hci_acl_hdr hdr;
> + struct iovec iov[3];
> + int fd, iovcnt;
> +
> + hdr.handle = L_CPU_TO_LE16(flags << 12 | handle);
> + hdr.dlen = L_CPU_TO_LE16(size);
> +
> + iov[0].iov_base = &type;
> + iov[0].iov_len = 1;
> + iov[1].iov_base = &hdr;
> + iov[1].iov_len = sizeof(hdr);
> +
> + if (size > 0) {
> + iov[2].iov_base = data;
> + iov[2].iov_len = size;
> + iovcnt = 3;
> + } else
> + iovcnt = 2;
> +
> + fd = l_io_get_fd(hci->io);
> + if (fd < 0) {
> + l_error("hci->io Bad");
> + return;
> + }
> +
> + if (writev(fd, iov, iovcnt) < 0) {
> + l_error("writev Bad");
> + return;
> + }
> +
> + hci->num_pkts--;
> +}
> +
> +static bool io_write_callback(struct l_io *io, void *user_data)
> +{
> + struct bt_hci *hci = user_data;
> + struct cmd *cmd;
> +
> + if (hci->num_cmds > 0)
> + cmd = l_queue_pop_head(hci->cmd_queue);
> + else
> + cmd = NULL;
> +
> + if (cmd) {
> + send_command(hci, cmd->opcode, cmd->data, cmd->size);
> + l_queue_push_tail(hci->rsp_queue, cmd);
> + } else {
> + struct pkt *pkt;
> +
> + if (hci->num_pkts < 1)
> + goto done;
> +
> + pkt = l_queue_pop_head(hci->pkt_queue);
> + if (pkt) {
> + send_packet(hci, pkt->handle, pkt->flags,
> + pkt->data, pkt->size);
> + pkt_free(pkt);
> + }
> + }
> +
> + if (!l_queue_isempty(hci->pkt_queue))
> + return true;
> +
> +done:
> + hci->writer_active = false;
> +
> + return false;
> +}
> +
> +static void wakeup_writer(struct bt_hci *hci)
> +{
> + if (hci->writer_active)
> + return;
> +
> + if (l_queue_isempty(hci->cmd_queue) && l_queue_isempty(hci->pkt_queue))
> + return;
> +
> + if (!l_io_set_write_handler(hci->io, io_write_callback, hci, NULL))
> + return;
> +
> + hci->writer_active = true;
> +}
> +
> +static bool match_cmd_opcode(const void *a, const void *b)
> +{
> + const struct cmd *cmd = a;
> + uint16_t opcode = L_PTR_TO_UINT(b);
> +
> + return cmd->opcode == opcode;
> +}
> +
> +static void process_response(struct bt_hci *hci, uint16_t opcode,
> + const void *data, size_t size)
> +{
> + struct cmd *cmd;
> +
> + if (opcode == BT_HCI_CMD_NOP)
> + goto done;
> +
> + cmd = l_queue_remove_if(hci->rsp_queue, match_cmd_opcode,
> + L_UINT_TO_PTR(opcode));
> + if (!cmd)
> + goto done;
> +
> + if (cmd->callback)
> + cmd->callback(data, size, cmd->user_data);
> +
> + cmd_free(cmd);
> +
> +done:
> + wakeup_writer(hci);
> +}
> +
> +static void process_notify(void *data, void *user_data)
> +{
> + struct bt_hci_evt_hdr *hdr = user_data;
> + struct evt *evt = data;
> +
> + if (evt->event == hdr->evt)
> + evt->callback(user_data + sizeof(struct bt_hci_evt_hdr),
> + hdr->plen, evt->user_data);
> +}
> +
> +static void process_event(struct bt_hci *hci, const void *data, size_t size)
> +{
> + const struct bt_hci_evt_hdr *hdr = data;
> + const struct bt_hci_evt_cmd_complete *cc;
> + const struct bt_hci_evt_cmd_status *cs;
> + const struct bt_hci_evt_num_completed_packets *cp;
> +
> + if (size < sizeof(struct bt_hci_evt_hdr))
> + return;
> +
> + data += sizeof(struct bt_hci_evt_hdr);
> + size -= sizeof(struct bt_hci_evt_hdr);
> +
> + if (hdr->plen != size)
> + return;
> +
> + switch (hdr->evt) {
> + case BT_HCI_EVT_CMD_COMPLETE:
> + if (size < sizeof(*cc))
> + return;
> + cc = data;
> + hci->num_cmds = cc->ncmd;
> + process_response(hci, L_LE16_TO_CPU(cc->opcode),
> + data + sizeof(*cc),
> + size - sizeof(*cc));
> + break;
> +
> + case BT_HCI_EVT_CMD_STATUS:
> + if (size < sizeof(*cs))
> + return;
> + cs = data;
> + hci->num_cmds = cs->ncmd;
> + process_response(hci, L_LE16_TO_CPU(cs->opcode),
> + &cs->status, 1);
> + break;
> +
> + case BT_HCI_EVT_NUM_COMPLETED_PACKETS:
> + if (size < sizeof(*cp))
> + return;
> + cp = data;
> + l_debug("BT_HCI_EVT_NUM_COMPLETED_PACKETS:%d", cp->count);
> + /* Ignoring handle information for now */
> + hci->num_pkts = cp->count;
> + wakeup_writer(hci);
> + break;
> +
> + default:
> + l_queue_foreach(hci->evt_list, process_notify, (void *) hdr);
> + break;
> + }
> +}
> +
> +static void process_acl(struct bt_hci *hci, const void *data, size_t size)
> +{
> + const struct bt_hci_acl_hdr *hdr = data;
> + uint16_t handle;
> +
> + if (size < sizeof(struct bt_hci_acl_hdr))
> + return;
> +
> + data += sizeof(struct bt_hci_acl_hdr);
> + size -= sizeof(struct bt_hci_acl_hdr);
> +
> + if (L_LE16_TO_CPU(hdr->dlen) != size)
> + return;
> +
> + handle = L_LE16_TO_CPU(hdr->handle);
> +
> + if (hci->receive_callback)
> + hci->receive_callback(handle & 0x0fff, handle >> 12,
> + data, size, hci->receive_data);
> +}
> +
> +static bool io_read_callback(struct l_io *io, void *user_data)
> +{
> + struct bt_hci *hci = user_data;
> + uint8_t buf[512];
> + ssize_t len;
> + int fd;
> +
> + fd = l_io_get_fd(hci->io);
> + if (fd < 0)
> + return false;
> +
> + if (hci->is_stream)
> + return false;
> +
> + len = read(fd, buf, sizeof(buf));
> + if (len < 0)
> + return false;
> +
> + if (len < 1)
> + return true;
> +
> + switch (buf[0]) {
> + case BT_H4_EVT_PKT:
> + process_event(hci, buf + 1, len - 1);
> + break;
> + case BT_H4_ACL_PKT:
> + process_acl(hci, buf + 1, len - 1);
> + break;
> + default:
> + l_info("%2.2x-RXed", buf[0]);
> + }
> +
> + return true;
> +}
> +
> +static struct bt_hci *create_hci(int fd)
> +{
> + struct bt_hci *hci;
> +
> + if (fd < 0)
> + return NULL;
> +
> + hci = l_new(struct bt_hci, 1);
> +
> + hci->io = l_io_new(fd);
> +
> + hci->is_stream = true;
> + hci->writer_active = false;
> + hci->num_cmds = 1;
> + hci->num_pkts = 1;
> + hci->next_cmd_id = 1;
> + hci->next_evt_id = 1;
> +
> + hci->cmd_queue = l_queue_new();
> + hci->rsp_queue = l_queue_new();
> + hci->evt_list = l_queue_new();
> + hci->pkt_queue = l_queue_new();
> +
> + if (!l_io_set_read_handler(hci->io, io_read_callback, hci, NULL)) {
> + l_queue_destroy(hci->pkt_queue, NULL);
> + l_queue_destroy(hci->evt_list, NULL);
> + l_queue_destroy(hci->rsp_queue, NULL);
> + l_queue_destroy(hci->cmd_queue, NULL);
> + l_io_destroy(hci->io);
> + l_free(hci);
> + return NULL;
> + }
> +
> + return bt_hci_ref(hci);
> +}
> +
> +struct bt_hci *bt_hci_new(int fd)
> +{
> + struct bt_hci *hci;
> +
> + hci = create_hci(fd);
> + if (!hci)
> + return NULL;
> +
> + return hci;
> +}
> +
> +static int create_socket(uint16_t index, uint16_t channel)
> +{
> + struct sockaddr_hci addr;
> + int fd;
> +
> + fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
> + BTPROTO_HCI);
> + if (fd < 0)
> + return -1;
> +
> + memset(&addr, 0, sizeof(addr));
> + addr.hci_family = AF_BLUETOOTH;
> + addr.hci_dev = index;
> + addr.hci_channel = channel;
> +
> + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
> + close(fd);
> + return -1;
> + }
> +
> + return fd;
> +}
> +
> +struct bt_hci *bt_hci_new_user_channel(uint16_t index)
> +{
> + struct bt_hci *hci;
> + int fd;
> +
> + fd = create_socket(index, HCI_CHANNEL_USER);
> + if (fd < 0)
> + return NULL;
> +
> + hci = create_hci(fd);
> + if (!hci) {
> + close(fd);
> + return NULL;
> + }
> +
> + hci->is_stream = false;
> +
> + bt_hci_set_close_on_unref(hci, true);
> +
> + return hci;
> +}
> +
> +struct bt_hci *bt_hci_ref(struct bt_hci *hci)
> +{
> + if (!hci)
> + return NULL;
> +
> + __sync_fetch_and_add(&hci->ref_count, 1);
> +
> + return hci;
> +}
> +
> +void bt_hci_unref(struct bt_hci *hci)
> +{
> + if (!hci)
> + return;
> +
> + if (__sync_sub_and_fetch(&hci->ref_count, 1))
> + return;
> +
> + if (hci->receive_destroy)
> + hci->receive_destroy(hci->receive_data);
> +
> + l_queue_destroy(hci->pkt_queue, pkt_free);
> + l_queue_destroy(hci->evt_list, evt_free);
> + l_queue_destroy(hci->cmd_queue, cmd_free);
> + l_queue_destroy(hci->rsp_queue, cmd_free);
> +
> + l_io_destroy(hci->io);
> +
> + l_free(hci);
> +}
> +
> +bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close)
> +{
> + if (!hci)
> + return false;
> +
> + return l_io_set_close_on_destroy(hci->io, do_close);
> +}
> +
> +unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode,
> + const void *data, uint8_t size,
> + bt_hci_callback_func_t callback,
> + void *user_data, bt_hci_destroy_func_t destroy)
> +{
> + struct cmd *cmd;
> +
> + if (!hci)
> + return 0;
> +
> + cmd = l_new(struct cmd, 1);
> +
> + cmd->opcode = opcode;
> + cmd->size = size;
> +
> + if (cmd->size > 0)
> + cmd->data = l_memdup(data, cmd->size);
> +
> + if (hci->next_cmd_id < 1)
> + hci->next_cmd_id = 1;
> +
> + cmd->id = hci->next_cmd_id++;
> +
> + cmd->callback = callback;
> + cmd->destroy = destroy;
> + cmd->user_data = user_data;
> +
> + if (!l_queue_push_tail(hci->cmd_queue, cmd)) {
> + l_free(cmd->data);
> + l_free(cmd);
> + return 0;
> + }
> +
> + wakeup_writer(hci);
> +
> + return cmd->id;
> +}
> +
> +static bool match_cmd_id(const void *a, const void *b)
> +{
> + const struct cmd *cmd = a;
> + unsigned int id = L_PTR_TO_UINT(b);
> +
> + return cmd->id == id;
> +}
> +
> +bool bt_hci_cancel(struct bt_hci *hci, unsigned int id)
> +{
> + struct cmd *cmd;
> +
> + if (!hci || !id)
> + return false;
> +
> + cmd = l_queue_remove_if(hci->cmd_queue, match_cmd_id,
> + L_UINT_TO_PTR(id));
> + if (!cmd) {
> + cmd = l_queue_remove_if(hci->rsp_queue, match_cmd_id,
> + L_UINT_TO_PTR(id));
> + if (!cmd)
> + return false;
> + }
> +
> + cmd_free(cmd);
> +
> + wakeup_writer(hci);
> +
> + return true;
> +}
> +
> +bool bt_hci_flush(struct bt_hci *hci)
> +{
> + if (!hci)
> + return false;
> +
> + if (hci->writer_active) {
> + l_io_set_write_handler(hci->io, NULL, NULL, NULL);
> + hci->writer_active = false;
> + }
> +
> + l_queue_clear(hci->cmd_queue, cmd_free);
> + l_queue_clear(hci->rsp_queue, cmd_free);
> +
> + return true;
> +}
> +
> +unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event,
> + bt_hci_callback_func_t callback,
> + void *user_data, bt_hci_destroy_func_t destroy)
> +{
> + struct evt *evt;
> +
> + if (!hci)
> + return 0;
> +
> + evt = l_new(struct evt, 1);
> +
> + evt->event = event;
> +
> + if (hci->next_evt_id < 1)
> + hci->next_evt_id = 1;
> +
> + evt->id = hci->next_evt_id++;
> +
> + evt->callback = callback;
> + evt->destroy = destroy;
> + evt->user_data = user_data;
> +
> + if (!l_queue_push_tail(hci->evt_list, evt)) {
> + l_free(evt);
> + return 0;
> + }
> +
> + return evt->id;
> +}
> +
> +static bool match_evt_id(const void *a, const void *b)
> +{
> + const struct evt *evt = a;
> + unsigned int id = L_PTR_TO_UINT(b);
> +
> + return evt->id == id;
> +}
> +
> +bool bt_hci_unregister(struct bt_hci *hci, unsigned int id)
> +{
> + struct evt *evt;
> +
> + if (!hci || !id)
> + return false;
> +
> + evt = l_queue_remove_if(hci->evt_list, match_evt_id,
> + L_UINT_TO_PTR(id));
> + if (!evt)
> + return false;
> +
> + evt_free(evt);
> +
> + return true;
> +}
> +
> +bool bt_hci_receive(struct bt_hci *hci, bt_hci_receive_func_t callback,
> + void *user_data, bt_hci_destroy_func_t destroy)
> +{
> + if (!hci)
> + return false;
> +
> + if (hci->receive_destroy)
> + hci->receive_destroy(hci->receive_data);
> +
> + hci->receive_callback = callback;
> + hci->receive_destroy = destroy;
> + hci->receive_data = user_data;
> +
> + return true;
> +}
> +
> +bool bt_hci_write(struct bt_hci *hci, uint16_t handle, uint8_t flags,
> + const void *data, uint16_t size)
> +{
> + struct pkt *pkt;
> +
> + if (!hci)
> + return false;
> +
> + pkt = l_new(struct pkt, 1);
> +
> + pkt->handle = handle;
> + pkt->flags = flags;
> + pkt->size = size;
> +
> + if (pkt->size > 0)
> + pkt->data = l_memdup(data, pkt->size);
> +
> + if (!l_queue_push_tail(hci->pkt_queue, pkt)) {
> + l_free(pkt->data);
> + l_free(pkt);
> + return false;
> + }
> +
> + wakeup_writer(hci);
> +
> + return true;
> +}
> diff --git a/mesh/mesh-io-generic.c b/mesh/mesh-io-generic.c
> new file mode 100644
> index 000000000..7a974cff3
> --- /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 "mesh/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..762eb2c6f
> --- /dev/null
> +++ b/mesh/mesh-io.c
> @@ -0,0 +1,187 @@
> +/*
> + *
> + * 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},
> +};
{ MESH.., &mesh.. }
> +
> +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;
No alignment of variable names please.
> +
> + l_info("%s %d\n", __func__, type);
Empty line here.
> + 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 == NULL || api->init == NULL || io != NULL)
> + return NULL;
> +
> + io = l_new(struct mesh_io, 1);
> +
> + if (io == NULL)
> + return NULL;
Use if (!io).
> +
> + io->type = type;
> + io->index = index;
> + io->api = api;
> + if (!api->init(index, io))
> + goto fail;
> +
> + if (io_list == NULL)
> + 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;
> +}
Regards
Marcel
^ permalink raw reply [flat|nested] 21+ messages in thread
* Re: [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon
2018-07-06 18:20 ` Marcel Holtmann
@ 2018-07-08 15:05 ` Gix, Brian
2018-07-14 16:23 ` Marcel Holtmann
0 siblings, 1 reply; 21+ messages in thread
From: Gix, Brian @ 2018-07-08 15:05 UTC (permalink / raw)
To: marcel; +Cc: johan.hedberg, linux-bluetooth, Stotland, Inga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^ permalink raw reply [flat|nested] 21+ messages in thread
* Re: [PATCH BlueZ v5 02/14] mesh: Mesh crypto support
2018-07-06 18:10 ` Marcel Holtmann
@ 2018-07-08 15:07 ` Gix, Brian
0 siblings, 0 replies; 21+ messages in thread
From: Gix, Brian @ 2018-07-08 15:07 UTC (permalink / raw)
To: marcel; +Cc: johan.hedberg, linux-bluetooth, Stotland, Inga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^ permalink raw reply [flat|nested] 21+ messages in thread
* Re: [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon
2018-07-08 15:05 ` Gix, Brian
@ 2018-07-14 16:23 ` Marcel Holtmann
0 siblings, 0 replies; 21+ messages in thread
From: Marcel Holtmann @ 2018-07-14 16:23 UTC (permalink / raw)
To: Brian Gix; +Cc: Johan Hedberg, linux-bluetooth, Stotland, Inga
Hi Brian,
>>> ---
>>> mesh/display.c | 67 +++++
>>> mesh/hci.c | 699 +++++++++++++++++++++++++++++++++++++++++++++++++
>>> mesh/mesh-io-generic.c | 660 ++++++++++++++++++++++++++++++++++++++++++++++
>>> mesh/mesh-io.c | 187 +++++++++++++
>>> 4 files changed, 1613 insertions(+)
>>> create mode 100644 mesh/display.c
>>> create mode 100644 mesh/hci.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..adf92cae0
>>> --- /dev/null
>>> +++ b/mesh/display.c
>>> @@ -0,0 +1,67 @@
>>> +/*
>>> + *
>>> + * 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"
>>> +
>>> +#define likely(x) __builtin_expect(!!(x), 1)
>>> +#define unlikely(x) __builtin_expect(!!(x), 0)
>>
>> I highly doubt we will make good use of likely and unlikely. Just remove that. We can measure that at some
>> point, but right now it seems useless. On a side note, I am not convinced that our usage in ELL is actually
>> worth while doing.
>
> This will be removed for v6 of patch-set
>
>>
>>> +
>>> +static unsigned int cached_num_columns;
>>> +
>>> +unsigned int num_columns(void)
>>> +{
>>> + struct winsize ws;
>>> +
>>> + if (likely(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_info("%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_info("%05d.%03d %s: empty",
>>> + (uint32_t) pkt_time.tv_sec % 100000,
>>> + (uint32_t) pkt_time.tv_usec/1000, label);
>>> +}
>>
>> I have no idea why we are not using util_hexdump() or l_util_hexdump() since they are hooked up properly with
>> debug defines etc.
>
> l_util_hexdump does not create strings in the format we most use, which is a form that can be fed back into
> l_util_from_hexstring. l_util_hexdump adds whitespace between each octet, and ascii gorp at the end of the
> line, while l_util_hexstring returns a tightly packed octets-to-asciihex.
the l_util_hexdump is intentionally this way sine it is readable without making the brain hurt ;)
> We can then use the output from this function to copy/paste strings between instances for example to:
> 1. simulate "Out Of Band" public and static key exchange
> 2. trade via email long ascii-hex strings to IOP and UPF partners
> 3. paste repeated over-the-air packets to repeat exact test runs without preconditioning sequence numbers and
> IV Indexs etc.
>
> If you like, I can rework this to use l_debug, but the current output format of the data is more useful in this
> form, than it is in hexdump format.
Use l_debug or something where the output format can be changed quickly. Since I disagree with you that large streams of hex octets are useful. During development maybe, but unless you build a log decoder for it, it is useless.
So if you want this kind, then abstract it properly and cleanly. Also be able to switch if off by default so that it doesn't clog your logs.
>>> diff --git a/mesh/hci.c b/mesh/hci.c
>>> new file mode 100644
>>> index 000000000..da6838a55
>>> --- /dev/null
>>> +++ b/mesh/hci.c
>>> @@ -0,0 +1,699 @@
>>> +/*
>>> + *
>>> + * 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 <stdlib.h>
>>> +#include <sys/uio.h>
>>> +#include <sys/socket.h>
>>> +#include <ell/ell.h>
>>> +
>>> +#include "monitor/bt.h"
>>> +
>>> +#include "mesh/hci.h"
>>> +
>>> +#define BTPROTO_HCI 1
>>> +struct sockaddr_hci {
>>> + sa_family_t hci_family;
>>> + unsigned short hci_dev;
>>> + unsigned short hci_channel;
>>> +};
>>> +#define HCI_CHANNEL_USER 1
>>> +
>>> +struct bt_hci {
>>> + int ref_count;
>>> + struct l_io *io;
>>> + bool is_stream;
>>> + bool writer_active;
>>> + uint8_t num_cmds;
>>> + uint8_t num_pkts;
>>> + unsigned int next_cmd_id;
>>> + unsigned int next_evt_id;
>>> + struct l_queue *cmd_queue;
>>> + struct l_queue *rsp_queue;
>>> + struct l_queue *evt_list;
>>> + struct l_queue *pkt_queue;
>>> + bt_hci_receive_func_t receive_callback;
>>> + bt_hci_destroy_func_t receive_destroy;
>>> + void *receive_data;
>>> +};
>>
>> What is wrong with src/shared/hci.[ch] instead having a copy here?
>
> The main barrier to this is that the src/shared/hci.[ch] uses glib conventions, and the mesh versions use ELL.
> I agree that these should be a single hci.[ch], but it may need to wait until the rest of bluez is transitioned
> to ELL.
>
> I can add a /* TODO */ to the mesh version to ensure that this is eventually addressed (?).
No. I am not buying your argument. src/shared/hci.c has no GLib in it. It is fully IO agnostic. Actually everything in src/shared is IO agnostic. It is that way for a reason. If you can not make this work with ELL, then lets address that problem and not paper over it.
Regards
Marcel
^ permalink raw reply [flat|nested] 21+ messages in thread
end of thread, other threads:[~2018-07-14 16:23 UTC | newest]
Thread overview: 21+ messages (download: mbox.gz / follow: Atom feed)
-- links below jump to the message on this page --
2018-07-06 17:12 [PATCH BlueZ v5 00/14] Bluetooth Mesh Daemon Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 01/14] mesh: Shared private meshd interfaces Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 02/14] mesh: Mesh crypto support Brian Gix
2018-07-06 18:10 ` Marcel Holtmann
2018-07-08 15:07 ` Gix, Brian
2018-07-06 17:12 ` [PATCH BlueZ v5 03/14] mesh: Infrastructure for Mesh daemon Brian Gix
2018-07-06 18:20 ` Marcel Holtmann
2018-07-08 15:05 ` Gix, Brian
2018-07-14 16:23 ` Marcel Holtmann
2018-07-06 17:12 ` [PATCH BlueZ v5 04/14] mesh: Initial Mesh Friendship support Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 05/14] mesh: Provisioning logic for mesh Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 06/14] mesh: Upper and Lower mesh transport Brian Gix
2018-07-06 17:12 ` [PATCH BlueZ v5 07/14] mesh: Add Accessors to Transport layer data Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 08/14] mesh: Header files for mesh access layer and utilities Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 09/14] mesh: Source " Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 10/14] mesh: Source code for handling access layer mux/demux Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 11/14] mesh: Mesh config server model Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 12/14] mesh: Read and write mesh configuration in JSON format Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 13/14] mesh: Sample device composition " Brian Gix
2018-07-06 17:13 ` [PATCH BlueZ v5 14/14] Makefile for meshd Brian Gix
2018-07-06 17:23 ` Marcel Holtmann
This is an external index of several public inboxes,
see mirroring instructions on how to clone and mirror
all data and code used by this external index.