blob: 0c117658ca0f2faddbde138734d880cd2d9a2b74 [file] [log] [blame]
/*
*
* 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)
{
uint16_t buf[len / 2 + 1];
uint8_t *dest = (uint8_t *) 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 = buf;
for (i = 0; i < len / 2; i++)
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++)
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;
}