mbim: Add message_assembly utilities

This commit is contained in:
Denis Kenzior 2017-10-03 17:51:19 -05:00
parent 0b606acde2
commit b1a089eb0f
1 changed files with 124 additions and 0 deletions

View File

@ -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);
}