/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
 *  Authors:
 *  Santiago Carot Nemesio <sancane at gmail.com>
 *  Jose Antonio Santos-Cadenas <santoscadenas at gmail.com>
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  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
 *
 */

#include <gdbus.h>

#include "log.h"
#include "error.h"
#include <stdint.h>
#include <hdp_types.h>
#include <hdp_util.h>
#include <adapter.h>
#include <device.h>
#include <hdp.h>
#include <mcap.h>
#include <btio.h>
#include <mcap_lib.h>
#include <l2cap.h>
#include <sdpd.h>
#include "../src/dbus-common.h"
#include <unistd.h>

#ifndef DBUS_TYPE_UNIX_FD
	#define DBUS_TYPE_UNIX_FD -1
#endif

static DBusConnection *connection = NULL;

static GSList *applications = NULL;
static GSList *devices = NULL;
static uint8_t next_app_id = HDP_MDEP_INITIAL;

static GSList *adapters;

static gboolean update_adapter(struct hdp_adapter *adapter);
static struct hdp_device *create_health_device(DBusConnection *conn,
						struct btd_device *device);

struct hdp_create_dc {
	DBusConnection		*conn;
	DBusMessage		*msg;
	struct hdp_application	*app;
	struct hdp_device	*dev;
	uint8_t			config;
	uint8_t			mdep;
	guint			ref;
};

struct hdp_tmp_dc_data {
	DBusConnection		*conn;
	DBusMessage		*msg;
	struct hdp_channel	*hdp_chann;
	guint			ref;
};

static void free_hdp_create_dc(struct hdp_create_dc *dc_data)
{
	dbus_message_unref(dc_data->msg);
	dbus_connection_unref(dc_data->conn);

	g_free(dc_data);
}

static struct hdp_create_dc *hdp_create_data_ref(struct hdp_create_dc *dc_data)
{
	dc_data->ref++;

	DBG("hdp_create_data_ref(%p): ref=%d", dc_data, dc_data->ref);

	return dc_data;
}

static void hdp_create_data_unref(struct hdp_create_dc *dc_data)
{
	dc_data->ref--;

	DBG("hdp_create_data_ref(%p): ref=%d", dc_data, dc_data->ref);

	if (dc_data->ref > 0)
		return;

	free_hdp_create_dc(dc_data);
}

static void free_hdp_conn_dc(struct hdp_tmp_dc_data *data)
{
	dbus_message_unref(data->msg);
	dbus_connection_unref(data->conn);

	g_free(data);
}

static struct hdp_tmp_dc_data *hdp_tmp_dc_data_ref(struct hdp_tmp_dc_data *data)
{
	data->ref++;

	DBG("hdp_conn_data_ref(%p): ref=%d", data, data->ref);

	return data;
}

static void hdp_tmp_dc_data_unref(struct hdp_tmp_dc_data *data)
{
	data->ref--;

	DBG("hdp_conn_data_ref(%p): ref=%d", data, data->ref);

	if (data->ref > 0)
		return;

	free_hdp_conn_dc(data);
}

static int cmp_app_id(gconstpointer a, gconstpointer b)
{
	const struct hdp_application *app = a;
	const uint8_t *id = b;

	return app->id - *id;
}

static int cmp_adapter(gconstpointer a, gconstpointer b)
{
	const struct hdp_adapter *hdp_adapter = a;
	const struct btd_adapter *adapter = b;

	if (hdp_adapter->btd_adapter == adapter)
		return 0;

	return -1;
}

static int cmp_device(gconstpointer a, gconstpointer b)
{
	const struct hdp_device *hdp_device = a;
	const struct btd_device *device = b;

	if (hdp_device->dev == device)
		return 0;

	return -1;
}

static gint cmp_dev_addr(gconstpointer a, gconstpointer dst)
{
	const struct hdp_device *device = a;
	bdaddr_t addr;

	device_get_address(device->dev, &addr);
	return bacmp(&addr, dst);
}

static gint cmp_dev_mcl(gconstpointer a, gconstpointer mcl)
{
	const struct hdp_device *device = a;

	if (mcl == device->mcl)
		return 0;
	return -1;
}

static gint cmp_chan_mdlid(gconstpointer a, gconstpointer b)
{
	const struct hdp_channel *chan = a;
	const uint16_t *mdlid = b;

	return chan->mdlid - *mdlid;
}

static gint cmp_chan_path(gconstpointer a, gconstpointer b)
{
	const struct hdp_channel *chan = a;
	const char *path = b;

	return g_ascii_strcasecmp(chan->path, path);
}

static gint cmp_chan_mdl(gconstpointer a, gconstpointer mdl)
{
	const struct hdp_channel *chan = a;

	if (chan->mdl == mdl)
		return 0;
	return -1;
}

static uint8_t get_app_id()
{
	GSList *l;
	uint8_t id = next_app_id;

	do {
		l = g_slist_find_custom(applications, &id, cmp_app_id);
		if (!l) {
			next_app_id = (id % HDP_MDEP_FINAL) + 1;
			return id;
		} else
			id = (id % HDP_MDEP_FINAL) + 1;
	} while (id != next_app_id);

	/* No more ids available */
	return 0;
}

static int cmp_app(gconstpointer a, gconstpointer b)
{
	const struct hdp_application *app = a;

	return g_strcmp0(app->path, b);
}

static gboolean set_app_path(struct hdp_application *app)
{
	app->id = get_app_id();
	if (!app->id)
		return FALSE;
	app->path = g_strdup_printf(MANAGER_PATH "/health_app_%d", app->id);

	return TRUE;
};

static void device_unref_mcl(struct hdp_device *hdp_device)
{
	if (!hdp_device->mcl)
		return;
	mcap_mcl_unref(hdp_device->mcl);
	hdp_device->mcl = NULL;
	hdp_device->mcl_conn = FALSE;
}

static void free_health_device(struct hdp_device *device)
{
	if (device->conn) {
		dbus_connection_unref(device->conn);
		device->conn = NULL;
	}

	if (device->dev) {
		btd_device_unref(device->dev);
		device->dev = NULL;
	}

	device_unref_mcl(device);

	g_free(device);
}

static void free_application(struct hdp_application *app)
{
	if (app->dbus_watcher)
		g_dbus_remove_watch(connection, app->dbus_watcher);

	g_free(app->oname);
	g_free(app->description);
	g_free(app->path);
	g_free(app);
}

static void remove_application(struct hdp_application *app)
{
	DBG("Application %s deleted", app->path);
	free_application(app);

	g_slist_foreach(adapters, (GFunc) update_adapter, NULL);
}

static void client_disconnected(DBusConnection *conn, void *user_data)
{
	struct hdp_application *app = user_data;

	DBG("Client disconnected from the bus, deleting hdp application");
	applications = g_slist_remove(applications, app);

	app->dbus_watcher = 0; /* Watcher shouldn't be freed in this case */
	remove_application(app);
}

static DBusMessage *manager_create_application(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	struct hdp_application *app;
	const char *name;
	DBusMessageIter iter;
	GError *err = NULL;
	DBusMessage *reply;

	dbus_message_iter_init(msg, &iter);
	app = hdp_get_app_config(&iter, &err);
	if (err) {
		reply = g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments: %s", err->message);
		g_error_free(err);
		return reply;
	}

	name = dbus_message_get_sender(msg);
	if (!name) {
		free_application(app);
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".HealthError",
					"Can't get sender name");
	}

	if (!set_app_path(app)){
		free_application(app);
		return g_dbus_create_error(msg,
				ERROR_INTERFACE ".HealthError",
				"Can't get a valid id for the application");
	}

	app->oname = g_strdup(name);

	applications = g_slist_prepend(applications, app);

	app->dbus_watcher = g_dbus_add_disconnect_watch(conn, name,
						client_disconnected, app, NULL);
	g_slist_foreach(adapters, (GFunc) update_adapter, NULL);

	DBG("Health application created with id %s", app->path);

	return g_dbus_create_reply(msg, DBUS_TYPE_OBJECT_PATH, &app->path,
							DBUS_TYPE_INVALID);
}

static DBusMessage *manager_destroy_application(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	const char *path;
	struct hdp_application *app;
	GSList *l;

	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
						DBUS_TYPE_INVALID)){
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments in method call");
	}

	l = g_slist_find_custom(applications, path, cmp_app);

	app = l->data;
	applications = g_slist_remove(applications, app);

	remove_application(app);

	return g_dbus_create_reply(msg, DBUS_TYPE_INVALID);
}

static void manager_path_unregister(gpointer data)
{
	g_slist_foreach(applications, (GFunc) free_application, NULL);

	g_slist_free(applications);
	applications = NULL;

	g_slist_foreach(adapters, (GFunc) update_adapter, NULL);
}

static GDBusMethodTable health_manager_methods[] = {
	{"CreateApplication", "a{sv}", "o", manager_create_application},
	{"DestroyApplication", "o", "", manager_destroy_application},
	{ NULL }
};

static DBusMessage *channel_get_properties(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	return g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
						"Function is not implemented");
}

static DBusMessage *channel_acquire(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	return g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
						"Function is not implemented");
}

static DBusMessage *channel_release(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	return g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
						"Function is not implemented");
}

static void health_channel_destroy(void *data)
{
	struct hdp_channel *hdp_chan = data;
	struct hdp_device *dev = hdp_chan->dev;

	DBG("Destroy Health Channel %s", hdp_chan->path);
	dev->channels = g_slist_remove(dev->channels, hdp_chan);

	g_free(hdp_chan->path);
	g_free(hdp_chan);
}

static GDBusMethodTable health_channels_methods[] = {
	{"GetProperties","",	"a{sv}",	channel_get_properties },
	{"Acquire",	"",	"h",		channel_acquire,
						G_DBUS_METHOD_FLAG_ASYNC },
	{"Release",	"",	"",		channel_release },
	{ NULL }
};

static struct hdp_channel *create_channel(struct hdp_device *dev,
						uint8_t config,
						struct mcap_mdl *mdl,
						uint16_t mdlid,
						struct hdp_application *app,
						GError **err)
{
	struct hdp_channel *hdp_chann;

	hdp_chann = g_new0(struct hdp_channel, 1);
	hdp_chann->config = config;
	hdp_chann->dev = dev;
	hdp_chann->mdep = app->id;
	hdp_chann->mdl = mdl;
	hdp_chann->mdlid = mdlid;
	hdp_chann->app = app;

	hdp_chann->path = g_strdup_printf("%s/chan%d",
					device_get_path(hdp_chann->dev->dev),
					hdp_chann->mdlid);

	if (!g_dbus_register_interface(dev->conn, hdp_chann->path,
					HEALTH_CHANNEL,
					health_channels_methods, NULL, NULL,
					hdp_chann, health_channel_destroy)) {
		g_set_error(err, HDP_ERROR, HDP_UNSPECIFIED_ERROR,
					"Can't register the channel interface");
		health_channel_destroy(hdp_chann);
		return NULL;
	}

	dev->channels = g_slist_append(dev->channels, hdp_chann);

	return hdp_chann;
}

static void close_channel(gpointer a, gpointer b)
{
	struct hdp_channel *chan = a;
	int fd;

	if (!chan->mdl_conn)
		return;

	fd = mcap_mdl_get_fd(chan->mdl);
	close(fd);
}

static void hdp_mcap_mdl_connected_cb(struct mcap_mdl *mdl, void *data)
{
	struct hdp_device *dev = data;

	DBG("hdp_mcap_mdl_connected_cb");
	if (!dev->ndc)
		return;

	dev->ndc->mdl = mdl;

	if (!g_slist_find(dev->channels, dev->ndc))
		dev->channels = g_slist_prepend(dev->channels, dev->ndc);

	g_dbus_emit_signal(dev->conn, device_get_path(dev->dev), HEALTH_DEVICE,
					"ChannelConnected",
					DBUS_TYPE_OBJECT_PATH, &dev->ndc->path,
					DBUS_TYPE_INVALID);

	dev->ndc->mdl_conn = TRUE;
	if (!dev->fr)
		dev->fr = dev->ndc;
	dev->ndc = NULL;

	emit_property_changed(dev->conn, device_get_path(dev->dev),
					HEALTH_DEVICE, "MainChannel",
					DBUS_TYPE_OBJECT_PATH, &dev->fr->path);
}

static void hdp_mcap_mdl_closed_cb(struct mcap_mdl *mdl, void *data)
{
	struct hdp_device *dev = data;
	struct hdp_channel *chan;
	GSList *l;

	DBG("hdp_mcap_mdl_closed_cb");
	l = g_slist_find_custom(dev->channels, mdl, cmp_chan_mdl);
	if (!l)
		return;

	chan = l->data;
	chan->mdl_conn = FALSE;
}

static void hdp_mcap_mdl_deleted_cb(struct mcap_mdl *mdl, void *data)
{
	struct hdp_device *dev = data;
	struct hdp_channel *chan;
	char *path, *empty_path;
	GSList *l;

	DBG("hdp_mcap_mdl_deleted_cb");
	l = g_slist_find_custom(dev->channels, mdl, cmp_chan_mdl);
	if (!l)
		return;

	chan = l->data;
	chan->mdl_conn = FALSE;

	g_dbus_emit_signal(dev->conn, device_get_path(dev->dev), HEALTH_DEVICE,
					"ChannelDeleted",
					DBUS_TYPE_OBJECT_PATH, &chan->path,
					DBUS_TYPE_INVALID);

	if (chan == dev->fr) {
		g_slist_foreach(dev->channels, close_channel, NULL);
		dev->fr = NULL;
		empty_path = "";
		emit_property_changed(dev->conn, device_get_path(dev->dev),
					HEALTH_DEVICE, "MainChannel",
					DBUS_TYPE_OBJECT_PATH, &empty_path);
	}

	path = g_strdup(chan->path);
	g_dbus_unregister_interface(dev->conn, path, HEALTH_CHANNEL);
	g_free(path);
}

static void hdp_mcap_mdl_aborted_cb(struct mcap_mdl *mdl, void *data)
{
	struct hdp_device *dev = data;

	DBG("hdp_mcap_mdl_aborted_cb");
	if (!dev->ndc)
		return;

	dev->ndc->mdl = mdl;

	if (!g_slist_find(dev->channels, dev->ndc))
		dev->channels = g_slist_prepend(dev->channels, dev->ndc);

	g_dbus_emit_signal(dev->conn, device_get_path(dev->dev), HEALTH_DEVICE,
					"ChannelConnected",
					DBUS_TYPE_OBJECT_PATH, &dev->ndc->path,
					DBUS_TYPE_INVALID);

	dev->ndc = NULL;
}

static uint8_t hdp_mcap_mdl_conn_req_cb(struct mcap_mcl *mcl, uint8_t mdepid,
				uint16_t mdlid, uint8_t *conf, void *data)
{
	struct hdp_device *dev = data;
	struct hdp_application *app;
	struct hdp_channel *chan;
	char *path;
	GSList *l;

	DBG("Data channel request");
	l = g_slist_find_custom(applications, &mdepid, cmp_app_id);
	if (!l)
		return MCAP_INVALID_MDEP;

	app = l->data;

	/* Check if is the first dc if so,
	* only reliable configuration is allowed */
	switch (*conf) {
	case HDP_NO_PREFERENCE_DC:
		if (app->role == HDP_SINK)
			return MCAP_CONFIGURATION_REJECTED;
		else if (app->chan_type_set)
			*conf = app->chan_type;
		else
			*conf = HDP_RELIABLE_DC;
		break;
	case HDP_STREAMING_DC:
		if (!dev->fr || app->role == HDP_SOURCE)
			return MCAP_CONFIGURATION_REJECTED;
	case HDP_RELIABLE_DC:
		if (app->role == HDP_SOURCE)
			return MCAP_CONFIGURATION_REJECTED;
		break;
	default:
		/* Special case defined in HDP spec 3.4. When an invalid
		* configuration is received we shall close the MCL when
		* we are still processing the callback. */
		mcap_close_mcl(dev->mcl, FALSE);
		dev->mcl_conn = FALSE;
		return MCAP_CONFIGURATION_REJECTED; /* not processed */
	}

	l = g_slist_find_custom(dev->channels, &mdlid, cmp_chan_mdlid);
	if (l) {
		chan = l->data;
		path = g_strdup(chan->path);
		g_dbus_unregister_interface(dev->conn, path, HEALTH_CHANNEL);
		g_free(path);
	}

	dev->ndc = create_channel(dev, *conf, NULL, mdlid, app, NULL);
	if (!dev->ndc)
		return MCAP_MDEP_BUSY;

	return MCAP_SUCCESS;
}

static uint8_t hdp_mcap_mdl_reconn_req_cb(struct mcap_mdl *mdl, void *data)
{
	struct hdp_device *dev = data;
	struct hdp_channel *chan;
	GSList *l;

	l = g_slist_find_custom(dev->channels, mdl, cmp_chan_mdl);
	if (!l)
		return MCAP_INVALID_MDL;

	chan = l->data;

	if (!dev->fr && (chan->config != HDP_RELIABLE_DC))
		return MCAP_UNSPECIFIED_ERROR;

	dev->ndc = chan;

	return MCAP_SUCCESS;
}

gboolean hdp_set_mcl_cb(struct hdp_device *device, GError **err)
{
	gboolean ret;

	ret =  mcap_mcl_set_cb(device->mcl, device, err,
		MCAP_MDL_CB_CONNECTED, hdp_mcap_mdl_connected_cb,
		MCAP_MDL_CB_CLOSED, hdp_mcap_mdl_closed_cb,
		MCAP_MDL_CB_DELETED, hdp_mcap_mdl_deleted_cb,
		MCAP_MDL_CB_ABORTED, hdp_mcap_mdl_aborted_cb,
		MCAP_MDL_CB_REMOTE_CONN_REQ, hdp_mcap_mdl_conn_req_cb,
		MCAP_MDL_CB_REMOTE_RECONN_REQ, hdp_mcap_mdl_reconn_req_cb,
		MCAP_MDL_CB_INVALID);

	if (ret)
		return TRUE;

	error("Can't set mcl callbacks, closing mcl");
	mcap_close_mcl(device->mcl, TRUE);
	device->mcl_conn = FALSE;
	return FALSE;
}

static void mcl_connected(struct mcap_mcl *mcl, gpointer data)
{
	struct hdp_adapter *hdp_adapter = data;
	struct hdp_device *hdp_device;
	struct btd_device *device;
	bdaddr_t addr;
	char str[18];
	GSList *l;

	mcap_mcl_get_addr(mcl, &addr);
	l = g_slist_find_custom(devices, &addr, cmp_dev_addr);
	if (!l) {
		ba2str(&addr, str);
		device = adapter_get_device(connection,
					hdp_adapter->btd_adapter, str);
		if (!device)
			return;
		hdp_device = create_health_device(connection, device);
		if (!hdp_device)
			return;
		devices = g_slist_append(devices, hdp_device);
	} else
		hdp_device = l->data;
	hdp_device->mcl = mcap_mcl_ref(mcl);
	hdp_device->mcl_conn = TRUE;

	DBG("New mcl connected from  %s", device_get_path(hdp_device->dev));

	hdp_set_mcl_cb(hdp_device, NULL);
}

static void mcl_reconnected(struct mcap_mcl *mcl, gpointer data)
{
	struct hdp_device *hdp_device;
	GSList *l;

	l = g_slist_find_custom(devices, mcl, cmp_dev_mcl);
	if (!l)
		return;

	hdp_device = l->data;
	hdp_device->mcl_conn = TRUE;

	DBG("MCL reconnected %s", device_get_path(hdp_device->dev));

	hdp_set_mcl_cb(hdp_device, NULL);
}

static void mcl_disconnected(struct mcap_mcl *mcl, gpointer data)
{
	struct hdp_device *hdp_device;
	GSList *l;

	l = g_slist_find_custom(devices, mcl, cmp_dev_mcl);
	if (!l)
		return;

	hdp_device = l->data;
	hdp_device->mcl_conn = FALSE;
	DBG("Mcl disconnected %s", device_get_path(hdp_device->dev));
}

static void mcl_uncached(struct mcap_mcl *mcl, gpointer data)
{
	struct hdp_device *hdp_device;
	const char *path;
	GSList *l;

	l = g_slist_find_custom(devices, mcl, cmp_dev_mcl);
	if (!l)
		return;

	hdp_device = l->data;
	device_unref_mcl(hdp_device);

	if (hdp_device->sdp_present)
		return;

	/* Because remote device hasn't announced an HDP record */
	/* the Bluetooth daemon won't notify when the device shall */
	/* be removed. Then we have to remove the HealthDevice */
	/* interface manually */
	path = device_get_path(hdp_device->dev);
	g_dbus_unregister_interface(hdp_device->conn, path, HEALTH_DEVICE);
	DBG("Mcl uncached %s", path);
}

static void check_devices_mcl()
{
	struct hdp_device *dev;
	GSList *l, *to_delete = NULL;
	const char *path;

	for (l = devices; l; l = l->next) {
		dev = l->data;
		device_unref_mcl(dev);

		if (!dev->sdp_present)
			to_delete = g_slist_append(to_delete, dev);
	}

	for (l = to_delete; l; l = l->next) {
		path = device_get_path(dev->dev);
		g_dbus_unregister_interface(dev->conn, path, HEALTH_DEVICE);
	}

	g_slist_free(to_delete);
}

static gboolean update_adapter(struct hdp_adapter *hdp_adapter)
{
	GError *err = NULL;
	bdaddr_t addr;

	if (!applications) {
		if (hdp_adapter->mi) {
			mcap_release_instance(hdp_adapter->mi);
			hdp_adapter->mi = NULL;
			check_devices_mcl();
		}
		goto update;
	}

	if (hdp_adapter->mi)
		goto update;

	adapter_get_address(hdp_adapter->btd_adapter, &addr);
	hdp_adapter->mi = mcap_create_instance(&addr, BT_IO_SEC_HIGH, 0, 0,
					mcl_connected, mcl_reconnected,
					mcl_disconnected, mcl_uncached,
					NULL, /* CSP is not used by now */
					hdp_adapter, &err);

	if (!hdp_adapter->mi) {
		error("Error creating the MCAP instance: %s", err->message);
		g_error_free(err);
		return FALSE;
	}

	hdp_adapter->ccpsm = mcap_get_ctrl_psm(hdp_adapter->mi, &err);
	if (err) {
		error("Error getting MCAP control PSM: %s", err->message);
		goto fail;
	}

	hdp_adapter->dcpsm = mcap_get_data_psm(hdp_adapter->mi, &err);
	if (err) {
		error("Error getting MCAP data PSM: %s", err->message);
		goto fail;
	}

update:
	if (hdp_update_sdp_record(hdp_adapter, applications))
		return TRUE;
	error("Error updating the SDP record");

fail:
	if (hdp_adapter->mi)
		mcap_release_instance(hdp_adapter->mi);
	if (err)
		g_error_free(err);
	return FALSE;
}

int hdp_adapter_register(DBusConnection *conn, struct btd_adapter *adapter)
{
	struct hdp_adapter *hdp_adapter;

	hdp_adapter = g_new0(struct hdp_adapter, 1);
	hdp_adapter->btd_adapter = btd_adapter_ref(adapter);

	if(!update_adapter(hdp_adapter))
		goto fail;

	adapters = g_slist_append(adapters, hdp_adapter);

	return 0;

fail:
	btd_adapter_unref(hdp_adapter->btd_adapter);
	g_free(hdp_adapter);
	return -1;
}

void hdp_adapter_unregister(struct btd_adapter *adapter)
{
	struct hdp_adapter *hdp_adapter;
	GSList *l;

	l = g_slist_find_custom(adapters, adapter, cmp_adapter);

	if (!l)
		return;

	hdp_adapter = l->data;
	adapters = g_slist_remove(adapters, hdp_adapter);
	if (hdp_adapter->sdp_handler)
		remove_record_from_server(hdp_adapter->sdp_handler);
	if (hdp_adapter->mi)
		mcap_release_instance(hdp_adapter->mi);
	btd_adapter_unref(hdp_adapter->btd_adapter);
	g_free(hdp_adapter);
}

static DBusMessage *device_echo(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	return g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
					"Echo function not implemented");
}

static void delete_mdl_cb(GError *err, gpointer data)
{
	if (err)
		error("Deleting error: %s", err->message);
}

static void destroy_create_dc_data(gpointer data)
{
	struct hdp_create_dc *dc_data = data;

	hdp_create_data_unref(dc_data);
}

static void abort_mdl_cb(GError *err, gpointer data)
{
	if (err)
		error("Aborting error: %s", err->message);
}

static void abort_and_del_mdl_cb(GError *err, gpointer data)
{
	struct mcap_mdl *mdl = data;
	GError *gerr = NULL;

	if (err) {
		error("%s", err->message);
		if (err->code == MCAP_INVALID_MDL) {
			/* MDL is removed from MCAP so we don't */
			/* need to delete it. */
			return;
		}
	}

	if (!mcap_delete_mdl(mdl, delete_mdl_cb, mdl, NULL, &gerr)) {
		error("%s", gerr->message);
		g_error_free(gerr);
	}
}

static void hdp_tmp_dc_data_destroy(gpointer data)
{
	struct hdp_tmp_dc_data *hdp_conn = data;

	hdp_tmp_dc_data_unref(hdp_conn);
}

static void hdp_mdl_conn_cb(struct mcap_mdl *mdl, GError *err, gpointer data)
{
	struct hdp_tmp_dc_data *hdp_conn =  data;
	struct hdp_channel *hdp_chann = hdp_conn->hdp_chann;
	DBusMessage *reply;
	GError *gerr = NULL;

	if (err) {
		error("%s", err->message);
		reply = g_dbus_create_reply(hdp_conn->msg,
					DBUS_TYPE_OBJECT_PATH, &hdp_chann->path,
					DBUS_TYPE_INVALID);
		g_dbus_send_message(hdp_conn->conn, reply);

		/* Send abort request because remote side */
		/* is now in PENDING state */
		if (!mcap_mdl_abort(hdp_chann->mdl, abort_mdl_cb, hdp_chann,
								NULL, &gerr)) {
			error("%s", gerr->message);
			g_error_free(gerr);
		}
		return;
	}

	hdp_chann->mdl_conn = TRUE;
	reply = g_dbus_create_reply(hdp_conn->msg,
					DBUS_TYPE_OBJECT_PATH, &hdp_chann->path,
					DBUS_TYPE_INVALID);
	g_dbus_send_message(hdp_conn->conn, reply);
}

static void hdp_get_dcpsm_cb(uint16_t dcpsm, gpointer user_data, GError *err)
{
	struct hdp_tmp_dc_data *hdp_conn = user_data;
	struct hdp_channel *hdp_chann = hdp_conn->hdp_chann;
	GError *gerr = NULL;
	DBusMessage *reply;
	uint8_t mode;

	if (err) {
		error("%s", err->message);
		goto fail;
	}

	if (hdp_chann->config == HDP_RELIABLE_DC)
		mode = L2CAP_MODE_ERTM;
	else
		mode = L2CAP_MODE_STREAMING;

	if (mcap_connect_mdl(hdp_chann->mdl, mode, dcpsm, hdp_mdl_conn_cb,
						hdp_tmp_dc_data_ref(hdp_conn),
						hdp_tmp_dc_data_destroy, &gerr))
		return;

fail:
	error("%s", gerr->message);
	g_error_free(gerr);
	gerr = NULL;

	reply = g_dbus_create_reply(hdp_conn->msg,
					DBUS_TYPE_OBJECT_PATH, &hdp_chann->path,
					DBUS_TYPE_INVALID);
	g_dbus_send_message(hdp_conn->conn, reply);
	hdp_tmp_dc_data_unref(hdp_conn);

	/* Send abort request because remote side is now in PENDING state */
	if (!mcap_mdl_abort(hdp_chann->mdl, abort_mdl_cb, hdp_chann, NULL,
								&gerr)) {
		error("%s", gerr->message);
		g_error_free(gerr);
	}
}

static void device_create_mdl_cb(struct mcap_mdl *mdl, uint8_t conf,
						GError *err, gpointer data)
{
	struct hdp_create_dc *user_data = data;
	struct hdp_tmp_dc_data *hdp_conn;
	struct hdp_channel *hdp_chan;
	GError *gerr = NULL;
	DBusMessage *reply;

	if (err) {
		reply = g_dbus_create_error(user_data->msg,
					ERROR_INTERFACE ".HealthError",
					"%s", err->message);
		g_dbus_send_message(user_data->conn, reply);
		return;
	}

	if (user_data->config == HDP_NO_PREFERENCE_DC) {
		if (!user_data->dev->fr && (conf != HDP_RELIABLE_DC)) {
			g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
					"Data channel aborted, fist data "
					"channel should be reliable");
			goto fail;
		} else if (conf == HDP_NO_PREFERENCE_DC ||
						conf > HDP_STREAMING_DC) {
			g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
							"Data channel aborted, "
							"configuration error");
			goto fail;
		}
	}

	hdp_chan = create_channel(user_data->dev, conf, mdl,
							mcap_mdl_get_mdlid(mdl),
							user_data->app, &gerr);
	if (!hdp_chan)
		goto fail;

	hdp_conn = g_new0(struct hdp_tmp_dc_data, 1);
	hdp_conn->msg = dbus_message_ref(user_data->msg);
	hdp_conn->conn = dbus_connection_ref(user_data->conn);
	hdp_conn->hdp_chann = hdp_chan;

	if (hdp_get_dcpsm(hdp_chan->dev, hdp_get_dcpsm_cb,
						hdp_tmp_dc_data_ref(hdp_conn),
						hdp_tmp_dc_data_destroy, &gerr))
		return;

	error("%s", gerr->message);
	g_error_free(gerr);
	gerr = NULL;

	reply = g_dbus_create_reply(hdp_conn->msg,
					DBUS_TYPE_OBJECT_PATH, &hdp_chan->path,
					DBUS_TYPE_INVALID);
	g_dbus_send_message(hdp_conn->conn, reply);
	hdp_tmp_dc_data_unref(hdp_conn);

	/* Send abort request because remote side is now in PENDING state */
	if (!mcap_mdl_abort(hdp_chan->mdl, abort_mdl_cb, hdp_chan, NULL,
								&gerr)) {
		error("%s", gerr->message);
		g_error_free(gerr);
	}

	return;

fail:

	reply = g_dbus_create_error(user_data->msg,
						ERROR_INTERFACE ".HealthError",
						"%s", gerr->message);
	g_dbus_send_message(user_data->conn, reply);
	g_error_free(gerr);
	gerr = NULL;

	/* Send abort request because remote side is now in PENDING */
	/* state. Then we have to delete it because we couldn't */
	/* register the HealthChannel interface */
	if (!mcap_mdl_abort(mdl, abort_and_del_mdl_cb, mdl, NULL, &gerr)) {
		error("%s", gerr->message);
		g_error_free(gerr);
	}
}

static void device_create_dc_cb(gpointer user_data, GError *err)
{
	struct hdp_create_dc *dc_data, *data = user_data;
	DBusMessage *reply;
	GError *gerr = NULL;

	if (err) {
		reply = g_dbus_create_error(data->msg,
					ERROR_INTERFACE ".HealthError",
					"%s", err->message);
		g_dbus_send_message(data->conn, reply);
		return;
	}

	dc_data = hdp_create_data_ref(data);

	if (mcap_create_mdl(dc_data->dev->mcl, dc_data->mdep, dc_data->config,
						device_create_mdl_cb, dc_data,
						destroy_create_dc_data, &gerr))
		return;

	reply = g_dbus_create_error(data->msg, ERROR_INTERFACE ".HealthError",
							"%s", gerr->message);
	hdp_create_data_unref(dc_data);
	g_error_free(gerr);
	g_dbus_send_message(data->conn, reply);
}

static void device_get_mdep_cb(uint8_t mdep, gpointer data, GError *err)
{
	struct hdp_create_dc *dc_data, *user_data = data;
	DBusMessage *reply;
	GError *gerr = NULL;

	if (err) {
		reply = g_dbus_create_error(user_data->msg,
						ERROR_INTERFACE ".HealthError",
						"%s", err->message);
		g_dbus_send_message(user_data->conn, reply);
		return;
	}

	dc_data = hdp_create_data_ref(user_data);
	dc_data->mdep = mdep;

	if (user_data->dev->mcl_conn) {
		device_create_dc_cb(dc_data, NULL);
		hdp_create_data_unref(dc_data);
		return;
	}

	if (hdp_establish_mcl(dc_data->dev, dc_data->app, device_create_dc_cb,
					dc_data, destroy_create_dc_data, &gerr))
		return;

	reply = g_dbus_create_error(user_data->msg,
						ERROR_INTERFACE ".HealthError",
						"%s", gerr->message);
	hdp_create_data_unref(dc_data);
	g_error_free(gerr);
	g_dbus_send_message(user_data->conn, reply);
}

static DBusMessage *device_create_channel(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	struct hdp_device *device = user_data;
	struct hdp_application *app;
	struct hdp_create_dc *data;
	char *app_path, *conf;
	DBusMessage *reply;
	GError *err = NULL;
	uint8_t config;
	GSList *l;

	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &app_path,
							DBUS_TYPE_STRING, &conf,
							DBUS_TYPE_INVALID)) {
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments in method call");
	}

	l = g_slist_find_custom(applications, app_path, cmp_app);
	if (!l)
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments in method call, "
					"no such application");
	app = l->data;

	if (g_ascii_strcasecmp("Reliable", conf) == 0)
		config = HDP_RELIABLE_DC;
	else if (g_ascii_strcasecmp("Streaming", conf) == 0)
		config = HDP_STREAMING_DC;
	else if (g_ascii_strcasecmp("Any", conf) == 0)
		config = HDP_NO_PREFERENCE_DC;
	else
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments in method call");

	if (app->role == HDP_SINK && config != HDP_NO_PREFERENCE_DC)
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Configuration not valid for sinks");

	if (app->role == HDP_SOURCE && config == HDP_NO_PREFERENCE_DC)
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Configuration not valid for sources");

	data = g_new0(struct hdp_create_dc, 1);
	data->dev = device;
	data->config = config;
	data->app = app;
	data->msg = dbus_message_ref(msg);
	data->conn = dbus_connection_ref(conn);

	if (hdp_get_mdep(device, l->data, device_get_mdep_cb,
						hdp_create_data_ref(data),
						destroy_create_dc_data, &err))
		return NULL;

	reply = g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
							"%s", err->message);
	g_error_free(err);
	hdp_create_data_unref(data);
	return reply;
}

static void hdp_mdl_delete_cb(GError *err, gpointer data)
{
	struct hdp_tmp_dc_data *del_data = data;
	DBusMessage *reply;
	char *path;

	if (err) {
		if (err->code != MCAP_INVALID_MDL) {
			reply = g_dbus_create_error(del_data->msg,
					ERROR_INTERFACE ".HealthError",
					"%s", err->message);
			g_dbus_send_message(del_data->conn, reply);
			return;
		}
	}

	path = g_strdup(del_data->hdp_chann->path);
	g_dbus_unregister_interface(del_data->conn, path, HEALTH_CHANNEL);
	g_free(path);

	reply = g_dbus_create_reply(del_data->msg, DBUS_TYPE_INVALID);
	g_dbus_send_message(del_data->conn, reply);
}

static void hdp_continue_del_cb(gpointer user_data, GError *err)
{
	struct hdp_tmp_dc_data *del_data = user_data;
	GError *gerr = NULL;
	DBusMessage *reply;

	if (err) {
		reply = g_dbus_create_error(del_data->msg,
					ERROR_INTERFACE ".HealthError",
					"%s", err->message);
		g_dbus_send_message(del_data->conn, reply);
		return;
	}

	if (mcap_delete_mdl(del_data->hdp_chann->mdl, hdp_mdl_delete_cb,
						hdp_tmp_dc_data_ref(del_data),
						hdp_tmp_dc_data_destroy, &gerr))
			return;

	reply = g_dbus_create_error(del_data->msg,
						ERROR_INTERFACE ".HealthError",
						"%s", gerr->message);
	hdp_tmp_dc_data_unref(del_data);
	g_error_free(gerr);
	g_dbus_send_message(del_data->conn, reply);
}

static DBusMessage *device_destroy_channel(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	struct hdp_device *device = user_data;
	struct hdp_tmp_dc_data *del_data;
	struct hdp_channel *hdp_chan;
	DBusMessage *reply;
	GError *err = NULL;
	char *path;
	GSList *l;

	if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
							DBUS_TYPE_INVALID)){
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments in method call");
	}

	l = g_slist_find_custom(device->channels, path, cmp_chan_path);
	if (!l)
		return g_dbus_create_error(msg,
					ERROR_INTERFACE ".InvalidArguments",
					"Invalid arguments in method call, "
					"no such channel");
	hdp_chan = l->data;
	del_data = g_new0(struct hdp_tmp_dc_data, 1);
	del_data->msg = dbus_message_ref(msg);
	del_data->conn = dbus_connection_ref(conn);
	del_data->hdp_chann = hdp_chan;

	if (device->mcl_conn) {
		if (mcap_delete_mdl(hdp_chan->mdl, hdp_mdl_delete_cb,
						hdp_tmp_dc_data_ref(del_data),
						hdp_tmp_dc_data_destroy, &err))
			return NULL;
		goto fail;
	}

	if (hdp_establish_mcl(device, hdp_chan->app, hdp_continue_del_cb,
						hdp_tmp_dc_data_ref(del_data),
						hdp_tmp_dc_data_destroy, &err))
		return NULL;

fail:
	reply = g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
							"%s", err->message);
	hdp_tmp_dc_data_unref(del_data);
	g_error_free(err);
	return reply;
}

static DBusMessage *device_get_properties(DBusConnection *conn,
					DBusMessage *msg, void *user_data)
{
	struct hdp_device *device = user_data;
	DBusMessageIter iter, dict;
	DBusMessage *reply;
	char *path;

	reply = dbus_message_new_method_return(msg);
	if (!reply)
		return NULL;

	dbus_message_iter_init_append(reply, &iter);

	dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
			DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
			DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING
			DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);

	if (device->fr)
		path = g_strdup(device->fr->path);
	else
		path = g_strdup("");
	dict_append_entry(&dict, "MainChannel", DBUS_TYPE_STRING, &path);
	g_free(path);
	dbus_message_iter_close_container(&iter, &dict);

	return reply;
}

static void health_device_destroy(void *data)
{
	struct hdp_device *device = data;

	DBG("Unregistered interface %s on path %s", HEALTH_DEVICE,
						device_get_path(device->dev));
	devices = g_slist_remove(devices, device);
	free_health_device(device);
}

static GDBusMethodTable health_device_methods[] = {
	{"Echo",		"",	"b",	device_echo,
						G_DBUS_METHOD_FLAG_ASYNC },
	{"CreateChannel",	"os",	"o",	device_create_channel,
						G_DBUS_METHOD_FLAG_ASYNC },
	{"DestroyChannel",	"o",	"",	device_destroy_channel,
						G_DBUS_METHOD_FLAG_ASYNC },
	{"GetProperties",	"",	"a{sv}", device_get_properties},
	{ NULL }
};

static GDBusSignalTable health_device_signals[] = {
	{"ChannelConnected",		"o"		},
	{"ChannelDeleted",		"o"		},
	{"PropertyChanged",		"sv"		},
	{ NULL }
};

static struct hdp_device *create_health_device(DBusConnection *conn,
						struct btd_device *device)
{
	struct btd_adapter *adapter = device_get_adapter(device);
	const gchar *path = device_get_path(device);
	struct hdp_device *dev;
	GSList *l;

	if (!device)
		return NULL;
	dev = g_new0(struct hdp_device, 1);
	dev->conn = dbus_connection_ref(conn);
	dev->dev = btd_device_ref(device);
	l = g_slist_find_custom(adapters, adapter, cmp_adapter);

	if (!l)
		goto fail;

	dev->hdp_adapter = l->data;

	if (!g_dbus_register_interface(conn, path,
					HEALTH_DEVICE,
					health_device_methods,
					health_device_signals, NULL,
					dev, health_device_destroy)) {
		error("D-Bus failed to register %s interface", HEALTH_DEVICE);
		goto fail;
	}

	DBG("Registered interface %s on path %s", HEALTH_DEVICE, path);
	return dev;

fail:
	free_health_device(dev);
	return NULL;
}

int hdp_device_register(DBusConnection *conn, struct btd_device *device)
{
	struct hdp_device *hdev;
	GSList *l;

	l = g_slist_find_custom(devices, device, cmp_device);
	if (l) {
		hdev = l->data;
		hdev->sdp_present = TRUE;
		return 0;
	}

	hdev = create_health_device(conn, device);
	if (!hdev)
		return -1;

	hdev->sdp_present = TRUE;

	devices = g_slist_prepend(devices, hdev);
	return 0;
}

void hdp_device_unregister(struct btd_device *device)
{
	struct hdp_device *hdp_dev;
	const char *path;
	GSList *l;

	l = g_slist_find_custom(devices, device, cmp_device);
	if (!l)
		return;

	hdp_dev = l->data;
	path = device_get_path(hdp_dev->dev);
	g_dbus_unregister_interface(hdp_dev->conn, path, HEALTH_DEVICE);
}

int hdp_manager_start(DBusConnection *conn)
{
	DBG("Starting Health manager");

	if (!g_dbus_register_interface(conn, MANAGER_PATH,
					HEALTH_MANAGER,
					health_manager_methods, NULL, NULL,
					NULL, manager_path_unregister)) {
		error("D-Bus failed to register %s interface", HEALTH_MANAGER);
		return -1;
	}

	connection = dbus_connection_ref(conn);

	return 0;
}

void hdp_manager_stop()
{
	g_dbus_unregister_interface(connection, MANAGER_PATH, HEALTH_MANAGER);

	dbus_connection_unref(connection);
	DBG("Stopped Health manager");
}