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

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

#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <assert.h>
#include <getopt.h>

#include <ell/ell.h>

#include "src/shared/btp.h"

struct btp_adapter {
	struct l_dbus_proxy *proxy;
	struct l_dbus_proxy *ad_proxy;
	uint8_t index;
	uint32_t supported_settings;
	uint32_t current_settings;
	struct l_queue *devices;
};

struct btp_device {
	struct l_dbus_proxy *proxy;
};

static struct l_queue *adapters;
static char *socket_path;
static struct btp *btp;

static bool gap_service_registered;

static bool str2addr(const char *str, uint8_t *addr)
{
	return sscanf(str, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx", &addr[5], &addr[4],
				&addr[3], &addr[2], &addr[1], &addr[0]) == 6;
}

static struct btp_adapter *find_adapter_by_proxy(struct l_dbus_proxy *proxy)
{
	const struct l_queue_entry *entry;

	for (entry = l_queue_get_entries(adapters); entry;
							entry = entry->next) {
		struct btp_adapter *adapter = entry->data;

		if (adapter->proxy == proxy)
			return adapter;
	}

	return NULL;
}

static struct btp_adapter *find_adapter_by_index(uint8_t index)
{
	const struct l_queue_entry *entry;

	for (entry = l_queue_get_entries(adapters); entry;
							entry = entry->next) {
		struct btp_adapter *adapter = entry->data;

		if (adapter->index == index)
			return adapter;
	}

	return NULL;
}

static struct btp_adapter *find_adapter_by_path(const char *path)
{
	const struct l_queue_entry *entry;

	for (entry = l_queue_get_entries(adapters); entry;
							entry = entry->next) {
		struct btp_adapter *adapter = entry->data;

		if (!strcmp(l_dbus_proxy_get_path(adapter->proxy), path))
			return adapter;
	}

	return NULL;
}

static void btp_gap_read_commands(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	uint16_t commands = 0;

	if (index != BTP_INDEX_NON_CONTROLLER) {
		btp_send_error(btp, BTP_GAP_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	commands |= (1 << BTP_OP_GAP_READ_SUPPORTED_COMMANDS);
	commands |= (1 << BTP_OP_GAP_READ_CONTROLLER_INDEX_LIST);
	commands |= (1 << BTP_OP_GAP_READ_COTROLLER_INFO);
	commands |= (1 << BTP_OP_GAP_RESET);
	commands |= (1 << BTP_OP_GAP_SET_POWERED);
	commands |= (1 << BTP_OP_GAP_SET_CONNECTABLE);
	commands |= (1 << BTP_OP_GAP_SET_DISCOVERABLE);
	commands |= (1 << BTP_OP_GAP_SET_BONDABLE);
	commands |= (1 << BTP_OP_GAP_START_DISCOVERY);
	commands |= (1 << BTP_OP_GAP_STOP_DISCOVERY);

	commands = L_CPU_TO_LE16(commands);

	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_READ_SUPPORTED_COMMANDS,
			BTP_INDEX_NON_CONTROLLER, sizeof(commands), &commands);
}

static void btp_gap_read_controller_index(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	const struct l_queue_entry *entry;
	struct btp_gap_read_index_rp *rp;
	uint8_t cnt;
	int i;

	if (index != BTP_INDEX_NON_CONTROLLER) {
		btp_send_error(btp, BTP_GAP_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	cnt = l_queue_length(adapters);

	rp = l_malloc(sizeof(*rp) + cnt);

	rp->num = cnt;

	for (i = 0, entry = l_queue_get_entries(adapters); entry;
						i++, entry = entry->next) {
		struct btp_adapter *adapter = entry->data;

		rp->indexes[i] = adapter->index;
	}

	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_READ_CONTROLLER_INDEX_LIST,
			BTP_INDEX_NON_CONTROLLER, sizeof(*rp) + cnt, rp);
}

static void btp_gap_read_info(uint8_t index, const void *param, uint16_t length,
								void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	struct btp_gap_read_info_rp rp;
	const char *str;
	uint8_t status = BTP_ERROR_FAIL;

	if (!adapter) {
		status = BTP_ERROR_INVALID_INDEX;
		goto failed;
	}

	memset(&rp, 0, sizeof(rp));

	if (!l_dbus_proxy_get_property(adapter->proxy, "Address", "s", &str))
		goto failed;

	if (!str2addr(str, rp.address))
		goto failed;

	if (!l_dbus_proxy_get_property(adapter->proxy, "Name", "s", &str)) {
		goto failed;
	}

	strncpy((char *) rp.name, str, sizeof(rp.name));
	strncpy((char *) rp.short_name, str, sizeof(rp.short_name));
	rp.supported_settings = L_CPU_TO_LE32(adapter->supported_settings);
	rp.current_settings = L_CPU_TO_LE32(adapter->current_settings);

	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_READ_COTROLLER_INFO, index,
							sizeof(rp), &rp);

	return;
failed:
	btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}

static void remove_device_setup(struct l_dbus_message *message,
							void *user_data)
{
	struct btp_device *device = user_data;

	l_dbus_message_set_arguments(message, "o",
					l_dbus_proxy_get_path(device->proxy));
}

static void remove_device_reply(struct l_dbus_proxy *proxy,
						struct l_dbus_message *result,
						void *user_data)
{
	struct btp_device *device = user_data;
	struct btp_adapter *adapter = find_adapter_by_proxy(proxy);

	if (!adapter)
		return;

	if (l_dbus_message_is_error(result)) {
		const char *name;

		l_dbus_message_get_error(result, &name, NULL);

		l_error("Failed to remove device %s (%s)",
					l_dbus_proxy_get_path(device->proxy),
					name);
		return;
	}

	l_queue_remove(adapter->devices, device);
}

static void btp_gap_reset(uint8_t index, const void *param, uint16_t length,
								void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	const struct l_queue_entry *entry;
	uint8_t status;
	bool prop;

	if (!adapter) {
		status = BTP_ERROR_INVALID_INDEX;
		goto failed;
	}

	/* Adapter needs to be powered to be able to remove devices */
	if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
									!prop) {
		status = BTP_ERROR_FAIL;
		goto failed;
	}

	for (entry = l_queue_get_entries(adapter->devices); entry;
							entry = entry->next) {
		struct btp_device *device = entry->data;

		l_dbus_proxy_method_call(adapter->proxy, "RemoveDevice",
						remove_device_setup,
						remove_device_reply, device,
						NULL);
	}

	/* TODO for we assume all went well */
	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_RESET, index, 0, NULL);
	return;

failed:
	btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}

struct set_setting_data {
	struct btp_adapter *adapter;
	uint8_t opcode;
	uint32_t setting;
	bool value;
};

static void set_setting_reply(struct l_dbus_proxy *proxy,
				struct l_dbus_message *result, void *user_data)
{
	struct set_setting_data *data = user_data;
	struct btp_adapter *adapter = data->adapter;
	uint32_t settings;

	if (l_dbus_message_is_error(result)) {
		btp_send_error(btp, BTP_GAP_SERVICE, data->adapter->index,
								BTP_ERROR_FAIL);
		return;
	}

	if (data->value)
		adapter->current_settings |= data->setting;
	else
		adapter->current_settings &= ~data->setting;

	settings = L_CPU_TO_LE32(adapter->current_settings);

	btp_send(btp, BTP_GAP_SERVICE, data->opcode, adapter->index,
						sizeof(settings), &settings);
}

static void btp_gap_set_powered(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	const struct btp_gap_set_powered_cp *cp = param;
	uint8_t status = BTP_ERROR_FAIL;
	struct set_setting_data *data;

	if (length < sizeof(*cp))
		goto failed;

	if (!adapter) {
		status = BTP_ERROR_INVALID_INDEX;
		goto failed;
	}

	data = l_new(struct set_setting_data, 1);
	data->adapter = adapter;
	data->opcode = BTP_OP_GAP_SET_POWERED;
	data->setting = BTP_GAP_SETTING_POWERED;
	data->value = cp->powered;

	if (l_dbus_proxy_set_property(adapter->proxy, set_setting_reply,
					data, l_free, "Powered", "b",
					data->value))
		return;

	l_free(data);

failed:
	btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}

static void update_current_settings(struct btp_adapter *adapter,
							uint32_t new_settings)
{
	struct btp_new_settings_ev ev;

	adapter->current_settings = new_settings;

	ev.current_settings = L_CPU_TO_LE32(adapter->current_settings);

	btp_send(btp, BTP_GAP_SERVICE, BTP_EV_GAP_NEW_SETTINGS, adapter->index,
							sizeof(ev), &ev);
}

static void btp_gap_set_connectable(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	const struct btp_gap_set_connectable_cp *cp = param;
	uint8_t status = BTP_ERROR_FAIL;
	uint32_t new_settings;

	if (length < sizeof(*cp))
		goto failed;

	if (!adapter) {
		status = BTP_ERROR_INVALID_INDEX;
		goto failed;
	}

	new_settings = adapter->current_settings;

	if (cp->connectable)
		new_settings |= 1 << BTP_GAP_SETTING_CONNECTABLE;
	else
		new_settings &= ~(1 << BTP_GAP_SETTING_CONNECTABLE);

	update_current_settings(adapter, new_settings);

	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_CONNECTABLE, index,
					sizeof(new_settings), &new_settings);

	return;

failed:
	btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}

static void btp_gap_set_discoverable(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	const struct btp_gap_set_discoverable_cp *cp = param;
	uint8_t status = BTP_ERROR_FAIL;
	struct set_setting_data *data;

	if (length < sizeof(*cp))
		goto failed;

	if (!adapter) {
		status = BTP_ERROR_INVALID_INDEX;
		goto failed;
	}

	data = l_new(struct set_setting_data, 1);
	data->adapter = adapter;
	data->opcode = BTP_OP_GAP_SET_DISCOVERABLE;
	data->setting = BTP_GAP_SETTING_DISCOVERABLE;
	data->value = cp->discoverable;

	if (l_dbus_proxy_set_property(adapter->proxy, set_setting_reply,
					data, l_free, "Discoverable", "b",
					data->value))
		return;

	l_free(data);

failed:
	btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}

static void btp_gap_set_bondable(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	const struct btp_gap_set_bondable_cp *cp = param;
	uint8_t status = BTP_ERROR_FAIL;
	struct set_setting_data *data;

	if (length < sizeof(*cp))
		goto failed;

	if (!adapter) {
		status = BTP_ERROR_INVALID_INDEX;
		goto failed;
	}

	data = l_new(struct set_setting_data, 1);
	data->adapter = adapter;
	data->opcode = BTP_OP_GAP_SET_BONDABLE;
	data->setting = BTP_GAP_SETTING_BONDABLE;
	data->value = cp->bondable;

	if (l_dbus_proxy_set_property(adapter->proxy, set_setting_reply,
					data, l_free, "Pairable", "b",
					data->value))
		return;

	l_free(data);

failed:
	btp_send_error(btp, BTP_GAP_SERVICE, index, status);
}

static void start_discovery_reply(struct l_dbus_proxy *proxy,
						struct l_dbus_message *result,
						void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_proxy(proxy);

	if (!adapter) {
		btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
								BTP_ERROR_FAIL);
		return;
	}

	if (l_dbus_message_is_error(result)) {
		const char *name, *desc;

		l_dbus_message_get_error(result, &name, &desc);
		l_error("Failed to start discovery (%s), %s", name, desc);

		btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
								BTP_ERROR_FAIL);
		return;
	}

	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_DISCOVERY,
						adapter->index, 0, NULL);
}

static void set_discovery_filter_setup(struct l_dbus_message *message,
							void *user_data)
{
	uint8_t flags = L_PTR_TO_UINT(user_data);
	struct l_dbus_message_builder *builder;

	if (!(flags & (BTP_GAP_DISCOVERY_FLAG_LE |
					BTP_GAP_DISCOVERY_FLAG_BREDR))) {
		l_info("Failed to start discovery - no transport set");
		return;
	}

	builder = l_dbus_message_builder_new(message);

	l_dbus_message_builder_enter_array(builder, "{sv}");
	l_dbus_message_builder_enter_dict(builder, "sv");

	/* Be in observer mode or in general mode (default in Bluez) */
	if (flags & BTP_GAP_DISCOVERY_FLAG_OBSERVATION) {
		l_dbus_message_builder_append_basic(builder, 's', "Transport");
		l_dbus_message_builder_enter_variant(builder, "s");

		if (flags & (BTP_GAP_DISCOVERY_FLAG_LE |
						BTP_GAP_DISCOVERY_FLAG_BREDR))
			l_dbus_message_builder_append_basic(builder, 's',
									"auto");
		else if (flags & BTP_GAP_DISCOVERY_FLAG_LE)
			l_dbus_message_builder_append_basic(builder, 's', "le");
		else if (flags & BTP_GAP_DISCOVERY_FLAG_BREDR)
			l_dbus_message_builder_append_basic(builder, 's',
								"bredr");

		l_dbus_message_builder_leave_variant(builder);
	}

	l_dbus_message_builder_leave_dict(builder);
	l_dbus_message_builder_leave_array(builder);

	/* TODO add passive, limited discovery */
	l_dbus_message_builder_finalize(builder);
	l_dbus_message_builder_destroy(builder);
}

static void set_discovery_filter_reply(struct l_dbus_proxy *proxy,
						struct l_dbus_message *result,
						void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_proxy(proxy);

	if (!adapter) {
		btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
								BTP_ERROR_FAIL);
		return;
	}

	if (l_dbus_message_is_error(result)) {
		const char *name, *desc;

		l_dbus_message_get_error(result, &name, &desc);
		l_error("Failed to set discovery filter (%s), %s", name, desc);

		btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
								BTP_ERROR_FAIL);
		return;
	}

	l_dbus_proxy_method_call(adapter->proxy, "StartDiscovery", NULL,
					start_discovery_reply, NULL, NULL);
}

static void btp_gap_start_discovery(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	const struct btp_gap_start_discovery_cp *cp = param;
	bool prop;

	if (!adapter) {
		btp_send_error(btp, BTP_GAP_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	/* Adapter needs to be powered to start discovery */
	if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
									!prop) {
		btp_send_error(btp, BTP_GAP_SERVICE, index, BTP_ERROR_FAIL);
		return;
	}

	l_dbus_proxy_method_call(adapter->proxy, "SetDiscoveryFilter",
						set_discovery_filter_setup,
						set_discovery_filter_reply,
						L_UINT_TO_PTR(cp->flags), NULL);
}

static void clear_discovery_filter_setup(struct l_dbus_message *message,
							void *user_data)
{
	struct l_dbus_message_builder *builder;

	builder = l_dbus_message_builder_new(message);

	/* Clear discovery filter setup */
	l_dbus_message_builder_enter_array(builder, "{sv}");
	l_dbus_message_builder_enter_dict(builder, "sv");
	l_dbus_message_builder_leave_dict(builder);
	l_dbus_message_builder_leave_array(builder);
	l_dbus_message_builder_finalize(builder);
	l_dbus_message_builder_destroy(builder);
}

static void clear_discovery_filter_reaply(struct l_dbus_proxy *proxy,
						struct l_dbus_message *result,
						void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_proxy(proxy);

	if (!adapter) {
		btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
								BTP_ERROR_FAIL);
		return;
	}

	if (l_dbus_message_is_error(result)) {
		const char *name, *desc;

		l_dbus_message_get_error(result, &name, &desc);
		l_error("Failed to set discovery filter (%s), %s", name, desc);

		btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
								BTP_ERROR_FAIL);
		return;
	}

	btp_send(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_DISCOVERY,
						adapter->index, 0, NULL);
}

static void stop_discovery_reply(struct l_dbus_proxy *proxy,
						struct l_dbus_message *result,
						void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_proxy(proxy);

	if (!adapter) {
		btp_send_error(btp, BTP_GAP_SERVICE, BTP_INDEX_NON_CONTROLLER,
								BTP_ERROR_FAIL);
		return;
	}

	if (l_dbus_message_is_error(result)) {
		const char *name;

		l_dbus_message_get_error(result, &name, NULL);
		l_error("Failed to stop discovery (%s)", name);

		btp_send_error(btp, BTP_GAP_SERVICE, adapter->index,
								BTP_ERROR_FAIL);
		return;
	}

	l_dbus_proxy_method_call(adapter->proxy, "SetDiscoveryFilter",
						clear_discovery_filter_setup,
						clear_discovery_filter_reaply,
						NULL, NULL);
}

static void btp_gap_stop_discovery(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	struct btp_adapter *adapter = find_adapter_by_index(index);
	bool prop;

	if (!adapter) {
		btp_send_error(btp, BTP_GAP_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	/* Adapter needs to be powered to be able to remove devices */
	if (!l_dbus_proxy_get_property(adapter->proxy, "Powered", "b", &prop) ||
									!prop) {
		btp_send_error(btp, BTP_GAP_SERVICE, index, BTP_ERROR_FAIL);
		return;
	}

	l_dbus_proxy_method_call(adapter->proxy, "StopDiscovery", NULL,
					stop_discovery_reply, NULL, NULL);
}

static void btp_gap_device_found_ev(struct l_dbus_proxy *proxy)
{
	struct btp_device_found_ev ev;
	const char *str;
	int16_t rssi;

	if (!l_dbus_proxy_get_property(proxy, "Address", "s", &str) ||
						!str2addr(str, ev.address))
		return;

	if (!l_dbus_proxy_get_property(proxy, "AddressType", "s", &str))
		return;

	ev.address_type = strcmp(str, "public") ? BTP_GAP_ADDR_RANDOM :
							BTP_GAP_ADDR_PUBLIC;

	if (!l_dbus_proxy_get_property(proxy, "RSSI", "n", &rssi))
		return;

	ev.rssi = rssi;

	/* TODO Temporary set all flags */
	ev.flags = (BTP_EV_GAP_DEVICE_FOUND_FLAG_RSSI |
					BTP_EV_GAP_DEVICE_FOUND_FLAG_AD |
					BTP_EV_GAP_DEVICE_FOUND_FLAG_SR);

	/* TODO Add eir to device found event */
	ev.eir_len = 0;

	btp_send(btp, BTP_GAP_SERVICE, BTP_EV_GAP_DEVICE_FOUND,
						BTP_INDEX_NON_CONTROLLER,
						sizeof(ev) + ev.eir_len, &ev);
}

static void register_gap_service(void)
{
	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_READ_SUPPORTED_COMMANDS,
					btp_gap_read_commands, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE,
				BTP_OP_GAP_READ_CONTROLLER_INDEX_LIST,
				btp_gap_read_controller_index, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_READ_COTROLLER_INFO,
						btp_gap_read_info, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_RESET,
						btp_gap_reset, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_POWERED,
					btp_gap_set_powered, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_CONNECTABLE,
					btp_gap_set_connectable, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_DISCOVERABLE,
					btp_gap_set_discoverable, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_SET_BONDABLE,
					btp_gap_set_bondable, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_START_DISCOVERY,
					btp_gap_start_discovery, NULL, NULL);

	btp_register(btp, BTP_GAP_SERVICE, BTP_OP_GAP_STOP_DISCOVERY,
					btp_gap_stop_discovery, NULL, NULL);
}

static void btp_core_read_commands(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	uint8_t commands = 0;

	if (index != BTP_INDEX_NON_CONTROLLER) {
		btp_send_error(btp, BTP_CORE_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	commands |= (1 << BTP_OP_CORE_READ_SUPPORTED_COMMANDS);
	commands |= (1 << BTP_OP_CORE_READ_SUPPORTED_SERVICES);
	commands |= (1 << BTP_OP_CORE_REGISTER);
	commands |= (1 << BTP_OP_CORE_UNREGISTER);

	btp_send(btp, BTP_CORE_SERVICE, BTP_OP_CORE_READ_SUPPORTED_COMMANDS,
			BTP_INDEX_NON_CONTROLLER, sizeof(commands), &commands);
}

static void btp_core_read_services(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	uint8_t services = 0;

	if (index != BTP_INDEX_NON_CONTROLLER) {
		btp_send_error(btp, BTP_CORE_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	services |= (1 << BTP_CORE_SERVICE);
	services |= (1 << BTP_GAP_SERVICE);

	btp_send(btp, BTP_CORE_SERVICE, BTP_OP_CORE_READ_SUPPORTED_SERVICES,
			BTP_INDEX_NON_CONTROLLER, sizeof(services), &services);
}

static void btp_core_register(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	const struct btp_core_register_cp  *cp = param;

	if (length < sizeof(*cp))
		goto failed;

	if (index != BTP_INDEX_NON_CONTROLLER) {
		btp_send_error(btp, BTP_CORE_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	switch (cp->service_id) {
	case BTP_GAP_SERVICE:
		if (gap_service_registered)
			goto failed;

		register_gap_service();
		gap_service_registered = true;
		break;
	case BTP_GATT_SERVICE:
	case BTP_L2CAP_SERVICE:
	case BTP_MESH_NODE_SERVICE:
	case BTP_CORE_SERVICE:
	default:
		goto failed;
	}

	btp_send(btp, BTP_CORE_SERVICE, BTP_OP_CORE_REGISTER,
					BTP_INDEX_NON_CONTROLLER, 0, NULL);
	return;

failed:
	btp_send_error(btp, BTP_CORE_SERVICE, index, BTP_ERROR_FAIL);
}

static void btp_core_unregister(uint8_t index, const void *param,
					uint16_t length, void *user_data)
{
	const struct btp_core_unregister_cp  *cp = param;

	if (length < sizeof(*cp))
		goto failed;

	if (index != BTP_INDEX_NON_CONTROLLER) {
		btp_send_error(btp, BTP_CORE_SERVICE, index,
						BTP_ERROR_INVALID_INDEX);
		return;
	}

	switch (cp->service_id) {
	case BTP_GAP_SERVICE:
		if (!gap_service_registered)
			goto failed;

		btp_unregister_service(btp, BTP_GAP_SERVICE);
		gap_service_registered = false;
		break;
	case BTP_GATT_SERVICE:
	case BTP_L2CAP_SERVICE:
	case BTP_MESH_NODE_SERVICE:
	case BTP_CORE_SERVICE:
	default:
		goto failed;
	}

	btp_send(btp, BTP_CORE_SERVICE, BTP_OP_CORE_UNREGISTER,
					BTP_INDEX_NON_CONTROLLER, 0, NULL);
	return;

failed:
	btp_send_error(btp, BTP_CORE_SERVICE, index, BTP_ERROR_FAIL);
}

static void register_core_service(void)
{
	btp_register(btp, BTP_CORE_SERVICE,
					BTP_OP_CORE_READ_SUPPORTED_COMMANDS,
					btp_core_read_commands, NULL, NULL);

	btp_register(btp, BTP_CORE_SERVICE,
					BTP_OP_CORE_READ_SUPPORTED_SERVICES,
					btp_core_read_services, NULL, NULL);

	btp_register(btp, BTP_CORE_SERVICE, BTP_OP_CORE_REGISTER,
						btp_core_register, NULL, NULL);

	btp_register(btp, BTP_CORE_SERVICE, BTP_OP_CORE_UNREGISTER,
					btp_core_unregister, NULL, NULL);
}

static void signal_handler(struct l_signal *signal, uint32_t signo,
							void *user_data)
{
	switch (signo) {
	case SIGINT:
	case SIGTERM:
		l_info("Terminating");
		l_main_quit();
		break;
	}
}

static void btp_device_free(struct btp_device *device)
{
	l_free(device);
}

static void btp_adapter_free(struct btp_adapter *adapter)
{
	l_queue_destroy(adapter->devices,
				(l_queue_destroy_func_t)btp_device_free);
	l_free(adapter);
}

static void extract_settings(struct l_dbus_proxy *proxy, uint32_t *current,
						uint32_t *supported)
{
	bool prop;

	*supported = 0;
	*current = 0;

	/* TODO not all info is available via D-Bus API */
	*supported |=  BTP_GAP_SETTING_POWERED;
	*supported |=  BTP_GAP_SETTING_CONNECTABLE;
	*supported |=  BTP_GAP_SETTING_DISCOVERABLE;
	*supported |=  BTP_GAP_SETTING_BONDABLE;
	*supported |=  BTP_GAP_SETTING_SSP;
	*supported |=  BTP_GAP_SETTING_BREDR;
	*supported |=  BTP_GAP_SETTING_LE;
	*supported |=  BTP_GAP_SETTING_ADVERTISING;
	*supported |=  BTP_GAP_SETTING_SC;
	*supported |=  BTP_GAP_SETTING_PRIVACY;
	/* *supported |=  BTP_GAP_SETTING_STATIC_ADDRESS; */

	/* TODO not all info is availbe via D-Bus API so some are assumed to be
	 * enabled by bluetoothd or simply hardcoded until API is extended
	 */
	*current |=  BTP_GAP_SETTING_CONNECTABLE;
	*current |=  BTP_GAP_SETTING_SSP;
	*current |=  BTP_GAP_SETTING_BREDR;
	*current |=  BTP_GAP_SETTING_LE;
	*current |=  BTP_GAP_SETTING_PRIVACY;
	*current |=  BTP_GAP_SETTING_SC;
	/* *supported |=  BTP_GAP_SETTING_STATIC_ADDRESS; */

	if (l_dbus_proxy_get_property(proxy, "Powered", "b", &prop) && prop)
		*current |=  BTP_GAP_SETTING_POWERED;

	if (l_dbus_proxy_get_property(proxy, "Discoverable", "b", &prop) &&
									prop)
		*current |=  BTP_GAP_SETTING_DISCOVERABLE;

	if (l_dbus_proxy_get_property(proxy, "Pairable", "b", &prop) && prop)
		*current |=  BTP_GAP_SETTING_BONDABLE;
}

static void proxy_added(struct l_dbus_proxy *proxy, void *user_data)
{
	const char *interface = l_dbus_proxy_get_interface(proxy);
	const char *path = l_dbus_proxy_get_path(proxy);

	l_info("Proxy added: %s (%s)", interface, path);

	if (!strcmp(interface, "org.bluez.Adapter1")) {
		struct btp_adapter *adapter;

		adapter = l_new(struct btp_adapter, 1);
		adapter->proxy = proxy;
		adapter->index = l_queue_length(adapters);
		adapter->devices = l_queue_new();

		extract_settings(proxy, &adapter->current_settings,
						&adapter->supported_settings);

		l_queue_push_tail(adapters, adapter);
		return;
	}

	if (!strcmp(interface, "org.bluez.Device1")) {
		struct btp_adapter *adapter;
		struct btp_device *device;
		char *str;

		if (!l_dbus_proxy_get_property(proxy, "Adapter", "o", &str))
			return;

		adapter = find_adapter_by_path(str);
		if (!adapter)
			return;

		device = l_new(struct btp_device, 1);
		device->proxy = proxy;

		l_queue_push_tail(adapter->devices, device);

		btp_gap_device_found_ev(proxy);

		return;
	}

	if (!strcmp(interface, "org.bluez.LEAdvertisingManager1")) {
		struct btp_adapter *adapter;

		adapter = find_adapter_by_path(path);
		if (!adapter)
			return;

		adapter->ad_proxy = proxy;

		return;
	}
}

static bool device_match_by_proxy(const void *a, const void *b)
{
	const struct btp_device *device = a;
	const struct l_dbus_proxy *proxy = b;

	return device->proxy == proxy;
}

static void proxy_removed(struct l_dbus_proxy *proxy, void *user_data)
{
	const char *interface = l_dbus_proxy_get_interface(proxy);
	const char *path = l_dbus_proxy_get_path(proxy);

	l_info("Proxy removed: %s (%s)", interface, path);

	if (!strcmp(interface, "org.bluez.Adapter1")) {
		l_info("Adapter removed, terminating.");
		l_main_quit();
		return;
	}

	if (!strcmp(interface, "org.bluez.Device1")) {
		struct btp_adapter *adapter;
		char *str;

		if (!l_dbus_proxy_get_property(proxy, "Adapter", "o", &str))
			return;

		adapter = find_adapter_by_path(str);
		if (!adapter)
			return;

		l_queue_remove_if(adapter->devices, device_match_by_proxy,
									proxy);

		return;
	}
}

static void property_changed(struct l_dbus_proxy *proxy, const char *name,
				struct l_dbus_message *msg, void *user_data)
{
	const char *interface = l_dbus_proxy_get_interface(proxy);
	const char *path = l_dbus_proxy_get_path(proxy);

	l_info("property_changed %s %s %s", name, path, interface);

	if (!strcmp(interface, "org.bluez.Adapter1")) {
		struct btp_adapter *adapter = find_adapter_by_proxy(proxy);
		uint32_t new_settings;

		if (!adapter)
			return;

		new_settings = adapter->current_settings;

		if (!strcmp(name, "Powered")) {
			bool prop;

			if (!l_dbus_message_get_arguments(msg, "b", &prop))
				return;

			if (prop)
				new_settings |= BTP_GAP_SETTING_POWERED;
			else
				new_settings &= ~BTP_GAP_SETTING_POWERED;
		} else if (!strcmp(name, "Discoverable")) {
			bool prop;

			if (!l_dbus_message_get_arguments(msg, "b", &prop))
				return;

			if (prop)
				new_settings |= BTP_GAP_SETTING_DISCOVERABLE;
			else
				new_settings &= ~BTP_GAP_SETTING_DISCOVERABLE;
		}

		if (!strcmp(name, "Pairable")) {
			bool prop;

			if (!l_dbus_message_get_arguments(msg, "b", &prop))
				return;

			if (prop)
				new_settings |= BTP_GAP_SETTING_BONDABLE;
			else
				new_settings &= ~BTP_GAP_SETTING_BONDABLE;
		}

		if (new_settings != adapter->current_settings)
			update_current_settings(adapter, new_settings);

		return;
	} else if (!strcmp(interface, "org.bluez.Device1")) {
		if (!strcmp(name, "RSSI")) {
			int16_t rssi;

			if (!l_dbus_message_get_arguments(msg, "n", &rssi))
				return;

			btp_gap_device_found_ev(proxy);
		}
	}
}

static void client_connected(struct l_dbus *dbus, void *user_data)
{
	l_info("D-Bus client connected");
}

static void client_disconnected(struct l_dbus *dbus, void *user_data)
{
	l_info("D-Bus client disconnected, terminated");
	l_main_quit();
}

static void client_ready(struct l_dbus_client *client, void *user_data)
{
	l_info("D-Bus client ready, connecting BTP");

	btp = btp_new(socket_path);
	if (!btp) {
		l_error("Failed to connect BTP, terminating");
		l_main_quit();
		return;
	}

	register_core_service();

	btp_send(btp, BTP_CORE_SERVICE, BTP_EV_CORE_READY,
					BTP_INDEX_NON_CONTROLLER, 0, NULL);
}

static void usage(void)
{
	l_info("btpclient - Bluetooth tester");
	l_info("Usage:");
	l_info("\tbtpclient [options]");
	l_info("options:\n"
	"\t-s, --socket <socket>  Socket to use for BTP\n"
	"\t-q, --quiet            Don't emit any logs\n"
	"\t-v, --version          Show version\n"
	"\t-h, --help             Show help options");
}

static const struct option options[] = {
	{ "socket",	1, 0, 's' },
	{ "quiet",	0, 0, 'q' },
	{ "version",	0, 0, 'v' },
	{ "help",	0, 0, 'h' },
	{ 0, 0, 0, 0 }
};

int main(int argc, char *argv[])
{
	struct l_dbus_client *client;
	struct l_signal *signal;
	struct l_dbus *dbus;
	sigset_t mask;
	int opt;

	l_log_set_stderr();

	while ((opt = getopt_long(argc, argv, "+hs:vq", options, NULL)) != -1) {
		switch (opt) {
		case 's':
			socket_path = l_strdup(optarg);
			break;
		case 'q':
			l_log_set_null();
			break;
		case 'd':
			break;
		case 'v':
			l_info("%s", VERSION);
			return EXIT_SUCCESS;
		case 'h':
		default:
			usage();
			return EXIT_SUCCESS;
		}
	}

	if (!socket_path) {
		l_info("Socket option is required");
		l_info("Type --help for usage");
		return EXIT_FAILURE;
	}

	if (!l_main_init())
		return EXIT_FAILURE;


	adapters = l_queue_new();

	sigemptyset(&mask);
	sigaddset(&mask, SIGINT);
	sigaddset(&mask, SIGTERM);
	signal = l_signal_create(&mask, signal_handler, NULL, NULL);

	dbus = l_dbus_new_default(L_DBUS_SYSTEM_BUS);
	client = l_dbus_client_new(dbus, "org.bluez", "/org/bluez");

	l_dbus_client_set_connect_handler(client, client_connected, NULL, NULL);
	l_dbus_client_set_disconnect_handler(client, client_disconnected, NULL,
									NULL);

	l_dbus_client_set_proxy_handlers(client, proxy_added, proxy_removed,
						property_changed, NULL, NULL);

	l_dbus_client_set_ready_handler(client, client_ready, NULL, NULL);

	l_main_run();

	l_dbus_client_destroy(client);
	l_dbus_destroy(dbus);
	l_signal_remove(signal);
	btp_cleanup(btp);

	l_queue_destroy(adapters, (l_queue_destroy_func_t)btp_adapter_free);

	l_free(socket_path);

	l_main_exit();

	return EXIT_SUCCESS;
}