mbim: Add message_assembly utilities

1 files changed, 124 insertions, 0 deletions

diff --git a/drivers/mbimmodem/mbim.c b/drivers/mbimmodem/mbim.c
index 9419ec3c..3f0ce5eb 100644
--- a/drivers/mbimmodem/mbim.c
+++ b/drivers/mbimmodem/mbim.c
@@ -76,6 +76,126 @@ const uint8_t mbim_uuid_dss[] = {
 	0x6e, 0x0d, 0x58, 0x3c, 0x4d ,0x0e
 };
 
+struct message_assembly_node {
+	struct mbim_message_header msg_hdr;
+	struct mbim_fragment_header frag_hdr;
+	struct iovec *iov;
+	size_t n_iov;
+	size_t cur_iov;
+} __attribute((packed))__;
+
+struct message_assembly {
+	struct l_queue *transactions;
+};
+
+static bool message_assembly_node_match_tid(const void *a, const void *b)
+{
+	const struct message_assembly_node *node = a;
+	uint32_t tid = L_PTR_TO_UINT(b);
+
+	return L_LE32_TO_CPU(node->msg_hdr.tid) == tid;
+}
+
+static void message_assembly_node_free(void *data)
+{
+	struct message_assembly_node *node = data;
+	size_t i;
+
+	for (i = 0; i < node->n_iov; i++)
+		l_free(node->iov[i].iov_base);
+
+	l_free(node->iov);
+	l_free(node);
+}
+
+static struct message_assembly *message_assembly_new()
+{
+	struct message_assembly *assembly = l_new(struct message_assembly, 1);
+
+	assembly->transactions = l_queue_new();
+
+	return assembly;
+}
+
+static void message_assembly_free(struct message_assembly *assembly)
+{
+	l_queue_destroy(assembly->transactions, message_assembly_node_free);
+	l_free(assembly);
+}
+
+static struct mbim_message *message_assembly_add(
+					struct message_assembly *assembly,
+					const void *header,
+					void *frag, size_t frag_len)
+{
+	const struct mbim_message_header *msg_hdr = header;
+	const struct mbim_fragment_header *frag_hdr = header +
+					sizeof(struct mbim_message_header);
+	uint32_t tid = L_LE32_TO_CPU(msg_hdr->tid);
+	uint32_t type = L_LE32_TO_CPU(msg_hdr->type);
+	uint32_t n_frags = L_LE32_TO_CPU(frag_hdr->num_frags);
+	uint32_t cur_frag = L_LE32_TO_CPU(frag_hdr->cur_frag);
+	struct message_assembly_node *node;
+	struct mbim_message *message;
+
+	if (unlikely(type != MBIM_COMMAND_DONE))
+		return NULL;
+
+	node = l_queue_find(assembly->transactions,
+				message_assembly_node_match_tid,
+				L_UINT_TO_PTR(tid));
+
+	if (!node) {
+		if (cur_frag != 0)
+			return NULL;
+
+		if (n_frags == 1) {
+			struct iovec *iov = l_new(struct iovec, 1);
+
+			iov[0].iov_base = frag;
+			iov[0].iov_len = frag_len;
+
+			return _mbim_message_build(header, iov, 1);
+		}
+
+		node = l_new(struct message_assembly_node, 1);
+		memcpy(&node->msg_hdr, msg_hdr, sizeof(*msg_hdr));
+		memcpy(&node->frag_hdr, frag_hdr, sizeof(*frag_hdr));
+		node->iov = l_new(struct iovec, n_frags);
+		node->n_iov = n_frags;
+		node->cur_iov = cur_frag;
+		node->iov[node->cur_iov].iov_base = frag;
+		node->iov[node->cur_iov].iov_len = frag_len;
+
+		l_queue_push_head(assembly->transactions, node);
+
+		return NULL;
+	}
+
+	if (node->n_iov != n_frags)
+		return NULL;
+
+	if (node->cur_iov + 1 != cur_frag)
+		return NULL;
+
+	node->cur_iov = cur_frag;
+	node->iov[node->cur_iov].iov_base = frag;
+	node->iov[node->cur_iov].iov_len = frag_len;
+
+	if (node->cur_iov + 1 < node->n_iov)
+		return NULL;
+
+	l_queue_remove(assembly->transactions, node);
+	message = _mbim_message_build(&node->msg_hdr, node->iov, node->n_iov);
+
+	if (!message)
+		message_assembly_node_free(node);
+	else
+		l_free(node);
+
+	return message;
+}
+
 struct mbim_device {
 	int ref_count;
 	struct l_io *io;
@@ -94,6 +214,7 @@ struct mbim_device {
 	uint8_t header[HEADER_SIZE];
 	size_t header_offset;
 	size_t segment_bytes_remaining;
+	struct message_assembly *assembly;
 };
 
 static inline uint32_t _mbim_device_get_next_tid(struct mbim_device *device)
@@ -346,6 +467,8 @@ struct mbim_device *mbim_device_new(int fd, uint32_t max_segment_size)
 	l_io_set_read_handler(device->io, open_read_handler, device, NULL);
 	l_io_set_write_handler(device->io, open_write_handler, device, NULL);
 
+	device->assembly = message_assembly_new();
+
 	return mbim_device_ref(device);
 }
 
@@ -378,6 +501,7 @@ void mbim_device_unref(struct mbim_device *device)
 	if (device->disconnect_destroy)
 		device->disconnect_destroy(device->disconnect_data);
 
+	message_assembly_free(device->assembly);
 	l_free(device);
 }