M7350/external/bluetooth/bluez/attrib/gattrib.c

773 lines
14 KiB
C
Raw Normal View History

2024-09-09 08:57:42 +00:00
/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2010 Nokia Corporation
* Copyright (C) 2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* 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 <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <glib.h>
#include <stdio.h>
#include <bluetooth/bluetooth.h>
#include "btio/btio.h"
#include "lib/uuid.h"
#include "src/shared/util.h"
#include "src/log.h"
#include "attrib/att.h"
#include "attrib/gattrib.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 = get_le16(&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)) {
struct command *c;
while ((c = g_queue_pop_head(attrib->requests))) {
if (c->func)
c->func(ATT_ECODE_IO, NULL, 0, c->user_data);
command_destroy(c);
}
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;
}