[v4,4/4] qmi: Enable QRTR service writes and reads

Commit Message

Steve Schrock March 1, 2024, 11:45 p.m. UTC

For simplicity the QRTR write method simply skips the QMUX
header. In the future it might make sense to add another ops
function that would allocate a larger buffer and populate
the QMUX header in the QMUX implementation.
---
 drivers/qmimodem/qmi.c | 119 +++++++++++++++++++++++++++++++++--------
 1 file changed, 98 insertions(+), 21 deletions(-)

Patch

diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c
index 570d9bfd..0af402eb 100644
--- a/drivers/qmimodem/qmi.c
+++ b/drivers/qmimodem/qmi.c
@@ -62,7 +62,7 @@  struct qmi_service_info {
 struct qmi_request {
 	uint16_t tid;
 	unsigned int group_id;		/* Always 0 for control */
-	uint8_t client;
+	uint8_t client;			/* Always 0 for control and qrtr */
 	struct qmi_service_info info;	/* Not used for control requests */
 	qmi_message_func_t callback;
 	void *user_data;
@@ -216,7 +216,7 @@  static void __qmi_service_appeared(struct qmi_device *device,
 				l_memdup(info, sizeof(struct qmi_service_info)));
 }
 
-static struct qmi_request *__request_alloc(uint8_t service,
+static struct qmi_request *__request_alloc(uint32_t service_type,
 				uint8_t client, uint16_t message,
 				const void *data,
 				uint16_t length, qmi_message_func_t func,
@@ -228,7 +228,7 @@  static struct qmi_request *__request_alloc(uint8_t service,
 	uint16_t hdrlen = QMI_MUX_HDR_SIZE;
 	uint16_t msglen;
 
-	if (service == QMI_SERVICE_CONTROL)
+	if (service_type == QMI_SERVICE_CONTROL)
 		hdrlen += QMI_CONTROL_HDR_SIZE;
 	else
 		hdrlen += QMI_SERVICE_HDR_SIZE;
@@ -244,7 +244,7 @@  static struct qmi_request *__request_alloc(uint8_t service,
 	hdr->frame = 0x01;
 	hdr->length = L_CPU_TO_LE16(req->len - 1);
 	hdr->flags = 0x00;
-	hdr->service = service;
+	hdr->service = service_type; /* qmux service types are 8 bits */
 	hdr->client = client;
 
 	msg = (struct qmi_message_hdr *) &req->data[hdrlen];
@@ -654,6 +654,26 @@  static void __qmux_debug_msg(const char dir, const void *buf, size_t len,
 			L_LE16_TO_CPU(hdr->length), function, user_data);
 }
 
+static void __qrtr_debug_msg(const char dir, const void *buf, size_t len,
+				uint32_t service_type,
+				qmi_debug_func_t function, void *user_data)
+{
+	const struct qmi_service_hdr *srv;
+	const struct qmi_message_hdr *msg;
+	uint16_t tid;
+
+	if (!len)
+		return;
+
+	srv = buf;
+	msg = buf + QMI_SERVICE_HDR_SIZE;
+
+	tid = L_LE16_TO_CPU(srv->transaction);
+
+	__debug_msg(dir, msg, service_type, srv->type, tid, 0, len,
+						function, user_data);
+}
+
 static void __debug_device(struct qmi_device *device,
 					const char *format, ...)
 {
@@ -762,7 +782,7 @@  static unsigned int service_list_create_hash(uint16_t service_type,
 }
 
 static void handle_indication(struct qmi_device *device,
-			uint8_t service_type, uint8_t client_id,
+			uint32_t service_type, uint8_t client_id,
 			uint16_t message, uint16_t length, const void *data)
 {
 	struct qmi_service *service;
@@ -795,7 +815,7 @@  static void handle_indication(struct qmi_device *device,
 }
 
 static void __rx_message(struct qmi_device *device,
-				uint8_t service_type, uint8_t client_id,
+				uint32_t service_type, uint8_t client_id,
 				const void *buf)
 {
 	const struct qmi_service_hdr *service = buf;
@@ -1935,6 +1955,43 @@  struct qmi_device_qrtr {
 	struct l_idle *shutdown_idle;
 };
 
+static int qmi_device_qrtr_write(struct qmi_device *device,
+					struct qmi_request *req)
+{
+	struct sockaddr_qrtr addr;
+	uint8_t *data;
+	uint16_t len;
+	ssize_t bytes_written;
+	int fd = l_io_get_fd(device->io);
+
+	/* Skip the QMUX header */
+	data = req->data + QMI_MUX_HDR_SIZE;
+	len = req->len - QMI_MUX_HDR_SIZE;
+
+	memset(&addr, 0, sizeof(addr));	/* Ensures internal padding is 0 */
+	addr.sq_family = AF_QIPCRTR;
+	addr.sq_node = req->info.qrtr_node;
+	addr.sq_port = req->info.qrtr_port;
+
+	bytes_written = sendto(fd, data, len, 0, (struct sockaddr *) &addr,
+							sizeof(addr));
+	if (bytes_written < 0) {
+		DBG("Failure sending data: %s", strerror(errno));
+		return -errno;
+	}
+
+	l_util_hexdump(false, data, bytes_written,
+			device->debug_func, device->debug_data);
+
+	__qrtr_debug_msg(' ', data, bytes_written,
+			req->info.service_type, device->debug_func,
+			device->debug_data);
+
+	l_queue_push_tail(device->service_queue, req);
+
+	return 0;
+}
+
 static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
 					qmi_debug_func_t function,
 					void *user_data)
@@ -1957,10 +2014,10 @@  static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
 	function(strbuf, user_data);
 }
 
-static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
-					const struct qrtr_ctrl_pkt *packet)
+static void qrtr_received_control_packet(struct qmi_device *device,
+						const void *buf, size_t len)
 {
-	struct qmi_device *device = &qrtr->super;
+	const struct qrtr_ctrl_pkt *packet = buf;
 	uint32_t cmd;
 	uint32_t type;
 	uint32_t instance;
@@ -1968,6 +2025,11 @@  static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
 	uint32_t node;
 	uint32_t port;
 
+	if (len < sizeof(*packet)) {
+		DBG("qrtr packet is too small");
+		return;
+	}
+
 	qrtr_debug_ctrl_request(packet, device->debug_func,
 				device->debug_data);
 
@@ -2013,22 +2075,33 @@  static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
 	}
 }
 
-static void qrtr_handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port,
-				const void *buf, ssize_t len)
+static void qrtr_received_service_message(struct qmi_device *device,
+						uint32_t node, uint32_t port,
+						const void *buf, size_t len)
 {
-	const struct qrtr_ctrl_pkt *packet = buf;
+	const struct l_queue_entry *entry;
+	uint32_t service_type = 0;
 
-	if (sending_port != QRTR_PORT_CTRL) {
-		DBG("Receive of service data is not implemented");
-		return;
+	for (entry = l_queue_get_entries(device->service_infos);
+				entry; entry = entry->next) {
+		struct qmi_service_info *info = entry->data;
+
+		if (info->qrtr_node == node && info->qrtr_port == port) {
+			service_type = info->service_type;
+			break;
+		}
 	}
 
-	if ((unsigned long) len < sizeof(*packet)) {
-		DBG("qrtr control packet is too small");
+	if (!service_type) {
+		DBG("Received msg from unknown service on node: %d, port: %d",
+			node, port);
 		return;
 	}
 
-	qrtr_handle_control_packet(qrtr, packet);
+	__qrtr_debug_msg(' ', buf, len, service_type,
+				device->debug_func, device->debug_data);
+
+	__rx_message(device, service_type, 0, buf);
 }
 
 static bool qrtr_received_data(struct l_io *io, void *user_data)
@@ -2042,7 +2115,7 @@  static bool qrtr_received_data(struct l_io *io, void *user_data)
 	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,
+	DBG("Received %zd bytes from Node: %d Port: %d", bytes_read,
 		addr.sq_node, addr.sq_port);
 
 	if (bytes_read < 0)
@@ -2051,7 +2124,11 @@  static bool qrtr_received_data(struct l_io *io, void *user_data)
 	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);
+	if (addr.sq_port == QRTR_PORT_CTRL)
+		qrtr_received_control_packet(&qrtr->super, buf, bytes_read);
+	else
+		qrtr_received_service_message(&qrtr->super, addr.sq_node,
+						addr.sq_port, buf, bytes_read);
 
 	return true;
 }
@@ -2152,7 +2229,7 @@  static void qmi_device_qrtr_destroy(struct qmi_device *device)
 }
 
 static const struct qmi_device_ops qrtr_ops = {
-	.write = NULL,
+	.write = qmi_device_qrtr_write,
 	.discover = qmi_device_qrtr_discover,
 	.client_create = NULL,
 	.client_release = NULL,