qmimodem: Add support for QMI control point handling

1 files changed, 1681 insertions, 0 deletions

diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c
new file mode 100644
index 00000000..b08321d3
--- /dev/null
+++ b/drivers/qmimodem/qmi.c
@@ -0,0 +1,1681 @@
+/*
+ *
+ *  oFono - Open Source Telephony
+ *
+ *  Copyright (C) 2011-2012  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
+
+#define _GNU_SOURCE
+#include <stdio.h>
+#include <ctype.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <glib.h>
+
+#include "qmi.h"
+#include "ctl.h"
+
+typedef void (*qmi_message_func_t)(uint16_t message, uint16_t length,
+					const void *buffer, void *user_data);
+
+struct qmi_device {
+	int ref_count;
+	int fd;
+	GIOChannel *io;
+	bool close_on_unref;
+	guint read_watch;
+	guint write_watch;
+	GQueue *req_queue;
+	GQueue *control_queue;
+	GQueue *service_queue;
+	unsigned int next_id;
+	uint8_t next_control_tid;
+	uint16_t next_service_tid;
+	qmi_debug_func_t debug_func;
+	void *debug_data;
+	uint16_t control_major;
+	uint16_t control_minor;
+	char *version_str;
+	struct qmi_version *version_list;
+	uint8_t version_count;
+	GHashTable *service_list;
+	unsigned int release_users;
+};
+
+struct qmi_service {
+	int ref_count;
+	struct qmi_device *device;
+	uint8_t type;
+	uint16_t major;
+	uint16_t minor;
+	uint8_t client_id;
+	GList *notify_list;
+};
+
+struct qmi_param {
+	void *data;
+	uint16_t length;
+};
+
+struct qmi_result {
+	uint16_t message;
+	uint16_t result;
+	uint16_t error;
+	const void *data;
+	uint16_t length;
+};
+
+struct qmi_request {
+	unsigned int id;
+	uint16_t tid;
+	void *buf;
+	size_t len;
+	qmi_message_func_t callback;
+	void *user_data;
+};
+
+struct qmi_notify {
+	unsigned int id;
+	uint16_t message;
+	qmi_result_func_t callback;
+	void *user_data;
+	qmi_destroy_func_t destroy;
+};
+
+struct qmi_mux_hdr {
+	uint8_t  frame;		/* Always 0x01 */
+	uint16_t length;	/* Packet size without frame byte */
+	uint8_t  flags;		/* Either 0x00 or 0x80 */
+	uint8_t  service;	/* Service type (0x00 for control) */
+	uint8_t  client;	/* Client identifier (0x00 for control) */
+} __attribute__ ((packed));
+#define QMI_MUX_HDR_SIZE 6
+
+struct qmi_control_hdr {
+	uint8_t  type;		/* Bit 1 = response, Bit 2 = indication */
+	uint8_t  transaction;	/* Transaction identifier */
+} __attribute__ ((packed));
+#define QMI_CONTROL_HDR_SIZE 2
+
+struct qmi_service_hdr {
+	uint8_t  type;		/* Bit 2 = response, Bit 3 = indication */
+	uint16_t transaction;	/* Transaction identifier */
+} __attribute__ ((packed));
+#define QMI_SERVICE_HDR_SIZE 3
+
+struct qmi_message_hdr {
+	uint16_t message;	/* Message identifier */
+	uint16_t length;	/* Message size without header */
+	uint8_t data[0];
+} __attribute__ ((packed));
+#define QMI_MESSAGE_HDR_SIZE 4
+
+struct qmi_tlv_hdr {
+	uint8_t type;
+	uint16_t length;
+	uint8_t value[0];
+} __attribute__ ((packed));
+#define QMI_TLV_HDR_SIZE 3
+
+void qmi_free(void *ptr)
+{
+	free(ptr);
+}
+
+static struct qmi_request *__request_alloc(uint8_t service,
+				uint8_t client, uint16_t message,
+				uint16_t headroom, const void *data,
+				uint16_t length, qmi_message_func_t func,
+				void *user_data, void **head)
+{
+	struct qmi_request *req;
+	struct qmi_mux_hdr *hdr;
+	struct qmi_message_hdr *msg;
+
+	req = g_try_new0(struct qmi_request, 1);
+	if (!req)
+		return NULL;
+
+	req->len = QMI_MUX_HDR_SIZE + headroom + QMI_MESSAGE_HDR_SIZE + length;
+
+	req->buf = g_try_malloc(req->len);
+	if (!req->buf) {
+		g_free(req);
+		return NULL;
+	}
+
+	hdr = req->buf;
+
+	hdr->frame = 0x01;
+	hdr->length = GUINT16_TO_LE(req->len - 1);
+	hdr->flags = 0x00;
+	hdr->service = service;
+	hdr->client = client;
+
+	msg = req->buf + QMI_MUX_HDR_SIZE + headroom;
+
+	msg->message = GUINT16_TO_LE(message);
+	msg->length = GUINT16_TO_LE(length);
+
+	if (data && length > 0)
+		memcpy(req->buf + QMI_MUX_HDR_SIZE + headroom +
+					QMI_MESSAGE_HDR_SIZE, data, length);
+
+	req->callback = func;
+	req->user_data = user_data;
+
+	*head = req->buf + QMI_MUX_HDR_SIZE;
+
+	return req;
+}
+
+static void __request_free(gpointer data, gpointer user_data)
+{
+	struct qmi_request *req = data;
+
+	g_free(req->buf);
+	g_free(req);
+}
+
+static gint __request_compare(gconstpointer a, gconstpointer b)
+{
+	const struct qmi_request *req = a;
+	uint16_t tid = GPOINTER_TO_UINT(b);
+
+	return req->tid - tid;
+}
+
+static void __notify_free(gpointer data, gpointer user_data)
+{
+	struct qmi_notify *notify = data;
+
+	if (notify->destroy)
+		notify->destroy(notify->user_data);
+
+	g_free(notify);
+}
+
+static void __hexdump(const char dir, const unsigned char *buf, size_t len,
+				qmi_debug_func_t function, void *user_data)
+{
+	static const char hexdigits[] = "0123456789abcdef";
+	char str[68];
+	size_t i;
+
+	if (!function || !len)
+		return;
+
+	str[0] = dir;
+
+	for (i = 0; i < len; i++) {
+		str[((i % 16) * 3) + 1] = ' ';
+		str[((i % 16) * 3) + 2] = hexdigits[buf[i] >> 4];
+		str[((i % 16) * 3) + 3] = hexdigits[buf[i] & 0xf];
+		str[(i % 16) + 51] = isprint(buf[i]) ? buf[i] : '.';
+
+		if ((i + 1) % 16 == 0) {
+			str[49] = ' ';
+			str[50] = ' ';
+			str[67] = '\0';
+			function(str, user_data);
+			str[0] = ' ';
+		}
+	}
+
+	if (i % 16 > 0) {
+		size_t j;
+		for (j = (i % 16); j < 16; j++) {
+			str[(j * 3) + 1] = ' ';
+			str[(j * 3) + 2] = ' ';
+			str[(j * 3) + 3] = ' ';
+			str[j + 51] = ' ';
+		}
+		str[49] = ' ';
+		str[50] = ' ';
+		str[67] = '\0';
+		function(str, user_data);
+	}
+}
+
+static const char *__service_type_to_string(uint8_t type)
+{
+	switch (type) {
+	case QMI_SERVICE_CONTROL:
+		return "CTL";
+	case QMI_SERVICE_WDS:
+		return "WDS";
+	case QMI_SERVICE_DMS:
+		return "DMS";
+	case QMI_SERVICE_NAS:
+		return "NAS";
+	case QMI_SERVICE_QOS:
+		return "QOS";
+	case QMI_SERVICE_WMS:
+		return "WMS";
+	case QMI_SERVICE_PDS:
+		return "PDS";
+	case QMI_SERVICE_AUTH:
+		return "AUTH";
+	case QMI_SERVICE_AT:
+		return "AT";
+	case QMI_SERVICE_VOICE:
+		return "VOICE";
+	case QMI_SERVICE_CAT:
+		return "CAT";
+	case QMI_SERVICE_UIM:
+		return "UIM";
+	case QMI_SERVICE_PBM:
+		return "PBM";
+	case QMI_SERVICE_RMTFS:
+		return "RMTFS";
+	case QMI_SERVICE_LOC:
+		return "LOC";
+	case QMI_SERVICE_SAR:
+		return "SAR";
+	case QMI_SERVICE_CSD:
+		return "CSD";
+	case QMI_SERVICE_EFS:
+		return "EFS";
+	case QMI_SERVICE_TS:
+		return "TS";
+	case QMI_SERVICE_TMD:
+		return "TMS";
+	case QMI_SERVICE_CAT_OLD:
+		return "CAT";
+	case QMI_SERVICE_RMS:
+		return "RMS";
+	case QMI_SERVICE_OMA:
+		return "OMA";
+	}
+
+	return NULL;
+}
+
+static void __debug_msg(const char dir, const void *buf, size_t len,
+				qmi_debug_func_t function, void *user_data)
+{
+	const struct qmi_mux_hdr *hdr;
+	const struct qmi_message_hdr *msg;
+	const char *service;
+	const void *ptr;
+	uint16_t offset;
+	char strbuf[72 + 16], *str;
+
+	if (!function || !len)
+		return;
+
+	hdr = buf;
+
+	str = strbuf;
+	service = __service_type_to_string(hdr->service);
+	if (service)
+		str += sprintf(str, "%c   %s", dir, service);
+	else
+		str += sprintf(str, "%c   %d", dir, hdr->service);
+
+	if (hdr->service == QMI_SERVICE_CONTROL) {
+		const struct qmi_control_hdr *ctl;
+		const char *type;
+
+		ctl = buf + QMI_MUX_HDR_SIZE;
+		msg = buf + QMI_MUX_HDR_SIZE + QMI_CONTROL_HDR_SIZE;
+		ptr = buf + QMI_MUX_HDR_SIZE + QMI_CONTROL_HDR_SIZE +
+							QMI_MESSAGE_HDR_SIZE;
+
+		switch (ctl->type) {
+		case 0x00:
+			type = "_req";
+			break;
+		case 0x01:
+			type = "_resp";
+			break;
+		case 0x02:
+			type = "_ind";
+			break;
+		default:
+			type = "";
+			break;
+		}
+
+		str += sprintf(str, "%s msg=%d len=%d", type,
+					GUINT16_FROM_LE(msg->message),
+					GUINT16_FROM_LE(msg->length));
+
+		str += sprintf(str, " [client=%d,type=%d,tid=%d,len=%d]",
+					hdr->client, ctl->type,
+					ctl->transaction,
+					GUINT16_FROM_LE(hdr->length));
+	} else {
+		const struct qmi_service_hdr *srv;
+		const char *type;
+
+		srv = buf + QMI_MUX_HDR_SIZE;
+		msg = buf + QMI_MUX_HDR_SIZE + QMI_SERVICE_HDR_SIZE;
+		ptr = buf + QMI_MUX_HDR_SIZE + QMI_SERVICE_HDR_SIZE +
+							QMI_MESSAGE_HDR_SIZE;
+
+		switch (srv->type) {
+		case 0x00:
+			type = "_req";
+			break;
+		case 0x02:
+			type = "_resp";
+			break;
+		case 0x04:
+			type = "_ind";
+			break;
+		default:
+			type = "";
+			break;
+		}
+
+		str += sprintf(str, "%s msg=%d len=%d", type,
+					GUINT16_FROM_LE(msg->message),
+					GUINT16_FROM_LE(msg->length));
+
+		str += sprintf(str, " [client=%d,type=%d,tid=%d,len=%d]",
+					hdr->client, srv->type,
+					GUINT16_FROM_LE(srv->transaction),
+					GUINT16_FROM_LE(hdr->length));
+	}
+
+	function(strbuf, user_data);
+
+	if (!msg->length)
+		return;
+
+	str = strbuf;
+	str += sprintf(str, "      ");
+	offset = 0;
+
+	while (offset + QMI_TLV_HDR_SIZE < GUINT16_FROM_LE(msg->length)) {
+		const struct qmi_tlv_hdr *tlv = ptr + offset;
+		uint16_t tlv_length = GUINT16_FROM_LE(tlv->length);
+
+		if (tlv->type == 0x02 && tlv_length == QMI_RESULT_CODE_SIZE) {
+			const struct qmi_result_code *result = ptr + offset +
+							QMI_TLV_HDR_SIZE;
+
+			str += sprintf(str, " {type=%d,error=%d}", tlv->type,
+						GUINT16_FROM_LE(result->error));
+		} else {
+			str += sprintf(str, " {type=%d,len=%d}", tlv->type,
+								tlv_length);
+		}
+
+		if (str - strbuf > 72) {
+			function(strbuf, user_data);
+
+			str = strbuf;
+			str += sprintf(str, "      ");
+		}
+
+		offset += QMI_TLV_HDR_SIZE + tlv_length;
+	}
+
+	function(strbuf, user_data);
+}
+
+static void __debug_device(struct qmi_device *device,
+					const char *format, ...)
+{
+	char strbuf[72 + 16];
+	va_list ap;
+
+	if (!device->debug_func)
+		return;
+
+	va_start(ap, format);
+	vsnprintf(strbuf, sizeof(strbuf), format, ap);
+	va_end(ap);
+
+	device->debug_func(strbuf, device->debug_data);
+}
+
+static gboolean can_write_data(GIOChannel *channel, GIOCondition cond,
+							gpointer user_data)
+{
+	struct qmi_device *device = user_data;
+	struct qmi_mux_hdr *hdr;
+	struct qmi_request *req;
+	ssize_t bytes_written;
+
+	req = g_queue_pop_head(device->req_queue);
+	if (!req)
+		return FALSE;
+
+	bytes_written = write(device->fd, req->buf, req->len);
+	if (bytes_written < 0)
+		return FALSE;
+
+	__hexdump('>', req->buf, bytes_written,
+				device->debug_func, device->debug_data);
+
+	__debug_msg(' ', req->buf, bytes_written,
+				device->debug_func, device->debug_data);
+
+	hdr = req->buf;
+
+	if (hdr->service == QMI_SERVICE_CONTROL)
+		g_queue_push_tail(device->control_queue, req);
+	else
+		g_queue_push_tail(device->service_queue, req);
+
+	g_free(req->buf);
+	req->buf = NULL;
+
+	if (g_queue_get_length(device->req_queue) > 0)
+		return TRUE;
+
+	return FALSE;
+}
+
+static void write_watch_destroy(gpointer user_data)
+{
+	struct qmi_device *device = user_data;
+
+	device->write_watch = 0;
+}
+
+static void wakeup_writer(struct qmi_device *device)
+{
+	if (device->write_watch > 0)
+		return;
+
+	device->write_watch = g_io_add_watch_full(device->io, G_PRIORITY_HIGH,
+				G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+				can_write_data, device, write_watch_destroy);
+}
+
+static void __request_submit(struct qmi_device *device,
+				struct qmi_request *req, uint16_t transaction)
+{
+	req->id = device->next_id++;
+	req->tid = transaction;
+
+	g_queue_push_tail(device->req_queue, req);
+
+	wakeup_writer(device);
+}
+
+static void service_notify(gpointer key, gpointer value, gpointer user_data)
+{
+	struct qmi_service *service = value;
+	struct qmi_result *result = user_data;
+	GList *list;
+
+	for (list = g_list_first(service->notify_list); list;
+						list = g_list_next(list)) {
+		struct qmi_notify *notify = list->data;
+
+		if (notify->message == result->message)
+			notify->callback(result, notify->user_data);
+	}
+}
+
+static void handle_indication(struct qmi_device *device,
+			uint8_t service_type, uint8_t client_id,
+			uint16_t message, uint16_t length, const void *data)
+{
+	struct qmi_service *service;
+	struct qmi_result result;
+	uint16_t hash_id;
+
+	if (service_type == QMI_SERVICE_CONTROL)
+		return;
+
+	result.result = 0;
+	result.error = 0;
+	result.message = message;
+	result.data = data;
+	result.length = length;
+
+	if (client_id == 0xff) {
+		g_hash_table_foreach(device->service_list,
+						service_notify, &result);
+		return;
+	}
+
+	hash_id = service_type | (client_id << 8);
+
+	service = g_hash_table_lookup(device->service_list,
+					GUINT_TO_POINTER(hash_id));
+	if (!service)
+		return;
+
+	service_notify(NULL, service, &result);
+}
+
+static void handle_packet(struct qmi_device *device,
+				const struct qmi_mux_hdr *hdr, const void *buf)
+{
+	struct qmi_request *req;
+	uint16_t message, length;
+	const void *data;
+
+	if (hdr->service == QMI_SERVICE_CONTROL) {
+		const struct qmi_control_hdr *control = buf;
+		const struct qmi_message_hdr *msg;
+		GList *list;
+
+		/* Ignore control messages with client identifier */
+		if (hdr->client != 0x00)
+			return;
+
+		msg = buf + QMI_CONTROL_HDR_SIZE;
+
+		message = GUINT16_FROM_LE(msg->message);
+		length = GUINT16_FROM_LE(msg->length);
+
+		data = buf + QMI_CONTROL_HDR_SIZE + QMI_MESSAGE_HDR_SIZE;
+
+		if (control->type == 0x02 && control->transaction == 0x00) {
+			handle_indication(device, hdr->service, hdr->client,
+							message, length, data);
+			return;
+		}
+
+		list = g_queue_find_custom(device->control_queue,
+					GUINT_TO_POINTER(control->transaction),
+					__request_compare);
+		if (!list)
+			return;
+
+		req = list->data;
+
+		g_queue_delete_link(device->control_queue, list);
+	} else {
+		const struct qmi_service_hdr *service = buf;
+		const struct qmi_message_hdr *msg;
+		uint16_t tid;
+		GList *list;
+
+		msg = buf + QMI_SERVICE_HDR_SIZE;
+
+		message = GUINT16_FROM_LE(msg->message);
+		length = GUINT16_FROM_LE(msg->length);
+
+		data = buf + QMI_SERVICE_HDR_SIZE + QMI_MESSAGE_HDR_SIZE;
+
+		tid = GUINT16_FROM_LE(service->transaction);
+
+		if (service->type == 0x04 && tid == 0x0000) {
+			handle_indication(device, hdr->service, hdr->client,
+							message, length, data);
+			return;
+		}
+
+		list = g_queue_find_custom(device->service_queue,
+				GUINT_TO_POINTER(tid), __request_compare);
+		if (!list)
+			return;
+
+		req = list->data;
+
+		g_queue_delete_link(device->service_queue, list);
+	}
+
+	if (req->callback)
+		req->callback(message, length, data, req->user_data);
+
+	__request_free(req, NULL);
+}
+
+static gboolean received_data(GIOChannel *channel, GIOCondition cond,
+							gpointer user_data)
+{
+	struct qmi_device *device = user_data;
+	struct qmi_mux_hdr *hdr;
+	unsigned char buf[2048];
+	ssize_t bytes_read;
+	uint16_t offset;
+
+	if (cond & G_IO_NVAL)
+		return FALSE;
+
+	bytes_read = read(device->fd, buf, sizeof(buf));
+	if (bytes_read < 0)
+		return TRUE;
+
+	__hexdump('<', buf, bytes_read,
+				device->debug_func, device->debug_data);
+
+	offset = 0;
+
+	while (offset < bytes_read) {
+		uint16_t len;
+
+		/* Check if QMI mux header fits into packet */
+		if (bytes_read - offset < QMI_MUX_HDR_SIZE)
+			break;
+
+		hdr = (void *) (buf + offset);
+
+		/* Check for fixed frame and flags value */
+		if (hdr->frame != 0x01 || hdr->flags != 0x80)
+			break;
+
+		len = GUINT16_FROM_LE(hdr->length) + 1;
+
+		/* Check that packet size matches frame size */
+		if (bytes_read - offset < len)
+			break;
+
+		__debug_msg(' ', buf + offset, len,
+				device->debug_func, device->debug_data);
+
+		handle_packet(device, hdr, buf + offset + QMI_MUX_HDR_SIZE);
+
+		offset += len;
+	}
+
+	return TRUE;
+}
+
+static void read_watch_destroy(gpointer user_data)
+{
+	struct qmi_device *device = user_data;
+
+	device->read_watch = 0;
+}
+
+static void service_destroy(gpointer data)
+{
+	struct qmi_service *service = data;
+
+	if (!service->device)
+		return;
+
+	service->device = NULL;
+}
+
+struct qmi_device *qmi_device_new(int fd)
+{
+	struct qmi_device *device;
+	long flags;
+
+	device = g_try_new0(struct qmi_device, 1);
+	if (!device)
+		return NULL;
+
+	__debug_device(device, "device %p new", device);
+
+	device->ref_count = 1;
+
+	device->fd = fd;
+	device->close_on_unref = false;
+
+	flags = fcntl(device->fd, F_GETFL, NULL);
+	if (flags < 0) {
+		g_free(device);
+		return NULL;
+	}
+
+	if (!(flags & O_NONBLOCK)) {
+		if (fcntl(device->fd, F_SETFL, flags | O_NONBLOCK) < 0) {
+			g_free(device);
+			return NULL;
+		}
+	}
+
+	device->io = g_io_channel_unix_new(device->fd);
+
+	g_io_channel_set_encoding(device->io, NULL, NULL);
+	g_io_channel_set_buffered(device->io, FALSE);
+
+	device->read_watch = g_io_add_watch_full(device->io, G_PRIORITY_DEFAULT,
+				G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
+				received_data, device, read_watch_destroy);
+
+	g_io_channel_unref(device->io);
+
+	device->req_queue = g_queue_new();
+	device->control_queue = g_queue_new();
+	device->service_queue = g_queue_new();
+
+	device->next_id = 1;
+
+	device->next_control_tid = 1;
+	device->next_service_tid = 1;
+
+	device->service_list = g_hash_table_new_full(g_direct_hash,
+					g_direct_equal, NULL, service_destroy);
+
+	return device;
+}
+
+struct qmi_device *qmi_device_ref(struct qmi_device *device)
+{
+	if (!device)
+		return NULL;
+
+	__sync_fetch_and_add(&device->ref_count, 1);
+
+	return device;
+}
+
+void qmi_device_unref(struct qmi_device *device)
+{
+	if (!device)
+		return;
+
+	if (__sync_sub_and_fetch(&device->ref_count, 1))
+		return;
+
+	__debug_device(device, "device %p free", device);
+
+	g_queue_foreach(device->control_queue, __request_free, NULL);
+	g_queue_free(device->control_queue);
+
+	g_queue_foreach(device->service_queue, __request_free, NULL);
+	g_queue_free(device->service_queue);
+
+	g_queue_foreach(device->req_queue, __request_free, NULL);
+	g_queue_free(device->req_queue);
+
+	if (device->write_watch > 0)
+		g_source_remove(device->write_watch);
+
+	if (device->read_watch > 0)
+		g_source_remove(device->read_watch);
+
+	if (device->close_on_unref)
+		close(device->fd);
+
+	g_hash_table_destroy(device->service_list);
+
+	g_free(device->version_str);
+	g_free(device->version_list);
+
+	g_free(device);
+}
+
+void qmi_device_set_debug(struct qmi_device *device,
+				qmi_debug_func_t func, void *user_data)
+{
+	if (device == NULL)
+		return;
+
+	device->debug_func = func;
+	device->debug_data = user_data;
+}
+
+void qmi_device_set_close_on_unref(struct qmi_device *device, bool do_close)
+{
+	if (!device)
+		return;
+
+	device->close_on_unref = do_close;
+}
+
+static const void *tlv_get(const void *data, uint16_t size,
+					uint8_t type, uint16_t *length)
+{
+	const void *ptr = data;
+	uint16_t len = size;
+
+	while (len > QMI_TLV_HDR_SIZE) {
+		const struct qmi_tlv_hdr *tlv = ptr;
+		uint16_t tlv_length = GUINT16_FROM_LE(tlv->length);
+
+		if (tlv->type == type) {
+			if (length)
+				*length = tlv_length;
+
+			return ptr + QMI_TLV_HDR_SIZE;
+		}
+
+		ptr += QMI_TLV_HDR_SIZE + tlv_length;
+		len -= QMI_TLV_HDR_SIZE + tlv_length;
+	}
+
+	return NULL;
+}
+
+struct discover_data {
+	struct qmi_device *device;
+	qmi_discover_func_t func;
+	void *user_data;
+	qmi_destroy_func_t destroy;
+	guint timeout;
+};
+
+static void discover_callback(uint16_t message, uint16_t length,
+					const void *buffer, void *user_data)
+{
+	struct discover_data *data = user_data;
+	struct qmi_device *device = data->device;
+	const struct qmi_result_code *result_code;
+	const struct qmi_service_list *service_list;
+	const void *ptr;
+	uint16_t len;
+	struct qmi_version *list;
+	uint8_t count;
+	unsigned int i;
+
+	g_source_remove(data->timeout);
+
+	count = 0;
+	list = NULL;
+
+	result_code = tlv_get(buffer, length, 0x02, &len);
+	if (!result_code)
+		goto done;
+
+	if (len != QMI_RESULT_CODE_SIZE)
+		goto done;
+
+	service_list = tlv_get(buffer, length, 0x01, &len);
+	if (!service_list)
+		goto done;
+
+	if (len < QMI_SERVICE_LIST_SIZE)
+		goto done;
+
+	list = g_try_malloc(sizeof(struct qmi_version) * service_list->count);
+	if (!list)
+		goto done;
+
+	for (i = 0; i < service_list->count; i++) {
+		uint16_t major =
+			GUINT16_FROM_LE(service_list->services[i].major);
+		uint16_t minor =
+			GUINT16_FROM_LE(service_list->services[i].minor);
+		uint8_t type = service_list->services[i].type;
+
+		if (type == QMI_SERVICE_CONTROL) {
+			device->control_major = major;
+			device->control_minor = minor;
+			continue;
+		}
+
+		list[count].type = type;
+		list[count].major = major;
+		list[count].minor = minor;
+		list[count].name = __service_type_to_string(type);
+
+		count++;
+	}
+
+	ptr = tlv_get(buffer, length, 0x10, &len);
+	if (!ptr)
+		goto done;
+
+	device->version_str = strndup(ptr + 1, *((uint8_t *) ptr));
+
+	service_list = ptr + *((uint8_t *) ptr) + 1;
+
+	for (i = 0; i < service_list->count; i++) {
+		if (service_list->services[i].type == QMI_SERVICE_CONTROL)
+			continue;
+	}
+
+done:
+	device->version_list = list;
+	device->version_count = count;
+
+	if (data->func)
+		data->func(count, list, data->user_data);
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	g_free(data);
+}
+
+static gboolean discover_reply(gpointer user_data)
+{
+	struct discover_data *data = user_data;
+	struct qmi_device *device = data->device;
+
+	data->timeout = 0;
+
+	if (data->func)
+		data->func(device->version_count,
+				device->version_list, data->user_data);
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	g_free(data);
+
+	return FALSE;
+}
+
+bool qmi_device_discover(struct qmi_device *device, qmi_discover_func_t func,
+				void *user_data, qmi_destroy_func_t destroy)
+{
+	struct discover_data *data;
+	struct qmi_request *req;
+	struct qmi_control_hdr *hdr;
+
+	if (!device)
+		return false;
+
+	__debug_device(device, "device %p discover", device);
+
+	data = g_try_new0(struct discover_data, 1);
+	if (!data)
+		return false;
+
+	data->device = device;
+	data->func = func;
+	data->user_data = user_data;
+	data->destroy = destroy;
+
+	if (device->version_list) {
+		g_timeout_add_seconds(0, discover_reply, data);
+		return true;
+	}
+
+	req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
+			QMI_CTL_GET_VERSION_INFO, QMI_CONTROL_HDR_SIZE,
+			NULL, 0, discover_callback, data, (void **) &hdr);
+	if (!req) {
+		g_free(data);
+		return false;
+	}
+
+	hdr->type = 0x00;
+	hdr->transaction = device->next_control_tid++;
+
+	__request_submit(device, req, hdr->transaction);
+
+	data->timeout = g_timeout_add_seconds(5, discover_reply, data);
+
+	return true;
+}
+
+static void release_client(struct qmi_device *device,
+				uint8_t type, uint8_t client_id,
+				qmi_message_func_t func, void *user_data)
+{
+	unsigned char release_req[] = { 0x01, 0x02, 0x00, type, client_id };
+	struct qmi_request *req;
+	struct qmi_control_hdr *hdr;
+
+	req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
+			QMI_CTL_RELEASE_CLIENT_ID, QMI_CONTROL_HDR_SIZE,
+			release_req, sizeof(release_req),
+			func, user_data, (void **) &hdr);
+	if (!req) {
+		func(0x0000, 0x0000, NULL, user_data);
+		return;
+	}
+
+	hdr->type = 0x00;
+	hdr->transaction = device->next_control_tid++;
+
+	__request_submit(device, req, hdr->transaction);
+}
+
+struct shutdown_data {
+	struct qmi_device *device;
+	qmi_shutdown_func_t func;
+	void *user_data;
+	qmi_destroy_func_t destroy;
+};
+
+static gboolean shutdown_reply(gpointer user_data)
+{
+	struct shutdown_data *data = user_data;
+
+	if (data->func)
+		data->func(data->user_data);
+
+	g_free(data);
+
+	return FALSE;
+}
+
+static gboolean shutdown_timeout(gpointer user_data)
+{
+	struct shutdown_data *data = user_data;
+	struct qmi_device *device = data->device;
+
+	if (device->release_users > 0)
+		return TRUE;
+
+	return shutdown_reply(data);
+}
+
+bool qmi_device_shutdown(struct qmi_device *device, qmi_shutdown_func_t func,
+				void *user_data, qmi_destroy_func_t destroy)
+{
+	struct shutdown_data *data;
+
+	if (!device)
+		return false;
+
+	__debug_device(device, "device %p shutdown", device);
+
+	data = g_try_new0(struct shutdown_data, 1);
+	if (!data)
+		return false;
+
+	data->device = device;
+	data->func = func;
+	data->user_data = user_data;
+	data->destroy = destroy;
+
+	if (device->release_users > 0)
+		g_timeout_add_seconds(0, shutdown_timeout, data);
+	else
+		g_timeout_add_seconds(0, shutdown_reply, data);
+
+	return true;
+}
+
+struct qmi_param *qmi_param_new(void)
+{
+	struct qmi_param *param;
+
+	param = g_try_new0(struct qmi_param, 1);
+	if (!param)
+		return NULL;
+
+	return param;
+}
+
+void qmi_param_free(struct qmi_param *param)
+{
+	if (!param)
+		return;
+
+	g_free(param->data);
+	g_free(param);
+}
+
+bool qmi_param_append(struct qmi_param *param, uint8_t type,
+					uint16_t length, const void *data)
+{
+	struct qmi_tlv_hdr *tlv;
+	void *ptr;
+
+	if (!param || !type)
+		return false;
+
+	if (!length)
+		return true;
+
+	if (!data)
+		return false;
+
+	if (param->data)
+		ptr = g_try_realloc(param->data,
+				param->length + QMI_TLV_HDR_SIZE + length);
+	else
+		ptr = g_try_malloc(QMI_TLV_HDR_SIZE + length);
+
+	if (!ptr)
+		return false;
+
+	tlv = ptr + param->length;
+
+	tlv->type = type;
+	tlv->length = GUINT16_TO_LE(length);
+	memcpy(tlv->value, data, length);
+
+	param->data = ptr;
+	param->length += QMI_TLV_HDR_SIZE + length;
+
+	return true;
+}
+
+bool qmi_param_append_uint8(struct qmi_param *param, uint8_t type,
+							uint8_t value)
+{
+	unsigned char buf[1] = { value };
+
+	return qmi_param_append(param, type, sizeof(buf), buf);
+}
+
+bool qmi_param_append_uint16(struct qmi_param *param, uint8_t type,
+							uint16_t value)
+{
+	unsigned char buf[2] = { value & 0xff, (value & 0xff00) >> 8 };
+
+	return qmi_param_append(param, type, sizeof(buf), buf);
+}
+
+bool qmi_param_append_uint32(struct qmi_param *param, uint8_t type,
+							uint32_t value)
+{
+	unsigned char buf[4] = { value & 0xff, (value & 0xff00) >> 8,
+					(value & 0xff0000) >> 16,
+					(value & 0xff000000) >> 24 };
+
+	return qmi_param_append(param, type, sizeof(buf), buf);
+}
+
+struct qmi_param *qmi_param_new_uint8(uint8_t type, uint8_t value)
+{
+	struct qmi_param *param;
+
+	param = qmi_param_new();
+	if (!param)
+		return NULL;
+
+	if (!qmi_param_append_uint8(param, type, value)) {
+		qmi_param_free(param);
+		return NULL;
+	}
+
+	return param;
+}
+
+struct qmi_param *qmi_param_new_uint16(uint8_t type, uint16_t value)
+{
+	struct qmi_param *param;
+
+	param = qmi_param_new();
+	if (!param)
+		return NULL;
+
+	if (!qmi_param_append_uint16(param, type, value)) {
+		qmi_param_free(param);
+		return NULL;
+	}
+
+	return param;
+}
+
+struct qmi_param *qmi_param_new_uint32(uint8_t type, uint32_t value)
+{
+	struct qmi_param *param;
+
+	param = qmi_param_new();
+	if (!param)
+		return NULL;
+
+	if (!qmi_param_append_uint32(param, type, value)) {
+		qmi_param_free(param);
+		return NULL;
+	}
+
+	return param;
+}
+
+bool qmi_result_set_error(struct qmi_result *result, uint16_t *error)
+{
+	if (!result) {
+		if (error)
+			*error = 0xffff;
+		return true;
+	}
+
+	if (result->result == 0x0000)
+		return false;
+
+	if (error)
+		*error = result->error;
+
+	return true;
+}
+
+const void *qmi_result_get(struct qmi_result *result, uint8_t type,
+							uint16_t *length)
+{
+	if (!result || !type)
+		return NULL;
+
+	return tlv_get(result->data, result->length, type, length);
+}
+
+char *qmi_result_get_string(struct qmi_result *result, uint8_t type)
+{
+	const void *ptr;
+	uint16_t len;
+
+	if (!result || !type)
+		return NULL;
+
+	ptr = tlv_get(result->data, result->length, type, &len);
+	if (!ptr)
+		return NULL;
+
+	return strndup(ptr, len);
+}
+
+bool qmi_result_get_uint8(struct qmi_result *result, uint8_t type,
+							uint8_t *value)
+{
+	const unsigned char *ptr;
+	uint16_t len;
+
+	if (!result || !type)
+		return false;
+
+	ptr = tlv_get(result->data, result->length, type, &len);
+	if (!ptr)
+		return false;
+
+	if (value)
+		*value = *ptr;
+
+	return true;
+}
+
+bool qmi_result_get_uint16(struct qmi_result *result, uint8_t type,
+							uint16_t *value)
+{
+	const unsigned char *ptr;
+	uint16_t len, tmp;
+
+	if (!result || !type)
+		return false;
+
+	ptr = tlv_get(result->data, result->length, type, &len);
+	if (!ptr)
+		return false;
+
+	memcpy(&tmp, ptr, 2);
+
+	if (value)
+		*value = GUINT16_FROM_LE(tmp);
+
+	return true;
+}
+
+bool qmi_result_get_uint32(struct qmi_result *result, uint8_t type,
+							uint32_t *value)
+{
+	const unsigned char *ptr;
+	uint16_t len;
+	uint32_t tmp;
+
+	if (!result || !type)
+		return false;
+
+	ptr = tlv_get(result->data, result->length, type, &len);
+	if (!ptr)
+		return false;
+
+	memcpy(&tmp, ptr, 4);
+
+	if (value)
+		*value = GUINT32_FROM_LE(tmp);
+
+	return true;
+}
+
+struct service_create_data {
+	struct qmi_device *device;
+	uint8_t type;
+	uint16_t major;
+	uint16_t minor;
+	qmi_create_func_t func;
+	void *user_data;
+	qmi_destroy_func_t destroy;
+	guint timeout;
+};
+
+static gboolean service_create_reply(gpointer user_data)
+{
+	struct service_create_data *data = user_data;
+
+	data->func(NULL, data->user_data);
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	g_free(data);
+
+	return FALSE;
+}
+
+static void service_create_callback(uint16_t message, uint16_t length,
+					const void *buffer, void *user_data)
+{
+	struct service_create_data *data = user_data;
+	struct qmi_device *device = data->device;
+	struct qmi_service *service = NULL;
+	const struct qmi_result_code *result_code;
+	const struct qmi_client_id *client_id;
+	uint16_t len, hash_id;
+
+	g_source_remove(data->timeout);
+
+	result_code = tlv_get(buffer, length, 0x02, &len);
+	if (!result_code)
+		goto done;
+
+	if (len != QMI_RESULT_CODE_SIZE)
+		goto done;
+
+	client_id = tlv_get(buffer, length, 0x01, &len);
+	if (!client_id)
+		goto done;
+
+	if (len != QMI_CLIENT_ID_SIZE)
+		goto done;
+
+	if (client_id->service != data->type)
+		goto done;
+
+	service = g_try_new0(struct qmi_service, 1);
+	if (!service)
+		goto done;
+
+	service->ref_count = 1;
+	service->device = data->device;
+
+	service->type = data->type;
+	service->major = data->major;
+	service->minor = data->minor;
+
+	service->client_id = client_id->client;
+
+	__debug_device(device, "service created [client=%d,type=%d]",
+					service->client_id, service->type);
+
+	hash_id = service->type | (service->client_id << 8);
+
+	g_hash_table_replace(device->service_list,
+				GUINT_TO_POINTER(hash_id), service);
+
+done:
+	data->func(service, data->user_data);
+
+	qmi_service_unref(service);
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	g_free(data);
+}
+
+static void service_create_discover(uint8_t count,
+			const struct qmi_version *list, void *user_data)
+{
+	struct service_create_data *data = user_data;
+	struct qmi_device *device = data->device;
+	struct qmi_request *req;
+	struct qmi_control_hdr *hdr;
+	unsigned char client_req[] = { 0x01, 0x01, 0x00, data->type };
+	unsigned int i;
+
+	__debug_device(device, "service create [type=%d]", data->type);
+
+	for (i = 0; i < count; i++) {
+		if (list[i].type == data->type) {
+			data->major = list[i].major;
+			data->minor = list[i].minor;
+			break;
+		}
+	}
+
+	req = __request_alloc(QMI_SERVICE_CONTROL, 0x00,
+			QMI_CTL_GET_CLIENT_ID, QMI_CONTROL_HDR_SIZE,
+			client_req, sizeof(client_req),
+			service_create_callback, data, (void **) &hdr);
+	if (!req) {
+		if (data->timeout > 0)
+			g_source_remove(data->timeout);
+
+		g_timeout_add_seconds(0, service_create_reply, data);
+		return;
+	}
+
+	hdr->type = 0x00;
+	hdr->transaction = device->next_control_tid++;
+
+	__request_submit(device, req, hdr->transaction);
+}
+
+bool qmi_service_create(struct qmi_device *device,
+				uint8_t type, qmi_create_func_t func,
+				void *user_data, qmi_destroy_func_t destroy)
+{
+	struct service_create_data *data;
+
+	if (!device || !func)
+		return false;
+
+	if (type == QMI_SERVICE_CONTROL)
+		return false;
+
+	data = g_try_new0(struct service_create_data, 1);
+	if (!data)
+		return false;
+
+	data->device = device;
+	data->type = type;
+	data->func = func;
+	data->user_data = user_data;
+
+	if (device->version_list) {
+		service_create_discover(device->version_count,
+						device->version_list, data);
+		goto done;
+	}
+
+	if (qmi_device_discover(device, service_create_discover, data, NULL))
+		goto done;
+
+	g_free(data);
+
+	return false;
+
+done:
+	data->timeout = g_timeout_add_seconds(8, service_create_reply, data);
+
+	return true;
+}
+
+static void service_release_callback(uint16_t message, uint16_t length,
+					const void *buffer, void *user_data)
+{
+	struct qmi_service *service = user_data;
+
+	if (service->device)
+		service->device->release_users--;
+
+	g_free(service);
+}
+
+struct qmi_service *qmi_service_ref(struct qmi_service *service)
+{
+	if (!service)
+		return NULL;
+
+	__sync_fetch_and_add(&service->ref_count, 1);
+
+	return service;
+}
+
+void qmi_service_unref(struct qmi_service *service)
+{
+	uint16_t hash_id;
+
+	if (!service)
+                return;
+
+	if (__sync_sub_and_fetch(&service->ref_count, 1))
+		return;
+
+	if (!service->device) {
+		g_free(service);
+		return;
+	}
+
+	hash_id = service->type | (service->client_id << 8);
+
+	g_hash_table_steal(service->device->service_list,
+					GUINT_TO_POINTER(hash_id));
+
+	service->device->release_users++;
+
+	release_client(service->device, service->type, service->client_id,
+					service_release_callback, service);
+}
+
+const char *qmi_service_get_identifier(struct qmi_service *service)
+{
+	if (!service)
+		return NULL;
+
+	return __service_type_to_string(service->type);
+}
+
+bool qmi_service_get_version(struct qmi_service *service,
+					uint16_t *major, uint16_t *minor)
+{
+	if (!service)
+		return false;
+
+	if (major)
+		*major = service->major;
+
+	if (minor)
+		*minor = service->minor;
+
+	return true;
+}
+
+struct service_send_data {
+	struct qmi_service *service;
+	struct qmi_param *param;
+	qmi_result_func_t func;
+	void *user_data;
+	qmi_destroy_func_t destroy;
+};
+
+static void service_send_callback(uint16_t message, uint16_t length,
+					const void *buffer, void *user_data)
+{
+	struct service_send_data *data = user_data;
+	const struct qmi_result_code *result_code;
+	uint16_t len;
+	struct qmi_result result;
+
+	result.message = message;
+	result.data = buffer;
+	result.length = length;
+
+	result_code = tlv_get(buffer, length, 0x02, &len);
+	if (!result_code)
+		goto done;
+
+	if (len != QMI_RESULT_CODE_SIZE)
+		goto done;
+
+	result.result = GUINT16_FROM_LE(result_code->result);
+	result.error = GUINT16_FROM_LE(result_code->error);
+
+done:
+	if (data->func)
+		data->func(&result, data->user_data);
+
+	if (data->destroy)
+		data->destroy(data->user_data);
+
+	qmi_param_free(data->param);
+	g_free(data);
+}
+
+unsigned int qmi_service_send(struct qmi_service *service,
+				uint16_t message, struct qmi_param *param,
+				qmi_result_func_t func,
+				void *user_data, qmi_destroy_func_t destroy)
+{
+	struct qmi_device *device;
+	struct service_send_data *data;
+	struct qmi_request *req;
+	struct qmi_service_hdr *hdr;
+
+	if (!service)
+		return 0;
+
+	if (!service->client_id)
+		return 0;
+
+	device = service->device;
+	if (!device)
+		return 0;
+
+	data = g_try_new0(struct service_send_data, 1);
+	if (!data)
+		return 0;
+
+	data->service = service;
+	data->param = param;
+	data->func = func;
+	data->user_data = user_data;
+	data->destroy = destroy;
+
+	req = __request_alloc(service->type, service->client_id,
+				message, QMI_SERVICE_HDR_SIZE,
+				data->param ? data->param->data : NULL,
+				data->param ? data->param->length : 0,
+				service_send_callback, data, (void **) &hdr);
+	if (!req) {
+		g_free(data);
+		return 0;
+	}
+
+	hdr->type = 0x00;
+	hdr->transaction = device->next_service_tid++;
+
+	__request_submit(device, req, hdr->transaction);
+
+	return req->id;
+}
+
+unsigned int qmi_service_register(struct qmi_service *service,
+				uint16_t message, qmi_result_func_t func,
+				void *user_data, qmi_destroy_func_t destroy)
+{
+	struct qmi_notify *notify;
+
+	if (!service || !func)
+		return 0;
+
+	notify = g_try_new0(struct qmi_notify, 1);
+	if (!notify)
+		return 0;
+
+	notify->id = service->device->next_id++;
+	notify->message = message;
+	notify->callback = func;
+	notify->user_data = user_data;
+	notify->destroy = destroy;
+
+	service->notify_list = g_list_append(service->notify_list, notify);
+
+	return notify->id;
+}
+
+bool qmi_service_unregister_all(struct qmi_service *service)
+{
+	if (!service)
+		return false;
+
+	g_list_foreach(service->notify_list, __notify_free, NULL);
+	g_list_free(service->notify_list);
+
+	service->notify_list = NULL;
+
+	return true;
+}