From 785c628805c47be849cfad770a9933b6791eb694 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Fri, 27 Dec 2013 19:33:20 -0800 Subject: [PATCH] shared: Add support simple HCI handling functions --- src/shared/hci.c | 547 +++++++++++++++++++++++++++++++++++++++++++++++ src/shared/hci.h | 52 +++++ 2 files changed, 599 insertions(+) create mode 100644 src/shared/hci.c create mode 100644 src/shared/hci.h diff --git a/src/shared/hci.c b/src/shared/hci.c new file mode 100644 index 000000000..3d95b43d0 --- /dev/null +++ b/src/shared/hci.c @@ -0,0 +1,547 @@ +/* + * + * 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 + +#include "src/shared/util.h" +#include "src/shared/queue.h" +#include "monitor/mainloop.h" +#include "monitor/bt.h" + +#include "hci.h" + +#define le16_to_cpu(val) (val) +#define le32_to_cpu(val) (val) +#define cpu_to_le16(val) (val) +#define cpu_to_le32(val) (val) + +#define BTPROTO_HCI 1 +struct sockaddr_hci { + sa_family_t hci_family; + unsigned short hci_dev; + unsigned short hci_channel; +}; +#define HCI_CHANNEL_USER 1 + +struct bt_hci { + int ref_count; + int fd; + bool close_on_unref; + bool writer_active; + uint8_t num_cmds; + unsigned int next_cmd_id; + unsigned int next_evt_id; + struct queue *cmd_queue; + struct queue *rsp_queue; + struct queue *evt_list; +}; + +struct cmd { + unsigned int id; + uint16_t opcode; + void *data; + uint8_t size; + bt_hci_callback_func_t callback; + bt_hci_destroy_func_t destroy; + void *user_data; +}; + +struct evt { + unsigned int id; + uint8_t event; + bt_hci_callback_func_t callback; + bt_hci_destroy_func_t destroy; + void *user_data; +}; + +static void cmd_free(void *data) +{ + struct cmd *cmd = data; + + if (cmd->destroy) + cmd->destroy(cmd->user_data); + + free(cmd->data); + free(cmd); +} + +static void evt_free(void *data) +{ + struct evt *evt = data; + + if (evt->destroy) + evt->destroy(evt->user_data); + + free(evt); +} + +static void send_command(struct bt_hci *hci, uint16_t opcode, + void *data, uint8_t size) +{ + uint8_t type = BT_H4_CMD_PKT; + struct bt_hci_cmd_hdr hdr; + struct iovec iov[3]; + int iovcnt; + + if (hci->num_cmds < 1) + return; + + hdr.opcode = cpu_to_le16(opcode); + hdr.plen = size; + + iov[0].iov_base = &type; + iov[0].iov_len = 1; + iov[1].iov_base = &hdr; + iov[1].iov_len = sizeof(hdr); + + if (size > 0) { + iov[2].iov_base = data; + iov[2].iov_len = size; + iovcnt = 3; + } else + iovcnt = 2; + + if (writev(hci->fd, iov, iovcnt) < 0) + return; + + hci->num_cmds--; +} + +static void wakeup_writer(struct bt_hci *hci) +{ + if (hci->writer_active) + return; + + if (hci->num_cmds < 1) + return; + + if (queue_isempty(hci->cmd_queue)) + return; + + if (mainloop_modify_fd(hci->fd, EPOLLIN | EPOLLOUT) < 0) + return; + + hci->writer_active = true; +} + +static void suspend_writer(struct bt_hci *hci) +{ + if (!hci->writer_active) + return; + + if (mainloop_modify_fd(hci->fd, EPOLLIN) < 0) + return; + + hci->writer_active = false; +} + +static bool match_cmd_opcode(const void *a, const void *b) +{ + const struct cmd *cmd = a; + uint16_t opcode = PTR_TO_UINT(b); + + return cmd->opcode == opcode; +} + +static void process_response(struct bt_hci *hci, uint16_t opcode, + const void *data, size_t size) +{ + struct cmd *cmd; + + if (opcode == BT_HCI_CMD_NOP) + goto done; + + cmd = queue_remove_if(hci->rsp_queue, match_cmd_opcode, + UINT_TO_PTR(opcode)); + if (!cmd) + return; + + if (cmd->callback) + cmd->callback(data, size, cmd->user_data); + + cmd_free(cmd); + +done: + wakeup_writer(hci); +} + +static void process_notify(void *data, void *user_data) +{ + struct bt_hci_evt_hdr *hdr = user_data; + struct evt *evt = data; + + if (evt->event == hdr->evt) + evt->callback(user_data + sizeof(struct bt_hci_evt_hdr), + hdr->plen, evt->user_data); +} + +static void process_event(struct bt_hci *hci, const void *data, size_t size) +{ + const struct bt_hci_evt_hdr *hdr = data; + const struct bt_hci_evt_cmd_complete *cc; + const struct bt_hci_evt_cmd_status *cs; + + if (size < sizeof(struct bt_hci_evt_hdr)) + return; + + data += sizeof(struct bt_hci_evt_hdr); + size -= sizeof(struct bt_hci_evt_hdr); + + if (hdr->plen != size) + return; + + switch (hdr->evt) { + case BT_HCI_EVT_CMD_COMPLETE: + if (size < sizeof(*cc)) + return; + cc = data; + hci->num_cmds = cc->ncmd; + process_response(hci, le16_to_cpu(cc->opcode), + data + sizeof(*cc), + size - sizeof(*cc)); + break; + + case BT_HCI_EVT_CMD_STATUS: + if (size < sizeof(*cs)) + return; + cs = data; + hci->num_cmds = cs->ncmd; + process_response(hci, le16_to_cpu(cs->opcode), &cs->status, 1); + break; + + default: + queue_foreach(hci->evt_list, process_notify, (void *) hdr); + break; + } +} + +static void io_callback(int fd, uint32_t events, void *user_data) +{ + struct bt_hci *hci = user_data; + + if (events & (EPOLLERR | EPOLLHUP)) + return; + + if (events & EPOLLIN) { + uint8_t buf[512]; + ssize_t len; + + len = read(hci->fd, buf, sizeof(buf)); + if (len < 0) + return; + + if (len < 1) + return; + + switch (buf[0]) { + case BT_H4_EVT_PKT: + process_event(hci, buf + 1, len - 1); + break; + } + } else if (events & EPOLLOUT) { + struct cmd *cmd; + + cmd = queue_pop_head(hci->cmd_queue); + if (cmd) { + send_command(hci, cmd->opcode, cmd->data, cmd->size); + queue_push_tail(hci->rsp_queue, cmd); + } + + suspend_writer(hci); + } +} + +struct bt_hci *bt_hci_new(int fd) +{ + struct bt_hci *hci; + + if (fd < 0) + return NULL; + + hci = new0(struct bt_hci, 1); + if (!hci) + return NULL; + + hci->fd = fd; + hci->close_on_unref = false; + hci->writer_active = false; + hci->num_cmds = 1; + hci->next_cmd_id = 1; + hci->next_evt_id = 1; + + hci->cmd_queue = queue_new(); + if (!hci->cmd_queue) { + free(hci); + return NULL; + } + + hci->rsp_queue = queue_new(); + if (!hci->rsp_queue) { + queue_destroy(hci->cmd_queue, NULL); + free(hci); + return NULL; + } + + hci->evt_list = queue_new(); + if (!hci->evt_list) { + queue_destroy(hci->rsp_queue, NULL); + queue_destroy(hci->cmd_queue, NULL); + free(hci); + return NULL; + } + + if (mainloop_add_fd(hci->fd, EPOLLIN, io_callback, hci, NULL) < 0) { + queue_destroy(hci->evt_list, NULL); + queue_destroy(hci->rsp_queue, NULL); + queue_destroy(hci->cmd_queue, NULL); + free(hci); + return NULL; + } + + return bt_hci_ref(hci); +} + +struct bt_hci *bt_hci_new_user_channel(uint16_t index) +{ + struct bt_hci *hci; + struct sockaddr_hci addr; + int fd; + + fd = socket(PF_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 = index; + addr.hci_channel = HCI_CHANNEL_USER; + + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + close(fd); + return NULL; + } + + hci = bt_hci_new(fd); + if (!hci) { + close(fd); + return NULL; + } + + bt_hci_set_close_on_unref(hci, true); + + return hci; +} + +struct bt_hci *bt_hci_ref(struct bt_hci *hci) +{ + if (!hci) + return NULL; + + __sync_fetch_and_add(&hci->ref_count, 1); + + return hci; +} + +void bt_hci_unref(struct bt_hci *hci) +{ + if (!hci) + return; + + if (__sync_sub_and_fetch(&hci->ref_count, 1)) + return; + + queue_destroy(hci->evt_list, evt_free); + queue_destroy(hci->cmd_queue, cmd_free); + queue_destroy(hci->rsp_queue, cmd_free); + + mainloop_remove_fd(hci->fd); + + if (hci->close_on_unref) + close(hci->fd); + + free(hci); +} + +bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close) +{ + if (!hci) + return false; + + hci->close_on_unref = do_close; + + return true; +} + +unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode, + const void *data, uint8_t size, + bt_hci_callback_func_t callback, + void *user_data, bt_hci_destroy_func_t destroy) +{ + struct cmd *cmd; + + if (!hci) + return 0; + + cmd = new0(struct cmd, 1); + if (!cmd) + return 0; + + cmd->opcode = opcode; + cmd->size = size; + + if (cmd->size > 0) { + cmd->data = malloc(cmd->size); + if (!cmd->data) { + free(cmd); + return 0; + } + + memcpy(cmd->data, data, cmd->size); + } + + if (hci->next_cmd_id < 1) + hci->next_cmd_id = 1; + + cmd->id = hci->next_cmd_id++; + + cmd->callback = callback; + cmd->destroy = destroy; + cmd->user_data = user_data; + + if (!queue_push_tail(hci->cmd_queue, cmd)) { + free(cmd->data); + free(cmd); + return 0; + } + + wakeup_writer(hci); + + return cmd->id; +} + +static bool match_cmd_id(const void *a, const void *b) +{ + const struct cmd *cmd = a; + unsigned int id = PTR_TO_UINT(b); + + return cmd->id == id; +} + +bool bt_hci_cancel(struct bt_hci *hci, unsigned int id) +{ + struct cmd *cmd; + + if (!hci || !id) + return false; + + cmd = queue_remove_if(hci->cmd_queue, match_cmd_id, UINT_TO_PTR(id)); + if (!cmd) { + cmd = queue_remove_if(hci->rsp_queue, match_cmd_id, + UINT_TO_PTR(id)); + if (!cmd) + return false; + } + + cmd_free(cmd); + + wakeup_writer(hci); + + return true; +} + +bool bt_hci_flush(struct bt_hci *hci) +{ + if (!hci) + return false; + + suspend_writer(hci); + + queue_remove_all(hci->cmd_queue, cmd_free); + queue_remove_all(hci->rsp_queue, cmd_free); + + return true; +} + +unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event, + bt_hci_callback_func_t callback, + void *user_data, bt_hci_destroy_func_t destroy) +{ + struct evt *evt; + + if (!hci) + return 0; + + evt = new0(struct evt, 1); + if (!evt) + return 0; + + evt->event = event; + + if (hci->next_evt_id < 1) + hci->next_evt_id = 1; + + evt->id = hci->next_evt_id++; + + evt->callback = callback; + evt->destroy = destroy; + evt->user_data = user_data; + + if (!queue_push_tail(hci->evt_list, evt)) { + free(evt); + return 0; + } + + return evt->id; +} + +static bool match_evt_id(const void *a, const void *b) +{ + const struct evt *evt = a; + unsigned int id = PTR_TO_UINT(b); + + return evt->id == id; +} + +bool bt_hci_unregister(struct bt_hci *hci, unsigned int id) +{ + struct evt *evt; + + if (!hci || !id) + return false; + + evt = queue_remove_if(hci->evt_list, match_evt_id, UINT_TO_PTR(id)); + if (!evt) + return false; + + evt_free(evt); + + return true; +} diff --git a/src/shared/hci.h b/src/shared/hci.h new file mode 100644 index 000000000..48406bf65 --- /dev/null +++ b/src/shared/hci.h @@ -0,0 +1,52 @@ +/* + * + * 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 (*bt_hci_destroy_func_t)(void *user_data); + +struct bt_hci; + +struct bt_hci *bt_hci_new(int fd); +struct bt_hci *bt_hci_new_user_channel(uint16_t index); + +struct bt_hci *bt_hci_ref(struct bt_hci *hci); +void bt_hci_unref(struct bt_hci *hci); + +bool bt_hci_set_close_on_unref(struct bt_hci *hci, bool do_close); + +typedef void (*bt_hci_callback_func_t)(const void *data, uint8_t size, + void *user_data); + +unsigned int bt_hci_send(struct bt_hci *hci, uint16_t opcode, + const void *data, uint8_t size, + bt_hci_callback_func_t callback, + void *user_data, bt_hci_destroy_func_t destroy); +bool bt_hci_cancel(struct bt_hci *hci, unsigned int id); +bool bt_hci_flush(struct bt_hci *hci); + +unsigned int bt_hci_register(struct bt_hci *hci, uint8_t event, + bt_hci_callback_func_t callback, + void *user_data, bt_hci_destroy_func_t destroy); +bool bt_hci_unregister(struct bt_hci *hci, unsigned int id); -- 2.47.3