src/shared/btp.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2012-2017  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
 *
 */

#include <stdbool.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <unistd.h>

#include <ell/ell.h>

#include "lib/bluetooth.h"
#include "src/shared/btp.h"

#define BTP_MTU 512

struct btp_handler {
	unsigned int id;
	uint8_t service;
	uint8_t opcode;

	btp_cmd_func_t callback;
	void *user_data;
	btp_destroy_func_t destroy;
};

struct btp {
	struct l_io *io;

	struct l_queue *pending;
	bool writer_active;
	bool reader_active;

	struct l_queue *handlers;
	unsigned int next_handler;

	uint8_t buf[BTP_MTU];

	btp_disconnect_func_t disconnect_cb;
	void *disconnect_cb_data;
	btp_destroy_func_t disconnect_cb_data_destroy;
};


static struct l_io *btp_connect(const char *path)
{
	struct sockaddr_un addr;
	struct l_io *io;
	int sk;

	sk = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
	if (sk < 0)
		return NULL;

	memset(&addr, 0, sizeof(addr));
	addr.sun_family = AF_UNIX;
	strncpy(addr.sun_path, path, sizeof(addr.sun_path) - 1);

	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		close(sk);
		return NULL;
	}

	io = l_io_new(sk);
	if (!io) {
		close(sk);
		return NULL;
	}

	l_io_set_close_on_destroy(io, true);
	return io;
}

static void disconnect_handler(struct l_io *io, void *user_data)
{
	struct btp *btp = user_data;

	btp->disconnect_cb(btp, btp->disconnect_cb_data);
}

static void disconnect_handler_destroy(void *user_data)
{
	struct btp *btp = user_data;

	if (btp->disconnect_cb_data_destroy)
		btp->disconnect_cb_data_destroy(btp->disconnect_cb_data);
}

struct handler_match_data {
	uint8_t service;
	uint8_t opcode;
};

static bool handler_match(const void *a, const void *b)
{
	const struct btp_handler *handler = a;
	const struct handler_match_data *match = b;

	return (handler->service == match->service) &&
					(handler->opcode == match->opcode);
}

static bool can_read_data(struct l_io *io, void *user_data)
{
	struct handler_match_data match;
	struct btp *btp = user_data;
	struct btp_handler *handler;
	struct btp_hdr *hdr;
	ssize_t bytes_read;
	uint16_t data_len;

	bytes_read = read(l_io_get_fd(btp->io), btp->buf, sizeof(btp->buf));
	if (bytes_read < 0)
		return false;

	if ((size_t) bytes_read < sizeof(*hdr))
		return false;

	hdr = (void *)btp->buf;

	data_len = L_LE16_TO_CPU(hdr->data_len);

	if ((size_t) bytes_read < sizeof(*hdr) + data_len)
		return false;

	match.service = hdr->service;
	match.opcode = hdr->opcode;

	handler = l_queue_find(btp->handlers, handler_match, &match);
	if (handler) {
		handler->callback(hdr->index, hdr->data, data_len,
							handler->user_data);
		return false;
	}

	/* keep reader active if we sent error reply */
	btp_send_error(btp, match.service, hdr->index, BTP_ERROR_UNKNOWN_CMD);
	return true;
}

static void read_watch_destroy(void *user_data)
{
	struct btp *btp = user_data;

	btp->reader_active = false;
}

static void wakeup_reader(struct btp *btp)
{
	if (btp->reader_active)
		return;

	btp->reader_active = l_io_set_read_handler(btp->io, can_read_data, btp,
							read_watch_destroy);
}

struct btp *btp_new(const char *path)
{
	struct btp *btp;
	struct l_io *io;

	io = btp_connect(path);
	if (!io)
		return NULL;

	btp = l_new(struct btp, 1);
	btp->pending = l_queue_new();
	btp->handlers = l_queue_new();
	btp->io = io;
	btp->next_handler = 1;

	wakeup_reader(btp);

	return btp;
}

struct pending_message {
	size_t len;
	void *data;
	bool wakeup_read;
};

static void destroy_message(struct pending_message *msg)
{
	l_free(msg->data);
	l_free(msg);
}

void btp_cleanup(struct btp *btp)
{
	if (!btp)
		return;

	l_io_destroy(btp->io);
	l_queue_destroy(btp->pending, (l_queue_destroy_func_t)destroy_message);
	l_queue_destroy(btp->handlers, (l_queue_destroy_func_t)l_free);
	l_free(btp);
}

bool btp_set_disconnect_handler(struct btp *btp, btp_disconnect_func_t callback,
				void *user_data, btp_destroy_func_t destroy)
{
	if (callback) {
		if (!l_io_set_disconnect_handler(btp->io, disconnect_handler,
					btp, disconnect_handler_destroy))
			return false;
	} else {
		if (!l_io_set_disconnect_handler(btp->io, NULL, NULL, NULL))
			return false;
	}

	btp->disconnect_cb = callback;
	btp->disconnect_cb_data = user_data;
	btp->disconnect_cb_data_destroy = destroy;

	return true;
}

static bool can_write_data(struct l_io *io, void *user_data)
{
	struct btp *btp = user_data;
	struct pending_message *msg;

	msg = l_queue_pop_head(btp->pending);
	if (!msg)
		return false;

	if (msg->wakeup_read)
		wakeup_reader(btp);

	if (write(l_io_get_fd(btp->io), msg->data, msg->len) < 0) {
		l_error("Failed to send BTP message");
		destroy_message(msg);
		return false;
	}

	destroy_message(msg);

	return !l_queue_isempty(btp->pending);
}

static void write_watch_destroy(void *user_data)
{
	struct btp *btp = user_data;

	btp->writer_active = false;
}

static void wakeup_writer(struct btp *btp)
{
	if (l_queue_isempty(btp->pending))
		return;

	if (btp->writer_active)
		return;

	btp->writer_active = l_io_set_write_handler(btp->io, can_write_data,
						btp, write_watch_destroy);
}

bool btp_send_error(struct btp *btp, uint8_t service, uint8_t index,
								uint8_t status)
{
	struct btp_error rsp;

	rsp.status = status;

	return btp_send(btp, service, BTP_OP_ERROR, index, sizeof(rsp), &rsp);
}

bool btp_send(struct btp *btp, uint8_t service, uint8_t opcode, uint8_t index,
					uint16_t length, const void *param)
{
	struct btp_hdr *hdr;
	struct pending_message *msg;
	size_t len;

	if (!btp)
		return false;

	len = sizeof(*hdr) + length;
	hdr = l_malloc(len);
	if (!hdr)
		return false;

	hdr->service = service;
	hdr->opcode = opcode;
	hdr->index = index;
	hdr->data_len = L_CPU_TO_LE16(length);
	if (length)
		memcpy(hdr->data, param, length);

	msg = l_new(struct pending_message, 1);
	msg->len = len;
	msg->data = hdr;
	msg->wakeup_read = opcode < 0x80;

	l_queue_push_tail(btp->pending, msg);
	wakeup_writer(btp);

	return true;
}

unsigned int btp_register(struct btp *btp, uint8_t service, uint8_t opcode,
				btp_cmd_func_t callback, void *user_data,
				btp_destroy_func_t destroy)
{
	struct btp_handler *handler;

	handler = l_new(struct btp_handler, 1);

	handler->id = btp->next_handler++;
	handler->service = service;
	handler->opcode = opcode;
	handler->callback = callback;
	handler->user_data = user_data;
	handler->destroy = destroy;

	l_queue_push_tail(btp->handlers, handler);

	return handler->id;
}

static bool handler_match_by_id(const void *a, const void *b)
{
	const struct btp_handler *handler = a;
	unsigned int id = L_PTR_TO_UINT(b);

	return handler->id == id;
}

bool btp_unregister(struct btp *btp, unsigned int id)
{
	struct btp_handler *handler;

	handler = l_queue_remove_if(btp->handlers, handler_match_by_id,
							L_UINT_TO_PTR(id));
	if (!handler)
		return false;

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

	l_free(handler);

	return true;
}

static bool handler_remove_by_service(void *a, void *b)
{
	struct btp_handler *handler = a;
	uint8_t service = L_PTR_TO_UINT(b);

	if (handler->service != service)
		return false;

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

	l_free(handler);
	return true;
}

void btp_unregister_service(struct btp *btp, uint8_t service)
{
	l_queue_foreach_remove(btp->handlers, handler_remove_by_service,
							L_UINT_TO_PTR(service));
}