bluez/src/shared/hfp.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
 *
 *
 *  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.1 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 St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

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

#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdarg.h>
#include <ctype.h>

#include "src/shared/util.h"
#include "src/shared/ringbuf.h"
#include "src/shared/queue.h"
#include "src/shared/io.h"
#include "src/shared/hfp.h"

struct hfp_gw {
	int ref_count;
	int fd;
	bool close_on_unref;
	struct io *io;
	struct ringbuf *read_buf;
	struct ringbuf *write_buf;
	struct queue *cmd_handlers;
	bool writer_active;
	bool permissive_syntax;
	bool result_pending;
	hfp_command_func_t command_callback;
	hfp_destroy_func_t command_destroy;
	void *command_data;
	hfp_debug_func_t debug_callback;
	hfp_destroy_func_t debug_destroy;
	void *debug_data;

	hfp_disconnect_func_t disconnect_callback;
	hfp_destroy_func_t disconnect_destroy;
	void *disconnect_data;

	bool in_disconnect;
	bool destroyed;
};

struct cmd_handler {
	char *prefix;
	void *user_data;
	hfp_destroy_func_t destroy;
	hfp_result_func_t callback;
};

struct hfp_gw_result {
	const char *data;
	unsigned int offset;
};

static void destroy_cmd_handler(void *data)
{
	struct cmd_handler *handler = data;

	if (handler->destroy)
		handler->destroy(handler->user_data);

	free(handler->prefix);

	free(handler);
}

static bool match_handler_prefix(const void *a, const void *b)
{
	const struct cmd_handler *handler = a;
	const char *prefix = b;

	if (strlen(handler->prefix) != strlen(prefix))
		return false;

	if (memcmp(handler->prefix, prefix, strlen(prefix)))
		return false;

	return true;
}

static void write_watch_destroy(void *user_data)
{
	struct hfp_gw *hfp = user_data;

	hfp->writer_active = false;
}

static bool can_write_data(struct io *io, void *user_data)
{
	struct hfp_gw *hfp = user_data;
	ssize_t bytes_written;

	bytes_written = ringbuf_write(hfp->write_buf, hfp->fd);
	if (bytes_written < 0)
		return false;

	if (ringbuf_len(hfp->write_buf) > 0)
		return true;

	return false;
}

static void wakeup_writer(struct hfp_gw *hfp)
{
	if (hfp->writer_active)
		return;

	if (!ringbuf_len(hfp->write_buf))
		return;

	if (!io_set_write_handler(hfp->io, can_write_data,
					hfp, write_watch_destroy))
		return;

	hfp->writer_active = true;
}

static void skip_whitespace(struct hfp_gw_result *result)
{
	while (result->data[result->offset] == ' ')
		result->offset++;
}

static bool call_prefix_handler(struct hfp_gw *hfp, const char *data)
{
	struct cmd_handler *handler;
	const char *separators = ";?=\0";
	struct hfp_gw_result result;
	enum hfp_gw_cmd_type type;
	char lookup_prefix[18];
	uint8_t pref_len = 0;
	const char *prefix;
	int i;

	result.offset = 0;
	result.data = data;

	skip_whitespace(&result);

	if (strlen(data + result.offset) < 3)
		return false;

	if (strncmp(data + result.offset, "AT", 2))
		if (strncmp(data + result.offset, "at", 2))
			return false;

	result.offset += 2;
	prefix = data + result.offset;

	if (isalpha(prefix[0])) {
		lookup_prefix[pref_len++] = toupper(prefix[0]);
	} else {
		pref_len = strcspn(prefix, separators);
		if (pref_len > 17 || pref_len < 2)
			return false;

		for (i = 0; i < pref_len; i++)
			lookup_prefix[i] = toupper(prefix[i]);
	}

	lookup_prefix[pref_len] = '\0';
	result.offset += pref_len;

	if (lookup_prefix[0] == 'D') {
		type = HFP_GW_CMD_TYPE_SET;
		goto done;
	}

	if (data[result.offset] == '=') {
		result.offset++;
		if (data[result.offset] == '?') {
			result.offset++;
			type = HFP_GW_CMD_TYPE_TEST;
		} else {
			type = HFP_GW_CMD_TYPE_SET;
		}
		goto done;
	}

	if (data[result.offset] == '?') {
		result.offset++;
		type = HFP_GW_CMD_TYPE_READ;
		goto done;
	}

	type = HFP_GW_CMD_TYPE_COMMAND;

done:

	handler = queue_find(hfp->cmd_handlers, match_handler_prefix,
								lookup_prefix);
	if (!handler)
		return false;

	handler->callback(&result, type, handler->user_data);

	return true;
}

static void next_field(struct hfp_gw_result *result)
{
	if (result->data[result->offset] == ',')
		result->offset++;
}

bool hfp_gw_result_get_number_default(struct hfp_gw_result *result,
						unsigned int *val,
						unsigned int default_val)
{
	skip_whitespace(result);

	if (result->data[result->offset] == ',') {
		if (val)
			*val = default_val;

		result->offset++;
		return true;
	}

	return hfp_gw_result_get_number(result, val);
}

bool hfp_gw_result_get_number(struct hfp_gw_result *result, unsigned int *val)
{
	unsigned int i;
	int tmp = 0;

	skip_whitespace(result);

	i = result->offset;

	while (result->data[i] >= '0' && result->data[i] <= '9')
		tmp = tmp * 10 + result->data[i++] - '0';

	if (i == result->offset)
		return false;

	if (val)
		*val = tmp;
	result->offset = i;

	skip_whitespace(result);
	next_field(result);

	return true;
}

bool hfp_gw_result_open_container(struct hfp_gw_result *result)
{
	skip_whitespace(result);

	/* The list shall be preceded by a left parenthesis "(") */
	if (result->data[result->offset] != '(')
		return false;

	result->offset++;

	return true;
}

bool hfp_gw_result_close_container(struct hfp_gw_result *result)
{
	skip_whitespace(result);

	/* The list shall be followed by a right parenthesis (")" V250 5.7.3.1*/
	if (result->data[result->offset] != ')')
		return false;

	result->offset++;

	return true;
}

bool hfp_gw_result_get_string(struct hfp_gw_result *result, char *buf,
								uint8_t len)
{
	int i = 0;
	const char *data = result->data;
	unsigned int offset;

	skip_whitespace(result);

	if (data[result->offset] != '"')
		return false;

	offset = result->offset;
	offset++;

	while (data[offset] != '\0' && data[offset] != '"') {
		if (i == len)
			return false;

		buf[i++] = data[offset];
		offset++;
	}

	if (i == len)
		return false;

	buf[i] = '\0';

	if (data[offset] == '"')
		offset++;
	else
		return false;

	result->offset = offset;

	skip_whitespace(result);
	next_field(result);

	return true;
}

bool hfp_gw_result_get_unquoted_string(struct hfp_gw_result *result, char *buf,
								uint8_t len)
{
	const char *data = result->data;
	unsigned int offset;
	int i = 0;
	char c;

	skip_whitespace(result);

	c = data[result->offset];
	if (c == '"' || c == ')' || c == '(')
		return false;

	offset = result->offset;

	while (data[offset] != '\0' && data[offset] != ',' &&
							data[offset] != ')') {
		if (i == len)
			return false;

		buf[i++] = data[offset];
		offset++;
	}

	if (i == len)
		return false;

	buf[i] = '\0';

	result->offset = offset;

	next_field(result);

	return true;
}

bool hfp_gw_result_has_next(struct hfp_gw_result *result)
{
	return result->data[result->offset] != '\0';
}

static void process_input(struct hfp_gw *hfp)
{
	char *str, *ptr;
	size_t len, count;

	str = ringbuf_peek(hfp->read_buf, 0, &len);
	if (!str)
		return;

	ptr = memchr(str, '\r', len);
	if (!ptr) {
		char *str2;
		size_t len2;

		/* If there is no more data in ringbuffer,
		 * it's just an incomplete command.
		 */
		if (len == ringbuf_len(hfp->read_buf))
			return;

		str2 = ringbuf_peek(hfp->read_buf, len, &len2);
		if (!str2)
			return;

		ptr = memchr(str2, '\r', len2);
		if (!ptr)
			return;

		*ptr = '\0';
		count = asprintf(&ptr, "%s%s", str, str2);
		str = ptr;
	} else {
		count = ptr - str;
		*ptr = '\0';
	}

	hfp->result_pending = true;

	if (!call_prefix_handler(hfp, str)) {
		if (hfp->command_callback)
			hfp->command_callback(str, hfp->command_data);
		else
			hfp_gw_send_result(hfp, HFP_RESULT_ERROR);
	}

	len = ringbuf_drain(hfp->read_buf, count + 1);

	if (str == ptr)
		free(ptr);
}

static void read_watch_destroy(void *user_data)
{
}

static bool can_read_data(struct io *io, void *user_data)
{
	struct hfp_gw *hfp = user_data;
	ssize_t bytes_read;

	bytes_read = ringbuf_read(hfp->read_buf, hfp->fd);
	if (bytes_read < 0)
		return false;

	if (hfp->result_pending)
		return true;

	process_input(hfp);

	return true;
}

struct hfp_gw *hfp_gw_new(int fd)
{
	struct hfp_gw *hfp;

	if (fd < 0)
		return NULL;

	hfp = new0(struct hfp_gw, 1);
	if (!hfp)
		return NULL;

	hfp->fd = fd;
	hfp->close_on_unref = false;

	hfp->read_buf = ringbuf_new(4096);
	if (!hfp->read_buf) {
		free(hfp);
		return NULL;
	}

	hfp->write_buf = ringbuf_new(4096);
	if (!hfp->write_buf) {
		ringbuf_free(hfp->read_buf);
		free(hfp);
		return NULL;
	}

	hfp->io = io_new(fd);
	if (!hfp->io) {
		ringbuf_free(hfp->write_buf);
		ringbuf_free(hfp->read_buf);
		free(hfp);
		return NULL;
	}

	hfp->cmd_handlers = queue_new();
	if (!hfp->cmd_handlers) {
		io_destroy(hfp->io);
		ringbuf_free(hfp->write_buf);
		ringbuf_free(hfp->read_buf);
		free(hfp);
		return NULL;
	}

	if (!io_set_read_handler(hfp->io, can_read_data,
					hfp, read_watch_destroy)) {
		queue_destroy(hfp->cmd_handlers,
						destroy_cmd_handler);
		io_destroy(hfp->io);
		ringbuf_free(hfp->write_buf);
		ringbuf_free(hfp->read_buf);
		free(hfp);
		return NULL;
	}

	hfp->writer_active = false;
	hfp->permissive_syntax = false;
	hfp->result_pending = false;

	return hfp_gw_ref(hfp);
}

struct hfp_gw *hfp_gw_ref(struct hfp_gw *hfp)
{
	if (!hfp)
		return NULL;

	__sync_fetch_and_add(&hfp->ref_count, 1);

	return hfp;
}

void hfp_gw_unref(struct hfp_gw *hfp)
{
	if (!hfp)
		return;

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

	hfp_gw_set_command_handler(hfp, NULL, NULL, NULL);

	io_set_write_handler(hfp->io, NULL, NULL, NULL);
	io_set_read_handler(hfp->io, NULL, NULL, NULL);
	io_set_disconnect_handler(hfp->io, NULL, NULL, NULL);

	io_destroy(hfp->io);
	hfp->io = NULL;

	if (hfp->close_on_unref)
		close(hfp->fd);

	hfp_gw_set_debug(hfp, NULL, NULL, NULL);

	ringbuf_free(hfp->read_buf);
	hfp->read_buf = NULL;

	ringbuf_free(hfp->write_buf);
	hfp->write_buf = NULL;

	queue_destroy(hfp->cmd_handlers, destroy_cmd_handler);
	hfp->cmd_handlers = NULL;

	if (!hfp->in_disconnect) {
		free(hfp);
		return;
	}

	hfp->destroyed = true;
}

static void read_tracing(const void *buf, size_t count, void *user_data)
{
	struct hfp_gw *hfp = user_data;

	util_hexdump('>', buf, count, hfp->debug_callback, hfp->debug_data);
}

static void write_tracing(const void *buf, size_t count, void *user_data)
{
	struct hfp_gw *hfp = user_data;

	util_hexdump('<', buf, count, hfp->debug_callback, hfp->debug_data);
}

bool hfp_gw_set_debug(struct hfp_gw *hfp, hfp_debug_func_t callback,
				void *user_data, hfp_destroy_func_t destroy)
{
	if (!hfp)
		return false;

	if (hfp->debug_destroy)
		hfp->debug_destroy(hfp->debug_data);

	hfp->debug_callback = callback;
	hfp->debug_destroy = destroy;
	hfp->debug_data = user_data;

	if (hfp->debug_callback) {
		ringbuf_set_input_tracing(hfp->read_buf, read_tracing, hfp);
		ringbuf_set_input_tracing(hfp->write_buf, write_tracing, hfp);
	} else {
		ringbuf_set_input_tracing(hfp->read_buf, NULL, NULL);
		ringbuf_set_input_tracing(hfp->write_buf, NULL, NULL);
	}

	return true;
}

bool hfp_gw_set_close_on_unref(struct hfp_gw *hfp, bool do_close)
{
	if (!hfp)
		return false;

	hfp->close_on_unref = do_close;

	return true;
}

bool hfp_gw_set_permissive_syntax(struct hfp_gw *hfp, bool permissive)
{
	if (!hfp)
		return false;

	hfp->permissive_syntax = permissive;

	return true;
}

bool hfp_gw_send_result(struct hfp_gw *hfp, enum hfp_result result)
{
	const char *str;

	if (!hfp)
		return false;

	switch (result) {
	case HFP_RESULT_OK:
		str = "OK";
		break;
	case HFP_RESULT_ERROR:
		str = "ERROR";
		break;
	default:
		return false;
	}

	if (ringbuf_printf(hfp->write_buf, "\r\n%s\r\n", str) < 0)
		return false;

	wakeup_writer(hfp);

	hfp->result_pending = false;

	return true;
}

bool hfp_gw_send_error(struct hfp_gw *hfp, enum hfp_error error)
{
	if (!hfp)
		return false;

	if (ringbuf_printf(hfp->write_buf, "\r\n+CME ERROR: %u\r\n", error) < 0)
		return false;

	wakeup_writer(hfp);

	hfp->result_pending = false;

	return true;
}

bool hfp_gw_send_info(struct hfp_gw *hfp, const char *format, ...)
{
	va_list ap;
	char *fmt;
	int len;

	if (!hfp || !format)
		return false;

	if (asprintf(&fmt, "\r\n%s\r\n", format) < 0)
		return false;

	va_start(ap, format);
	len = ringbuf_vprintf(hfp->write_buf, fmt, ap);
	va_end(ap);

	free(fmt);

	if (len < 0)
		return false;

	if (hfp->result_pending)
		return true;

	wakeup_writer(hfp);

	return true;
}

bool hfp_gw_set_command_handler(struct hfp_gw *hfp,
				hfp_command_func_t callback,
				void *user_data, hfp_destroy_func_t destroy)
{
	if (!hfp)
		return false;

	if (hfp->command_destroy)
		hfp->command_destroy(hfp->command_data);

	hfp->command_callback = callback;
	hfp->command_destroy = destroy;
	hfp->command_data = user_data;

	return true;
}

bool hfp_gw_register(struct hfp_gw *hfp, hfp_result_func_t callback,
						const char *prefix,
						void *user_data,
						hfp_destroy_func_t destroy)
{
	struct cmd_handler *handler;

	handler = new0(struct cmd_handler, 1);
	if (!handler)
		return false;

	handler->callback = callback;
	handler->user_data = user_data;

	handler->prefix = strdup(prefix);
	if (!handler->prefix) {
		free(handler);
		return false;
	}

	if (queue_find(hfp->cmd_handlers, match_handler_prefix,
							handler->prefix)) {
		destroy_cmd_handler(handler);
		return false;
	}

	handler->destroy = destroy;

	return queue_push_tail(hfp->cmd_handlers, handler);
}

bool hfp_gw_unregister(struct hfp_gw *hfp, const char *prefix)
{
	struct cmd_handler *handler;
	char *lookup_prefix;

	lookup_prefix = strdup(prefix);
	if (!lookup_prefix)
		return false;

	handler = queue_remove_if(hfp->cmd_handlers, match_handler_prefix,
								lookup_prefix);
	free(lookup_prefix);

	if (!handler)
		return false;

	destroy_cmd_handler(handler);

	return true;
}

static void disconnect_watch_destroy(void *user_data)
{
	struct hfp_gw *hfp = user_data;

	if (hfp->disconnect_destroy)
		hfp->disconnect_destroy(hfp->disconnect_data);

	if (hfp->destroyed)
		free(hfp);
}

static bool io_disconnected(struct io *io, void *user_data)
{
	struct hfp_gw *hfp = user_data;

	hfp->in_disconnect = true;

	if (hfp->disconnect_callback)
		hfp->disconnect_callback(hfp->disconnect_data);

	hfp->in_disconnect = false;

	return false;
}

bool hfp_gw_set_disconnect_handler(struct hfp_gw *hfp,
					hfp_disconnect_func_t callback,
					void *user_data,
					hfp_destroy_func_t destroy)
{
	if (!hfp)
		return false;

	if (hfp->disconnect_destroy)
		hfp->disconnect_destroy(hfp->disconnect_data);

	if (!io_set_disconnect_handler(hfp->io, io_disconnected, hfp,
						disconnect_watch_destroy)) {
		hfp->disconnect_callback = NULL;
		hfp->disconnect_destroy = NULL;
		hfp->disconnect_data = NULL;
		return false;
	}

	hfp->disconnect_callback = callback;
	hfp->disconnect_destroy = destroy;
	hfp->disconnect_data = user_data;

	return true;
}

bool hfp_gw_disconnect(struct hfp_gw *hfp)
{
	if (!hfp)
		return false;

	return io_shutdown(hfp->io);
}