From aed99d1ef9ea329fa6d4dd4c3f1d279a97df74c6 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Mon, 31 Dec 2012 18:50:00 -0800 Subject: [PATCH] shared: Add support for management interface handling --- src/shared/mgmt.c | 606 ++++++++++++++++++++++++++++++++++++++++++++++ src/shared/mgmt.h | 61 +++++ 2 files changed, 667 insertions(+) create mode 100644 src/shared/mgmt.c create mode 100644 src/shared/mgmt.h diff --git a/src/shared/mgmt.c b/src/shared/mgmt.c new file mode 100644 index 000000000..0e686d00d --- /dev/null +++ b/src/shared/mgmt.c @@ -0,0 +1,606 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include + +#include + +#include "lib/bluetooth.h" +#include "lib/mgmt.h" +#include "lib/hci.h" + +#include "src/shared/util.h" +#include "src/shared/mgmt.h" + +struct mgmt { + int ref_count; + int fd; + bool close_on_unref; + GIOChannel *io; + guint read_watch; + guint write_watch; + GQueue *request_queue; + GList *pending_list; + GList *notify_list; + unsigned int next_request_id; + unsigned int next_notify_id; + void *buf; + uint16_t len; + mgmt_debug_func_t debug_callback; + mgmt_destroy_func_t debug_destroy; + void *debug_data; +}; + +struct mgmt_request { + unsigned int id; + uint16_t opcode; + uint16_t index; + void *buf; + uint16_t len; + mgmt_request_func_t callback; + mgmt_destroy_func_t destroy; + void *user_data; +}; + +struct mgmt_notify { + unsigned int id; + uint16_t event; + uint16_t index; + mgmt_notify_func_t callback; + mgmt_destroy_func_t destroy; + void *user_data; +}; + +static void destroy_request(gpointer data, gpointer user_data) +{ + struct mgmt_request *request = data; + + if (request->destroy) + request->destroy(request->user_data); + + g_free(request->buf); + g_free(request); +} + +static gint compare_request_id(gconstpointer a, gconstpointer b) +{ + const struct mgmt_request *request = a; + unsigned int id = GPOINTER_TO_UINT(b); + + return request->id - id; +} + +static void destroy_notify(gpointer data, gpointer user_data) +{ + struct mgmt_notify *notify = data; + + if (notify->destroy) + notify->destroy(notify->user_data); + + g_free(notify); +} + +static gint compare_notify_id(gconstpointer a, gconstpointer b) +{ + const struct mgmt_notify *notify = a; + unsigned int id = GPOINTER_TO_UINT(b); + + return notify->id - id; +} + +static void write_watch_destroy(gpointer user_data) +{ + struct mgmt *mgmt = user_data; + + mgmt->write_watch = 0; +} + +static gboolean can_write_data(GIOChannel *channel, GIOCondition cond, + gpointer user_data) +{ + struct mgmt *mgmt = user_data; + struct mgmt_request *request; + ssize_t bytes_written; + int fd; + + request = g_queue_pop_head(mgmt->request_queue); + if (!request) + return FALSE; + + fd = g_io_channel_unix_get_fd(mgmt->io); + + bytes_written = write(fd, request->buf, request->len); + if (bytes_written < 0) { + g_queue_push_head(mgmt->request_queue, request); + return FALSE; + } + + util_debug(mgmt->debug_callback, mgmt->debug_data, + "[0x%04x] command 0x%04x", + request->index, request->opcode); + + util_hexdump('>', request->buf, bytes_written, + mgmt->debug_callback, mgmt->debug_data); + + mgmt->pending_list = g_list_append(mgmt->pending_list, request); + + return FALSE; +} + +static void wakeup_writer(struct mgmt *mgmt) +{ + if (mgmt->pending_list) + return; + + if (mgmt->write_watch > 0) + return; + + mgmt->write_watch = g_io_add_watch_full(mgmt->io, G_PRIORITY_HIGH, + G_IO_OUT | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + can_write_data, mgmt, write_watch_destroy); +} + +static GList *lookup_pending(struct mgmt *mgmt, uint16_t opcode, uint16_t index) +{ + GList *list; + + for (list = g_list_first(mgmt->pending_list); list; + list = g_list_next(list)) { + struct mgmt_request *request = list->data; + + if (request->opcode == opcode && request->index == index) + return list; + } + + return NULL; +} + +static void request_complete(struct mgmt *mgmt, uint8_t status, + uint16_t opcode, uint16_t index, + uint16_t length, const void *param) +{ + struct mgmt_request *request; + GList *list; + + list = lookup_pending(mgmt, opcode, index); + if (!list) + return; + + request = list->data; + + mgmt->pending_list = g_list_delete_link(mgmt->pending_list, list); + + if (request->callback) + request->callback(status, length, param, request->user_data); + + destroy_request(request, NULL); + + wakeup_writer(mgmt); +} + +static void process_notify(struct mgmt *mgmt, uint16_t event, uint16_t index, + uint16_t length, const void *param) +{ + GList *list; + + for (list = g_list_first(mgmt->notify_list); list; + list = g_list_next(list)) { + struct mgmt_notify *notify = list->data; + + if (notify->event != event) + continue; + + if (notify->index != index && notify->index != MGMT_INDEX_NONE) + continue; + + if (notify->callback) + notify->callback(index, length, param, + notify->user_data); + } +} + +static void read_watch_destroy(gpointer user_data) +{ + struct mgmt *mgmt = user_data; + + mgmt->read_watch = 0; +} + +static gboolean received_data(GIOChannel *channel, GIOCondition cond, + gpointer user_data) +{ + struct mgmt *mgmt = user_data; + struct mgmt_hdr *hdr; + struct mgmt_ev_cmd_complete *cc; + struct mgmt_ev_cmd_status *cs; + ssize_t bytes_read; + uint16_t opcode, event, index, length; + int fd; + + if (cond & G_IO_NVAL) + return FALSE; + + fd = g_io_channel_unix_get_fd(mgmt->io); + + bytes_read = read(fd, mgmt->buf, mgmt->len); + if (bytes_read < 0) + return TRUE; + + util_hexdump('<', mgmt->buf, bytes_read, + mgmt->debug_callback, mgmt->debug_data); + + if (bytes_read < MGMT_HDR_SIZE) + return TRUE; + + hdr = mgmt->buf; + event = btohs(hdr->opcode); + index = btohs(hdr->index); + length = btohs(hdr->len); + + if (bytes_read < length + MGMT_HDR_SIZE) + return TRUE; + + switch (event) { + case MGMT_EV_CMD_COMPLETE: + cc = mgmt->buf + MGMT_HDR_SIZE; + opcode = btohs(cc->opcode); + + util_debug(mgmt->debug_callback, mgmt->debug_data, + "[0x%04x] command 0x%04x complete: 0x%02x", + index, opcode, cc->status); + + request_complete(mgmt, cc->status, opcode, index, length - 3, + mgmt->buf + MGMT_HDR_SIZE + 3); + break; + case MGMT_EV_CMD_STATUS: + cs = mgmt->buf + MGMT_HDR_SIZE; + opcode = btohs(cs->opcode); + + util_debug(mgmt->debug_callback, mgmt->debug_data, + "[0x%04x] command 0x%02x status: 0x%02x", + index, opcode, cs->status); + + request_complete(mgmt, cs->status, opcode, index, 0, NULL); + break; + default: + util_debug(mgmt->debug_callback, mgmt->debug_data, + "[0x%04x] event 0x%04x", index, event); + + process_notify(mgmt, event, index, length, + mgmt->buf + MGMT_HDR_SIZE); + break; + } + + return TRUE; +} + +struct mgmt *mgmt_new(int fd) +{ + struct mgmt *mgmt; + + mgmt = g_try_new0(struct mgmt, 1); + if (!mgmt) + return NULL; + + mgmt->fd = fd; + mgmt->close_on_unref = false; + + mgmt->len = 512; + mgmt->buf = g_try_malloc(mgmt->len); + if (!mgmt->buf) { + g_free(mgmt); + return NULL; + } + + mgmt->io = g_io_channel_unix_new(mgmt->fd); + + g_io_channel_set_encoding(mgmt->io, NULL, NULL); + g_io_channel_set_buffered(mgmt->io, FALSE); + + mgmt->request_queue = g_queue_new(); + + mgmt->read_watch = g_io_add_watch_full(mgmt->io, G_PRIORITY_DEFAULT, + G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + received_data, mgmt, read_watch_destroy); + + return mgmt_ref(mgmt); +} + +struct mgmt *mgmt_new_default(void) +{ + struct mgmt *mgmt; + struct sockaddr_hci addr; + int fd; + + fd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK, + BTPROTO_HCI); + if (fd < 0) + return NULL; + + memset(&addr, 0, sizeof(addr)); + addr.hci_family = AF_BLUETOOTH; + addr.hci_dev = HCI_DEV_NONE; + addr.hci_channel = HCI_CHANNEL_CONTROL; + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(fd); + return NULL; + } + + mgmt = mgmt_new(fd); + if (!mgmt) { + close(fd); + return NULL; + } + + mgmt->close_on_unref = true; + + return mgmt; +} + +struct mgmt *mgmt_ref(struct mgmt *mgmt) +{ + if (!mgmt) + return NULL; + + __sync_fetch_and_add(&mgmt->ref_count, 1); + + return mgmt; +} + +void mgmt_unref(struct mgmt *mgmt) +{ + if (!mgmt) + return; + + if (__sync_sub_and_fetch(&mgmt->ref_count, 1)) + return; + + g_list_foreach(mgmt->notify_list, destroy_notify, NULL); + g_list_free(mgmt->notify_list); + + g_list_foreach(mgmt->pending_list, destroy_request, NULL); + g_list_free(mgmt->pending_list); + + g_queue_foreach(mgmt->request_queue, destroy_request, NULL); + g_queue_free(mgmt->request_queue); + + if (mgmt->write_watch > 0) + g_source_remove(mgmt->write_watch); + + if (mgmt->read_watch > 0) + g_source_remove(mgmt->read_watch); + + g_io_channel_unref(mgmt->io); + + if (mgmt->close_on_unref) + close(mgmt->fd); + + if (mgmt->debug_destroy) + mgmt->debug_destroy(mgmt->debug_data); + + g_free(mgmt->buf); + g_free(mgmt); +} + +bool mgmt_set_debug(struct mgmt *mgmt, mgmt_debug_func_t callback, + void *user_data, mgmt_destroy_func_t destroy) +{ + if (!mgmt) + return false; + + if (mgmt->debug_destroy) + mgmt->debug_destroy(mgmt->debug_data); + + mgmt->debug_callback = callback; + mgmt->debug_destroy = destroy; + mgmt->debug_data = user_data; + + return true; +} + +bool mgmt_set_close_on_unref(struct mgmt *mgmt, bool do_close) +{ + if (!mgmt) + return false; + + mgmt->close_on_unref = do_close; + + return true; +} + +unsigned int mgmt_send(struct mgmt *mgmt, uint16_t opcode, uint16_t index, + uint16_t length, const void *param, + mgmt_request_func_t callback, + void *user_data, mgmt_destroy_func_t destroy) +{ + struct mgmt_request *request; + struct mgmt_hdr *hdr; + + if (!mgmt || !opcode) + return 0; + + if (length > 0 && !param) + return 0; + + request = g_try_new0(struct mgmt_request, 1); + if (!request) + return 0; + + request->len = length + MGMT_HDR_SIZE; + request->buf = g_try_malloc(request->len); + if (!request->buf) { + g_free(request); + return 0; + } + + if (length > 0) + memcpy(request->buf + MGMT_HDR_SIZE, param, length); + + hdr = request->buf; + hdr->opcode = htobs(opcode); + hdr->index = htobs(index); + hdr->len = htobs(length); + + request->opcode = opcode; + request->index = index; + + request->callback = callback; + request->destroy = destroy; + request->user_data = user_data; + + if (mgmt->next_request_id < 1) + mgmt->next_request_id = 1; + + request->id = mgmt->next_request_id++; + + g_queue_push_tail(mgmt->request_queue, request); + + wakeup_writer(mgmt); + + return request->id; +} + +bool mgmt_cancel(struct mgmt *mgmt, unsigned int id) +{ + struct mgmt_request *request; + GList *list; + + if (!mgmt || !id) + return false; + + list = g_queue_find_custom(mgmt->request_queue, + GUINT_TO_POINTER(id), compare_request_id); + if (list) { + request = list->data; + + g_queue_delete_link(mgmt->request_queue, list); + } else { + list = g_list_find_custom(mgmt->pending_list, + GUINT_TO_POINTER(id), compare_request_id); + if (!list) + return false; + + request = list->data; + + mgmt->pending_list = g_list_delete_link(mgmt->pending_list, + list); + } + + destroy_request(request, NULL); + + return true; +} + +unsigned int mgmt_register(struct mgmt *mgmt, uint16_t event, uint16_t index, + mgmt_notify_func_t callback, + void *user_data, mgmt_destroy_func_t destroy) +{ + struct mgmt_notify *notify; + + if (!mgmt || !event) + return 0; + + notify = g_try_new0(struct mgmt_notify, 1); + if (!notify) + return 0; + + notify->event = event; + notify->index = index; + + notify->callback = callback; + notify->destroy = destroy; + notify->user_data = user_data; + + if (mgmt->next_notify_id < 1) + mgmt->next_notify_id = 1; + + notify->id = mgmt->next_notify_id++; + + mgmt->notify_list = g_list_append(mgmt->notify_list, notify); + + return notify->id; +} + +bool mgmt_unregister(struct mgmt *mgmt, unsigned int id) +{ + struct mgmt_notify *notify; + GList *list; + + if (!mgmt || !id) + return false; + + list = g_list_find_custom(mgmt->notify_list, + GUINT_TO_POINTER(id), compare_notify_id); + if (!list) + return false; + + notify = list->data; + + mgmt->notify_list = g_list_delete_link(mgmt->notify_list, list); + + destroy_notify(notify, NULL); + + return true; +} + +bool mgmt_unregister_index(struct mgmt *mgmt, uint16_t index) +{ + GList *list, *next; + + if (!mgmt) + return false; + + for (list = g_list_first(mgmt->notify_list); list; list = next) { + struct mgmt_notify *notify = list->data; + + next = g_list_next(list); + + if (notify->index != index) + continue; + + mgmt->notify_list = g_list_delete_link(mgmt->notify_list, list); + + destroy_notify(notify, NULL); + } + + return true; +} + +bool mgmt_unregister_all(struct mgmt *mgmt) +{ + if (!mgmt) + return false; + + g_list_foreach(mgmt->notify_list, destroy_notify, NULL); + g_list_free(mgmt->notify_list); + + mgmt->notify_list = NULL; + + return true; +} diff --git a/src/shared/mgmt.h b/src/shared/mgmt.h new file mode 100644 index 000000000..33190b563 --- /dev/null +++ b/src/shared/mgmt.h @@ -0,0 +1,61 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include +#include + +typedef void (*mgmt_destroy_func_t)(void *user_data); + +struct mgmt; + +struct mgmt *mgmt_new(int fd); +struct mgmt *mgmt_new_default(void); + +struct mgmt *mgmt_ref(struct mgmt *mgmt); +void mgmt_unref(struct mgmt *mgmt); + +typedef void (*mgmt_debug_func_t)(const char *str, void *user_data); + +bool mgmt_set_debug(struct mgmt *mgmt, mgmt_debug_func_t callback, + void *user_data, mgmt_destroy_func_t destroy); + +bool mgmt_set_close_on_unref(struct mgmt *mgmt, bool do_close); + +typedef void (*mgmt_request_func_t)(uint8_t status, uint16_t length, + const void *param, void *user_data); + +unsigned int mgmt_send(struct mgmt *mgmt, uint16_t opcode, uint16_t index, + uint16_t length, const void *param, + mgmt_request_func_t callback, + void *user_data, mgmt_destroy_func_t destroy); +bool mgmt_cancel(struct mgmt *mgmt, unsigned int id); + +typedef void (*mgmt_notify_func_t)(uint16_t index, uint16_t length, + const void *param, void *user_data); + +unsigned int mgmt_register(struct mgmt *mgmt, uint16_t event, uint16_t index, + mgmt_notify_func_t callback, + void *user_data, mgmt_destroy_func_t destroy); +bool mgmt_unregister(struct mgmt *mgmt, unsigned int id); +bool mgmt_unregister_index(struct mgmt *mgmt, uint16_t index); +bool mgmt_unregister_all(struct mgmt *mgmt); -- 2.47.3