From 3c89fd52679c8ccebceb30294a4bd815b51ede19 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Mon, 5 Dec 2016 11:22:26 +0100 Subject: Import Drive SDK --- drive-sdk/deps/bzle/src/gattrib.c | 763 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 763 insertions(+) create mode 100644 drive-sdk/deps/bzle/src/gattrib.c (limited to 'drive-sdk/deps/bzle/src/gattrib.c') diff --git a/drive-sdk/deps/bzle/src/gattrib.c b/drive-sdk/deps/bzle/src/gattrib.c new file mode 100644 index 0000000..f865956 --- /dev/null +++ b/drive-sdk/deps/bzle/src/gattrib.c @@ -0,0 +1,763 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2010 Nokia Corporation + * Copyright (C) 2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include "log.h" + +#define GATT_TIMEOUT 30 + +struct _GAttrib { + GIOChannel *io; + int refs; + uint8_t *buf; + size_t buflen; + guint read_watch; + guint write_watch; + guint timeout_watch; + GQueue *requests; + GQueue *responses; + GSList *events; + guint next_cmd_id; + GDestroyNotify destroy; + gpointer destroy_user_data; + bool stale; +}; + +struct command { + guint id; + guint8 opcode; + guint8 *pdu; + guint16 len; + guint8 expected; + bool sent; + GAttribResultFunc func; + gpointer user_data; + GDestroyNotify notify; +}; + +struct event { + guint id; + guint8 expected; + guint16 handle; + GAttribNotifyFunc func; + gpointer user_data; + GDestroyNotify notify; +}; + +static guint8 opcode2expected(guint8 opcode) +{ + switch (opcode) { + case ATT_OP_MTU_REQ: + return ATT_OP_MTU_RESP; + + case ATT_OP_FIND_INFO_REQ: + return ATT_OP_FIND_INFO_RESP; + + case ATT_OP_FIND_BY_TYPE_REQ: + return ATT_OP_FIND_BY_TYPE_RESP; + + case ATT_OP_READ_BY_TYPE_REQ: + return ATT_OP_READ_BY_TYPE_RESP; + + case ATT_OP_READ_REQ: + return ATT_OP_READ_RESP; + + case ATT_OP_READ_BLOB_REQ: + return ATT_OP_READ_BLOB_RESP; + + case ATT_OP_READ_MULTI_REQ: + return ATT_OP_READ_MULTI_RESP; + + case ATT_OP_READ_BY_GROUP_REQ: + return ATT_OP_READ_BY_GROUP_RESP; + + case ATT_OP_WRITE_REQ: + return ATT_OP_WRITE_RESP; + + case ATT_OP_PREP_WRITE_REQ: + return ATT_OP_PREP_WRITE_RESP; + + case ATT_OP_EXEC_WRITE_REQ: + return ATT_OP_EXEC_WRITE_RESP; + + case ATT_OP_HANDLE_IND: + return ATT_OP_HANDLE_CNF; + } + + return 0; +} + +static bool is_response(guint8 opcode) +{ + switch (opcode) { + case ATT_OP_ERROR: + case ATT_OP_MTU_RESP: + case ATT_OP_FIND_INFO_RESP: + case ATT_OP_FIND_BY_TYPE_RESP: + case ATT_OP_READ_BY_TYPE_RESP: + case ATT_OP_READ_RESP: + case ATT_OP_READ_BLOB_RESP: + case ATT_OP_READ_MULTI_RESP: + case ATT_OP_READ_BY_GROUP_RESP: + case ATT_OP_WRITE_RESP: + case ATT_OP_PREP_WRITE_RESP: + case ATT_OP_EXEC_WRITE_RESP: + case ATT_OP_HANDLE_CNF: + return true; + } + + return false; +} + +GAttrib *g_attrib_ref(GAttrib *attrib) +{ + int refs; + + if (!attrib) + return NULL; + + refs = __sync_add_and_fetch(&attrib->refs, 1); + + DBG("%p: ref=%d", attrib, refs); + + return attrib; +} + +static void command_destroy(struct command *cmd) +{ + if (cmd->notify) + cmd->notify(cmd->user_data); + + g_free(cmd->pdu); + g_free(cmd); +} + +static void event_destroy(struct event *evt) +{ + if (evt->notify) + evt->notify(evt->user_data); + + g_free(evt); +} + +static void attrib_destroy(GAttrib *attrib) +{ + GSList *l; + struct command *c; + + while ((c = g_queue_pop_head(attrib->requests))) + command_destroy(c); + + while ((c = g_queue_pop_head(attrib->responses))) + command_destroy(c); + + g_queue_free(attrib->requests); + attrib->requests = NULL; + + g_queue_free(attrib->responses); + attrib->responses = NULL; + + for (l = attrib->events; l; l = l->next) + event_destroy(l->data); + + g_slist_free(attrib->events); + attrib->events = NULL; + + if (attrib->timeout_watch > 0) + g_source_remove(attrib->timeout_watch); + + if (attrib->write_watch > 0) + g_source_remove(attrib->write_watch); + + if (attrib->read_watch > 0) + g_source_remove(attrib->read_watch); + + if (attrib->io) + g_io_channel_unref(attrib->io); + + g_free(attrib->buf); + + if (attrib->destroy) + attrib->destroy(attrib->destroy_user_data); + + g_free(attrib); +} + +void g_attrib_unref(GAttrib *attrib) +{ + int refs; + + if (!attrib) + return; + + refs = __sync_sub_and_fetch(&attrib->refs, 1); + + DBG("%p: ref=%d", attrib, refs); + + if (refs > 0) + return; + + attrib_destroy(attrib); +} + +GIOChannel *g_attrib_get_channel(GAttrib *attrib) +{ + if (!attrib) + return NULL; + + return attrib->io; +} + +gboolean g_attrib_set_destroy_function(GAttrib *attrib, + GDestroyNotify destroy, gpointer user_data) +{ + if (attrib == NULL) + return FALSE; + + attrib->destroy = destroy; + attrib->destroy_user_data = user_data; + + return TRUE; +} + +static gboolean disconnect_timeout(gpointer data) +{ + struct _GAttrib *attrib = data; + struct command *c; + + g_attrib_ref(attrib); + + c = g_queue_pop_head(attrib->requests); + if (c == NULL) + goto done; + + if (c->func) + c->func(ATT_ECODE_TIMEOUT, NULL, 0, c->user_data); + + command_destroy(c); + + while ((c = g_queue_pop_head(attrib->requests))) { + if (c->func) + c->func(ATT_ECODE_ABORTED, NULL, 0, c->user_data); + command_destroy(c); + } + +done: + attrib->stale = true; + + g_attrib_unref(attrib); + + return FALSE; +} + +static gboolean can_write_data(GIOChannel *io, GIOCondition cond, + gpointer data) +{ + struct _GAttrib *attrib = data; + struct command *cmd; + GError *gerr = NULL; + gsize len; + GIOStatus iostat; + GQueue *queue; + + if (attrib->stale) + return FALSE; + + if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL)) + return FALSE; + + queue = attrib->responses; + cmd = g_queue_peek_head(queue); + if (cmd == NULL) { + queue = attrib->requests; + cmd = g_queue_peek_head(queue); + } + if (cmd == NULL) + return FALSE; + + /* + * Verify that we didn't already send this command. This can only + * happen with elementes from attrib->requests. + */ + if (cmd->sent) + return FALSE; + + iostat = g_io_channel_write_chars(io, (char *) cmd->pdu, cmd->len, + &len, &gerr); + if (iostat != G_IO_STATUS_NORMAL) { + if (gerr) { + error("%s", gerr->message); + g_error_free(gerr); + } + + return FALSE; + } + + if (cmd->expected == 0) { + g_queue_pop_head(queue); + command_destroy(cmd); + + return TRUE; + } + + cmd->sent = true; + + if (attrib->timeout_watch == 0) + attrib->timeout_watch = g_timeout_add_seconds(GATT_TIMEOUT, + disconnect_timeout, attrib); + + return FALSE; +} + +static void destroy_sender(gpointer data) +{ + struct _GAttrib *attrib = data; + + attrib->write_watch = 0; + g_attrib_unref(attrib); +} + +static void wake_up_sender(struct _GAttrib *attrib) +{ + if (attrib->write_watch > 0) + return; + + attrib = g_attrib_ref(attrib); + attrib->write_watch = g_io_add_watch_full(attrib->io, + G_PRIORITY_DEFAULT, G_IO_OUT, + can_write_data, attrib, destroy_sender); +} + +static bool match_event(struct event *evt, const uint8_t *pdu, gsize len) +{ + guint16 handle; + + if (evt->expected == GATTRIB_ALL_EVENTS) + return true; + + if (!is_response(pdu[0]) && evt->expected == GATTRIB_ALL_REQS) + return true; + + if (evt->expected == pdu[0] && evt->handle == GATTRIB_ALL_HANDLES) + return true; + + if (len < 3) + return false; + + handle = att_get_u16(&pdu[1]); + + if (evt->expected == pdu[0] && evt->handle == handle) + return true; + + return false; +} + +static gboolean received_data(GIOChannel *io, GIOCondition cond, gpointer data) +{ + struct _GAttrib *attrib = data; + struct command *cmd = NULL; + GSList *l; + uint8_t buf[512], status; + gsize len; + GIOStatus iostat; + + if (attrib->stale) + return FALSE; + + if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL)) { + attrib->read_watch = 0; + return FALSE; + } + + memset(buf, 0, sizeof(buf)); + + iostat = g_io_channel_read_chars(io, (char *) buf, sizeof(buf), + &len, NULL); + if (iostat != G_IO_STATUS_NORMAL) { + status = ATT_ECODE_IO; + goto done; + } + + for (l = attrib->events; l; l = l->next) { + struct event *evt = l->data; + + if (match_event(evt, buf, len)) + evt->func(buf, len, evt->user_data); + } + + if (!is_response(buf[0])) + return TRUE; + + if (attrib->timeout_watch > 0) { + g_source_remove(attrib->timeout_watch); + attrib->timeout_watch = 0; + } + + cmd = g_queue_pop_head(attrib->requests); + if (cmd == NULL) { + /* Keep the watch if we have events to report */ + return attrib->events != NULL; + } + + if (buf[0] == ATT_OP_ERROR) { + status = buf[4]; + goto done; + } + + if (cmd->expected != buf[0]) { + status = ATT_ECODE_IO; + goto done; + } + + status = 0; + +done: + if (!g_queue_is_empty(attrib->requests) || + !g_queue_is_empty(attrib->responses)) + wake_up_sender(attrib); + + if (cmd) { + if (cmd->func) + cmd->func(status, buf, len, cmd->user_data); + + command_destroy(cmd); + } + + return TRUE; +} + +GAttrib *g_attrib_new(GIOChannel *io) +{ + struct _GAttrib *attrib; + uint16_t imtu; + uint16_t att_mtu; + uint16_t cid; + GError *gerr = NULL; + + g_io_channel_set_encoding(io, NULL, NULL); + g_io_channel_set_buffered(io, FALSE); + + bt_io_get(io, &gerr, BT_IO_OPT_IMTU, &imtu, + BT_IO_OPT_CID, &cid, BT_IO_OPT_INVALID); + if (gerr) { + error("%s", gerr->message); + g_error_free(gerr); + return NULL; + } + + attrib = g_try_new0(struct _GAttrib, 1); + if (attrib == NULL) + return NULL; + + att_mtu = (cid == ATT_CID) ? ATT_DEFAULT_LE_MTU : imtu; + + attrib->buf = g_malloc0(att_mtu); + attrib->buflen = att_mtu; + + attrib->io = g_io_channel_ref(io); + attrib->requests = g_queue_new(); + attrib->responses = g_queue_new(); + + attrib->read_watch = g_io_add_watch(attrib->io, + G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL, + received_data, attrib); + + return g_attrib_ref(attrib); +} + +guint g_attrib_send(GAttrib *attrib, guint id, const guint8 *pdu, guint16 len, + GAttribResultFunc func, gpointer user_data, + GDestroyNotify notify) +{ + struct command *c; + GQueue *queue; + uint8_t opcode; + + if (attrib->stale) + return 0; + + c = g_try_new0(struct command, 1); + if (c == NULL) + return 0; + + opcode = pdu[0]; + + c->opcode = opcode; + c->expected = opcode2expected(opcode); + c->pdu = g_malloc(len); + memcpy(c->pdu, pdu, len); + c->len = len; + c->func = func; + c->user_data = user_data; + c->notify = notify; + + if (is_response(opcode)) + queue = attrib->responses; + else + queue = attrib->requests; + + if (id) { + c->id = id; + if (!is_response(opcode)) + g_queue_push_head(queue, c); + else + /* Don't re-order responses even if an ID is given */ + g_queue_push_tail(queue, c); + } else { + c->id = ++attrib->next_cmd_id; + g_queue_push_tail(queue, c); + } + + /* + * If a command was added to the queue and it was empty before, wake up + * the sender. If the sender was already woken up by the second queue, + * wake_up_sender will just return. + */ + if (g_queue_get_length(queue) == 1) + wake_up_sender(attrib); + + return c->id; +} + +static int command_cmp_by_id(gconstpointer a, gconstpointer b) +{ + const struct command *cmd = a; + guint id = GPOINTER_TO_UINT(b); + + return cmd->id - id; +} + +gboolean g_attrib_cancel(GAttrib *attrib, guint id) +{ + GList *l = NULL; + struct command *cmd; + GQueue *queue; + + if (attrib == NULL) + return FALSE; + + queue = attrib->requests; + if (queue) + l = g_queue_find_custom(queue, GUINT_TO_POINTER(id), + command_cmp_by_id); + if (l == NULL) { + queue = attrib->responses; + if (!queue) + return FALSE; + l = g_queue_find_custom(queue, GUINT_TO_POINTER(id), + command_cmp_by_id); + } + + if (l == NULL) + return FALSE; + + cmd = l->data; + + if (cmd == g_queue_peek_head(queue) && cmd->sent) + cmd->func = NULL; + else { + g_queue_remove(queue, cmd); + command_destroy(cmd); + } + + return TRUE; +} + +static gboolean cancel_all_per_queue(GQueue *queue) +{ + struct command *c, *head = NULL; + gboolean first = TRUE; + + if (queue == NULL) + return FALSE; + + while ((c = g_queue_pop_head(queue))) { + if (first && c->sent) { + /* If the command was sent ignore its callback ... */ + c->func = NULL; + head = c; + continue; + } + + first = FALSE; + command_destroy(c); + } + + if (head) { + /* ... and put it back in the queue */ + g_queue_push_head(queue, head); + } + + return TRUE; +} + +gboolean g_attrib_cancel_all(GAttrib *attrib) +{ + gboolean ret; + + if (attrib == NULL) + return FALSE; + + ret = cancel_all_per_queue(attrib->requests); + ret = cancel_all_per_queue(attrib->responses) && ret; + + return ret; +} + +gboolean g_attrib_set_debug(GAttrib *attrib, + GAttribDebugFunc func, gpointer user_data) +{ + return TRUE; +} + +uint8_t *g_attrib_get_buffer(GAttrib *attrib, size_t *len) +{ + if (len == NULL) + return NULL; + + *len = attrib->buflen; + + return attrib->buf; +} + +gboolean g_attrib_set_mtu(GAttrib *attrib, int mtu) +{ + if (mtu < ATT_DEFAULT_LE_MTU) + return FALSE; + + attrib->buf = g_realloc(attrib->buf, mtu); + + attrib->buflen = mtu; + + return TRUE; +} + +guint g_attrib_register(GAttrib *attrib, guint8 opcode, guint16 handle, + GAttribNotifyFunc func, gpointer user_data, + GDestroyNotify notify) +{ + static guint next_evt_id = 0; + struct event *event; + + event = g_try_new0(struct event, 1); + if (event == NULL) + return 0; + + event->expected = opcode; + event->handle = handle; + event->func = func; + event->user_data = user_data; + event->notify = notify; + event->id = ++next_evt_id; + + attrib->events = g_slist_append(attrib->events, event); + + return event->id; +} + +static int event_cmp_by_id(gconstpointer a, gconstpointer b) +{ + const struct event *evt = a; + guint id = GPOINTER_TO_UINT(b); + + return evt->id - id; +} + +gboolean g_attrib_is_encrypted(GAttrib *attrib) +{ + BtIOSecLevel sec_level; + + if (!bt_io_get(attrib->io, NULL, + BT_IO_OPT_SEC_LEVEL, &sec_level, + BT_IO_OPT_INVALID)) + return FALSE; + + return sec_level > BT_IO_SEC_LOW; +} + +gboolean g_attrib_unregister(GAttrib *attrib, guint id) +{ + struct event *evt; + GSList *l; + + if (id == 0) { + warn("%s: invalid id", __func__); + return FALSE; + } + + l = g_slist_find_custom(attrib->events, GUINT_TO_POINTER(id), + event_cmp_by_id); + if (l == NULL) + return FALSE; + + evt = l->data; + + attrib->events = g_slist_remove(attrib->events, evt); + + if (evt->notify) + evt->notify(evt->user_data); + + g_free(evt); + + return TRUE; +} + +gboolean g_attrib_unregister_all(GAttrib *attrib) +{ + GSList *l; + + if (attrib->events == NULL) + return FALSE; + + for (l = attrib->events; l; l = l->next) { + struct event *evt = l->data; + + if (evt->notify) + evt->notify(evt->user_data); + + g_free(evt); + } + + g_slist_free(attrib->events); + attrib->events = NULL; + + return TRUE; +} -- cgit v1.2.3