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 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 hfp_hf {
	int ref_count;
	int fd;
	bool close_on_unref;
	struct io *io;
	struct ringbuf *read_buf;
	struct ringbuf *write_buf;

	bool writer_active;
	struct queue *cmd_queue;

	struct queue *event_handlers;

	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_context {
	const char *data;
	unsigned int offset;
};

struct cmd_response {
	hfp_response_func_t resp_cb;
	struct hfp_context *response;
	char *resp_data;
	void *user_data;
};

struct event_handler {
	char *prefix;
	void *user_data;
	hfp_destroy_func_t destroy;
	hfp_hf_result_func_t callback;
};

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 (strcmp(handler->prefix, prefix) != 0)
		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_context *context)
{
	while (context->data[context->offset] == ' ')
		context->offset++;
}

static void handle_unknown_at_command(struct hfp_gw *hfp,
							const char *data)
{
	if (hfp->command_callback) {
		hfp->result_pending = true;
		hfp->command_callback(data, hfp->command_data);
	} else {
		hfp_gw_send_result(hfp, HFP_RESULT_ERROR);
	}
}

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

	context.offset = 0;
	context.data = data;

	skip_whitespace(&context);

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

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

	context.offset += 2;
	prefix = data + context.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';
	context.offset += pref_len;

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

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

	if (data[context.offset] == '?') {
		context.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) {
		handle_unknown_at_command(hfp, data);
		return true;
	}

	hfp->result_pending = true;
	handler->callback(&context, type, handler->user_data);

	return true;
}

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

bool hfp_context_get_number_default(struct hfp_context *context,
						unsigned int *val,
						unsigned int default_val)
{
	skip_whitespace(context);

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

		context->offset++;
		return true;
	}

	return hfp_context_get_number(context, val);
}

bool hfp_context_get_number(struct hfp_context *context,
							unsigned int *val)
{
	unsigned int i;
	int tmp = 0;

	skip_whitespace(context);

	i = context->offset;

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

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

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

	skip_whitespace(context);
	next_field(context);

	return true;
}

bool hfp_context_open_container(struct hfp_context *context)
{
	skip_whitespace(context);

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

	context->offset++;

	return true;
}

bool hfp_context_close_container(struct hfp_context *context)
{
	skip_whitespace(context);

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

	context->offset++;

	next_field(context);

	return true;
}

bool hfp_context_get_string(struct hfp_context *context, char *buf,
								uint8_t len)
{
	int i = 0;
	const char *data = context->data;
	unsigned int offset;

	skip_whitespace(context);

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

	offset = context->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;

	context->offset = offset;

	skip_whitespace(context);
	next_field(context);

	return true;
}

bool hfp_context_get_unquoted_string(struct hfp_context *context,
							char *buf, uint8_t len)
{
	const char *data = context->data;
	unsigned int offset;
	int i = 0;
	char c;

	skip_whitespace(context);

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

	offset = context->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';

	context->offset = offset;

	next_field(context);

	return true;
}

bool hfp_context_has_next(struct hfp_context *context)
{
	return context->data[context->offset] != '\0';
}

void hfp_context_skip_field(struct hfp_context *context)
{
	const char *data = context->data;
	unsigned int offset = context->offset;

	while (data[offset] != '\0' && data[offset] != ',')
		offset++;

	context->offset = offset;
	next_field(context);
}

bool hfp_context_get_range(struct hfp_context *context, uint32_t *min,
								uint32_t *max)
{
	uint32_t l, h;
	uint32_t start;

	start = context->offset;

	if (!hfp_context_get_number(context, &l))
		goto failed;

	if (context->data[context->offset] != '-')
		goto failed;

	context->offset++;

	if (!hfp_context_get_number(context, &h))
		goto failed;

	*min = l;
	*max = h;

	next_field(context);

	return true;

failed:
	context->offset = start;
	return false;
}

static void process_input(struct hfp_gw *hfp)
{
	char *str, *ptr;
	size_t len, count;
	bool free_ptr = false;
	bool read_again;

	do {
		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 = len2 + len;
			ptr = malloc(count);
			if (!ptr)
				return;

			memcpy(ptr, str, len);
			memcpy(ptr + len, str2, len2);

			free_ptr = true;
			str = ptr;
		} else {
			count = ptr - str;
			*ptr = '\0';
		}

		if (!handle_at_command(hfp, str))
			/*
			 * Command is not handled that means that was some
			 * trash. Let's skip that and keep reading from ring
			 * buffer.
			 */
			read_again = true;
		else
			/*
			 * Command has been handled. If we are waiting for a
			 * result from upper layer, we can stop reading. If we
			 * already reply i.e. ERROR on unknown command, then we
			 * can keep reading ring buffer. Actually ring buffer
			 * should be empty but lets just look there.
			 */
			read_again = !hfp->result_pending;

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

		if (free_ptr)
			free(ptr);

	} while (read_again);
}

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);
	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 (!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->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_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;
	case HFP_RESULT_RING:
	case HFP_RESULT_NO_CARRIER:
	case HFP_RESULT_BUSY:
	case HFP_RESULT_NO_ANSWER:
	case HFP_RESULT_DELAYED:
	case HFP_RESULT_BLACKLISTED:
	case HFP_RESULT_CME_ERROR:
	case HFP_RESULT_NO_DIALTONE:
	case HFP_RESULT_CONNECT:
	default:
		return false;
	}

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

	wakeup_writer(hfp);

	/*
	 * There might be already something to read in the ring buffer.
	 * If so, let's read it.
	 */
	if (hfp->result_pending) {
		hfp->result_pending = false;
		process_input(hfp);
	}

	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);

	/*
	 * There might be already something to read in the ring buffer.
	 * If so, let's read it.
	 */
	if (hfp->result_pending) {
		hfp->result_pending = false;
		process_input(hfp);
	}

	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);
	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);
}

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

	if (strcmp(handler->prefix, prefix) != 0)
		return false;

	return true;
}

static void destroy_event_handler(void *data)
{
	struct event_handler *handler = data;

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

	free(handler->prefix);

	free(handler);
}

static bool hf_can_write_data(struct io *io, void *user_data)
{
	struct hfp_hf *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 hf_write_watch_destroy(void *user_data)
{
	struct hfp_hf *hfp = user_data;

	hfp->writer_active = false;
}

static void hf_skip_whitespace(struct hfp_context *context)
{
	while (context->data[context->offset] == ' ')
		context->offset++;
}

static bool is_response(const char *prefix, enum hfp_result *result,
						enum hfp_error *cme_err,
						struct hfp_context *context)
{
	if (strcmp(prefix, "OK") == 0) {
		*result = HFP_RESULT_OK;
		/*
		 * Set cme_err to 0 as this is not valid when result is not
		 * CME ERROR
		 */
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "ERROR") == 0) {
		*result = HFP_RESULT_ERROR;
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "NO CARRIER") == 0) {
		*result = HFP_RESULT_NO_CARRIER;
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "NO ANSWER") == 0) {
		*result = HFP_RESULT_NO_ANSWER;
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "BUSY") == 0) {
		*result = HFP_RESULT_BUSY;
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "DELAYED") == 0) {
		*result = HFP_RESULT_DELAYED;
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "BLACKLISTED") == 0) {
		*result = HFP_RESULT_BLACKLISTED;
		*cme_err = 0;
		return true;
	}

	if (strcmp(prefix, "+CME ERROR") == 0) {
		uint32_t val;

		*result = HFP_RESULT_CME_ERROR;

		if (hfp_context_get_number(context, &val) &&
					val <= HFP_ERROR_NETWORK_NOT_ALLOWED)
			*cme_err = val;
		else
			*cme_err = HFP_ERROR_AG_FAILURE;

		return true;
	}

	return false;
}

static void hf_wakeup_writer(struct hfp_hf *hfp)
{
	if (hfp->writer_active)
		return;

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

	if (!io_set_write_handler(hfp->io, hf_can_write_data,
					hfp, hf_write_watch_destroy))
		return;

	hfp->writer_active = true;
}

static void hf_call_prefix_handler(struct hfp_hf *hfp, const char *data)
{
	struct event_handler *handler;
	const char *separators = ";:\0";
	struct hfp_context context;
	enum hfp_result result;
	enum hfp_error cme_err;
	char lookup_prefix[18];
	uint8_t pref_len = 0;
	const char *prefix;
	int i;

	context.offset = 0;
	context.data = data;

	hf_skip_whitespace(&context);

	if (strlen(data + context.offset) < 2)
		return;

	prefix = data + context.offset;

	pref_len = strcspn(prefix, separators);
	if (pref_len > 17 || pref_len < 2)
		return;

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

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

	if (is_response(lookup_prefix, &result, &cme_err, &context)) {
		struct cmd_response *cmd;

		cmd = queue_peek_head(hfp->cmd_queue);
		if (!cmd)
			return;

		cmd->resp_cb(result, cme_err, cmd->user_data);

		queue_remove(hfp->cmd_queue, cmd);
		free(cmd);

		hf_wakeup_writer(hfp);
		return;
	}

	handler = queue_find(hfp->event_handlers, match_handler_event_prefix,
								lookup_prefix);
	if (!handler)
		return;

	handler->callback(&context, handler->user_data);
}

static char *find_cr_lf(char *str, size_t len)
{
	char *ptr;
	size_t count, offset;

	offset = 0;

	ptr = memchr(str, '\r', len);
	while (ptr) {
		/*
		 * Check if there is more data after '\r'. If so check for
		 * '\n'
		 */
		count = ptr - str;
		if ((count < (len - 1)) && *(ptr + 1) == '\n')
			return ptr;

		/* There is only '\r'? Let's try to find next one */
		offset += count + 1;

		if (offset >= len)
			return NULL;

		ptr = memchr(str + offset, '\r', len - offset);
	}

	return NULL;
}

static void hf_process_input(struct hfp_hf *hfp)
{
	char *str, *ptr, *str2, *tmp;
	size_t len, count, offset, len2;
	bool free_tmp = false;

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

	offset = 0;

	ptr = find_cr_lf(str, len);
	while (ptr) {
		count = ptr - (str + offset);
		if (count == 0) {
			/* 2 is for <cr><lf> */
			offset += 2;
		} else {
			*ptr = '\0';
			hf_call_prefix_handler(hfp, str + offset);
			offset += count + 2;
		}

		ptr = find_cr_lf(str + offset, len - offset);
	}

	/*
	 * Just check if there is no wrapped data in ring buffer.
	 * Should not happen too often
	 */
	if (len == ringbuf_len(hfp->read_buf))
		goto done;

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

	ptr = find_cr_lf(str2, len2);
	if (!ptr) {
		/* Might happen that we wrap between \r and \n */
		ptr = memchr(str2, '\n', len2);
		if (!ptr)
			goto done;
	}

	count = ptr - str2;

	if (count) {
		*ptr = '\0';

		tmp = malloc(len + count);
		if (!tmp)
			goto done;

		/* "str" here is not a string so we need to use memcpy */
		memcpy(tmp, str, len);
		memcpy(tmp + len, str2, count);

		free_tmp = true;
	} else {
		str[len-1] = '\0';
		tmp = str;
	}

	hf_call_prefix_handler(hfp, tmp);
	offset += count;

done:
	ringbuf_drain(hfp->read_buf, offset);

	if (free_tmp)
		free(tmp);
}

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

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

	hf_process_input(hfp);

	return true;
}

struct hfp_hf *hfp_hf_new(int fd)
{
	struct hfp_hf *hfp;

	if (fd < 0)
		return NULL;

	hfp = new0(struct hfp_hf, 1);
	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->event_handlers = queue_new();
	hfp->cmd_queue = queue_new();
	hfp->writer_active = false;

	if (!io_set_read_handler(hfp->io, hf_can_read_data, hfp,
							read_watch_destroy)) {
		queue_destroy(hfp->event_handlers,
						destroy_event_handler);
		io_destroy(hfp->io);
		ringbuf_free(hfp->write_buf);
		ringbuf_free(hfp->read_buf);
		free(hfp);
		return NULL;
	}

	return hfp_hf_ref(hfp);
}

struct hfp_hf *hfp_hf_ref(struct hfp_hf *hfp)
{
	if (!hfp)
		return NULL;

	__sync_fetch_and_add(&hfp->ref_count, 1);

	return hfp;
}

void hfp_hf_unref(struct hfp_hf *hfp)
{
	if (!hfp)
		return;

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

	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_hf_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->event_handlers, destroy_event_handler);
	hfp->event_handlers = NULL;

	queue_destroy(hfp->cmd_queue, free);
	hfp->cmd_queue = NULL;

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

	hfp->destroyed = true;
}

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

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

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

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

bool hfp_hf_set_debug(struct hfp_hf *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, hf_read_tracing, hfp);
		ringbuf_set_input_tracing(hfp->write_buf, hf_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_hf_set_close_on_unref(struct hfp_hf *hfp, bool do_close)
{
	if (!hfp)
		return false;

	hfp->close_on_unref = do_close;

	return true;
}

bool hfp_hf_send_command(struct hfp_hf *hfp, hfp_response_func_t resp_cb,
				void *user_data, const char *format, ...)
{
	va_list ap;
	char *fmt;
	int len;
	struct cmd_response *cmd;

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

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

	cmd = new0(struct cmd_response, 1);

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

	free(fmt);

	if (len < 0) {
		free(cmd);
		return false;
	}

	cmd->resp_cb = resp_cb;
	cmd->user_data = user_data;

	if (!queue_push_tail(hfp->cmd_queue, cmd)) {
		ringbuf_drain(hfp->write_buf, len);
		free(cmd);
		return false;
	}

	hf_wakeup_writer(hfp);

	return true;
}

bool hfp_hf_register(struct hfp_hf *hfp, hfp_hf_result_func_t callback,
						const char *prefix,
						void *user_data,
						hfp_destroy_func_t destroy)
{
	struct event_handler *handler;

	if (!callback)
		return false;

	handler = new0(struct event_handler, 1);
	handler->callback = callback;
	handler->user_data = user_data;

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

	if (queue_find(hfp->event_handlers, match_handler_event_prefix,
							handler->prefix)) {
		destroy_event_handler(handler);
		return false;
	}

	handler->destroy = destroy;

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

bool hfp_hf_unregister(struct hfp_hf *hfp, const char *prefix)
{
	struct cmd_handler *handler;

	/* Cast to void as queue_remove needs that */
	handler = queue_remove_if(hfp->event_handlers,
						match_handler_event_prefix,
						(void *) prefix);

	if (!handler)
		return false;

	destroy_event_handler(handler);

	return true;
}

static void hf_disconnect_watch_destroy(void *user_data)
{
	struct hfp_hf *hfp = user_data;

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

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

static bool hf_io_disconnected(struct io *io, void *user_data)
{
	struct hfp_hf *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_hf_set_disconnect_handler(struct hfp_hf *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, hf_io_disconnected, hfp,
						hf_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_hf_disconnect(struct hfp_hf *hfp)
{
	if (!hfp)
		return false;

	return io_shutdown(hfp->io);
}