ofono/drivers/qmimodem/qmi.c

/*
 *
 *  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;
}