/* * * 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; }