ofono/drivers/mbimmodem/mbim-message.c

/*
 *
 *  oFono - Open Source Telephony
 *
 *  Copyright (C) 2017  Intel Corporation. All rights reserved.
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <stdlib.h>
#include <sys/uio.h>
#include <linux/types.h>

#include <ell/ell.h>

#include "mbim-message.h"
#include "mbim-private.h"

#define MAX_NESTING 2 /* a(uss) */
#define HEADER_SIZE (sizeof(struct mbim_message_header) + \
					sizeof(struct mbim_fragment_header))

static const char CONTAINER_TYPE_ARRAY	= 'a';
static const char CONTAINER_TYPE_STRUCT	= 'r';
static const char CONTAINER_TYPE_DATABUF = 'd';
static const char *simple_types = "syqut";

struct mbim_message {
	int ref_count;
	uint8_t header[HEADER_SIZE];
	struct iovec *frags;
	uint32_t n_frags;
	uint8_t uuid[16];
	uint32_t cid;
	union {
		uint32_t status;
		uint32_t command_type;
	};
	uint32_t info_buf_len;

	bool sealed : 1;
};

static const char *_signature_end(const char *signature)
{
	const char *ptr = signature;
	unsigned int indent = 0;
	char expect;

	switch (*signature) {
	case '(':
		expect = ')';
		break;
	case 'a':
		return _signature_end(signature + 1);
	case '0' ... '9':
		expect = 'y';
		break;
	default:
		return signature;
	}

	for (ptr = signature; *ptr != '\0'; ptr++) {
		if (*ptr == *signature)
			indent++;
		else if (*ptr == expect)
			if (!--indent)
				return ptr;
	}

	return NULL;
}

static int get_alignment(const char type)
{
	switch (type) {
	case 'y':
		return 1;
	case 'q':
		return 2;
	case 'u':
	case 's':
		return 4;
	case 't':
		return 4;
	case 'a':
		return 4;
	case 'v':
		return 4;
	default:
		return 0;
	}
}

static int get_basic_size(const char type)
{
	switch (type) {
	case 'y':
		return 1;
	case 'q':
		return 2;
	case 'u':
		return 4;
	case 't':
		return 8;
	default:
		return 0;
	}
}

static bool is_fixed_size(const char *sig_start, const char *sig_end)
{
	while (sig_start <= sig_end) {
		if (*sig_start == 'a' || *sig_start == 's' || *sig_start == 'v')
			return false;

		sig_start++;
	}

	return true;
}

static inline const void *_iter_get_data(struct mbim_message_iter *iter,
						size_t pos)
{
	pos = iter->base_offset + pos;

	while (pos >= iter->cur_iov_offset + iter->iov[iter->cur_iov].iov_len) {
		iter->cur_iov_offset += iter->iov[iter->cur_iov].iov_len;
		iter->cur_iov += 1;
	}

	return iter->iov[iter->cur_iov].iov_base + pos - iter->cur_iov_offset;
}

static bool _iter_copy_string(struct mbim_message_iter *iter,
					uint32_t offset, uint32_t len,
					char **out)
{
	uint8_t buf[len];
	uint8_t *dest = buf;
	uint32_t remaining = len;
	uint32_t iov_start = 0;
	uint32_t i = 0;
	uint32_t tocopy;

	if (!len) {
		*out = NULL;
		return true;
	}

	if (offset + len > iter->len)
		return false;

	offset += iter->base_offset;

	while (offset >= iov_start + iter->iov[i].iov_len)
		iov_start += iter->iov[i++].iov_len;

	tocopy = iter->iov[i].iov_len - (offset - iov_start);

	if (tocopy > remaining)
		tocopy = remaining;

	memcpy(dest, iter->iov[i].iov_base + offset - iov_start, tocopy);
	remaining -= tocopy;
	dest += tocopy;
	i += 1;

	while (remaining) {
		tocopy = remaining;

		if (remaining > iter->iov[i].iov_len)
			tocopy = iter->iov[i].iov_len;

		memcpy(dest, iter->iov[i].iov_base, tocopy);
		remaining -= tocopy;
		dest += tocopy;
	}

	/* Strings are in UTF16-LE, so convert to UTF16-CPU first if needed */
	if (L_CPU_TO_LE16(0x8000) != 0x8000) {
		uint16_t *le = (uint16_t *) buf;

		for (i = 0; i < len; i+= 2)
			le[i] = __builtin_bswap16(le[i]);
	}

	*out = l_utf8_from_utf16(buf, len);
	return true;
}

static inline void _iter_init_internal(struct mbim_message_iter *iter,
					char container_type,
					const char *sig_start,
					const char *sig_end,
					const struct iovec *iov, uint32_t n_iov,
					size_t len, size_t base_offset,
					size_t pos, uint32_t n_elem)
{
	size_t sig_len;

	if (sig_end)
		sig_len = sig_end - sig_start;
	else
		sig_len = strlen(sig_start);

	iter->sig_start = sig_start;
	iter->sig_len = sig_len;
	iter->sig_pos = 0;
	iter->iov = iov;
	iter->n_iov = n_iov;
	iter->cur_iov = 0;
	iter->cur_iov_offset = 0;
	iter->len = len;
	iter->base_offset = base_offset;
	iter->pos = pos;
	iter->n_elem = n_elem;
	iter->container_type = container_type;
}

static bool _iter_next_entry_basic(struct mbim_message_iter *iter,
							char type, void *out)
{
	uint8_t uint8_val;
	uint16_t uint16_val;
	uint32_t uint32_val;
	uint64_t uint64_val;
	uint32_t offset, length;
	const void *data;
	size_t pos;

	if (iter->container_type == CONTAINER_TYPE_ARRAY && !iter->n_elem)
		return false;

	if (iter->pos >= iter->len)
		return false;

	pos = align_len(iter->pos, get_alignment(type));

	switch (type) {
	case 'y':
		if (pos + 1 > iter->len)
			return false;

		data = _iter_get_data(iter, pos);
		uint8_val = l_get_u8(data);
		*(uint8_t *) out = uint8_val;
		iter->pos = pos + 1;
		break;
	case 'q':
		if (pos + 2 > iter->len)
			return false;
		data = _iter_get_data(iter, pos);
		uint16_val = l_get_le16(data);
		*(uint16_t *) out = uint16_val;
		iter->pos = pos + 2;
		break;
	case 'u':
		if (pos + 4 > iter->len)
			return false;
		data = _iter_get_data(iter, pos);
		uint32_val = l_get_le32(data);
		*(uint32_t *) out = uint32_val;
		iter->pos = pos + 4;
		break;
	case 't':
		if (pos + 8 > iter->len)
			return false;
		data = _iter_get_data(iter, pos);
		uint64_val = l_get_le64(data);
		*(uint64_t *) out = uint64_val;
		iter->pos = pos + 8;
		break;
	case 's':
		/*
		 * String consists of two uint32_t values:
		 * offset followed by length
		 */
		if (pos + 8 > iter->len)
			return false;

		data = _iter_get_data(iter, pos);
		offset = l_get_le32(data);
		data = _iter_get_data(iter, pos + 4);
		length = l_get_le32(data);

		if (!_iter_copy_string(iter, offset, length, out))
			return false;

		iter->pos = pos + 8;
		break;
	default:
		return false;
	}

	if (iter->container_type != CONTAINER_TYPE_ARRAY)
		iter->sig_pos += 1;

	return true;
}

static bool _iter_enter_array(struct mbim_message_iter *iter,
					struct mbim_message_iter *array)
{
	size_t pos;
	uint32_t n_elem;
	const char *sig_start;
	const char *sig_end;
	const void *data;
	bool fixed;
	uint32_t offset;

	if (iter->container_type == CONTAINER_TYPE_ARRAY && !iter->n_elem)
		return false;

	if (iter->sig_start[iter->sig_pos] != 'a')
		return false;

	sig_start = iter->sig_start + iter->sig_pos + 1;
	sig_end = _signature_end(sig_start) + 1;

	/*
	 * Two possibilities:
	 * 1. Element Count, followed by OL_PAIR_LIST
	 * 2. Offset, followed by element length or size for raw buffers
	 */
	fixed = is_fixed_size(sig_start, sig_end);

	if (fixed) {
		pos = align_len(iter->pos, 4);
		if (pos + 4 > iter->len)
			return false;

		data = _iter_get_data(iter, pos);
		offset = l_get_le32(data);
		iter->pos += 4;
	}

	pos = align_len(iter->pos, 4);
	if (pos + 4 > iter->len)
		return false;

	data = _iter_get_data(iter, pos);
	n_elem = l_get_le32(data);
	pos += 4;

	if (iter->container_type != CONTAINER_TYPE_ARRAY)
		iter->sig_pos += sig_end - sig_start + 1;

	if (fixed) {
		_iter_init_internal(array, CONTAINER_TYPE_ARRAY,
					sig_start, sig_end,
					iter->iov, iter->n_iov,
					iter->len, iter->base_offset,
					offset, n_elem);
		return true;
	}

	_iter_init_internal(array, CONTAINER_TYPE_ARRAY, sig_start, sig_end,
				iter->iov, iter->n_iov,
				iter->len, iter->base_offset, pos, n_elem);

	iter->pos = pos + 8 * n_elem;

	return true;
}

static bool _iter_enter_struct(struct mbim_message_iter *iter,
					struct mbim_message_iter *structure)
{
	size_t offset;
	size_t len;
	size_t pos;
	const char *sig_start;
	const char *sig_end;
	const void *data;

	if (iter->container_type == CONTAINER_TYPE_ARRAY && !iter->n_elem)
		return false;

	if (iter->sig_start[iter->sig_pos] != '(')
		return false;

	sig_start = iter->sig_start + iter->sig_pos + 1;
	sig_end = _signature_end(iter->sig_start + iter->sig_pos);

	/* TODO: support fixed size structures */
	if (is_fixed_size(sig_start, sig_end))
		return false;

	pos = align_len(iter->pos, 4);
	if (pos + 8 > iter->len)
		return false;

	data = _iter_get_data(iter, pos);
	offset = l_get_le32(data);
	pos += 4;
	data = _iter_get_data(iter, pos);
	len = l_get_le32(data);

	_iter_init_internal(structure, CONTAINER_TYPE_STRUCT,
				sig_start, sig_end, iter->iov, iter->n_iov,
				len, iter->base_offset + offset, 0, 0);

	if (iter->container_type != CONTAINER_TYPE_ARRAY)
		iter->sig_pos += sig_end - sig_start + 2;

	iter->pos = pos + 4;

	return true;
}

static bool _iter_enter_databuf(struct mbim_message_iter *iter,
					const char *signature,
					struct mbim_message_iter *databuf)
{
	if (iter->container_type != CONTAINER_TYPE_STRUCT)
		return false;

	_iter_init_internal(databuf, CONTAINER_TYPE_DATABUF,
				signature, NULL, iter->iov, iter->n_iov,
				iter->len - iter->pos,
				iter->base_offset + iter->pos, 0, 0);

	iter->pos = iter->len;

	return true;
}

static bool message_iter_next_entry_valist(struct mbim_message_iter *orig,
						va_list args)
{
	struct mbim_message_iter *iter = orig;
	const char *signature = orig->sig_start + orig->sig_pos;
	const char *end;
	uint32_t *out_n_elem;
	struct mbim_message_iter *sub_iter;
	struct mbim_message_iter stack[MAX_NESTING];
	unsigned int indent = 0;
	void *arg;

	while (signature < orig->sig_start + orig->sig_len) {
		if (strchr(simple_types, *signature)) {
			arg = va_arg(args, void *);
			if (!_iter_next_entry_basic(iter, *signature, arg))
				return false;

			signature += 1;
			continue;
		}

		switch (*signature) {
		case '0' ... '9':
		{
			uint32_t i;
			uint32_t n_elem;
			size_t pos;
			const void *src;

			if (iter->pos >= iter->len)
				return false;

			pos = align_len(iter->pos, 4);
			end = _signature_end(signature);
			n_elem = strtol(signature, NULL, 10);

			if (pos + n_elem > iter->len)
				return false;

			arg = va_arg(args, uint8_t *);

			for (i = 0; i + 4 < n_elem; i += 4) {
				src = _iter_get_data(iter, pos + i);
				memcpy(arg + i, src, 4);
			}

			src = _iter_get_data(iter, pos + i);
			memcpy(arg + i, src, n_elem - i);
			iter->pos = pos + n_elem;
			signature = end + 1;
			break;
		}
		case '(':
			signature += 1;
			indent += 1;

			if (unlikely(indent > MAX_NESTING))
				return false;

			if (!_iter_enter_struct(iter, &stack[indent - 1]))
				return false;

			iter = &stack[indent - 1];

			break;
		case ')':
			if (unlikely(indent == 0))
				return false;

			signature += 1;
			indent -= 1;

			if (indent == 0)
				iter = orig;
			else
				iter = &stack[indent - 1];
			break;
		case 'a':
			out_n_elem = va_arg(args, uint32_t *);
			sub_iter = va_arg(args, void *);

			if (!_iter_enter_array(iter, sub_iter))
				return false;

			*out_n_elem = sub_iter->n_elem;

			end = _signature_end(signature + 1);
			signature = end + 1;
			break;
		case 'd':
		{
			const char *s = va_arg(args, const char *);
			sub_iter = va_arg(args, void *);

			if (!_iter_enter_databuf(iter, s, sub_iter))
				return false;

			signature += 1;
			break;
		}

		default:
			return false;
		}
	}

	if (iter->container_type == CONTAINER_TYPE_ARRAY)
		iter->n_elem -= 1;

	return true;
}

bool mbim_message_iter_next_entry(struct mbim_message_iter *iter, ...)
{
	va_list args;
	bool result;

	if (unlikely(!iter))
		return false;

	va_start(args, iter);
	result = message_iter_next_entry_valist(iter, args);
	va_end(args);

	return result;
}

uint32_t _mbim_information_buffer_offset(uint32_t type)
{
	switch (type) {
	case MBIM_COMMAND_MSG:
	case MBIM_COMMAND_DONE:
		return 28;
	case MBIM_INDICATE_STATUS_MSG:
		return 24;
	}

	return 0;
}

static struct mbim_message *_mbim_message_new_common(uint32_t type,
							const uint8_t *uuid,
							uint32_t cid)
{
	struct mbim_message *msg;
	struct mbim_message_header *hdr;
	struct mbim_fragment_header *frag;

	msg = l_new(struct mbim_message, 1);
	hdr = (struct mbim_message_header *) msg->header;
	hdr->type = L_CPU_TO_LE32(type);

	frag = (struct mbim_fragment_header *) (msg->header + sizeof(*hdr));
	frag->num_frags = L_CPU_TO_LE32(1);
	frag->cur_frag = L_CPU_TO_LE32(0);

	memcpy(msg->uuid, uuid, 16);
	msg->cid = cid;

	return mbim_message_ref(msg);
}

struct mbim_message *_mbim_message_new_command_done(const uint8_t *uuid,
					uint32_t cid, uint32_t status)
{
	struct mbim_message *message =
		_mbim_message_new_common(MBIM_COMMAND_DONE, uuid, cid);

	if (!message)
		return NULL;

	message->status = status;

	return message;
}

void _mbim_message_set_tid(struct mbim_message *message, uint32_t tid)
{
	struct mbim_message_header *hdr =
				(struct mbim_message_header *) message->header;

	hdr->tid = L_CPU_TO_LE32(tid);
}

void *_mbim_message_to_bytearray(struct mbim_message *message, size_t *out_len)
{
	unsigned int i;
	struct mbim_message_header *hdr;
	void *binary;
	size_t pos;
	size_t len;

	if (!message->sealed)
		return NULL;

	hdr = (struct mbim_message_header *) message->header;
	len = L_LE32_TO_CPU(hdr->len);
	binary = l_malloc(len);

	memcpy(binary, message->header, HEADER_SIZE);
	pos = HEADER_SIZE;

	for (i = 0; i < message->n_frags; i++) {
		memcpy(binary + pos, message->frags[i].iov_base,
				message->frags[i].iov_len);
		pos += message->frags[i].iov_len;
	}

	if (out_len)
		*out_len = len;

	return binary;
}

struct mbim_message *mbim_message_new(const uint8_t *uuid, uint32_t cid,
					enum mbim_command_type type)
{
	struct mbim_message *message =
		_mbim_message_new_common(MBIM_COMMAND_MSG, uuid, cid);

	if (!message)
		return NULL;

	message->command_type = type;

	return message;
}

struct mbim_message *mbim_message_ref(struct mbim_message *msg)
{
	if (unlikely(!msg))
		return NULL;

	__sync_fetch_and_add(&msg->ref_count, 1);

	return msg;
}

void mbim_message_unref(struct mbim_message *msg)
{
	unsigned int i;

	if (unlikely(!msg))
		return;

	if (__sync_sub_and_fetch(&msg->ref_count, 1))
		return;

	for (i = 0; i < msg->n_frags; i++)
		l_free(msg->frags[i].iov_base);

	l_free(msg->frags);
	l_free(msg);
}

struct mbim_message *_mbim_message_build(const void *header,
						struct iovec *frags,
						uint32_t n_frags)
{
	struct mbim_message *msg;
	struct mbim_message_header *hdr = (struct mbim_message_header *) header;
	struct mbim_message_iter iter;
	bool r = false;

	msg = l_new(struct mbim_message, 1);

	msg->ref_count = 1;
	memcpy(msg->header, header, HEADER_SIZE);
	msg->frags = frags;
	msg->n_frags = n_frags;
	msg->sealed = true;

	switch (L_LE32_TO_CPU(hdr->type)) {
	case MBIM_COMMAND_DONE:
		_iter_init_internal(&iter, CONTAINER_TYPE_STRUCT,
						"16yuuu", NULL,
						frags, n_frags,
						frags[0].iov_len, 0, 0, 0);
		r = mbim_message_iter_next_entry(&iter, msg->uuid, &msg->cid,
						&msg->status,
						&msg->info_buf_len);
		break;
	case MBIM_COMMAND_MSG:
		_iter_init_internal(&iter, CONTAINER_TYPE_STRUCT,
						"16yuuu", NULL,
						frags, n_frags,
						frags[0].iov_len, 0, 0, 0);
		r = mbim_message_iter_next_entry(&iter, msg->uuid, &msg->cid,
						&msg->command_type,
						&msg->info_buf_len);
		break;
	case MBIM_INDICATE_STATUS_MSG:
		_iter_init_internal(&iter, CONTAINER_TYPE_STRUCT,
						"16yuu", NULL,
						frags, n_frags,
						frags[0].iov_len, 0, 0, 0);
		r = mbim_message_iter_next_entry(&iter, msg->uuid, &msg->cid,
						&msg->info_buf_len);
		break;
	default:
		break;
	}

	if (!r) {
		l_free(msg);
		msg = NULL;
	}

	return msg;
}

uint32_t mbim_message_get_error(struct mbim_message *message)
{
	struct mbim_message_header *hdr;

	if (unlikely(!message))
		return false;

	if (unlikely(!message->sealed))
		return false;

	hdr = (struct mbim_message_header *) message->header;

	if (L_LE32_TO_CPU(hdr->type) != MBIM_COMMAND_DONE)
		return 0;

	return message->status;
}

uint32_t mbim_message_get_cid(struct mbim_message *message)
{
	if (unlikely(!message))
		return false;

	return message->cid;
}

const uint8_t *mbim_message_get_uuid(struct mbim_message *message)
{
	if (unlikely(!message))
		return false;

	return message->uuid;
}

bool mbim_message_get_arguments(struct mbim_message *message,
						const char *signature, ...)
{
	struct mbim_message_iter iter;
	va_list args;
	bool result;
	struct mbim_message_header *hdr;
	uint32_t type;
	size_t begin;

	if (unlikely(!message))
		return false;

	if (unlikely(!message->sealed))
		return false;

	hdr = (struct mbim_message_header *) message->header;
	type = L_LE32_TO_CPU(hdr->type);
	begin = _mbim_information_buffer_offset(type);

	_iter_init_internal(&iter, CONTAINER_TYPE_STRUCT,
				signature, NULL,
				message->frags, message->n_frags,
				message->info_buf_len, begin, 0, 0);

	va_start(args, signature);
	result = message_iter_next_entry_valist(&iter, args);
	va_end(args);

	return result;
}

static bool _mbim_message_get_data(struct mbim_message *message,
					uint32_t offset,
					void *dest, size_t len)
{
	struct mbim_message_iter iter;
	struct mbim_message_header *hdr;
	uint32_t type;
	size_t begin;
	const void *src;
	size_t pos;
	uint32_t i;

	if (unlikely(!message))
		return false;

	if (unlikely(!message->sealed))
		return false;

	hdr = (struct mbim_message_header *) message->header;
	type = L_LE32_TO_CPU(hdr->type);
	begin = _mbim_information_buffer_offset(type);

	_iter_init_internal(&iter, CONTAINER_TYPE_STRUCT,
				"", NULL,
				message->frags, message->n_frags,
				message->info_buf_len, begin, offset, 0);

	pos = align_len(iter.pos, 4);
	if (pos + len > iter.len)
		return false;

	for (i = 0; i + 4 < len; i += 4) {
		src = _iter_get_data(&iter, pos + i);
		memcpy(dest + i, src, 4);
	}

	src = _iter_get_data(&iter, pos + i);
	memcpy(dest + i, src, len - i);

	return true;
}

bool mbim_message_get_ipv4_address(struct mbim_message *message,
					uint32_t offset,
					struct in_addr *addr)
{
	return _mbim_message_get_data(message, offset, &addr->s_addr, 4);
}

bool mbim_message_get_ipv4_element(struct mbim_message *message,
					uint32_t offset,
					uint32_t *prefix_len,
					struct in_addr *addr)
{
	uint8_t buf[8];

	if (!_mbim_message_get_data(message, offset, buf, 8))
		return false;

	*prefix_len = l_get_le32(buf);
	memcpy(&addr->s_addr, buf + 4, 4);

	return true;
}

bool mbim_message_get_ipv6_address(struct mbim_message *message,
					uint32_t offset,
					struct in6_addr *addr)
{
	return _mbim_message_get_data(message, offset, addr->s6_addr, 16);
}

bool mbim_message_get_ipv6_element(struct mbim_message *message,
					uint32_t offset,
					uint32_t *prefix_len,
					struct in6_addr *addr)
{
	uint8_t buf[20];

	if (!_mbim_message_get_data(message, offset, buf, 20))
		return false;

	*prefix_len = l_get_le32(buf);
	memcpy(&addr->s6_addr, buf + 4, 16);

	return true;
}

struct container {
	void *sbuf;		/* static buffer */
	size_t sbuf_size;
	size_t sbuf_pos;
	void *dbuf;		/* data buffer */
	size_t dbuf_size;
	size_t dbuf_pos;
	void *obuf;		/* offset buffer */
	size_t obuf_size;
	size_t obuf_pos;
	char container_type;
	char signature[64];
	uint8_t sigindex;
	uint32_t base_offset;
	uint32_t array_start;
};

static void container_update_offsets(struct container *container)
{
	size_t i;

	if (!container->obuf)
		return;

	for (i = 0; i < container->obuf_pos; i += 4) {
		uint32_t sbuf_offset = l_get_u32(container->obuf + i);
		uint32_t dbuf_offset = l_get_u32(container->sbuf + sbuf_offset);

		dbuf_offset += container->sbuf_pos - container->base_offset;
		l_put_le32(dbuf_offset, container->sbuf + sbuf_offset);
	}

	l_free(container->obuf);
	container->obuf = NULL;
	container->obuf_pos = 0;
	container->obuf_size = 0;
}

struct mbim_message_builder {
	struct mbim_message *message;
	struct container stack[MAX_NESTING + 1];
	uint32_t index;
};

static inline size_t grow_buf(void **buf, size_t *buf_size, size_t *pos,
				size_t len, unsigned int alignment)
{
	size_t size = align_len(*pos, alignment);

	if (size + len > *buf_size) {
		*buf = l_realloc(*buf, size + len);
		*buf_size = size + len;
	}

	if (size - *pos > 0)
		memset(*buf + *pos, 0, size - *pos);

	*pos = size + len;
	return size;
}

#define GROW_SBUF(c, len, alignment) \
	grow_buf(&c->sbuf, &c->sbuf_size, &c->sbuf_pos, \
			len, alignment)

#define GROW_DBUF(c, len, alignment) \
	grow_buf(&c->dbuf, &c->dbuf_size, &c->dbuf_pos, \
			len, alignment)

#define GROW_OBUF(c) \
	grow_buf(&c->obuf, &c->obuf_size, &c->obuf_pos, 4, 4)

static void add_offset_and_length(struct container *container,
					uint32_t offset, uint32_t len)
{
	size_t start;
	/*
	 * note the relative offset in the data buffer.  Store it in native
	 * endian order for now.  It will be fixed up later once we finalize
	 * the structure
	 */
	start = GROW_SBUF(container, 8, 4);
	l_put_u32(offset, container->sbuf + start);
	l_put_le32(len, container->sbuf + start + 4);

	/* Make a note in offset buffer to update the offset at this position */
	offset = start;
	start = GROW_OBUF(container);
	l_put_u32(offset, container->obuf + start);
}

struct mbim_message_builder *mbim_message_builder_new(struct mbim_message *msg)
{
	struct mbim_message_builder *ret;
	struct mbim_message_header *hdr;
	uint32_t type;
	struct container *container;

	if (unlikely(!msg))
		return NULL;

	if (msg->sealed)
		return NULL;

	hdr = (struct mbim_message_header *) msg->header;
	type = L_LE32_TO_CPU(hdr->type);

	ret = l_new(struct mbim_message_builder, 1);
	ret->message = mbim_message_ref(msg);

	/* Reserve space in the static buffer for UUID, CID, Status, etc */
	container = &ret->stack[ret->index];
	container->base_offset = _mbim_information_buffer_offset(type);
	container->container_type = CONTAINER_TYPE_STRUCT;
	GROW_SBUF(container, container->base_offset, 0);

	return ret;
}

void mbim_message_builder_free(struct mbim_message_builder *builder)
{
	uint32_t i;

	if (unlikely(!builder))
		return;

	mbim_message_unref(builder->message);

	for (i = 0; i <= builder->index; i++) {
		if (builder->stack[i].container_type == CONTAINER_TYPE_ARRAY)
			continue;

		l_free(builder->stack[i].sbuf);
		l_free(builder->stack[i].dbuf);
		l_free(builder->stack[i].obuf);
	}

	l_free(builder);
}

bool mbim_message_builder_append_basic(struct mbim_message_builder *builder,
					char type, const void *value)
{
	struct container *container = &builder->stack[builder->index];
	struct container *array = NULL;
	size_t start;
	unsigned int alignment;
	size_t len;
	uint16_t *utf16;

	if (unlikely(!builder))
		return false;

	if (unlikely(!strchr(simple_types, type)))
		return false;

	alignment = get_alignment(type);
	if (!alignment)
		return false;

	if (builder->index > 0 &&
			container->signature[container->sigindex] != type)
		return false;

	len = get_basic_size(type);

	if (container->container_type == CONTAINER_TYPE_ARRAY) {
		array = container;
		container = &builder->stack[builder->index - 1];
	}

	if (len) {
		uint16_t swapped_u16;
		uint32_t swapped_u32;
		uint64_t swapped_u64;

		switch (len) {
		case 2:
			swapped_u16 = L_CPU_TO_LE16(l_get_u16(value));
			value = &swapped_u16;
			break;
		case 4:
			swapped_u32 = L_CPU_TO_LE32(l_get_u32(value));
			value = &swapped_u32;
			break;
		case 8:
			swapped_u64 = L_CPU_TO_LE64(l_get_u64(value));
			value = &swapped_u64;
			break;
		}

		if (array) {
			uint32_t n_elem = l_get_le32(container->sbuf +
					array->array_start + 4);
			start = GROW_DBUF(container, len, alignment);
			memcpy(container->dbuf + start, value, len);
			l_put_le32(n_elem + 1,
				container->sbuf + array->array_start + 4);
		} else {
			start = GROW_SBUF(container, len, alignment);
			memcpy(container->sbuf + start, value, len);
		}

		goto done;
	}

	/* Null string? */
	if (!value) {
		start = GROW_SBUF(container, 8, 4);
		l_put_le32(0, container->sbuf + start);
		l_put_le32(0, container->sbuf + start + 4);
		goto done;
	}

	utf16 = l_utf8_to_utf16(value, &len);
	if (!utf16)
		return false;

	/* Strings are in UTF16-LE, so convert if needed */
	if (L_CPU_TO_LE16(0x8000) != 0x8000) {
		size_t i;

		for (i = 0; i < len - 2; i += 2)
			utf16[i] = __builtin_bswap16(utf16[i]);
	}

	/*
	 * First grow the data buffer.
	 * MBIM v1.0-errata1, Section 10.3:
	 * "If the size of the payload in the variable field is not a multiple
	 * of 4 bytes, the field shall be padded up to the next 4 byte multiple.
	 * This shall be true even for the last payload in DataBuffer."
	 */
	start = GROW_DBUF(container, len - 2, 4);
	memcpy(container->dbuf + start, utf16, len - 2);
	l_free(utf16);

	add_offset_and_length(container, start, len - 2);

	if (array) {
		uint32_t n_elem = l_get_le32(container->sbuf +
						array->array_start);
		l_put_le32(n_elem + 1,
				container->sbuf + array->array_start);
	}
done:
	if (!array)
		container->sigindex += 1;

	return true;
}

bool mbim_message_builder_append_bytes(struct mbim_message_builder *builder,
					size_t len, const uint8_t *bytes)
{
	struct container *container = &builder->stack[builder->index];
	size_t start;

	if (unlikely(!builder))
		return false;

	if (container->container_type == CONTAINER_TYPE_ARRAY) {
		struct container *array;

		if (unlikely(container->sigindex != 0))
			return false;

		if (unlikely(container->signature[container->sigindex] != 'y'))
			return false;

		array = container;
		container = &builder->stack[builder->index - 1];

		start = GROW_DBUF(container, len, 1);
		memcpy(container->dbuf + start, bytes, len);
		l_put_le32(len, container->sbuf + array->array_start + 4);

		return true;
	} else if (container->container_type == CONTAINER_TYPE_STRUCT) {
		if (builder->index > 0) {
			unsigned int i = container->sigindex;
			const char *sig = container->signature + i;
			size_t n_elem;
			const char *sigend;

			if (*sig < '0' || *sig > '9')
				return false;

			n_elem = strtol(sig, NULL, 10);
			if (n_elem != len)
				return false;

			sigend = _signature_end(sig);
			if (!sigend)
				return false;

			container->sigindex += sigend - sig + 1;
		}

		start = GROW_SBUF(container, len, 1);
		memcpy(container->sbuf + start, bytes, len);

		return true;
	}

	return false;
}

bool mbim_message_builder_enter_struct(struct mbim_message_builder *builder,
					const char *signature)
{
	struct container *container;

	if (strlen(signature) > sizeof(((struct container *) 0)->signature) - 1)
		return false;

	if (builder->index == L_ARRAY_SIZE(builder->stack) - 1)
		return false;

	builder->index += 1;

	container = &builder->stack[builder->index];
	memset(container, 0, sizeof(*container));
	strcpy(container->signature, signature);
	container->sigindex = 0;
	container->container_type = CONTAINER_TYPE_STRUCT;

	return true;
}

bool mbim_message_builder_leave_struct(struct mbim_message_builder *builder)
{
	struct container *container;
	struct container *parent;
	struct container *array = NULL;
	size_t start;

	if (unlikely(builder->index == 0))
		return false;

	container = &builder->stack[builder->index];

	if (unlikely(container->container_type != CONTAINER_TYPE_STRUCT))
		return false;

	builder->index -= 1;
	parent = &builder->stack[builder->index];
	GROW_DBUF(container, 0, 4);
	container_update_offsets(container);

	if (parent->container_type == CONTAINER_TYPE_ARRAY) {
		array = parent;
		parent = &builder->stack[builder->index - 1];
	}

	/*
	 * Copy the structure buffers into parent's buffers
	 */
	start = GROW_DBUF(parent, container->sbuf_pos + container->dbuf_pos, 4);
	memcpy(parent->dbuf + start, container->sbuf, container->sbuf_pos);
	memcpy(parent->dbuf + start + container->sbuf_pos,
				container->dbuf, container->dbuf_pos);
	l_free(container->sbuf);
	l_free(container->dbuf);

	add_offset_and_length(parent, start,
			container->sbuf_pos + container->dbuf_pos);

	if (array) {
		uint32_t n_elem = l_get_le32(parent->sbuf +
						array->array_start);
		l_put_le32(n_elem + 1,
				parent->sbuf + array->array_start);
	}

	memset(container, 0, sizeof(*container));

	return true;
}

bool mbim_message_builder_enter_array(struct mbim_message_builder *builder,
					const char *signature)
{
	struct container *parent;
	struct container *container;

	if (strlen(signature) > sizeof(((struct container *) 0)->signature) - 1)
		return false;

	if (builder->index == L_ARRAY_SIZE(builder->stack) - 1)
		return false;

	/*
	 * TODO: validate that arrays consist of a single simple type or
	 * a single struct
	 */
	parent = &builder->stack[builder->index++];
	container = &builder->stack[builder->index];

	/* Arrays add on to the parent's buffers */
	container->container_type = CONTAINER_TYPE_ARRAY;
	strcpy(container->signature, signature);
	container->sigindex = 0;

	/* First grow the body enough to cover preceding length */
	container->array_start = GROW_SBUF(parent, 4, 4);
	l_put_le32(0, parent->sbuf + container->array_start);

	/* For arrays of fixed-size elements, it is offset followed by length */
	if (is_fixed_size(container->signature,
				_signature_end(container->signature))) {
		/* Note down offset into the data buffer */
		size_t start = GROW_DBUF(parent, 0, 4);
		l_put_u32(start, parent->sbuf + container->array_start);
		/* Set length to 0 */
		start = GROW_SBUF(parent, 4, 4);
		l_put_le32(0, parent->sbuf + start);
		/* Note down offset position to recalculate */
		start = GROW_OBUF(parent);
		l_put_u32(container->array_start, parent->obuf + start);
	}

	return true;
}

bool mbim_message_builder_leave_array(struct mbim_message_builder *builder)
{
	struct container *container;

	if (unlikely(builder->index == 0))
		return false;

	container = &builder->stack[builder->index];

	if (unlikely(container->container_type != CONTAINER_TYPE_ARRAY))
		return false;

	builder->index -= 1;
	memset(container, 0, sizeof(*container));

	return true;
}

bool mbim_message_builder_enter_databuf(struct mbim_message_builder *builder,
					const char *signature)
{
	struct container *container;

	if (strlen(signature) > sizeof(((struct container *) 0)->signature) - 1)
		return false;

	if (builder->index != 0)
		return false;

	builder->index += 1;

	container = &builder->stack[builder->index];
	memset(container, 0, sizeof(*container));
	strcpy(container->signature, signature);
	container->sigindex = 0;
	container->container_type = CONTAINER_TYPE_DATABUF;

	return true;
}

bool mbim_message_builder_leave_databuf(struct mbim_message_builder *builder)
{
	struct container *container;
	struct container *parent;
	size_t start;

	if (unlikely(builder->index == 0))
		return false;

	container = &builder->stack[builder->index];

	if (unlikely(container->container_type != CONTAINER_TYPE_DATABUF))
		return false;

	builder->index -= 1;
	parent = &builder->stack[builder->index];
	GROW_DBUF(container, 0, 4);
	container_update_offsets(container);

	/*
	 * Copy the structure buffers into parent's buffers
	 */
	start = GROW_SBUF(parent, container->sbuf_pos + container->dbuf_pos, 4);
	memcpy(parent->sbuf + start, container->sbuf, container->sbuf_pos);
	memcpy(parent->sbuf + start + container->sbuf_pos,
				container->dbuf, container->dbuf_pos);
	l_free(container->sbuf);
	l_free(container->dbuf);

	memset(container, 0, sizeof(*container));

	return true;
}

struct mbim_message *mbim_message_builder_finalize(
					struct mbim_message_builder *builder)
{
	struct container *root;
	struct mbim_message_header *hdr;

	if (unlikely(!builder))
		return NULL;

	if (builder->index != 0)
		return NULL;

	hdr = (struct mbim_message_header *) builder->message->header;

	root = &builder->stack[0];
	GROW_DBUF(root, 0, 4);
	container_update_offsets(root);

	memcpy(root->sbuf, builder->message->uuid, 16);
	l_put_le32(builder->message->cid, root->sbuf + 16);

	switch (L_LE32_TO_CPU(hdr->type)) {
	case MBIM_COMMAND_DONE:
		l_put_le32(builder->message->status, root->sbuf + 20);
		break;
	case MBIM_COMMAND_MSG:
		l_put_le32(builder->message->command_type, root->sbuf + 20);
		break;
	default:
		break;
	}

	builder->message->info_buf_len = root->dbuf_pos + root->sbuf_pos -
						root->base_offset;
	l_put_le32(builder->message->info_buf_len,
					root->sbuf + root->base_offset - 4);

	builder->message->n_frags = 2;
	builder->message->frags = l_new(struct iovec, 2);
	builder->message->frags[0].iov_base = root->sbuf;
	builder->message->frags[0].iov_len = root->sbuf_pos;
	builder->message->frags[1].iov_base = root->dbuf;
	builder->message->frags[1].iov_len = root->dbuf_pos;

	root->sbuf = NULL;
	root->dbuf = NULL;

	hdr->len = L_CPU_TO_LE32(HEADER_SIZE + root->dbuf_pos + root->sbuf_pos);

	builder->message->sealed = true;

	return builder->message;
}

static bool append_arguments(struct mbim_message *message,
					const char *signature, va_list args)
{
	struct mbim_message_builder *builder;
	char subsig[64];
	const char *sigend;
	struct {
		char type;
		const char *sig_start;
		const char *sig_end;
		unsigned int n_items;
	} stack[MAX_NESTING + 1];
	unsigned int stack_index = 0;

	if (strlen(signature) > sizeof(subsig) - 1)
		return false;

	builder = mbim_message_builder_new(message);

	stack[stack_index].type = CONTAINER_TYPE_STRUCT;
	stack[stack_index].sig_start = signature;
	stack[stack_index].sig_end = signature + strlen(signature);
	stack[stack_index].n_items = 0;

	while (stack_index != 0 || stack[0].sig_start != stack[0].sig_end) {
		const char *s;
		const char *str;

		if (stack[stack_index].type == CONTAINER_TYPE_ARRAY &&
				stack[stack_index].n_items == 0)
			stack[stack_index].sig_start =
				stack[stack_index].sig_end;

		if (stack[stack_index].sig_start ==
				stack[stack_index].sig_end) {
			bool r = false;

			if (stack_index == 0)
				goto error;

			if (stack[stack_index].type == CONTAINER_TYPE_ARRAY)
				r = mbim_message_builder_leave_array(builder);
			if (stack[stack_index].type == CONTAINER_TYPE_STRUCT)
				r = mbim_message_builder_leave_struct(builder);
			if (stack[stack_index].type == CONTAINER_TYPE_DATABUF)
				r = mbim_message_builder_leave_databuf(builder);

			if (!r)
				goto error;

			stack_index -= 1;
			continue;
		}

		s = stack[stack_index].sig_start;

		if (stack[stack_index].type != CONTAINER_TYPE_ARRAY)
			stack[stack_index].sig_start += 1;
		else
			stack[stack_index].n_items -= 1;

		switch (*s) {
		case '0' ... '9':
		{
			uint32_t n_elem = strtol(s, NULL, 10);
			const uint8_t *arg = va_arg(args, const uint8_t *);

			sigend = _signature_end(s);
			if (!sigend)
				goto error;

			if (!mbim_message_builder_append_bytes(builder,
								n_elem, arg))
				goto error;

			stack[stack_index].sig_start = sigend + 1;
			break;
		}
		case 's':
			str = va_arg(args, const char *);

			if (!mbim_message_builder_append_basic(builder,
								*s, str))
				goto error;
			break;
		case 'y':
		{
			uint8_t y = (uint8_t) va_arg(args, int);

			if (!mbim_message_builder_append_basic(builder, *s, &y))
				goto error;

			break;
		}
		case 'q':
		{
			uint16_t n = (uint16_t) va_arg(args, int);

			if (!mbim_message_builder_append_basic(builder, *s, &n))
				goto error;

			break;
		}
		case 'u':
		{
			uint32_t u = va_arg(args, uint32_t);

			if (!mbim_message_builder_append_basic(builder, *s, &u))
				goto error;

			break;
		}
		case 't':
		{
			uint64_t u = va_arg(args, uint64_t);

			if (!mbim_message_builder_append_basic(builder, *s, &u))
				goto error;

			break;
		}
		case 'v': /* Structure with variable signature */
		{
			if (stack_index == MAX_NESTING)
				goto error;

			str = va_arg(args, const char *);
			if (!str)
				goto error;

			if (!mbim_message_builder_enter_struct(builder, str))
				goto error;

			stack_index += 1;
			stack[stack_index].sig_start = str;
			stack[stack_index].sig_end = str + strlen(str);
			stack[stack_index].n_items = 0;
			stack[stack_index].type = CONTAINER_TYPE_STRUCT;

			break;
		}
		case 'd':
		{
			if (stack_index == MAX_NESTING)
				goto error;

			str = va_arg(args, const char *);
			if (!str)
				goto error;

			if (!mbim_message_builder_enter_databuf(builder, str))
				goto error;

			stack_index += 1;
			stack[stack_index].sig_start = str;
			stack[stack_index].sig_end = str + strlen(str);
			stack[stack_index].n_items = 0;
			stack[stack_index].type = CONTAINER_TYPE_DATABUF;

			break;
		}
		case '(':
			if (stack_index == MAX_NESTING)
				goto error;

			sigend = _signature_end(s);
			memcpy(subsig, s + 1, sigend - s - 1);
			subsig[sigend - s - 1] = '\0';

			if (!mbim_message_builder_enter_struct(builder, subsig))
				goto error;

			if (stack[stack_index].type !=
					CONTAINER_TYPE_ARRAY)
				stack[stack_index].sig_start = sigend + 1;

			stack_index += 1;
			stack[stack_index].sig_start = s + 1;
			stack[stack_index].sig_end = sigend;
			stack[stack_index].n_items = 0;
			stack[stack_index].type = CONTAINER_TYPE_STRUCT;

			break;
		case 'a':
			if (stack_index == MAX_NESTING)
				goto error;

			sigend = _signature_end(s + 1) + 1;
			memcpy(subsig, s + 1, sigend - s - 1);
			subsig[sigend - s - 1] = '\0';

			if (!mbim_message_builder_enter_array(builder, subsig))
				goto error;

			if (stack[stack_index].type != CONTAINER_TYPE_ARRAY)
				stack[stack_index].sig_start = sigend;

			stack_index += 1;
			stack[stack_index].sig_start = s + 1;
			stack[stack_index].sig_end = sigend;
			stack[stack_index].n_items = va_arg(args, unsigned int);
			stack[stack_index].type = CONTAINER_TYPE_ARRAY;

			/* Special case of byte arrays, just copy the data */
			if (!strcmp(subsig, "y")) {
				const uint8_t *bytes =
						va_arg(args, const uint8_t *);

				if (!mbim_message_builder_append_bytes(builder,
						stack[stack_index].n_items,
						bytes))
					goto error;

				stack[stack_index].n_items = 0;
			}

			break;
		default:
			goto error;
		}
	}

	mbim_message_builder_finalize(builder);
	mbim_message_builder_free(builder);

	return true;

error:
	mbim_message_builder_free(builder);
	return false;
}

bool mbim_message_set_arguments(struct mbim_message *message,
						const char *signature, ...)
{
	va_list args;
	bool result;

	if (unlikely(!message))
		return false;

	if (unlikely(message->sealed))
		return false;

	if (!signature)
		return true;

	va_start(args, signature);
	result = append_arguments(message, signature, args);
	va_end(args);

	return result;
}

void *_mbim_message_get_header(struct mbim_message *message, size_t *out_len)
{
	if (out_len)
		*out_len = HEADER_SIZE;

	return message->header;
}

struct iovec *_mbim_message_get_body(struct mbim_message *message,
					size_t *out_n_iov, size_t *out_len)
{
	if (out_len)
		*out_len = message->info_buf_len;

	if (out_n_iov)
		*out_n_iov = message->info_buf_len ? message->n_frags :
							message->n_frags - 1;

	return message->frags;
}