Received-SPF: pass (google.com: domain of linux-bluetooth-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) client-ip=23.128.96.18;
From:   =?UTF-8?q?Micha=C5=82=20Lowas-Rzechonek?= 
        <michal.lowas-rzechonek@silvair.com>
To:     linux-bluetooth@vger.kernel.org
Subject: [RFC BlueZ 2/2] mesh: Implement AttachFD method
Date:   Tue, 16 Jun 2020 14:27:45 +0200
Message-Id: <20200616122745.25056-3-michal.lowas-rzechonek@silvair.com>
In-Reply-To: <20200616122745.25056-1-michal.lowas-rzechonek@silvair.com>
References: <20200616122745.25056-1-michal.lowas-rzechonek@silvair.com>
MIME-Version: 1.0
Content-Transfer-Encoding: 8bit
Sender: linux-bluetooth-owner@vger.kernel.org
Precedence: bulk

---
 mesh/mesh.c  |  12 ++++-
 mesh/model.c | 126 ++++++++++++++++++++++++++++++++++++++++++++++++++-
 mesh/node.c  |  83 ++++++++++++++++++++++++++++++++-
 mesh/node.h  |   4 +-
 4 files changed, 219 insertions(+), 6 deletions(-)

diff --git a/mesh/mesh.c b/mesh/mesh.c
index 4abae4d92..f43d00b7c 100644
--- a/mesh/mesh.c
+++ b/mesh/mesh.c
@@ -612,14 +612,18 @@ static void attach_ready_cb(void *user_data, int status, struct mesh_node *node)
 {
 	struct l_dbus_message *reply;
 	struct l_dbus_message *pending_msg;
+	const char *method;
 
 	pending_msg = l_queue_remove_if(pending_queue, simple_match, user_data);
 	if (!pending_msg)
 		return;
 
+	method = l_dbus_message_get_member(pending_msg);
+
 	if (status == MESH_ERROR_NONE) {
 		reply = l_dbus_message_new_method_return(pending_msg);
-		node_build_attach_reply(node, reply);
+		node_build_attach_reply(node, reply,
+						!strcmp(method, "AttachFD"));
 	} else
 		reply = dbus_error(pending_msg, status, "Attach failed");
 
@@ -635,7 +639,7 @@ static struct l_dbus_message *attach_call(struct l_dbus *dbus,
 	const char *app_path, *sender;
 	struct l_dbus_message *pending_msg;
 
-	l_debug("Attach");
+	l_debug("%s", l_dbus_message_get_member(msg));
 
 	if (!l_dbus_message_get_arguments(msg, "ot", &app_path, &token))
 		return dbus_error(msg, MESH_ERROR_INVALID_ARGS, NULL);
@@ -846,6 +850,10 @@ static void setup_network_interface(struct l_dbus_interface *iface)
 					"oa(ya(qa{sv}))", "ot", "node",
 					"configuration", "app", "token");
 
+	l_dbus_interface_method(iface, "AttachFD", 0, attach_call,
+					"oa(ya(qa{sv}))h", "ot", "node",
+					"configuration", "fd", "app", "token");
+
 	l_dbus_interface_method(iface, "Leave", 0, leave_call, "", "t",
 								"token");
 
diff --git a/mesh/model.c b/mesh/model.c
index 5ed95afac..0bef11e36 100644
--- a/mesh/model.c
+++ b/mesh/model.c
@@ -21,6 +21,8 @@
 #include <config.h>
 #endif
 
+#include <sys/types.h>
+#include <sys/socket.h>
 #include <sys/time.h>
 #include <ell/ell.h>
 
@@ -44,6 +46,33 @@
 
 #define VIRTUAL_BASE			0x10000
 
+enum fd_msg_type {
+	DEV_KEY_MSG = 0,
+	APP_KEY_MSG = 1,
+};
+
+struct fd_msg {
+
+	uint8_t element;
+	uint16_t src;
+
+	enum fd_msg_type type :8;
+	union {
+		struct {
+			uint16_t net_idx;
+			uint8_t remote;
+		} dev;
+
+		struct {
+			uint16_t app_idx;
+			uint16_t dst;
+			uint8_t label[16];
+		} app;
+	};
+
+	uint8_t data[];
+} __attribute__((packed));
+
 struct mesh_model {
 	const struct mesh_model_ops *cbs;
 	void *user_data;
@@ -782,7 +811,50 @@ static int add_sub(struct mesh_net *net, struct mesh_model *mod,
 	return MESH_STATUS_SUCCESS;
 }
 
-static void send_dev_key_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
+static struct fd_msg *fd_msg_new(uint8_t ele_idx, uint16_t src, uint16_t size, const uint8_t *data, enum fd_msg_type type)
+{
+	size_t msg_len = sizeof(struct fd_msg) + size;
+	struct fd_msg *msg = l_malloc(msg_len);
+
+	msg->element = ele_idx;
+	msg->src = src;
+	msg->type = type;
+
+	memcpy(msg->data, data, size);
+
+	return msg;
+}
+
+static void fd_msg_send(struct l_io *io, struct fd_msg *msg, size_t size)
+{
+	struct iovec iov = {
+		.iov_base = msg,
+		.iov_len = sizeof(struct fd_msg) + size,
+	};
+	struct msghdr hdr = {
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+
+	(void)sendmsg(l_io_get_fd(io), &hdr, MSG_NOSIGNAL);
+
+	l_free(msg);
+}
+
+static void send_fd_dev_key_msg_rcvd(struct l_io *io, uint8_t ele_idx,
+				     uint16_t src, uint16_t app_idx,
+				     uint16_t net_idx, uint16_t size,
+				     const uint8_t *data)
+{
+	struct fd_msg *msg = fd_msg_new(ele_idx, src, size, data, DEV_KEY_MSG);
+
+	msg->dev.net_idx = net_idx;
+	msg->dev.remote = (app_idx != APP_IDX_DEV_LOCAL);
+
+	fd_msg_send(io, msg, size);
+}
+
+static void send_dbus_dev_key_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
 					uint16_t src, uint16_t app_idx,
 					uint16_t net_idx, uint16_t size,
 					const uint8_t *data)
@@ -818,7 +890,40 @@ static void send_dev_key_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
 	l_dbus_send(dbus, msg);
 }
 
-static void send_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
+static void send_dev_key_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
+				       uint16_t src, uint16_t app_idx,
+				       uint16_t net_idx, uint16_t size,
+				       const uint8_t *data)
+{
+	struct l_io *io = node_get_fd_io(node);
+
+	if (io)
+		send_fd_dev_key_msg_rcvd(io, ele_idx, src, app_idx, net_idx,
+					 size, data);
+	else
+		send_dbus_dev_key_msg_rcvd(node, ele_idx, src, app_idx, net_idx,
+					   size, data);
+}
+
+static void send_fd_msg_rcvd(struct l_io *io, uint8_t ele_idx,
+			     uint16_t src, uint16_t dst,
+			     const struct mesh_virtual *virt,
+			     uint16_t app_idx,
+			     uint16_t size, const uint8_t *data)
+{
+	struct fd_msg *msg = fd_msg_new(ele_idx, src, size, data, APP_KEY_MSG);
+
+	msg->app.app_idx = app_idx;
+	msg->app.dst = dst;
+
+	if (virt)
+		memcpy(msg->app.label, virt, sizeof(msg->app.label));
+
+	fd_msg_send(io, msg, size);
+}
+
+
+static void send_dbus_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
 					uint16_t src, uint16_t dst,
 					const struct mesh_virtual *virt,
 					uint16_t app_idx,
@@ -863,6 +968,22 @@ static void send_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
 	l_dbus_send(dbus, msg);
 }
 
+static void send_msg_rcvd(struct mesh_node *node, uint8_t ele_idx,
+			  uint16_t src, uint16_t dst,
+			  const struct mesh_virtual *virt,
+			  uint16_t app_idx,
+			  uint16_t size, const uint8_t *data)
+{
+	struct l_io *io = node_get_fd_io(node);
+
+	if (io)
+		send_fd_msg_rcvd(io, ele_idx, src, dst, virt, app_idx,
+				 size, data);
+	else
+		send_dbus_msg_rcvd(node, ele_idx, src, dst, virt, app_idx,
+				   size, data);
+}
+
 bool mesh_model_rx(struct mesh_node *node, bool szmict, uint32_t seq0,
 			uint32_t seq, uint32_t iv_index,
 			uint16_t net_idx, uint16_t src, uint16_t dst,
@@ -986,6 +1107,7 @@ bool mesh_model_rx(struct mesh_node *node, bool szmict, uint32_t seq0,
 				send_msg_rcvd(node, i, src, dst, decrypt_virt,
 						forward.app_idx, forward.size,
 						forward.data);
+
 			else if (decrypt_idx == APP_IDX_DEV_REMOTE ||
 				 decrypt_idx == APP_IDX_DEV_LOCAL)
 				send_dev_key_msg_rcvd(node, i, src, decrypt_idx,
diff --git a/mesh/node.c b/mesh/node.c
index 6140fdf9f..e00bed785 100644
--- a/mesh/node.c
+++ b/mesh/node.c
@@ -26,6 +26,8 @@
 #include <limits.h>
 #include <stdio.h>
 #include <sys/time.h>
+#include <sys/socket.h>
+#include <unistd.h>
 
 #include <ell/ell.h>
 
@@ -105,6 +107,7 @@ struct mesh_node {
 	uint8_t proxy;
 	uint8_t friend;
 	uint8_t beacon;
+	struct l_io *fd_io;
 };
 
 struct node_import {
@@ -260,6 +263,7 @@ static void set_defaults(struct mesh_node *node)
 							MESH_MODE_UNSUPPORTED;
 	node->ttl = TTL_MASK;
 	node->seq_number = DEFAULT_SEQUENCE_NUMBER;
+	node->fd_io = NULL;
 }
 
 static struct mesh_node *node_new(const uint8_t uuid[16])
@@ -341,6 +345,13 @@ static void free_node_resources(void *data)
 	mesh_agent_remove(node->agent);
 	mesh_config_release(node->cfg);
 	mesh_net_free(node->net);
+
+	if (node->fd_io)
+	{
+		l_io_destroy(node->fd_io);
+		node->fd_io = NULL;
+	}
+
 	l_free(node->storage_dir);
 	l_free(node);
 }
@@ -745,6 +756,11 @@ uint16_t node_get_crpl(struct mesh_node *node)
 	return node->comp.crpl;
 }
 
+struct l_io *node_get_fd_io(struct mesh_node *node)
+{
+	return node->fd_io;
+}
+
 uint8_t node_relay_mode_get(struct mesh_node *node, uint8_t *count,
 							uint16_t *interval)
 {
@@ -1650,6 +1666,53 @@ static void send_managed_objects_request(const char *destination,
 					req, l_free, DEFAULT_DBUS_TIMEOUT);
 }
 
+static void fd_io_hup(struct l_io *io, void *user_data)
+{
+	struct mesh_node *node = user_data;
+
+	node->fd_io = NULL;
+
+	l_io_destroy(io);
+}
+
+static struct l_io *fd_io_new(struct mesh_node *node, int *fd)
+{
+	struct l_io *io;
+	int fds[2];
+
+	if (socketpair(AF_LOCAL, SOCK_DGRAM | SOCK_NONBLOCK | SOCK_CLOEXEC,
+		       0, fds) < 0)
+	{
+		return NULL;
+	}
+
+	io = l_io_new(fds[0]);
+	if (!io)
+	{
+		goto fail;
+	}
+
+	l_io_set_close_on_destroy(io, true);
+
+	if (!l_io_set_disconnect_handler(io, fd_io_hup, node, NULL))
+	{
+		goto fail;
+	}
+
+	*fd = fds[1];
+
+	return io;
+
+fail:
+	if (io)
+		l_io_destroy(io);
+
+	close(fds[0]);
+	close(fds[1]);
+
+	return NULL;
+}
+
 /* Establish relationship between application and mesh node */
 void node_attach(const char *app_root, const char *sender, uint64_t token,
 					node_ready_func_t cb, void *user_data)
@@ -1770,8 +1833,16 @@ static void build_element_config(void *a, void *b)
 	l_dbus_message_builder_leave_struct(builder);
 }
 
+static void append_fd(struct l_dbus_message_builder *builder, ...)
+{
+	va_list args;
+	va_start(args, builder);
+	l_dbus_message_builder_append_from_valist(builder, "h", args);
+	va_end(args);
+}
+
 void node_build_attach_reply(struct mesh_node *node,
-						struct l_dbus_message *reply)
+				struct l_dbus_message *reply, bool use_fd)
 {
 	struct l_dbus_message_builder *builder;
 
@@ -1784,6 +1855,16 @@ void node_build_attach_reply(struct mesh_node *node,
 	l_dbus_message_builder_enter_array(builder, "(ya(qa{sv}))");
 	l_queue_foreach(node->elements, build_element_config, builder);
 	l_dbus_message_builder_leave_array(builder);
+
+	if (use_fd)
+	{
+		int fd = -1;
+		node->fd_io = fd_io_new(node, &fd);
+		append_fd(builder, fd);
+
+		close(fd);
+	}
+
 	l_dbus_message_builder_finalize(builder);
 	l_dbus_message_builder_destroy(builder);
 }
diff --git a/mesh/node.h b/mesh/node.h
index e26d410c8..6d0696824 100644
--- a/mesh/node.h
+++ b/mesh/node.h
@@ -23,6 +23,7 @@ struct mesh_io;
 struct mesh_agent;
 struct mesh_config;
 struct mesh_config_node;
+struct l_io;
 
 typedef void (*node_ready_func_t) (void *user_data, int status,
 							struct mesh_node *node);
@@ -77,13 +78,14 @@ uint8_t node_friend_mode_get(struct mesh_node *node);
 const char *node_get_element_path(struct mesh_node *node, uint8_t ele_idx);
 const char *node_get_owner(struct mesh_node *node);
 const char *node_get_app_path(struct mesh_node *node);
+struct l_io *node_get_fd_io(struct mesh_node *node);
 bool node_add_pending_local(struct mesh_node *node, void *info);
 void node_attach_io_all(struct mesh_io *io);
 void node_attach_io(struct mesh_node *node, struct mesh_io *io);
 void node_attach(const char *app_root, const char *sender, uint64_t token,
 					node_ready_func_t cb, void *user_data);
 void node_build_attach_reply(struct mesh_node *node,
-						struct l_dbus_message *reply);
+				struct l_dbus_message *reply, bool use_fd);
 void node_create(const char *app_root, const char *sender, const uint8_t *uuid,
 					node_ready_func_t cb, void *user_data);
 void node_import(const char *app_root, const char *sender, const uint8_t *uuid,
-- 
2.20.1