Imported upstream 4.91upstream-4.91upstreampackaging

13 files changed, 7596 insertions, 0 deletions

diff --git a/health/hdp.c b/health/hdp.c
new file mode 100644
index 0000000..7fed483
--- /dev/null
+++ b/health/hdp.c
@@ -0,0 +1,2223 @@
+/*
+ *
+ *  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 <stdlib.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
+
+#define ECHO_TIMEOUT	1 /* second */
+#define HDP_ECHO_LEN	15
+
+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);
+static void free_echo_data(struct hdp_echo_data *edata);
+
+struct hdp_create_dc {
+	DBusConnection			*conn;
+	DBusMessage			*msg;
+	struct hdp_application		*app;
+	struct hdp_device		*dev;
+	uint8_t				config;
+	uint8_t				mdep;
+	guint				ref;
+	mcap_mdl_operation_cb		cb;
+};
+
+struct hdp_tmp_dc_data {
+	DBusConnection			*conn;
+	DBusMessage			*msg;
+	struct hdp_channel		*hdp_chann;
+	guint				ref;
+	mcap_mdl_operation_cb		cb;
+};
+
+struct hdp_echo_data {
+	gboolean		echo_done;	/* Is a echo was already done */
+	gpointer		buf;		/* echo packet sent */
+	uint			tid;		/* echo timeout */
+};
+
+static struct hdp_channel *hdp_channel_ref(struct hdp_channel *chan)
+{
+	if (!chan)
+		return NULL;
+
+	chan->ref++;
+
+	DBG("health_channel_ref(%p): ref=%d", chan, chan->ref);
+	return chan;
+}
+
+static void free_health_channel(struct hdp_channel *chan)
+{
+	if (chan->mdep == HDP_MDEP_ECHO) {
+		free_echo_data(chan->edata);
+		chan->edata = NULL;
+	}
+
+	mcap_mdl_unref(chan->mdl);
+	hdp_application_unref(chan->app);
+	health_device_unref(chan->dev);
+	g_free(chan->path);
+	g_free(chan);
+}
+
+static void hdp_channel_unref(struct hdp_channel *chan)
+{
+	if (!chan)
+		return;
+
+	chan->ref --;
+	DBG("health_channel_unref(%p): ref=%d", chan, chan->ref);
+
+	if (chan->ref > 0)
+		return;
+
+	free_health_channel(chan);
+}
+
+static void free_hdp_create_dc(struct hdp_create_dc *dc_data)
+{
+	dbus_message_unref(dc_data->msg);
+	dbus_connection_unref(dc_data->conn);
+	hdp_application_unref(dc_data->app);
+	health_device_unref(dc_data->dev);
+
+	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_unref(%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);
+	hdp_channel_unref(data->hdp_chann);
+
+	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_unref(%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()
+{
+	uint8_t id = next_app_id;
+
+	do {
+		GSList *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_close_mcl(hdp_device->mcl, FALSE);
+	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 remove_application(struct hdp_application *app)
+{
+	DBG("Application %s deleted", app->path);
+	hdp_application_unref(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;
+
+	dbus_message_iter_init(msg, &iter);
+	app = hdp_get_app_config(&iter, &err);
+	if (err) {
+		g_error_free(err);
+		return btd_error_invalid_args(msg);
+	}
+
+	name = dbus_message_get_sender(msg);
+	if (!name) {
+		hdp_application_unref(app);
+		return g_dbus_create_error(msg,
+					ERROR_INTERFACE ".HealthError",
+					"Can't get sender name");
+	}
+
+	if (!set_app_path(app)) {
+		hdp_application_unref(app);
+		return g_dbus_create_error(msg,
+				ERROR_INTERFACE ".HealthError",
+				"Can't get a valid id for the application");
+	}
+
+	app->oname = g_strdup(name);
+	app->conn = dbus_connection_ref(conn);
+
+	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 btd_error_invalid_args(msg);
+
+	l = g_slist_find_custom(applications, 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;
+	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) hdp_application_unref, 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)
+{
+	struct hdp_channel *chan = user_data;
+	DBusMessageIter iter, dict;
+	DBusMessage *reply;
+	const char *path;
+	char *type;
+
+	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);
+
+	path = device_get_path(chan->dev->dev);
+	dict_append_entry(&dict, "Device", DBUS_TYPE_OBJECT_PATH, &path);
+
+	path = chan->app->path;
+	dict_append_entry(&dict, "Application", DBUS_TYPE_OBJECT_PATH, &path);
+
+	if (chan->config == HDP_RELIABLE_DC)
+		type = g_strdup("Reliable");
+	else
+		type = g_strdup("Streaming");
+
+	dict_append_entry(&dict, "Type", DBUS_TYPE_STRING, &type);
+
+	g_free(type);
+
+	dbus_message_iter_close_container(&iter, &dict);
+
+	return reply;
+}
+
+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 abort_mdl_cb(GError *err, gpointer data)
+{
+	if (err)
+		error("Aborting error: %s", err->message);
+}
+
+static void hdp_mdl_reconn_cb(struct mcap_mdl *mdl, GError *err, gpointer data)
+{
+	struct hdp_tmp_dc_data *dc_data = data;
+	DBusMessage *reply;
+	int fd;
+
+	if (err) {
+		struct hdp_channel *chan = dc_data->hdp_chann;
+		GError *gerr = NULL;
+
+		error("%s", err->message);
+		reply = g_dbus_create_error(dc_data->msg,
+					ERROR_INTERFACE ".HealthError",
+					"Cannot reconnect: %s", err->message);
+		g_dbus_send_message(dc_data->conn, reply);
+
+		/* Send abort request because remote side */
+		/* is now in PENDING state */
+		if (!mcap_mdl_abort(chan->mdl, abort_mdl_cb, NULL, NULL,
+								&gerr)) {
+			error("%s", gerr->message);
+			g_error_free(gerr);
+		}
+		return;
+	}
+
+	fd = mcap_mdl_get_fd(dc_data->hdp_chann->mdl);
+	if (fd < 0) {
+		reply = g_dbus_create_error(dc_data->msg,
+						ERROR_INTERFACE ".HealthError",
+						"Cannot get file descriptor");
+		g_dbus_send_message(dc_data->conn, reply);
+		return;
+	}
+
+	reply = g_dbus_create_reply(dc_data->msg, DBUS_TYPE_UNIX_FD,
+							&fd, DBUS_TYPE_INVALID);
+	g_dbus_send_message(dc_data->conn, reply);
+
+	g_dbus_emit_signal(dc_data->conn,
+			device_get_path(dc_data->hdp_chann->dev->dev),
+			HEALTH_DEVICE, "ChannelConnected",
+			DBUS_TYPE_OBJECT_PATH, &dc_data->hdp_chann->path,
+			DBUS_TYPE_INVALID);
+}
+
+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;
+	uint8_t mode;
+
+	if (err) {
+		hdp_conn->cb(hdp_chann->mdl, err, hdp_conn);
+		return;
+	}
+
+	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_conn->cb,
+					hdp_tmp_dc_data_ref(hdp_conn),
+					hdp_tmp_dc_data_destroy, &gerr))
+		return;
+
+	hdp_tmp_dc_data_unref(hdp_conn);
+	hdp_conn->cb(hdp_chann->mdl, err, hdp_conn);
+	g_error_free(gerr);
+	gerr = NULL;
+}
+
+static void device_reconnect_mdl_cb(struct mcap_mdl *mdl, GError *err,
+								gpointer data)
+{
+	struct hdp_tmp_dc_data *dc_data = data;
+	GError *gerr = NULL;
+	DBusMessage *reply;
+
+	if (err) {
+		reply = g_dbus_create_error(dc_data->msg,
+					ERROR_INTERFACE ".HealthError",
+					"Cannot reconnect: %s", err->message);
+		g_dbus_send_message(dc_data->conn, reply);
+		return;
+	}
+
+	dc_data->cb = hdp_mdl_reconn_cb;
+
+	if (hdp_get_dcpsm(dc_data->hdp_chann->dev, hdp_get_dcpsm_cb,
+					hdp_tmp_dc_data_ref(dc_data),
+					hdp_tmp_dc_data_destroy, &gerr))
+		return;
+
+	error("%s", gerr->message);
+
+	reply = g_dbus_create_error(dc_data->msg,
+					ERROR_INTERFACE ".HealthError",
+					"Cannot reconnect: %s", gerr->message);
+	g_dbus_send_message(dc_data->conn, reply);
+	hdp_tmp_dc_data_unref(dc_data);
+	g_error_free(gerr);
+	gerr = NULL;
+
+	/* Send abort request because remote side is now in PENDING state */
+	if (!mcap_mdl_abort(mdl, abort_mdl_cb, NULL, NULL, &gerr)) {
+		error("%s", gerr->message);
+		g_error_free(gerr);
+	}
+}
+
+static DBusMessage *channel_acquire_continue(struct hdp_tmp_dc_data *data,
+								GError *err)
+{
+	DBusMessage *reply;
+	GError *gerr = NULL;
+	int fd;
+
+	if (err) {
+		return g_dbus_create_error(data->msg,
+						ERROR_INTERFACE ".HealthError",
+						"%s", err->message);
+	}
+
+	fd = mcap_mdl_get_fd(data->hdp_chann->mdl);
+	if (fd >= 0)
+		return g_dbus_create_reply(data->msg, DBUS_TYPE_UNIX_FD, &fd,
+							DBUS_TYPE_INVALID);
+
+	hdp_tmp_dc_data_ref(data);
+	if (mcap_reconnect_mdl(data->hdp_chann->mdl, device_reconnect_mdl_cb,
+					data, hdp_tmp_dc_data_destroy, &gerr))
+		return NULL;
+
+	hdp_tmp_dc_data_unref(data);
+	reply = g_dbus_create_error(data->msg, ERROR_INTERFACE ".HealthError",
+					"Cannot reconnect: %s", gerr->message);
+	g_error_free(gerr);
+
+	return reply;
+}
+
+static void channel_acquire_cb(gpointer data, GError *err)
+{
+	struct hdp_tmp_dc_data *dc_data = data;
+	DBusMessage *reply;
+
+	reply = channel_acquire_continue(data, err);
+
+	if (reply)
+		g_dbus_send_message(dc_data->conn, reply);
+}
+
+static DBusMessage *channel_acquire(DBusConnection *conn,
+					DBusMessage *msg, void *user_data)
+{
+	struct hdp_channel *chan = user_data;
+	struct hdp_tmp_dc_data *dc_data;
+	GError *gerr = NULL;
+	DBusMessage *reply;
+
+	dc_data = g_new0(struct hdp_tmp_dc_data, 1);
+	dc_data->conn = dbus_connection_ref(conn);
+	dc_data->msg = dbus_message_ref(msg);
+	dc_data->hdp_chann = hdp_channel_ref(chan);
+
+	if (chan->dev->mcl_conn) {
+		reply = channel_acquire_continue(hdp_tmp_dc_data_ref(dc_data),
+									NULL);
+		hdp_tmp_dc_data_unref(dc_data);
+		return reply;
+	}
+
+	if (hdp_establish_mcl(chan->dev, channel_acquire_cb,
+						hdp_tmp_dc_data_ref(dc_data),
+						hdp_tmp_dc_data_destroy, &gerr))
+		return NULL;
+
+	reply = g_dbus_create_error(msg, ERROR_INTERFACE ".HealthError",
+					"%s", gerr->message);
+	hdp_tmp_dc_data_unref(dc_data);
+	g_error_free(gerr);
+
+	return reply;
+}
+
+static void close_mdl(struct hdp_channel *hdp_chann)
+{
+	int fd;
+
+	fd = mcap_mdl_get_fd(hdp_chann->mdl);
+	if (fd < 0)
+		return;
+
+	close(fd);
+}
+
+static DBusMessage *channel_release(DBusConnection *conn,
+					DBusMessage *msg, void *user_data)
+{
+	struct hdp_channel *hdp_chann = user_data;
+
+	close_mdl(hdp_chann);
+
+	return g_dbus_create_reply(msg, DBUS_TYPE_INVALID);
+}
+
+static void free_echo_data(struct hdp_echo_data *edata)
+{
+	if (!edata)
+		return;
+
+	if (edata->tid)
+		g_source_remove(edata->tid);
+
+	if (edata->buf)
+		g_free(edata->buf);
+
+
+	g_free(edata);
+}
+
+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);
+	if (!g_slist_find(dev->channels, hdp_chan))
+		goto end;
+
+	dev->channels = g_slist_remove(dev->channels, hdp_chan);
+
+	if (hdp_chan->mdep != HDP_MDEP_ECHO)
+		g_dbus_emit_signal(dev->conn, device_get_path(dev->dev),
+					HEALTH_DEVICE, "ChannelDeleted",
+					DBUS_TYPE_OBJECT_PATH, &hdp_chan->path,
+					DBUS_TYPE_INVALID);
+
+	if (hdp_chan == dev->fr) {
+		char *empty_path;
+
+		hdp_channel_unref(dev->fr);
+		dev->fr = NULL;
+		empty_path = "/";
+		emit_property_changed(dev->conn, device_get_path(dev->dev),
+					HEALTH_DEVICE, "MainChannel",
+					DBUS_TYPE_OBJECT_PATH, &empty_path);
+	}
+
+end:
+	hdp_channel_unref(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;
+
+	if (!dev)
+		return NULL;
+
+	hdp_chann = g_new0(struct hdp_channel, 1);
+	hdp_chann->config = config;
+	hdp_chann->dev = health_device_ref(dev);
+	hdp_chann->mdlid = mdlid;
+
+	if (mdl)
+		hdp_chann->mdl = mcap_mdl_ref(mdl);
+
+	if (app) {
+		hdp_chann->mdep = app->id;
+		hdp_chann->app = hdp_application_ref(app);
+	} else
+		hdp_chann->edata = g_new0(struct hdp_echo_data, 1);
+
+	hdp_chann->path = g_strdup_printf("%s/chan%d",
+					device_get_path(hdp_chann->dev->dev),
+					hdp_chann->mdlid);
+
+	dev->channels = g_slist_append(dev->channels,
+						hdp_channel_ref(hdp_chann));
+
+	if (hdp_chann->mdep == HDP_MDEP_ECHO)
+		return hdp_channel_ref(hdp_chann);
+
+	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;
+	}
+
+	return hdp_channel_ref(hdp_chann);
+}
+
+static void remove_channels(struct hdp_device *dev)
+{
+	struct hdp_channel *chan;
+	char *path;
+
+	while (dev->channels) {
+		chan = dev->channels->data;
+
+		path = g_strdup(chan->path);
+		if (!g_dbus_unregister_interface(dev->conn, path,
+								HEALTH_CHANNEL))
+			health_channel_destroy(chan);
+		g_free(path);
+	}
+}
+
+static void close_device_con(struct hdp_device *dev, gboolean cache)
+{
+	if (!dev->mcl)
+		return;
+
+	mcap_close_mcl(dev->mcl, cache);
+	dev->mcl_conn = FALSE;
+
+	if (cache)
+		return;
+
+	device_unref_mcl(dev);
+	remove_channels(dev);
+
+	if (!dev->sdp_present) {
+		const char *path;
+
+		path = device_get_path(dev->dev);
+		g_dbus_unregister_interface(dev->conn, path, HEALTH_DEVICE);
+	}
+}
+
+static int send_echo_data(int sock, const void *buf, uint32_t size)
+{
+	const uint8_t *buf_b = buf;
+	uint32_t sent = 0;
+
+	while (sent < size) {
+		int n = write(sock, buf_b + sent, size - sent);
+		if (n < 0)
+			return -1;
+		sent += n;
+	}
+
+	return 0;
+}
+
+static gboolean serve_echo(GIOChannel *io_chan, GIOCondition cond,
+								gpointer data)
+{
+	struct hdp_channel *chan = data;
+	uint8_t buf[MCAP_DC_MTU];
+	int fd, len;
+
+	if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL)) {
+		hdp_channel_unref(chan);
+		return FALSE;
+	}
+
+	if (chan->edata->echo_done)
+		goto fail;
+
+	chan->edata->echo_done = TRUE;
+
+	fd = g_io_channel_unix_get_fd(io_chan);
+	len = read(fd, buf, sizeof(buf));
+
+	if (send_echo_data(fd, buf, len)  >= 0)
+		return TRUE;
+
+fail:
+	close_device_con(chan->dev, FALSE);
+	hdp_channel_unref(chan);
+	return FALSE;
+}
+
+static gboolean check_channel_conf(struct hdp_channel *chan)
+{
+	GError *err = NULL;
+	GIOChannel *io;
+	uint8_t mode;
+	uint16_t imtu, omtu;
+	int fd;
+
+	fd = mcap_mdl_get_fd(chan->mdl);
+	if (fd < 0)
+		return FALSE;
+	io = g_io_channel_unix_new(fd);
+
+	if (!bt_io_get(io, BT_IO_L2CAP, &err,
+			BT_IO_OPT_MODE, &mode,
+			BT_IO_OPT_IMTU, &imtu,
+			BT_IO_OPT_OMTU, &omtu,
+			BT_IO_OPT_INVALID)) {
+		error("Error: %s", err->message);
+		g_io_channel_unref(io);
+		g_error_free(err);
+		return FALSE;
+	}
+
+	g_io_channel_unref(io);
+
+	switch (chan->config) {
+	case HDP_RELIABLE_DC:
+		if (mode != L2CAP_MODE_ERTM)
+			return FALSE;
+		break;
+	case HDP_STREAMING_DC:
+		if (mode != L2CAP_MODE_STREAMING)
+			return FALSE;
+		break;
+	default:
+		error("Error: Connected with unknown configuration");
+		return FALSE;
+	}
+
+	DBG("MDL imtu %d omtu %d Channel imtu %d omtu %d", imtu, omtu,
+						chan->imtu, chan->omtu);
+
+	if (!chan->imtu)
+		chan->imtu = imtu;
+	if (!chan->omtu)
+		chan->omtu = omtu;
+
+	if (chan->imtu != imtu || chan->omtu != omtu)
+		return FALSE;
+
+	return TRUE;
+}
+
+static void hdp_mcap_mdl_connected_cb(struct mcap_mdl *mdl, void *data)
+{
+	struct hdp_device *dev = data;
+	struct hdp_channel *chan;
+
+	DBG("hdp_mcap_mdl_connected_cb");
+	if (!dev->ndc)
+		return;
+
+	chan = dev->ndc;
+	if (!chan->mdl)
+		chan->mdl = mcap_mdl_ref(mdl);
+
+	if (!g_slist_find(dev->channels, chan))
+		dev->channels = g_slist_prepend(dev->channels,
+							hdp_channel_ref(chan));
+
+	if (!check_channel_conf(chan)) {
+		close_mdl(chan);
+		goto end;
+	}
+
+	if (chan->mdep == HDP_MDEP_ECHO) {
+		GIOChannel *io;
+		int fd;
+
+		fd = mcap_mdl_get_fd(chan->mdl);
+		if (fd < 0)
+			goto end;
+
+		chan->edata->echo_done = FALSE;
+		io = g_io_channel_unix_new(fd);
+		g_io_add_watch(io, G_IO_ERR | G_IO_HUP | G_IO_NVAL | G_IO_IN,
+				serve_echo, hdp_channel_ref(chan));
+		g_io_channel_unref(io);
+		goto end;
+	}
+
+	g_dbus_emit_signal(dev->conn, device_get_path(dev->dev), HEALTH_DEVICE,
+					"ChannelConnected",
+					DBUS_TYPE_OBJECT_PATH, &chan->path,
+					DBUS_TYPE_INVALID);
+
+	if (dev->fr)
+		goto end;
+
+	dev->fr = hdp_channel_ref(chan);
+
+	emit_property_changed(dev->conn, device_get_path(dev->dev),
+					HEALTH_DEVICE, "MainChannel",
+					DBUS_TYPE_OBJECT_PATH, &dev->fr->path);
+
+end:
+	hdp_channel_unref(dev->ndc);
+	dev->ndc = NULL;
+}
+
+static void hdp_mcap_mdl_closed_cb(struct mcap_mdl *mdl, void *data)
+{
+	/* struct hdp_device *dev = data; */
+
+	DBG("hdp_mcap_mdl_closed_cb");
+
+	/* Nothing to do */
+}
+
+static void hdp_mcap_mdl_deleted_cb(struct mcap_mdl *mdl, void *data)
+{
+	struct hdp_device *dev = data;
+	struct hdp_channel *chan;
+	char *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;
+
+	path = g_strdup(chan->path);
+	if (!g_dbus_unregister_interface(dev->conn, path, HEALTH_CHANNEL))
+		health_channel_destroy(chan);
+	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 = mcap_mdl_ref(mdl);
+
+	if (!g_slist_find(dev->channels, dev->ndc))
+		dev->channels = g_slist_prepend(dev->channels,
+						hdp_channel_ref(dev->ndc));
+
+	if (dev->ndc->mdep != HDP_MDEP_ECHO)
+		g_dbus_emit_signal(dev->conn, device_get_path(dev->dev),
+					HEALTH_DEVICE, "ChannelConnected",
+					DBUS_TYPE_OBJECT_PATH, &dev->ndc->path,
+					DBUS_TYPE_INVALID);
+
+	hdp_channel_unref(dev->ndc);
+	dev->ndc = NULL;
+}
+
+static uint8_t hdp2l2cap_mode(uint8_t hdp_mode)
+{
+	return hdp_mode == HDP_STREAMING_DC ? L2CAP_MODE_STREAMING :
+								L2CAP_MODE_ERTM;
+}
+
+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;
+	GError *err = NULL;
+	GSList *l;
+
+	DBG("Data channel request");
+
+	if (mdepid == HDP_MDEP_ECHO) {
+		switch (*conf) {
+		case HDP_NO_PREFERENCE_DC:
+			*conf = HDP_RELIABLE_DC;
+		case HDP_RELIABLE_DC:
+			break;
+		case HDP_STREAMING_DC:
+			return MCAP_CONFIGURATION_REJECTED;
+		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. */
+			close_device_con(dev, FALSE);
+			return MCAP_CONFIGURATION_REJECTED; /* not processed */
+		}
+
+		if (!mcap_set_data_chan_mode(dev->hdp_adapter->mi,
+						L2CAP_MODE_ERTM, &err)) {
+			error("Error: %s", err->message);
+			g_error_free(err);
+			return MCAP_MDL_BUSY;
+		}
+
+		dev->ndc = create_channel(dev, *conf, NULL, mdlid, NULL, NULL);
+		if (!dev->ndc)
+			return MCAP_MDL_BUSY;
+
+		return MCAP_SUCCESS;
+	}
+
+	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 (dev->fr && 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. */
+		close_device_con(dev, FALSE);
+		return MCAP_CONFIGURATION_REJECTED; /* not processed */
+	}
+
+	l = g_slist_find_custom(dev->channels, &mdlid, cmp_chan_mdlid);
+	if (l) {
+		struct hdp_channel *chan = l->data;
+		char *path;
+
+		path = g_strdup(chan->path);
+		g_dbus_unregister_interface(dev->conn, path, HEALTH_CHANNEL);
+		g_free(path);
+	}
+
+	if (!mcap_set_data_chan_mode(dev->hdp_adapter->mi,
+						hdp2l2cap_mode(*conf), &err)) {
+		error("Error: %s", err->message);
+		g_error_free(err);
+		return MCAP_MDL_BUSY;
+	}
+
+	dev->ndc = create_channel(dev, *conf, NULL, mdlid, app, NULL);
+	if (!dev->ndc)
+		return MCAP_MDL_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;
+	GError *err = NULL;
+	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) &&
+						(chan->mdep != HDP_MDEP_ECHO))
+		return MCAP_UNSPECIFIED_ERROR;
+
+	if (!mcap_set_data_chan_mode(dev->hdp_adapter->mi,
+					hdp2l2cap_mode(chan->config), &err)) {
+		error("Error: %s", err->message);
+		g_error_free(err);
+		return MCAP_MDL_BUSY;
+	}
+
+	dev->ndc = hdp_channel_ref(chan);
+
+	return MCAP_SUCCESS;
+}
+
+gboolean hdp_set_mcl_cb(struct hdp_device *device, GError **err)
+{
+	gboolean ret;
+
+	if (!device->mcl)
+		return FALSE;
+
+	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");
+	close_device_con(device, TRUE);
+
+	return FALSE;
+}
+
+static void mcl_connected(struct mcap_mcl *mcl, gpointer data)
+{
+	struct hdp_device *hdp_device;
+	bdaddr_t addr;
+	GSList *l;
+
+	mcap_mcl_get_addr(mcl, &addr);
+	l = g_slist_find_custom(devices, &addr, cmp_dev_addr);
+	if (!l) {
+		struct hdp_adapter *hdp_adapter = data;
+		struct btd_device *device;
+		char str[18];
+
+		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;
+
+	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);
+		else
+			remove_channels(dev);
+	}
+
+	for (l = to_delete; l; l = l->next) {
+		const char *path;
+
+		path = device_get_path(dev->dev);
+		g_dbus_unregister_interface(dev->conn, path, HEALTH_DEVICE);
+	}
+
+	g_slist_free(to_delete);
+}
+
+static void release_adapter_instance(struct hdp_adapter *hdp_adapter)
+{
+	if (!hdp_adapter->mi)
+		return;
+
+	check_devices_mcl();
+	mcap_release_instance(hdp_adapter->mi);
+	mcap_instance_unref(hdp_adapter->mi);
+	hdp_adapter->mi = NULL;
+}
+
+static gboolean update_adapter(struct hdp_adapter *hdp_adapter)
+{
+	GError *err = NULL;
+	bdaddr_t addr;
+
+	if (!applications) {
+		release_adapter_instance(hdp_adapter);
+		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_MEDIUM, 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:
+	release_adapter_instance(hdp_adapter);
+	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);
+	release_adapter_instance(hdp_adapter);
+	btd_adapter_unref(hdp_adapter->btd_adapter);
+	g_free(hdp_adapter);
+}
+
+static void delete_echo_channel_cb(GError *err, gpointer chan)
+{
+	if (err && err->code != MCAP_INVALID_MDL) {
+		/* TODO: Decide if more action is required here */
+		error("Error deleting echo channel: %s", err->message);
+		return;
+	}
+
+	health_channel_destroy(chan);
+}
+
+static void delete_echo_channel(struct hdp_channel *chan)
+{
+	GError *err = NULL;
+
+	if (!chan->dev->mcl_conn) {
+		error("Echo channel cannot be deleted: mcl closed");
+		return;
+	}
+
+	if (mcap_delete_mdl(chan->mdl, delete_echo_channel_cb,
+				hdp_channel_ref(chan),
+				(GDestroyNotify) hdp_channel_unref, &err))
+		return;
+
+	hdp_channel_unref(chan);
+	error("Error deleting the echo channel: %s", err->message);
+	g_error_free(err);
+
+	/* TODO: Decide if more action is required here */
+}
+
+static void abort_echo_channel_cb(GError *err, gpointer data)
+{
+	struct hdp_channel *chan = data;
+
+	if (err && err->code != MCAP_ERROR_INVALID_OPERATION) {
+		error("Aborting error: %s", err->message);
+		if (err->code == MCAP_INVALID_MDL) {
+			/* MDL is removed from MCAP so we can */
+			/* free the data channel without sending */
+			/* a MD_DELETE_MDL_REQ */
+			/* TODO review the above comment */
+			/* hdp_channel_unref(chan); */
+		}
+		return;
+	}
+
+	delete_echo_channel(chan);
+}
+
+static void destroy_create_dc_data(gpointer data)
+{
+	struct hdp_create_dc *dc_data = data;
+
+	hdp_create_data_unref(dc_data);
+}
+
+static void *generate_echo_packet()
+{
+	uint8_t *buf;
+	int i;
+
+	buf = g_malloc(HDP_ECHO_LEN);
+	srand(time(NULL));
+
+	for(i = 0; i < HDP_ECHO_LEN; i++)
+		buf[i] = rand() % UINT8_MAX;
+
+	return buf;
+}
+
+static gboolean check_echo(GIOChannel *io_chan, GIOCondition cond,
+								gpointer data)
+{
+	struct hdp_tmp_dc_data *hdp_conn =  data;
+	struct hdp_echo_data *edata = hdp_conn->hdp_chann->edata;
+	struct hdp_channel *chan = hdp_conn->hdp_chann;
+	uint8_t buf[MCAP_DC_MTU];
+	DBusMessage *reply;
+	gboolean value;
+	int fd, len;
+
+	if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL)) {
+		value = FALSE;
+		goto end;
+	}
+
+	fd = g_io_channel_unix_get_fd(io_chan);
+	len = read(fd, buf, sizeof(buf));
+
+	if (len != HDP_ECHO_LEN) {
+		value = FALSE;
+		goto end;
+	}
+
+	value = (memcmp(buf, edata->buf, len) == 0);
+
+end:
+	reply = g_dbus_create_reply(hdp_conn->msg, DBUS_TYPE_BOOLEAN, &value,
+							DBUS_TYPE_INVALID);
+	g_dbus_send_message(hdp_conn->conn, reply);
+	g_source_remove(edata->tid);
+	edata->tid = 0;
+	g_free(edata->buf);
+	edata->buf = NULL;
+
+	if (!value)
+		close_device_con(chan->dev, FALSE);
+	else
+		delete_echo_channel(chan);
+	hdp_tmp_dc_data_unref(hdp_conn);
+
+	return FALSE;
+}
+
+static gboolean echo_timeout(gpointer data)
+{
+	struct hdp_channel *chan = data;
+	GIOChannel *io;
+	int fd;
+
+	error("Error: Echo request timeout");
+	chan->edata->tid = 0;
+
+	fd = mcap_mdl_get_fd(chan->mdl);
+	if (fd < 0)
+		return FALSE;
+
+	io = g_io_channel_unix_new(fd);
+	g_io_channel_shutdown(io, TRUE, NULL);
+
+	return FALSE;
+}
+
+static void hdp_echo_connect_cb(struct mcap_mdl *mdl, GError *err,
+								gpointer data)
+{
+	struct hdp_tmp_dc_data *hdp_conn =  data;
+	struct hdp_echo_data *edata;
+	GError *gerr = NULL;
+	DBusMessage *reply;
+	GIOChannel *io;
+	int fd;
+
+	if (err) {
+		reply = g_dbus_create_error(hdp_conn->msg,
+						ERROR_INTERFACE ".HealthError",
+						"%s", err->message);
+		g_dbus_send_message(hdp_conn->conn, reply);
+
+		/* Send abort request because remote */
+		/* side is now in PENDING state. */
+		if (!mcap_mdl_abort(hdp_conn->hdp_chann->mdl,
+					abort_echo_channel_cb,
+					hdp_channel_ref(hdp_conn->hdp_chann),
+					(GDestroyNotify) hdp_channel_unref,
+					&gerr)) {
+			error("%s", gerr->message);
+			g_error_free(gerr);
+			hdp_channel_unref(hdp_conn->hdp_chann);
+		}
+		return;
+	}
+
+	fd = mcap_mdl_get_fd(hdp_conn->hdp_chann->mdl);
+	if (fd < 0) {
+		reply = g_dbus_create_error(hdp_conn->msg,
+						ERROR_INTERFACE ".HealthError",
+						"Can't write in echo channel");
+		g_dbus_send_message(hdp_conn->conn, reply);
+		delete_echo_channel(hdp_conn->hdp_chann);
+		return;
+	}
+
+	edata = hdp_conn->hdp_chann->edata;
+	edata->buf = generate_echo_packet();
+	send_echo_data(fd, edata->buf, HDP_ECHO_LEN);
+
+	io = g_io_channel_unix_new(fd);
+	g_io_add_watch(io, G_IO_ERR | G_IO_HUP | G_IO_NVAL | G_IO_IN,
+			check_echo, hdp_tmp_dc_data_ref(hdp_conn));
+
+	edata->tid  = g_timeout_add_seconds_full(G_PRIORITY_DEFAULT,
+					ECHO_TIMEOUT, echo_timeout,
+					hdp_channel_ref(hdp_conn->hdp_chann),
+					(GDestroyNotify) hdp_channel_unref);
+
+	g_io_channel_unref(io);
+}
+
+static void delete_mdl_cb(GError *err, gpointer data)
+{
+	if (err)
+		error("Deleting 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, NULL, NULL, &gerr)) {
+		error("%s", gerr->message);
+		g_error_free(gerr);
+	}
+}
+
+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;
+	struct hdp_device *dev = hdp_chann->dev;
+	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, NULL,
+								NULL, &gerr)) {
+			error("%s", gerr->message);
+			g_error_free(gerr);
+		}
+		return;
+	}
+
+	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);
+
+	if (!check_channel_conf(hdp_chann)) {
+		close_mdl(hdp_chann);
+		return;
+	}
+
+	if (dev->fr)
+		return;
+
+	dev->fr = hdp_channel_ref(hdp_chann);
+
+	emit_property_changed(dev->conn, device_get_path(dev->dev),
+					HEALTH_DEVICE, "MainChannel",
+					DBUS_TYPE_OBJECT_PATH, &dev->fr->path);
+}
+
+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->mdep != HDP_MDEP_ECHO &&
+				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, first 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;
+
+	if (user_data->mdep != HDP_MDEP_ECHO)
+		g_dbus_emit_signal(user_data->conn,
+					device_get_path(hdp_chan->dev->dev),
+					HEALTH_DEVICE,
+					"ChannelConnected",
+					DBUS_TYPE_OBJECT_PATH, &hdp_chan->path,
+					DBUS_TYPE_INVALID);
+
+	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;
+	hdp_conn->cb = user_data->cb;
+	hdp_chan->mdep = user_data->mdep;
+
+	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(mdl, abort_mdl_cb, NULL, 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, mcap_mdl_ref(mdl), NULL,
+								&gerr)) {
+		error("%s", gerr->message);
+		g_error_free(gerr);
+		mcap_mdl_unref(mdl);
+	}
+}
+
+static void device_create_dc_cb(gpointer user_data, GError *err)
+{
+	struct hdp_create_dc *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;
+	}
+
+	if (!data->dev->mcl) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+				"Mcl was closed");
+		goto fail;
+	}
+
+	hdp_create_data_ref(data);
+
+	if (mcap_create_mdl(data->dev->mcl, data->mdep, data->config,
+						device_create_mdl_cb, data,
+						destroy_create_dc_data, &gerr))
+		return;
+	hdp_create_data_unref(data);
+
+fail:
+	reply = g_dbus_create_error(data->msg, ERROR_INTERFACE ".HealthError",
+							"%s", gerr->message);
+	g_error_free(gerr);
+	g_dbus_send_message(data->conn, reply);
+}
+
+static DBusMessage *device_echo(DBusConnection *conn,
+					DBusMessage *msg, void *user_data)
+{
+	struct hdp_device *device = user_data;
+	struct hdp_create_dc *data;
+	DBusMessage *reply;
+	GError *err = NULL;
+
+	data = g_new0(struct hdp_create_dc, 1);
+	data->dev = health_device_ref(device);
+	data->mdep = HDP_MDEP_ECHO;
+	data->config = HDP_RELIABLE_DC;
+	data->msg = dbus_message_ref(msg);
+	data->conn = dbus_connection_ref(conn);
+	data->cb = hdp_echo_connect_cb;
+	hdp_create_data_ref(data);
+
+	if (device->mcl_conn && device->mcl) {
+		if (mcap_create_mdl(device->mcl, data->mdep, data->config,
+						device_create_mdl_cb, data,
+						destroy_create_dc_data, &err))
+			return NULL;
+		goto fail;
+	}
+
+	if (hdp_establish_mcl(data->dev, device_create_dc_cb,
+					data, destroy_create_dc_data, &err))
+		return NULL;
+
+fail:
+	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 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, 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 btd_error_invalid_args(msg);
+
+	l = g_slist_find_custom(applications, app_path, cmp_app);
+	if (!l)
+		return btd_error_invalid_args(msg);
+
+	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 btd_error_invalid_args(msg);
+
+	if (app->role == HDP_SINK && config != HDP_NO_PREFERENCE_DC)
+		return btd_error_invalid_args(msg);
+
+	if (app->role == HDP_SOURCE && config == HDP_NO_PREFERENCE_DC)
+		return btd_error_invalid_args(msg);
+
+	if (!device->fr && config == HDP_STREAMING_DC)
+		return btd_error_invalid_args(msg);
+
+	data = g_new0(struct hdp_create_dc, 1);
+	data->dev = health_device_ref(device);
+	data->config = config;
+	data->app = hdp_application_ref(app);
+	data->msg = dbus_message_ref(msg);
+	data->conn = dbus_connection_ref(conn);
+	data->cb = hdp_mdl_conn_cb;
+
+	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 && 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 btd_error_invalid_args(msg);
+	}
+
+	l = g_slist_find_custom(device->channels, path, cmp_chan_path);
+	if (!l)
+		return btd_error_invalid_args(msg);
+
+	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_channel_ref(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_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));
+
+	remove_channels(device);
+	if (device->ndc) {
+		hdp_channel_unref(device->ndc);
+		device->ndc = NULL;
+	}
+
+	devices = g_slist_remove(devices, device);
+	health_device_unref(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);
+	health_device_ref(dev);
+
+	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:
+	health_device_unref(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");
+}
+
+struct hdp_device *health_device_ref(struct hdp_device *hdp_dev)
+{
+	hdp_dev->ref++;
+
+	DBG("health_device_ref(%p): ref=%d", hdp_dev, hdp_dev->ref);
+
+	return hdp_dev;
+}
+
+void health_device_unref(struct hdp_device *hdp_dev)
+{
+	hdp_dev->ref--;
+
+	DBG("health_device_unref(%p): ref=%d", hdp_dev, hdp_dev->ref);
+
+	if (hdp_dev->ref > 0)
+		return;
+
+	free_health_device(hdp_dev);
+}
diff --git a/health/hdp.h b/health/hdp.h
new file mode 100644
index 0000000..3cf87ca
--- /dev/null
+++ b/health/hdp.h
@@ -0,0 +1,35 @@
+/*
+ *
+ *  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
+ *
+ */
+
+int hdp_adapter_register(DBusConnection *conn, struct btd_adapter *btd_adapter);
+void hdp_adapter_unregister(struct btd_adapter *btd_adapter);
+
+int hdp_device_register(DBusConnection *conn, struct btd_device *device);
+void hdp_device_unregister(struct btd_device *device);
+
+int hdp_manager_start(DBusConnection *conn);
+void hdp_manager_stop();
+
+gboolean hdp_set_mcl_cb(struct hdp_device *device, GError **err);
diff --git a/health/hdp_main.c b/health/hdp_main.c
new file mode 100644
index 0000000..b40a37c
--- /dev/null
+++ b/health/hdp_main.c
@@ -0,0 +1,62 @@
+/*
+ *
+ *  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
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <errno.h>
+
+#include <gdbus.h>
+
+#include "plugin.h"
+#include "hdp_manager.h"
+
+static DBusConnection *connection = NULL;
+
+static int hdp_init(void)
+{
+	connection = dbus_bus_get(DBUS_BUS_SYSTEM, NULL);
+	if (connection == NULL)
+		return -EIO;
+
+	if (hdp_manager_init(connection) < 0) {
+		dbus_connection_unref(connection);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void hdp_exit(void)
+{
+	hdp_manager_exit();
+
+	dbus_connection_unref(connection);
+	connection = NULL;
+}
+
+BLUETOOTH_PLUGIN_DEFINE(health, VERSION,
+			BLUETOOTH_PLUGIN_PRIORITY_DEFAULT, hdp_init, hdp_exit)
diff --git a/health/hdp_manager.c b/health/hdp_manager.c
new file mode 100644
index 0000000..88b49fc
--- /dev/null
+++ b/health/hdp_manager.c
@@ -0,0 +1,100 @@
+/*
+ *
+ *  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
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <bluetooth/sdp.h>
+#include <bluetooth/sdp_lib.h>
+
+#include <btio.h>
+#include <adapter.h>
+#include <device.h>
+
+#include "hdp_types.h"
+
+#include "log.h"
+#include "hdp_manager.h"
+#include "hdp.h"
+
+#include "glib-helper.h"
+
+static DBusConnection *connection = NULL;
+
+static int hdp_adapter_probe(struct btd_adapter *adapter)
+{
+	return hdp_adapter_register(connection, adapter);
+}
+
+static void hdp_adapter_remove(struct btd_adapter *adapter)
+{
+	hdp_adapter_unregister(adapter);
+}
+
+static struct btd_adapter_driver hdp_adapter_driver = {
+	.name	= "hdp-adapter-driver",
+	.probe	= hdp_adapter_probe,
+	.remove	= hdp_adapter_remove,
+};
+
+static int hdp_driver_probe(struct btd_device *device, GSList *uuids)
+{
+	return hdp_device_register(connection, device);
+}
+
+static void hdp_driver_remove(struct btd_device *device)
+{
+	hdp_device_unregister(device);
+}
+
+static struct btd_device_driver hdp_device_driver = {
+	.name	= "hdp-device-driver",
+	.uuids	= BTD_UUIDS(HDP_UUID, HDP_SOURCE_UUID, HDP_SINK_UUID),
+	.probe	= hdp_driver_probe,
+	.remove	= hdp_driver_remove
+};
+
+int hdp_manager_init(DBusConnection *conn)
+{
+	if (hdp_manager_start(conn))
+		return -1;
+
+	connection = dbus_connection_ref(conn);
+	btd_register_adapter_driver(&hdp_adapter_driver);
+	btd_register_device_driver(&hdp_device_driver);
+
+	return 0;
+}
+
+void hdp_manager_exit(void)
+{
+	btd_unregister_device_driver(&hdp_device_driver);
+	btd_unregister_adapter_driver(&hdp_adapter_driver);
+	hdp_manager_stop();
+
+	dbus_connection_unref(connection);
+	connection = NULL;
+}
diff --git a/health/hdp_manager.h b/health/hdp_manager.h
new file mode 100644
index 0000000..b91ef75
--- /dev/null
+++ b/health/hdp_manager.h
@@ -0,0 +1,27 @@
+/*
+ *
+ *  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
+ *
+ */
+
+int hdp_manager_init(DBusConnection *conn);
+void hdp_manager_exit(void);
diff --git a/health/hdp_types.h b/health/hdp_types.h
new file mode 100644
index 0000000..7d23293
--- /dev/null
+++ b/health/hdp_types.h
@@ -0,0 +1,129 @@
+/*
+ *
+ *  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
+ *
+ */
+
+#ifndef __HDP_TYPES_H__
+#define __HDP_TYPES_H__
+
+#define HDP_UUID		"00001400-0000-1000-8000-00805F9B34FB"
+#define HDP_SOURCE_UUID		"00001401-0000-1000-8000-00805F9B34FB"
+#define HDP_SINK_UUID		"00001402-0000-1000-8000-00805F9B34FB"
+
+#define MANAGER_PATH		"/org/bluez"
+
+#define HEALTH_MANAGER		"org.bluez.HealthManager"
+#define HEALTH_DEVICE		"org.bluez.HealthDevice"
+#define HEALTH_CHANNEL		"org.bluez.HealthChannel"
+
+#define HDP_VERSION		0x0100
+
+#define HDP_SERVICE_NAME	"Bluez HDP"
+#define HDP_SERVICE_DSC		"A Bluez health device profile implementation"
+#define HDP_SERVICE_PROVIDER	"Bluez"
+
+#define HDP_MDEP_ECHO		0x00
+#define HDP_MDEP_INITIAL	0x01
+#define HDP_MDEP_FINAL		0x7F
+
+#define HDP_ERROR		g_quark_from_static_string("hdp-error-quark")
+
+#define HDP_NO_PREFERENCE_DC	0x00
+#define HDP_RELIABLE_DC		0x01
+#define HDP_STREAMING_DC	0x02
+
+#define HDP_SINK_ROLE_AS_STRING		"sink"
+#define HDP_SOURCE_ROLE_AS_STRING	"source"
+
+typedef enum {
+	HDP_SOURCE = 0x00,
+	HDP_SINK = 0x01
+} HdpRole;
+
+typedef enum {
+	HDP_DIC_PARSE_ERROR,
+	HDP_DIC_ENTRY_PARSE_ERROR,
+	HDP_CONNECTION_ERROR,
+	HDP_UNSPECIFIED_ERROR,
+	HDP_UNKNOWN_ERROR
+} HdpError;
+
+enum data_specs {
+	DATA_EXCHANGE_SPEC_11073 = 0x01
+};
+
+struct hdp_application {
+	DBusConnection		*conn;		/* For dbus watcher */
+	char			*path;		/* The path of the application */
+	uint16_t		data_type;	/* Data type handled for this application */
+	gboolean		data_type_set;	/* Flag for dictionary parsing */
+	uint8_t			role;		/* Role of this application */
+	gboolean		role_set;	/* Flag for dictionary parsing */
+	uint8_t			chan_type;	/* QoS preferred by source applications */
+	gboolean		chan_type_set;	/* Flag for dictionary parsing */
+	char			*description;	/* Options description for SDP record */
+	uint8_t			id;		/* The identification is also the mdepid */
+	char			*oname;		/* Name of the owner application */
+	int			dbus_watcher;	/* Watch for clients disconnection */
+	gint			ref;		/* Reference counter */
+};
+
+struct hdp_adapter {
+	struct btd_adapter	*btd_adapter;	/* Bluetooth adapter */
+	struct mcap_instance	*mi;		/* Mcap instance in */
+	uint16_t		ccpsm;		/* Control channel psm */
+	uint16_t		dcpsm;		/* Data channel psm */
+	uint32_t		sdp_handler;	/* SDP record handler */
+	uint32_t		record_state;	/* Service record state */
+};
+
+struct hdp_device {
+	DBusConnection		*conn;		/* For name listener handling */
+	struct btd_device	*dev;		/* Device reference */
+	struct hdp_adapter	*hdp_adapter;	/* hdp_adapater */
+	struct mcap_mcl		*mcl;		/* The mcap control channel */
+	gboolean		mcl_conn;	/* Mcl status */
+	gboolean		sdp_present;	/* Has an sdp record */
+	GSList			*channels;	/* Data Channel list */
+	struct hdp_channel	*ndc;		/* Data channel being negotiated */
+	struct hdp_channel	*fr;		/* First reliable data channel */
+	gint			ref;		/* Reference counting */
+};
+
+struct hdp_echo_data;
+
+struct hdp_channel {
+	struct hdp_device	*dev;		/* Device where this channel belongs */
+	struct hdp_application	*app;		/* Application */
+	struct mcap_mdl		*mdl;		/* The data channel reference */
+	char			*path;		/* The path of the channel */
+	uint8_t			config;		/* Channel configuration */
+	uint8_t			mdep;		/* Remote MDEP */
+	uint16_t		mdlid;		/* Data channel Id */
+	uint16_t		imtu;		/* Channel incoming MTU */
+	uint16_t		omtu;		/* Channel outgoing MTU */
+	struct hdp_echo_data	*edata;		/* private data used by echo channels */
+	gint			ref;		/* Reference counter */
+};
+
+#endif /* __HDP_TYPES_H__ */
diff --git a/health/hdp_util.c b/health/hdp_util.c
new file mode 100644
index 0000000..aefe5f9
--- /dev/null
+++ b/health/hdp_util.c
@@ -0,0 +1,1211 @@
+/*
+ *
+ *  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 <adapter.h>
+#include <device.h>
+#include <stdint.h>
+#include <hdp_types.h>
+#include <hdp_util.h>
+#include <mcap.h>
+#include <hdp.h>
+
+#include <sdpd.h>
+#include <sdp_lib.h>
+#include <glib-helper.h>
+
+#include <btio.h>
+#include <mcap_lib.h>
+
+#include <log.h>
+
+typedef gboolean (*parse_item_f)(DBusMessageIter *iter, gpointer user_data,
+								GError **err);
+
+struct dict_entry_func {
+	char		*key;
+	parse_item_f	func;
+};
+
+struct get_mdep_data {
+	struct hdp_application	*app;
+	gpointer		data;
+	hdp_continue_mdep_f	func;
+	GDestroyNotify		destroy;
+};
+
+struct conn_mcl_data {
+	int			refs;
+	gpointer		data;
+	hdp_continue_proc_f	func;
+	GDestroyNotify		destroy;
+	struct hdp_device	*dev;
+};
+
+struct get_dcpsm_data {
+	gpointer		data;
+	hdp_continue_dcpsm_f	func;
+	GDestroyNotify		destroy;
+};
+
+static gboolean parse_dict_entry(struct dict_entry_func dict_context[],
+							DBusMessageIter *iter,
+							GError **err,
+							gpointer user_data)
+{
+	DBusMessageIter entry;
+	char *key;
+	int ctype, i;
+	struct dict_entry_func df;
+
+	dbus_message_iter_recurse(iter, &entry);
+	ctype = dbus_message_iter_get_arg_type(&entry);
+	if (ctype != DBUS_TYPE_STRING) {
+		g_set_error(err, HDP_ERROR, HDP_DIC_ENTRY_PARSE_ERROR,
+			"Dictionary entries should have a string as key");
+		return FALSE;
+	}
+
+	dbus_message_iter_get_basic(&entry, &key);
+	dbus_message_iter_next(&entry);
+	/* Find function and call it */
+	for (i = 0, df = dict_context[0]; df.key; i++, df = dict_context[i]) {
+		if (g_ascii_strcasecmp(df.key, key) == 0)
+			return df.func(&entry, user_data, err);
+	}
+
+	g_set_error(err, HDP_ERROR, HDP_DIC_ENTRY_PARSE_ERROR,
+			"No function found for parsing value for key %s", key);
+	return FALSE;
+}
+
+static gboolean parse_dict(struct dict_entry_func dict_context[],
+							DBusMessageIter *iter,
+							GError **err,
+							gpointer user_data)
+{
+	int ctype;
+	DBusMessageIter dict;
+
+	ctype = dbus_message_iter_get_arg_type(iter);
+	if (ctype != DBUS_TYPE_ARRAY) {
+		g_set_error(err, HDP_ERROR, HDP_DIC_PARSE_ERROR,
+					"Dictionary should be an array");
+		return FALSE;
+	}
+
+	dbus_message_iter_recurse(iter, &dict);
+	while ((ctype = dbus_message_iter_get_arg_type(&dict)) !=
+							DBUS_TYPE_INVALID) {
+		if (ctype != DBUS_TYPE_DICT_ENTRY) {
+			g_set_error(err, HDP_ERROR, HDP_DIC_PARSE_ERROR,
+						"Dictionary array should "
+						"contain dict entries");
+			return FALSE;
+		}
+
+		/* Start parsing entry */
+		if (!parse_dict_entry(dict_context, &dict, err,
+							user_data))
+			return FALSE;
+		/* Finish entry parsing */
+
+		dbus_message_iter_next(&dict);
+	}
+
+	return TRUE;
+}
+
+static gboolean parse_data_type(DBusMessageIter *iter, gpointer data,
+								GError **err)
+{
+	struct hdp_application *app = data;
+	DBusMessageIter *value;
+	int ctype;
+
+	ctype = dbus_message_iter_get_arg_type(iter);
+	value = iter;
+	if (ctype == DBUS_TYPE_VARIANT) {
+		DBusMessageIter variant;
+
+		/* Get value inside the variable */
+		dbus_message_iter_recurse(iter, &variant);
+		ctype = dbus_message_iter_get_arg_type(&variant);
+		value = &variant;
+	}
+
+	if (ctype != DBUS_TYPE_UINT16) {
+		g_set_error(err, HDP_ERROR, HDP_DIC_ENTRY_PARSE_ERROR,
+			"Final value for data type should be uint16");
+		return FALSE;
+	}
+
+	dbus_message_iter_get_basic(value, &app->data_type);
+	app->data_type_set = TRUE;
+	return TRUE;
+}
+
+static gboolean parse_role(DBusMessageIter *iter, gpointer data, GError **err)
+{
+	struct hdp_application *app = data;
+	DBusMessageIter *string;
+	int ctype;
+	const char *role;
+
+	ctype = dbus_message_iter_get_arg_type(iter);
+	if (ctype == DBUS_TYPE_VARIANT) {
+		DBusMessageIter value;
+
+		/* Get value inside the variable */
+		dbus_message_iter_recurse(iter, &value);
+		ctype = dbus_message_iter_get_arg_type(&value);
+		string = &value;
+	} else {
+		string = iter;
+	}
+
+	if (ctype != DBUS_TYPE_STRING) {
+		g_set_error(err, HDP_ERROR, HDP_UNSPECIFIED_ERROR,
+				"Value data spec should be variable or string");
+		return FALSE;
+	}
+
+	dbus_message_iter_get_basic(string, &role);
+	if (g_ascii_strcasecmp(role, HDP_SINK_ROLE_AS_STRING) == 0) {
+		app->role = HDP_SINK;
+	} else if (g_ascii_strcasecmp(role, HDP_SOURCE_ROLE_AS_STRING) == 0) {
+		app->role = HDP_SOURCE;
+	} else {
+		g_set_error(err, HDP_ERROR, HDP_UNSPECIFIED_ERROR,
+			"Role value should be \"source\" or \"sink\"");
+		return FALSE;
+	}
+
+	app->role_set = TRUE;
+
+	return TRUE;
+}
+
+static gboolean parse_desc(DBusMessageIter *iter, gpointer data, GError **err)
+{
+	struct hdp_application *app = data;
+	DBusMessageIter *string;
+	int ctype;
+	const char *desc;
+
+	ctype = dbus_message_iter_get_arg_type(iter);
+	if (ctype == DBUS_TYPE_VARIANT) {
+		DBusMessageIter variant;
+
+		/* Get value inside the variable */
+		dbus_message_iter_recurse(iter, &variant);
+		ctype = dbus_message_iter_get_arg_type(&variant);
+		string = &variant;
+	} else {
+		string = iter;
+	}
+
+	if (ctype != DBUS_TYPE_STRING) {
+		g_set_error(err, HDP_ERROR, HDP_DIC_ENTRY_PARSE_ERROR,
+				"Value data spec should be variable or string");
+		return FALSE;
+	}
+
+	dbus_message_iter_get_basic(string, &desc);
+	app->description = g_strdup(desc);
+	return TRUE;
+}
+
+static gboolean parse_chan_type(DBusMessageIter *iter, gpointer data,
+								GError **err)
+{
+	struct hdp_application *app = data;
+	DBusMessageIter *value;
+	char *chan_type;
+	int ctype;
+
+	ctype = dbus_message_iter_get_arg_type(iter);
+	value = iter;
+	if (ctype == DBUS_TYPE_VARIANT) {
+		DBusMessageIter variant;
+
+		/* Get value inside the variable */
+		dbus_message_iter_recurse(iter, &variant);
+		ctype = dbus_message_iter_get_arg_type(&variant);
+		value = &variant;
+	}
+
+	if (ctype != DBUS_TYPE_STRING) {
+		g_set_error(err, HDP_ERROR, HDP_DIC_ENTRY_PARSE_ERROR,
+			"Final value for channel type should be an string");
+		return FALSE;
+	}
+
+	dbus_message_iter_get_basic(value, &chan_type);
+
+	if (g_ascii_strcasecmp("Reliable", chan_type) == 0)
+		app->chan_type = HDP_RELIABLE_DC;
+	else if (g_ascii_strcasecmp("Streaming", chan_type) == 0)
+		app->chan_type = HDP_STREAMING_DC;
+	else {
+		g_set_error(err, HDP_ERROR, HDP_DIC_ENTRY_PARSE_ERROR,
+						"Invalid value for data type");
+		return FALSE;
+	}
+
+	app->chan_type_set = TRUE;
+
+	return TRUE;
+}
+
+static struct dict_entry_func dict_parser[] = {
+	{"DataType",		parse_data_type},
+	{"Role",		parse_role},
+	{"Description",		parse_desc},
+	{"ChannelType",		parse_chan_type},
+	{NULL, NULL}
+};
+
+struct hdp_application *hdp_get_app_config(DBusMessageIter *iter, GError **err)
+{
+	struct hdp_application *app;
+
+	app = g_new0(struct hdp_application, 1);
+	app->ref = 1;
+	if (!parse_dict(dict_parser, iter, err, app))
+		goto fail;
+	if (!app->data_type_set || !app->role_set) {
+		g_set_error(err, HDP_ERROR, HDP_DIC_PARSE_ERROR,
+						"Mandatory fields aren't set");
+		goto fail;
+	}
+	return app;
+
+fail:
+	hdp_application_unref(app);
+	return NULL;
+}
+
+static gboolean is_app_role(GSList *app_list, HdpRole role)
+{
+	GSList *l;
+
+	for (l = app_list; l; l = l->next) {
+		struct hdp_application *app = l->data;
+
+		if (app->role == role)
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static gboolean set_sdp_services_uuid(sdp_record_t *record, HdpRole role)
+{
+	uuid_t svc_uuid_source, svc_uuid_sink;
+	sdp_list_t *svc_list = NULL;
+
+	sdp_uuid16_create(&svc_uuid_sink, HDP_SINK_SVCLASS_ID);
+	sdp_uuid16_create(&svc_uuid_source, HDP_SOURCE_SVCLASS_ID);
+
+	sdp_get_service_classes(record, &svc_list);
+
+	if (role == HDP_SOURCE) {
+		if (!sdp_list_find(svc_list, &svc_uuid_source, sdp_uuid_cmp))
+			svc_list = sdp_list_append(svc_list, &svc_uuid_source);
+	} else if (role == HDP_SINK) {
+		if (!sdp_list_find(svc_list, &svc_uuid_sink, sdp_uuid_cmp))
+			svc_list = sdp_list_append(svc_list, &svc_uuid_sink);
+	}
+
+	if (sdp_set_service_classes(record, svc_list) < 0) {
+		sdp_list_free(svc_list, NULL);
+		return FALSE;
+	}
+
+	sdp_list_free(svc_list, NULL);
+
+	return TRUE;
+}
+
+static gboolean register_service_protocols(struct hdp_adapter *adapter,
+						sdp_record_t *sdp_record)
+{
+	gboolean ret;
+	uuid_t l2cap_uuid, mcap_c_uuid;
+	sdp_list_t *l2cap_list, *proto_list = NULL, *mcap_list = NULL;
+	sdp_list_t *access_proto_list = NULL;
+	sdp_data_t *psm = NULL, *mcap_ver = NULL;
+	uint16_t version = MCAP_VERSION;
+
+	/* set l2cap information */
+	sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID);
+	l2cap_list = sdp_list_append(NULL, &l2cap_uuid);
+	if (!l2cap_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	psm = sdp_data_alloc(SDP_UINT16, &adapter->ccpsm);
+	if (!psm) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (!sdp_list_append(l2cap_list, psm)) {
+		ret = FALSE;
+		goto end;
+	}
+
+	proto_list = sdp_list_append(NULL, l2cap_list);
+	if (!proto_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	/* set mcap information */
+	sdp_uuid16_create(&mcap_c_uuid, MCAP_CTRL_UUID);
+	mcap_list = sdp_list_append(NULL, &mcap_c_uuid);
+	if (!mcap_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	mcap_ver = sdp_data_alloc(SDP_UINT16, &version);
+	if (!mcap_ver) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (!sdp_list_append(mcap_list, mcap_ver)) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (!sdp_list_append(proto_list, mcap_list)) {
+		ret = FALSE;
+		goto end;
+	}
+
+	/* attach protocol information to service record */
+	access_proto_list = sdp_list_append(NULL, proto_list);
+	if (!access_proto_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (sdp_set_access_protos(sdp_record, access_proto_list) < 0) {
+		ret = FALSE;
+		goto end;
+	}
+	ret = TRUE;
+
+end:
+	if (l2cap_list)
+		sdp_list_free(l2cap_list, NULL);
+	if (mcap_list)
+		sdp_list_free(mcap_list, NULL);
+	if (proto_list)
+		sdp_list_free(proto_list, NULL);
+	if (access_proto_list)
+		sdp_list_free(access_proto_list, NULL);
+	if (psm)
+		sdp_data_free(psm);
+	if (mcap_ver)
+		sdp_data_free(mcap_ver);
+
+	return ret;
+}
+
+static gboolean register_service_profiles(sdp_record_t *sdp_record)
+{
+	gboolean ret;
+	sdp_list_t *profile_list;
+	sdp_profile_desc_t hdp_profile;
+
+	/* set hdp information */
+	sdp_uuid16_create(&hdp_profile.uuid, HDP_SVCLASS_ID);
+	hdp_profile.version = HDP_VERSION;
+	profile_list = sdp_list_append(NULL, &hdp_profile);
+	if (!profile_list)
+		return FALSE;
+
+	/* set profile descriptor list */
+	if (sdp_set_profile_descs(sdp_record, profile_list) < 0)
+		ret = FALSE;
+	else
+		ret = TRUE;
+
+	sdp_list_free(profile_list, NULL);
+
+	return ret;
+}
+
+static gboolean register_service_additional_protocols(
+						struct hdp_adapter *adapter,
+						sdp_record_t *sdp_record)
+{
+	gboolean ret;
+	uuid_t l2cap_uuid, mcap_d_uuid;
+	sdp_list_t *l2cap_list, *proto_list = NULL, *mcap_list = NULL;
+	sdp_list_t *access_proto_list = NULL;
+	sdp_data_t *psm = NULL;
+
+	/* set l2cap information */
+	sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID);
+	l2cap_list = sdp_list_append(NULL, &l2cap_uuid);
+	if (!l2cap_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	psm = sdp_data_alloc(SDP_UINT16, &adapter->dcpsm);
+	if (!psm) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (!sdp_list_append(l2cap_list, psm)) {
+		ret = FALSE;
+		goto end;
+	}
+
+	proto_list = sdp_list_append(NULL, l2cap_list);
+	if (!proto_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	/* set mcap information */
+	sdp_uuid16_create(&mcap_d_uuid, MCAP_DATA_UUID);
+	mcap_list = sdp_list_append(NULL, &mcap_d_uuid);
+	if (!mcap_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (!sdp_list_append(proto_list, mcap_list)) {
+		ret = FALSE;
+		goto end;
+	}
+
+	/* attach protocol information to service record */
+	access_proto_list = sdp_list_append(NULL, proto_list);
+	if (!access_proto_list) {
+		ret = FALSE;
+		goto end;
+	}
+
+	if (sdp_set_add_access_protos(sdp_record, access_proto_list) < 0)
+		ret = FALSE;
+	else
+		ret = TRUE;
+
+end:
+	if (l2cap_list)
+		sdp_list_free(l2cap_list, NULL);
+	if (mcap_list)
+		sdp_list_free(mcap_list, NULL);
+	if (proto_list)
+		sdp_list_free(proto_list, NULL);
+	if (access_proto_list)
+		sdp_list_free(access_proto_list, NULL);
+	if (psm)
+		sdp_data_free(psm);
+
+	return ret;
+}
+
+static sdp_list_t *app_to_sdplist(struct hdp_application *app)
+{
+	sdp_data_t *mdepid,
+		*dtype = NULL,
+		*role = NULL,
+		*desc = NULL;
+	sdp_list_t *f_list = NULL;
+
+	mdepid = sdp_data_alloc(SDP_UINT8, &app->id);
+	if (!mdepid)
+		return NULL;
+
+	dtype = sdp_data_alloc(SDP_UINT16, &app->data_type);
+	if (!dtype)
+		goto fail;
+
+	role = sdp_data_alloc(SDP_UINT8, &app->role);
+	if (!role)
+		goto fail;
+
+	if (app->description) {
+		desc = sdp_data_alloc(SDP_TEXT_STR8, app->description);
+		if (!desc)
+			goto fail;
+	}
+
+	f_list = sdp_list_append(NULL, mdepid);
+	if (!f_list)
+		goto fail;
+
+	if (!sdp_list_append(f_list, dtype))
+		goto fail;
+
+	if (!sdp_list_append(f_list, role))
+		goto fail;
+
+	if (desc)
+		if (!sdp_list_append(f_list, desc))
+			goto fail;
+
+	return f_list;
+
+fail:
+	if (f_list)
+		sdp_list_free(f_list, NULL);
+	if (mdepid)
+		sdp_data_free(mdepid);
+	if (dtype)
+		sdp_data_free(dtype);
+	if (role)
+		sdp_data_free(role);
+	if (desc)
+		sdp_data_free(desc);
+
+	return NULL;
+}
+
+static gboolean register_features(struct hdp_application *app,
+						sdp_list_t **sup_features)
+{
+	sdp_list_t *hdp_feature;
+
+	hdp_feature = app_to_sdplist(app);
+	if (!hdp_feature)
+		goto fail;
+
+	if (!*sup_features) {
+		*sup_features = sdp_list_append(NULL, hdp_feature);
+		if (!*sup_features)
+			goto fail;
+	} else if (!sdp_list_append(*sup_features, hdp_feature)) {
+		goto fail;
+	}
+
+	return TRUE;
+
+fail:
+	if (hdp_feature)
+		sdp_list_free(hdp_feature, (sdp_free_func_t)sdp_data_free);
+	return FALSE;
+}
+
+static void free_hdp_list(void *list)
+{
+	sdp_list_t *hdp_list = list;
+
+	sdp_list_free(hdp_list, (sdp_free_func_t)sdp_data_free);
+}
+
+static gboolean register_service_sup_features(GSList *app_list,
+						sdp_record_t *sdp_record)
+{
+	GSList *l;
+	sdp_list_t *sup_features = NULL;
+
+	for (l = app_list; l; l = l->next) {
+		if (!register_features(l->data, &sup_features))
+			return FALSE;
+	}
+
+	if (sdp_set_supp_feat(sdp_record, sup_features) < 0) {
+		sdp_list_free(sup_features, free_hdp_list);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean register_data_exchange_spec(sdp_record_t *record)
+{
+	sdp_data_t *spec;
+	uint8_t data_spec = DATA_EXCHANGE_SPEC_11073;
+	/* As by now 11073 is the only supported we set it by default */
+
+	spec = sdp_data_alloc(SDP_UINT8, &data_spec);
+	if (!spec)
+		return FALSE;
+
+	if (sdp_attr_add(record, SDP_ATTR_DATA_EXCHANGE_SPEC, spec) < 0) {
+		sdp_data_free(spec);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean register_mcap_features(sdp_record_t *sdp_record)
+{
+	sdp_data_t *mcap_proc;
+	uint8_t mcap_sup_proc = MCAP_SUP_PROC;
+
+	mcap_proc = sdp_data_alloc(SDP_UINT8, &mcap_sup_proc);
+	if (!mcap_proc)
+		return FALSE;
+
+	if (sdp_attr_add(sdp_record, SDP_ATTR_MCAP_SUPPORTED_PROCEDURES,
+							mcap_proc) < 0) {
+		sdp_data_free(mcap_proc);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+gboolean hdp_update_sdp_record(struct hdp_adapter *adapter, GSList *app_list)
+{
+	sdp_record_t *sdp_record;
+	bdaddr_t addr;
+
+	if (adapter->sdp_handler)
+		remove_record_from_server(adapter->sdp_handler);
+
+	if (!app_list) {
+		adapter->sdp_handler = 0;
+		return TRUE;
+	}
+
+	sdp_record = sdp_record_alloc();
+	if (!sdp_record)
+		return FALSE;
+
+	if (adapter->sdp_handler)
+		sdp_record->handle = adapter->sdp_handler;
+	else
+		sdp_record->handle = 0xffffffff; /* Set automatically */
+
+	if (is_app_role(app_list, HDP_SINK))
+		set_sdp_services_uuid(sdp_record, HDP_SINK);
+	if (is_app_role(app_list, HDP_SOURCE))
+		set_sdp_services_uuid(sdp_record, HDP_SOURCE);
+
+	if (!register_service_protocols(adapter, sdp_record))
+		goto fail;
+	if (!register_service_profiles(sdp_record))
+		goto fail;
+	if (!register_service_additional_protocols(adapter, sdp_record))
+		goto fail;
+
+	sdp_set_info_attr(sdp_record, HDP_SERVICE_NAME, HDP_SERVICE_PROVIDER,
+							HDP_SERVICE_DSC);
+	if (!register_service_sup_features(app_list, sdp_record))
+		goto fail;
+	if (!register_data_exchange_spec(sdp_record))
+		goto fail;
+
+	register_mcap_features(sdp_record);
+
+	if (sdp_set_record_state(sdp_record, adapter->record_state++))
+		goto fail;
+
+	adapter_get_address(adapter->btd_adapter, &addr);
+
+	if (add_record_to_server(&addr, sdp_record) < 0)
+		goto fail;
+	adapter->sdp_handler = sdp_record->handle;
+	return TRUE;
+
+fail:
+	if (sdp_record)
+		sdp_record_free(sdp_record);
+	return FALSE;
+}
+
+static gboolean check_role(uint8_t rec_role, uint8_t app_role)
+{
+	if ((rec_role == HDP_SINK && app_role == HDP_SOURCE) ||
+			(rec_role == HDP_SOURCE && app_role == HDP_SINK))
+		return TRUE;
+
+	return FALSE;
+}
+
+static gboolean get_mdep_from_rec(const sdp_record_t *rec, uint8_t role,
+				uint16_t d_type, uint8_t *mdep, char **desc)
+{
+	sdp_data_t *list, *feat;
+
+	if (!desc && !mdep)
+		return TRUE;
+
+	list = sdp_data_get(rec, SDP_ATTR_SUPPORTED_FEATURES_LIST);
+
+	if (list->dtd != SDP_SEQ8 && list->dtd != SDP_SEQ16 &&
+							list->dtd != SDP_SEQ32)
+		return FALSE;
+
+	for (feat = list->val.dataseq; feat; feat = feat->next) {
+		sdp_data_t *data_type, *mdepid, *role_t, *desc_t;
+
+		if (feat->dtd != SDP_SEQ8 && feat->dtd != SDP_SEQ16 &&
+						feat->dtd != SDP_SEQ32)
+			continue;
+
+		mdepid = feat->val.dataseq;
+		if (!mdepid)
+			continue;
+
+		data_type = mdepid->next;
+		if (!data_type)
+			continue;
+
+		role_t = data_type->next;
+		if (!role_t)
+			continue;
+
+		desc_t = role_t->next;
+
+		if (data_type->dtd != SDP_UINT16 || mdepid->dtd != SDP_UINT8 ||
+						role_t->dtd != SDP_UINT8)
+			continue;
+
+		if (data_type->val.uint16 != d_type ||
+					!check_role(role_t->val.uint8, role))
+			continue;
+
+		if (mdep)
+			*mdep = mdepid->val.uint8;
+
+		if (desc  && desc_t && (desc_t->dtd == SDP_TEXT_STR8 ||
+					desc_t->dtd == SDP_TEXT_STR16  ||
+					desc_t->dtd == SDP_TEXT_STR32))
+			*desc = g_strdup(desc_t->val.str);
+
+		return TRUE;
+	}
+
+	return FALSE;
+}
+
+static void get_mdep_cb(sdp_list_t *recs, int err, gpointer user_data)
+{
+	struct get_mdep_data *mdep_data = user_data;
+	GError *gerr = NULL;
+	uint8_t mdep;
+
+	if (err || !recs) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+					"Error getting remote SDP records");
+		mdep_data->func(0, mdep_data->data, gerr);
+		g_error_free(gerr);
+		return;
+	}
+
+	if (!get_mdep_from_rec(recs->data, mdep_data->app->role,
+				mdep_data->app->data_type, &mdep, NULL)) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+					"No matching MDEP found");
+		mdep_data->func(0, mdep_data->data, gerr);
+		g_error_free(gerr);
+		return;
+	}
+
+	mdep_data->func(mdep, mdep_data->data, NULL);
+}
+
+static void free_mdep_data(gpointer data)
+{
+	struct get_mdep_data *mdep_data = data;
+
+	if (mdep_data->destroy)
+		mdep_data->destroy(mdep_data->data);
+	hdp_application_unref(mdep_data->app);
+
+	g_free(mdep_data);
+}
+
+gboolean hdp_get_mdep(struct hdp_device *device, struct hdp_application *app,
+				hdp_continue_mdep_f func, gpointer data,
+				GDestroyNotify destroy, GError **err)
+{
+	struct get_mdep_data *mdep_data;
+	bdaddr_t dst, src;
+	uuid_t uuid;
+
+	device_get_address(device->dev, &dst);
+	adapter_get_address(device_get_adapter(device->dev), &src);
+
+	mdep_data = g_new0(struct get_mdep_data, 1);
+	mdep_data->app = hdp_application_ref(app);
+	mdep_data->func = func;
+	mdep_data->data = data;
+	mdep_data->destroy = destroy;
+
+	bt_string2uuid(&uuid, HDP_UUID);
+	if (bt_search_service(&src, &dst, &uuid, get_mdep_cb, mdep_data,
+							free_mdep_data)) {
+		g_set_error(err, HDP_ERROR, HDP_CONNECTION_ERROR,
+						"Can't get remote SDP record");
+		g_free(mdep_data);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean get_prot_desc_entry(sdp_data_t *entry, int type, guint16 *val)
+{
+	sdp_data_t *iter;
+	int proto;
+
+	if (!entry || (entry->dtd != SDP_SEQ8 && entry->dtd != SDP_SEQ16 &&
+						entry->dtd != SDP_SEQ32))
+		return FALSE;
+
+	iter = entry->val.dataseq;
+	if (!(iter->dtd & SDP_UUID_UNSPEC))
+		return FALSE;
+
+	proto = sdp_uuid_to_proto(&iter->val.uuid);
+	if (proto != type)
+		return FALSE;
+
+	if (!val)
+		return TRUE;
+
+	iter = iter->next;
+	if (iter->dtd != SDP_UINT16)
+		return FALSE;
+
+	*val = iter->val.uint16;
+
+	return TRUE;
+}
+
+static gboolean hdp_get_prot_desc_list(const sdp_record_t *rec, guint16 *psm,
+							guint16 *version)
+{
+	sdp_data_t *pdl, *p0, *p1;
+
+	if (!psm && !version)
+		return TRUE;
+
+	pdl = sdp_data_get(rec, SDP_ATTR_PROTO_DESC_LIST);
+	if (pdl->dtd != SDP_SEQ8 && pdl->dtd != SDP_SEQ16 &&
+							pdl->dtd != SDP_SEQ32)
+		return FALSE;
+
+	p0 = pdl->val.dataseq;
+	if (!get_prot_desc_entry(p0, L2CAP_UUID, psm))
+		return FALSE;
+
+	p1 = p0->next;
+	if (!get_prot_desc_entry(p1, MCAP_CTRL_UUID, version))
+		return FALSE;
+
+	return TRUE;
+}
+
+static gboolean hdp_get_add_prot_desc_list(const sdp_record_t *rec,
+								guint16 *psm)
+{
+	sdp_data_t *pdl, *p0, *p1;
+
+	if (!psm)
+		return TRUE;
+
+	pdl = sdp_data_get(rec, SDP_ATTR_ADD_PROTO_DESC_LIST);
+	if (pdl->dtd != SDP_SEQ8)
+		return FALSE;
+	pdl = pdl->val.dataseq;
+	if (pdl->dtd != SDP_SEQ8)
+		return FALSE;
+
+	p0 = pdl->val.dataseq;
+
+	if (!get_prot_desc_entry(p0, L2CAP_UUID, psm))
+		return FALSE;
+	p1 = p0->next;
+	if (!get_prot_desc_entry(p1, MCAP_DATA_UUID, NULL))
+		return FALSE;
+
+	return TRUE;
+}
+
+static gboolean get_ccpsm(sdp_list_t *recs, uint16_t *ccpsm)
+{
+	sdp_list_t *l;
+
+	for (l = recs; l; l = l->next) {
+		sdp_record_t *rec = l->data;
+
+		if (hdp_get_prot_desc_list(rec, ccpsm, NULL))
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static gboolean get_dcpsm(sdp_list_t *recs, uint16_t *dcpsm)
+{
+	sdp_list_t *l;
+
+	for (l = recs; l; l = l->next) {
+		sdp_record_t *rec = l->data;
+
+		if (hdp_get_add_prot_desc_list(rec, dcpsm))
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+static void con_mcl_data_unref(struct conn_mcl_data *conn_data)
+{
+	if (!conn_data)
+		return;
+
+	if (--conn_data->refs > 0)
+		return;
+
+	if (conn_data->destroy)
+		conn_data->destroy(conn_data->data);
+
+	health_device_unref(conn_data->dev);
+	g_free(conn_data);
+}
+
+static void destroy_con_mcl_data(gpointer data)
+{
+	con_mcl_data_unref(data);
+}
+
+static struct conn_mcl_data *con_mcl_data_ref(struct conn_mcl_data *conn_data)
+{
+	if (!conn_data)
+		return NULL;
+
+	conn_data->refs++;
+	return conn_data;
+}
+
+static void create_mcl_cb(struct mcap_mcl *mcl, GError *err, gpointer data)
+{
+	struct conn_mcl_data *conn_data = data;
+	struct hdp_device *device = conn_data->dev;
+	GError *gerr = NULL;
+
+	if (err) {
+		conn_data->func(conn_data->data, err);
+		return;
+	}
+
+	if (!device->mcl)
+		device->mcl = mcap_mcl_ref(mcl);
+	device->mcl_conn = TRUE;
+
+	hdp_set_mcl_cb(device, &gerr);
+
+	conn_data->func(conn_data->data, gerr);
+	if (gerr)
+		g_error_free(gerr);
+}
+
+static void search_cb(sdp_list_t *recs, int err, gpointer user_data)
+{
+	struct conn_mcl_data *conn_data = user_data;
+	GError *gerr = NULL;
+	bdaddr_t dst;
+	uint16_t ccpsm;
+
+	if (!conn_data->dev->hdp_adapter->mi) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+						"Mcap instance released");
+		goto fail;
+	}
+
+	if (err || !recs) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+					"Error getting remote SDP records");
+		goto fail;
+	}
+
+	if (!get_ccpsm(recs, &ccpsm)) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+				"Can't get remote PSM for control channel");
+		goto fail;
+	}
+
+	conn_data = con_mcl_data_ref(conn_data);
+
+	device_get_address(conn_data->dev->dev, &dst);
+	if (!mcap_create_mcl(conn_data->dev->hdp_adapter->mi, &dst, ccpsm,
+						create_mcl_cb, conn_data,
+						destroy_con_mcl_data, &gerr)) {
+		con_mcl_data_unref(conn_data);
+		goto fail;
+	}
+	return;
+fail:
+	conn_data->func(conn_data->data, gerr);
+	g_error_free(gerr);
+}
+
+gboolean hdp_establish_mcl(struct hdp_device *device,
+						hdp_continue_proc_f func,
+						gpointer data,
+						GDestroyNotify destroy,
+						GError **err)
+{
+	struct conn_mcl_data *conn_data;
+	bdaddr_t dst, src;
+	uuid_t uuid;
+
+	device_get_address(device->dev, &dst);
+	adapter_get_address(device_get_adapter(device->dev), &src);
+
+	conn_data = g_new0(struct conn_mcl_data, 1);
+	conn_data->refs = 1;
+	conn_data->func = func;
+	conn_data->data = data;
+	conn_data->destroy = destroy;
+	conn_data->dev = health_device_ref(device);
+
+	bt_string2uuid(&uuid, HDP_UUID);
+	if (bt_search_service(&src, &dst, &uuid, search_cb, conn_data,
+						destroy_con_mcl_data)) {
+		g_set_error(err, HDP_ERROR, HDP_CONNECTION_ERROR,
+						"Can't get remote SDP record");
+		g_free(conn_data);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static void get_dcpsm_cb(sdp_list_t *recs, int err, gpointer data)
+{
+	struct get_dcpsm_data *dcpsm_data = data;
+	GError *gerr = NULL;
+	uint16_t dcpsm;
+
+	if (err || !recs) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+					"Error getting remote SDP records");
+		goto fail;
+	}
+
+	if (!get_dcpsm(recs, &dcpsm)) {
+		g_set_error(&gerr, HDP_ERROR, HDP_CONNECTION_ERROR,
+				"Can't get remote PSM for data channel");
+		goto fail;
+	}
+
+	dcpsm_data->func(dcpsm, dcpsm_data->data, NULL);
+	return;
+
+fail:
+	dcpsm_data->func(0, dcpsm_data->data, gerr);
+	g_error_free(gerr);
+}
+
+static void free_dcpsm_data(gpointer data)
+{
+	struct get_dcpsm_data *dcpsm_data = data;
+
+	if (!dcpsm_data)
+		return;
+
+	if (dcpsm_data->destroy)
+		dcpsm_data->destroy(dcpsm_data->data);
+
+	g_free(dcpsm_data);
+}
+
+gboolean hdp_get_dcpsm(struct hdp_device *device, hdp_continue_dcpsm_f func,
+							gpointer data,
+							GDestroyNotify destroy,
+							GError **err)
+{
+	struct get_dcpsm_data *dcpsm_data;
+	bdaddr_t dst, src;
+	uuid_t uuid;
+
+	device_get_address(device->dev, &dst);
+	adapter_get_address(device_get_adapter(device->dev), &src);
+
+	dcpsm_data = g_new0(struct get_dcpsm_data, 1);
+	dcpsm_data->func = func;
+	dcpsm_data->data = data;
+	dcpsm_data->destroy = destroy;
+
+	bt_string2uuid(&uuid, HDP_UUID);
+	if (bt_search_service(&src, &dst, &uuid, get_dcpsm_cb, dcpsm_data,
+							free_dcpsm_data)) {
+		g_set_error(err, HDP_ERROR, HDP_CONNECTION_ERROR,
+						"Can't get remote SDP record");
+		g_free(dcpsm_data);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static void hdp_free_application(struct hdp_application *app)
+{
+	if (app->dbus_watcher)
+		g_dbus_remove_watch(app->conn, app->dbus_watcher);
+
+	if (app->conn)
+		dbus_connection_unref(app->conn);
+	g_free(app->oname);
+	g_free(app->description);
+	g_free(app->path);
+	g_free(app);
+}
+
+struct hdp_application *hdp_application_ref(struct hdp_application *app)
+{
+	if (!app)
+		return NULL;
+
+	app->ref++;
+
+	DBG("health_application_ref(%p): ref=%d", app, app->ref);
+	return app;
+}
+
+void hdp_application_unref(struct hdp_application *app)
+{
+	if (!app)
+		return;
+
+	app->ref --;
+
+	DBG("health_application_unref(%p): ref=%d", app, app->ref);
+	if (app->ref > 0)
+		return;
+
+	hdp_free_application(app);
+}
diff --git a/health/hdp_util.h b/health/hdp_util.h
new file mode 100644
index 0000000..fc59f50
--- /dev/null
+++ b/health/hdp_util.h
@@ -0,0 +1,61 @@
+/*
+ *
+ *  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
+ *
+ */
+
+#ifndef __HDP_UTIL_H__
+#define __HDP_UTIL_H__
+
+typedef void (*hdp_continue_mdep_f)(uint8_t mdep, gpointer user_data,
+								GError *err);
+typedef void (*hdp_continue_dcpsm_f)(uint16_t dcpsm, gpointer user_data,
+								GError *err);
+typedef void (*hdp_continue_proc_f)(gpointer user_data, GError *err);
+
+struct hdp_application *hdp_get_app_config(DBusMessageIter *iter, GError **err);
+gboolean hdp_update_sdp_record(struct hdp_adapter *adapter, GSList *app_list);
+gboolean hdp_get_mdep(struct hdp_device *device, struct hdp_application *app,
+				hdp_continue_mdep_f func,
+				gpointer data, GDestroyNotify destroy,
+				GError **err);
+
+gboolean hdp_establish_mcl(struct hdp_device *device,
+						hdp_continue_proc_f func,
+						gpointer data,
+						GDestroyNotify destroy,
+						GError **err);
+
+gboolean hdp_get_dcpsm(struct hdp_device *device, hdp_continue_dcpsm_f func,
+							gpointer data,
+							GDestroyNotify destroy,
+							GError **err);
+
+
+struct hdp_application *hdp_application_ref(struct hdp_application *app);
+void hdp_application_unref(struct hdp_application *app);
+
+struct hdp_device *health_device_ref(struct hdp_device *hdp_dev);
+void health_device_unref(struct hdp_device *hdp_dev);
+
+
+#endif /* __HDP_UTIL_H__ */
diff --git a/health/mcap.c b/health/mcap.c
new file mode 100644
index 0000000..48866d4
--- /dev/null
+++ b/health/mcap.c
@@ -0,0 +1,2195 @@
+/*
+ *
+ *  MCAP for 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 "log.h"
+#include "error.h"
+
+#include <netinet/in.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include "btio.h"
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include "mcap.h"
+#include "mcap_lib.h"
+#include "mcap_internal.h"
+
+#define RESPONSE_TIMER	6	/* seconds */
+#define MAX_CACHED	10	/* 10 devices */
+
+#define MCAP_ERROR g_quark_from_static_string("mcap-error-quark")
+
+#define RELEASE_TIMER(__mcl) do {		\
+	if (__mcl->tid) {			\
+		g_source_remove(__mcl->tid);	\
+		__mcl->tid = 0;			\
+	}					\
+} while(0)
+
+struct connect_mcl {
+	struct mcap_mcl		*mcl;		/* MCL for this operation */
+	mcap_mcl_connect_cb	connect_cb;	/* Connect callback */
+	GDestroyNotify		destroy;	/* Destroy callback */
+	gpointer		user_data;	/* Callback user data */
+};
+
+typedef union {
+	mcap_mdl_operation_cb		op;
+	mcap_mdl_operation_conf_cb	op_conf;
+	mcap_mdl_notify_cb		notify;
+} mcap_cb_type;
+
+struct mcap_mdl_op_cb {
+	struct mcap_mdl		*mdl;		/* MDL for this operation */
+	mcap_cb_type		cb;		/* Operation callback */
+	GDestroyNotify		destroy;	/* Destroy callback */
+	gpointer		user_data;	/* Callback user data */
+};
+
+/* MCAP finite state machine functions */
+static void proc_req_connected(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t l);
+static void proc_req_pending(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t l);
+static void proc_req_active(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t l);
+
+static void (*proc_req[])(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) = {
+	proc_req_connected,
+	proc_req_pending,
+	proc_req_active
+};
+
+static void mcap_cache_mcl(struct mcap_mcl *mcl);
+
+static void default_mdl_connected_cb(struct mcap_mdl *mdl, gpointer data)
+{
+	DBG("MCAP Unmanaged mdl connection");
+}
+
+static void default_mdl_closed_cb(struct mcap_mdl *mdl, gpointer data)
+{
+	DBG("MCAP Unmanaged mdl closed");
+}
+
+static void default_mdl_deleted_cb(struct mcap_mdl *mdl, gpointer data)
+{
+	DBG("MCAP Unmanaged mdl deleted");
+}
+
+static void default_mdl_aborted_cb(struct mcap_mdl *mdl, gpointer data)
+{
+	DBG("MCAP Unmanaged mdl aborted");
+}
+
+static uint8_t default_mdl_conn_req_cb(struct mcap_mcl *mcl,
+						uint8_t mdepid, uint16_t mdlid,
+						uint8_t *conf, gpointer data)
+{
+	DBG("MCAP mdl remote connection aborted");
+	/* Due to this callback isn't managed this request won't be supported */
+	return MCAP_REQUEST_NOT_SUPPORTED;
+}
+
+static uint8_t default_mdl_reconn_req_cb(struct mcap_mdl *mdl,
+						gpointer data)
+{
+	DBG("MCAP mdl remote reconnection aborted");
+	/* Due to this callback isn't managed this request won't be supported */
+	return MCAP_REQUEST_NOT_SUPPORTED;
+}
+
+static void set_default_cb(struct mcap_mcl *mcl)
+{
+	if (!mcl->cb)
+		mcl->cb = g_new0(struct mcap_mdl_cb, 1);
+
+	mcl->cb->mdl_connected = default_mdl_connected_cb;
+	mcl->cb->mdl_closed = default_mdl_closed_cb;
+	mcl->cb->mdl_deleted = default_mdl_deleted_cb;
+	mcl->cb->mdl_aborted = default_mdl_aborted_cb;
+	mcl->cb->mdl_conn_req = default_mdl_conn_req_cb;
+	mcl->cb->mdl_reconn_req = default_mdl_reconn_req_cb;
+}
+
+static char *error2str(uint8_t rc)
+{
+	switch (rc) {
+	case MCAP_SUCCESS:
+		return "Success";
+	case MCAP_INVALID_OP_CODE:
+		return "Invalid Op Code";
+	case MCAP_INVALID_PARAM_VALUE:
+		return "Invalid Parameter Value";
+	case MCAP_INVALID_MDEP:
+		return "Invalid MDEP";
+	case MCAP_MDEP_BUSY:
+		return "MDEP Busy";
+	case MCAP_INVALID_MDL:
+		return "Invalid MDL";
+	case MCAP_MDL_BUSY:
+		return "MDL Busy";
+	case MCAP_INVALID_OPERATION:
+		return "Invalid Operation";
+	case MCAP_RESOURCE_UNAVAILABLE:
+		return "Resource Unavailable";
+	case MCAP_UNSPECIFIED_ERROR:
+		return "Unspecified Error";
+	case MCAP_REQUEST_NOT_SUPPORTED:
+		return "Request Not Supported";
+	case MCAP_CONFIGURATION_REJECTED:
+		return "Configuration Rejected";
+	default:
+		return "Unknown Response Code";
+	}
+}
+
+static gboolean mcap_send_std_opcode(struct mcap_mcl *mcl, void *cmd,
+						uint32_t size, GError **err)
+{
+	if (mcl->state == MCL_IDLE) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+							"MCL is not connected");
+		return FALSE;
+	}
+
+	if (mcl->req != MCL_AVAILABLE) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_RESOURCE_UNAVAILABLE,
+							"Pending request");
+		return FALSE;
+	}
+
+	if (!(mcl->ctrl & MCAP_CTRL_STD_OP)) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_REQUEST_NOT_SUPPORTED,
+				"Remote does not support standard opcodes");
+		return FALSE;
+	}
+
+	if (mcl->state == MCL_PENDING) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_OPERATION,
+			"Not Std Op. Codes can be sent in PENDING State");
+		return FALSE;
+	}
+
+	if (mcap_send_data(g_io_channel_unix_get_fd(mcl->cc), cmd, size) < 0) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+					"Command can't be sent, write error");
+		return FALSE;
+	}
+
+	mcl->lcmd = cmd;
+	mcl->req = MCL_WAITING_RSP;
+
+	return TRUE;
+}
+
+static void update_mcl_state(struct mcap_mcl *mcl)
+{
+	GSList *l;
+	struct mcap_mdl *mdl;
+
+	if (mcl->state == MCL_PENDING)
+		return;
+
+	for (l = mcl->mdls; l; l = l->next) {
+		mdl = l->data;
+
+		if (mdl->state == MDL_CONNECTED) {
+			mcl->state = MCL_ACTIVE;
+			return;
+		}
+	}
+
+	mcl->state = MCL_CONNECTED;
+}
+
+static void shutdown_mdl(struct mcap_mdl *mdl)
+{
+	mdl->state = MDL_CLOSED;
+
+	if (mdl->wid) {
+		g_source_remove(mdl->wid);
+		mdl->wid = 0;
+	}
+
+	if (mdl->dc) {
+		g_io_channel_shutdown(mdl->dc, TRUE, NULL);
+		g_io_channel_unref(mdl->dc);
+		mdl->dc = NULL;
+	}
+}
+
+static void free_mdl(struct mcap_mdl *mdl)
+{
+	if (!mdl)
+		return;
+
+	mcap_mcl_unref(mdl->mcl);
+	g_free(mdl);
+}
+
+static gint cmp_mdl_state(gconstpointer a, gconstpointer b)
+{
+	const struct mcap_mdl *mdl = a;
+	const MDLState *st = b;
+
+	if (mdl->state == *st)
+		return 0;
+	else if (mdl->state < *st)
+		return -1;
+	else
+		return 1;
+}
+
+static void free_mcap_mdl_op(struct mcap_mdl_op_cb *op)
+{
+	if (op->destroy)
+		op->destroy(op->user_data);
+
+	if (op->mdl)
+		mcap_mdl_unref(op->mdl);
+
+	g_free(op);
+}
+
+static void free_mcl_priv_data(struct mcap_mcl *mcl)
+{
+	free_mcap_mdl_op(mcl->priv_data);
+	mcl->priv_data = NULL;
+}
+
+static void mcap_notify_error(struct mcap_mcl *mcl, GError *err)
+{
+	struct mcap_mdl_op_cb *con = mcl->priv_data;
+	struct mcap_mdl *mdl;
+	MDLState st;
+	GSList *l;
+
+	if (!con || !mcl->lcmd)
+		return;
+
+	switch (mcl->lcmd[0]) {
+	case MCAP_MD_CREATE_MDL_REQ:
+		st = MDL_WAITING;
+		l = g_slist_find_custom(mcl->mdls, &st, cmp_mdl_state);
+		mdl = l->data;
+		mcl->mdls = g_slist_remove(mcl->mdls, mdl);
+		mcap_mdl_unref(mdl);
+		update_mcl_state(mcl);
+		con->cb.op_conf(NULL, 0, err, con->user_data);
+		break;
+	case MCAP_MD_ABORT_MDL_REQ:
+		st = MDL_WAITING;
+		l = g_slist_find_custom(mcl->mdls, &st, cmp_mdl_state);
+		shutdown_mdl(l->data);
+		update_mcl_state(mcl);
+		con->cb.notify(err, con->user_data);
+		break;
+	case MCAP_MD_DELETE_MDL_REQ:
+		for (l = mcl->mdls; l; l = l->next) {
+			mdl = l->data;
+			if (mdl->state == MDL_DELETING)
+				mdl->state = (mdl->dc) ? MDL_CONNECTED :
+								MDL_CLOSED;
+		}
+		update_mcl_state(mcl);
+		con->cb.notify(err, con->user_data);
+		break;
+	case MCAP_MD_RECONNECT_MDL_REQ:
+		st = MDL_WAITING;
+		l = g_slist_find_custom(mcl->mdls, &st, cmp_mdl_state);
+		shutdown_mdl(l->data);
+		update_mcl_state(mcl);
+		con->cb.op(NULL, err, con->user_data);
+		break;
+	}
+
+	free_mcl_priv_data(mcl);
+	g_free(mcl->lcmd);
+	mcl->lcmd = NULL;
+}
+
+int mcap_send_data(int sock, const void *buf, uint32_t size)
+{
+	const uint8_t *buf_b = buf;
+	uint32_t sent = 0;
+
+	while (sent < size) {
+		int n = write(sock, buf_b + sent, size - sent);
+		if (n < 0)
+			return -1;
+		sent += n;
+	}
+
+	return 0;
+}
+
+static int mcap_send_cmd(struct mcap_mcl *mcl, uint8_t oc, uint8_t rc,
+					uint16_t mdl, uint8_t *data, size_t len)
+{
+	mcap_rsp *cmd;
+	int sock, sent;
+
+	if (mcl->cc == NULL)
+		return -1;
+
+	sock = g_io_channel_unix_get_fd(mcl->cc);
+
+	cmd = g_malloc(sizeof(mcap_rsp) + len);
+	cmd->op = oc;
+	cmd->rc = rc;
+	cmd->mdl = htons(mdl);
+
+	if (data && len > 0)
+		memcpy(cmd->data, data, len);
+
+	sent = mcap_send_data(sock, cmd, sizeof(mcap_rsp) + len);
+	g_free(cmd);
+
+	return sent;
+}
+
+static struct mcap_mdl *get_mdl(struct mcap_mcl *mcl, uint16_t mdlid)
+{
+	GSList *l;
+	struct mcap_mdl *mdl;
+
+	for (l = mcl->mdls; l; l = l->next) {
+		mdl = l->data;
+		if (mdlid == mdl->mdlid)
+			return mdl;
+	}
+
+	return NULL;
+}
+
+static uint16_t generate_mdlid(struct mcap_mcl *mcl)
+{
+	uint16_t mdlid = mcl->next_mdl;
+	struct mcap_mdl *mdl;
+
+	do {
+		mdl = get_mdl(mcl, mdlid);
+		if (!mdl) {
+			mcl->next_mdl = (mdlid % MCAP_MDLID_FINAL) + 1;
+			return mdlid;
+		} else
+			mdlid = (mdlid % MCAP_MDLID_FINAL) + 1;
+	} while (mdlid != mcl->next_mdl);
+
+	/* No more mdlids availables */
+	return 0;
+}
+
+static mcap_md_req *create_req(uint8_t op, uint16_t mdl_id)
+{
+	mcap_md_req *req_cmd;
+
+	req_cmd = g_new0(mcap_md_req, 1);
+
+	req_cmd->op = op;
+	req_cmd->mdl = htons(mdl_id);
+
+	return req_cmd;
+}
+
+static mcap_md_create_mdl_req *create_mdl_req(uint16_t mdl_id, uint8_t mdep,
+								uint8_t conf)
+{
+	mcap_md_create_mdl_req *req_mdl;
+
+	req_mdl = g_new0(mcap_md_create_mdl_req, 1);
+
+	req_mdl->op = MCAP_MD_CREATE_MDL_REQ;
+	req_mdl->mdl = htons(mdl_id);
+	req_mdl->mdep = mdep;
+	req_mdl->conf = conf;
+
+	return req_mdl;
+}
+
+static gint compare_mdl(gconstpointer a, gconstpointer b)
+{
+	const struct mcap_mdl *mdla = a;
+	const struct mcap_mdl *mdlb = b;
+
+	if (mdla->mdlid == mdlb->mdlid)
+		return 0;
+	else if (mdla->mdlid < mdlb->mdlid)
+		return -1;
+	else
+		return 1;
+}
+
+static gboolean wait_response_timer(gpointer data)
+{
+	struct mcap_mcl *mcl = data;
+
+	GError *gerr = NULL;
+
+	RELEASE_TIMER(mcl);
+
+	g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_FAILED,
+					"Timeout waiting response");
+
+	mcap_notify_error(mcl, gerr);
+
+	g_error_free(gerr);
+	mcl->mi->mcl_disconnected_cb(mcl, mcl->mi->user_data);
+	mcap_cache_mcl(mcl);
+
+	return FALSE;
+}
+
+gboolean mcap_create_mdl(struct mcap_mcl *mcl,
+				uint8_t mdepid,
+				uint8_t conf,
+				mcap_mdl_operation_conf_cb connect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err)
+{
+	struct mcap_mdl *mdl;
+	struct mcap_mdl_op_cb *con;
+	mcap_md_create_mdl_req *cmd;
+	uint16_t id;
+
+	id = generate_mdlid(mcl);
+	if (!id) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+					"Not more mdlids available");
+		return FALSE;
+	}
+
+	mdl = g_new0(struct mcap_mdl, 1);
+	mdl->mcl = mcap_mcl_ref(mcl);
+	mdl->mdlid = id;
+	mdl->mdep_id = mdepid;
+	mdl->state = MDL_WAITING;
+
+	con = g_new0(struct mcap_mdl_op_cb, 1);
+	con->mdl = mcap_mdl_ref(mdl);
+	con->cb.op_conf = connect_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+	cmd = create_mdl_req(id, mdepid, conf);
+	if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_create_mdl_req),
+									err)) {
+		mcap_mdl_unref(con->mdl);
+		g_free(con);
+		g_free(cmd);
+		return FALSE;
+	}
+
+	mcl->state = MCL_ACTIVE;
+	mcl->priv_data = con;
+
+	mcl->mdls = g_slist_insert_sorted(mcl->mdls, mcap_mdl_ref(mdl),
+								compare_mdl);
+	mcl->tid = g_timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
+									mcl);
+	return TRUE;
+}
+
+gboolean mcap_reconnect_mdl(struct mcap_mdl *mdl,
+				mcap_mdl_operation_cb reconnect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err)
+{
+	struct mcap_mdl_op_cb *con;
+	struct mcap_mcl *mcl = mdl->mcl;
+	mcap_md_req *cmd;
+
+	if (mdl->state != MDL_CLOSED) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+					"MDL is not closed");
+		return FALSE;
+	}
+
+	cmd = create_req(MCAP_MD_RECONNECT_MDL_REQ, mdl->mdlid);
+	if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_req), err)) {
+		g_free(cmd);
+		return FALSE;
+	}
+
+	mdl->state = MDL_WAITING;
+
+	con = g_new0(struct mcap_mdl_op_cb, 1);
+	con->mdl = mcap_mdl_ref(mdl);
+	con->cb.op = reconnect_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+	mcl->state = MCL_ACTIVE;
+	mcl->priv_data = con;
+
+	mcl->tid = g_timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
+									mcl);
+	return TRUE;
+}
+
+static gboolean send_delete_req(struct mcap_mcl *mcl,
+						struct mcap_mdl_op_cb *con,
+						uint16_t mdlid,
+						GError **err)
+{
+	mcap_md_req *cmd;
+
+	cmd = create_req(MCAP_MD_DELETE_MDL_REQ, mdlid);
+	if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_req), err)) {
+		g_free(cmd);
+		return FALSE;
+	}
+
+	mcl->priv_data = con;
+
+	mcl->tid = g_timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
+									mcl);
+	return TRUE;
+}
+
+gboolean mcap_delete_all_mdls(struct mcap_mcl *mcl,
+					mcap_mdl_notify_cb delete_cb,
+					gpointer user_data,
+					GDestroyNotify destroy,
+					GError **err)
+{
+	GSList *l;
+	struct mcap_mdl *mdl;
+	struct mcap_mdl_op_cb *con;
+
+	DBG("MCL in state: %d", mcl->state);
+	if (!mcl->mdls) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+				"There are not MDLs created");
+		return FALSE;
+	}
+
+	for (l = mcl->mdls; l; l = l->next) {
+		mdl = l->data;
+		if (mdl->state != MDL_WAITING)
+			mdl->state = MDL_DELETING;
+	}
+
+	con = g_new0(struct mcap_mdl_op_cb, 1);
+	con->mdl = NULL;
+	con->cb.notify = delete_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+
+	if (!send_delete_req(mcl, con, MCAP_ALL_MDLIDS, err)) {
+		g_free(con);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+gboolean mcap_delete_mdl(struct mcap_mdl *mdl, mcap_mdl_notify_cb delete_cb,
+							gpointer user_data,
+							GDestroyNotify destroy,
+							GError **err)
+{
+	struct mcap_mcl *mcl= mdl->mcl;
+	struct mcap_mdl_op_cb *con;
+	GSList *l;
+
+	l = g_slist_find(mcl->mdls, mdl);
+
+	if (!l) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_MDL,
+					"%s" , error2str(MCAP_INVALID_MDEP));
+		return FALSE;
+	}
+
+	if (mdl->state == MDL_WAITING) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+							"Mdl is not created");
+		return FALSE;
+	}
+
+	mdl->state = MDL_DELETING;
+
+	con = g_new0(struct mcap_mdl_op_cb, 1);
+	con->mdl = mcap_mdl_ref(mdl);
+	con->cb.notify = delete_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+	if (!send_delete_req(mcl, con, mdl->mdlid, err)) {
+		mcap_mdl_unref(con->mdl);
+		g_free(con);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+gboolean mcap_mdl_abort(struct mcap_mdl *mdl, mcap_mdl_notify_cb abort_cb,
+							gpointer user_data,
+							GDestroyNotify destroy,
+							GError **err)
+{
+	struct mcap_mdl_op_cb *con;
+	struct mcap_mcl *mcl = mdl->mcl;
+	mcap_md_req *cmd;
+
+	if (mdl->state != MDL_WAITING) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
+							"Mdl in invalid state");
+		return FALSE;
+	}
+
+	cmd = create_req(MCAP_MD_ABORT_MDL_REQ, mdl->mdlid);
+	if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_req), err)) {
+		g_free(cmd);
+		return FALSE;
+	}
+
+	con = g_new0(struct mcap_mdl_op_cb, 1);
+	con->mdl = mcap_mdl_ref(mdl);
+	con->cb.notify = abort_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+	mcl->priv_data = con;
+	mcl->tid = g_timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
+									mcl);
+	return TRUE;
+}
+
+static struct mcap_mcl *find_mcl(GSList *list, const bdaddr_t *addr)
+{
+	GSList *l;
+	struct mcap_mcl *mcl;
+
+	for (l = list; l; l = l->next) {
+		mcl = l->data;
+
+		if (!bacmp(&mcl->addr, addr))
+			return mcl;
+	}
+
+	return NULL;
+}
+
+int mcap_mdl_get_fd(struct mcap_mdl *mdl)
+{
+	if (!mdl || mdl->state != MDL_CONNECTED)
+		return -ENOTCONN;
+
+	return g_io_channel_unix_get_fd(mdl->dc);
+}
+
+uint16_t mcap_mdl_get_mdlid(struct mcap_mdl *mdl)
+{
+	if (!mdl)
+		return MCAP_MDLID_RESERVED;
+
+	return mdl->mdlid;
+}
+
+static void close_mcl(struct mcap_mcl *mcl, gboolean cache_requested)
+{
+	gboolean save = ((!(mcl->ctrl & MCAP_CTRL_FREE)) && cache_requested);
+
+	RELEASE_TIMER(mcl);
+
+	if (mcl->cc) {
+		g_io_channel_shutdown(mcl->cc, TRUE, NULL);
+		g_io_channel_unref(mcl->cc);
+		mcl->cc = NULL;
+	}
+
+	if (mcl->wid) {
+		g_source_remove(mcl->wid);
+		mcl->wid = 0;
+	}
+
+	if (mcl->lcmd) {
+		g_free(mcl->lcmd);
+		mcl->lcmd = NULL;
+	}
+
+	if (mcl->priv_data)
+		free_mcl_priv_data(mcl);
+
+	g_slist_foreach(mcl->mdls, (GFunc) shutdown_mdl, NULL);
+
+	mcap_sync_stop(mcl);
+
+	mcl->state = MCL_IDLE;
+
+	if (save)
+		return;
+
+	g_slist_foreach(mcl->mdls, (GFunc) mcap_mdl_unref, NULL);
+	g_slist_free(mcl->mdls);
+	mcl->mdls = NULL;
+}
+
+static void mcap_mcl_shutdown(struct mcap_mcl *mcl)
+{
+	close_mcl(mcl, TRUE);
+}
+
+static void mcap_mcl_release(struct mcap_mcl *mcl)
+{
+	close_mcl(mcl, FALSE);
+}
+
+static void mcap_cache_mcl(struct mcap_mcl *mcl)
+{
+	GSList *l;
+	struct mcap_mcl *last;
+	int len;
+
+	if (mcl->ctrl & MCAP_CTRL_CACHED)
+		return;
+
+	mcl->mi->mcls = g_slist_remove(mcl->mi->mcls, mcl);
+
+	if (mcl->ctrl & MCAP_CTRL_NOCACHE) {
+		mcl->mi->cached = g_slist_remove(mcl->mi->cached, mcl);
+		mcap_mcl_release(mcl);
+		mcap_mcl_unref(mcl);
+		return;
+	}
+
+	DBG("Caching MCL");
+
+	len = g_slist_length(mcl->mi->cached);
+	if (len == MAX_CACHED) {
+		/* Remove the latest cached mcl */
+		l = g_slist_last(mcl->mi->cached);
+		last = l->data;
+		mcl->mi->cached = g_slist_remove(mcl->mi->cached, last);
+		last->ctrl &= ~MCAP_CTRL_CACHED;
+		if (last->ctrl & MCAP_CTRL_CONN) {
+			/* We have to release this MCL if */
+			/* connection is not succesful    */
+			last->ctrl |= MCAP_CTRL_FREE;
+		} else {
+			mcap_mcl_release(last);
+			last->mi->mcl_uncached_cb(last, last->mi->user_data);
+		}
+		mcap_mcl_unref(last);
+	}
+
+	mcl->mi->cached = g_slist_prepend(mcl->mi->cached, mcl);
+	mcl->ctrl |= MCAP_CTRL_CACHED;
+	mcap_mcl_shutdown(mcl);
+}
+
+static void mcap_uncache_mcl(struct mcap_mcl *mcl)
+{
+	if (!(mcl->ctrl & MCAP_CTRL_CACHED))
+		return;
+
+	DBG("Got MCL from cache");
+
+	mcl->mi->cached = g_slist_remove(mcl->mi->cached, mcl);
+	mcl->mi->mcls = g_slist_prepend(mcl->mi->mcls, mcl);
+	mcl->ctrl &= ~MCAP_CTRL_CACHED;
+	mcl->ctrl &= ~MCAP_CTRL_FREE;
+}
+
+void mcap_close_mcl(struct mcap_mcl *mcl, gboolean cache)
+{
+	if (!mcl)
+		return;
+
+	if (mcl->ctrl & MCAP_CTRL_FREE) {
+		mcap_mcl_release(mcl);
+		return;
+	}
+
+	if (!cache)
+		mcl->ctrl |= MCAP_CTRL_NOCACHE;
+
+	if (mcl->cc) {
+		g_io_channel_shutdown(mcl->cc, TRUE, NULL);
+		g_io_channel_unref(mcl->cc);
+		mcl->cc = NULL;
+		mcl->state = MCL_IDLE;
+	} else if ((mcl->ctrl & MCAP_CTRL_CACHED) &&
+					(mcl->ctrl & MCAP_CTRL_NOCACHE)) {
+		mcl->ctrl &= ~MCAP_CTRL_CACHED;
+		mcl->mi->cached = g_slist_remove(mcl->mi->cached, mcl);
+		mcap_mcl_release(mcl);
+		mcap_mcl_unref(mcl);
+	}
+}
+
+struct mcap_mcl *mcap_mcl_ref(struct mcap_mcl *mcl)
+{
+	mcl->ref++;
+
+	DBG("mcap_mcl_ref(%p): ref=%d", mcl, mcl->ref);
+
+	return mcl;
+}
+
+void mcap_mcl_unref(struct mcap_mcl *mcl)
+{
+	mcl->ref--;
+
+	DBG("mcap_mcl_unref(%p): ref=%d", mcl, mcl->ref);
+
+	if (mcl->ref > 0)
+		return;
+
+	mcap_mcl_release(mcl);
+	mcap_instance_unref(mcl->mi);
+	g_free(mcl->cb);
+	g_free(mcl);
+}
+
+static gboolean parse_set_opts(struct mcap_mdl_cb *mdl_cb, GError **err,
+						McapMclCb cb1, va_list args)
+{
+	McapMclCb cb = cb1;
+	struct mcap_mdl_cb *c;
+
+	c = g_new0(struct mcap_mdl_cb, 1);
+
+	while (cb != MCAP_MDL_CB_INVALID) {
+		switch (cb) {
+		case MCAP_MDL_CB_CONNECTED:
+			c->mdl_connected = va_arg(args, mcap_mdl_event_cb);
+			break;
+		case MCAP_MDL_CB_CLOSED:
+			c->mdl_closed = va_arg(args, mcap_mdl_event_cb);
+			break;
+		case MCAP_MDL_CB_DELETED:
+			c->mdl_deleted = va_arg(args, mcap_mdl_event_cb);
+			break;
+		case MCAP_MDL_CB_ABORTED:
+			c->mdl_aborted = va_arg(args, mcap_mdl_event_cb);
+			break;
+		case MCAP_MDL_CB_REMOTE_CONN_REQ:
+			c->mdl_conn_req = va_arg(args,
+						mcap_remote_mdl_conn_req_cb);
+			break;
+		case MCAP_MDL_CB_REMOTE_RECONN_REQ:
+			c->mdl_reconn_req = va_arg(args,
+						mcap_remote_mdl_reconn_req_cb);
+			break;
+		default:
+			g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+						"Unknown option %d", cb);
+			return FALSE;
+		}
+		cb = va_arg(args, int);
+	}
+
+	/* Set new callbacks */
+	if (c->mdl_connected)
+		mdl_cb->mdl_connected = c->mdl_connected;
+	if (c->mdl_closed)
+		mdl_cb->mdl_closed = c->mdl_closed;
+	if (c->mdl_deleted)
+		mdl_cb->mdl_deleted = c->mdl_deleted;
+	if (c->mdl_aborted)
+		mdl_cb->mdl_aborted = c->mdl_aborted;
+	if (c->mdl_conn_req)
+		mdl_cb->mdl_conn_req = c->mdl_conn_req;
+	if (c->mdl_reconn_req)
+		mdl_cb->mdl_reconn_req = c->mdl_reconn_req;
+
+	g_free(c);
+
+	return TRUE;
+}
+
+gboolean mcap_mcl_set_cb(struct mcap_mcl *mcl, gpointer user_data,
+					GError **gerr, McapMclCb cb1, ...)
+{
+	va_list args;
+	gboolean ret;
+
+	va_start(args, cb1);
+	ret = parse_set_opts(mcl->cb, gerr, cb1, args);
+	va_end(args);
+
+	if (!ret)
+		return FALSE;
+
+	mcl->cb->user_data = user_data;
+	return TRUE;
+}
+
+void mcap_mcl_get_addr(struct mcap_mcl *mcl, bdaddr_t *addr)
+{
+	bacpy(addr, &mcl->addr);
+}
+
+static void mcap_del_mdl(gpointer elem, gpointer user_data)
+{
+	struct mcap_mdl *mdl = elem;
+	gboolean notify = *(gboolean *) user_data;
+
+	shutdown_mdl(mdl);
+	if (notify)
+		mdl->mcl->cb->mdl_deleted(mdl, mdl->mcl->cb->user_data);
+
+	mcap_mdl_unref(mdl);
+}
+
+static gboolean check_cmd_req_length(struct mcap_mcl *mcl, void *cmd,
+				uint32_t rlen, uint32_t explen, uint8_t rspcod)
+{
+	mcap_md_req *req;
+	uint16_t mdl_id;
+
+	if (rlen != explen) {
+		if (rlen >= sizeof(mcap_md_req)) {
+			req = cmd;
+			mdl_id = ntohs(req->mdl);
+		} else {
+			/* We can't get mdlid */
+			mdl_id = MCAP_MDLID_RESERVED;
+		}
+		mcap_send_cmd(mcl, rspcod, MCAP_INVALID_PARAM_VALUE, mdl_id,
+								NULL, 0);
+		return FALSE;
+	}
+	return TRUE;
+}
+
+static void process_md_create_mdl_req(struct mcap_mcl *mcl, void *cmd,
+								uint32_t len)
+{
+	mcap_md_create_mdl_req *req;
+	struct mcap_mdl *mdl;
+	uint16_t mdl_id;
+	uint8_t mdep_id;
+	uint8_t cfga, conf;
+	uint8_t rsp;
+
+	if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_create_mdl_req),
+							MCAP_MD_CREATE_MDL_RSP))
+		return;
+
+	req = cmd;
+	mdl_id = ntohs(req->mdl);
+	if (mdl_id < MCAP_MDLID_INITIAL || mdl_id > MCAP_MDLID_FINAL) {
+		mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_INVALID_MDL,
+							mdl_id, NULL, 0);
+		return;
+	}
+
+	mdep_id = req->mdep;
+	if (mdep_id > MCAP_MDEPID_FINAL) {
+		mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_INVALID_MDEP,
+							mdl_id, NULL, 0);
+		return;
+	}
+
+	mdl = get_mdl(mcl, mdl_id);
+	if (mdl && (mdl->state == MDL_WAITING || mdl->state == MDL_DELETING )) {
+		/* Creation request arrives for a MDL that is being managed
+		* at current moment */
+		mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_MDL_BUSY,
+							mdl_id, NULL, 0);
+		return;
+	}
+
+	cfga = conf = req->conf;
+	/* Callback to upper layer */
+	rsp = mcl->cb->mdl_conn_req(mcl, mdep_id, mdl_id, &conf,
+							mcl->cb->user_data);
+	if (mcl->state == MCL_IDLE) {
+		/* MCL has been closed int the callback */
+		return;
+	}
+
+	if (cfga != 0 && cfga != conf) {
+		/* Remote device set default configuration but upper profile */
+		/* has changed it. Protocol Error: force closing the MCL by */
+		/* remote device using UNSPECIFIED_ERROR response */
+		mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP,
+				MCAP_UNSPECIFIED_ERROR, mdl_id, NULL, 0);
+		return;
+	}
+	if (rsp != MCAP_SUCCESS) {
+		mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, rsp, mdl_id,
+								NULL, 0);
+		return;
+	}
+
+	if (!mdl) {
+		mdl = g_new0(struct mcap_mdl, 1);
+		mdl->mcl = mcap_mcl_ref(mcl);
+		mdl->mdlid = mdl_id;
+		mcl->mdls = g_slist_insert_sorted(mcl->mdls, mcap_mdl_ref(mdl),
+								compare_mdl);
+	} else if (mdl->state == MDL_CONNECTED) {
+		/* MCAP specification says that we should close the MCL if
+		 * it is open when we receive a MD_CREATE_MDL_REQ */
+		shutdown_mdl(mdl);
+	}
+
+	mdl->mdep_id = mdep_id;
+	mdl->state = MDL_WAITING;
+
+	mcl->state = MCL_PENDING;
+	mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_SUCCESS, mdl_id,
+								&conf, 1);
+}
+
+static void process_md_reconnect_mdl_req(struct mcap_mcl *mcl, void *cmd,
+								uint32_t len)
+{
+	mcap_md_req *req;
+	struct mcap_mdl *mdl;
+	uint16_t mdl_id;
+	uint8_t rsp;
+
+	if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_req),
+						MCAP_MD_RECONNECT_MDL_RSP))
+		return;
+
+	req = cmd;
+	mdl_id = ntohs(req->mdl);
+
+	mdl = get_mdl(mcl, mdl_id);
+	if (!mdl) {
+		mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, MCAP_INVALID_MDL,
+							mdl_id, NULL, 0);
+		return;
+	} else if (mdl->state == MDL_WAITING || mdl->state == MDL_DELETING ) {
+		/* Creation request arrives for a MDL that is being managed
+		* at current moment */
+		mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, MCAP_MDL_BUSY,
+							mdl_id, NULL, 0);
+		return;
+	}
+
+	/* Callback to upper layer */
+	rsp = mcl->cb->mdl_reconn_req(mdl, mcl->cb->user_data);
+	if (mcl->state == MCL_IDLE)
+		return;
+
+	if (rsp != MCAP_SUCCESS) {
+		mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, rsp, mdl_id,
+								NULL, 0);
+		return;
+	}
+
+	if (mdl->state == MDL_CONNECTED)
+		shutdown_mdl(mdl);
+
+	mdl->state = MDL_WAITING;
+	mcl->state = MCL_PENDING;
+	mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, MCAP_SUCCESS, mdl_id,
+								NULL, 0);
+}
+
+static void process_md_abort_mdl_req(struct mcap_mcl *mcl, void *cmd,
+								uint32_t len)
+{
+	mcap_md_req *req;
+	GSList *l;
+	struct mcap_mdl *mdl, *abrt;
+	uint16_t mdl_id;
+
+	if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_req),
+							MCAP_MD_ABORT_MDL_RSP))
+		return;
+
+	req = cmd;
+	mdl_id = ntohs(req->mdl);
+	mcl->state = MCL_CONNECTED;
+	abrt = NULL;
+	for (l = mcl->mdls; l; l = l->next) {
+		mdl = l->data;
+		if (mdl_id == mdl->mdlid && mdl->state == MDL_WAITING) {
+			abrt = mdl;
+			if (mcl->state != MCL_CONNECTED)
+				break;
+			continue;
+		}
+		if (mdl->state == MDL_CONNECTED && mcl->state != MCL_ACTIVE)
+			mcl->state = MCL_ACTIVE;
+
+		if (abrt && mcl->state == MCL_ACTIVE)
+			break;
+	}
+
+	if (!abrt) {
+		mcap_send_cmd(mcl, MCAP_MD_ABORT_MDL_RSP, MCAP_INVALID_MDL,
+							mdl_id, NULL, 0);
+		return;
+	}
+
+	mcl->cb->mdl_aborted(abrt, mcl->cb->user_data);
+	abrt->state = MDL_CLOSED;
+	mcap_send_cmd(mcl, MCAP_MD_ABORT_MDL_RSP, MCAP_SUCCESS, mdl_id,
+								NULL, 0);
+}
+
+static void process_md_delete_mdl_req(struct mcap_mcl *mcl, void *cmd,
+								uint32_t len)
+{
+	mcap_md_req *req;
+	struct mcap_mdl *mdl, *aux;
+	uint16_t mdlid;
+	gboolean notify;
+	GSList *l;
+
+	if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_req),
+							MCAP_MD_DELETE_MDL_RSP))
+		return;
+
+	req = cmd;
+	mdlid = ntohs(req->mdl);
+	if (mdlid == MCAP_ALL_MDLIDS) {
+		notify = FALSE;
+		g_slist_foreach(mcl->mdls, mcap_del_mdl, &notify);
+		g_slist_free(mcl->mdls);
+		mcl->mdls = NULL;
+		mcl->state = MCL_CONNECTED;
+		/* NULL mdl means ALL_MDLS */
+		mcl->cb->mdl_deleted(NULL, mcl->cb->user_data);
+		goto resp;
+	}
+
+	if (mdlid < MCAP_MDLID_INITIAL || mdlid > MCAP_MDLID_FINAL) {
+		mcap_send_cmd(mcl, MCAP_MD_DELETE_MDL_RSP, MCAP_INVALID_MDL,
+								mdlid, NULL, 0);
+		return;
+	}
+
+	for (l = mcl->mdls, mdl = NULL; l; l = l->next) {
+		aux = l->data;
+		if (aux->mdlid == mdlid) {
+			mdl = aux;
+			break;
+		}
+	}
+
+	if (!mdl || mdl->state == MDL_WAITING) {
+		mcap_send_cmd(mcl, MCAP_MD_DELETE_MDL_RSP, MCAP_INVALID_MDL,
+								mdlid, NULL, 0);
+		return;
+	}
+
+	mcl->mdls = g_slist_remove(mcl->mdls, mdl);
+	update_mcl_state(mcl);
+	notify = TRUE;
+	mcap_del_mdl(mdl, &notify);
+
+resp:
+	mcap_send_cmd(mcl, MCAP_MD_DELETE_MDL_RSP, MCAP_SUCCESS, mdlid,
+								NULL, 0);
+}
+
+static void invalid_req_state(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	uint16_t mdlr;
+
+	error("Invalid cmd received (op code = %d) in state %d", cmd[0],
+								mcl->state);
+	/* Get previously mdlid sent to generate an appropriate
+	 * response if it is possible */
+	mdlr = len < sizeof(mcap_md_req) ? MCAP_MDLID_RESERVED :
+					ntohs(((mcap_md_req *) cmd)->mdl);
+	mcap_send_cmd(mcl, cmd[0]+1, MCAP_INVALID_OPERATION, mdlr, NULL, 0);
+}
+
+/* Function used to process commands depending of MCL state */
+static void proc_req_connected(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	switch (cmd[0]) {
+	case MCAP_MD_CREATE_MDL_REQ:
+		process_md_create_mdl_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_RECONNECT_MDL_REQ:
+		process_md_reconnect_mdl_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_DELETE_MDL_REQ:
+		process_md_delete_mdl_req(mcl, cmd, len);
+		break;
+	default:
+		invalid_req_state(mcl, cmd, len);
+	}
+}
+
+static void proc_req_pending(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	if (cmd[0] == MCAP_MD_ABORT_MDL_REQ)
+		process_md_abort_mdl_req(mcl, cmd, len);
+	else
+		invalid_req_state(mcl, cmd, len);
+}
+
+static void proc_req_active(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	switch (cmd[0]) {
+	case MCAP_MD_CREATE_MDL_REQ:
+		process_md_create_mdl_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_RECONNECT_MDL_REQ:
+		process_md_reconnect_mdl_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_DELETE_MDL_REQ:
+		process_md_delete_mdl_req(mcl, cmd, len);
+		break;
+	default:
+		invalid_req_state(mcl, cmd, len);
+	}
+}
+
+/* Function used to process replies */
+static gboolean check_err_rsp(struct mcap_mcl *mcl, mcap_rsp *rsp,
+				uint32_t rlen, uint32_t len, GError **gerr)
+{
+	mcap_md_req *cmdlast = (mcap_md_req *) mcl->lcmd;
+	gint err = MCAP_ERROR_FAILED;
+	gboolean close = FALSE;
+	char *msg;
+
+	if (rsp->op == MCAP_ERROR_RSP) {
+		msg = "MCAP_ERROR_RSP received";
+		close = FALSE;
+		goto fail;
+	}
+
+	/* Check if the response matches with the last request */
+	if (rlen < sizeof(mcap_rsp) || (mcl->lcmd[0] + 1) != rsp->op) {
+		msg = "Protocol error";
+		close = FALSE;
+		goto fail;
+	}
+
+	if (rlen < len) {
+		msg = "Protocol error";
+		close = FALSE;
+		goto fail;
+	}
+
+	if (rsp->mdl != cmdlast->mdl) {
+		msg = "MDLID received doesn't match with MDLID sent";
+		close = TRUE;
+		goto fail;
+	}
+
+	if (rsp->rc == MCAP_REQUEST_NOT_SUPPORTED) {
+		msg = "Remote does not support opcodes";
+		mcl->ctrl &= ~MCAP_CTRL_STD_OP;
+		goto fail;
+	}
+
+	if (rsp->rc == MCAP_UNSPECIFIED_ERROR) {
+		msg = "Unspecified error";
+		close = TRUE;
+		goto fail;
+	}
+
+	if (rsp->rc != MCAP_SUCCESS) {
+		msg = error2str(rsp->rc);
+		err = rsp->rc;
+		goto fail;
+	}
+
+	return FALSE;
+
+fail:
+	g_set_error(gerr, MCAP_ERROR, err, "%s", msg);
+	return close;
+}
+
+static gboolean process_md_create_mdl_rsp(struct mcap_mcl *mcl,
+						mcap_rsp *rsp, uint32_t len)
+{
+	mcap_md_create_mdl_req *cmdlast = (mcap_md_create_mdl_req *) mcl->lcmd;
+	struct mcap_mdl_op_cb *conn = mcl->priv_data;
+	mcap_mdl_operation_conf_cb connect_cb = conn->cb.op_conf;
+	gpointer user_data = conn->user_data;
+	struct mcap_mdl *mdl = conn->mdl;
+	uint8_t conf = cmdlast->conf;
+	gboolean close;
+	GError *gerr = NULL;
+
+	close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp) + 1, &gerr);
+	g_free(mcl->lcmd);
+	mcl->lcmd = NULL;
+	mcl->req = MCL_AVAILABLE;
+
+	if (gerr)
+		goto fail;
+
+	/* Check if preferences changed */
+	if (conf != 0x00 && rsp->data[0] != conf) {
+		g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_FAILED,
+						"Configuration changed");
+		close = TRUE;
+		goto fail;
+	}
+
+	connect_cb(mdl, rsp->data[0], gerr, user_data);
+	return close;
+
+fail:
+	connect_cb(NULL, 0, gerr, user_data);
+	mcl->mdls = g_slist_remove(mcl->mdls, mdl);
+	mcap_mdl_unref(mdl);
+	g_error_free(gerr);
+	update_mcl_state(mcl);
+	return close;
+}
+
+static gboolean process_md_reconnect_mdl_rsp(struct mcap_mcl *mcl,
+						mcap_rsp *rsp, uint32_t len)
+{
+	struct mcap_mdl_op_cb *reconn = mcl->priv_data;
+	mcap_mdl_operation_cb reconn_cb = reconn->cb.op;
+	gpointer user_data = reconn->user_data;
+	struct mcap_mdl *mdl = reconn->mdl;
+	GError *gerr = NULL;
+	gboolean close;
+
+	close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp), &gerr);
+
+	g_free(mcl->lcmd);
+	mcl->lcmd = NULL;
+	mcl->req = MCL_AVAILABLE;
+
+	reconn_cb(mdl, gerr, user_data);
+	if (!gerr)
+		return close;
+
+	g_error_free(gerr);
+	shutdown_mdl(mdl);
+	update_mcl_state(mcl);
+
+	if (rsp->rc != MCAP_INVALID_MDL)
+		return close;
+
+	/* Remove cached mdlid */
+	mcl->mdls = g_slist_remove(mcl->mdls, mdl);
+	mcl->cb->mdl_deleted(mdl, mcl->cb->user_data);
+	mcap_mdl_unref(mdl);
+
+	return close;
+}
+
+static gboolean process_md_abort_mdl_rsp(struct mcap_mcl *mcl,
+						mcap_rsp *rsp, uint32_t len)
+{
+	struct mcap_mdl_op_cb *abrt = mcl->priv_data;
+	mcap_mdl_notify_cb abrt_cb = abrt->cb.notify;
+	gpointer user_data = abrt->user_data;
+	struct mcap_mdl *mdl = abrt->mdl;
+	GError *gerr = NULL;
+	gboolean close;
+
+	close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp), &gerr);
+
+	g_free(mcl->lcmd);
+	mcl->lcmd = NULL;
+	mcl->req = MCL_AVAILABLE;
+
+	abrt_cb(gerr, user_data);
+	shutdown_mdl(mdl);
+
+	if (len >= sizeof(mcap_rsp) && rsp->rc == MCAP_INVALID_MDL) {
+		mcl->mdls = g_slist_remove(mcl->mdls, mdl);
+		mcl->cb->mdl_deleted(mdl, mcl->cb->user_data);
+		mcap_mdl_unref(mdl);
+	}
+
+	if (gerr)
+		g_error_free(gerr);
+
+	update_mcl_state(mcl);
+
+	return close;
+}
+
+static void restore_mdl(gpointer elem, gpointer data)
+{
+	struct mcap_mdl *mdl = elem;
+
+	if (mdl->state == MDL_DELETING) {
+		if (mdl->dc)
+			mdl->state = MDL_CONNECTED;
+		else
+			mdl->state = MDL_CLOSED;
+	} else if (mdl->state == MDL_CLOSED)
+		mdl->mcl->cb->mdl_closed(mdl, mdl->mcl->cb->user_data);
+}
+
+static void check_mdl_del_err(struct mcap_mdl *mdl, mcap_rsp *rsp)
+{
+	if (rsp->rc != MCAP_ERROR_INVALID_MDL) {
+		restore_mdl(mdl, NULL);
+		return;
+	}
+
+	/* MDL does not exist in remote side, we can delete it */
+	mdl->mcl->mdls = g_slist_remove(mdl->mcl->mdls, mdl);
+	mcap_mdl_unref(mdl);
+}
+
+static gboolean process_md_delete_mdl_rsp(struct mcap_mcl *mcl, mcap_rsp *rsp,
+								uint32_t len)
+{
+	struct mcap_mdl_op_cb *del = mcl->priv_data;
+	struct mcap_mdl *mdl = del->mdl;
+	mcap_mdl_notify_cb deleted_cb = del->cb.notify;
+	gpointer user_data = del->user_data;
+	mcap_md_req *cmdlast = (mcap_md_req *) mcl->lcmd;
+	uint16_t mdlid = ntohs(cmdlast->mdl);
+	GError *gerr = NULL;
+	gboolean close;
+	gboolean notify = FALSE;
+
+	close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp), &gerr);
+
+	g_free(mcl->lcmd);
+	mcl->lcmd = NULL;
+	mcl->req = MCL_AVAILABLE;
+
+	if (gerr) {
+		if (mdl)
+			check_mdl_del_err(mdl, rsp);
+		else
+			g_slist_foreach(mcl->mdls, restore_mdl, NULL);
+		deleted_cb(gerr, user_data);
+		g_error_free(gerr);
+		return close;
+	}
+
+	if (mdlid == MCAP_ALL_MDLIDS) {
+		g_slist_foreach(mcl->mdls, mcap_del_mdl, &notify);
+		g_slist_free(mcl->mdls);
+		mcl->mdls = NULL;
+		mcl->state = MCL_CONNECTED;
+	} else {
+		mcl->mdls = g_slist_remove(mcl->mdls, mdl);
+		update_mcl_state(mcl);
+		mcap_del_mdl(mdl, &notify);
+	}
+
+	deleted_cb(gerr, user_data);
+
+	return close;
+}
+
+static void post_process_rsp(struct mcap_mcl *mcl, struct mcap_mdl_op_cb *op)
+{
+	if (mcl->priv_data != op) {
+		/* Queued MCAP request in some callback. */
+		/* We should not delete the mcl private data */
+		free_mcap_mdl_op(op);
+	} else {
+		/* This is not a queued request. It's safe */
+		/* delete the mcl private data here. */
+		free_mcl_priv_data(mcl);
+	}
+}
+
+static void proc_response(struct mcap_mcl *mcl, void *buf, uint32_t len)
+{
+	struct mcap_mdl_op_cb *op = mcl->priv_data;
+	mcap_rsp *rsp = buf;
+	gboolean close;
+
+	RELEASE_TIMER(mcl);
+
+	switch (mcl->lcmd[0] + 1) {
+	case MCAP_MD_CREATE_MDL_RSP:
+		close = process_md_create_mdl_rsp(mcl, rsp, len);
+		post_process_rsp(mcl, op);
+		break;
+	case MCAP_MD_RECONNECT_MDL_RSP:
+		close = process_md_reconnect_mdl_rsp(mcl, rsp, len);
+		post_process_rsp(mcl, op);
+		break;
+	case MCAP_MD_ABORT_MDL_RSP:
+		close = process_md_abort_mdl_rsp(mcl, rsp, len);
+		post_process_rsp(mcl, op);
+		break;
+	case MCAP_MD_DELETE_MDL_RSP:
+		close = process_md_delete_mdl_rsp(mcl, rsp, len);
+		post_process_rsp(mcl, op);
+		break;
+	default:
+		DBG("Unknown cmd response received (op code = %d)", rsp->op);
+		close = TRUE;
+		break;
+	}
+
+	if (close) {
+		mcl->mi->mcl_disconnected_cb(mcl, mcl->mi->user_data);
+		mcap_cache_mcl(mcl);
+	}
+}
+
+static void proc_cmd(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	GError *gerr = NULL;
+
+	if (cmd[0] > MCAP_MD_SYNC_INFO_IND ||
+					(cmd[0] > MCAP_MD_DELETE_MDL_RSP &&
+					cmd[0] < MCAP_MD_SYNC_CAP_REQ)) {
+		error("Unknown cmd received (op code = %d)", cmd[0]);
+		mcap_send_cmd(mcl, MCAP_ERROR_RSP, MCAP_INVALID_OP_CODE,
+						MCAP_MDLID_RESERVED, NULL, 0);
+		return;
+	}
+
+	if (cmd[0] >= MCAP_MD_SYNC_CAP_REQ &&
+					cmd[0] <= MCAP_MD_SYNC_INFO_IND) {
+		proc_sync_cmd(mcl, cmd, len);
+		return;
+	}
+
+	if (!(mcl->ctrl & MCAP_CTRL_STD_OP)) {
+		/* In case the remote device doesn't work correctly */
+		error("Remote device does not support opcodes, cmd ignored");
+		return;
+	}
+
+	if (mcl->req == MCL_WAITING_RSP) {
+		if (cmd[0] & 0x01) {
+			/* Request arrived when a response is expected */
+			if (mcl->role == MCL_INITIATOR)
+				/* ignore */
+				return;
+			/* Initiator will ignore our last request */
+			RELEASE_TIMER(mcl);
+			mcl->req = MCL_AVAILABLE;
+			g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_REQ_IGNORED,
+				"Initiator sent a request with more priority");
+			mcap_notify_error(mcl, gerr);
+			proc_req[mcl->state](mcl, cmd, len);
+			return;
+		}
+		proc_response(mcl, cmd, len);
+	} else if (cmd[0] & 0x01)
+		proc_req[mcl->state](mcl, cmd, len);
+}
+
+static gboolean mdl_event_cb(GIOChannel *chan, GIOCondition cond, gpointer data)
+{
+
+	struct mcap_mdl *mdl = data;
+	gboolean notify;
+
+	DBG("Close MDL %d", mdl->mdlid);
+
+	notify = (mdl->state == MDL_CONNECTED);
+	shutdown_mdl(mdl);
+
+	update_mcl_state(mdl->mcl);
+
+	if (notify) {
+		/*Callback to upper layer */
+		mdl->mcl->cb->mdl_closed(mdl, mdl->mcl->cb->user_data);
+	}
+
+	return FALSE;
+}
+
+static void mcap_connect_mdl_cb(GIOChannel *chan, GError *conn_err,
+								gpointer data)
+{
+	struct mcap_mdl_op_cb *con = data;
+	struct mcap_mdl *mdl = con->mdl;
+	mcap_mdl_operation_cb cb = con->cb.op;
+	gpointer user_data = con->user_data;
+
+	DBG("mdl connect callback");
+
+	if (conn_err) {
+		DBG("ERROR: mdl connect callback");
+		mdl->state = MDL_CLOSED;
+		g_io_channel_unref(mdl->dc);
+		mdl->dc = NULL;
+		cb(mdl, conn_err, user_data);
+		return;
+	}
+
+	mdl->state = MDL_CONNECTED;
+	mdl->wid = g_io_add_watch_full(mdl->dc, G_PRIORITY_DEFAULT,
+					G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+					(GIOFunc) mdl_event_cb,
+					mcap_mdl_ref(mdl),
+					(GDestroyNotify) mcap_mdl_unref);
+
+	cb(mdl, conn_err, user_data);
+}
+
+gboolean mcap_connect_mdl(struct mcap_mdl *mdl, uint8_t mode,
+					uint16_t dcpsm,
+					mcap_mdl_operation_cb connect_cb,
+					gpointer user_data,
+					GDestroyNotify destroy,
+					GError **err)
+{
+	struct mcap_mdl_op_cb *con;
+
+	if (mdl->state != MDL_WAITING) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_MDL,
+					"%s", error2str(MCAP_INVALID_MDL));
+		return FALSE;
+	}
+
+	if ((mode != L2CAP_MODE_ERTM) && (mode != L2CAP_MODE_STREAMING)) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+						"Invalid MDL configuration");
+		return FALSE;
+	}
+
+	con = g_new0(struct mcap_mdl_op_cb, 1);
+	con->mdl = mcap_mdl_ref(mdl);
+	con->cb.op = connect_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+	mdl->dc = bt_io_connect(BT_IO_L2CAP, mcap_connect_mdl_cb, con,
+				(GDestroyNotify) free_mcap_mdl_op, err,
+				BT_IO_OPT_SOURCE_BDADDR, &mdl->mcl->mi->src,
+				BT_IO_OPT_DEST_BDADDR, &mdl->mcl->addr,
+				BT_IO_OPT_PSM, dcpsm,
+				BT_IO_OPT_MTU, MCAP_DC_MTU,
+				BT_IO_OPT_SEC_LEVEL, mdl->mcl->mi->sec,
+				BT_IO_OPT_MODE, mode,
+				BT_IO_OPT_INVALID);
+	if (!mdl->dc) {
+		DBG("MDL Connection error");
+		mdl->state = MDL_CLOSED;
+		mcap_mdl_unref(con->mdl);
+		g_free(con);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static gboolean mcl_control_cb(GIOChannel *chan, GIOCondition cond,
+								gpointer data)
+{
+	GError *gerr = NULL;
+	struct mcap_mcl *mcl = data;
+	int sk, len;
+	uint8_t buf[MCAP_CC_MTU];
+
+	if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL))
+		goto fail;
+
+	sk = g_io_channel_unix_get_fd(chan);
+	len = read(sk, buf, sizeof(buf));
+	if (len < 0)
+		goto fail;
+
+	proc_cmd(mcl, buf, (uint32_t) len);
+	return TRUE;
+
+fail:
+	if (mcl->state != MCL_IDLE) {
+		if (mcl->req == MCL_WAITING_RSP) {
+			/* notify error in pending callback */
+			g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_MCL_CLOSED,
+								"MCL closed");
+			mcap_notify_error(mcl, gerr);
+			g_error_free(gerr);
+		}
+		mcl->mi->mcl_disconnected_cb(mcl, mcl->mi->user_data);
+	}
+	mcap_cache_mcl(mcl);
+	return FALSE;
+}
+
+static void mcap_connect_mcl_cb(GIOChannel *chan, GError *conn_err,
+							gpointer user_data)
+{
+	char dstaddr[18];
+	struct connect_mcl *con = user_data;
+	struct mcap_mcl *aux, *mcl = con->mcl;
+	mcap_mcl_connect_cb connect_cb = con->connect_cb;
+	gpointer data = con->user_data;
+	GError *gerr = NULL;
+
+	mcl->ctrl &= ~MCAP_CTRL_CONN;
+
+	if (conn_err) {
+		if (mcl->ctrl & MCAP_CTRL_FREE) {
+			mcap_mcl_release(mcl);
+			mcl->mi->mcl_uncached_cb(mcl, mcl->mi->user_data);
+		}
+		connect_cb(NULL, conn_err, data);
+		return;
+	}
+
+	ba2str(&mcl->addr, dstaddr);
+
+	aux = find_mcl(mcl->mi->mcls, &mcl->addr);
+	if (aux) {
+		/* Double MCL connection case */
+		error("MCL error: Device %s is already connected", dstaddr);
+		g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_ALREADY_EXISTS,
+					"MCL %s is already connected", dstaddr);
+		connect_cb(NULL, gerr, data);
+		g_error_free(gerr);
+		return;
+	}
+
+	mcl->state = MCL_CONNECTED;
+	mcl->role = MCL_INITIATOR;
+	mcl->req = MCL_AVAILABLE;
+	mcl->ctrl |= MCAP_CTRL_STD_OP;
+
+	mcap_sync_init(mcl);
+
+	if (mcl->ctrl & MCAP_CTRL_CACHED)
+		mcap_uncache_mcl(mcl);
+	else {
+		mcl->ctrl &= ~MCAP_CTRL_FREE;
+		mcl->mi->mcls = g_slist_prepend(mcl->mi->mcls,
+							mcap_mcl_ref(mcl));
+	}
+
+	mcl->wid = g_io_add_watch_full(mcl->cc, G_PRIORITY_DEFAULT,
+				G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+				(GIOFunc) mcl_control_cb,
+				mcap_mcl_ref(mcl),
+				(GDestroyNotify) mcap_mcl_unref);
+	connect_cb(mcl, gerr, data);
+}
+
+static void set_mdl_properties(GIOChannel *chan, struct mcap_mdl *mdl)
+{
+	struct mcap_mcl *mcl = mdl->mcl;
+
+	mdl->state = MDL_CONNECTED;
+	mdl->dc = g_io_channel_ref(chan);
+	mdl->wid = g_io_add_watch_full(mdl->dc, G_PRIORITY_DEFAULT,
+					G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+					(GIOFunc) mdl_event_cb,
+					mcap_mdl_ref(mdl),
+					(GDestroyNotify) mcap_mdl_unref);
+
+	mcl->state = MCL_ACTIVE;
+	mcl->cb->mdl_connected(mdl, mcl->cb->user_data);
+}
+
+static void mcl_io_destroy(gpointer data)
+{
+	struct connect_mcl *con = data;
+
+	mcap_mcl_unref(con->mcl);
+	if (con->destroy)
+		con->destroy(con->user_data);
+	g_free(con);
+}
+
+gboolean mcap_create_mcl(struct mcap_instance *mi,
+				const bdaddr_t *addr,
+				uint16_t ccpsm,
+				mcap_mcl_connect_cb connect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err)
+{
+	struct mcap_mcl *mcl;
+	struct connect_mcl *con;
+
+	mcl = find_mcl(mi->mcls, addr);
+	if (mcl) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_ALREADY_EXISTS,
+					"MCL is already connected.");
+		return FALSE;
+	}
+
+	mcl = find_mcl(mi->cached, addr);
+	if (!mcl) {
+		mcl = g_new0(struct mcap_mcl, 1);
+		mcl->mi = mcap_instance_ref(mi);
+		mcl->state = MCL_IDLE;
+		bacpy(&mcl->addr, addr);
+		set_default_cb(mcl);
+		mcl->next_mdl = (rand() % MCAP_MDLID_FINAL) + 1;
+	}
+
+	mcl->ctrl |= MCAP_CTRL_CONN;
+
+	con = g_new0(struct connect_mcl, 1);
+	con->mcl = mcap_mcl_ref(mcl);
+	con->connect_cb = connect_cb;
+	con->destroy = destroy;
+	con->user_data = user_data;
+
+	mcl->cc = bt_io_connect(BT_IO_L2CAP, mcap_connect_mcl_cb, con,
+				mcl_io_destroy, err,
+				BT_IO_OPT_SOURCE_BDADDR, &mi->src,
+				BT_IO_OPT_DEST_BDADDR, addr,
+				BT_IO_OPT_PSM, ccpsm,
+				BT_IO_OPT_MTU, MCAP_CC_MTU,
+				BT_IO_OPT_SEC_LEVEL, mi->sec,
+				BT_IO_OPT_MODE, L2CAP_MODE_ERTM,
+				BT_IO_OPT_INVALID);
+	if (!mcl->cc) {
+		mcl->ctrl &= ~MCAP_CTRL_CONN;
+		if (mcl->ctrl & MCAP_CTRL_FREE) {
+			mcap_mcl_release(mcl);
+			mcl->mi->mcl_uncached_cb(mcl, mcl->mi->user_data);
+		}
+		mcap_mcl_unref(con->mcl);
+		g_free(con);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+static void connect_dc_event_cb(GIOChannel *chan, GError *gerr,
+							gpointer user_data)
+{
+	struct mcap_instance *mi = user_data;
+	struct mcap_mcl *mcl;
+	struct mcap_mdl *mdl;
+	GError *err = NULL;
+	bdaddr_t dst;
+	GSList *l;
+
+	if (gerr)
+		return;
+
+	bt_io_get(chan, BT_IO_L2CAP, &err,
+			BT_IO_OPT_DEST_BDADDR, &dst,
+			BT_IO_OPT_INVALID);
+	if (err) {
+		error("%s", err->message);
+		g_error_free(err);
+		goto drop;
+	}
+
+	mcl = find_mcl(mi->mcls, &dst);
+	if (!mcl || mcl->state != MCL_PENDING)
+		goto drop;
+
+	for (l = mcl->mdls; l; l = l->next) {
+		mdl = l->data;
+		if (mdl->state == MDL_WAITING) {
+			set_mdl_properties(chan, mdl);
+			return;
+		}
+	}
+
+drop:
+	g_io_channel_shutdown(chan, TRUE, NULL);
+}
+
+static void set_mcl_conf(GIOChannel *chan, struct mcap_mcl *mcl)
+{
+	gboolean reconn;
+
+	mcl->state = MCL_CONNECTED;
+	mcl->role = MCL_ACCEPTOR;
+	mcl->req = MCL_AVAILABLE;
+	mcl->cc = g_io_channel_ref(chan);
+	mcl->ctrl |= MCAP_CTRL_STD_OP;
+
+	mcap_sync_init(mcl);
+
+	reconn = (mcl->ctrl & MCAP_CTRL_CACHED);
+	if (reconn)
+		mcap_uncache_mcl(mcl);
+	else
+		mcl->mi->mcls = g_slist_prepend(mcl->mi->mcls,
+							mcap_mcl_ref(mcl));
+
+	mcl->wid = g_io_add_watch_full(mcl->cc, G_PRIORITY_DEFAULT,
+				G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
+				(GIOFunc) mcl_control_cb,
+				mcap_mcl_ref(mcl),
+				(GDestroyNotify) mcap_mcl_unref);
+
+	/* Callback to report new MCL */
+	if (reconn)
+		mcl->mi->mcl_reconnected_cb(mcl, mcl->mi->user_data);
+	else
+		mcl->mi->mcl_connected_cb(mcl, mcl->mi->user_data);
+}
+
+static void connect_mcl_event_cb(GIOChannel *chan, GError *gerr,
+							gpointer user_data)
+{
+	struct mcap_instance *mi = user_data;
+	struct mcap_mcl *mcl;
+	bdaddr_t dst;
+	char address[18], srcstr[18];
+	GError *err = NULL;
+
+	if (gerr)
+		return;
+
+	bt_io_get(chan, BT_IO_L2CAP, &err,
+			BT_IO_OPT_DEST_BDADDR, &dst,
+			BT_IO_OPT_DEST, address,
+			BT_IO_OPT_INVALID);
+	if (err) {
+		error("%s", err->message);
+		g_error_free(err);
+		goto drop;
+	}
+
+	ba2str(&mi->src, srcstr);
+	mcl = find_mcl(mi->mcls, &dst);
+	if (mcl) {
+		error("Control channel already created with %s on adapter %s",
+				address, srcstr);
+		goto drop;
+	}
+
+	mcl = find_mcl(mi->cached, &dst);
+	if (!mcl) {
+		mcl = g_new0(struct mcap_mcl, 1);
+		mcl->mi = mcap_instance_ref(mi);
+		bacpy(&mcl->addr, &dst);
+		set_default_cb(mcl);
+		mcl->next_mdl = (rand() % MCAP_MDLID_FINAL) + 1;
+	}
+
+	set_mcl_conf(chan, mcl);
+
+	return;
+drop:
+	g_io_channel_shutdown(chan, TRUE, NULL);
+}
+
+struct mcap_instance *mcap_create_instance(bdaddr_t *src,
+					BtIOSecLevel sec,
+					uint16_t ccpsm,
+					uint16_t dcpsm,
+					mcap_mcl_event_cb mcl_connected,
+					mcap_mcl_event_cb mcl_reconnected,
+					mcap_mcl_event_cb mcl_disconnected,
+					mcap_mcl_event_cb mcl_uncached,
+					mcap_info_ind_event_cb mcl_sync_info_ind,
+					gpointer user_data,
+					GError **gerr)
+{
+	struct mcap_instance *mi;
+
+	if (sec < BT_IO_SEC_MEDIUM) {
+		g_set_error(gerr, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+				"Security level can't be minor of %d",
+				BT_IO_SEC_MEDIUM);
+		return NULL;
+	}
+
+	if (!(mcl_connected && mcl_reconnected &&
+			mcl_disconnected && mcl_uncached)) {
+		g_set_error(gerr, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+				"The callbacks can't be null");
+		return NULL;
+	}
+
+	mi = g_new0(struct mcap_instance, 1);
+
+	bacpy(&mi->src, src);
+
+	mi->sec = sec;
+	mi->mcl_connected_cb = mcl_connected;
+	mi->mcl_reconnected_cb = mcl_reconnected;
+	mi->mcl_disconnected_cb = mcl_disconnected;
+	mi->mcl_uncached_cb = mcl_uncached;
+	mi->mcl_sync_infoind_cb = mcl_sync_info_ind;
+	mi->user_data = user_data;
+	mi->csp_enabled = FALSE;
+
+	/* Listen incoming connections in control channel */
+	mi->ccio = bt_io_listen(BT_IO_L2CAP, connect_mcl_event_cb, NULL, mi,
+				NULL, gerr,
+				BT_IO_OPT_SOURCE_BDADDR, &mi->src,
+				BT_IO_OPT_PSM, ccpsm,
+				BT_IO_OPT_MTU, MCAP_CC_MTU,
+				BT_IO_OPT_SEC_LEVEL, sec,
+				BT_IO_OPT_MODE, L2CAP_MODE_ERTM,
+				BT_IO_OPT_INVALID);
+	if (!mi->ccio) {
+		error("%s", (*gerr)->message);
+		g_free(mi);
+		return NULL;
+	}
+
+	/* Listen incoming connections in data channels */
+	mi->dcio = bt_io_listen(BT_IO_L2CAP, connect_dc_event_cb, NULL, mi,
+				NULL, gerr,
+				BT_IO_OPT_SOURCE_BDADDR, &mi->src,
+				BT_IO_OPT_PSM, dcpsm,
+				BT_IO_OPT_MTU, MCAP_DC_MTU,
+				BT_IO_OPT_SEC_LEVEL, sec,
+				BT_IO_OPT_INVALID);
+	if (!mi->dcio) {
+		g_io_channel_shutdown(mi->ccio, TRUE, NULL);
+		g_io_channel_unref(mi->ccio);
+		mi->ccio = NULL;
+		error("%s", (*gerr)->message);
+		g_free(mi);
+		return NULL;
+	}
+
+	/* Initialize random seed to generate mdlids for this instance */
+	srand(time(NULL));
+
+	return mcap_instance_ref(mi);;
+}
+
+void mcap_release_instance(struct mcap_instance *mi)
+{
+	GSList *l;
+
+	if (!mi)
+		return;
+
+	if (mi->ccio) {
+		g_io_channel_shutdown(mi->ccio, TRUE, NULL);
+		g_io_channel_unref(mi->ccio);
+		mi->ccio = NULL;
+	}
+
+	if (mi->dcio) {
+		g_io_channel_shutdown(mi->dcio, TRUE, NULL);
+		g_io_channel_unref(mi->dcio);
+		mi->dcio = NULL;
+	}
+
+	for (l = mi->mcls; l; l = l->next) {
+		mcap_mcl_release(l->data);
+		mcap_mcl_unref(l->data);
+	}
+
+	g_slist_free(mi->mcls);
+	mi->mcls = NULL;
+
+	for (l = mi->cached; l; l = l->next) {
+		mcap_mcl_release(l->data);
+		mcap_mcl_unref(l->data);
+	}
+
+	g_slist_free(mi->cached);
+	mi->cached = NULL;
+}
+
+struct mcap_instance *mcap_instance_ref(struct mcap_instance *mi)
+{
+	mi->ref++;
+
+	DBG("mcap_instance_ref(%p): ref=%d", mi, mi->ref);
+
+	return mi;
+}
+
+void mcap_instance_unref(struct mcap_instance *mi)
+{
+	mi->ref--;
+
+	DBG("mcap_instance_unref(%p): ref=%d", mi, mi->ref);
+
+	if (mi->ref > 0)
+		return;
+
+	mcap_release_instance(mi);
+	g_free(mi);
+}
+
+uint16_t mcap_get_ctrl_psm(struct mcap_instance *mi, GError **err)
+{
+	uint16_t lpsm;
+
+	if (!(mi && mi->ccio)) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+			"Invalid MCAP instance");
+		return 0;
+	}
+
+	if (!bt_io_get(mi->ccio, BT_IO_L2CAP, err,
+			BT_IO_OPT_PSM, &lpsm,
+			BT_IO_OPT_INVALID))
+		return 0;
+
+	return lpsm;
+}
+
+uint16_t mcap_get_data_psm(struct mcap_instance *mi, GError **err)
+{
+	uint16_t lpsm;
+
+	if (!(mi && mi->dcio)) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+			"Invalid MCAP instance");
+		return 0;
+	}
+
+	if (!bt_io_get(mi->dcio, BT_IO_L2CAP, err,
+			BT_IO_OPT_PSM, &lpsm,
+			BT_IO_OPT_INVALID))
+		return 0;
+
+	return lpsm;
+}
+
+gboolean mcap_set_data_chan_mode(struct mcap_instance *mi, uint8_t mode,
+								GError **err)
+{
+	if (!(mi && mi->dcio)) {
+		g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
+						"Invalid MCAP instance");
+		return FALSE;
+	}
+
+	return bt_io_set(mi->dcio, BT_IO_L2CAP, err, BT_IO_OPT_MODE, mode,
+							BT_IO_OPT_INVALID);
+}
+
+struct mcap_mdl *mcap_mdl_ref(struct mcap_mdl *mdl)
+{
+	mdl->ref++;
+
+	DBG("mcap_mdl_ref(%p): ref=%d", mdl, mdl->ref);
+
+	return mdl;
+}
+
+void mcap_mdl_unref(struct mcap_mdl *mdl)
+{
+	mdl->ref--;
+
+	DBG("mcap_mdl_unref(%p): ref=%d", mdl, mdl->ref);
+
+	if (mdl->ref > 0)
+		return;
+
+	free_mdl(mdl);
+}
diff --git a/health/mcap.h b/health/mcap.h
new file mode 100644
index 0000000..81f53f1
--- /dev/null
+++ b/health/mcap.h
@@ -0,0 +1,169 @@
+/*
+ *
+ *  MCAP for BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
+ *  Copyright (C) 2010 Signove
+ *
+ *  Authors:
+ *  Santiago Carot-Nemesio <sancane at gmail.com>
+ *  Jose Antonio Santos-Cadenas <santoscadenas at gmail.com>
+ *  Elvis Pfützenreuter <epx at signove.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
+ *
+ */
+
+#ifndef __MCAP_H
+#define __MCAP_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MCAP_VERSION	0x0100	/* current version 01.00 */
+
+/* bytes to get MCAP Supported Procedures */
+#define MCAP_SUP_PROC	0x06
+
+/* maximum transmission unit for channels */
+#define MCAP_CC_MTU	48
+#define MCAP_DC_MTU	L2CAP_DEFAULT_MTU
+
+/* MCAP Standard Op Codes */
+#define MCAP_ERROR_RSP			0x00
+#define MCAP_MD_CREATE_MDL_REQ		0x01
+#define MCAP_MD_CREATE_MDL_RSP		0x02
+#define MCAP_MD_RECONNECT_MDL_REQ	0x03
+#define MCAP_MD_RECONNECT_MDL_RSP	0x04
+#define MCAP_MD_ABORT_MDL_REQ		0x05
+#define MCAP_MD_ABORT_MDL_RSP		0x06
+#define MCAP_MD_DELETE_MDL_REQ		0x07
+#define MCAP_MD_DELETE_MDL_RSP		0x08
+
+/* MCAP Clock Sync Op Codes */
+#define MCAP_MD_SYNC_CAP_REQ		0x11
+#define MCAP_MD_SYNC_CAP_RSP		0x12
+#define MCAP_MD_SYNC_SET_REQ		0x13
+#define MCAP_MD_SYNC_SET_RSP		0x14
+#define MCAP_MD_SYNC_INFO_IND		0x15
+
+/* MCAP Response codes */
+#define MCAP_SUCCESS			0x00
+#define MCAP_INVALID_OP_CODE		0x01
+#define MCAP_INVALID_PARAM_VALUE	0x02
+#define MCAP_INVALID_MDEP		0x03
+#define MCAP_MDEP_BUSY			0x04
+#define MCAP_INVALID_MDL		0x05
+#define MCAP_MDL_BUSY			0x06
+#define MCAP_INVALID_OPERATION		0x07
+#define MCAP_RESOURCE_UNAVAILABLE	0x08
+#define MCAP_UNSPECIFIED_ERROR		0x09
+#define MCAP_REQUEST_NOT_SUPPORTED	0x0A
+#define MCAP_CONFIGURATION_REJECTED	0x0B
+
+/* MDL IDs */
+#define MCAP_MDLID_RESERVED		0x0000
+#define MCAP_MDLID_INITIAL		0x0001
+#define MCAP_MDLID_FINAL		0xFEFF
+#define MCAP_ALL_MDLIDS			0xFFFF
+
+/* MDEP IDs */
+#define MCAP_MDEPID_INITIAL		0x00
+#define MCAP_MDEPID_FINAL		0x7F
+
+/* CSP special values */
+#define MCAP_BTCLOCK_IMMEDIATE		0xffffffffUL
+#define MCAP_TMSTAMP_DONTSET		0xffffffffffffffffULL
+#define MCAP_BTCLOCK_MAX		0x0fffffff
+#define MCAP_BTCLOCK_FIELD		(MCAP_BTCLOCK_MAX + 1)
+
+/*
+ * MCAP Request Packet Format
+ */
+
+typedef struct {
+	uint8_t		op;
+	uint16_t	mdl;
+	uint8_t		mdep;
+	uint8_t		conf;
+} __attribute__ ((packed)) mcap_md_create_mdl_req;
+
+typedef struct {
+	uint8_t		op;
+	uint16_t	mdl;
+} __attribute__ ((packed)) mcap_md_req;
+
+/*
+ * MCAP Response Packet Format
+ */
+
+typedef struct {
+	uint8_t		op;
+	uint8_t		rc;
+	uint16_t	mdl;
+	uint8_t		data[0];
+} __attribute__ ((packed)) mcap_rsp;
+
+/*
+ * MCAP Clock Synchronization Protocol
+ */
+
+typedef struct {
+	uint8_t		op;
+	uint16_t	timest;
+} __attribute__ ((packed)) mcap_md_sync_cap_req;
+
+typedef struct {
+	uint8_t		op;
+	uint8_t		rc;
+} __attribute__ ((packed)) mcap_md_sync_rsp;
+
+typedef struct {
+        uint8_t         op;
+	uint8_t         rc;
+	uint8_t         btclock;
+        uint16_t        sltime;
+	uint16_t        timestnr;
+	uint16_t        timestna;
+} __attribute__ ((packed)) mcap_md_sync_cap_rsp;
+
+typedef struct {
+	uint8_t		op;
+	uint8_t		timestui;
+	uint32_t	btclock;
+	uint64_t	timestst;
+} __attribute__ ((packed)) mcap_md_sync_set_req;
+
+typedef struct {
+	int8_t		op;
+	uint8_t		rc;
+	uint32_t	btclock;
+	uint64_t	timestst;
+	uint16_t	timestsa;
+} __attribute__ ((packed)) mcap_md_sync_set_rsp;
+
+typedef struct {
+	uint8_t		op;
+	uint32_t	btclock;
+	uint64_t	timestst;
+	uint16_t	timestsa;
+} __attribute__ ((packed)) mcap_md_sync_info_ind;
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MCAP_H */
diff --git a/health/mcap_internal.h b/health/mcap_internal.h
new file mode 100644
index 0000000..f82e967
--- /dev/null
+++ b/health/mcap_internal.h
@@ -0,0 +1,141 @@
+/*
+ *
+ *  MCAP for 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
+ *
+ */
+
+#ifndef __MCAP_INTERNAL_H
+#define __MCAP_INTERNAL_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+	MCL_CONNECTED,
+	MCL_PENDING,
+	MCL_ACTIVE,
+	MCL_IDLE
+} MCLState;
+
+typedef enum {
+	MCL_ACCEPTOR,
+	MCL_INITIATOR
+} MCLRole;
+
+typedef enum {
+	MCL_AVAILABLE,
+	MCL_WAITING_RSP
+} MCAPCtrl;
+
+typedef enum {
+	MDL_WAITING,
+	MDL_CONNECTED,
+	MDL_DELETING,
+	MDL_CLOSED
+} MDLState;
+
+struct mcap_mdl_cb {
+	mcap_mdl_event_cb		mdl_connected;	/* Remote device has created a MDL */
+	mcap_mdl_event_cb		mdl_closed;	/* Remote device has closed a MDL */
+	mcap_mdl_event_cb		mdl_deleted;	/* Remote device requested deleting a MDL */
+	mcap_mdl_event_cb		mdl_aborted;	/* Remote device aborted the mdl creation */
+	mcap_remote_mdl_conn_req_cb	mdl_conn_req;	/* Remote device requested creating a MDL */
+	mcap_remote_mdl_reconn_req_cb	mdl_reconn_req;	/* Remote device requested reconnecting a MDL */
+	gpointer			user_data;	/* User data */
+};
+
+struct mcap_instance {
+	bdaddr_t		src;			/* Source address */
+	GIOChannel		*ccio;			/* Control Channel IO */
+	GIOChannel		*dcio;			/* Data Channel IO */
+	GSList			*mcls;			/* MCAP instance list */
+	GSList			*cached;		/* List with all cached MCLs (MAX_CACHED macro) */
+	BtIOSecLevel		sec;			/* Security level */
+	mcap_mcl_event_cb	mcl_connected_cb;	/* New MCL connected */
+	mcap_mcl_event_cb	mcl_reconnected_cb;	/* Old MCL has been reconnected */
+	mcap_mcl_event_cb	mcl_disconnected_cb;	/* MCL disconnected */
+	mcap_mcl_event_cb	mcl_uncached_cb;	/* MCL has been removed from MCAP cache */
+	mcap_info_ind_event_cb	mcl_sync_infoind_cb;	/* (CSP Master) Received info indication */
+	gpointer		user_data;		/* Data to be provided in callbacks */
+	gint			ref;			/* Reference counter */
+
+	gboolean		csp_enabled;		/* CSP: functionality enabled */
+};
+
+struct mcap_csp;
+struct mcap_mdl_op_cb;
+
+struct mcap_mcl {
+	struct mcap_instance	*mi;		/* MCAP instance where this MCL belongs */
+	bdaddr_t		addr;		/* Device address */
+	GIOChannel		*cc;		/* MCAP Control Channel IO */
+	guint			wid;		/* MCL Watcher id */
+	GSList			*mdls;		/* List of Data Channels shorted by mdlid */
+	MCLState		state;		/* Current MCL State */
+	MCLRole			role;		/* Initiator or acceptor of this MCL */
+	MCAPCtrl		req;		/* Request control flag */
+	struct mcap_mdl_op_cb	*priv_data;	/* Temporal data to manage responses */
+	struct mcap_mdl_cb	*cb;		/* MDL callbacks */
+	guint			tid;		/* Timer id for waiting for a response */
+	uint8_t			*lcmd;		/* Last command sent */
+	gint			ref;		/* References counter */
+	uint8_t			ctrl;		/* MCL control flag */
+	uint16_t		next_mdl;	/* id used to create next MDL */
+	struct mcap_csp		*csp;		/* CSP control structure */
+};
+
+#define	MCAP_CTRL_CACHED	0x01	/* MCL is cached */
+#define	MCAP_CTRL_STD_OP	0x02	/* Support for standard op codes */
+#define	MCAP_CTRL_SYNC_OP	0x04	/* Support for synchronization commands */
+#define	MCAP_CTRL_CONN		0x08	/* MCL is in connecting process */
+#define	MCAP_CTRL_FREE		0x10	/* MCL is marked as releasable */
+#define	MCAP_CTRL_NOCACHE	0x20	/* MCL is marked as not cacheable */
+
+struct mcap_mdl {
+	struct mcap_mcl		*mcl;		/* MCL where this MDL belongs */
+	GIOChannel		*dc;		/* MCAP Data Channel IO */
+	guint			wid;		/* MDL Watcher id */
+	uint16_t		mdlid;		/* MDL id */
+	uint8_t			mdep_id;	/* MCAP Data End Point */
+	MDLState		state;		/* MDL state */
+	gint			ref;		/* References counter */
+};
+
+struct sync_info_ind_data {
+	uint32_t	btclock;
+	uint64_t	timestamp;
+	uint16_t	accuracy;
+};
+
+int mcap_send_data(int sock, const void *buf, uint32_t size);
+
+void proc_sync_cmd(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len);
+void mcap_sync_init(struct mcap_mcl *mcl);
+void mcap_sync_stop(struct mcap_mcl *mcl);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MCAP_INTERNAL_H */
diff --git a/health/mcap_lib.h b/health/mcap_lib.h
new file mode 100644
index 0000000..9c1e508
--- /dev/null
+++ b/health/mcap_lib.h
@@ -0,0 +1,228 @@
+/*
+ *
+ *  MCAP for 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
+ *
+ */
+
+#ifndef __MCAP_LIB_H
+#define __MCAP_LIB_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum {
+/* MCAP Error Response Codes */
+	MCAP_ERROR_INVALID_OP_CODE = 1,
+	MCAP_ERROR_INVALID_PARAM_VALUE,
+	MCAP_ERROR_INVALID_MDEP,
+	MCAP_ERROR_MDEP_BUSY,
+	MCAP_ERROR_INVALID_MDL,
+	MCAP_ERROR_MDL_BUSY,
+	MCAP_ERROR_INVALID_OPERATION,
+	MCAP_ERROR_RESOURCE_UNAVAILABLE,
+	MCAP_ERROR_UNSPECIFIED_ERROR,
+	MCAP_ERROR_REQUEST_NOT_SUPPORTED,
+	MCAP_ERROR_CONFIGURATION_REJECTED,
+/* MCAP Internal Errors */
+	MCAP_ERROR_INVALID_ARGS,
+	MCAP_ERROR_ALREADY_EXISTS,
+	MCAP_ERROR_REQ_IGNORED,
+	MCAP_ERROR_MCL_CLOSED,
+	MCAP_ERROR_FAILED
+} McapError;
+
+typedef enum {
+	MCAP_MDL_CB_INVALID,
+	MCAP_MDL_CB_CONNECTED,		/* mcap_mdl_event_cb */
+	MCAP_MDL_CB_CLOSED,		/* mcap_mdl_event_cb */
+	MCAP_MDL_CB_DELETED,		/* mcap_mdl_event_cb */
+	MCAP_MDL_CB_ABORTED,		/* mcap_mdl_event_cb */
+	MCAP_MDL_CB_REMOTE_CONN_REQ,	/* mcap_remote_mdl_conn_req_cb */
+	MCAP_MDL_CB_REMOTE_RECONN_REQ	/* mcap_remote_mdl_reconn_req_cb */
+} McapMclCb;
+
+struct mcap_instance;
+struct mcap_mcl;
+struct mcap_mdl;
+struct sync_info_ind_data;
+
+/************ Callbacks ************/
+
+/* MDL callbacks */
+
+typedef void (* mcap_mdl_event_cb) (struct mcap_mdl *mdl, gpointer data);
+typedef void (* mcap_mdl_operation_conf_cb) (struct mcap_mdl *mdl, uint8_t conf,
+						GError *err, gpointer data);
+typedef void (* mcap_mdl_operation_cb) (struct mcap_mdl *mdl, GError *err,
+						gpointer data);
+typedef void (* mcap_mdl_notify_cb) (GError *err, gpointer data);
+
+/* Next function should return an MCAP appropriate response code */
+typedef uint8_t (* mcap_remote_mdl_conn_req_cb) (struct mcap_mcl *mcl,
+						uint8_t mdepid, uint16_t mdlid,
+						uint8_t *conf, gpointer data);
+typedef uint8_t (* mcap_remote_mdl_reconn_req_cb) (struct mcap_mdl *mdl,
+						gpointer data);
+
+/* MCL callbacks */
+
+typedef void (* mcap_mcl_event_cb) (struct mcap_mcl *mcl, gpointer data);
+typedef void (* mcap_mcl_connect_cb) (struct mcap_mcl *mcl, GError *err,
+								gpointer data);
+
+/* CSP callbacks */
+
+typedef void (* mcap_info_ind_event_cb) (struct mcap_mcl *mcl,
+					struct sync_info_ind_data *data);
+
+typedef void (* mcap_sync_cap_cb) (struct mcap_mcl *mcl,
+					uint8_t mcap_err,
+					uint8_t btclockres,
+					uint16_t synclead,
+					uint16_t tmstampres,
+					uint16_t tmstampacc,
+					GError *err,
+					gpointer data);
+
+typedef void (* mcap_sync_set_cb) (struct mcap_mcl *mcl,
+					uint8_t mcap_err,
+					uint32_t btclock,
+					uint64_t timestamp,
+					uint16_t accuracy,
+					GError *err,
+					gpointer data);
+
+/************ Operations ************/
+
+/* MDL operations */
+
+gboolean mcap_create_mdl(struct mcap_mcl *mcl,
+				uint8_t mdepid,
+				uint8_t conf,
+				mcap_mdl_operation_conf_cb connect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+gboolean mcap_reconnect_mdl(struct mcap_mdl *mdl,
+				mcap_mdl_operation_cb reconnect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+gboolean mcap_delete_all_mdls(struct mcap_mcl *mcl,
+				mcap_mdl_notify_cb delete_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+gboolean mcap_delete_mdl(struct mcap_mdl *mdl,
+				mcap_mdl_notify_cb delete_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+gboolean mcap_connect_mdl(struct mcap_mdl *mdl,
+				uint8_t mode,
+				uint16_t dcpsm,
+				mcap_mdl_operation_cb connect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+gboolean mcap_mdl_abort(struct mcap_mdl *mdl,
+				mcap_mdl_notify_cb abort_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+
+int mcap_mdl_get_fd(struct mcap_mdl *mdl);
+uint16_t mcap_mdl_get_mdlid(struct mcap_mdl *mdl);
+
+struct mcap_mdl *mcap_mdl_ref(struct mcap_mdl *mdl);
+void mcap_mdl_unref(struct mcap_mdl *mdl);
+
+/* MCL operations */
+
+gboolean mcap_create_mcl(struct mcap_instance *mi,
+				const bdaddr_t *addr,
+				uint16_t ccpsm,
+				mcap_mcl_connect_cb connect_cb,
+				gpointer user_data,
+				GDestroyNotify destroy,
+				GError **err);
+void mcap_close_mcl(struct mcap_mcl *mcl, gboolean cache);
+gboolean mcap_mcl_set_cb(struct mcap_mcl *mcl, gpointer user_data,
+					GError **gerr, McapMclCb cb1, ...);
+void mcap_mcl_get_addr(struct mcap_mcl *mcl, bdaddr_t *addr);
+
+struct mcap_mcl *mcap_mcl_ref(struct mcap_mcl *mcl);
+void mcap_mcl_unref(struct mcap_mcl *mcl);
+
+/* CSP operations */
+
+void mcap_enable_csp(struct mcap_instance *mi);
+void mcap_disable_csp(struct mcap_instance *mi);
+
+uint64_t mcap_get_timestamp(struct mcap_mcl *mcl,
+				struct timespec *given_time);
+uint32_t mcap_get_btclock(struct mcap_mcl *mcl);
+
+void mcap_sync_cap_req(struct mcap_mcl *mcl,
+			uint16_t reqacc,
+			mcap_sync_cap_cb cb,
+			gpointer user_data,
+			GError **err);
+
+void mcap_sync_set_req(struct mcap_mcl *mcl,
+			uint8_t update,
+			uint32_t btclock,
+			uint64_t timestamp,
+			mcap_sync_set_cb cb,
+			gpointer user_data,
+			GError **err);
+
+/* MCAP main operations */
+
+struct mcap_instance *mcap_create_instance(bdaddr_t *src,
+					BtIOSecLevel sec, uint16_t ccpsm,
+					uint16_t dcpsm,
+					mcap_mcl_event_cb mcl_connected,
+					mcap_mcl_event_cb mcl_reconnected,
+					mcap_mcl_event_cb mcl_disconnected,
+					mcap_mcl_event_cb mcl_uncached,
+					mcap_info_ind_event_cb mcl_sync_info_ind,
+					gpointer user_data,
+					GError **gerr);
+void mcap_release_instance(struct mcap_instance *mi);
+
+struct mcap_instance *mcap_instance_ref(struct mcap_instance *mi);
+void mcap_instance_unref(struct mcap_instance *mi);
+
+uint16_t mcap_get_ctrl_psm(struct mcap_instance *mi, GError **err);
+uint16_t mcap_get_data_psm(struct mcap_instance *mi, GError **err);
+
+gboolean mcap_set_data_chan_mode(struct mcap_instance *mi, uint8_t mode,
+								GError **err);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MCAP_LIB_H */
diff --git a/health/mcap_sync.c b/health/mcap_sync.c
new file mode 100644
index 0000000..f4b005a
--- /dev/null
+++ b/health/mcap_sync.c
@@ -0,0 +1,1015 @@
+/*
+ *
+ *  MCAP for BlueZ - Bluetooth protocol stack for Linux
+ *
+ *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
+ *  Copyright (C) 2010 Signove
+ *
+ *  Authors:
+ *  Santiago Carot-Nemesio <sancane at gmail.com>
+ *  Jose Antonio Santos-Cadenas <santoscadenas at gmail.com>
+ *  Elvis Pfützenreuter <epx at signove.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 "btio.h"
+#include <stdint.h>
+#include <netinet/in.h>
+#include <time.h>
+#include <stdlib.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/l2cap.h>
+#include "../src/adapter.h"
+#include "../src/manager.h"
+#include <sys/ioctl.h>
+
+#include "config.h"
+#include "log.h"
+
+#include <bluetooth/bluetooth.h>
+#include "mcap.h"
+#include "mcap_lib.h"
+#include "mcap_internal.h"
+
+#define MCAP_BTCLOCK_HALF (MCAP_BTCLOCK_FIELD / 2)
+#define CLK CLOCK_MONOTONIC
+
+#define MCAP_CSP_ERROR g_quark_from_static_string("mcap-csp-error-quark")
+#define MAX_RETRIES	10
+#define SAMPLE_COUNT	20
+
+struct mcap_csp {
+	uint64_t	base_tmstamp;	/* CSP base timestamp */
+	struct timespec	base_time;	/* CSP base time when timestamp set */
+	guint		local_caps;	/* CSP-Master: have got remote caps */
+	guint		remote_caps;	/* CSP-Slave: remote master got caps */
+	guint		rem_req_acc;	/* CSP-Slave: accuracy required by master */
+	guint		ind_expected;	/* CSP-Master: indication expected */
+	MCAPCtrl	csp_req;	/* CSP-Master: Request control flag */
+	guint		ind_timer;	/* CSP-Slave: indication timer */
+	guint		set_timer;	/* CSP-Slave: delayed set timer */
+	void		*set_data;	/* CSP-Slave: delayed set data */
+	void		*csp_priv_data;	/* CSP-Master: In-flight request data */
+};
+
+struct mcap_sync_cap_cbdata {
+	mcap_sync_cap_cb	cb;
+	gpointer		user_data;
+};
+
+struct mcap_sync_set_cbdata {
+	mcap_sync_set_cb	cb;
+	gpointer		user_data;
+};
+
+struct csp_caps {
+	int ts_acc;		/* timestamp accuracy */
+	int ts_res;		/* timestamp resolution */
+	int latency;		/* Read BT clock latency */
+	int preempt_thresh;	/* Preemption threshold for latency */
+	int syncleadtime_ms;	/* SyncLeadTime in ms */
+};
+
+struct sync_set_data {
+	uint8_t update;
+	uint32_t sched_btclock;
+	uint64_t timestamp;
+	int ind_freq;
+	gboolean role;
+};
+
+#define hton64(x)     ntoh64(x)
+
+static gboolean csp_caps_initialized = FALSE;
+struct csp_caps _caps;
+
+static int send_sync_cmd(struct mcap_mcl *mcl, const void *buf, uint32_t size)
+{
+	int sock;
+
+	if (mcl->cc == NULL)
+		return -1;
+
+	sock = g_io_channel_unix_get_fd(mcl->cc);
+	return mcap_send_data(sock, buf, size);
+}
+
+static int send_unsupported_cap_req(struct mcap_mcl *mcl)
+{
+	mcap_md_sync_cap_rsp *cmd;
+	int sent;
+
+	cmd = g_new0(mcap_md_sync_cap_rsp, 1);
+	cmd->op = MCAP_MD_SYNC_CAP_RSP;
+	cmd->rc = MCAP_REQUEST_NOT_SUPPORTED;
+
+	sent = send_sync_cmd(mcl, cmd, sizeof(*cmd));
+	g_free(cmd);
+
+	return sent;
+}
+
+static int send_unsupported_set_req(struct mcap_mcl *mcl)
+{
+	mcap_md_sync_set_rsp *cmd;
+	int sent;
+
+	cmd = g_new0(mcap_md_sync_set_rsp, 1);
+	cmd->op = MCAP_MD_SYNC_SET_RSP;
+	cmd->rc = MCAP_REQUEST_NOT_SUPPORTED;
+
+	sent = send_sync_cmd(mcl, cmd, sizeof(*cmd));
+	g_free(cmd);
+
+	return sent;
+}
+
+static void reset_tmstamp(struct mcap_csp *csp, struct timespec *base_time,
+				uint64_t new_tmstamp)
+{
+	csp->base_tmstamp = new_tmstamp;
+	if (base_time)
+		csp->base_time = *base_time;
+	else
+		clock_gettime(CLK, &csp->base_time);
+}
+
+void mcap_sync_init(struct mcap_mcl *mcl)
+{
+	if (!mcl->mi->csp_enabled) {
+		mcl->csp = NULL;
+		return;
+	}
+
+	mcl->csp = g_new0(struct mcap_csp, 1);
+
+	mcl->csp->rem_req_acc = 10000; /* safe divisor */
+	mcl->csp->set_data = NULL;
+	mcl->csp->csp_priv_data = NULL;
+
+	reset_tmstamp(mcl->csp, NULL, 0);
+}
+
+void mcap_sync_stop(struct mcap_mcl *mcl)
+{
+	if (!mcl->csp)
+		return;
+
+	if (mcl->csp->ind_timer)
+		g_source_remove(mcl->csp->ind_timer);
+
+	if (mcl->csp->set_timer)
+		g_source_remove(mcl->csp->set_timer);
+
+	if (mcl->csp->set_data)
+		g_free(mcl->csp->set_data);
+
+	if (mcl->csp->csp_priv_data)
+		g_free(mcl->csp->csp_priv_data);
+
+	mcl->csp->ind_timer = 0;
+	mcl->csp->set_timer = 0;
+	mcl->csp->set_data = NULL;
+	mcl->csp->csp_priv_data = NULL;
+
+	g_free(mcl->csp);
+	mcl->csp = NULL;
+}
+
+static uint64_t time_us(struct timespec *tv)
+{
+	return tv->tv_sec * 1000000 + tv->tv_nsec / 1000;
+}
+
+static int64_t bt2us(int bt)
+{
+	return bt * 312.5;
+}
+
+static int bt2ms(int bt)
+{
+	return bt * 312.5 / 1000;
+}
+
+static int btoffset(uint32_t btclk1, uint32_t btclk2)
+{
+	int offset = btclk2 - btclk1;
+
+	if (offset <= -MCAP_BTCLOCK_HALF)
+		offset += MCAP_BTCLOCK_FIELD;
+	else if (offset > MCAP_BTCLOCK_HALF)
+		offset -= MCAP_BTCLOCK_FIELD;
+
+	return offset;
+}
+
+static int btdiff(uint32_t btclk1, uint32_t btclk2)
+{
+	return btoffset(btclk1, btclk2);
+}
+
+static gboolean valid_btclock(uint32_t btclk)
+{
+	return btclk <= MCAP_BTCLOCK_MAX;
+}
+
+/* This call may fail; either deal with retry or use read_btclock_retry */
+static gboolean read_btclock(struct mcap_mcl *mcl, uint32_t *btclock,
+							uint16_t *btaccuracy)
+{
+	int which = 1;
+	struct btd_adapter *adapter;
+
+	adapter = manager_find_adapter(&mcl->mi->src);
+
+	if (!adapter)
+		return FALSE;
+
+	if (btd_adapter_read_clock(adapter, &mcl->addr, which, 1000,
+						btclock, btaccuracy) < 0)
+		return FALSE;
+
+	return TRUE;
+}
+
+static gboolean read_btclock_retry(struct mcap_mcl *mcl, uint32_t *btclock,
+							uint16_t *btaccuracy)
+{
+	int retries = 5;
+
+	while (--retries >= 0) {
+		if (read_btclock(mcl, btclock, btaccuracy))
+			return TRUE;
+		DBG("CSP: retrying to read bt clock...");
+	}
+
+	return FALSE;
+}
+
+static gboolean get_btrole(struct mcap_mcl *mcl)
+{
+	int sock, flags;
+	socklen_t len;
+
+	if (mcl->cc == NULL)
+		return -1;
+
+	sock = g_io_channel_unix_get_fd(mcl->cc);
+	len = sizeof(flags);
+
+	if (getsockopt(sock, SOL_L2CAP, L2CAP_LM, &flags, &len))
+		DBG("CSP: could not read role");
+
+	return flags & L2CAP_LM_MASTER;
+}
+
+uint64_t mcap_get_timestamp(struct mcap_mcl *mcl,
+				struct timespec *given_time)
+{
+	struct timespec now;
+	uint64_t tmstamp;
+
+	if (!mcl->csp)
+		return MCAP_TMSTAMP_DONTSET;
+
+	if (given_time)
+		now = *given_time;
+	else
+		clock_gettime(CLK, &now);
+
+	tmstamp = time_us(&now) - time_us(&mcl->csp->base_time)
+		+ mcl->csp->base_tmstamp;
+
+	return tmstamp;
+}
+
+uint32_t mcap_get_btclock(struct mcap_mcl *mcl)
+{
+	uint32_t btclock;
+	uint16_t accuracy;
+
+	if (!mcl->csp)
+		return MCAP_BTCLOCK_IMMEDIATE;
+
+	if (!read_btclock_retry(mcl, &btclock, &accuracy))
+		btclock = 0xffffffff;
+
+	return btclock;
+}
+
+static gboolean initialize_caps(struct mcap_mcl *mcl)
+{
+	struct timespec t1, t2;
+	int latencies[SAMPLE_COUNT];
+	int latency, avg, dev;
+	uint32_t btclock;
+	uint16_t btaccuracy;
+	int i;
+	int retries;
+
+	clock_getres(CLK, &t1);
+
+	_caps.ts_res = time_us(&t1);
+	if (_caps.ts_res < 1)
+		_caps.ts_res = 1;
+
+	_caps.ts_acc = 20; /* ppm, estimated */
+
+	/* A little exercise before measuing latency */
+	clock_gettime(CLK, &t1);
+	read_btclock_retry(mcl, &btclock, &btaccuracy);
+
+	/* Read clock a number of times and measure latency */
+	avg = 0;
+	i = 0;
+	retries = MAX_RETRIES;
+	while (i < SAMPLE_COUNT && retries > 0) {
+		clock_gettime(CLK, &t1);
+		if (!read_btclock(mcl, &btclock, &btaccuracy)) {
+			retries--;
+			continue;
+		}
+		clock_gettime(CLK, &t2);
+
+		latency = time_us(&t2) - time_us(&t1);
+		latencies[i] = latency;
+		avg += latency;
+		i++;
+	}
+
+	if (retries <= 0)
+		return FALSE;
+
+	/* Calculate average and deviation */
+	avg /= SAMPLE_COUNT;
+	dev = 0;
+	for (i = 0; i < SAMPLE_COUNT; ++i)
+		dev += abs(latencies[i] - avg);
+	dev /= SAMPLE_COUNT;
+
+	/* Calculate corrected average, without 'freak' latencies */
+	latency = 0;
+	for (i = 0; i < SAMPLE_COUNT; ++i) {
+		if (latencies[i] > (avg + dev * 6))
+			latency += avg;
+		else
+			latency += latencies[i];
+	}
+	latency /= SAMPLE_COUNT;
+
+	_caps.latency = latency;
+	_caps.preempt_thresh = latency * 4;
+	_caps.syncleadtime_ms = latency * 50 / 1000;
+
+	csp_caps_initialized = TRUE;
+	return TRUE;
+}
+
+static struct csp_caps *caps(struct mcap_mcl *mcl)
+{
+	if (!csp_caps_initialized)
+		if (!initialize_caps(mcl)) {
+			/* Temporary failure in reading BT clock */
+			return NULL;
+		}
+
+	return &_caps;
+}
+
+static int send_sync_cap_rsp(struct mcap_mcl *mcl, uint8_t rspcode,
+			uint8_t btclockres, uint16_t synclead,
+			uint16_t tmstampres, uint16_t tmstampacc)
+{
+	mcap_md_sync_cap_rsp *rsp;
+	int sent;
+
+	rsp = g_new0(mcap_md_sync_cap_rsp, 1);
+
+	rsp->op = MCAP_MD_SYNC_CAP_RSP;
+	rsp->rc = rspcode;
+
+	rsp->btclock = btclockres;
+	rsp->sltime = htons(synclead);
+	rsp->timestnr = htons(tmstampres);
+	rsp->timestna = htons(tmstampacc);
+
+	sent = send_sync_cmd(mcl, rsp, sizeof(*rsp));
+	g_free(rsp);
+
+	return sent;
+}
+
+static void proc_sync_cap_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	mcap_md_sync_cap_req *req;
+	uint16_t required_accuracy;
+	uint16_t our_accuracy;
+	uint32_t btclock;
+	uint16_t btres;
+
+	if (len != sizeof(mcap_md_sync_cap_req)) {
+		send_sync_cap_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
+					0, 0, 0, 0);
+		return;
+	}
+
+	if (!caps(mcl)) {
+		send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE,
+					0, 0, 0, 0);
+		return;
+	}
+
+	req = (mcap_md_sync_cap_req *) cmd;
+	required_accuracy = ntohs(req->timest);
+	our_accuracy = caps(mcl)->ts_acc;
+
+	if (required_accuracy < our_accuracy || required_accuracy < 1) {
+		send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE,
+					0, 0, 0, 0);
+		return;
+	}
+
+	if (!read_btclock_retry(mcl, &btclock, &btres)) {
+		send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE,
+					0, 0, 0, 0);
+		return;
+	}
+
+	mcl->csp->remote_caps = 1;
+	mcl->csp->rem_req_acc = required_accuracy;
+
+	send_sync_cap_rsp(mcl, MCAP_SUCCESS, btres,
+				caps(mcl)->syncleadtime_ms,
+				caps(mcl)->ts_res, our_accuracy);
+}
+
+static int send_sync_set_rsp(struct mcap_mcl *mcl, uint8_t rspcode,
+			uint32_t btclock, uint64_t timestamp,
+			uint16_t tmstampres)
+{
+	mcap_md_sync_set_rsp *rsp;
+	int sent;
+
+	rsp = g_new0(mcap_md_sync_set_rsp, 1);
+
+	rsp->op = MCAP_MD_SYNC_SET_RSP;
+	rsp->rc = rspcode;
+	rsp->btclock = htonl(btclock);
+	rsp->timestst = hton64(timestamp);
+	rsp->timestsa = htons(tmstampres);
+
+	sent = send_sync_cmd(mcl, rsp, sizeof(*rsp));
+	g_free(rsp);
+
+	return sent;
+}
+
+static gboolean get_all_clocks(struct mcap_mcl *mcl, uint32_t *btclock,
+				struct timespec *base_time,
+				uint64_t *timestamp)
+{
+	int latency;
+	int retry = 5;
+	uint16_t btres;
+	struct timespec t0;
+
+	if (!caps(mcl))
+		return FALSE;
+
+	latency = caps(mcl)->preempt_thresh + 1;
+
+	while (latency > caps(mcl)->preempt_thresh && --retry >= 0) {
+
+		clock_gettime(CLK, &t0);
+
+		if (!read_btclock(mcl, btclock, &btres))
+			continue;
+
+		clock_gettime(CLK, base_time);
+
+		/* Tries to detect preemption between clock_gettime
+		 * and read_btclock by measuring transaction time
+		 */
+		latency = time_us(base_time) - time_us(&t0);
+	}
+
+	*timestamp = mcap_get_timestamp(mcl, base_time);
+
+	return TRUE;
+}
+
+static gboolean sync_send_indication(gpointer user_data)
+{
+	struct mcap_mcl *mcl;
+	mcap_md_sync_info_ind *cmd;
+	uint32_t btclock;
+	uint64_t tmstamp;
+	struct timespec base_time;
+	int sent;
+
+	if (!user_data)
+		return FALSE;
+
+	mcl = user_data;
+
+	if (!caps(mcl))
+		return FALSE;
+
+	if (!get_all_clocks(mcl, &btclock, &base_time, &tmstamp))
+		return FALSE;
+
+	cmd = g_new0(mcap_md_sync_info_ind, 1);
+
+	cmd->op = MCAP_MD_SYNC_INFO_IND;
+	cmd->btclock = htonl(btclock);
+	cmd->timestst = hton64(tmstamp);
+	cmd->timestsa = htons(caps(mcl)->latency);
+
+	sent = send_sync_cmd(mcl, cmd, sizeof(*cmd));
+	g_free(cmd);
+
+	return !sent;
+}
+
+static gboolean proc_sync_set_req_phase2(gpointer user_data)
+{
+	struct mcap_mcl *mcl;
+	struct sync_set_data *data;
+	uint8_t update;
+	uint32_t sched_btclock;
+	uint64_t new_tmstamp;
+	int ind_freq;
+	int role;
+	uint32_t btclock;
+	uint64_t tmstamp;
+	struct timespec base_time;
+	uint16_t tmstampacc;
+	gboolean reset;
+	int delay;
+
+	if (!user_data)
+		return FALSE;
+
+	mcl = user_data;
+
+	if (!mcl->csp->set_data)
+		return FALSE;
+
+	data = mcl->csp->set_data;
+	update = data->update;
+	sched_btclock = data->sched_btclock;
+	new_tmstamp = data->timestamp;
+	ind_freq = data->ind_freq;
+	role = data->role;
+
+	if (!caps(mcl)) {
+		send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
+		return FALSE;
+	}
+
+	if (!get_all_clocks(mcl, &btclock, &base_time, &tmstamp)) {
+		send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
+		return FALSE;
+	}
+
+	if (get_btrole(mcl) != role) {
+		send_sync_set_rsp(mcl, MCAP_INVALID_OPERATION, 0, 0, 0);
+		return FALSE;
+	}
+
+	reset = (new_tmstamp != MCAP_TMSTAMP_DONTSET);
+
+	if (reset) {
+		if (sched_btclock != MCAP_BTCLOCK_IMMEDIATE) {
+			delay = bt2us(btdiff(sched_btclock, btclock));
+			if (delay >= 0 || ((new_tmstamp - delay) > 0)) {
+				new_tmstamp += delay;
+				DBG("CSP: reset w/ delay %dus, compensated",
+									delay);
+			} else
+				DBG("CSP: reset w/ delay %dus, uncompensated",
+									delay);
+		}
+
+		reset_tmstamp(mcl->csp, &base_time, new_tmstamp);
+		tmstamp = new_tmstamp;
+	}
+
+	tmstampacc = caps(mcl)->latency + caps(mcl)->ts_acc;
+
+	if (mcl->csp->ind_timer) {
+		g_source_remove(mcl->csp->ind_timer);
+		mcl->csp->ind_timer = 0;
+	}
+
+	if (update) {
+		int when = ind_freq + caps(mcl)->syncleadtime_ms;
+		mcl->csp->ind_timer = g_timeout_add(when,
+						sync_send_indication,
+						mcl);
+	}
+
+	send_sync_set_rsp(mcl, MCAP_SUCCESS, btclock, tmstamp, tmstampacc);
+
+	/* First indication after set is immediate */
+	if (update)
+		sync_send_indication(mcl);
+
+	return FALSE;
+}
+
+static void proc_sync_set_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	mcap_md_sync_set_req *req;
+	uint32_t sched_btclock, cur_btclock;
+	uint16_t btres;
+	uint8_t update;
+	uint64_t timestamp;
+	struct sync_set_data *set_data;
+	int phase2_delay, ind_freq, when;
+
+	if (len != sizeof(mcap_md_sync_set_req)) {
+		send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
+		return;
+	}
+
+	req = (mcap_md_sync_set_req *) cmd;
+	sched_btclock = ntohl(req->btclock);
+	update = req->timestui;
+	timestamp = ntoh64(req->timestst);
+
+	if (sched_btclock != MCAP_BTCLOCK_IMMEDIATE &&
+			!valid_btclock(sched_btclock)) {
+		send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
+		return;
+	}
+
+	if (update > 1) {
+		send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
+		return;
+	}
+
+	if (!mcl->csp->remote_caps) {
+		/* Remote side did not ask our capabilities yet */
+		send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
+		return;
+	}
+
+	if (!caps(mcl)) {
+		send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
+		return;
+	}
+
+	if (!read_btclock_retry(mcl, &cur_btclock, &btres)) {
+		send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
+		return;
+	}
+
+	if (sched_btclock == MCAP_BTCLOCK_IMMEDIATE)
+		phase2_delay = 0;
+	else {
+		phase2_delay = btdiff(cur_btclock, sched_btclock);
+
+		if (phase2_delay < 0) {
+			/* can not reset in the past tense */
+			send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
+						0, 0, 0);
+			return;
+		}
+
+		/* Convert to miliseconds */
+		phase2_delay = bt2ms(phase2_delay);
+
+		if (phase2_delay > 61*1000) {
+			/* More than 60 seconds in the future */
+			send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
+						0, 0, 0);
+			return;
+		} else if (phase2_delay < caps(mcl)->latency / 1000) {
+			/* Too fast for us to do in time */
+			send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
+						0, 0, 0);
+			return;
+		}
+	}
+
+	if (update) {
+		/* Indication frequency: required accuracy divided by ours */
+		/* Converted to milisseconds */
+		ind_freq = (1000 * mcl->csp->rem_req_acc) / caps(mcl)->ts_acc;
+
+		if (ind_freq < MAX(caps(mcl)->latency * 2 / 1000, 100)) {
+			/* Too frequent, we can't handle */
+			send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
+						0, 0, 0);
+			return;
+		}
+
+		DBG("CSP: indication every %dms", ind_freq);
+	} else
+		ind_freq = 0;
+
+	if (mcl->csp->ind_timer) {
+		/* Old indications are no longer sent */
+		g_source_remove(mcl->csp->ind_timer);
+		mcl->csp->ind_timer = 0;
+	}
+
+	if (!mcl->csp->set_data)
+		mcl->csp->set_data = g_new0(struct sync_set_data, 1);
+
+	set_data = (struct sync_set_data *) mcl->csp->set_data;
+
+	set_data->update = update;
+	set_data->sched_btclock = sched_btclock;
+	set_data->timestamp = timestamp;
+	set_data->ind_freq = ind_freq;
+	set_data->role = get_btrole(mcl);
+
+	/* TODO is there some way to schedule a call based directly on
+	 * a BT clock value, instead of this estimation that uses
+	 * the SO clock? */
+
+	if (phase2_delay > 0) {
+		when = phase2_delay + caps(mcl)->syncleadtime_ms;
+		mcl->csp->set_timer = g_timeout_add(when,
+						proc_sync_set_req_phase2,
+						mcl);
+	} else
+		proc_sync_set_req_phase2(mcl);
+
+	/* First indication is immediate */
+	if (update)
+		sync_send_indication(mcl);
+}
+
+static void proc_sync_cap_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	mcap_md_sync_cap_rsp *rsp;
+	uint8_t mcap_err;
+	uint8_t btclockres;
+	uint16_t synclead;
+	uint16_t tmstampres;
+	uint16_t tmstampacc;
+	struct mcap_sync_cap_cbdata *cbdata;
+	mcap_sync_cap_cb cb;
+	gpointer user_data;
+
+	if (mcl->csp->csp_req != MCAP_MD_SYNC_CAP_REQ) {
+		DBG("CSP: got unexpected cap respose");
+		return;
+	}
+
+	if (!mcl->csp->csp_priv_data) {
+		DBG("CSP: no priv data for cap respose");
+		return;
+	}
+
+	cbdata = mcl->csp->csp_priv_data;
+	cb = cbdata->cb;
+	user_data = cbdata->user_data;
+	g_free(cbdata);
+
+	mcl->csp->csp_priv_data = NULL;
+	mcl->csp->csp_req = 0;
+
+	if (len != sizeof(mcap_md_sync_cap_rsp)) {
+		DBG("CSP: got corrupted cap respose");
+		return;
+	}
+
+	rsp = (mcap_md_sync_cap_rsp *) cmd;
+	mcap_err = rsp->rc;
+	btclockres = rsp->btclock;
+	synclead = ntohs(rsp->sltime);
+	tmstampres = ntohs(rsp->timestnr);
+	tmstampacc = ntohs(rsp->timestna);
+
+	if (!mcap_err)
+		mcl->csp->local_caps = TRUE;
+
+	cb(mcl, mcap_err, btclockres, synclead, tmstampres, tmstampacc, NULL,
+								user_data);
+}
+
+static void proc_sync_set_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	mcap_md_sync_set_rsp *rsp;
+	uint8_t mcap_err;
+	uint32_t btclock;
+	uint64_t timestamp;
+	uint16_t accuracy;
+	struct mcap_sync_set_cbdata *cbdata;
+	mcap_sync_set_cb cb;
+	gpointer user_data;
+
+	if (mcl->csp->csp_req != MCAP_MD_SYNC_SET_REQ) {
+		DBG("CSP: got unexpected set respose");
+		return;
+	}
+
+	if (!mcl->csp->csp_priv_data) {
+		DBG("CSP: no priv data for set respose");
+		return;
+	}
+
+	cbdata = mcl->csp->csp_priv_data;
+	cb = cbdata->cb;
+	user_data = cbdata->user_data;
+	g_free(cbdata);
+
+	mcl->csp->csp_priv_data = NULL;
+	mcl->csp->csp_req = 0;
+
+	if (len != sizeof(mcap_md_sync_set_rsp)) {
+		DBG("CSP: got corrupted set respose");
+		return;
+	}
+
+	rsp = (mcap_md_sync_set_rsp *) cmd;
+	mcap_err = rsp->rc;
+	btclock = ntohl(rsp->btclock);
+	timestamp = ntoh64(rsp->timestst);
+	accuracy = ntohs(rsp->timestsa);
+
+	if (!mcap_err && !valid_btclock(btclock))
+		mcap_err = MCAP_ERROR_INVALID_ARGS;
+
+	cb(mcl, mcap_err, btclock, timestamp, accuracy, NULL, user_data);
+}
+
+static void proc_sync_info_ind(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	mcap_md_sync_info_ind *req;
+	struct sync_info_ind_data data;
+	uint32_t btclock;
+
+	if (!mcl->csp->ind_expected) {
+		DBG("CSP: received unexpected info indication");
+		return;
+	}
+
+	if (len != sizeof(mcap_md_sync_info_ind))
+		return;
+
+	req = (mcap_md_sync_info_ind *) cmd;
+
+	btclock = ntohl(req->btclock);
+
+	if (!valid_btclock(btclock))
+		return;
+
+	data.btclock = btclock;
+	data.timestamp = ntoh64(req->timestst);
+	data.accuracy = ntohs(req->timestsa);
+
+	if (mcl->mi->mcl_sync_infoind_cb)
+		mcl->mi->mcl_sync_infoind_cb(mcl, &data);
+}
+
+void proc_sync_cmd(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
+{
+	if (!mcl->mi->csp_enabled || !mcl->csp) {
+		switch (cmd[0]) {
+		case MCAP_MD_SYNC_CAP_REQ:
+			send_unsupported_cap_req(mcl);
+			break;
+		case MCAP_MD_SYNC_SET_REQ:
+			send_unsupported_set_req(mcl);
+			break;
+		}
+		return;
+	}
+
+	switch (cmd[0]) {
+	case MCAP_MD_SYNC_CAP_REQ:
+		proc_sync_cap_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_SYNC_CAP_RSP:
+		proc_sync_cap_rsp(mcl, cmd, len);
+		break;
+	case MCAP_MD_SYNC_SET_REQ:
+		proc_sync_set_req(mcl, cmd, len);
+		break;
+	case MCAP_MD_SYNC_SET_RSP:
+		proc_sync_set_rsp(mcl, cmd, len);
+		break;
+	case MCAP_MD_SYNC_INFO_IND:
+		proc_sync_info_ind(mcl, cmd, len);
+		break;
+	}
+}
+
+void mcap_sync_cap_req(struct mcap_mcl *mcl, uint16_t reqacc,
+			mcap_sync_cap_cb cb, gpointer user_data,
+			GError **err)
+{
+	struct mcap_sync_cap_cbdata *cbdata;
+	mcap_md_sync_cap_req *cmd;
+
+	if (!mcl->mi->csp_enabled || !mcl->csp) {
+		g_set_error(err,
+			MCAP_CSP_ERROR,
+			MCAP_ERROR_RESOURCE_UNAVAILABLE,
+			"CSP not enabled for the instance");
+		return;
+	}
+
+	if (mcl->csp->csp_req) {
+		g_set_error(err,
+			MCAP_CSP_ERROR,
+			MCAP_ERROR_RESOURCE_UNAVAILABLE,
+			"Pending CSP request");
+		return;
+	}
+
+	mcl->csp->csp_req = MCAP_MD_SYNC_CAP_REQ;
+	cmd = g_new0(mcap_md_sync_cap_req, 1);
+
+	cmd->op = MCAP_MD_SYNC_CAP_REQ;
+	cmd->timest = htons(reqacc);
+
+	cbdata = g_new0(struct mcap_sync_cap_cbdata, 1);
+	cbdata->cb = cb;
+	cbdata->user_data = user_data;
+	mcl->csp->csp_priv_data = cbdata;
+
+	send_sync_cmd(mcl, cmd, sizeof(*cmd));
+
+	g_free(cmd);
+}
+
+void mcap_sync_set_req(struct mcap_mcl *mcl, uint8_t update, uint32_t btclock,
+			uint64_t timestamp, mcap_sync_set_cb cb,
+			gpointer user_data, GError **err)
+{
+	mcap_md_sync_set_req *cmd;
+	struct mcap_sync_set_cbdata *cbdata;
+
+	if (!mcl->mi->csp_enabled || !mcl->csp) {
+		g_set_error(err,
+			MCAP_CSP_ERROR,
+			MCAP_ERROR_RESOURCE_UNAVAILABLE,
+			"CSP not enabled for the instance");
+		return;
+	}
+
+	if (!mcl->csp->local_caps) {
+		g_set_error(err,
+			MCAP_CSP_ERROR,
+			MCAP_ERROR_RESOURCE_UNAVAILABLE,
+			"Did not get CSP caps from slave yet");
+		return;
+	}
+
+	if (mcl->csp->csp_req) {
+		g_set_error(err,
+			MCAP_CSP_ERROR,
+			MCAP_ERROR_RESOURCE_UNAVAILABLE,
+			"Pending CSP request");
+		return;
+	}
+
+	mcl->csp->csp_req = MCAP_MD_SYNC_SET_REQ;
+	cmd = g_new0(mcap_md_sync_set_req, 1);
+
+	cmd->op = MCAP_MD_SYNC_SET_REQ;
+	cmd->timestui = update;
+	cmd->btclock = htonl(btclock);
+	cmd->timestst = hton64(timestamp);
+
+	mcl->csp->ind_expected = update;
+
+	cbdata = g_new0(struct mcap_sync_set_cbdata, 1);
+	cbdata->cb = cb;
+	cbdata->user_data = user_data;
+	mcl->csp->csp_priv_data = cbdata;
+
+	send_sync_cmd(mcl, cmd, sizeof(*cmd));
+
+	g_free(cmd);
+}
+
+void mcap_enable_csp(struct mcap_instance *mi)
+{
+	mi->csp_enabled = TRUE;
+}
+
+void mcap_disable_csp(struct mcap_instance *mi)
+{
+	mi->csp_enabled = FALSE;
+}