qmimodem: QRTR service discovery

QRTR service discovery works by sending QRTR_TYPE_NEW_LOOKUP to
the special socket of type AF_QIPCRTR. Then the services are received
one-by-one. Soon they will be stored and made available for use by the
qmi client.

There was a re-entrancy problem where the qmi_device could be
destroyed in a callback, but then the code running would still try
to access that device. This was solved by creating a common macro
that would do things in the right order so that the qmi_device was
not accessed again after calling back into the client.

QRTR modem discovery and instantiation will be implemented later.

2 files changed, 267 insertions, 23 deletions

diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c
index 35751d7ce..36b99f8d3 100644
--- a/drivers/qmimodem/qmi.c
+++ b/drivers/qmimodem/qmi.c
@@ -33,6 +33,8 @@
 #include <stdlib.h>
 #include <string.h>
 #include <linux/limits.h>
+#include <linux/qrtr.h>
+#include <sys/socket.h>
 
 #include <ell/ell.h>
 
@@ -784,10 +786,22 @@ static void __qmi_device_discovery_complete(struct qmi_device *device,
 {
 	if (!l_queue_remove(device->discovery_queue, d))
 		return;
-
-	__discovery_free(d);
 }
 
+/*
+ * Prevents re-entrancy problems by removing the entry from the discovery_queue
+ * before calling the callback.
+ */
+#define DISCOVERY_DONE(data, ...)\
+do {\
+	__qmi_device_discovery_complete(data->device, &data->super);\
+\
+	if (data->func)\
+		data->func(__VA_ARGS__);\
+\
+	__discovery_free(&data->super);\
+} while (0)
+
 static void service_destroy(void *data)
 {
 	struct qmi_service *service = data;
@@ -1344,9 +1358,8 @@ static void service_create_shared_reply(struct l_idle *idle, void *user_data)
 
 	l_idle_remove(data->idle);
 	data->idle = NULL;
-	data->func(data->service, data->user_data);
 
-	__qmi_device_discovery_complete(data->device, &data->super);
+	DISCOVERY_DONE(data, data->service, data->user_data);
 }
 
 static void service_create_shared_pending_reply(struct qmi_device *device,
@@ -1406,10 +1419,7 @@ static void qmux_sync_callback(uint16_t message, uint16_t length,
 {
 	struct discover_data *data = user_data;
 
-	if (data->func)
-		data->func(data->user_data);
-
-	__qmi_device_discovery_complete(data->device, &data->super);
+	DISCOVERY_DONE(data, data->user_data);
 }
 
 /* sync will release all previous clients */
@@ -1510,10 +1520,7 @@ done:
 		return;
 	}
 
-	if (data->func)
-		data->func(data->user_data);
-
-	__qmi_device_discovery_complete(data->device, &data->super);
+	DISCOVERY_DONE(data, data->user_data);
 }
 
 static void qmux_discover_reply_timeout(struct l_timeout *timeout,
@@ -1531,10 +1538,7 @@ static void qmux_discover_reply_timeout(struct l_timeout *timeout,
 	/* remove request from queues */
 	req = find_control_request(qmux, data->tid);
 
-	if (data->func)
-		data->func(data->user_data);
-
-	__qmi_device_discovery_complete(device, &data->super);
+	DISCOVERY_DONE(data, data->user_data);
 
 	if (req)
 		__request_free(req);
@@ -1620,10 +1624,7 @@ static void qmux_client_create_reply(struct l_timeout *timeout, void *user_data)
 	l_timeout_remove(data->timeout);
 	data->timeout = NULL;
 
-	if (data->func)
-		data->func(NULL, data->user_data);
-
-	__qmi_device_discovery_complete(device, &data->super);
+	DISCOVERY_DONE(data, NULL, data->user_data);
 
 	if (req)
 		__request_free(req);
@@ -1684,10 +1685,8 @@ static void qmux_client_create_callback(uint16_t message, uint16_t length,
 done:
 	service_create_shared_pending_reply(device, data->type, service);
 
-	data->func(service, data->user_data);
+	DISCOVERY_DONE(data, service, data->user_data);
 	qmi_service_unref(service);
-
-	__qmi_device_discovery_complete(data->device, &data->super);
 }
 
 static int qmi_device_qmux_client_create(struct qmi_device *device,
@@ -1874,6 +1873,250 @@ struct qmi_device *qmi_device_new_qmux(const char *device)
 	return &qmux->super;
 }
 
+struct qmi_device_qrtr {
+	struct qmi_device super;
+	qmi_shutdown_func_t shutdown_func;
+	void *shutdown_user_data;
+	qmi_destroy_func_t shutdown_destroy;
+	struct l_idle *shutdown_idle;
+};
+
+static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
+					qmi_debug_func_t function,
+					void *user_data)
+{
+	char strbuf[72 + 16], *str;
+	const char *type;
+
+	if (!function)
+		return;
+
+	str = strbuf;
+	str += sprintf(str, "    %s",
+			__service_type_to_string(QMI_SERVICE_CONTROL));
+
+	type = "_pkt";
+
+	str += sprintf(str, "%s cmd=%d", type,
+				L_LE32_TO_CPU(packet->cmd));
+
+	function(strbuf, user_data);
+}
+
+static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
+					const struct qrtr_ctrl_pkt *packet)
+{
+	struct qmi_device *device = &qrtr->super;
+	uint32_t cmd;
+	uint32_t type;
+	uint32_t instance;
+	uint32_t version;
+	uint32_t node;
+	uint32_t port;
+
+	qrtr_debug_ctrl_request(packet, device->debug_func,
+				device->debug_data);
+
+	cmd = L_LE32_TO_CPU(packet->cmd);
+	if (cmd != QRTR_TYPE_NEW_SERVER) {
+		DBG("Unknown command: %d", cmd);
+		return;
+	}
+
+	if (!packet->server.service && !packet->server.instance &&
+			!packet->server.node && !packet->server.port) {
+		struct discover_data *data;
+
+		DBG("Initial service discovery has completed");
+
+		data = l_queue_peek_head(device->discovery_queue);
+		DISCOVERY_DONE(data, data->user_data);
+
+		return;
+	}
+
+	type = L_LE32_TO_CPU(packet->server.service);
+	version = L_LE32_TO_CPU(packet->server.instance) & 0xff;
+	instance = L_LE32_TO_CPU(packet->server.instance) >> 8;
+
+	node = L_LE32_TO_CPU(packet->server.node);
+	port = L_LE32_TO_CPU(packet->server.port);
+
+	if (cmd == QRTR_TYPE_NEW_SERVER) {
+		DBG("New server: Type: %d Version: %d Instance: %d Node: %d Port: %d",
+			type, version, instance, node, port);
+	}
+}
+
+static void qrtr_handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port,
+				const void *buf, ssize_t len)
+{
+	const struct qrtr_ctrl_pkt *packet = buf;
+
+	if (sending_port != QRTR_PORT_CTRL) {
+		DBG("Receive of service data is not implemented");
+		return;
+	}
+
+	if ((unsigned long) len < sizeof(*packet)) {
+		DBG("qrtr control packet is too small");
+		return;
+	}
+
+	qrtr_handle_control_packet(qrtr, packet);
+}
+
+static bool qrtr_received_data(struct l_io *io, void *user_data)
+{
+	struct qmi_device_qrtr *qrtr = user_data;
+	struct sockaddr_qrtr addr;
+	unsigned char buf[2048];
+	ssize_t bytes_read;
+	socklen_t addr_size;
+
+	addr_size = sizeof(addr);
+	bytes_read = recvfrom(l_io_get_fd(qrtr->super.io), buf, sizeof(buf), 0,
+				(struct sockaddr *) &addr, &addr_size);
+	DBG("Received %zd bytes from Node: %d Port: 0x%x", bytes_read,
+		addr.sq_node, addr.sq_port);
+
+	if (bytes_read < 0)
+		return true;
+
+	l_util_hexdump(true, buf, bytes_read, qrtr->super.debug_func,
+			qrtr->super.debug_data);
+
+	qrtr_handle_packet(qrtr, addr.sq_port, buf, bytes_read);
+
+	return true;
+}
+
+static void qrtr_discover_reply_timeout(struct l_timeout *timeout,
+							void *user_data)
+{
+	struct discover_data *data = user_data;
+
+	l_timeout_remove(data->timeout);
+	data->timeout = NULL;
+
+	DISCOVERY_DONE(data, data->user_data);
+}
+
+static int qmi_device_qrtr_discover(struct qmi_device *device,
+					qmi_discover_func_t func,
+					void *user_data,
+					qmi_destroy_func_t destroy)
+{
+	struct discover_data *data;
+	struct qrtr_ctrl_pkt packet;
+	struct sockaddr_qrtr addr;
+	socklen_t addr_len;
+	int rc;
+	ssize_t bytes_written;
+	int fd;
+
+	__debug_device(device, "device %p discover", device);
+
+	if (l_queue_length(device->discovery_queue) > 0)
+		return -EINPROGRESS;
+
+	data = l_new(struct discover_data, 1);
+
+	data->super.destroy = discover_data_free;
+	data->device = device;
+	data->func = func;
+	data->user_data = user_data;
+	data->destroy = destroy;
+
+	fd = l_io_get_fd(device->io);
+
+	/*
+	 * The control node is configured by the system. Use getsockname to
+	 * get its value.
+	 */
+	addr_len = sizeof(addr);
+	rc = getsockname(fd, (struct sockaddr *) &addr, &addr_len);
+	if (rc) {
+		DBG("getsockname failed: %s", strerror(errno));
+		rc = -errno;
+		goto error;
+	}
+
+	if (addr.sq_family != AF_QIPCRTR || addr_len != sizeof(addr)) {
+		DBG("Unexpected sockaddr from getsockname. family: %d size: %d",
+			addr.sq_family, addr_len);
+		rc = -EIO;
+		goto error;
+	}
+
+	addr.sq_port = QRTR_PORT_CTRL;
+	memset(&packet, 0, sizeof(packet));
+	packet.cmd = L_CPU_TO_LE32(QRTR_TYPE_NEW_LOOKUP);
+
+	bytes_written = sendto(fd, &packet,
+				sizeof(packet), 0,
+				(struct sockaddr *) &addr, addr_len);
+	if (bytes_written < 0) {
+		DBG("Failure sending data: %s", strerror(errno));
+		rc = -errno;
+		goto error;
+	}
+
+	l_util_hexdump(false, &packet, bytes_written,
+			device->debug_func, device->debug_data);
+
+	data->timeout = l_timeout_create(5, qrtr_discover_reply_timeout,
+								data, NULL);
+
+	__qmi_device_discovery_started(device, &data->super);
+
+	return 0;
+
+error:
+	__discovery_free(&data->super);
+
+	return rc;
+}
+
+static void qmi_device_qrtr_destroy(struct qmi_device *device)
+{
+	struct qmi_device_qrtr *qrtr =
+		l_container_of(device, struct qmi_device_qrtr, super);
+
+	l_free(qrtr);
+}
+
+static const struct qmi_device_ops qrtr_ops = {
+	.write = NULL,
+	.discover = qmi_device_qrtr_discover,
+	.client_create = NULL,
+	.client_release = NULL,
+	.shutdown = NULL,
+	.destroy = qmi_device_qrtr_destroy,
+};
+
+struct qmi_device *qmi_device_new_qrtr(void)
+{
+	struct qmi_device_qrtr *qrtr;
+	int fd;
+
+	fd = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+	if (fd < 0)
+		return NULL;
+
+	qrtr = l_new(struct qmi_device_qrtr, 1);
+
+	if (qmi_device_init(&qrtr->super, fd, &qrtr_ops) < 0) {
+		close(fd);
+		l_free(qrtr);
+		return NULL;
+	}
+
+	l_io_set_read_handler(qrtr->super.io, qrtr_received_data, qrtr, NULL);
+
+	return &qrtr->super;
+}
+
 struct qmi_param *qmi_param_new(void)
 {
 	struct qmi_param *param;
diff --git a/drivers/qmimodem/qmi.h b/drivers/qmimodem/qmi.h
index 6a3d3415d..506fed6b3 100644
--- a/drivers/qmimodem/qmi.h
+++ b/drivers/qmimodem/qmi.h
@@ -101,6 +101,7 @@ bool qmi_device_set_expected_data_format(struct qmi_device *device,
 			enum qmi_device_expected_data_format format);
 
 struct qmi_device *qmi_device_new_qmux(const char *device);
+struct qmi_device *qmi_device_new_qrtr(void);
 
 struct qmi_param;