From 799757ccf1d03c33c75bc597cd5ef77741dcb6a7 Mon Sep 17 00:00:00 2001 From: Adrian Bunk Date: Fri, 3 Jun 2011 09:17:04 +0000 Subject: Imported upstream 4.91 --- health/hdp.c | 2223 ++++++++++++++++++++++++++++++++++++++++++++++++ health/hdp.h | 35 + health/hdp_main.c | 62 ++ health/hdp_manager.c | 100 +++ health/hdp_manager.h | 27 + health/hdp_types.h | 129 +++ health/hdp_util.c | 1211 ++++++++++++++++++++++++++ health/hdp_util.h | 61 ++ health/mcap.c | 2195 +++++++++++++++++++++++++++++++++++++++++++++++ health/mcap.h | 169 ++++ health/mcap_internal.h | 141 +++ health/mcap_lib.h | 228 +++++ health/mcap_sync.c | 1015 ++++++++++++++++++++++ 13 files changed, 7596 insertions(+) create mode 100644 health/hdp.c create mode 100644 health/hdp.h create mode 100644 health/hdp_main.c create mode 100644 health/hdp_manager.c create mode 100644 health/hdp_manager.h create mode 100644 health/hdp_types.h create mode 100644 health/hdp_util.c create mode 100644 health/hdp_util.h create mode 100644 health/mcap.c create mode 100644 health/mcap.h create mode 100644 health/mcap_internal.h create mode 100644 health/mcap_lib.h create mode 100644 health/mcap_sync.c (limited to 'health') 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 + * Jose Antonio Santos-Cadenas + * + * 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 + +#include "log.h" +#include "error.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../src/dbus-common.h" +#include + +#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 + * Jose Antonio Santos-Cadenas + * + * 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 + * Jose Antonio Santos-Cadenas + * + * 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 +#endif + +#include + +#include + +#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 + * Jose Antonio Santos-Cadenas + * + * 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 +#endif + +#include +#include + +#include +#include +#include + +#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 + * Jose Antonio Santos-Cadenas + * + * 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 + * Jose Antonio Santos-Cadenas + * + * 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 + * Jose Antonio Santos-Cadenas + * + * 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 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +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 + * Jose Antonio Santos-Cadenas + * + * 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 + * Jose Antonio Santos-Cadenas + * + * 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 +#include +#include +#include + +#include "btio.h" +#include +#include +#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, ¬ify); + 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, ¬ify); + +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, ¬ify); + 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, ¬ify); + } + + 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 + * Jose Antonio Santos-Cadenas + * Elvis Pfützenreuter + * + * 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 + * Jose Antonio Santos-Cadenas + * + * 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 + * Jose Antonio Santos-Cadenas + * + * 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 + * Jose Antonio Santos-Cadenas + * Elvis Pfützenreuter + * + * 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 +#include +#include +#include +#include +#include +#include "../src/adapter.h" +#include "../src/manager.h" +#include + +#include "config.h" +#include "log.h" + +#include +#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; +} -- cgit v1.2.3