diff options
| -rw-r--r-- | src/Makefile.am | 2 | ||||
| -rw-r--r-- | src/qmi-errors.h | 6 | ||||
| -rw-r--r-- | src/qmi-message.c | 639 | ||||
| -rw-r--r-- | src/qmi-message.h | 91 | ||||
| -rw-r--r-- | src/qmimsg.c | 480 | ||||
| -rw-r--r-- | src/qmimsg.h | 115 |
6 files changed, 738 insertions, 595 deletions
diff --git a/src/Makefile.am b/src/Makefile.am index 61add98..fda1aa1 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -38,12 +38,14 @@ qmi-enum-types.c: qmi-enums.h qmi-enum-types.h $(top_srcdir)/build-aux/qmi-enum- # Additional dependencies qmi-device.c: qmi-error-types.h +qmi-message.c: qmi-error-types.h libqmi_glib_la_SOURCES = \ libqmi-glib.h \ qmi-errors.h qmi-error-types.h qmi-error-types.c \ qmi-enums.h qmi-enum-types.h qmi-enum-types.c \ qmi-utils.h qmi-utils.c \ + qmi-message.h qmi-message.c \ qmi-device.h qmi-device.c libqmi_glib_la_LIBADD = \ diff --git a/src/qmi-errors.h b/src/qmi-errors.h index bba8411..8d546c8 100644 --- a/src/qmi-errors.h +++ b/src/qmi-errors.h @@ -28,6 +28,9 @@ * @QMI_CORE_ERROR_FAILED: Operation failed. * @QMI_CORE_ERROR_WRONG_STATE: Operation cannot be executed in the current state. * @QMI_CORE_ERROR_INVALID_ARGS: Invalid arguments given. + * @QMI_CORE_ERROR_INVALID_MESSAGE: QMI message is invalid. + * @QMI_CORE_ERROR_TLV_NOT_FOUND: TLV not found. + * @QMI_CORE_ERROR_TLV_TOO_LONG: TLV is too long. * * Common errors that may be reported by libqmi-glib. */ @@ -35,6 +38,9 @@ typedef enum { QMI_CORE_ERROR_FAILED, QMI_CORE_ERROR_WRONG_STATE, QMI_CORE_ERROR_INVALID_ARGS, + QMI_CORE_ERROR_INVALID_MESSAGE, + QMI_CORE_ERROR_TLV_NOT_FOUND, + QMI_CORE_ERROR_TLV_TOO_LONG, } QmiCoreError; #endif /* _LIBQMI_GLIB_QMI_ERRORS_H_ */ diff --git a/src/qmi-message.c b/src/qmi-message.c new file mode 100644 index 0000000..33a7532 --- /dev/null +++ b/src/qmi-message.c @@ -0,0 +1,639 @@ +/* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ + +/* + * Copyright (c) 2012 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file of the libqmi library. + */ + +/* + * libqmi-glib -- GLib/GIO based library to control QMI devices + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + * + * Copyright (C) 2012 Aleksander Morgado <aleksander@lanedo.com> + */ + +#include <glib.h> +#include <stdint.h> +#include <stdio.h> +#include <string.h> +#include <endian.h> + +#include "qmi-message.h" +#include "qmi-utils.h" +#include "qmi-error-types.h" + +#define PACKED __attribute__((packed)) + +struct qmux { + uint16_t length; + uint8_t flags; + uint8_t service; + uint8_t client; +} PACKED; + +struct control_header { + uint8_t flags; + uint8_t transaction; + uint16_t message; + uint16_t tlv_length; +} PACKED; + +struct service_header { + uint8_t flags; + uint16_t transaction; + uint16_t message; + uint16_t tlv_length; +} PACKED; + +struct tlv { + uint8_t type; + uint16_t length; + char value[]; +} PACKED; + +struct control_message { + struct control_header header; + struct tlv tlv[]; +} PACKED; + +struct service_message { + struct service_header header; + struct tlv tlv[]; +} PACKED; + +struct full_message { + uint8_t marker; + struct qmux qmux; + union { + struct control_message control; + struct service_message service; + } qmi; +} PACKED; + +struct _QmiMessage { + /* TODO: avoid memory split here */ + struct full_message *buf; /* buf allocated using g_malloc, not g_slice_alloc */ + gsize len; /* cached size of *buf; not part of message. */ + volatile gint ref_count; /* the ref count */ +}; + +static inline uint16_t +qmux_length (QmiMessage *self) +{ + return le16toh (self->buf->qmux.length); +} + +static inline void +set_qmux_length (QmiMessage *self, + uint16_t length) +{ + self->buf->qmux.length = htole16 (length); +} + +gboolean +qmi_message_is_control (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, FALSE); + + return self->buf->qmux.service == QMI_SERVICE_CTL; +} + +guint8 +qmi_message_get_qmux_flags (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, 0); + + return self->buf->qmux.flags; +} + +QmiService +qmi_message_get_service (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, QMI_SERVICE_UNKNOWN); + + return (QmiService)self->buf->qmux.service; +} + +guint8 +qmi_message_get_client_id (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, 0); + + return self->buf->qmux.client; +} + +guint8 +qmi_message_get_qmi_flags (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, 0); + + if (qmi_message_is_control (self)) + return self->buf->qmi.control.header.flags; + + return self->buf->qmi.service.header.flags; +} + +guint16 +qmi_message_get_transaction_id (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, 0); + + if (qmi_message_is_control (self)) + return (guint16)self->buf->qmi.control.header.transaction; + + return le16toh (self->buf->qmi.service.header.transaction); +} + +guint16 +qmi_message_get_message_id (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, 0); + + if (qmi_message_is_control (self)) + return le16toh (self->buf->qmi.control.header.message); + + return le16toh (self->buf->qmi.service.header.message); +} + +gsize +qmi_message_get_length (QmiMessage *self) +{ + g_return_val_if_fail (self != NULL, 0); + + return self->len; +} + +static uint16_t +qmi_tlv_length (QmiMessage *self) +{ + if (qmi_message_is_control (self)) + return le16toh (self->buf->qmi.control.header.tlv_length); + + return le16toh (self->buf->qmi.service.header.tlv_length); +} + +static void +set_qmi_tlv_length (QmiMessage *self, + uint16_t length) +{ + if (qmi_message_is_control (self)) + self->buf->qmi.control.header.tlv_length = htole16 (length); + else + self->buf->qmi.service.header.tlv_length = htole16 (length); +} + +static struct tlv * +qmi_tlv (QmiMessage *self) +{ + if (qmi_message_is_control (self)) + return self->buf->qmi.control.tlv; + + return self->buf->qmi.service.tlv; +} + +static char * +qmi_end (QmiMessage *self) +{ + return (char *) self->buf + self->len; +} + +static struct tlv * +tlv_next (struct tlv *tlv) +{ + return (struct tlv *)((char *)tlv + sizeof(struct tlv) + tlv->length); +} + +static struct tlv * +qmi_tlv_first (QmiMessage *self) +{ + if (qmi_tlv_length (self)) + return qmi_tlv (self); + + return NULL; +} + +static struct tlv * +qmi_tlv_next (QmiMessage *self, + struct tlv *tlv) +{ + struct tlv *end; + struct tlv *next; + + end = (struct tlv *) qmi_end (self); + next = tlv_next (tlv); + + return (next < end ? next : NULL); +} + +/** + * Checks the validity of a QMI message. + * + * In particular, checks: + * 1. The message has space for all required headers. + * 2. The length of the buffer, the qmux length field, and the QMI tlv_length + * field are all consistent. + * 3. The TLVs in the message fit exactly in the payload size. + * + * Returns non-zero if the message is valid, zero if invalid. + */ +gboolean +qmi_message_check (QmiMessage *self, + GError **error) +{ + size_t header_length; + char *end; + struct tlv *tlv; + + g_assert (self != NULL); + g_assert (self->buf != NULL); + + if (self->buf->marker != QMI_MESSAGE_QMUX_MARKER) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "Marker is incorrect"); + return FALSE; + } + + if (qmux_length (self) < sizeof (struct qmux)) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "QMUX length too short for QMUX header"); + return FALSE; + } + + /* + * qmux length is one byte shorter than buffer length because qmux + * length does not include the qmux frame marker. + */ + if (qmux_length (self) != self->len - 1) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "QMUX length and buffer length don't match"); + return FALSE; + } + + header_length = sizeof (struct qmux) + (qmi_message_is_control (self) ? + sizeof (struct control_header) : + sizeof (struct service_header)); + + if (qmux_length (self) < header_length) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "QMUX length too short for QMI header"); + return FALSE; + } + + if (qmux_length (self) - header_length != qmi_tlv_length (self)) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "QMUX length and QMI TLV lengths don't match"); + return FALSE; + } + + end = qmi_end (self); + for (tlv = qmi_tlv (self); tlv < (struct tlv *)end; tlv = tlv_next (tlv)) { + if (tlv->value > end) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "TLV header runs over buffer"); + return FALSE; + } + if (tlv->value + tlv->length > end) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_INVALID_MESSAGE, + "TLV value runs over buffer"); + return FALSE; + } + } + + /* + * If this assert triggers, one of the if statements in the loop is wrong. + * (It shouldn't be reached on malformed QMI messages.) + */ + g_assert (tlv == (struct tlv *)end); + + return TRUE; +} + +QmiMessage * +qmi_message_new (QmiService service, + guint8 client_id, + guint16 transaction_id, + guint16 message_id) +{ + QmiMessage *self; + + /* Transaction ID in the control service is 8bit only */ + g_assert (service != QMI_SERVICE_CTL || + transaction_id <= G_MAXUINT8); + + self = g_slice_new (QmiMessage); + self->ref_count = 1; + + self->len = 1 + sizeof (struct qmux) + (service == QMI_SERVICE_CTL ? + sizeof (struct control_header) : + sizeof (struct service_header)); + + /* TODO: Allocate both the message and the buffer together */ + self->buf = g_malloc (self->len); + + self->buf->marker = QMI_MESSAGE_QMUX_MARKER; + self->buf->qmux.flags = 0; + self->buf->qmux.service = service; + self->buf->qmux.client = client_id; + set_qmux_length (self, self->len - 1); + + if (service == QMI_SERVICE_CTL) { + self->buf->qmi.control.header.flags = 0; + self->buf->qmi.control.header.transaction = (uint8_t)transaction_id; + self->buf->qmi.control.header.message = htole16 (message_id); + } else { + self->buf->qmi.service.header.flags = 0; + self->buf->qmi.service.header.transaction = htole16 (transaction_id); + self->buf->qmi.service.header.message = htole16 (message_id); + } + + set_qmi_tlv_length (self, 0); + + g_assert (qmi_message_check (self, NULL)); + + return self; +} + +QmiMessage * +qmi_message_ref (QmiMessage *self) +{ + g_assert (self != NULL); + + g_atomic_int_inc (&self->ref_count); + return self; +} + +void +qmi_message_unref (QmiMessage *self) +{ + g_assert (self != NULL); + + if (g_atomic_int_dec_and_test (&self->ref_count)) { + g_free (self->buf); + g_slice_free (QmiMessage, self); + } +} + +gconstpointer +qmi_message_get_raw (QmiMessage *self, + gsize *len, + GError **error) +{ + g_assert (self != NULL); + g_assert (len != NULL); + + if (!qmi_message_check (self, error)) + return NULL; + + *len = self->len; + return self->buf; +} + +static gboolean +qmimsg_tlv_get_internal (QmiMessage *self, + guint8 type, + guint16 *length, + gpointer value, + gboolean length_exact, + GError **error) +{ + struct tlv *tlv; + + g_assert (self != NULL); + g_assert (self->buf != NULL); + g_assert (length != NULL); + /* note: we allow querying only for the exact length */ + + for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) { + if (tlv->type == type) { + if (length_exact && (tlv->length != *length)) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_TLV_NOT_FOUND, + "TLV found but wrong length (%u != %u)", + tlv->length, + *length); + return FALSE; + } else if (value && tlv->length > *length) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_TLV_TOO_LONG, + "TLV found but too long (%u > %u)", + tlv->length, + *length); + return FALSE; + } + + *length = tlv->length; + if (value) + memcpy (value, tlv->value, tlv->length); + return TRUE; + } + } + + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_TLV_TOO_LONG, + "TLV not found"); + return FALSE; +} + +gboolean +qmi_message_tlv_get (QmiMessage *self, + guint8 type, + guint16 length, + gpointer value, + GError **error) +{ + return qmimsg_tlv_get_internal (self, type, &length, value, TRUE, error); +} + +gboolean +qmi_message_tlv_get_varlen (QmiMessage *self, + guint8 type, + guint16 *length, + gpointer value, + GError **error) +{ + return qmimsg_tlv_get_internal (self, type, length, value, FALSE, error); +} + +void +qmi_message_tlv_foreach (QmiMessage *self, + QmiMessageForeachTlvFn callback, + gpointer user_data) +{ + struct tlv *tlv; + + g_assert (self != NULL); + g_assert (callback != NULL); + + for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) { + callback (tlv->type, + (gsize)(tlv->length), + (gconstpointer)tlv->value, + user_data); + } +} + +gboolean +qmi_message_tlv_add (QmiMessage *self, + guint8 type, + gsize length, + gconstpointer value, + GError **error) +{ + size_t tlv_len; + struct tlv *tlv; + + g_assert (self != NULL); + g_assert ((length == 0) || value != NULL); + + /* Make sure nothing's broken to start. */ + if (!qmi_message_check (self, error)) { + g_prefix_error (error, "Invalid QMI message detected: "); + return FALSE; + } + + /* Find length of new TLV. */ + tlv_len = sizeof (struct tlv) + length; + + /* Check for overflow of message size. */ + if (qmux_length (self) + tlv_len > UINT16_MAX) { + g_set_error (error, + QMI_CORE_ERROR, + QMI_CORE_ERROR_TLV_TOO_LONG, + "TLV to add is too long"); + return FALSE; + } + + /* Resize buffer. */ + self->len += tlv_len; + self->buf = g_realloc (self->buf, self->len); + + /* Fill in new TLV. */ + tlv = (struct tlv *)(qmi_end (self) - tlv_len); + tlv->type = type; + tlv->length = length; + if (value) + memcpy (tlv->value, value, length); + + /* Update length fields. */ + set_qmux_length (self, (uint16_t)(qmux_length (self) + tlv_len)); + set_qmi_tlv_length (self, (uint16_t)(qmi_tlv_length(self) + tlv_len)); + + /* Make sure we didn't break anything. */ + if (!qmi_message_check (self, error)) { + g_prefix_error (error, "Invalid QMI message built: "); + return FALSE; + } + + return TRUE; +} + +QmiMessage * +qmi_message_new_from_raw (const guint8 *raw, + gsize raw_len) +{ + QmiMessage *self; + gsize message_len; + struct qmux *qmux; + + /* If we didn't even read the header, leave */ + if (raw_len < (sizeof (struct qmux) + 1)) + return NULL; + + /* We need to have read the length reported by the header. + * Otherwise, return. */ + message_len = le16toh (((struct full_message *)raw)->qmux.length); + if (raw_len < (message_len - 1)) + return NULL; + + /* Ok, so we should have all the data available already */ + self = g_slice_new (QmiMessage); + self->ref_count = 1; + self->len = message_len + 1; + self->buf = g_malloc (self->len); + memcpy (self->buf, raw, self->len); + + /* NOTE: we don't check if the message is valid here, let the caller do it */ + + return self; +} + +gchar * +qmi_message_get_printable (QmiMessage *self) +{ + GString *printable; + struct tlv *tlv; + + if (!qmi_message_check (self, NULL)) + return NULL; + + printable = g_string_new (""); + g_string_append_printf (printable, + "QMUX:\n" + "\tlength=0x%04x\n" + "\tflags=0x%02x\n" + "\tservice=0x%02x\n" + "\tclient=0x%02x\n", + qmux_length (self), + qmi_message_get_qmux_flags (self), + qmi_message_get_service (self), + qmi_message_get_client_id (self)); + g_string_append_printf (printable, + "QMI:\n" + "\tflags=0x%02x\n" + "\ttransaction=0x%04x\n" + "\tmessage=0x%04x\n" + "\ttlv_length=0x%04x\n", + qmi_message_get_qmi_flags (self), + qmi_message_get_transaction_id (self), + qmi_message_get_message_id (self), + qmi_tlv_length (self)); + + for (tlv = qmi_tlv_first (self); tlv; tlv = qmi_tlv_next (self, tlv)) { + gchar *value_hex; + + value_hex = qmi_utils_str_hex (tlv->value, tlv->length, ':'); + g_string_append_printf (printable, + "TLV:\n" + "\ttype=0x%02x\n" + "\tlength=0x%04x\n" + "\tvalue=%s\n", + tlv->type, + tlv->length, + value_hex); + g_free (value_hex); + } + + return g_string_free (printable, FALSE); +} diff --git a/src/qmi-message.h b/src/qmi-message.h new file mode 100644 index 0000000..1d2b023 --- /dev/null +++ b/src/qmi-message.h @@ -0,0 +1,91 @@ +/* -*- Mode: C; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ + +/* + * Copyright (c) 2012 The Chromium OS Authors. All rights reserved. + * Use of this source code is governed by a BSD-style license that can be + * found in the LICENSE file. + */ + +/* + * libqmi-glib -- GLib/GIO based library to control QMI devices + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library 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 + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA. + * + * Copyright (C) 2012 Aleksander Morgado <aleksander@lanedo.com> + */ + +/* NOTE: this is a private non-installable header */ + +#ifndef _LIBQMI_GLIB_QMI_MESSAGE_H_ +#define _LIBQMI_GLIB_QMI_MESSAGE_H_ + +#include <glib.h> + +#include "qmi-enums.h" + +G_BEGIN_DECLS + +#define QMI_MESSAGE_QMUX_MARKER (guint8)0x01 +typedef struct _QmiMessage QmiMessage; + +QmiMessage *qmi_message_new (QmiService service, + guint8 client_id, + guint16 transaction_id, + guint16 message_id); +QmiMessage *qmi_message_new_from_raw (const guint8 *raw, + gsize raw_len); +QmiMessage *qmi_message_ref (QmiMessage *self); +void qmi_message_unref (QmiMessage *self); + +typedef void (* QmiMessageForeachTlvFn) (guint8 type, + gsize length, + gconstpointer value, + gpointer user_data); +void qmi_message_tlv_foreach (QmiMessage *self, + QmiMessageForeachTlvFn callback, + gpointer user_data); + +gboolean qmi_message_tlv_get (QmiMessage *self, + guint8 type, + guint16 length, + gpointer value, + GError **error); +gboolean qmi_message_tlv_get_varlen (QmiMessage *self, + guint8 type, + guint16 *length, + gpointer value, + GError **error); + +gboolean qmi_message_tlv_add (QmiMessage *self, + guint8 type, + gsize length, + gconstpointer value, + GError **error); + +gconstpointer qmi_message_get_raw (QmiMessage *self, + gsize *length, + GError **error); + +gsize qmi_message_get_length (QmiMessage *self); + +gchar *qmi_message_get_printable (QmiMessage *self); + +gboolean qmi_message_check (QmiMessage *self, + GError **error); + +G_END_DECLS + +#endif /* _LIBQMI_GLIB_QMI_MESSAGE_H_ */ diff --git a/src/qmimsg.c b/src/qmimsg.c deleted file mode 100644 index 57a8ce4..0000000 --- a/src/qmimsg.c +++ /dev/null @@ -1,480 +0,0 @@ -/* - * Copyright (c) 2012 The Chromium OS Authors. All rights reserved. - * Use of this source code is governed by a BSD-style license that can be - * found in the LICENSE file. - */ - -/* - * qmimsg: QMI messages, and how they are read and written. - * - * Sources used in writing this file (see README for links): - * [Gobi]/Core/QMIBuffers.h - * [GobiNet]/QMI.c - * [cros-kerne]/drivers/net/usb/gobi/qmi.c - */ - -#include "qmimsg.h" - -#include <assert.h> -#include <endian.h> -#include <errno.h> -#include <glib.h> -#include <stdint.h> -#include <stdio.h> -#include <string.h> - -#include "error.h" -#include "util.h" - -#define PACKED __attribute__((packed)) - -static const uint8_t QMUX_MARKER = 0x01; - -struct qmux { - uint16_t length; - uint8_t flags; - uint8_t service; - uint8_t client; -} PACKED; - -struct control_header { - uint8_t flags; - uint8_t transaction; - uint16_t message; - uint16_t tlv_length; -} PACKED; - -struct service_header { - uint8_t flags; - uint16_t transaction; - uint16_t message; - uint16_t tlv_length; -} PACKED; - -struct tlv { - uint8_t type; - uint16_t length; - char value[]; -} PACKED; - -struct control_message { - struct control_header header; - struct tlv tlv[]; -} PACKED; - -struct service_message { - struct service_header header; - struct tlv tlv[]; -} PACKED; - -struct qmimsg { - struct { - uint8_t marker; - struct qmux qmux; - union { - struct control_message control; - struct service_message service; - } qmi; - } PACKED *buf; /* buf allocated using g_malloc, not g_slice_alloc */ - size_t len; /* cached size of *buf; not part of message. */ -}; - -static uint16_t qmux_length(struct qmimsg *m) -{ - return le16toh(m->buf->qmux.length); -} - -static void set_qmux_length(struct qmimsg *m, uint16_t length) -{ - m->buf->qmux.length = htole16(length); -} - -static uint8_t qmux_flags(struct qmimsg *m) { return m->buf->qmux.flags; } -static uint8_t qmux_service(struct qmimsg *m) { return m->buf->qmux.service; } -static uint8_t qmux_client(struct qmimsg *m) { return m->buf->qmux.client; } - -static int qmi_is_control(struct qmimsg *m) { return !m->buf->qmux.service; } - -static uint8_t qmi_flags(struct qmimsg *m) -{ - if (qmi_is_control(m)) - return m->buf->qmi.control.header.flags; - else - return m->buf->qmi.service.header.flags; -} - -static uint16_t qmi_transaction(struct qmimsg *m) -{ - if (qmi_is_control(m)) - return (uint16_t)m->buf->qmi.control.header.transaction; - else - return le16toh(m->buf->qmi.service.header.transaction); -} - -static uint16_t qmi_message(struct qmimsg *m) -{ - if (qmi_is_control(m)) - return le16toh(m->buf->qmi.control.header.message); - else - return le16toh(m->buf->qmi.service.header.message); -} - -static uint16_t qmi_tlv_length(struct qmimsg *m) -{ - if (qmi_is_control(m)) - return le16toh(m->buf->qmi.control.header.tlv_length); - else - return le16toh(m->buf->qmi.service.header.tlv_length); -} - -static void set_qmi_tlv_length(struct qmimsg *m, uint16_t length) -{ - if (qmi_is_control(m)) - m->buf->qmi.control.header.tlv_length = htole16(length); - else - m->buf->qmi.service.header.tlv_length = htole16(length); -} - -static struct tlv *qmi_tlv(struct qmimsg *m) -{ - if (qmi_is_control(m)) - return m->buf->qmi.control.tlv; - else - return m->buf->qmi.service.tlv; -} - -static char *qmi_end(struct qmimsg *m) -{ - return (char *)m->buf + m->len; -} - -static struct tlv *tlv_next(struct tlv *tlv) -{ - return (struct tlv *)((char *)tlv + sizeof(struct tlv) + tlv->length); -} - -static struct tlv *qmi_tlv_first(struct qmimsg *m) -{ - if (qmi_tlv_length(m)) - return qmi_tlv(m); - else - return NULL; -} - -static struct tlv *qmi_tlv_next(struct qmimsg *m, struct tlv *tlv) -{ - struct tlv *end = (struct tlv *)qmi_end(m); - struct tlv *next = tlv_next(tlv); - if (next < end) - return next; - else - return NULL; -} - -/** - * Checks the validity of a QMI message. - * - * In particular, checks: - * 1. The message has space for all required headers. - * 2. The length of the buffer, the qmux length field, and the QMI tlv_length - * field are all consistent. - * 3. The TLVs in the message fit exactly in the payload size. - * - * Returns non-zero if the message is valid, zero if invalid. - */ -static int qmi_check(struct qmimsg *m) -{ - assert(m); - assert(m->buf); - - if (m->buf->marker != QMUX_MARKER) { - fprintf(stderr, "qmi_check: marker is incorrect\n"); - return 0; - } - - if (qmux_length(m) < sizeof(struct qmux)) { - fprintf(stderr, "qmi_check: QMUX length too short for QMUX header\n"); - return 0; - } - - /* - * qmux length is one byte shorter than buffer length because qmux - * length does not include the qmux frame marker. - */ - if (qmux_length(m) != m->len - 1) { - fprintf(stderr, "qmi_check: QMUX length and buffer length don't match\n"); - return 0; - } - - size_t header_length; - if (qmi_is_control(m)) - header_length = sizeof(struct qmux) + sizeof(struct control_header); - else - header_length = sizeof(struct qmux) + sizeof(struct service_header); - - if (qmux_length(m) < header_length) { - fprintf(stderr, "qmi_check: QMUX length too short for QMI header\n"); - return 0; - } - - if (qmux_length(m) - header_length != qmi_tlv_length(m)) { - fprintf(stderr, "qmi_check: QMUX length and QMI TLV lengths don't match\n"); - return 0; - } - - char *end = qmi_end(m); - struct tlv *tlv; - for (tlv = qmi_tlv(m); tlv < (struct tlv *)end; tlv = tlv_next(tlv)) { - if (tlv->value > end) { - fprintf(stderr, "qmi_check: TLV header runs over buffer\n"); - return 0; - } - if (tlv->value + tlv->length > end) { - fprintf(stderr, "qmi_check: TLV value runs over buffer\n"); - return 0; - } - } - /* - * If this assert triggers, one of the if statements in the loop is wrong. - * (It shouldn't be reached on malformed QMI messages.) - */ - assert(tlv == (struct tlv *)end); - - return 1; -} - -int qmimsg_new(uint8_t qmux_flags, uint8_t service, uint8_t client, - uint8_t qmi_flags, uint16_t transaction, uint16_t message, - struct qmimsg **message_out) -{ - assert(service || (transaction <= UINT8_MAX)); - - struct qmimsg *m = g_slice_new(struct qmimsg); - - if (!service) - m->len = 1 + sizeof(struct qmux) + sizeof(struct control_header); - else - m->len = 1 + sizeof(struct qmux) + sizeof(struct service_header); - m->buf = g_malloc(m->len); - - m->buf->marker = QMUX_MARKER; - - m->buf->qmux.flags = qmux_flags; - m->buf->qmux.service = service; - m->buf->qmux.client = client; - set_qmux_length(m, m->len - 1); - - if (!service) { - m->buf->qmi.control.header.flags = qmi_flags; - m->buf->qmi.control.header.transaction = (uint8_t)transaction; - m->buf->qmi.control.header.message = htole16(message); - } else { - m->buf->qmi.service.header.flags = qmi_flags; - m->buf->qmi.service.header.transaction = htole16(transaction); - m->buf->qmi.service.header.message = htole16(message); - } - set_qmi_tlv_length(m, 0); - - assert(qmi_check(m)); - - *message_out = m; - - return 0; -} - -int qmimsg_read(qmimsg_read_fn read_fn, void *context, - struct qmimsg **message_out) -{ - struct qmimsg *m; - struct { uint8_t marker; struct qmux qmux; } PACKED framed_qmux; - int result; - - result = read_fn(context, &framed_qmux, sizeof(framed_qmux)); - if (result) - return result; - if (framed_qmux.marker != QMUX_MARKER) - return QMI_ERR_FRAMING_INVALID; - - m = g_slice_new(struct qmimsg); - m->len = le16toh(framed_qmux.qmux.length) + 1; - m->buf = g_malloc(m->len); - - memcpy(m->buf, &framed_qmux, sizeof(framed_qmux)); - result = read_fn(context, &m->buf->qmi, m->len - sizeof(framed_qmux)); - if (result) { - qmimsg_free(m); - return result; - } - - if (!qmi_check(m)) { - qmimsg_free(m); - return QMI_ERR_HEADER_INVALID; - } - - *message_out = m; - - return 0; -} - -int qmimsg_write(struct qmimsg *m, qmimsg_write_fn write_fn, void *context) -{ - int result; - - assert(m); - assert(write_fn); - assert(qmi_check(m)); - - result = write_fn(context, m->buf, m->len); - if (result) - return result; - - return 0; -} - -void qmimsg_get_header(struct qmimsg *message, - uint8_t *service_out, - uint8_t *client_out, - uint8_t *qmi_flags_out, - uint16_t *transaction_out, - uint16_t *message_out) -{ - assert(message); - - if (service_out) - *service_out = qmux_service(message); - if (client_out) - *client_out = qmux_client(message); - if (qmi_flags_out) - *qmi_flags_out = qmi_flags(message); - if (transaction_out) - *transaction_out = qmi_transaction(message); - if (message_out) - *message_out = qmi_message(message); -} - -void qmimsg_print(struct qmimsg *m) -{ - assert(m); - - fprintf(stderr, - "QMUX: length=0x%04x flags=0x%02x service=0x%02x client=0x%02x\n", - qmux_length(m), - qmux_flags(m), - qmux_service(m), - qmux_client(m)); - - fprintf(stderr, - "QMI: flags=0x%02x transaction=0x%04x " - "message=0x%04x tlv_length=0x%04x\n", - qmi_flags(m), - qmi_transaction(m), - qmi_message(m), - qmi_tlv_length(m)); - - struct tlv *tlv; - for (tlv = qmi_tlv_first(m); tlv; tlv = qmi_tlv_next(m, tlv)) { - fprintf(stderr, "TLV: type=0x%02x length=0x%04x\n", - tlv->type, tlv->length); - hexdump(tlv->value, tlv->length); - } -} - -void qmimsg_free(struct qmimsg *message) -{ - assert(message); - g_free(message->buf); - g_slice_free(struct qmimsg, message); -} - -static int qmimsg_tlv_get_internal(struct qmimsg *message, - uint8_t type, - uint16_t *length, - void *value, - int length_exact) -{ - assert(message); - assert(message->buf); - assert(length); - - struct tlv *tlv; - for (tlv = qmi_tlv_first(message); tlv; tlv = qmi_tlv_next(message, tlv)) { - if (tlv->type == type) { - if (length_exact && (tlv->length != *length)) { - return QMI_ERR_TLV_NOT_FOUND; - } else if (value && tlv->length > *length) { - return QMI_ERR_TOO_LONG; - } - *length = tlv->length; - if (value) - memcpy(value, tlv->value, tlv->length); - return 0; - } - } - - return QMI_ERR_TLV_NOT_FOUND; -} - -int qmimsg_tlv_get(struct qmimsg *message, - uint8_t type, uint16_t length, void *value) -{ - assert(value); - - return qmimsg_tlv_get_internal(message, type, &length, value, 1); -} - -int qmimsg_tlv_get_varlen(struct qmimsg *message, - uint8_t type, uint16_t *length, void *value) -{ - return qmimsg_tlv_get_internal(message, type, length, value, 0); -} - -void qmimsg_tlv_foreach(struct qmimsg *message, - qmimsg_tlv_foreach_fn func, void *context) -{ - assert(message); - assert(func); - - struct tlv *tlv; - for (tlv = qmi_tlv_first(message); tlv; tlv = qmi_tlv_next(message, tlv)) { - func(context, tlv->type, tlv->length, tlv->value); - } -} - - -int qmimsg_tlv_add(struct qmimsg *m, - uint8_t type, uint16_t length, const void *value) -{ - assert(m); - assert((length == 0) || value); - - /* Make sure nothing's broken to start. */ - assert(qmi_check(m)); - - /* Find length of new TLV. */ - size_t tlv_len = sizeof(struct tlv) + length; - - /* Check for overflow of message size. */ - if (qmux_length(m) + tlv_len > UINT16_MAX) { - return QMI_ERR_TOO_LONG; - } - - /* Resize buffer. */ - m->len += tlv_len; - m->buf = g_realloc(m->buf, m->len); - - /* Fill in new TLV. */ - struct tlv *tlv = (struct tlv *)(qmi_end(m) - tlv_len); - tlv->type = type; - tlv->length = length; - if (value) - memcpy(tlv->value, value, length); - - /* Update length fields. */ - set_qmux_length(m, (uint16_t)(qmux_length(m) + tlv_len)); - set_qmi_tlv_length(m, (uint16_t)(qmi_tlv_length(m) + tlv_len)); - - /* Make sure we didn't break anything. */ - assert(qmi_check(m)); - - return 0; -} diff --git a/src/qmimsg.h b/src/qmimsg.h deleted file mode 100644 index 105cf2d..0000000 --- a/src/qmimsg.h +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Copyright (c) 2012 The Chromium OS Authors. All rights reserved. - * Use of this source code is governed by a BSD-style license that can be - * found in the LICENSE file. - */ - -/* - * qmimsg: QMI messages, and how they are read and written. - * - * (See qmimsg.c for sources.) - */ - -#ifndef LIBQMI_QMIMSG_H -#define LIBQMI_QMIMSG_H - -#include <stddef.h> -#include <stdint.h> - -struct qmimsg; - -/** - * Callback type used by qmimsg_read to read bytes from the input. - */ -typedef int (*qmimsg_read_fn)(void *context, void *buf, size_t len); - -/** - * Callback type used by qmimsg_write to write bytes to the output. - */ -typedef int (*qmimsg_write_fn)(void *context, const void *buf, size_t len); - -/** - * Creates a new QMI message with the given header data and no TLVs. - */ -int qmimsg_new(uint8_t qmux_flags, uint8_t service, uint8_t client, - uint8_t qmi_flags, uint16_t transaction, uint16_t message, - struct qmimsg **message_out); - -/** - * Reads a QMI message from the given input (read_fn and context). - */ -int qmimsg_read(qmimsg_read_fn read_fn, void *context, - struct qmimsg **message_out); - -/** - * Frees a QMI message returned by qmimsg_new or qmimsg_read. - */ -void qmimsg_free(struct qmimsg *message); - -/** - * Writes a QMI message to the given output (write_fn and context). - */ -int qmimsg_write(struct qmimsg *message, - qmimsg_write_fn write_fn, void *context); - -/** - * Prints the contents of a QMI message to stderr for debugging purposes. - */ -void qmimsg_print(struct qmimsg *message); - -/** - * Retrieves a tasteful subset of the header fields in a QMI message. - */ -void qmimsg_get_header(struct qmimsg *message, - uint8_t *service_out, - uint8_t *client_out, - uint8_t *qmi_flags_out, - uint16_t *transaction_out, - uint16_t *message_out); - -/** - * Finds a TLV element with the given type in the payload of the given QMI - * message, checks that the length is length, and copies the value into value. - * - * Returns 0 if the element was found and was the correct length; - * EINVAL if the element was found but the length was incorrect; - * ENOENT if the element was not found. - */ -int qmimsg_tlv_get(struct qmimsg *message, - uint8_t type, uint16_t length, void *value); - -/** - * Finds a TLV element with the given type in the payload of the given QMI - * message, copies the length into length, and copies the value into value. - * - * If value is NULL, overwrites length with the length of the message without - * checking the existing value of length. - * - * Returns 0 if the element was found and fit into the buffer; - * ENOSPC if the element was found but the value was too big; - * ENOENT if the element was not found. - */ -int qmimsg_tlv_get_varlen(struct qmimsg *message, - uint8_t type, uint16_t *length, void *value); - -typedef void (*qmimsg_tlv_foreach_fn)(void *context, - uint8_t type, - uint16_t length, - void *value); - -void qmimsg_tlv_foreach(struct qmimsg *message, - qmimsg_tlv_foreach_fn func, void *context); - -/** - * Appends a TLV element with the given type, length, and value to the payload - * of the given QMI message. - * - * On success, returns zero; on failure, returns errno. - * - * Returns 0 if the element was added successfully; - * ENOSPC if adding the element would overflow one of the length fields. - */ -int qmimsg_tlv_add(struct qmimsg *message, - uint8_t type, uint16_t length, const void *buf); - -#endif /* LIBQMI_QMIMSG_H */ |