From 7e25356deec3369773e3949fe7336d84c10834c0 Mon Sep 17 00:00:00 2001 From: root Date: Tue, 1 Sep 2015 10:10:22 +0100 Subject: fish --- .gitignore | 7 + Makefile | 12 +- ble.c | 329 ++++- ble.h | 27 +- bluez/att-types.h | 118 ++ bluez/att.c | 1434 ++++++++++++++++++++++ bluez/att.h | 93 ++ bluez/crypto.c | 681 +++++++++++ bluez/crypto.h | 61 + bluez/gatt-client.c | 3013 ++++++++++++++++++++++++++++++++++++++++++++++ bluez/gatt-client.h | 134 +++ bluez/gatt-db.c | 1636 +++++++++++++++++++++++++ bluez/gatt-db.h | 219 ++++ bluez/gatt-helpers.c | 1490 +++++++++++++++++++++++ bluez/gatt-helpers.h | 116 ++ bluez/io-mainloop.c | 325 +++++ bluez/io.h | 47 + bluez/mainloop.c | 405 +++++++ bluez/mainloop.h | 52 + bluez/queue.c | 417 +++++++ bluez/queue.h | 65 + bluez/timeout-mainloop.c | 85 ++ bluez/timeout.h | 27 + bluez/util.c | 135 +++ bluez/util.h | 157 +++ bluez/uuid.c | 311 +++++ bluez/uuid.h | 181 +++ dfu.c | 28 +- dfu.h | 38 +- manifest.c | 1 - manifest.h | 12 +- nrfdfu.c | 3 +- project.h | 8 +- prototypes.h | 5 +- util.c | 2 + 35 files changed, 11606 insertions(+), 68 deletions(-) create mode 100644 .gitignore create mode 100644 bluez/att-types.h create mode 100644 bluez/att.c create mode 100644 bluez/att.h create mode 100644 bluez/crypto.c create mode 100644 bluez/crypto.h create mode 100644 bluez/gatt-client.c create mode 100644 bluez/gatt-client.h create mode 100644 bluez/gatt-db.c create mode 100644 bluez/gatt-db.h create mode 100644 bluez/gatt-helpers.c create mode 100644 bluez/gatt-helpers.h create mode 100644 bluez/io-mainloop.c create mode 100644 bluez/io.h create mode 100644 bluez/mainloop.c create mode 100644 bluez/mainloop.h create mode 100644 bluez/queue.c create mode 100644 bluez/queue.h create mode 100644 bluez/timeout-mainloop.c create mode 100644 bluez/timeout.h create mode 100644 bluez/util.c create mode 100644 bluez/util.h create mode 100644 bluez/uuid.c create mode 100644 bluez/uuid.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..35c695c --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +main +test.zip +*.o +core.* +*~ +nrfdfu +.*.swp diff --git a/Makefile b/Makefile index 08587b8..f4a387b 100644 --- a/Makefile +++ b/Makefile @@ -1,8 +1,10 @@ CSRCS=nrfdfu.c util.c zip.c ble.c manifest.c dfu.c +BLUEZ_SRCS=bluez/att.c bluez/queue.c bluez/crypto.c bluez/util.c bluez/io-mainloop.c bluez/timeout-mainloop.c bluez/mainloop.c bluez/gatt-db.c bluez/uuid.c bluez/gatt-client.c bluez/gatt-helpers.c PROG=nrfdfu OPT=-g +WARN=-Wall CPROTO=cproto @@ -17,8 +19,8 @@ BLUEZ_LIBS=$(shell pkg-config --libs bluez) INCLUDES=${ZIP_INCLUDES} ${JSON_C_INCLUDES} ${BLUEZ_INCLUDES} LIBS=${ZIP_LIBS} ${JSON_C_LIBS} ${BLUEZ_LIBS} CPPFLAGS=${DEFINES} ${INCLUDES} -CFLAGS=${OPT} -OBJS=${CSRCS:%.c=%.o} +CFLAGS=${OPT} ${WARN} +OBJS=${CSRCS:%.c=%.o} ${BLUEZ_SRCS:%.c=%.o} all: ${PROG} @@ -32,4 +34,8 @@ protos: mv prototypes.new prototypes.h clean: - /bin/rm -f ${OBJS} + /bin/rm -f ${OBJS} ${PROG} *~ core.* core + +test: ${PROG} + ./${PROG} -b fd:f9:62:4a:8a:c8 -p test.zip + diff --git a/ble.c b/ble.c index 9f0b683..6e16adf 100644 --- a/ble.c +++ b/ble.c @@ -1,9 +1,60 @@ #include "project.h" +#include "bluez/gatt-client.h" +#include "bluez/mainloop.h" + static int verbose = 1; #define ATT_CID 4 +static bt_uuid_t fota_uuid,cp_uuid,data_uuid,cccd_uuid; + + + + +static void +att_debug_cb (const char *str, void *user_data) +{ + const char *prefix = user_data; + + fprintf (stderr, "%s:%s\n", prefix, str); +} + +static void +gatt_debug_cb (const char *str, void *user_data) +{ + const char *prefix = user_data; + + fprintf (stderr, "%s:%s\n", prefix, str); +} + +static void +log_service_event (struct gatt_db_attribute *attr, const char *str) +{ + char uuid_str[MAX_LEN_UUID_STR]; + bt_uuid_t uuid; + uint16_t start, end; + + gatt_db_attribute_get_service_uuid (attr, &uuid); + bt_uuid_to_string (&uuid, uuid_str, sizeof (uuid_str)); + + gatt_db_attribute_get_service_handles (attr, &start, &end); + + fprintf (stderr, "%s - UUID: %s start: 0x%04x end: 0x%04x\n", str, uuid_str, + start, end); +} + +static void +service_added_cb (struct gatt_db_attribute *attr, void *user_data) +{ + log_service_event (attr, "Service Added"); +} + +static void +service_removed_cb (struct gatt_db_attribute *attr, void *user_data) +{ + log_service_event (attr, "Service Removed"); +} static int @@ -81,23 +132,167 @@ l2cap_le_att_connect (bdaddr_t * src, bdaddr_t * dst, uint8_t dst_type, -void ble_finish(struct ble *ble) +void +ble_close (BLE *ble) +{ + if (!ble) + return; + + + if (ble->notify_id) + bt_gatt_client_unregister_notify(ble->gatt, ble->notify_id); + + if (ble->gatt) + bt_gatt_client_unref(ble->gatt); + + if (ble->db) + gatt_db_unref(ble->db); + + if (ble->att) + bt_att_unref (ble->att); + if (ble->fd > 0) + close (ble->fd); + + + free (ble); +} + + +static void +att_disconnect_cb (int err, void *user_data) +{ + printf ("Device disconnected: %s\n", strerror (err)); + + mainloop_exit_failure (); +} + +static void print_uuid(const bt_uuid_t *uuid) +{ + char uuid_str[MAX_LEN_UUID_STR]; + bt_uuid_t uuid128; + + bt_uuid_to_uuid128(uuid, &uuid128); + bt_uuid_to_string(&uuid128, uuid_str, sizeof(uuid_str)); + + printf("%s\n", uuid_str); +} + + + +static void scan_desc(struct gatt_db_attribute *attr, void *user_data) +{ +BLE *ble=user_data; +uint16_t handle=gatt_db_attribute_get_handle(attr); +const bt_uuid_t *uuid=gatt_db_attribute_get_type(attr); + + printf("\t\tdesc - handle: 0x%04x, uuid: ",handle); + print_uuid(uuid); + + if (!bt_uuid_cmp(uuid,&cccd_uuid)) + ble->cccd_handle=handle; + +} + + + +static void scan_chrc(struct gatt_db_attribute *attr, void *user_data) { -if (!ble) return; + BLE *ble=user_data; + uint16_t handle, value_handle; + uint8_t properties; + bt_uuid_t uuid; + + if (!gatt_db_attribute_get_char_data(attr, &handle, + &value_handle, + &properties, + &uuid)) + return; + + printf("\tchar - start: 0x%04x, value: 0x%04x, props: 0x%02x, uuid: ", + handle, value_handle, properties); -if (ble->att) bt_att_unref(ble->att); -if (ble->fd>0) close(ble->fd); -free(ble); + print_uuid(&uuid); + + if (!bt_uuid_cmp(&uuid,&data_uuid)) + ble->data_handle=value_handle; + + if (!bt_uuid_cmp(&uuid,&cp_uuid)) { + ble->cp_handle=value_handle; + gatt_db_service_foreach_desc(attr, scan_desc, ble); + } + } -struct ble *ble_start (const char *bdaddr) + + +static void scan_service(struct gatt_db_attribute *attr, void *user_data) { - struct ble *ble; + BLE *ble = user_data; + uint16_t start, end; + bool primary; + bt_uuid_t uuid; - ble=xmalloc(sizeof(*ble)); - memset(ble,0,sizeof(*ble)); + if (!gatt_db_attribute_get_service_data(attr, &start, &end, &primary, + &uuid)) + return; + + gatt_db_service_foreach_char(attr, scan_chrc, ble); + + printf("\n"); +} + + +static void +ready_cb (bool success, uint8_t att_ecode, void *user_data) +{ + BLE *ble = user_data; + + if (!success) + { + fprintf (stderr, + "GATT discovery procedures failed - error code: 0x%02x\n", + att_ecode); + mainloop_exit_failure (); + return; + } + + printf ("GATT discovery procedures complete\n"); + + + gatt_db_foreach_service(ble->db, &fota_uuid, scan_service, ble); + + + printf("Handles:\n\tdata: 0x%04x\n\tcp : 0x%04x\n\tcccd: 0x%04x\n", + ble->data_handle,ble->cp_handle,ble->cccd_handle); + + if (ble->cccd_handle && ble->cp_handle && ble->data_handle) { + mainloop_exit_success (); + return; + } + + mainloop_exit_failure(); +} + + +void ble_init(void) +{ + bt_string_to_uuid(&fota_uuid, "00001530-1212-efde-1523-785feabcd123"); + bt_string_to_uuid(&cp_uuid, "00001531-1212-efde-1523-785feabcd123"); + bt_string_to_uuid(&data_uuid, "00001532-1212-efde-1523-785feabcd123"); + bt_string_to_uuid(&cccd_uuid, "00002902-0000-1000-8000-00805f9b34fb"); + + mainloop_init (); +} + +BLE * +ble_open (const char *bdaddr) +{ + BLE *ble; + + ble = xmalloc (sizeof (*ble)); + memset (ble, 0, sizeof (*ble)); ble->sec = BT_SECURITY_LOW; ble->dst_type = BDADDR_LE_RANDOM; @@ -106,42 +301,122 @@ struct ble *ble_start (const char *bdaddr) if (str2ba (bdaddr, &ble->dst_addr) < 0) { fprintf (stderr, "Invalid remote address: %s\n", bdaddr); - ble_finish(ble); + ble_close (ble); return NULL; } - ble->fd = l2cap_le_att_connect (&ble->src_addr, &ble->dst_addr, ble->dst_type, ble->sec); + ble->fd = + l2cap_le_att_connect (&ble->src_addr, &ble->dst_addr, ble->dst_type, + ble->sec); if (ble->fd < 0) { perror ("l2cap_le_att_connect"); - ble_finish(ble); + ble_close (ble); return NULL; } - ble->att = bt_att_new(ble->fd); + ble->att = bt_att_new (ble->fd); + + if (!ble->att) + { + fprintf (stderr, "Failed to initialze ATT transport layer\n"); + ble_close (ble); + return NULL; + } + + if (!bt_att_set_close_on_unref (ble->att, true)) + { + fprintf (stderr, "Failed to set up ATT transport layer\n"); + ble_close (ble); + return NULL; + } - if (!ble->att) { - fprintf(stderr, "Failed to initialze ATT transport layer\n"); - ble_finish(ble); - return NULL; + if (!bt_att_register_disconnect (ble->att, att_disconnect_cb, NULL, NULL)) + { + fprintf (stderr, "Failed to set ATT disconnect handler\n"); + ble_close (ble); + return NULL; + } + + + ble->db = gatt_db_new (); + ble->gatt = bt_gatt_client_new (ble->db, ble->att, ble->mtu); + + + gatt_db_register (ble->db, service_added_cb, service_removed_cb, + NULL, NULL); + + if (verbose) + { + bt_att_set_debug (ble->att, att_debug_cb, "att: ", NULL); + bt_gatt_client_set_debug (ble->gatt, gatt_debug_cb, "gatt: ", NULL); + } + + bt_gatt_client_set_ready_handler (ble->gatt, ready_cb, ble, NULL); + + + if (mainloop_run () == EXIT_SUCCESS) + return ble; + + ble_close (ble); + return NULL; +} + +static void notify_cb(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + int i; + + printf("Handle Value Not/Ind: 0x%04x - ", value_handle); + + if (length == 0) { + printf("(0 bytes)\n"); + return; } - if (!bt_att_set_close_on_unref(ble->att, true)) { - fprintf(stderr, "Failed to set up ATT transport layer\n"); - ble_finish(ble); - return NULL; + printf("(%u bytes): ", length); + + for (i = 0; i < length; i++) + printf("%02x ", value[i]); + + printf("\n"); +} + +static void register_notify_cb(uint16_t att_ecode, void *user_data) +{ + if (att_ecode) { + printf("Failed to register notify handler " + "- error code: 0x%02x\n", att_ecode); + mainloop_exit_failure(); + return; } - if (!bt_att_register_disconnect(ble->att, att_disconnect_cb, NULL, - NULL)) { - fprintf(stderr, "Failed to set ATT disconnect handler\n"); - ble_finish(ble); - return NULL; + printf("Registered notify handler!\n"); + mainloop_exit_success(); +} + + +int ble_register_notify(BLE *ble) { + + + + if (!bt_gatt_client_is_ready(ble->gatt)) { + printf("GATT client not initialized\n"); + return EXIT_FAILURE; } + ble->notify_id = bt_gatt_client_register_notify(ble->gatt, ble->cp_handle, + register_notify_cb, + notify_cb, NULL, NULL); + if (!ble->notify_id) { + printf("Failed to register notify handler\n"); + return EXIT_FAILURE; + } + printf("requesting notify\n"); - printf ("Connected!\n"); + return mainloop_run(); } + diff --git a/ble.h b/ble.h index 634191e..5073cd6 100644 --- a/ble.h +++ b/ble.h @@ -1,13 +1,22 @@ -struct ble { - bdaddr_t src_addr, dst_addr; - int sec; - uint8_t dst_type; +struct ble +{ + bdaddr_t src_addr, dst_addr; + int sec; + uint8_t dst_type; - int fd; + uint16_t mtu; - struct bt_att *att; - struct gatt_db *db; - struct bt_gatt_client *gatt; -}; + int fd; + + struct bt_att *att; + struct gatt_db *db; + struct bt_gatt_client *gatt; + unsigned notify_id; + + uint16_t cp_handle; + uint16_t cccd_handle; + uint16_t data_handle; +}; +typedef struct ble BLE; diff --git a/bluez/att-types.h b/bluez/att-types.h new file mode 100644 index 0000000..aa7f0da --- /dev/null +++ b/bluez/att-types.h @@ -0,0 +1,118 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 + +#ifndef __packed +#define __packed __attribute__((packed)) +#endif + +#define BT_ATT_DEFAULT_LE_MTU 23 +#define BT_ATT_MAX_LE_MTU 517 +#define BT_ATT_MAX_VALUE_LEN 512 + +/* ATT protocol opcodes */ +#define BT_ATT_OP_ERROR_RSP 0x01 +#define BT_ATT_OP_MTU_REQ 0x02 +#define BT_ATT_OP_MTU_RSP 0x03 +#define BT_ATT_OP_FIND_INFO_REQ 0x04 +#define BT_ATT_OP_FIND_INFO_RSP 0x05 +#define BT_ATT_OP_FIND_BY_TYPE_VAL_REQ 0x06 +#define BT_ATT_OP_FIND_BY_TYPE_VAL_RSP 0x07 +#define BT_ATT_OP_READ_BY_TYPE_REQ 0x08 +#define BT_ATT_OP_READ_BY_TYPE_RSP 0x09 +#define BT_ATT_OP_READ_REQ 0x0a +#define BT_ATT_OP_READ_RSP 0x0b +#define BT_ATT_OP_READ_BLOB_REQ 0x0c +#define BT_ATT_OP_READ_BLOB_RSP 0x0d +#define BT_ATT_OP_READ_MULT_REQ 0x0e +#define BT_ATT_OP_READ_MULT_RSP 0x0f +#define BT_ATT_OP_READ_BY_GRP_TYPE_REQ 0x10 +#define BT_ATT_OP_READ_BY_GRP_TYPE_RSP 0x11 +#define BT_ATT_OP_WRITE_REQ 0x12 +#define BT_ATT_OP_WRITE_RSP 0x13 +#define BT_ATT_OP_WRITE_CMD 0x52 +#define BT_ATT_OP_SIGNED_WRITE_CMD 0xD2 +#define BT_ATT_OP_PREP_WRITE_REQ 0x16 +#define BT_ATT_OP_PREP_WRITE_RSP 0x17 +#define BT_ATT_OP_EXEC_WRITE_REQ 0x18 +#define BT_ATT_OP_EXEC_WRITE_RSP 0x19 +#define BT_ATT_OP_HANDLE_VAL_NOT 0x1B +#define BT_ATT_OP_HANDLE_VAL_IND 0x1D +#define BT_ATT_OP_HANDLE_VAL_CONF 0x1E + +/* Packed struct definitions for ATT protocol PDUs */ +/* TODO: Complete these definitions for all opcodes */ +struct bt_att_pdu_error_rsp { + uint8_t opcode; + uint16_t handle; + uint8_t ecode; +} __packed; + +/* Special opcode to receive all requests (legacy servers) */ +#define BT_ATT_ALL_REQUESTS 0x00 + +/* Error codes for Error response PDU */ +#define BT_ATT_ERROR_INVALID_HANDLE 0x01 +#define BT_ATT_ERROR_READ_NOT_PERMITTED 0x02 +#define BT_ATT_ERROR_WRITE_NOT_PERMITTED 0x03 +#define BT_ATT_ERROR_INVALID_PDU 0x04 +#define BT_ATT_ERROR_AUTHENTICATION 0x05 +#define BT_ATT_ERROR_REQUEST_NOT_SUPPORTED 0x06 +#define BT_ATT_ERROR_INVALID_OFFSET 0x07 +#define BT_ATT_ERROR_AUTHORIZATION 0x08 +#define BT_ATT_ERROR_PREPARE_QUEUE_FULL 0x09 +#define BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND 0x0A +#define BT_ATT_ERROR_ATTRIBUTE_NOT_LONG 0x0B +#define BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION_KEY_SIZE 0x0C +#define BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN 0x0D +#define BT_ATT_ERROR_UNLIKELY 0x0E +#define BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION 0x0F +#define BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE 0x10 +#define BT_ATT_ERROR_INSUFFICIENT_RESOURCES 0x11 + +/* + * ATT attribute permission bitfield values. Permissions are grouped as + * "Access", "Encryption", "Authentication", and "Authorization". A bitmask of + * permissions is a byte that encodes a combination of these. + */ +#define BT_ATT_PERM_READ 0x01 +#define BT_ATT_PERM_WRITE 0x02 +#define BT_ATT_PERM_ENCRYPT 0x04 +#define BT_ATT_PERM_AUTHEN 0x08 +#define BT_ATT_PERM_AUTHOR 0x10 +#define BT_ATT_PERM_NONE 0x20 + +/* GATT Characteristic Properties Bitfield values */ +#define BT_GATT_CHRC_PROP_BROADCAST 0x01 +#define BT_GATT_CHRC_PROP_READ 0x02 +#define BT_GATT_CHRC_PROP_WRITE_WITHOUT_RESP 0x04 +#define BT_GATT_CHRC_PROP_WRITE 0x08 +#define BT_GATT_CHRC_PROP_NOTIFY 0x10 +#define BT_GATT_CHRC_PROP_INDICATE 0x20 +#define BT_GATT_CHRC_PROP_AUTH 0x40 +#define BT_GATT_CHRC_PROP_EXT_PROP 0x80 + +/* GATT Characteristic Extended Properties Bitfield values */ +#define BT_GATT_CHRC_EXT_PROP_RELIABLE_WRITE 0x01 +#define BT_GATT_CHRC_EXT_PROP_WRITABLE_AUX 0x02 diff --git a/bluez/att.c b/bluez/att.c new file mode 100644 index 0000000..fd6ef05 --- /dev/null +++ b/bluez/att.c @@ -0,0 +1,1434 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 +#include +#include + +#include + +#include "io.h" +#include "queue.h" +#include "util.h" +#include "timeout.h" +#include "uuid.h" +#include "att.h" +#include "crypto.h" + +#define ATT_MIN_PDU_LEN 1 /* At least 1 byte for the opcode. */ +#define ATT_OP_CMD_MASK 0x40 +#define ATT_OP_SIGNED_MASK 0x80 +#define ATT_TIMEOUT_INTERVAL 30000 /* 30000 ms */ + +/* + * Common Profile and Service Error Code descriptions (see Supplement to the + * Bluetooth Core Specification, sections 1.2 and 2). The error codes within + * 0xE0-0xFC are reserved for future use. The remaining 3 are defined as the + * following: + */ +#define BT_ERROR_CCC_IMPROPERLY_CONFIGURED 0xfd +#define BT_ERROR_ALREADY_IN_PROGRESS 0xfe +#define BT_ERROR_OUT_OF_RANGE 0xff + +/* Length of signature in write signed packet */ +#define BT_ATT_SIGNATURE_LEN 12 + +struct att_send_op; + +struct bt_att { + int ref_count; + int fd; + struct io *io; + bool io_on_l2cap; + int io_sec_level; /* Only used for non-L2CAP */ + + struct queue *req_queue; /* Queued ATT protocol requests */ + struct att_send_op *pending_req; + struct queue *ind_queue; /* Queued ATT protocol indications */ + struct att_send_op *pending_ind; + struct queue *write_queue; /* Queue of PDUs ready to send */ + bool writer_active; + + struct queue *notify_list; /* List of registered callbacks */ + struct queue *disconn_list; /* List of disconnect handlers */ + + bool in_req; /* There's a pending incoming request */ + + uint8_t *buf; + uint16_t mtu; + + unsigned int next_send_id; /* IDs for "send" ops */ + unsigned int next_reg_id; /* IDs for registered callbacks */ + + bt_att_timeout_func_t timeout_callback; + bt_att_destroy_func_t timeout_destroy; + void *timeout_data; + + bt_att_debug_func_t debug_callback; + bt_att_destroy_func_t debug_destroy; + void *debug_data; + + struct bt_crypto *crypto; + + struct sign_info *local_sign; + struct sign_info *remote_sign; +}; + +struct sign_info { + uint8_t key[16]; + bt_att_counter_func_t counter; + void *user_data; +}; + +enum att_op_type { + ATT_OP_TYPE_REQ, + ATT_OP_TYPE_RSP, + ATT_OP_TYPE_CMD, + ATT_OP_TYPE_IND, + ATT_OP_TYPE_NOT, + ATT_OP_TYPE_CONF, + ATT_OP_TYPE_UNKNOWN, +}; + +static const struct { + uint8_t opcode; + enum att_op_type type; +} att_opcode_type_table[] = { + { BT_ATT_OP_ERROR_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_MTU_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_MTU_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_FIND_INFO_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_FIND_INFO_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_FIND_BY_TYPE_VAL_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_FIND_BY_TYPE_VAL_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_READ_BY_TYPE_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_READ_BY_TYPE_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_READ_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_READ_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_READ_BLOB_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_READ_BLOB_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_READ_MULT_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_READ_MULT_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_READ_BY_GRP_TYPE_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_READ_BY_GRP_TYPE_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_WRITE_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_WRITE_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_WRITE_CMD, ATT_OP_TYPE_CMD }, + { BT_ATT_OP_SIGNED_WRITE_CMD, ATT_OP_TYPE_CMD }, + { BT_ATT_OP_PREP_WRITE_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_PREP_WRITE_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_EXEC_WRITE_REQ, ATT_OP_TYPE_REQ }, + { BT_ATT_OP_EXEC_WRITE_RSP, ATT_OP_TYPE_RSP }, + { BT_ATT_OP_HANDLE_VAL_NOT, ATT_OP_TYPE_NOT }, + { BT_ATT_OP_HANDLE_VAL_IND, ATT_OP_TYPE_IND }, + { BT_ATT_OP_HANDLE_VAL_CONF, ATT_OP_TYPE_CONF }, + { } +}; + +static enum att_op_type get_op_type(uint8_t opcode) +{ + int i; + + for (i = 0; att_opcode_type_table[i].opcode; i++) { + if (att_opcode_type_table[i].opcode == opcode) + return att_opcode_type_table[i].type; + } + + return ATT_OP_TYPE_UNKNOWN; +} + +static const struct { + uint8_t req_opcode; + uint8_t rsp_opcode; +} att_req_rsp_mapping_table[] = { + { BT_ATT_OP_MTU_REQ, BT_ATT_OP_MTU_RSP }, + { BT_ATT_OP_FIND_INFO_REQ, BT_ATT_OP_FIND_INFO_RSP}, + { BT_ATT_OP_FIND_BY_TYPE_VAL_REQ, BT_ATT_OP_FIND_BY_TYPE_VAL_RSP }, + { BT_ATT_OP_READ_BY_TYPE_REQ, BT_ATT_OP_READ_BY_TYPE_RSP }, + { BT_ATT_OP_READ_REQ, BT_ATT_OP_READ_RSP }, + { BT_ATT_OP_READ_BLOB_REQ, BT_ATT_OP_READ_BLOB_RSP }, + { BT_ATT_OP_READ_MULT_REQ, BT_ATT_OP_READ_MULT_RSP }, + { BT_ATT_OP_READ_BY_GRP_TYPE_REQ, BT_ATT_OP_READ_BY_GRP_TYPE_RSP }, + { BT_ATT_OP_WRITE_REQ, BT_ATT_OP_WRITE_RSP }, + { BT_ATT_OP_PREP_WRITE_REQ, BT_ATT_OP_PREP_WRITE_RSP }, + { BT_ATT_OP_EXEC_WRITE_REQ, BT_ATT_OP_EXEC_WRITE_RSP }, + { } +}; + +static uint8_t get_req_opcode(uint8_t rsp_opcode) +{ + int i; + + for (i = 0; att_req_rsp_mapping_table[i].rsp_opcode; i++) { + if (att_req_rsp_mapping_table[i].rsp_opcode == rsp_opcode) + return att_req_rsp_mapping_table[i].req_opcode; + } + + return 0; +} + +struct att_send_op { + unsigned int id; + unsigned int timeout_id; + enum att_op_type type; + uint16_t opcode; + void *pdu; + uint16_t len; + bt_att_response_func_t callback; + bt_att_destroy_func_t destroy; + void *user_data; +}; + +static void destroy_att_send_op(void *data) +{ + struct att_send_op *op = data; + + if (op->timeout_id) + timeout_remove(op->timeout_id); + + if (op->destroy) + op->destroy(op->user_data); + + free(op->pdu); + free(op); +} + +static void cancel_att_send_op(struct att_send_op *op) +{ + if (op->destroy) + op->destroy(op->user_data); + + op->user_data = NULL; + op->callback = NULL; + op->destroy = NULL; +} + +struct att_notify { + unsigned int id; + uint16_t opcode; + bt_att_notify_func_t callback; + bt_att_destroy_func_t destroy; + void *user_data; +}; + +static void destroy_att_notify(void *data) +{ + struct att_notify *notify = data; + + if (notify->destroy) + notify->destroy(notify->user_data); + + free(notify); +} + +static bool match_notify_id(const void *a, const void *b) +{ + const struct att_notify *notify = a; + unsigned int id = PTR_TO_UINT(b); + + return notify->id == id; +} + +struct att_disconn { + unsigned int id; + bool removed; + bt_att_disconnect_func_t callback; + bt_att_destroy_func_t destroy; + void *user_data; +}; + +static void destroy_att_disconn(void *data) +{ + struct att_disconn *disconn = data; + + if (disconn->destroy) + disconn->destroy(disconn->user_data); + + free(disconn); +} + +static bool match_disconn_id(const void *a, const void *b) +{ + const struct att_disconn *disconn = a; + unsigned int id = PTR_TO_UINT(b); + + return disconn->id == id; +} + +static bool encode_pdu(struct bt_att *att, struct att_send_op *op, + const void *pdu, uint16_t length) +{ + uint16_t pdu_len = 1; + struct sign_info *sign = att->local_sign; + uint32_t sign_cnt; + + if (sign && (op->opcode & ATT_OP_SIGNED_MASK)) + pdu_len += BT_ATT_SIGNATURE_LEN; + + if (length && pdu) + pdu_len += length; + + if (pdu_len > att->mtu) + return false; + + op->len = pdu_len; + op->pdu = malloc(op->len); + if (!op->pdu) + return false; + + ((uint8_t *) op->pdu)[0] = op->opcode; + if (pdu_len > 1) + memcpy(op->pdu + 1, pdu, length); + + if (!sign || !(op->opcode & ATT_OP_SIGNED_MASK)) + return true; + + if (!sign->counter(&sign_cnt, sign->user_data)) + goto fail; + + if ((bt_crypto_sign_att(att->crypto, sign->key, op->pdu, 1 + length, + sign_cnt, &((uint8_t *) op->pdu)[1 + length]))) + return true; + + util_debug(att->debug_callback, att->debug_data, + "ATT unable to generate signature"); + +fail: + free(op->pdu); + return false; +} + +static struct att_send_op *create_att_send_op(struct bt_att *att, + uint8_t opcode, + const void *pdu, + uint16_t length, + bt_att_response_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy) +{ + struct att_send_op *op; + enum att_op_type op_type; + + if (length && !pdu) + return NULL; + + op_type = get_op_type(opcode); + if (op_type == ATT_OP_TYPE_UNKNOWN) + return NULL; + + /* If the opcode corresponds to an operation type that does not elicit a + * response from the remote end, then no callback should have been + * provided, since it will never be called. + */ + if (callback && op_type != ATT_OP_TYPE_REQ && op_type != ATT_OP_TYPE_IND) + return NULL; + + /* Similarly, if the operation does elicit a response then a callback + * must be provided. + */ + if (!callback && (op_type == ATT_OP_TYPE_REQ || op_type == ATT_OP_TYPE_IND)) + return NULL; + + op = new0(struct att_send_op, 1); + if (!op) + return NULL; + + op->type = op_type; + op->opcode = opcode; + op->callback = callback; + op->destroy = destroy; + op->user_data = user_data; + + if (!encode_pdu(att, op, pdu, length)) { + free(op); + return NULL; + } + + return op; +} + +static struct att_send_op *pick_next_send_op(struct bt_att *att) +{ + struct att_send_op *op; + + /* See if any operations are already in the write queue */ + op = queue_pop_head(att->write_queue); + if (op) + return op; + + /* If there is no pending request, pick an operation from the + * request queue. + */ + if (!att->pending_req) { + op = queue_pop_head(att->req_queue); + if (op) + return op; + } + + /* There is either a request pending or no requests queued. If there is + * no pending indication, pick an operation from the indication queue. + */ + if (!att->pending_ind) { + op = queue_pop_head(att->ind_queue); + if (op) + return op; + } + + return NULL; +} + +struct timeout_data { + struct bt_att *att; + unsigned int id; +}; + +static bool timeout_cb(void *user_data) +{ + struct timeout_data *timeout = user_data; + struct bt_att *att = timeout->att; + struct att_send_op *op = NULL; + + if (att->pending_req && att->pending_req->id == timeout->id) { + op = att->pending_req; + att->pending_req = NULL; + } else if (att->pending_ind && att->pending_ind->id == timeout->id) { + op = att->pending_ind; + att->pending_ind = NULL; + } + + if (!op) + return false; + + util_debug(att->debug_callback, att->debug_data, + "Operation timed out: 0x%02x", op->opcode); + + if (att->timeout_callback) + att->timeout_callback(op->id, op->opcode, att->timeout_data); + + op->timeout_id = 0; + destroy_att_send_op(op); + + /* + * Directly terminate the connection as required by the ATT protocol. + * This should trigger an io disconnect event which will clean up the + * io and notify the upper layer. + */ + io_shutdown(att->io); + + return false; +} + +static void write_watch_destroy(void *user_data) +{ + struct bt_att *att = user_data; + + att->writer_active = false; +} + +static bool can_write_data(struct io *io, void *user_data) +{ + struct bt_att *att = user_data; + struct att_send_op *op; + struct timeout_data *timeout; + ssize_t ret; + struct iovec iov; + + op = pick_next_send_op(att); + if (!op) + return false; + + iov.iov_base = op->pdu; + iov.iov_len = op->len; + + ret = io_send(io, &iov, 1); + if (ret < 0) { + util_debug(att->debug_callback, att->debug_data, + "write failed: %s", strerror(-ret)); + if (op->callback) + op->callback(BT_ATT_OP_ERROR_RSP, NULL, 0, + op->user_data); + + destroy_att_send_op(op); + return true; + } + + util_debug(att->debug_callback, att->debug_data, + "ATT op 0x%02x", op->opcode); + + util_hexdump('<', op->pdu, ret, att->debug_callback, att->debug_data); + + /* Based on the operation type, set either the pending request or the + * pending indication. If it came from the write queue, then there is + * no need to keep it around. + */ + switch (op->type) { + case ATT_OP_TYPE_REQ: + att->pending_req = op; + break; + case ATT_OP_TYPE_IND: + att->pending_ind = op; + break; + case ATT_OP_TYPE_RSP: + /* Set in_req to false to indicate that no request is pending */ + att->in_req = false; + + /* Fall through to the next case */ + case ATT_OP_TYPE_CMD: + case ATT_OP_TYPE_NOT: + case ATT_OP_TYPE_CONF: + case ATT_OP_TYPE_UNKNOWN: + default: + destroy_att_send_op(op); + return true; + } + + timeout = new0(struct timeout_data, 1); + if (!timeout) + return true; + + timeout->att = att; + timeout->id = op->id; + op->timeout_id = timeout_add(ATT_TIMEOUT_INTERVAL, timeout_cb, + timeout, free); + + /* Return true as there may be more operations ready to write. */ + return true; +} + +static void wakeup_writer(struct bt_att *att) +{ + if (att->writer_active) + return; + + /* Set the write handler only if there is anything that can be sent + * at all. + */ + if (queue_isempty(att->write_queue)) { + if ((att->pending_req || queue_isempty(att->req_queue)) && + (att->pending_ind || queue_isempty(att->ind_queue))) + return; + } + + if (!io_set_write_handler(att->io, can_write_data, att, + write_watch_destroy)) + return; + + att->writer_active = true; +} + +static void disconn_handler(void *data, void *user_data) +{ + struct att_disconn *disconn = data; + int err = PTR_TO_INT(user_data); + + if (disconn->removed) + return; + + if (disconn->callback) + disconn->callback(err, disconn->user_data); +} + +static bool disconnect_cb(struct io *io, void *user_data) +{ + struct bt_att *att = user_data; + int err; + socklen_t len; + + len = sizeof(err); + + if (getsockopt(att->fd, SOL_SOCKET, SO_ERROR, &err, &len) < 0) { + util_debug(att->debug_callback, att->debug_data, + "Failed to obtain disconnect error: %s", + strerror(errno)); + err = 0; + } + + util_debug(att->debug_callback, att->debug_data, + "Physical link disconnected: %s", + strerror(err)); + + io_destroy(att->io); + att->io = NULL; + + bt_att_cancel_all(att); + + bt_att_ref(att); + + queue_foreach(att->disconn_list, disconn_handler, INT_TO_PTR(err)); + + bt_att_unregister_all(att); + bt_att_unref(att); + + return false; +} + +static void handle_rsp(struct bt_att *att, uint8_t opcode, uint8_t *pdu, + ssize_t pdu_len) +{ + struct att_send_op *op = att->pending_req; + uint8_t req_opcode; + uint8_t rsp_opcode; + uint8_t *rsp_pdu = NULL; + uint16_t rsp_pdu_len = 0; + + /* + * If no request is pending, then the response is unexpected. Disconnect + * the bearer. + */ + if (!op) { + util_debug(att->debug_callback, att->debug_data, + "Received unexpected ATT response"); + io_shutdown(att->io); + return; + } + + /* + * If the received response doesn't match the pending request, or if + * the request is malformed, end the current request with failure. + */ + if (opcode == BT_ATT_OP_ERROR_RSP) { + if (pdu_len != 4) + goto fail; + + req_opcode = pdu[0]; + } else if (!(req_opcode = get_req_opcode(opcode))) + goto fail; + + if (req_opcode != op->opcode) + goto fail; + + rsp_opcode = opcode; + + if (pdu_len > 0) { + rsp_pdu = pdu; + rsp_pdu_len = pdu_len; + } + + goto done; + +fail: + util_debug(att->debug_callback, att->debug_data, + "Failed to handle response PDU; opcode: 0x%02x", opcode); + + rsp_opcode = BT_ATT_OP_ERROR_RSP; + +done: + if (op->callback) + op->callback(rsp_opcode, rsp_pdu, rsp_pdu_len, op->user_data); + + destroy_att_send_op(op); + att->pending_req = NULL; + + wakeup_writer(att); +} + +static void handle_conf(struct bt_att *att, uint8_t *pdu, ssize_t pdu_len) +{ + struct att_send_op *op = att->pending_ind; + + /* + * Disconnect the bearer if the confirmation is unexpected or the PDU is + * invalid. + */ + if (!op || pdu_len) { + util_debug(att->debug_callback, att->debug_data, + "Received unexpected/invalid ATT confirmation"); + io_shutdown(att->io); + return; + } + + if (op->callback) + op->callback(BT_ATT_OP_HANDLE_VAL_CONF, NULL, 0, op->user_data); + + destroy_att_send_op(op); + att->pending_ind = NULL; + + wakeup_writer(att); +} + +struct notify_data { + uint8_t opcode; + uint8_t *pdu; + ssize_t pdu_len; + bool handler_found; +}; + +static bool opcode_match(uint8_t opcode, uint8_t test_opcode) +{ + enum att_op_type op_type = get_op_type(test_opcode); + + if (opcode == BT_ATT_ALL_REQUESTS && (op_type == ATT_OP_TYPE_REQ || + op_type == ATT_OP_TYPE_CMD)) + return true; + + return opcode == test_opcode; +} + +static void respond_not_supported(struct bt_att *att, uint8_t opcode) +{ + uint8_t pdu[4]; + + pdu[0] = opcode; + pdu[1] = 0; + pdu[2] = 0; + pdu[3] = BT_ATT_ERROR_REQUEST_NOT_SUPPORTED; + + bt_att_send(att, BT_ATT_OP_ERROR_RSP, pdu, sizeof(pdu), NULL, NULL, + NULL); +} + +static bool handle_signed(struct bt_att *att, uint8_t opcode, uint8_t *pdu, + ssize_t pdu_len) +{ + uint8_t *signature; + uint32_t sign_cnt; + struct sign_info *sign; + + /* Check if there is enough data for a signature */ + if (pdu_len < 2 + BT_ATT_SIGNATURE_LEN) + goto fail; + + sign = att->remote_sign; + if (!sign) + goto fail; + + signature = pdu + (pdu_len - BT_ATT_SIGNATURE_LEN); + sign_cnt = get_le32(signature); + + /* Validate counter */ + if (!sign->counter(&sign_cnt, sign->user_data)) + goto fail; + + /* Generate signature and verify it */ + if (!bt_crypto_sign_att(att->crypto, sign->key, pdu, + pdu_len - BT_ATT_SIGNATURE_LEN, sign_cnt, + signature)) + goto fail; + + return true; + +fail: + util_debug(att->debug_callback, att->debug_data, + "ATT failed to verify signature: 0x%02x", opcode); + + return false; +} + +static void handle_notify(struct bt_att *att, uint8_t opcode, uint8_t *pdu, + ssize_t pdu_len) +{ + const struct queue_entry *entry; + bool found; + + if (opcode & ATT_OP_SIGNED_MASK) { + if (!handle_signed(att, opcode, pdu, pdu_len)) + return; + pdu_len -= BT_ATT_SIGNATURE_LEN; + } + + bt_att_ref(att); + + found = false; + entry = queue_get_entries(att->notify_list); + + while (entry) { + struct att_notify *notify = entry->data; + + entry = entry->next; + + if (!opcode_match(notify->opcode, opcode)) + continue; + + found = true; + + if (notify->callback) + notify->callback(opcode, pdu, pdu_len, + notify->user_data); + + /* callback could remove all entries from notify list */ + if (queue_isempty(att->notify_list)) + break; + } + + /* + * If this was a request and no handler was registered for it, respond + * with "Not Supported" + */ + if (!found && get_op_type(opcode) == ATT_OP_TYPE_REQ) + respond_not_supported(att, opcode); + + bt_att_unref(att); +} + +static bool can_read_data(struct io *io, void *user_data) +{ + struct bt_att *att = user_data; + uint8_t opcode; + uint8_t *pdu; + ssize_t bytes_read; + + bytes_read = read(att->fd, att->buf, att->mtu); + if (bytes_read < 0) + return false; + + util_hexdump('>', att->buf, bytes_read, + att->debug_callback, att->debug_data); + + if (bytes_read < ATT_MIN_PDU_LEN) + return true; + + pdu = att->buf; + opcode = pdu[0]; + + bt_att_ref(att); + + /* Act on the received PDU based on the opcode type */ + switch (get_op_type(opcode)) { + case ATT_OP_TYPE_RSP: + util_debug(att->debug_callback, att->debug_data, + "ATT response received: 0x%02x", opcode); + handle_rsp(att, opcode, pdu + 1, bytes_read - 1); + break; + case ATT_OP_TYPE_CONF: + util_debug(att->debug_callback, att->debug_data, + "ATT confirmation received: 0x%02x", opcode); + handle_conf(att, pdu + 1, bytes_read - 1); + break; + case ATT_OP_TYPE_REQ: + /* + * If a request is currently pending, then the sequential + * protocol was violated. Disconnect the bearer, which will + * promptly notify the upper layer via disconnect handlers. + */ + if (att->in_req) { + util_debug(att->debug_callback, att->debug_data, + "Received request while another is " + "pending: 0x%02x", opcode); + io_shutdown(att->io); + bt_att_unref(att); + + return false; + } + + att->in_req = true; + + /* Fall through to the next case */ + case ATT_OP_TYPE_CMD: + case ATT_OP_TYPE_NOT: + case ATT_OP_TYPE_UNKNOWN: + case ATT_OP_TYPE_IND: + default: + /* For all other opcodes notify the upper layer of the PDU and + * let them act on it. + */ + util_debug(att->debug_callback, att->debug_data, + "ATT PDU received: 0x%02x", opcode); + handle_notify(att, opcode, pdu + 1, bytes_read - 1); + break; + } + + bt_att_unref(att); + + return true; +} + +static bool is_io_l2cap_based(int fd) +{ + int domain; + int proto; + int err; + socklen_t len; + + domain = 0; + len = sizeof(domain); + err = getsockopt(fd, SOL_SOCKET, SO_DOMAIN, &domain, &len); + if (err < 0) + return false; + + if (domain != AF_BLUETOOTH) + return false; + + proto = 0; + len = sizeof(proto); + err = getsockopt(fd, SOL_SOCKET, SO_PROTOCOL, &proto, &len); + if (err < 0) + return false; + + return proto == BTPROTO_L2CAP; +} + +static void bt_att_free(struct bt_att *att) +{ + if (att->pending_req) + destroy_att_send_op(att->pending_req); + + if (att->pending_ind) + destroy_att_send_op(att->pending_ind); + + io_destroy(att->io); + bt_crypto_unref(att->crypto); + + queue_destroy(att->req_queue, NULL); + queue_destroy(att->ind_queue, NULL); + queue_destroy(att->write_queue, NULL); + queue_destroy(att->notify_list, NULL); + queue_destroy(att->disconn_list, NULL); + + if (att->timeout_destroy) + att->timeout_destroy(att->timeout_data); + + if (att->debug_destroy) + att->debug_destroy(att->debug_data); + + free(att->local_sign); + free(att->remote_sign); + + free(att->buf); + + free(att); +} + +struct bt_att *bt_att_new(int fd) +{ + struct bt_att *att; + + if (fd < 0) + return NULL; + + att = new0(struct bt_att, 1); + if (!att) + return NULL; + + att->fd = fd; + + att->mtu = BT_ATT_DEFAULT_LE_MTU; + att->buf = malloc(att->mtu); + if (!att->buf) + goto fail; + + att->io = io_new(fd); + if (!att->io) + goto fail; + + /* crypto is optional, if not available leave it NULL */ + att->crypto = bt_crypto_new(); + + att->req_queue = queue_new(); + if (!att->req_queue) + goto fail; + + att->ind_queue = queue_new(); + if (!att->ind_queue) + goto fail; + + att->write_queue = queue_new(); + if (!att->write_queue) + goto fail; + + att->notify_list = queue_new(); + if (!att->notify_list) + goto fail; + + att->disconn_list = queue_new(); + if (!att->disconn_list) + goto fail; + + if (!io_set_read_handler(att->io, can_read_data, att, NULL)) + goto fail; + + if (!io_set_disconnect_handler(att->io, disconnect_cb, att, NULL)) + goto fail; + + att->io_on_l2cap = is_io_l2cap_based(att->fd); + if (!att->io_on_l2cap) + att->io_sec_level = BT_SECURITY_LOW; + + return bt_att_ref(att); + +fail: + bt_att_free(att); + + return NULL; +} + +struct bt_att *bt_att_ref(struct bt_att *att) +{ + if (!att) + return NULL; + + __sync_fetch_and_add(&att->ref_count, 1); + + return att; +} + +void bt_att_unref(struct bt_att *att) +{ + if (!att) + return; + + if (__sync_sub_and_fetch(&att->ref_count, 1)) + return; + + bt_att_unregister_all(att); + bt_att_cancel_all(att); + + bt_att_free(att); +} + +bool bt_att_set_close_on_unref(struct bt_att *att, bool do_close) +{ + if (!att || !att->io) + return false; + + return io_set_close_on_destroy(att->io, do_close); +} + +int bt_att_get_fd(struct bt_att *att) +{ + if (!att) + return -1; + + return att->fd; +} + +bool bt_att_set_debug(struct bt_att *att, bt_att_debug_func_t callback, + void *user_data, bt_att_destroy_func_t destroy) +{ + if (!att) + return false; + + if (att->debug_destroy) + att->debug_destroy(att->debug_data); + + att->debug_callback = callback; + att->debug_destroy = destroy; + att->debug_data = user_data; + + return true; +} + +uint16_t bt_att_get_mtu(struct bt_att *att) +{ + if (!att) + return 0; + + return att->mtu; +} + +bool bt_att_set_mtu(struct bt_att *att, uint16_t mtu) +{ + void *buf; + + if (!att) + return false; + + if (mtu < BT_ATT_DEFAULT_LE_MTU) + return false; + + buf = malloc(mtu); + if (!buf) + return false; + + free(att->buf); + + att->mtu = mtu; + att->buf = buf; + + return true; +} + +bool bt_att_set_timeout_cb(struct bt_att *att, bt_att_timeout_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy) +{ + if (!att) + return false; + + if (att->timeout_destroy) + att->timeout_destroy(att->timeout_data); + + att->timeout_callback = callback; + att->timeout_destroy = destroy; + att->timeout_data = user_data; + + return true; +} + +unsigned int bt_att_register_disconnect(struct bt_att *att, + bt_att_disconnect_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy) +{ + struct att_disconn *disconn; + + if (!att || !att->io) + return 0; + + disconn = new0(struct att_disconn, 1); + if (!disconn) + return 0; + + disconn->callback = callback; + disconn->destroy = destroy; + disconn->user_data = user_data; + + if (att->next_reg_id < 1) + att->next_reg_id = 1; + + disconn->id = att->next_reg_id++; + + if (!queue_push_tail(att->disconn_list, disconn)) { + free(disconn); + return 0; + } + + return disconn->id; +} + +bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id) +{ + struct att_disconn *disconn; + + if (!att || !id) + return false; + + disconn = queue_remove_if(att->disconn_list, match_disconn_id, + UINT_TO_PTR(id)); + if (!disconn) + return false; + + destroy_att_disconn(disconn); + return true; +} + +unsigned int bt_att_send(struct bt_att *att, uint8_t opcode, + const void *pdu, uint16_t length, + bt_att_response_func_t callback, void *user_data, + bt_att_destroy_func_t destroy) +{ + struct att_send_op *op; + bool result; + + if (!att || !att->io) + return 0; + + op = create_att_send_op(att, opcode, pdu, length, callback, user_data, + destroy); + if (!op) + return 0; + + if (att->next_send_id < 1) + att->next_send_id = 1; + + op->id = att->next_send_id++; + + /* Add the op to the correct queue based on its type */ + switch (op->type) { + case ATT_OP_TYPE_REQ: + result = queue_push_tail(att->req_queue, op); + break; + case ATT_OP_TYPE_IND: + result = queue_push_tail(att->ind_queue, op); + break; + case ATT_OP_TYPE_CMD: + case ATT_OP_TYPE_NOT: + case ATT_OP_TYPE_UNKNOWN: + case ATT_OP_TYPE_RSP: + case ATT_OP_TYPE_CONF: + default: + result = queue_push_tail(att->write_queue, op); + break; + } + + if (!result) { + free(op->pdu); + free(op); + return 0; + } + + wakeup_writer(att); + + return op->id; +} + +static bool match_op_id(const void *a, const void *b) +{ + const struct att_send_op *op = a; + unsigned int id = PTR_TO_UINT(b); + + return op->id == id; +} + +bool bt_att_cancel(struct bt_att *att, unsigned int id) +{ + struct att_send_op *op; + + if (!att || !id) + return false; + + if (att->pending_req && att->pending_req->id == id) { + /* Don't cancel the pending request; remove it's handlers */ + cancel_att_send_op(att->pending_req); + return true; + } + + if (att->pending_ind && att->pending_ind->id == id) { + /* Don't cancel the pending indication; remove it's handlers */ + cancel_att_send_op(att->pending_ind); + return true; + } + + op = queue_remove_if(att->req_queue, match_op_id, UINT_TO_PTR(id)); + if (op) + goto done; + + op = queue_remove_if(att->ind_queue, match_op_id, UINT_TO_PTR(id)); + if (op) + goto done; + + op = queue_remove_if(att->write_queue, match_op_id, UINT_TO_PTR(id)); + if (op) + goto done; + + if (!op) + return false; + +done: + destroy_att_send_op(op); + + wakeup_writer(att); + + return true; +} + +bool bt_att_cancel_all(struct bt_att *att) +{ + if (!att) + return false; + + queue_remove_all(att->req_queue, NULL, NULL, destroy_att_send_op); + queue_remove_all(att->ind_queue, NULL, NULL, destroy_att_send_op); + queue_remove_all(att->write_queue, NULL, NULL, destroy_att_send_op); + + if (att->pending_req) + /* Don't cancel the pending request; remove it's handlers */ + cancel_att_send_op(att->pending_req); + + if (att->pending_ind) + /* Don't cancel the pending request; remove it's handlers */ + cancel_att_send_op(att->pending_ind); + + return true; +} + +static uint8_t att_ecode_from_error(int err) +{ + /* + * If the error fits in a single byte, treat it as an ATT protocol + * error as is. Since "0" is not a valid ATT protocol error code, we map + * that to UNLIKELY below. + */ + if (err > 0 && err < UINT8_MAX) + return err; + + /* + * Since we allow UNIX errnos, map them to appropriate ATT protocol + * and "Common Profile and Service" error codes. + */ + switch (err) { + case -ENOENT: + return BT_ATT_ERROR_INVALID_HANDLE; + case -ENOMEM: + return BT_ATT_ERROR_INSUFFICIENT_RESOURCES; + case -EALREADY: + return BT_ERROR_ALREADY_IN_PROGRESS; + case -EOVERFLOW: + return BT_ERROR_OUT_OF_RANGE; + } + + return BT_ATT_ERROR_UNLIKELY; +} + +unsigned int bt_att_send_error_rsp(struct bt_att *att, uint8_t opcode, + uint16_t handle, int error) +{ + struct bt_att_pdu_error_rsp pdu; + uint8_t ecode; + + if (!att || !opcode) + return 0; + + ecode = att_ecode_from_error(error); + + memset(&pdu, 0, sizeof(pdu)); + + pdu.opcode = opcode; + put_le16(handle, &pdu.handle); + pdu.ecode = ecode; + + return bt_att_send(att, BT_ATT_OP_ERROR_RSP, &pdu, sizeof(pdu), + NULL, NULL, NULL); +} + +unsigned int bt_att_register(struct bt_att *att, uint8_t opcode, + bt_att_notify_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy) +{ + struct att_notify *notify; + + if (!att || !callback || !att->io) + return 0; + + notify = new0(struct att_notify, 1); + if (!notify) + return 0; + + notify->opcode = opcode; + notify->callback = callback; + notify->destroy = destroy; + notify->user_data = user_data; + + if (att->next_reg_id < 1) + att->next_reg_id = 1; + + notify->id = att->next_reg_id++; + + if (!queue_push_tail(att->notify_list, notify)) { + free(notify); + return 0; + } + + return notify->id; +} + +bool bt_att_unregister(struct bt_att *att, unsigned int id) +{ + struct att_notify *notify; + + if (!att || !id) + return false; + + notify = queue_remove_if(att->notify_list, match_notify_id, + UINT_TO_PTR(id)); + if (!notify) + return false; + + destroy_att_notify(notify); + return true; +} + +bool bt_att_unregister_all(struct bt_att *att) +{ + if (!att) + return false; + + queue_remove_all(att->notify_list, NULL, NULL, destroy_att_notify); + queue_remove_all(att->disconn_list, NULL, NULL, destroy_att_disconn); + + return true; +} + +int bt_att_get_sec_level(struct bt_att *att) +{ + struct bt_security sec; + socklen_t len; + + if (!att) + return -EINVAL; + + if (!att->io_on_l2cap) + return att->io_sec_level; + + memset(&sec, 0, sizeof(sec)); + len = sizeof(sec); + if (getsockopt(att->fd, SOL_BLUETOOTH, BT_SECURITY, &sec, &len) < 0) + return -EIO; + + return sec.level; +} + +bool bt_att_set_sec_level(struct bt_att *att, int level) +{ + struct bt_security sec; + + if (!att || level < BT_SECURITY_LOW || level > BT_SECURITY_HIGH) + return false; + + if (!att->io_on_l2cap) { + att->io_sec_level = level; + return true; + } + + memset(&sec, 0, sizeof(sec)); + sec.level = level; + + if (setsockopt(att->fd, SOL_BLUETOOTH, BT_SECURITY, &sec, + sizeof(sec)) < 0) + return false; + + return true; +} + +static bool sign_set_key(struct sign_info **sign, uint8_t key[16], + bt_att_counter_func_t func, void *user_data) +{ + if (!(*sign)) { + *sign = new0(struct sign_info, 1); + if (!(*sign)) + return false; + } + + (*sign)->counter = func; + (*sign)->user_data = user_data; + memcpy((*sign)->key, key, 16); + + return true; +} + +bool bt_att_set_local_key(struct bt_att *att, uint8_t sign_key[16], + bt_att_counter_func_t func, void *user_data) +{ + if (!att) + return false; + + return sign_set_key(&att->local_sign, sign_key, func, user_data); +} + +bool bt_att_set_remote_key(struct bt_att *att, uint8_t sign_key[16], + bt_att_counter_func_t func, void *user_data) +{ + if (!att) + return false; + + return sign_set_key(&att->remote_sign, sign_key, func, user_data); +} + +bool bt_att_has_crypto(struct bt_att *att) +{ + if (!att) + return false; + + return att->crypto ? true : false; +} diff --git a/bluez/att.h b/bluez/att.h new file mode 100644 index 0000000..aa75c25 --- /dev/null +++ b/bluez/att.h @@ -0,0 +1,93 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 + +#include "att-types.h" + +struct bt_att; + +struct bt_att *bt_att_new(int fd); + +struct bt_att *bt_att_ref(struct bt_att *att); +void bt_att_unref(struct bt_att *att); + +bool bt_att_set_close_on_unref(struct bt_att *att, bool do_close); + +int bt_att_get_fd(struct bt_att *att); + +typedef void (*bt_att_response_func_t)(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data); +typedef void (*bt_att_notify_func_t)(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data); +typedef void (*bt_att_destroy_func_t)(void *user_data); +typedef void (*bt_att_debug_func_t)(const char *str, void *user_data); +typedef void (*bt_att_timeout_func_t)(unsigned int id, uint8_t opcode, + void *user_data); +typedef void (*bt_att_disconnect_func_t)(int err, void *user_data); +typedef bool (*bt_att_counter_func_t)(uint32_t *sign_cnt, void *user_data); + +bool bt_att_set_debug(struct bt_att *att, bt_att_debug_func_t callback, + void *user_data, bt_att_destroy_func_t destroy); + +uint16_t bt_att_get_mtu(struct bt_att *att); +bool bt_att_set_mtu(struct bt_att *att, uint16_t mtu); + +bool bt_att_set_timeout_cb(struct bt_att *att, bt_att_timeout_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy); + +unsigned int bt_att_send(struct bt_att *att, uint8_t opcode, + const void *pdu, uint16_t length, + bt_att_response_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy); +bool bt_att_cancel(struct bt_att *att, unsigned int id); +bool bt_att_cancel_all(struct bt_att *att); + +unsigned int bt_att_send_error_rsp(struct bt_att *att, uint8_t opcode, + uint16_t handle, int error); + +unsigned int bt_att_register(struct bt_att *att, uint8_t opcode, + bt_att_notify_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy); +bool bt_att_unregister(struct bt_att *att, unsigned int id); + +unsigned int bt_att_register_disconnect(struct bt_att *att, + bt_att_disconnect_func_t callback, + void *user_data, + bt_att_destroy_func_t destroy); +bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id); + +bool bt_att_unregister_all(struct bt_att *att); + +int bt_att_get_sec_level(struct bt_att *att); +bool bt_att_set_sec_level(struct bt_att *att, int level); + +bool bt_att_set_local_key(struct bt_att *att, uint8_t sign_key[16], + bt_att_counter_func_t func, void *user_data); +bool bt_att_set_remote_key(struct bt_att *att, uint8_t sign_key[16], + bt_att_counter_func_t func, void *user_data); +bool bt_att_has_crypto(struct bt_att *att); diff --git a/bluez/crypto.c b/bluez/crypto.c new file mode 100644 index 0000000..1f5a963 --- /dev/null +++ b/bluez/crypto.c @@ -0,0 +1,681 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 +#include +#include +#include + +#include "util.h" +#include "crypto.h" + +#ifndef HAVE_LINUX_IF_ALG_H +#ifndef HAVE_LINUX_TYPES_H +typedef uint8_t __u8; +typedef uint16_t __u16; +typedef uint32_t __u32; +#else +#include +#endif + +struct sockaddr_alg { + __u16 salg_family; + __u8 salg_type[14]; + __u32 salg_feat; + __u32 salg_mask; + __u8 salg_name[64]; +}; + +struct af_alg_iv { + __u32 ivlen; + __u8 iv[0]; +}; + +#define ALG_SET_KEY 1 +#define ALG_SET_IV 2 +#define ALG_SET_OP 3 + +#define ALG_OP_DECRYPT 0 +#define ALG_OP_ENCRYPT 1 + +#define PF_ALG 38 /* Algorithm sockets. */ +#define AF_ALG PF_ALG +#else +#include +#endif + +#ifndef SOL_ALG +#define SOL_ALG 279 +#endif + +/* Maximum message length that can be passed to aes_cmac */ +#define CMAC_MSG_MAX 80 + +struct bt_crypto { + int ref_count; + int ecb_aes; + int urandom; + int cmac_aes; +}; + +static int urandom_setup(void) +{ + int fd; + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) + return -1; + + return fd; +} + +static int ecb_aes_setup(void) +{ + struct sockaddr_alg salg; + int fd; + + fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0); + if (fd < 0) + return -1; + + memset(&salg, 0, sizeof(salg)); + salg.salg_family = AF_ALG; + strcpy((char *) salg.salg_type, "skcipher"); + strcpy((char *) salg.salg_name, "ecb(aes)"); + + if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) { + close(fd); + return -1; + } + + return fd; +} + +static int cmac_aes_setup(void) +{ + struct sockaddr_alg salg; + int fd; + + fd = socket(PF_ALG, SOCK_SEQPACKET | SOCK_CLOEXEC, 0); + if (fd < 0) + return -1; + + memset(&salg, 0, sizeof(salg)); + salg.salg_family = AF_ALG; + strcpy((char *) salg.salg_type, "hash"); + strcpy((char *) salg.salg_name, "cmac(aes)"); + + if (bind(fd, (struct sockaddr *) &salg, sizeof(salg)) < 0) { + close(fd); + return -1; + } + + return fd; +} + +struct bt_crypto *bt_crypto_new(void) +{ + struct bt_crypto *crypto; + + crypto = new0(struct bt_crypto, 1); + if (!crypto) + return NULL; + + crypto->ecb_aes = ecb_aes_setup(); + if (crypto->ecb_aes < 0) { + free(crypto); + return NULL; + } + + crypto->urandom = urandom_setup(); + if (crypto->urandom < 0) { + close(crypto->ecb_aes); + free(crypto); + return NULL; + } + + crypto->cmac_aes = cmac_aes_setup(); + if (crypto->cmac_aes < 0) { + close(crypto->urandom); + close(crypto->ecb_aes); + free(crypto); + return NULL; + } + + return bt_crypto_ref(crypto); +} + +struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto) +{ + if (!crypto) + return NULL; + + __sync_fetch_and_add(&crypto->ref_count, 1); + + return crypto; +} + +void bt_crypto_unref(struct bt_crypto *crypto) +{ + if (!crypto) + return; + + if (__sync_sub_and_fetch(&crypto->ref_count, 1)) + return; + + close(crypto->urandom); + close(crypto->ecb_aes); + close(crypto->cmac_aes); + + free(crypto); +} + +bool bt_crypto_random_bytes(struct bt_crypto *crypto, + uint8_t *buf, uint8_t num_bytes) +{ + ssize_t len; + + if (!crypto) + return false; + + len = read(crypto->urandom, buf, num_bytes); + if (len < num_bytes) + return false; + + return true; +} + +static int alg_new(int fd, const void *keyval, socklen_t keylen) +{ + if (setsockopt(fd, SOL_ALG, ALG_SET_KEY, keyval, keylen) < 0) + return -1; + + /* FIXME: This should use accept4() with SOCK_CLOEXEC */ + return accept(fd, NULL, 0); +} + +static bool alg_encrypt(int fd, const void *inbuf, size_t inlen, + void *outbuf, size_t outlen) +{ + __u32 alg_op = ALG_OP_ENCRYPT; + char cbuf[CMSG_SPACE(sizeof(alg_op))]; + struct cmsghdr *cmsg; + struct msghdr msg; + struct iovec iov; + ssize_t len; + + memset(cbuf, 0, sizeof(cbuf)); + memset(&msg, 0, sizeof(msg)); + + msg.msg_control = cbuf; + msg.msg_controllen = sizeof(cbuf); + + cmsg = CMSG_FIRSTHDR(&msg); + cmsg->cmsg_level = SOL_ALG; + cmsg->cmsg_type = ALG_SET_OP; + cmsg->cmsg_len = CMSG_LEN(sizeof(alg_op)); + memcpy(CMSG_DATA(cmsg), &alg_op, sizeof(alg_op)); + + iov.iov_base = (void *) inbuf; + iov.iov_len = inlen; + + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + + len = sendmsg(fd, &msg, 0); + if (len < 0) + return false; + + len = read(fd, outbuf, outlen); + if (len < 0) + return false; + + return true; +} + +static inline void swap_buf(const uint8_t *src, uint8_t *dst, uint16_t len) +{ + int i; + + for (i = 0; i < len; i++) + dst[len - 1 - i] = src[i]; +} + +bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16], + const uint8_t *m, uint16_t m_len, + uint32_t sign_cnt, uint8_t signature[12]) +{ + int fd; + int len; + uint8_t tmp[16], out[16]; + uint16_t msg_len = m_len + sizeof(uint32_t); + uint8_t msg[msg_len]; + uint8_t msg_s[msg_len]; + + if (!crypto) + return false; + + memset(msg, 0, msg_len); + memcpy(msg, m, m_len); + + /* Add sign_counter to the message */ + put_le32(sign_cnt, msg + m_len); + + /* The most significant octet of key corresponds to key[0] */ + swap_buf(key, tmp, 16); + + fd = alg_new(crypto->cmac_aes, tmp, 16); + if (fd < 0) + return false; + + /* Swap msg before signing */ + swap_buf(msg, msg_s, msg_len); + + len = send(fd, msg_s, msg_len, 0); + if (len < 0) { + close(fd); + return false; + } + + len = read(fd, out, 16); + if (len < 0) { + close(fd); + return false; + } + + close(fd); + + /* + * As to BT spec. 4.1 Vol[3], Part C, chapter 10.4.1 sign counter should + * be placed in the signature + */ + put_be32(sign_cnt, out + 8); + + /* + * The most significant octet of hash corresponds to out[0] - swap it. + * Then truncate in most significant bit first order to a length of + * 12 octets + */ + swap_buf(out, tmp, 16); + memcpy(signature, tmp + 4, 12); + + return true; +} +/* + * Security function e + * + * Security function e generates 128-bit encryptedData from a 128-bit key + * and 128-bit plaintextData using the AES-128-bit block cypher: + * + * encryptedData = e(key, plaintextData) + * + * The most significant octet of key corresponds to key[0], the most + * significant octet of plaintextData corresponds to in[0] and the + * most significant octet of encryptedData corresponds to out[0]. + * + */ +bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16], + const uint8_t plaintext[16], uint8_t encrypted[16]) +{ + uint8_t tmp[16], in[16], out[16]; + int fd; + + if (!crypto) + return false; + + /* The most significant octet of key corresponds to key[0] */ + swap_buf(key, tmp, 16); + + fd = alg_new(crypto->ecb_aes, tmp, 16); + if (fd < 0) + return false; + + + /* Most significant octet of plaintextData corresponds to in[0] */ + swap_buf(plaintext, in, 16); + + if (!alg_encrypt(fd, in, 16, out, 16)) { + close(fd); + return false; + } + + /* Most significant octet of encryptedData corresponds to out[0] */ + swap_buf(out, encrypted, 16); + + close(fd); + + return true; +} + +/* + * Random Address Hash function ah + * + * The random address hash function ah is used to generate a hash value + * that is used in resolvable private addresses. + * + * The following are inputs to the random address hash function ah: + * + * k is 128 bits + * r is 24 bits + * padding is 104 bits + * + * r is concatenated with padding to generate r' which is used as the + * 128-bit input parameter plaintextData to security function e: + * + * r' = padding || r + * + * The least significant octet of r becomes the least significant octet + * of r’ and the most significant octet of padding becomes the most + * significant octet of r'. + * + * For example, if the 24-bit value r is 0x423456 then r' is + * 0x00000000000000000000000000423456. + * + * The output of the random address function ah is: + * + * ah(k, r) = e(k, r') mod 2^24 + * + * The output of the security function e is then truncated to 24 bits by + * taking the least significant 24 bits of the output of e as the result + * of ah. + */ +bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16], + const uint8_t r[3], uint8_t hash[3]) +{ + uint8_t rp[16]; + uint8_t encrypted[16]; + + if (!crypto) + return false; + + /* r' = padding || r */ + memcpy(rp, r, 3); + memset(rp + 3, 0, 13); + + /* e(k, r') */ + if (!bt_crypto_e(crypto, k, rp, encrypted)) + return false; + + /* ah(k, r) = e(k, r') mod 2^24 */ + memcpy(hash, encrypted, 3); + + return true; +} + +typedef struct { + uint64_t a, b; +} u128; + +static inline void u128_xor(const uint8_t p[16], const uint8_t q[16], + uint8_t r[16]) +{ + u128 pp, qq, rr; + + memcpy(&pp, p, 16); + memcpy(&qq, q, 16); + + rr.a = pp.a ^ qq.a; + rr.b = pp.b ^ qq.b; + + memcpy(r, &rr, 16); +} + +/* + * Confirm value generation function c1 + * + * During the pairing process confirm values are exchanged. This confirm + * value generation function c1 is used to generate the confirm values. + * + * The following are inputs to the confirm value generation function c1: + * + * k is 128 bits + * r is 128 bits + * pres is 56 bits + * preq is 56 bits + * iat is 1 bit + * ia is 48 bits + * rat is 1 bit + * ra is 48 bits + * padding is 32 bits of 0 + * + * iat is concatenated with 7-bits of 0 to create iat' which is 8 bits + * in length. iat is the least significant bit of iat' + * + * rat is concatenated with 7-bits of 0 to create rat' which is 8 bits + * in length. rat is the least significant bit of rat' + * + * pres, preq, rat' and iat' are concatenated to generate p1 which is + * XORed with r and used as 128-bit input parameter plaintextData to + * security function e: + * + * p1 = pres || preq || rat' || iat' + * + * The octet of iat' becomes the least significant octet of p1 and the + * most significant octet of pres becomes the most significant octet of + * p1. + * + * ra is concatenated with ia and padding to generate p2 which is XORed + * with the result of the security function e using p1 as the input + * paremter plaintextData and is then used as the 128-bit input + * parameter plaintextData to security function e: + * + * p2 = padding || ia || ra + * + * The least significant octet of ra becomes the least significant octet + * of p2 and the most significant octet of padding becomes the most + * significant octet of p2. + * + * The output of the confirm value generation function c1 is: + * + * c1(k, r, preq, pres, iat, rat, ia, ra) = e(k, e(k, r XOR p1) XOR p2) + * + * The 128-bit output of the security function e is used as the result + * of confirm value generation function c1. + */ +bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16], + const uint8_t r[16], const uint8_t pres[7], + const uint8_t preq[7], uint8_t iat, + const uint8_t ia[6], uint8_t rat, + const uint8_t ra[6], uint8_t res[16]) +{ + uint8_t p1[16], p2[16]; + + /* p1 = pres || preq || _rat || _iat */ + p1[0] = iat; + p1[1] = rat; + memcpy(p1 + 2, preq, 7); + memcpy(p1 + 9, pres, 7); + + /* p2 = padding || ia || ra */ + memcpy(p2, ra, 6); + memcpy(p2 + 6, ia, 6); + memset(p2 + 12, 0, 4); + + /* res = r XOR p1 */ + u128_xor(r, p1, res); + + /* res = e(k, res) */ + if (!bt_crypto_e(crypto, k, res, res)) + return false; + + /* res = res XOR p2 */ + u128_xor(res, p2, res); + + /* res = e(k, res) */ + return bt_crypto_e(crypto, k, res, res); +} + +/* + * Key generation function s1 + * + * The key generation function s1 is used to generate the STK during the + * pairing process. + * + * The following are inputs to the key generation function s1: + * + * k is 128 bits + * r1 is 128 bits + * r2 is 128 bits + * + * The most significant 64-bits of r1 are discarded to generate r1' and + * the most significant 64-bits of r2 are discarded to generate r2'. + * + * r1' is concatenated with r2' to generate r' which is used as the + * 128-bit input parameter plaintextData to security function e: + * + * r' = r1' || r2' + * + * The least significant octet of r2' becomes the least significant + * octet of r' and the most significant octet of r1' becomes the most + * significant octet of r'. + * + * The output of the key generation function s1 is: + * + * s1(k, r1, r2) = e(k, r') + * + * The 128-bit output of the security function e is used as the result + * of key generation function s1. + */ +bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16], + const uint8_t r1[16], const uint8_t r2[16], + uint8_t res[16]) +{ + memcpy(res, r2, 8); + memcpy(res + 8, r1, 8); + + return bt_crypto_e(crypto, k, res, res); +} + +static bool aes_cmac(struct bt_crypto *crypto, uint8_t key[16], uint8_t *msg, + size_t msg_len, uint8_t res[16]) +{ + uint8_t key_msb[16], out[16], msg_msb[CMAC_MSG_MAX]; + ssize_t len; + int fd; + + if (msg_len > CMAC_MSG_MAX) + return false; + + swap_buf(key, key_msb, 16); + fd = alg_new(crypto->cmac_aes, key_msb, 16); + if (fd < 0) + return false; + + swap_buf(msg, msg_msb, msg_len); + len = send(fd, msg_msb, msg_len, 0); + if (len < 0) { + close(fd); + return false; + } + + len = read(fd, out, 16); + if (len < 0) { + close(fd); + return false; + } + + swap_buf(out, res, 16); + + close(fd); + + return true; +} + +bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32], + uint8_t x[16], uint8_t z, uint8_t res[16]) +{ + uint8_t m[65]; + + if (!crypto) + return false; + + m[0] = z; + memcpy(&m[1], v, 32); + memcpy(&m[33], u, 32); + + return aes_cmac(crypto, x, m, sizeof(m), res); +} + +bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16], + uint8_t n2[16], uint8_t a1[7], uint8_t a2[7], + uint8_t mackey[16], uint8_t ltk[16]) +{ + uint8_t btle[4] = { 0x65, 0x6c, 0x74, 0x62 }; + uint8_t salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60, + 0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c }; + uint8_t length[2] = { 0x00, 0x01 }; + uint8_t m[53], t[16]; + + if (!aes_cmac(crypto, salt, w, 32, t)) + return false; + + memcpy(&m[0], length, 2); + memcpy(&m[2], a2, 7); + memcpy(&m[9], a1, 7); + memcpy(&m[16], n2, 16); + memcpy(&m[32], n1, 16); + memcpy(&m[48], btle, 4); + + m[52] = 0; /* Counter */ + if (!aes_cmac(crypto, t, m, sizeof(m), mackey)) + return false; + + m[52] = 1; /* Counter */ + return aes_cmac(crypto, t, m, sizeof(m), ltk); +} + +bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16], + uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3], + uint8_t a1[7], uint8_t a2[7], uint8_t res[16]) +{ + uint8_t m[65]; + + memcpy(&m[0], a2, 7); + memcpy(&m[7], a1, 7); + memcpy(&m[14], io_cap, 3); + memcpy(&m[17], r, 16); + memcpy(&m[33], n2, 16); + memcpy(&m[49], n1, 16); + + return aes_cmac(crypto, w, m, sizeof(m), res); +} + +bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32], + uint8_t x[16], uint8_t y[16], uint32_t *val) +{ + uint8_t m[80], tmp[16]; + + memcpy(&m[0], y, 16); + memcpy(&m[16], v, 32); + memcpy(&m[48], u, 32); + + if (!aes_cmac(crypto, x, m, sizeof(m), tmp)) + return false; + + *val = get_le32(tmp); + *val %= 1000000; + + return true; +} diff --git a/bluez/crypto.h b/bluez/crypto.h new file mode 100644 index 0000000..9ba5803 --- /dev/null +++ b/bluez/crypto.h @@ -0,0 +1,61 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 + +struct bt_crypto; + +struct bt_crypto *bt_crypto_new(void); + +struct bt_crypto *bt_crypto_ref(struct bt_crypto *crypto); +void bt_crypto_unref(struct bt_crypto *crypto); + +bool bt_crypto_random_bytes(struct bt_crypto *crypto, + uint8_t *buf, uint8_t num_bytes); + +bool bt_crypto_e(struct bt_crypto *crypto, const uint8_t key[16], + const uint8_t plaintext[16], uint8_t encrypted[16]); +bool bt_crypto_ah(struct bt_crypto *crypto, const uint8_t k[16], + const uint8_t r[3], uint8_t hash[3]); +bool bt_crypto_c1(struct bt_crypto *crypto, const uint8_t k[16], + const uint8_t r[16], const uint8_t pres[7], + const uint8_t preq[7], uint8_t iat, + const uint8_t ia[6], uint8_t rat, + const uint8_t ra[6], uint8_t res[16]); +bool bt_crypto_s1(struct bt_crypto *crypto, const uint8_t k[16], + const uint8_t r1[16], const uint8_t r2[16], + uint8_t res[16]); +bool bt_crypto_f4(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32], + uint8_t x[16], uint8_t z, uint8_t res[16]); +bool bt_crypto_f5(struct bt_crypto *crypto, uint8_t w[32], uint8_t n1[16], + uint8_t n2[16], uint8_t a1[7], uint8_t a2[7], + uint8_t mackey[16], uint8_t ltk[16]); +bool bt_crypto_f6(struct bt_crypto *crypto, uint8_t w[16], uint8_t n1[16], + uint8_t n2[16], uint8_t r[16], uint8_t io_cap[3], + uint8_t a1[7], uint8_t a2[7], uint8_t res[16]); +bool bt_crypto_g2(struct bt_crypto *crypto, uint8_t u[32], uint8_t v[32], + uint8_t x[16], uint8_t y[16], uint32_t *val); +bool bt_crypto_sign_att(struct bt_crypto *crypto, const uint8_t key[16], + const uint8_t *m, uint16_t m_len, + uint32_t sign_cnt, uint8_t signature[12]); diff --git a/bluez/gatt-client.c b/bluez/gatt-client.c new file mode 100644 index 0000000..a569992 --- /dev/null +++ b/bluez/gatt-client.c @@ -0,0 +1,3013 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 "att.h" +#include +#include "uuid.h" +#include "gatt-helpers.h" +#include "util.h" +#include "queue.h" +#include "gatt-db.h" +#include "gatt-client.h" + +#include +#include +#include + +#ifndef MAX +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#endif + +#ifndef MIN +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#endif + +#define UUID_BYTES (BT_GATT_UUID_SIZE * sizeof(uint8_t)) + +#define GATT_SVC_UUID 0x1801 +#define SVC_CHNGD_UUID 0x2a05 + +struct bt_gatt_client { + struct bt_att *att; + int ref_count; + + bt_gatt_client_callback_t ready_callback; + bt_gatt_client_destroy_func_t ready_destroy; + void *ready_data; + + bt_gatt_client_service_changed_callback_t svc_chngd_callback; + bt_gatt_client_destroy_func_t svc_chngd_destroy; + void *svc_chngd_data; + + bt_gatt_client_debug_func_t debug_callback; + bt_gatt_client_destroy_func_t debug_destroy; + void *debug_data; + + struct gatt_db *db; + bool in_init; + bool ready; + + /* + * Queue of long write requests. An error during "prepare write" + * requests can result in a cancel through "execute write". To prevent + * cancelation of prepared writes to the wrong attribute and multiple + * requests to the same attribute that may result in a corrupted final + * value, we avoid interleaving prepared writes. + */ + struct queue *long_write_queue; + bool in_long_write; + + unsigned int reliable_write_session_id; + + /* List of registered disconnect/notification/indication callbacks */ + struct queue *notify_list; + struct queue *notify_chrcs; + int next_reg_id; + unsigned int disc_id, notify_id, ind_id; + + /* + * Handles of the GATT Service and the Service Changed characteristic + * value handle. These will have the value 0 if they are not present on + * the remote peripheral. + */ + unsigned int svc_chngd_ind_id; + bool svc_chngd_registered; + struct queue *svc_chngd_queue; /* Queued service changed events */ + bool in_svc_chngd; + + /* + * List of pending read/write operations. For operations that span + * across multiple PDUs, this list provides a mapping from an operation + * id to an ATT request id. + */ + struct queue *pending_requests; + unsigned int next_request_id; + + struct bt_gatt_request *discovery_req; + unsigned int mtu_req_id; +}; + +struct request { + struct bt_gatt_client *client; + bool long_write; + bool prep_write; + bool removed; + int ref_count; + unsigned int id; + unsigned int att_id; + void *data; + void (*destroy)(void *); +}; + +static struct request *request_ref(struct request *req) +{ + __sync_fetch_and_add(&req->ref_count, 1); + + return req; +} + +static struct request *request_create(struct bt_gatt_client *client) +{ + struct request *req; + + req = new0(struct request, 1); + if (!req) + return NULL; + + if (client->next_request_id < 1) + client->next_request_id = 1; + + queue_push_tail(client->pending_requests, req); + req->client = client; + req->id = client->next_request_id++; + + return request_ref(req); +} + +static void request_unref(void *data) +{ + struct request *req = data; + + if (__sync_sub_and_fetch(&req->ref_count, 1)) + return; + + if (req->destroy) + req->destroy(req->data); + + if (!req->removed) + queue_remove(req->client->pending_requests, req); + + free(req); +} + +struct notify_chrc { + uint16_t value_handle; + uint16_t ccc_handle; + uint16_t properties; + int notify_count; /* Reference count of registered notify callbacks */ + + /* Pending calls to register_notify are queued here so that they can be + * processed after a write that modifies the CCC descriptor. + */ + struct queue *reg_notify_queue; + unsigned int ccc_write_id; +}; + +struct notify_data { + struct bt_gatt_client *client; + unsigned int id; + unsigned int att_id; + int ref_count; + struct notify_chrc *chrc; + bt_gatt_client_register_callback_t callback; + bt_gatt_client_notify_callback_t notify; + void *user_data; + bt_gatt_client_destroy_func_t destroy; +}; + +static struct notify_data *notify_data_ref(struct notify_data *notify_data) +{ + __sync_fetch_and_add(¬ify_data->ref_count, 1); + + return notify_data; +} + +static void notify_data_unref(void *data) +{ + struct notify_data *notify_data = data; + + if (__sync_sub_and_fetch(¬ify_data->ref_count, 1)) + return; + + if (notify_data->destroy) + notify_data->destroy(notify_data->user_data); + + free(notify_data); +} + +static void find_ccc(struct gatt_db_attribute *attr, void *user_data) +{ + struct gatt_db_attribute **ccc_ptr = user_data; + bt_uuid_t uuid; + + if (*ccc_ptr) + return; + + bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID); + + if (bt_uuid_cmp(&uuid, gatt_db_attribute_get_type(attr))) + return; + + *ccc_ptr = attr; +} + +static struct notify_chrc *notify_chrc_create(struct bt_gatt_client *client, + uint16_t value_handle) +{ + struct gatt_db_attribute *attr, *ccc; + struct notify_chrc *chrc; + bt_uuid_t uuid; + uint8_t properties; + + /* Check that chrc_value_handle belongs to a known characteristic */ + attr = gatt_db_get_attribute(client->db, value_handle - 1); + if (!attr) + return NULL; + + bt_uuid16_create(&uuid, GATT_CHARAC_UUID); + if (bt_uuid_cmp(&uuid, gatt_db_attribute_get_type(attr))) + return NULL; + + if (!gatt_db_attribute_get_char_data(attr, NULL, NULL, + &properties, NULL)) + return NULL; + + chrc = new0(struct notify_chrc, 1); + if (!chrc) + return NULL; + + chrc->reg_notify_queue = queue_new(); + if (!chrc->reg_notify_queue) { + free(chrc); + return NULL; + } + + /* + * Find the CCC characteristic. Some characteristics that allow + * notifications may not have a CCC descriptor. We treat these as + * automatically successful. + */ + ccc = NULL; + gatt_db_service_foreach_desc(attr, find_ccc, &ccc); + if (ccc) + chrc->ccc_handle = gatt_db_attribute_get_handle(ccc); + + chrc->value_handle = value_handle; + chrc->properties = properties; + + queue_push_tail(client->notify_chrcs, chrc); + + return chrc; +} + +static void notify_chrc_free(void *data) +{ + struct notify_chrc *chrc = data; + + queue_destroy(chrc->reg_notify_queue, notify_data_unref); + free(chrc); +} + +static bool match_notify_data_id(const void *a, const void *b) +{ + const struct notify_data *notify_data = a; + unsigned int id = PTR_TO_UINT(b); + + return notify_data->id == id; +} + +struct handle_range { + uint16_t start; + uint16_t end; +}; + +static bool match_notify_data_handle_range(const void *a, const void *b) +{ + const struct notify_data *notify_data = a; + struct notify_chrc *chrc = notify_data->chrc; + const struct handle_range *range = b; + + return chrc->value_handle >= range->start && + chrc->value_handle <= range->end; +} + +static bool match_notify_chrc_handle_range(const void *a, const void *b) +{ + const struct notify_chrc *chrc = a; + const struct handle_range *range = b; + + return chrc->value_handle >= range->start && + chrc->value_handle <= range->end; +} + +static void gatt_client_remove_all_notify_in_range( + struct bt_gatt_client *client, + uint16_t start_handle, uint16_t end_handle) +{ + struct handle_range range; + + range.start = start_handle; + range.end = end_handle; + + queue_remove_all(client->notify_list, match_notify_data_handle_range, + &range, notify_data_unref); +} + +static void gatt_client_remove_notify_chrcs_in_range( + struct bt_gatt_client *client, + uint16_t start_handle, uint16_t end_handle) +{ + struct handle_range range; + + range.start = start_handle; + range.end = end_handle; + + queue_remove_all(client->notify_chrcs, match_notify_chrc_handle_range, + &range, notify_chrc_free); +} + +struct discovery_op; + +typedef void (*discovery_op_complete_func_t)(struct discovery_op *op, + bool success, + uint8_t att_ecode); +typedef void (*discovery_op_fail_func_t)(struct discovery_op *op); + +struct discovery_op { + struct bt_gatt_client *client; + struct queue *pending_svcs; + struct queue *pending_chrcs; + struct queue *tmp_queue; + struct gatt_db_attribute *cur_svc; + bool success; + uint16_t start; + uint16_t end; + int ref_count; + discovery_op_complete_func_t complete_func; + discovery_op_fail_func_t failure_func; +}; + +static void discovery_op_free(struct discovery_op *op) +{ + queue_destroy(op->pending_svcs, NULL); + queue_destroy(op->pending_chrcs, free); + queue_destroy(op->tmp_queue, NULL); + free(op); +} + +static struct discovery_op *discovery_op_create(struct bt_gatt_client *client, + uint16_t start, uint16_t end, + discovery_op_complete_func_t complete_func, + discovery_op_fail_func_t failure_func) +{ + struct discovery_op *op; + + op = new0(struct discovery_op, 1); + if (!op) + return NULL; + + op->pending_svcs = queue_new(); + if (!op->pending_svcs) + goto fail; + + op->pending_chrcs = queue_new(); + if (!op->pending_chrcs) + goto fail; + + op->tmp_queue = queue_new(); + if (!op->tmp_queue) + goto fail; + + op->client = client; + op->complete_func = complete_func; + op->failure_func = failure_func; + op->start = start; + op->end = end; + + return op; + +fail: + discovery_op_free(op); + return NULL; +} + +static struct discovery_op *discovery_op_ref(struct discovery_op *op) +{ + __sync_fetch_and_add(&op->ref_count, 1); + + return op; +} + +static void discovery_op_unref(void *data) +{ + struct discovery_op *op = data; + + if (__sync_sub_and_fetch(&op->ref_count, 1)) + return; + + if (!op->success) + op->failure_func(op); + + discovery_op_free(op); +} + +static void discovery_req_clear(struct bt_gatt_client *client) +{ + if (!client->discovery_req) + return; + + bt_gatt_request_unref(client->discovery_req); + client->discovery_req = NULL; +} + +static void discover_chrcs_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data); + +static void discover_incl_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, void *user_data) +{ + struct discovery_op *op = user_data; + struct bt_gatt_client *client = op->client; + struct bt_gatt_iter iter; + struct gatt_db_attribute *attr, *tmp; + uint16_t handle, start, end; + uint128_t u128; + bt_uuid_t uuid; + char uuid_str[MAX_LEN_UUID_STR]; + unsigned int includes_count, i; + + discovery_req_clear(client); + + if (!success) { + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) + goto next; + + goto failed; + } + + /* Get the currently processed service */ + attr = op->cur_svc; + if (!attr) + goto failed; + + if (!result || !bt_gatt_iter_init(&iter, result)) + goto failed; + + includes_count = bt_gatt_result_included_count(result); + if (includes_count == 0) + goto failed; + + util_debug(client->debug_callback, client->debug_data, + "Included services found: %u", + includes_count); + + for (i = 0; i < includes_count; i++) { + if (!bt_gatt_iter_next_included_service(&iter, &handle, &start, + &end, u128.data)) + break; + + bt_uuid128_create(&uuid, u128); + + /* Log debug message */ + bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str)); + util_debug(client->debug_callback, client->debug_data, + "handle: 0x%04x, start: 0x%04x, end: 0x%04x," + "uuid: %s", handle, start, end, uuid_str); + + tmp = gatt_db_get_attribute(client->db, start); + if (!tmp) + goto failed; + + tmp = gatt_db_service_add_included(attr, tmp); + if (!tmp) + goto failed; + + /* + * GATT requires that all include definitions precede + * characteristic declarations. Based on the order we're adding + * these entries, the correct handle must be assigned to the new + * attribute. + */ + if (gatt_db_attribute_get_handle(tmp) != handle) + goto failed; + } + +next: + /* Move on to the next service */ + attr = queue_pop_head(op->pending_svcs); + if (!attr) { + struct queue *tmp_queue; + + tmp_queue = op->pending_svcs; + op->pending_svcs = op->tmp_queue; + op->tmp_queue = tmp_queue; + + /* + * We have processed all include definitions. Move on to + * characteristics. + */ + attr = queue_pop_head(op->pending_svcs); + if (!attr) + goto failed; + + if (!gatt_db_attribute_get_service_handles(attr, &start, &end)) + goto failed; + + op->cur_svc = attr; + + client->discovery_req = bt_gatt_discover_characteristics( + client->att, + start, end, + discover_chrcs_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to start characteristic discovery"); + discovery_op_unref(op); + goto failed; + } + + queue_push_tail(op->tmp_queue, attr); + op->cur_svc = attr; + if (!gatt_db_attribute_get_service_handles(attr, &start, &end)) + goto failed; + + if (start == end) + goto next; + + client->discovery_req = bt_gatt_discover_included_services(client->att, + start, end, + discover_incl_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to start included discovery"); + discovery_op_unref(op); + +failed: + op->success = false; + op->complete_func(op, false, att_ecode); +} + +struct chrc { + uint16_t start_handle; + uint16_t end_handle; + uint16_t value_handle; + uint8_t properties; + bt_uuid_t uuid; +}; + +static void discover_descs_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data); + +static bool discover_descs(struct discovery_op *op, bool *discovering) +{ + struct bt_gatt_client *client = op->client; + struct gatt_db_attribute *attr; + struct chrc *chrc_data; + uint16_t desc_start; + + *discovering = false; + + while ((chrc_data = queue_pop_head(op->pending_chrcs))) { + attr = gatt_db_service_add_characteristic(op->cur_svc, + &chrc_data->uuid, 0, + chrc_data->properties, + NULL, NULL, NULL); + + if (!attr) + goto failed; + + if (gatt_db_attribute_get_handle(attr) != + chrc_data->value_handle) + goto failed; + + desc_start = chrc_data->value_handle + 1; + + if (desc_start > chrc_data->end_handle) { + free(chrc_data); + continue; + } + + client->discovery_req = bt_gatt_discover_descriptors( + client->att, desc_start, + chrc_data->end_handle, + discover_descs_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) { + *discovering = true; + goto done; + } + + util_debug(client->debug_callback, client->debug_data, + "Failed to start descriptor discovery"); + discovery_op_unref(op); + + goto failed; + } + +done: + free(chrc_data); + return true; + +failed: + free(chrc_data); + return false; +} + +static void discover_descs_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data) +{ + struct discovery_op *op = user_data; + struct bt_gatt_client *client = op->client; + struct bt_gatt_iter iter; + struct gatt_db_attribute *attr; + uint16_t handle, start, end; + uint128_t u128; + bt_uuid_t uuid; + char uuid_str[MAX_LEN_UUID_STR]; + unsigned int desc_count; + bool discovering; + + discovery_req_clear(client); + + if (!success) { + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) { + success = true; + goto next; + } + + goto done; + } + + if (!result || !bt_gatt_iter_init(&iter, result)) + goto failed; + + desc_count = bt_gatt_result_descriptor_count(result); + if (desc_count == 0) + goto failed; + + util_debug(client->debug_callback, client->debug_data, + "Descriptors found: %u", desc_count); + + while (bt_gatt_iter_next_descriptor(&iter, &handle, u128.data)) { + bt_uuid128_create(&uuid, u128); + + /* Log debug message */ + bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str)); + util_debug(client->debug_callback, client->debug_data, + "handle: 0x%04x, uuid: %s", + handle, uuid_str); + + attr = gatt_db_service_add_descriptor(op->cur_svc, &uuid, 0, + NULL, NULL, NULL); + if (!attr) + goto failed; + + if (gatt_db_attribute_get_handle(attr) != handle) + goto failed; + } + + if (!discover_descs(op, &discovering)) + goto failed; + + if (discovering) + return; + +next: + /* Done with the current service */ + gatt_db_service_set_active(op->cur_svc, true); + + attr = queue_pop_head(op->pending_svcs); + if (!attr) + goto done; + + if (!gatt_db_attribute_get_service_handles(attr, &start, &end)) + goto failed; + + if (start == end) + goto next; + + /* Move on to the next service */ + op->cur_svc = attr; + + client->discovery_req = bt_gatt_discover_characteristics(client->att, + start, end, + discover_chrcs_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to start characteristic discovery"); + discovery_op_unref(op); + +failed: + success = false; + +done: + op->success = success; + op->complete_func(op, success, att_ecode); +} + +static void discover_chrcs_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data) +{ + struct discovery_op *op = user_data; + struct bt_gatt_client *client = op->client; + struct bt_gatt_iter iter; + struct gatt_db_attribute *attr; + struct chrc *chrc_data; + uint16_t start, end, value; + uint8_t properties; + uint128_t u128; + bt_uuid_t uuid; + char uuid_str[MAX_LEN_UUID_STR]; + unsigned int chrc_count; + bool discovering; + + discovery_req_clear(client); + + if (!success) { + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) { + success = true; + goto next; + } + + goto done; + } + + if (!op->cur_svc || !result || !bt_gatt_iter_init(&iter, result)) + goto failed; + + chrc_count = bt_gatt_result_characteristic_count(result); + util_debug(client->debug_callback, client->debug_data, + "Characteristics found: %u", chrc_count); + + if (chrc_count == 0) + goto failed; + + while (bt_gatt_iter_next_characteristic(&iter, &start, &end, &value, + &properties, u128.data)) { + bt_uuid128_create(&uuid, u128); + + /* Log debug message */ + bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str)); + util_debug(client->debug_callback, client->debug_data, + "start: 0x%04x, end: 0x%04x, value: 0x%04x, " + "props: 0x%02x, uuid: %s", + start, end, value, properties, uuid_str); + + chrc_data = new0(struct chrc, 1); + if (!chrc_data) + goto failed; + + chrc_data->start_handle = start; + chrc_data->end_handle = end; + chrc_data->value_handle = value; + chrc_data->properties = properties; + chrc_data->uuid = uuid; + + queue_push_tail(op->pending_chrcs, chrc_data); + } + + /* + * Sequentially discover descriptors for each characteristic and insert + * the characteristics into the database as we proceed. + */ + if (!discover_descs(op, &discovering)) + goto failed; + + if (discovering) + return; + +next: + /* Done with the current service */ + gatt_db_service_set_active(op->cur_svc, true); + + attr = queue_pop_head(op->pending_svcs); + if (!attr) + goto done; + + if (!gatt_db_attribute_get_service_handles(attr, &start, &end)) + goto failed; + + if (start == end) + goto next; + + /* Move on to the next service */ + op->cur_svc = attr; + + client->discovery_req = bt_gatt_discover_characteristics(client->att, + start, end, + discover_chrcs_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to start characteristic discovery"); + discovery_op_unref(op); + +failed: + success = false; + +done: + op->success = success; + op->complete_func(op, success, att_ecode); +} + +static void discover_secondary_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data) +{ + struct discovery_op *op = user_data; + struct bt_gatt_client *client = op->client; + struct bt_gatt_iter iter; + struct gatt_db_attribute *attr; + uint16_t start, end; + uint128_t u128; + bt_uuid_t uuid; + char uuid_str[MAX_LEN_UUID_STR]; + + discovery_req_clear(client); + + if (!success) { + util_debug(client->debug_callback, client->debug_data, + "Secondary service discovery failed." + " ATT ECODE: 0x%02x", att_ecode); + switch (att_ecode) { + case BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND: + case BT_ATT_ERROR_UNSUPPORTED_GROUP_TYPE: + goto next; + default: + goto done; + } + } + + if (!result || !bt_gatt_iter_init(&iter, result)) { + success = false; + goto done; + } + + util_debug(client->debug_callback, client->debug_data, + "Secondary services found: %u", + bt_gatt_result_service_count(result)); + + while (bt_gatt_iter_next_service(&iter, &start, &end, u128.data)) { + bt_uuid128_create(&uuid, u128); + + /* Log debug message */ + bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str)); + util_debug(client->debug_callback, client->debug_data, + "start: 0x%04x, end: 0x%04x, uuid: %s", + start, end, uuid_str); + + /* Store the service */ + attr = gatt_db_insert_service(client->db, start, &uuid, false, + end - start + 1); + if (!attr) { + util_debug(client->debug_callback, client->debug_data, + "Failed to create service"); + success = false; + goto done; + } + + queue_push_tail(op->pending_svcs, attr); + } + +next: + /* Sequentially discover included services */ + attr = queue_pop_head(op->pending_svcs); + + /* Complete with success if queue is empty */ + if (!attr) + goto done; + + /* + * Store the service in the tmp queue to be reused during + * characteristics discovery later. + */ + queue_push_tail(op->tmp_queue, attr); + op->cur_svc = attr; + + if (!gatt_db_attribute_get_service_handles(attr, &start, &end)) { + success = false; + goto done; + } + + client->discovery_req = bt_gatt_discover_included_services(client->att, + start, end, + discover_incl_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to start included services discovery"); + discovery_op_unref(op); + +done: + op->success = success; + op->complete_func(op, success, att_ecode); +} + +static void discover_primary_cb(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data) +{ + struct discovery_op *op = user_data; + struct bt_gatt_client *client = op->client; + struct bt_gatt_iter iter; + struct gatt_db_attribute *attr; + uint16_t start, end; + uint128_t u128; + bt_uuid_t uuid; + char uuid_str[MAX_LEN_UUID_STR]; + + discovery_req_clear(client); + + if (!success) { + util_debug(client->debug_callback, client->debug_data, + "Primary service discovery failed." + " ATT ECODE: 0x%02x", att_ecode); + goto secondary; + } + + if (!result || !bt_gatt_iter_init(&iter, result)) { + success = false; + goto done; + } + + util_debug(client->debug_callback, client->debug_data, + "Primary services found: %u", + bt_gatt_result_service_count(result)); + + while (bt_gatt_iter_next_service(&iter, &start, &end, u128.data)) { + bt_uuid128_create(&uuid, u128); + + /* Log debug message. */ + bt_uuid_to_string(&uuid, uuid_str, sizeof(uuid_str)); + util_debug(client->debug_callback, client->debug_data, + "start: 0x%04x, end: 0x%04x, uuid: %s", + start, end, uuid_str); + + attr = gatt_db_insert_service(client->db, start, &uuid, true, + end - start + 1); + if (!attr) { + util_debug(client->debug_callback, client->debug_data, + "Failed to store service"); + success = false; + goto done; + } + + queue_push_tail(op->pending_svcs, attr); + } + +secondary: + /* Discover secondary services */ + client->discovery_req = bt_gatt_discover_secondary_services(client->att, + NULL, op->start, op->end, + discover_secondary_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to start secondary service discovery"); + discovery_op_unref(op); + success = false; + +done: + op->success = success; + op->complete_func(op, success, att_ecode); +} + +static void notify_client_ready(struct bt_gatt_client *client, bool success, + uint8_t att_ecode) +{ + if (!client->ready_callback) + return; + + bt_gatt_client_ref(client); + client->ready_callback(success, att_ecode, client->ready_data); + bt_gatt_client_unref(client); +} + +static void exchange_mtu_cb(bool success, uint8_t att_ecode, void *user_data) +{ + struct discovery_op *op = user_data; + struct bt_gatt_client *client = op->client; + + op->success = success; + client->mtu_req_id = 0; + + if (!success) { + util_debug(client->debug_callback, client->debug_data, + "MTU Exchange failed. ATT ECODE: 0x%02x", + att_ecode); + + client->in_init = false; + notify_client_ready(client, success, att_ecode); + + return; + } + + util_debug(client->debug_callback, client->debug_data, + "MTU exchange complete, with MTU: %u", + bt_att_get_mtu(client->att)); + + /* Don't do discovery if the database was pre-populated */ + if (!gatt_db_isempty(client->db)) { + op->complete_func(op, true, 0); + return; + } + + client->discovery_req = bt_gatt_discover_all_primary_services( + client->att, NULL, + discover_primary_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to initiate primary service discovery"); + + client->in_init = false; + notify_client_ready(client, false, att_ecode); + + discovery_op_unref(op); +} + +struct service_changed_op { + struct bt_gatt_client *client; + uint16_t start_handle; + uint16_t end_handle; +}; + +static void service_changed_reregister_cb(uint16_t att_ecode, void *user_data) +{ + struct bt_gatt_client *client = user_data; + + if (!att_ecode) { + util_debug(client->debug_callback, client->debug_data, + "Re-registered handler for \"Service Changed\" after " + "change in GATT service"); + client->svc_chngd_registered = true; + return; + } + + util_debug(client->debug_callback, client->debug_data, + "Failed to register handler for \"Service Changed\""); + client->svc_chngd_ind_id = 0; +} + +static void process_service_changed(struct bt_gatt_client *client, + uint16_t start_handle, + uint16_t end_handle); +static void service_changed_cb(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data); + +static void get_first_attribute(struct gatt_db_attribute *attrib, + void *user_data) +{ + struct gatt_db_attribute **stored = user_data; + + if (*stored) + return; + + *stored = attrib; +} + +static void service_changed_complete(struct discovery_op *op, bool success, + uint8_t att_ecode) +{ + struct bt_gatt_client *client = op->client; + struct service_changed_op *next_sc_op; + uint16_t start_handle = op->start; + uint16_t end_handle = op->end; + struct gatt_db_attribute *attr = NULL; + bt_uuid_t uuid; + + client->in_svc_chngd = false; + + if (!success && att_ecode != BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND) { + util_debug(client->debug_callback, client->debug_data, + "Failed to discover services within changed range - " + "error: 0x%02x", att_ecode); + + gatt_db_clear_range(client->db, start_handle, end_handle); + } + + /* Notify the upper layer of changed services */ + if (client->svc_chngd_callback) + client->svc_chngd_callback(start_handle, end_handle, + client->svc_chngd_data); + + /* Process any queued events */ + next_sc_op = queue_pop_head(client->svc_chngd_queue); + if (next_sc_op) { + process_service_changed(client, next_sc_op->start_handle, + next_sc_op->end_handle); + free(next_sc_op); + return; + } + + bt_uuid16_create(&uuid, SVC_CHNGD_UUID); + + gatt_db_find_by_type(client->db, start_handle, end_handle, &uuid, + get_first_attribute, &attr); + if (!attr) + return; + + /* The GATT service was modified. Re-register the handler for + * indications from the "Service Changed" characteristic. + */ + client->svc_chngd_registered = false; + client->svc_chngd_ind_id = bt_gatt_client_register_notify(client, + gatt_db_attribute_get_handle(attr), + service_changed_reregister_cb, + service_changed_cb, + client, NULL); + if (client->svc_chngd_ind_id) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to re-register handler for \"Service Changed\""); +} + +static void service_changed_failure(struct discovery_op *op) +{ + struct bt_gatt_client *client = op->client; + + gatt_db_clear_range(client->db, op->start, op->end); +} + +static void process_service_changed(struct bt_gatt_client *client, + uint16_t start_handle, + uint16_t end_handle) +{ + struct discovery_op *op; + + /* Invalidate and remove all effected notify callbacks */ + gatt_client_remove_all_notify_in_range(client, start_handle, + end_handle); + gatt_client_remove_notify_chrcs_in_range(client, start_handle, + end_handle); + + /* Remove all services that overlap the modified range since we'll + * rediscover them + */ + gatt_db_clear_range(client->db, start_handle, end_handle); + + op = discovery_op_create(client, start_handle, end_handle, + service_changed_complete, + service_changed_failure); + if (!op) + goto fail; + + client->discovery_req = bt_gatt_discover_primary_services(client->att, + NULL, start_handle, end_handle, + discover_primary_cb, + discovery_op_ref(op), + discovery_op_unref); + if (client->discovery_req) { + client->in_svc_chngd = true; + return; + } + + discovery_op_free(op); + +fail: + util_debug(client->debug_callback, client->debug_data, + "Failed to initiate service discovery" + " after Service Changed"); +} + +static void service_changed_cb(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + struct bt_gatt_client *client = user_data; + struct service_changed_op *op; + uint16_t start, end; + + if (length != 4) + return; + + start = get_le16(value); + end = get_le16(value + 2); + + if (start > end) { + util_debug(client->debug_callback, client->debug_data, + "Service Changed received with invalid handles"); + return; + } + + util_debug(client->debug_callback, client->debug_data, + "Service Changed received - start: 0x%04x end: 0x%04x", + start, end); + + if (!client->in_svc_chngd) { + process_service_changed(client, start, end); + return; + } + + op = new0(struct service_changed_op, 1); + if (!op) + return; + + op->start_handle = start; + op->end_handle = end; + + queue_push_tail(client->svc_chngd_queue, op); +} + +static void service_changed_register_cb(uint16_t att_ecode, void *user_data) +{ + bool success; + struct bt_gatt_client *client = user_data; + + if (att_ecode) { + util_debug(client->debug_callback, client->debug_data, + "Failed to register handler for \"Service Changed\""); + success = false; + client->svc_chngd_ind_id = 0; + goto done; + } + + client->svc_chngd_registered = true; + client->ready = true; + success = true; + util_debug(client->debug_callback, client->debug_data, + "Registered handler for \"Service Changed\": %u", + client->svc_chngd_ind_id); + +done: + notify_client_ready(client, success, att_ecode); +} + +static void init_complete(struct discovery_op *op, bool success, + uint8_t att_ecode) +{ + struct bt_gatt_client *client = op->client; + struct gatt_db_attribute *attr = NULL; + bt_uuid_t uuid; + + client->in_init = false; + + if (!success) + goto fail; + + bt_uuid16_create(&uuid, SVC_CHNGD_UUID); + + gatt_db_find_by_type(client->db, 0x0001, 0xffff, &uuid, + get_first_attribute, &attr); + if (!attr) { + client->ready = true; + goto done; + } + + /* Register an indication handler for the "Service Changed" + * characteristic and report ready only if the handler is registered + * successfully. Temporarily set "ready" to true so that we can register + * the handler using the existing framework. + */ + client->ready = true; + client->svc_chngd_ind_id = bt_gatt_client_register_notify(client, + gatt_db_attribute_get_handle(attr), + service_changed_register_cb, + service_changed_cb, + client, NULL); + + if (!client->svc_chngd_registered) + client->ready = false; + + if (client->svc_chngd_ind_id) + return; + + util_debug(client->debug_callback, client->debug_data, + "Failed to register handler for \"Service Changed\""); + success = false; + +fail: + util_debug(client->debug_callback, client->debug_data, + "Failed to initialize gatt-client"); + + op->success = false; + +done: + notify_client_ready(client, success, att_ecode); +} + +static void init_fail(struct discovery_op *op) +{ + struct bt_gatt_client *client = op->client; + + gatt_db_clear(client->db); +} + +static bool gatt_client_init(struct bt_gatt_client *client, uint16_t mtu) +{ + struct discovery_op *op; + + if (client->in_init || client->ready) + return false; + + op = discovery_op_create(client, 0x0001, 0xffff, init_complete, + init_fail); + if (!op) + return false; + + /* Configure the MTU */ + client->mtu_req_id = bt_gatt_exchange_mtu(client->att, + MAX(BT_ATT_DEFAULT_LE_MTU, mtu), + exchange_mtu_cb, + discovery_op_ref(op), + discovery_op_unref); + if (!client->mtu_req_id) { + discovery_op_free(op); + return false; + } + + client->in_init = true; + + return true; +} + +struct pdu_data { + const void *pdu; + uint16_t length; +}; + +static void complete_notify_request(void *data) +{ + struct notify_data *notify_data = data; + + /* Increment the per-characteristic ref count of notify handlers */ + __sync_fetch_and_add(¬ify_data->chrc->notify_count, 1); + + notify_data->att_id = 0; + notify_data->callback(0, notify_data->user_data); +} + +static bool notify_data_write_ccc(struct notify_data *notify_data, bool enable, + bt_att_response_func_t callback) +{ + uint8_t pdu[4]; + unsigned int att_id; + + assert(notify_data->chrc->ccc_handle); + memset(pdu, 0, sizeof(pdu)); + put_le16(notify_data->chrc->ccc_handle, pdu); + + if (enable) { + /* Try to enable notifications and/or indications based on + * whatever the characteristic supports. + */ + if (notify_data->chrc->properties & BT_GATT_CHRC_PROP_NOTIFY) + pdu[2] = 0x01; + + if (notify_data->chrc->properties & BT_GATT_CHRC_PROP_INDICATE) + pdu[2] |= 0x02; + + if (!pdu[2]) + return false; + } + + att_id = bt_att_send(notify_data->client->att, BT_ATT_OP_WRITE_REQ, + pdu, sizeof(pdu), callback, + notify_data_ref(notify_data), + notify_data_unref); + notify_data->chrc->ccc_write_id = notify_data->att_id = att_id; + + return !!att_id; +} + +static uint8_t process_error(const void *pdu, uint16_t length) +{ + const struct bt_att_pdu_error_rsp *error_pdu; + + if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp)) + return 0; + + error_pdu = pdu; + + return error_pdu->ecode; +} + +static void enable_ccc_callback(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct notify_data *notify_data = user_data; + uint16_t att_ecode; + + assert(!notify_data->chrc->notify_count); + assert(notify_data->chrc->ccc_write_id); + + notify_data->chrc->ccc_write_id = 0; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + att_ecode = process_error(pdu, length); + + /* Failed to enable. Complete the current request and move on to + * the next one in the queue. If there was an error sending the + * write request, then just move on to the next queued entry. + */ + queue_remove(notify_data->client->notify_list, notify_data); + notify_data->callback(att_ecode, notify_data->user_data); + + while ((notify_data = queue_pop_head( + notify_data->chrc->reg_notify_queue))) { + + if (notify_data_write_ccc(notify_data, true, + enable_ccc_callback)) + return; + } + + return; + } + + /* Success! Report success for all remaining requests. */ + bt_gatt_client_ref(notify_data->client); + + complete_notify_request(notify_data); + queue_remove_all(notify_data->chrc->reg_notify_queue, NULL, NULL, + complete_notify_request); + + bt_gatt_client_unref(notify_data->client); +} + +static void disable_ccc_callback(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct notify_data *notify_data = user_data; + struct notify_data *next_data; + + assert(!notify_data->chrc->notify_count); + assert(notify_data->chrc->ccc_write_id); + + notify_data->chrc->ccc_write_id = 0; + + /* This is a best effort procedure, so ignore errors and process any + * queued requests. + */ + while (1) { + next_data = queue_pop_head(notify_data->chrc->reg_notify_queue); + if (!next_data || notify_data_write_ccc(notify_data, true, + enable_ccc_callback)) + return; + } +} + +static void complete_unregister_notify(void *data) +{ + struct notify_data *notify_data = data; + + /* + * If a procedure to enable the CCC is still pending, then cancel it and + * return. + */ + if (notify_data->att_id) { + bt_att_cancel(notify_data->client->att, notify_data->att_id); + goto done; + } + + if (__sync_sub_and_fetch(¬ify_data->chrc->notify_count, 1) || + !notify_data->chrc->ccc_handle) + goto done; + + if (notify_data_write_ccc(notify_data, false, disable_ccc_callback)) + return; + +done: + notify_data_unref(notify_data); +} + +static void notify_handler(void *data, void *user_data) +{ + struct notify_data *notify_data = data; + struct pdu_data *pdu_data = user_data; + uint16_t value_handle; + const uint8_t *value = NULL; + + value_handle = get_le16(pdu_data->pdu); + + if (notify_data->chrc->value_handle != value_handle) + return; + + if (pdu_data->length > 2) + value = pdu_data->pdu + 2; + + /* + * Even if the notify data has a pending ATT request to write to the + * CCC, there is really no reason not to notify the handlers. + */ + if (notify_data->notify) + notify_data->notify(value_handle, value, pdu_data->length - 2, + notify_data->user_data); +} + +static void notify_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct bt_gatt_client *client = user_data; + struct pdu_data pdu_data; + + bt_gatt_client_ref(client); + + memset(&pdu_data, 0, sizeof(pdu_data)); + pdu_data.pdu = pdu; + pdu_data.length = length; + + queue_foreach(client->notify_list, notify_handler, &pdu_data); + + if (opcode == BT_ATT_OP_HANDLE_VAL_IND) + bt_att_send(client->att, BT_ATT_OP_HANDLE_VAL_CONF, NULL, 0, + NULL, NULL, NULL); + + bt_gatt_client_unref(client); +} + +static void notify_data_cleanup(void *data) +{ + struct notify_data *notify_data = data; + + if (notify_data->att_id) + bt_att_cancel(notify_data->client->att, notify_data->att_id); + + notify_data_unref(notify_data); +} + +static void bt_gatt_client_free(struct bt_gatt_client *client) +{ + bt_gatt_client_cancel_all(client); + + queue_destroy(client->notify_list, notify_data_cleanup); + + if (client->ready_destroy) + client->ready_destroy(client->ready_data); + + if (client->debug_destroy) + client->debug_destroy(client->debug_data); + + if (client->att) { + bt_att_unregister_disconnect(client->att, client->disc_id); + bt_att_unregister(client->att, client->notify_id); + bt_att_unregister(client->att, client->ind_id); + bt_att_unref(client->att); + } + + gatt_db_unref(client->db); + + queue_destroy(client->svc_chngd_queue, free); + queue_destroy(client->long_write_queue, request_unref); + queue_destroy(client->notify_chrcs, notify_chrc_free); + queue_destroy(client->pending_requests, request_unref); + + free(client); +} + +static void att_disconnect_cb(int err, void *user_data) +{ + struct bt_gatt_client *client = user_data; + bool in_init = client->in_init; + + client->disc_id = 0; + + bt_att_unref(client->att); + client->att = NULL; + + client->in_init = false; + client->ready = false; + + if (in_init) + notify_client_ready(client, false, 0); +} + +struct bt_gatt_client *bt_gatt_client_new(struct gatt_db *db, + struct bt_att *att, + uint16_t mtu) +{ + struct bt_gatt_client *client; + + if (!att || !db) + return NULL; + + client = new0(struct bt_gatt_client, 1); + if (!client) + return NULL; + + client->disc_id = bt_att_register_disconnect(att, att_disconnect_cb, + client, NULL); + if (!client->disc_id) + goto fail; + + client->long_write_queue = queue_new(); + if (!client->long_write_queue) + goto fail; + + client->svc_chngd_queue = queue_new(); + if (!client->svc_chngd_queue) + goto fail; + + client->notify_list = queue_new(); + if (!client->notify_list) + goto fail; + + client->notify_chrcs = queue_new(); + if (!client->notify_chrcs) + goto fail; + + client->pending_requests = queue_new(); + if (!client->pending_requests) + goto fail; + + client->notify_id = bt_att_register(att, BT_ATT_OP_HANDLE_VAL_NOT, + notify_cb, client, NULL); + if (!client->notify_id) + goto fail; + + client->ind_id = bt_att_register(att, BT_ATT_OP_HANDLE_VAL_IND, + notify_cb, client, NULL); + if (!client->ind_id) + goto fail; + + client->att = bt_att_ref(att); + client->db = gatt_db_ref(db); + + if (!gatt_client_init(client, mtu)) + goto fail; + + return bt_gatt_client_ref(client); + +fail: + bt_gatt_client_free(client); + return NULL; +} + +struct bt_gatt_client *bt_gatt_client_ref(struct bt_gatt_client *client) +{ + if (!client) + return NULL; + + __sync_fetch_and_add(&client->ref_count, 1); + + return client; +} + +void bt_gatt_client_unref(struct bt_gatt_client *client) +{ + if (!client) + return; + + if (__sync_sub_and_fetch(&client->ref_count, 1)) + return; + + bt_gatt_client_free(client); +} + +bool bt_gatt_client_is_ready(struct bt_gatt_client *client) +{ + return (client && client->ready); +} + +bool bt_gatt_client_set_ready_handler(struct bt_gatt_client *client, + bt_gatt_client_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + if (!client) + return false; + + if (client->ready_destroy) + client->ready_destroy(client->ready_data); + + client->ready_callback = callback; + client->ready_destroy = destroy; + client->ready_data = user_data; + + return true; +} + +bool bt_gatt_client_set_service_changed(struct bt_gatt_client *client, + bt_gatt_client_service_changed_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + if (!client) + return false; + + if (client->svc_chngd_destroy) + client->svc_chngd_destroy(client->svc_chngd_data); + + client->svc_chngd_callback = callback; + client->svc_chngd_destroy = destroy; + client->svc_chngd_data = user_data; + + return true; +} + +bool bt_gatt_client_set_debug(struct bt_gatt_client *client, + bt_gatt_client_debug_func_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) { + if (!client) + return false; + + if (client->debug_destroy) + client->debug_destroy(client->debug_data); + + client->debug_callback = callback; + client->debug_destroy = destroy; + client->debug_data = user_data; + + return true; +} + +uint16_t bt_gatt_client_get_mtu(struct bt_gatt_client *client) +{ + if (!client || !client->att) + return 0; + + return bt_att_get_mtu(client->att); +} + +static bool match_req_id(const void *a, const void *b) +{ + const struct request *req = a; + unsigned int id = PTR_TO_UINT(b); + + return req->id == id; +} + +static void cancel_long_write_cb(uint8_t opcode, const void *pdu, uint16_t len, + void *user_data) +{ + struct bt_gatt_client *client = user_data; + + if (queue_isempty(client->long_write_queue)) + client->in_long_write = false; +} + +static bool cancel_long_write_req(struct bt_gatt_client *client, + struct request *req) +{ + uint8_t pdu = 0x00; + + /* + * att_id == 0 means that request has been queued and no prepare write + * has been sent so far.Let's just remove if from the queue. + * Otherwise execute write needs to be send. + */ + if (!req->att_id) + return queue_remove(client->long_write_queue, req); + + return !!bt_att_send(client->att, BT_ATT_OP_EXEC_WRITE_REQ, &pdu, + sizeof(pdu), + cancel_long_write_cb, + client, NULL); + +} + +static void cancel_prep_write_cb(uint8_t opcode, const void *pdu, uint16_t len, + void *user_data) +{ + struct request *req = user_data; + struct bt_gatt_client *client = req->client; + + client->reliable_write_session_id = 0; +} + +static bool cancel_prep_write_session(struct bt_gatt_client *client, + struct request *req) +{ + uint8_t pdu = 0x00; + + return !!bt_att_send(client->att, BT_ATT_OP_EXEC_WRITE_REQ, &pdu, + sizeof(pdu), + cancel_prep_write_cb, + req, request_unref); +} + +bool bt_gatt_client_cancel(struct bt_gatt_client *client, unsigned int id) +{ + struct request *req; + + if (!client || !id || !client->att) + return false; + + req = queue_remove_if(client->pending_requests, match_req_id, + UINT_TO_PTR(id)); + if (!req) + return false; + + req->removed = true; + + if (!bt_att_cancel(client->att, req->att_id) && !req->long_write && + !req->prep_write) + return false; + + /* If this was a long-write, we need to abort all prepared writes */ + if (req->long_write) + return cancel_long_write_req(client, req); + + if (req->prep_write) + return cancel_prep_write_session(client, req); + + return true; +} + +static void cancel_request(void *data) +{ + struct request *req = data; + + req->removed = true; + + bt_att_cancel(req->client->att, req->att_id); + + if (req->long_write) + cancel_long_write_req(req->client, req); + + if (req->prep_write) + cancel_prep_write_session(req->client, req); +} + +bool bt_gatt_client_cancel_all(struct bt_gatt_client *client) +{ + if (!client || !client->att) + return false; + + queue_remove_all(client->pending_requests, NULL, NULL, cancel_request); + + if (client->discovery_req) { + bt_gatt_request_cancel(client->discovery_req); + bt_gatt_request_unref(client->discovery_req); + client->discovery_req = NULL; + } + + if (client->mtu_req_id) + bt_att_cancel(client->att, client->mtu_req_id); + + return true; +} + +struct read_op { + bt_gatt_client_read_callback_t callback; + void *user_data; + bt_gatt_client_destroy_func_t destroy; +}; + +static void destroy_read_op(void *data) +{ + struct read_op *op = data; + + if (op->destroy) + op->destroy(op->user_data); + + free(op); +} + +static void read_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct read_op *op = req->data; + bool success; + uint8_t att_ecode = 0; + const uint8_t *value = NULL; + uint16_t value_len = 0; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_READ_RSP || (!pdu && length)) { + success = false; + goto done; + } + + success = true; + value_len = length; + if (value_len) + value = pdu; + +done: + if (op->callback) + op->callback(success, att_ecode, value, length, op->user_data); +} + +unsigned int bt_gatt_client_read_value(struct bt_gatt_client *client, + uint16_t value_handle, + bt_gatt_client_read_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct request *req; + struct read_op *op; + uint8_t pdu[2]; + + if (!client) + return 0; + + op = new0(struct read_op, 1); + if (!op) + return 0; + + req = request_create(client); + if (!req) { + free(op); + return 0; + } + + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + req->data = op; + req->destroy = destroy_read_op; + + put_le16(value_handle, pdu); + + req->att_id = bt_att_send(client->att, BT_ATT_OP_READ_REQ, + pdu, sizeof(pdu), + read_cb, req, + request_unref); + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + return req->id; +} + +static void read_multiple_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct read_op *op = req->data; + uint8_t att_ecode; + bool success; + + if (opcode != BT_ATT_OP_READ_MULT_RSP || (!pdu && length)) { + success = false; + + if (opcode == BT_ATT_OP_ERROR_RSP) + att_ecode = process_error(pdu, length); + else + att_ecode = 0; + + pdu = NULL; + length = 0; + } else { + success = true; + att_ecode = 0; + } + + if (op->callback) + op->callback(success, att_ecode, pdu, length, op->user_data); +} + +unsigned int bt_gatt_client_read_multiple(struct bt_gatt_client *client, + uint16_t *handles, uint8_t num_handles, + bt_gatt_client_read_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + uint8_t pdu[num_handles * 2]; + struct request *req; + struct read_op *op; + int i; + + if (!client) + return 0; + + if (num_handles < 2) + return 0; + + if (num_handles * 2 > bt_att_get_mtu(client->att) - 1) + return 0; + + op = new0(struct read_op, 1); + if (!op) + return 0; + + req = request_create(client); + if (!req) { + free(op); + return 0; + } + + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + req->data = op; + req->destroy = destroy_read_op; + + for (i = 0; i < num_handles; i++) + put_le16(handles[i], pdu + (2 * i)); + + req->att_id = bt_att_send(client->att, BT_ATT_OP_READ_MULT_REQ, + pdu, sizeof(pdu), + read_multiple_cb, req, + request_unref); + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + return req->id; +} + +struct read_long_op { + struct bt_gatt_client *client; + int ref_count; + uint16_t value_handle; + uint16_t offset; + struct iovec iov; + bt_gatt_client_read_callback_t callback; + void *user_data; + bt_gatt_client_destroy_func_t destroy; +}; + +static void destroy_read_long_op(void *data) +{ + struct read_long_op *op = data; + + if (op->destroy) + op->destroy(op->user_data); + + free(op->iov.iov_base); + free(op); +} + +static bool append_chunk(struct read_long_op *op, const uint8_t *data, + uint16_t len) +{ + void *buf; + + /* Truncate if the data would exceed maximum length */ + if (op->offset + len > BT_ATT_MAX_VALUE_LEN) + len = BT_ATT_MAX_VALUE_LEN - op->offset; + + buf = realloc(op->iov.iov_base, op->iov.iov_len + len); + if (!buf) + return false; + + op->iov.iov_base = buf; + + memcpy(op->iov.iov_base + op->iov.iov_len, data, len); + + op->iov.iov_len += len; + op->offset += len; + + return true; +} + +static void read_long_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct request *req = user_data; + struct read_long_op *op = req->data; + bool success; + uint8_t att_ecode = 0; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_READ_BLOB_RSP || (!pdu && length)) { + success = false; + goto done; + } + + if (!length) + goto success; + + if (!append_chunk(op, pdu, length)) { + success = false; + goto done; + } + + if (op->offset >= BT_ATT_MAX_VALUE_LEN) + goto success; + + if (length >= bt_att_get_mtu(op->client->att) - 1) { + uint8_t pdu[4]; + + put_le16(op->value_handle, pdu); + put_le16(op->offset, pdu + 2); + + req->att_id = bt_att_send(op->client->att, + BT_ATT_OP_READ_BLOB_REQ, + pdu, sizeof(pdu), + read_long_cb, + request_ref(req), + request_unref); + if (req->att_id) + return; + + request_unref(req); + success = false; + goto done; + } + +success: + success = true; + +done: + if (op->callback) + op->callback(success, att_ecode, op->iov.iov_base, + op->iov.iov_len, op->user_data); +} + +unsigned int bt_gatt_client_read_long_value(struct bt_gatt_client *client, + uint16_t value_handle, uint16_t offset, + bt_gatt_client_read_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct request *req; + struct read_long_op *op; + uint8_t pdu[4]; + + if (!client) + return 0; + + op = new0(struct read_long_op, 1); + if (!op) + return 0; + + req = request_create(client); + if (!req) { + free(op); + return 0; + } + + op->client = client; + op->value_handle = value_handle; + op->offset = offset; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + req->data = op; + req->destroy = destroy_read_long_op; + + put_le16(value_handle, pdu); + put_le16(offset, pdu + 2); + + req->att_id = bt_att_send(client->att, BT_ATT_OP_READ_BLOB_REQ, + pdu, sizeof(pdu), + read_long_cb, req, + request_unref); + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + return req->id; +} + +unsigned int bt_gatt_client_write_without_response( + struct bt_gatt_client *client, + uint16_t value_handle, + bool signed_write, + const uint8_t *value, uint16_t length) { + uint8_t pdu[2 + length]; + struct request *req; + int security; + uint8_t op; + + if (!client) + return 0; + + req = request_create(client); + if (!req) + return 0; + + /* Only use signed write if unencrypted */ + if (signed_write) { + security = bt_att_get_sec_level(client->att); + op = security > BT_SECURITY_LOW ? BT_ATT_OP_WRITE_CMD : + BT_ATT_OP_SIGNED_WRITE_CMD; + } else + op = BT_ATT_OP_WRITE_CMD; + + put_le16(value_handle, pdu); + memcpy(pdu + 2, value, length); + + req->att_id = bt_att_send(client->att, op, pdu, sizeof(pdu), NULL, req, + request_unref); + if (!req->att_id) { + request_unref(req); + return 0; + } + + return req->id; +} + +struct write_op { + struct bt_gatt_client *client; + bt_gatt_client_callback_t callback; + void *user_data; + bt_gatt_destroy_func_t destroy; +}; + +static void destroy_write_op(void *data) +{ + struct write_op *op = data; + + if (op->destroy) + op->destroy(op->user_data); + + free(op); +} + +static void write_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct write_op *op = req->data; + bool success = true; + uint8_t att_ecode = 0; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_WRITE_RSP || pdu || length) + success = false; + +done: + if (op->callback) + op->callback(success, att_ecode, op->user_data); +} + +unsigned int bt_gatt_client_write_value(struct bt_gatt_client *client, + uint16_t value_handle, + const uint8_t *value, uint16_t length, + bt_gatt_client_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct request *req; + struct write_op *op; + uint8_t pdu[2 + length]; + + if (!client) + return 0; + + op = new0(struct write_op, 1); + if (!op) + return 0; + + req = request_create(client); + if (!req) { + free(op); + return 0; + } + + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + req->data = op; + req->destroy = destroy_write_op; + + put_le16(value_handle, pdu); + memcpy(pdu + 2, value, length); + + req->att_id = bt_att_send(client->att, BT_ATT_OP_WRITE_REQ, + pdu, sizeof(pdu), + write_cb, req, + request_unref); + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + return req->id; +} + +struct long_write_op { + struct bt_gatt_client *client; + bool reliable; + bool success; + uint8_t att_ecode; + bool reliable_error; + uint16_t value_handle; + uint8_t *value; + uint16_t length; + uint16_t offset; + uint16_t index; + uint16_t cur_length; + bt_gatt_client_write_long_callback_t callback; + void *user_data; + bt_gatt_client_destroy_func_t destroy; +}; + +static void long_write_op_free(void *data) +{ + struct long_write_op *op = data; + + if (op->destroy) + op->destroy(op->user_data); + + free(op->value); + free(op); +} + +static void prepare_write_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data); +static void complete_write_long_op(struct request *req, bool success, + uint8_t att_ecode, bool reliable_error); + +static void handle_next_prep_write(struct request *req) +{ + struct long_write_op *op = req->data; + bool success = true; + uint8_t *pdu; + + pdu = malloc(op->cur_length + 4); + if (!pdu) { + success = false; + goto done; + } + + put_le16(op->value_handle, pdu); + put_le16(op->offset + op->index, pdu + 2); + memcpy(pdu + 4, op->value + op->index, op->cur_length); + + req->att_id = bt_att_send(op->client->att, BT_ATT_OP_PREP_WRITE_REQ, + pdu, op->cur_length + 4, + prepare_write_cb, + request_ref(req), + request_unref); + if (!req->att_id) { + request_unref(req); + success = false; + } + + free(pdu); + + /* If so far successful, then the operation should continue. + * Otherwise, there was an error and the procedure should be + * completed. + */ + if (success) + return; + +done: + complete_write_long_op(req, success, 0, false); +} + +static void start_next_long_write(struct bt_gatt_client *client) +{ + struct request *req; + + if (queue_isempty(client->long_write_queue)) { + client->in_long_write = false; + return; + } + + req = queue_pop_head(client->long_write_queue); + if (!req) + return; + + handle_next_prep_write(req); + + /* + * send_next_prep_write adds an extra ref. Unref here to clean up if + * necessary, since we also added a ref before pushing to the queue. + */ + request_unref(req); +} + +static void execute_write_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct long_write_op *op = req->data; + bool success = op->success; + uint8_t att_ecode = op->att_ecode; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + } else if (opcode != BT_ATT_OP_EXEC_WRITE_RSP || pdu || length) + success = false; + + if (op->callback) + op->callback(success, op->reliable_error, att_ecode, + op->user_data); + + start_next_long_write(op->client); +} + +static void complete_write_long_op(struct request *req, bool success, + uint8_t att_ecode, bool reliable_error) +{ + struct long_write_op *op = req->data; + uint8_t pdu; + + op->success = success; + op->att_ecode = att_ecode; + op->reliable_error = reliable_error; + + if (success) + pdu = 0x01; /* Write */ + else + pdu = 0x00; /* Cancel */ + + req->att_id = bt_att_send(op->client->att, BT_ATT_OP_EXEC_WRITE_REQ, + &pdu, sizeof(pdu), + execute_write_cb, + request_ref(req), + request_unref); + if (req->att_id) + return; + + + request_unref(req); + success = false; + + if (op->callback) + op->callback(success, reliable_error, att_ecode, op->user_data); + + start_next_long_write(op->client); +} + +static void prepare_write_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct long_write_op *op = req->data; + bool success = true; + bool reliable_error = false; + uint8_t att_ecode = 0; + uint16_t next_index; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_PREP_WRITE_RSP) { + success = false; + goto done; + } + + if (op->reliable) { + if (!pdu || length != (op->cur_length + 4)) { + success = false; + reliable_error = true; + goto done; + } + + if (get_le16(pdu) != op->value_handle || + get_le16(pdu + 2) != (op->offset + op->index)) { + success = false; + reliable_error = true; + goto done; + } + + if (memcmp(pdu + 4, op->value + op->index, op->cur_length)) { + success = false; + reliable_error = true; + goto done; + } + } + + next_index = op->index + op->cur_length; + if (next_index == op->length) { + /* All bytes written */ + goto done; + } + + /* If the last written length was greater than or equal to what can fit + * inside a PDU, then there is more data to send. + */ + if (op->cur_length >= bt_att_get_mtu(op->client->att) - 5) { + op->index = next_index; + op->cur_length = MIN(op->length - op->index, + bt_att_get_mtu(op->client->att) - 5); + handle_next_prep_write(req); + return; + } + +done: + complete_write_long_op(req, success, att_ecode, reliable_error); +} + +unsigned int bt_gatt_client_write_long_value(struct bt_gatt_client *client, + bool reliable, + uint16_t value_handle, uint16_t offset, + const uint8_t *value, uint16_t length, + bt_gatt_client_write_long_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct request *req; + struct long_write_op *op; + uint8_t *pdu; + + if (!client) + return 0; + + if ((size_t)(length + offset) > UINT16_MAX) + return 0; + + /* Don't allow writing a 0-length value using this procedure. The + * upper-layer should use bt_gatt_write_value for that instead. + */ + if (!length || !value) + return 0; + + op = new0(struct long_write_op, 1); + if (!op) + return 0; + + op->value = malloc(length); + if (!op->value) { + free(op); + return 0; + } + + req = request_create(client); + if (!req) { + free(op->value); + free(op); + return 0; + } + + memcpy(op->value, value, length); + + op->client = client; + op->reliable = reliable; + op->value_handle = value_handle; + op->length = length; + op->offset = offset; + op->cur_length = MIN(length, bt_att_get_mtu(client->att) - 5); + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + req->data = op; + req->destroy = long_write_op_free; + req->long_write = true; + + if (client->in_long_write || client->reliable_write_session_id > 0) { + queue_push_tail(client->long_write_queue, req); + return req->id; + } + + pdu = malloc(op->cur_length + 4); + if (!pdu) { + free(op->value); + free(op); + return 0; + } + + put_le16(value_handle, pdu); + put_le16(offset, pdu + 2); + memcpy(pdu + 4, op->value, op->cur_length); + + req->att_id = bt_att_send(client->att, BT_ATT_OP_PREP_WRITE_REQ, + pdu, op->cur_length + 4, + prepare_write_cb, req, + request_unref); + free(pdu); + + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + client->in_long_write = true; + + return req->id; +} + +struct prep_write_op { + bt_gatt_client_write_long_callback_t callback; + void *user_data; + bt_gatt_destroy_func_t destroy; + uint8_t *pdu; + uint16_t pdu_len; +}; + +static void destroy_prep_write_op(void *data) +{ + struct prep_write_op *op = data; + + if (op->destroy) + op->destroy(op->user_data); + + free(op->pdu); + free(op); +} + +static void prep_write_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct prep_write_op *op = req->data; + bool success; + uint8_t att_ecode; + bool reliable_error; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + reliable_error = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_PREP_WRITE_RSP) { + success = false; + reliable_error = false; + att_ecode = 0; + goto done; + } + + if (!pdu || length != op->pdu_len || + memcmp(pdu, op->pdu, op->pdu_len)) { + success = false; + reliable_error = true; + att_ecode = 0; + goto done; + } + + success = true; + reliable_error = false; + att_ecode = 0; + +done: + if (op->callback) + op->callback(success, reliable_error, att_ecode, op->user_data); +} + +static struct request *get_reliable_request(struct bt_gatt_client *client, + unsigned int id) +{ + struct request *req; + struct prep_write_op *op; + + op = new0(struct prep_write_op, 1); + if (!op) + return NULL; + + /* Following prepare writes */ + if (id != 0) + req = queue_find(client->pending_requests, match_req_id, + UINT_TO_PTR(id)); + else + req = request_create(client); + + if (!req) { + free(op); + return NULL; + } + + req->data = op; + + return req; +} + +unsigned int bt_gatt_client_prepare_write(struct bt_gatt_client *client, + unsigned int id, uint16_t value_handle, + uint16_t offset, uint8_t *value, + uint16_t length, + bt_gatt_client_write_long_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct request *req; + struct prep_write_op *op; + uint8_t pdu[4 + length]; + + if (!client) + return 0; + + if (client->in_long_write) + return 0; + + /* + * Make sure that client who owns reliable session continues with + * prepare writes or this is brand new reliable session (id == 0) + */ + if (id != client->reliable_write_session_id) { + util_debug(client->debug_callback, client->debug_data, + "There is other reliable write session ongoing %u", + client->reliable_write_session_id); + + return 0; + } + + req = get_reliable_request(client, id); + if (!req) + return 0; + + op = (struct prep_write_op *)req->data; + + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + req->destroy = destroy_prep_write_op; + req->prep_write = true; + + put_le16(value_handle, pdu); + put_le16(offset, pdu + 2); + memcpy(pdu + 4, value, length); + + /* + * Before sending command we need to remember pdu as we need to validate + * it in the response. Store handle, offset and value. Therefore + * increase length by 4 (handle + offset) as we need it in couple places + * below + */ + length += 4; + + op->pdu = malloc(length); + if (!op->pdu) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + memcpy(op->pdu, pdu, length); + op->pdu_len = length; + + /* + * Now we are ready to send command + * Note that request_unref will be done on write execute + */ + req->att_id = bt_att_send(client->att, BT_ATT_OP_PREP_WRITE_REQ, pdu, + sizeof(pdu), prep_write_cb, req, + NULL); + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + /* + * Store first request id for prepare write and treat it as a session id + * valid until write execute is done + */ + if (client->reliable_write_session_id == 0) + client->reliable_write_session_id = req->id; + + return client->reliable_write_session_id; +} + +static void exec_write_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct request *req = user_data; + struct write_op *op = req->data; + bool success; + uint8_t att_ecode; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_EXEC_WRITE_RSP || pdu || length) { + success = false; + att_ecode = 0; + goto done; + } + + success = true; + att_ecode = 0; + +done: + if (op->callback) + op->callback(success, att_ecode, op->user_data); + + op->client->reliable_write_session_id = 0; + + start_next_long_write(op->client); +} + +unsigned int bt_gatt_client_write_execute(struct bt_gatt_client *client, + unsigned int id, + bt_gatt_client_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct request *req; + struct write_op *op; + uint8_t pdu; + + if (!client) + return 0; + + if (client->in_long_write) + return 0; + + if (client->reliable_write_session_id != id) + return 0; + + op = new0(struct write_op, 1); + if (!op) + return 0; + + req = queue_find(client->pending_requests, match_req_id, + UINT_TO_PTR(id)); + if (!req) { + free(op); + return 0; + } + + op->client = client; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + pdu = 0x01; + + req->data = op; + req->destroy = destroy_write_op; + + req->att_id = bt_att_send(client->att, BT_ATT_OP_EXEC_WRITE_REQ, &pdu, + sizeof(pdu), exec_write_cb, + req, request_unref); + if (!req->att_id) { + op->destroy = NULL; + request_unref(req); + return 0; + } + + return id; +} + +static bool match_notify_chrc_value_handle(const void *a, const void *b) +{ + const struct notify_chrc *chrc = a; + uint16_t value_handle = PTR_TO_UINT(b); + + return chrc->value_handle == value_handle; +} + +unsigned int bt_gatt_client_register_notify(struct bt_gatt_client *client, + uint16_t chrc_value_handle, + bt_gatt_client_register_callback_t callback, + bt_gatt_client_notify_callback_t notify, + void *user_data, + bt_gatt_client_destroy_func_t destroy) +{ + struct notify_data *notify_data; + struct notify_chrc *chrc = NULL; + + if (!client || !client->db || !chrc_value_handle || !callback) + return 0; + + if (!bt_gatt_client_is_ready(client) || client->in_svc_chngd) + return 0; + + /* Check if a characteristic ref count has been started already */ + chrc = queue_find(client->notify_chrcs, match_notify_chrc_value_handle, + UINT_TO_PTR(chrc_value_handle)); + + if (!chrc) { + /* + * Create an entry if the characteristic is known and has a CCC + * descriptor. + */ + chrc = notify_chrc_create(client, chrc_value_handle); + if (!chrc) + return 0; + } + + /* Fail if we've hit the maximum allowed notify sessions */ + if (chrc->notify_count == INT_MAX) + return 0; + + notify_data = new0(struct notify_data, 1); + if (!notify_data) + return 0; + + notify_data->client = client; + notify_data->ref_count = 1; + notify_data->chrc = chrc; + notify_data->callback = callback; + notify_data->notify = notify; + notify_data->user_data = user_data; + notify_data->destroy = destroy; + + /* Add the handler to the bt_gatt_client's general list */ + queue_push_tail(client->notify_list, notify_data); + + /* Assign an ID to the handler. */ + if (client->next_reg_id < 1) + client->next_reg_id = 1; + + notify_data->id = client->next_reg_id++; + + /* + * If a write to the CCC descriptor is in progress, then queue this + * request. + */ + if (chrc->ccc_write_id) { + queue_push_tail(chrc->reg_notify_queue, notify_data); + return notify_data->id; + } + + /* + * If the ref count is not zero, then notifications are already enabled. + */ + if (chrc->notify_count > 0 || !chrc->ccc_handle) { + complete_notify_request(notify_data); + return notify_data->id; + } + + /* Write to the CCC descriptor */ + if (!notify_data_write_ccc(notify_data, true, enable_ccc_callback)) { + queue_remove(client->notify_list, notify_data); + free(notify_data); + return 0; + } + + return notify_data->id; +} + +bool bt_gatt_client_unregister_notify(struct bt_gatt_client *client, + unsigned int id) +{ + struct notify_data *notify_data; + + if (!client || !id) + return false; + + notify_data = queue_remove_if(client->notify_list, match_notify_data_id, + UINT_TO_PTR(id)); + if (!notify_data) + return false; + + assert(notify_data->chrc->notify_count > 0); + assert(!notify_data->chrc->ccc_write_id); + + complete_unregister_notify(notify_data); + return true; +} + +bool bt_gatt_client_set_sec_level(struct bt_gatt_client *client, + int level) +{ + if (!client) + return false; + + return bt_att_set_sec_level(client->att, level); +} + +int bt_gatt_client_get_sec_level(struct bt_gatt_client *client) +{ + if (!client) + return -1; + + return bt_att_get_sec_level(client->att); +} diff --git a/bluez/gatt-client.h b/bluez/gatt-client.h new file mode 100644 index 0000000..8e5e0f5 --- /dev/null +++ b/bluez/gatt-client.h @@ -0,0 +1,134 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 +#include + +#define BT_GATT_UUID_SIZE 16 + +struct bt_gatt_client; + +struct bt_gatt_client *bt_gatt_client_new(struct gatt_db *db, + struct bt_att *att, + uint16_t mtu); + +struct bt_gatt_client *bt_gatt_client_ref(struct bt_gatt_client *client); +void bt_gatt_client_unref(struct bt_gatt_client *client); + +typedef void (*bt_gatt_client_destroy_func_t)(void *user_data); +typedef void (*bt_gatt_client_callback_t)(bool success, uint8_t att_ecode, + void *user_data); +typedef void (*bt_gatt_client_debug_func_t)(const char *str, void *user_data); +typedef void (*bt_gatt_client_read_callback_t)(bool success, uint8_t att_ecode, + const uint8_t *value, uint16_t length, + void *user_data); +typedef void (*bt_gatt_client_write_long_callback_t)(bool success, + bool reliable_error, uint8_t att_ecode, + void *user_data); +typedef void (*bt_gatt_client_notify_callback_t)(uint16_t value_handle, + const uint8_t *value, uint16_t length, + void *user_data); +typedef void (*bt_gatt_client_register_callback_t)(uint16_t att_ecode, + void *user_data); +typedef void (*bt_gatt_client_service_changed_callback_t)(uint16_t start_handle, + uint16_t end_handle, + void *user_data); + +bool bt_gatt_client_is_ready(struct bt_gatt_client *client); +bool bt_gatt_client_set_ready_handler(struct bt_gatt_client *client, + bt_gatt_client_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +bool bt_gatt_client_set_service_changed(struct bt_gatt_client *client, + bt_gatt_client_service_changed_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +bool bt_gatt_client_set_debug(struct bt_gatt_client *client, + bt_gatt_client_debug_func_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); + +uint16_t bt_gatt_client_get_mtu(struct bt_gatt_client *client); + +bool bt_gatt_client_cancel(struct bt_gatt_client *client, unsigned int id); +bool bt_gatt_client_cancel_all(struct bt_gatt_client *client); + +unsigned int bt_gatt_client_read_value(struct bt_gatt_client *client, + uint16_t value_handle, + bt_gatt_client_read_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +unsigned int bt_gatt_client_read_long_value(struct bt_gatt_client *client, + uint16_t value_handle, uint16_t offset, + bt_gatt_client_read_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +unsigned int bt_gatt_client_read_multiple(struct bt_gatt_client *client, + uint16_t *handles, uint8_t num_handles, + bt_gatt_client_read_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); + +unsigned int bt_gatt_client_write_without_response( + struct bt_gatt_client *client, + uint16_t value_handle, + bool signed_write, + const uint8_t *value, uint16_t length); +unsigned int bt_gatt_client_write_value(struct bt_gatt_client *client, + uint16_t value_handle, + const uint8_t *value, uint16_t length, + bt_gatt_client_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +unsigned int bt_gatt_client_write_long_value(struct bt_gatt_client *client, + bool reliable, + uint16_t value_handle, uint16_t offset, + const uint8_t *value, uint16_t length, + bt_gatt_client_write_long_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +unsigned int bt_gatt_client_prepare_write(struct bt_gatt_client *client, + unsigned int id, + uint16_t value_handle, uint16_t offset, + uint8_t *value, uint16_t length, + bt_gatt_client_write_long_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +unsigned int bt_gatt_client_write_execute(struct bt_gatt_client *client, + unsigned int id, + bt_gatt_client_callback_t callback, + void *user_data, + bt_gatt_client_destroy_func_t destroy); + +unsigned int bt_gatt_client_register_notify(struct bt_gatt_client *client, + uint16_t chrc_value_handle, + bt_gatt_client_register_callback_t callback, + bt_gatt_client_notify_callback_t notify, + void *user_data, + bt_gatt_client_destroy_func_t destroy); +bool bt_gatt_client_unregister_notify(struct bt_gatt_client *client, + unsigned int id); + +bool bt_gatt_client_set_sec_level(struct bt_gatt_client *client, int level); +int bt_gatt_client_get_sec_level(struct bt_gatt_client *client); diff --git a/bluez/gatt-db.c b/bluez/gatt-db.c new file mode 100644 index 0000000..29dda55 --- /dev/null +++ b/bluez/gatt-db.c @@ -0,0 +1,1636 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 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 + +#include +#include "uuid.h" +#include "util.h" +#include "queue.h" +#include "timeout.h" +#include "att.h" +#include "gatt-db.h" + +#ifndef MAX +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#endif + +#define MAX_CHAR_DECL_VALUE_LEN 19 +#define MAX_INCLUDED_VALUE_LEN 6 +#define ATTRIBUTE_TIMEOUT 5000 + +static const bt_uuid_t primary_service_uuid = { .type = BT_UUID16, + .value.u16 = GATT_PRIM_SVC_UUID }; +static const bt_uuid_t secondary_service_uuid = { .type = BT_UUID16, + .value.u16 = GATT_SND_SVC_UUID }; +static const bt_uuid_t characteristic_uuid = { .type = BT_UUID16, + .value.u16 = GATT_CHARAC_UUID }; +static const bt_uuid_t included_service_uuid = { .type = BT_UUID16, + .value.u16 = GATT_INCLUDE_UUID }; + +struct gatt_db { + int ref_count; + uint16_t next_handle; + struct queue *services; + + struct queue *notify_list; + unsigned int next_notify_id; +}; + +struct notify { + unsigned int id; + gatt_db_attribute_cb_t service_added; + gatt_db_attribute_cb_t service_removed; + gatt_db_destroy_func_t destroy; + void *user_data; +}; + +struct pending_read { + struct gatt_db_attribute *attrib; + unsigned int id; + unsigned int timeout_id; + gatt_db_attribute_read_t func; + void *user_data; +}; + +struct pending_write { + struct gatt_db_attribute *attrib; + unsigned int id; + unsigned int timeout_id; + gatt_db_attribute_write_t func; + void *user_data; +}; + +struct gatt_db_attribute { + struct gatt_db_service *service; + uint16_t handle; + bt_uuid_t uuid; + uint32_t permissions; + uint16_t value_len; + uint8_t *value; + + gatt_db_read_t read_func; + gatt_db_write_t write_func; + void *user_data; + + unsigned int read_id; + struct queue *pending_reads; + + unsigned int write_id; + struct queue *pending_writes; +}; + +struct gatt_db_service { + struct gatt_db *db; + bool active; + bool claimed; + uint16_t num_handles; + struct gatt_db_attribute **attributes; +}; + +static void pending_read_result(struct pending_read *p, int err, + const uint8_t *data, size_t length) +{ + if (p->timeout_id > 0) + timeout_remove(p->timeout_id); + + p->func(p->attrib, err, data, length, p->user_data); + + free(p); +} + +static void pending_read_free(void *data) +{ + struct pending_read *p = data; + + pending_read_result(p, -ECANCELED, NULL, 0); +} + +static void pending_write_result(struct pending_write *p, int err) +{ + if (p->timeout_id > 0) + timeout_remove(p->timeout_id); + + p->func(p->attrib, err, p->user_data); + + free(p); +} + +static void pending_write_free(void *data) +{ + struct pending_write *p = data; + + pending_write_result(p, -ECANCELED); +} + +static void attribute_destroy(struct gatt_db_attribute *attribute) +{ + /* Attribute was not initialized by user */ + if (!attribute) + return; + + queue_destroy(attribute->pending_reads, pending_read_free); + queue_destroy(attribute->pending_writes, pending_write_free); + + free(attribute->value); + free(attribute); +} + +static struct gatt_db_attribute *new_attribute(struct gatt_db_service *service, + const bt_uuid_t *type, + const uint8_t *val, + uint16_t len) +{ + struct gatt_db_attribute *attribute; + + attribute = new0(struct gatt_db_attribute, 1); + if (!attribute) + return NULL; + + attribute->service = service; + attribute->uuid = *type; + attribute->value_len = len; + if (len) { + attribute->value = malloc0(len); + if (!attribute->value) + goto failed; + + memcpy(attribute->value, val, len); + } + + attribute->pending_reads = queue_new(); + if (!attribute->pending_reads) + goto failed; + + attribute->pending_writes = queue_new(); + if (!attribute->pending_reads) + goto failed; + + return attribute; + +failed: + attribute_destroy(attribute); + return NULL; +} + +struct gatt_db *gatt_db_ref(struct gatt_db *db) +{ + if (!db) + return NULL; + + __sync_fetch_and_add(&db->ref_count, 1); + + return db; +} + +struct gatt_db *gatt_db_new(void) +{ + struct gatt_db *db; + + db = new0(struct gatt_db, 1); + if (!db) + return NULL; + + db->services = queue_new(); + if (!db->services) { + free(db); + return NULL; + } + + db->notify_list = queue_new(); + if (!db->notify_list) { + queue_destroy(db->services, NULL); + free(db); + return NULL; + } + + db->next_handle = 0x0001; + + return gatt_db_ref(db); +} + +static void notify_destroy(void *data) +{ + struct notify *notify = data; + + if (notify->destroy) + notify->destroy(notify->user_data); + + free(notify); +} + +static bool match_notify_id(const void *a, const void *b) +{ + const struct notify *notify = a; + unsigned int id = PTR_TO_UINT(b); + + return notify->id == id; +} + +struct notify_data { + struct gatt_db_attribute *attr; + bool added; +}; + +static void handle_notify(void *data, void *user_data) +{ + struct notify *notify = data; + struct notify_data *notify_data = user_data; + + if (notify_data->added) + notify->service_added(notify_data->attr, notify->user_data); + else + notify->service_removed(notify_data->attr, notify->user_data); +} + +static void notify_service_changed(struct gatt_db *db, + struct gatt_db_service *service, + bool added) +{ + struct notify_data data; + + if (queue_isempty(db->notify_list)) + return; + + data.attr = service->attributes[0]; + data.added = added; + + gatt_db_ref(db); + + queue_foreach(db->notify_list, handle_notify, &data); + + gatt_db_unref(db); +} + +static void gatt_db_service_destroy(void *data) +{ + struct gatt_db_service *service = data; + int i; + + if (service->active) + notify_service_changed(service->db, service, false); + + for (i = 0; i < service->num_handles; i++) + attribute_destroy(service->attributes[i]); + + free(service->attributes); + free(service); +} + +static void gatt_db_destroy(struct gatt_db *db) +{ + if (!db) + return; + + /* + * Clear the notify list before clearing the services to prevent the + * latter from sending service_removed events. + */ + queue_destroy(db->notify_list, notify_destroy); + db->notify_list = NULL; + + queue_destroy(db->services, gatt_db_service_destroy); + free(db); +} + +void gatt_db_unref(struct gatt_db *db) +{ + if (!db) + return; + + if (__sync_sub_and_fetch(&db->ref_count, 1)) + return; + + gatt_db_destroy(db); +} + +bool gatt_db_isempty(struct gatt_db *db) +{ + if (!db) + return true; + + return queue_isempty(db->services); +} + +static int uuid_to_le(const bt_uuid_t *uuid, uint8_t *dst) +{ + bt_uuid_t uuid128; + + if (uuid->type == BT_UUID16) { + put_le16(uuid->value.u16, dst); + return bt_uuid_len(uuid); + } + + bt_uuid_to_uuid128(uuid, &uuid128); + bswap_128(&uuid128.value.u128, dst); + return bt_uuid_len(&uuid128); +} + +static bool le_to_uuid(const uint8_t *src, size_t len, bt_uuid_t *uuid) +{ + uint128_t u128; + + if (len == 2) { + bt_uuid16_create(uuid, get_le16(src)); + return true; + } + + if (len == 4) { + bt_uuid32_create(uuid, get_le32(src)); + return true; + } + + if (len != 16) + return false; + + bswap_128(src, &u128); + bt_uuid128_create(uuid, u128); + + return true; +} + +static struct gatt_db_service *gatt_db_service_create(const bt_uuid_t *uuid, + bool primary, + uint16_t num_handles) +{ + struct gatt_db_service *service; + const bt_uuid_t *type; + uint8_t value[16]; + uint16_t len; + + if (num_handles < 1) + return NULL; + + service = new0(struct gatt_db_service, 1); + if (!service) + return NULL; + + service->attributes = new0(struct gatt_db_attribute *, num_handles); + if (!service->attributes) { + free(service); + return NULL; + } + + if (primary) + type = &primary_service_uuid; + else + type = &secondary_service_uuid; + + len = uuid_to_le(uuid, value); + + service->attributes[0] = new_attribute(service, type, value, len); + if (!service->attributes[0]) { + gatt_db_service_destroy(service); + return NULL; + } + + return service; +} + + +bool gatt_db_remove_service(struct gatt_db *db, + struct gatt_db_attribute *attrib) +{ + struct gatt_db_service *service; + + if (!db || !attrib) + return false; + + service = attrib->service; + + queue_remove(db->services, service); + + gatt_db_service_destroy(service); + + return true; +} + +bool gatt_db_clear(struct gatt_db *db) +{ + if (!db) + return false; + + queue_remove_all(db->services, NULL, NULL, gatt_db_service_destroy); + + db->next_handle = 0; + + return true; +} + +static void gatt_db_service_get_handles(const struct gatt_db_service *service, + uint16_t *start_handle, + uint16_t *end_handle) +{ + if (start_handle) + *start_handle = service->attributes[0]->handle; + + if (end_handle) + *end_handle = service->attributes[0]->handle + + service->num_handles - 1; +} + +struct clear_range { + uint16_t start, end; +}; + +static bool match_range(const void *a, const void *b) +{ + const struct gatt_db_service *service = a; + const struct clear_range *range = b; + uint16_t svc_start, svc_end; + + gatt_db_service_get_handles(service, &svc_start, &svc_end); + + return svc_start <= range->end && svc_end >= range->start; +} + +bool gatt_db_clear_range(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle) +{ + struct clear_range range; + + if (!db || start_handle > end_handle) + return false; + + range.start = start_handle; + range.end = end_handle; + + queue_remove_all(db->services, match_range, &range, + gatt_db_service_destroy); + + return true; +} + +static bool find_insert_loc(struct gatt_db *db, uint16_t start, uint16_t end, + struct gatt_db_service **after) +{ + const struct queue_entry *services_entry; + struct gatt_db_service *service; + uint16_t cur_start, cur_end; + + *after = NULL; + + services_entry = queue_get_entries(db->services); + + while (services_entry) { + service = services_entry->data; + + gatt_db_service_get_handles(service, &cur_start, &cur_end); + + if (start >= cur_start && start <= cur_end) + return false; + + if (end >= cur_start && end <= cur_end) + return false; + + if (end < cur_start) + return true; + + *after = service; + services_entry = services_entry->next; + } + + return true; +} + +struct gatt_db_attribute *gatt_db_insert_service(struct gatt_db *db, + uint16_t handle, + const bt_uuid_t *uuid, + bool primary, + uint16_t num_handles) +{ + struct gatt_db_service *service, *after; + + after = NULL; + + if (!db || handle < 1) + return NULL; + + if (num_handles < 1 || (handle + num_handles - 1) > UINT16_MAX) + return NULL; + + if (!find_insert_loc(db, handle, handle + num_handles - 1, &after)) + return NULL; + + service = gatt_db_service_create(uuid, primary, num_handles); + + if (!service) + return NULL; + + if (after) { + if (!queue_push_after(db->services, after, service)) + goto fail; + } else if (!queue_push_head(db->services, service)) { + goto fail; + } + + service->db = db; + service->attributes[0]->handle = handle; + service->num_handles = num_handles; + + /* Fast-forward next_handle if the new service was added to the end */ + db->next_handle = MAX(handle + num_handles, db->next_handle); + + return service->attributes[0]; + +fail: + gatt_db_service_destroy(service); + return NULL; +} + +struct gatt_db_attribute *gatt_db_add_service(struct gatt_db *db, + const bt_uuid_t *uuid, + bool primary, + uint16_t num_handles) +{ + return gatt_db_insert_service(db, db->next_handle, uuid, primary, + num_handles); +} + +unsigned int gatt_db_register(struct gatt_db *db, + gatt_db_attribute_cb_t service_added, + gatt_db_attribute_cb_t service_removed, + void *user_data, + gatt_db_destroy_func_t destroy) +{ + struct notify *notify; + + if (!db || !(service_added || service_removed)) + return 0; + + notify = new0(struct notify, 1); + if (!notify) + return 0; + + notify->service_added = service_added; + notify->service_removed = service_removed; + notify->destroy = destroy; + notify->user_data = user_data; + + if (db->next_notify_id < 1) + db->next_notify_id = 1; + + notify->id = db->next_notify_id++; + + if (!queue_push_tail(db->notify_list, notify)) { + free(notify); + return 0; + } + + return notify->id; +} + +bool gatt_db_unregister(struct gatt_db *db, unsigned int id) +{ + struct notify *notify; + + if (!db || !id) + return false; + + notify = queue_find(db->notify_list, match_notify_id, UINT_TO_PTR(id)); + if (!notify) + return false; + + queue_remove(db->notify_list, notify); + notify_destroy(notify); + + return true; +} + +static uint16_t get_attribute_index(struct gatt_db_service *service, + int end_offset) +{ + int i = 0; + + /* Here we look for first free attribute index with given offset */ + while (i < (service->num_handles - end_offset) && + service->attributes[i]) + i++; + + return i == (service->num_handles - end_offset) ? 0 : i; +} + +static uint16_t get_handle_at_index(struct gatt_db_service *service, + int index) +{ + return service->attributes[index]->handle; +} + +static struct gatt_db_attribute * +attribute_update(struct gatt_db_service *service, int index) +{ + uint16_t previous_handle; + + /* We call this function with index > 0, because index 0 is reserved + * for service declaration, and is set in add_service() + */ + previous_handle = service->attributes[index - 1]->handle; + service->attributes[index]->handle = previous_handle + 1; + + return service->attributes[index]; +} + +static void set_attribute_data(struct gatt_db_attribute *attribute, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + uint32_t permissions, + void *user_data) +{ + attribute->permissions = permissions; + attribute->read_func = read_func; + attribute->write_func = write_func; + attribute->user_data = user_data; +} + +struct gatt_db_attribute * +gatt_db_service_add_characteristic(struct gatt_db_attribute *attrib, + const bt_uuid_t *uuid, + uint32_t permissions, + uint8_t properties, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + void *user_data) +{ + struct gatt_db_service *service; + uint8_t value[MAX_CHAR_DECL_VALUE_LEN]; + uint16_t len = 0; + int i; + + if (!attrib) + return NULL; + + service = attrib->service; + + i = get_attribute_index(service, 1); + if (!i) + return NULL; + + value[0] = properties; + len += sizeof(properties); + /* We set handle of characteristic value, which will be added next */ + put_le16(get_handle_at_index(service, i - 1) + 2, &value[1]); + len += sizeof(uint16_t); + len += uuid_to_le(uuid, &value[3]); + + service->attributes[i] = new_attribute(service, &characteristic_uuid, + value, len); + if (!service->attributes[i]) + return NULL; + + attribute_update(service, i++); + + service->attributes[i] = new_attribute(service, uuid, NULL, 0); + if (!service->attributes[i]) { + free(service->attributes[i - 1]); + return NULL; + } + + set_attribute_data(service->attributes[i], read_func, write_func, + permissions, user_data); + + return attribute_update(service, i); +} + +struct gatt_db_attribute * +gatt_db_service_add_descriptor(struct gatt_db_attribute *attrib, + const bt_uuid_t *uuid, + uint32_t permissions, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + void *user_data) +{ + struct gatt_db_service *service; + int i; + + if (!attrib) + return NULL; + + service = attrib->service; + + i = get_attribute_index(service, 0); + if (!i) + return NULL; + + service->attributes[i] = new_attribute(service, uuid, NULL, 0); + if (!service->attributes[i]) + return NULL; + + set_attribute_data(service->attributes[i], read_func, write_func, + permissions, user_data); + + return attribute_update(service, i); +} + +struct gatt_db_attribute * +gatt_db_service_add_included(struct gatt_db_attribute *attrib, + struct gatt_db_attribute *include) +{ + struct gatt_db_service *service, *included; + uint8_t value[MAX_INCLUDED_VALUE_LEN]; + uint16_t included_handle, len = 0; + int index; + + if (!attrib || !include) + return NULL; + + service = attrib->service; + included = include->service; + + /* Adjust include to point to the first attribute */ + if (include != included->attributes[0]) + include = included->attributes[0]; + + included_handle = include->handle; + + put_le16(included_handle, &value[len]); + len += sizeof(uint16_t); + + put_le16(included_handle + included->num_handles - 1, &value[len]); + len += sizeof(uint16_t); + + /* The Service UUID shall only be present when the UUID is a 16-bit + * Bluetooth UUID. Vol 2. Part G. 3.2 + */ + if (include->value_len == sizeof(uint16_t)) { + memcpy(&value[len], include->value, include->value_len); + len += include->value_len; + } + + index = get_attribute_index(service, 0); + if (!index) + return NULL; + + service->attributes[index] = new_attribute(service, + &included_service_uuid, + value, len); + if (!service->attributes[index]) + return NULL; + + /* The Attribute Permissions shall be read only and not require + * authentication or authorization. Vol 2. Part G. 3.2 + * + * TODO handle permissions + */ + set_attribute_data(service->attributes[index], NULL, NULL, 0, NULL); + + return attribute_update(service, index); +} + +bool gatt_db_service_set_active(struct gatt_db_attribute *attrib, bool active) +{ + struct gatt_db_service *service; + + if (!attrib) + return false; + + service = attrib->service; + + if (service->active == active) + return true; + + service->active = active; + + notify_service_changed(service->db, service, active); + + return true; +} + +bool gatt_db_service_get_active(struct gatt_db_attribute *attrib) +{ + if (!attrib) + return false; + + return attrib->service->active; +} + +bool gatt_db_service_set_claimed(struct gatt_db_attribute *attrib, + bool claimed) +{ + if (!attrib) + return false; + + attrib->service->claimed = claimed; + + return true; +} + +bool gatt_db_service_get_claimed(struct gatt_db_attribute *attrib) +{ + if (!attrib) + return false; + + return attrib->service->claimed; +} + +void gatt_db_read_by_group_type(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t type, + struct queue *queue) +{ + const struct queue_entry *services_entry; + struct gatt_db_service *service; + uint16_t grp_start, grp_end, uuid_size; + + uuid_size = 0; + + services_entry = queue_get_entries(db->services); + + while (services_entry) { + service = services_entry->data; + + if (!service->active) + goto next_service; + + if (bt_uuid_cmp(&type, &service->attributes[0]->uuid)) + goto next_service; + + grp_start = service->attributes[0]->handle; + grp_end = grp_start + service->num_handles - 1; + + if (grp_end < start_handle || grp_start > end_handle) + goto next_service; + + if (grp_start < start_handle || grp_start > end_handle) + goto next_service; + + if (!uuid_size) + uuid_size = service->attributes[0]->value_len; + else if (uuid_size != service->attributes[0]->value_len) + return; + + queue_push_tail(queue, service->attributes[0]); + +next_service: + services_entry = services_entry->next; + } +} + +struct find_by_type_value_data { + bt_uuid_t uuid; + uint16_t start_handle; + uint16_t end_handle; + gatt_db_attribute_cb_t func; + void *user_data; + const void *value; + size_t value_len; + unsigned int num_of_res; +}; + +static void find_by_type(void *data, void *user_data) +{ + struct find_by_type_value_data *search_data = user_data; + struct gatt_db_service *service = data; + struct gatt_db_attribute *attribute; + int i; + + if (!service->active) + return; + + for (i = 0; i < service->num_handles; i++) { + attribute = service->attributes[i]; + + if (!attribute) + continue; + + if ((attribute->handle < search_data->start_handle) || + (attribute->handle > search_data->end_handle)) + continue; + + if (bt_uuid_cmp(&search_data->uuid, &attribute->uuid)) + continue; + + /* TODO: fix for read-callback based attributes */ + if (search_data->value && memcmp(attribute->value, + search_data->value, + search_data->value_len)) + continue; + + search_data->num_of_res++; + search_data->func(attribute, search_data->user_data); + } +} + +unsigned int gatt_db_find_by_type(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t *type, + gatt_db_attribute_cb_t func, + void *user_data) +{ + struct find_by_type_value_data data; + + memset(&data, 0, sizeof(data)); + + data.uuid = *type; + data.start_handle = start_handle; + data.end_handle = end_handle; + data.func = func; + data.user_data = user_data; + + queue_foreach(db->services, find_by_type, &data); + + return data.num_of_res; +} + +unsigned int gatt_db_find_by_type_value(struct gatt_db *db, + uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t *type, + const void *value, + size_t value_len, + gatt_db_attribute_cb_t func, + void *user_data) +{ + struct find_by_type_value_data data; + + data.uuid = *type; + data.start_handle = start_handle; + data.end_handle = end_handle; + data.func = func; + data.user_data = user_data; + data.value = value; + data.value_len = value_len; + + queue_foreach(db->services, find_by_type, &data); + + return data.num_of_res; +} + +struct read_by_type_data { + struct queue *queue; + bt_uuid_t uuid; + uint16_t start_handle; + uint16_t end_handle; +}; + +static void read_by_type(void *data, void *user_data) +{ + struct read_by_type_data *search_data = user_data; + struct gatt_db_service *service = data; + struct gatt_db_attribute *attribute; + int i; + + if (!service->active) + return; + + for (i = 0; i < service->num_handles; i++) { + attribute = service->attributes[i]; + if (!attribute) + continue; + + if (attribute->handle < search_data->start_handle) + continue; + + if (attribute->handle > search_data->end_handle) + return; + + if (bt_uuid_cmp(&search_data->uuid, &attribute->uuid)) + continue; + + queue_push_tail(search_data->queue, attribute); + } +} + +void gatt_db_read_by_type(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t type, + struct queue *queue) +{ + struct read_by_type_data data; + data.uuid = type; + data.start_handle = start_handle; + data.end_handle = end_handle; + data.queue = queue; + + queue_foreach(db->services, read_by_type, &data); +} + + +struct find_information_data { + struct queue *queue; + uint16_t start_handle; + uint16_t end_handle; +}; + +static void find_information(void *data, void *user_data) +{ + struct find_information_data *search_data = user_data; + struct gatt_db_service *service = data; + struct gatt_db_attribute *attribute; + int i; + + if (!service->active) + return; + + /* Check if service is in range */ + if ((service->attributes[0]->handle + service->num_handles - 1) < + search_data->start_handle) + return; + + for (i = 0; i < service->num_handles; i++) { + attribute = service->attributes[i]; + if (!attribute) + continue; + + if (attribute->handle < search_data->start_handle) + continue; + + if (attribute->handle > search_data->end_handle) + return; + + queue_push_tail(search_data->queue, attribute); + } +} + +void gatt_db_find_information(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + struct queue *queue) +{ + struct find_information_data data; + + data.start_handle = start_handle; + data.end_handle = end_handle; + data.queue = queue; + + queue_foreach(db->services, find_information, &data); +} + +void gatt_db_foreach_service(struct gatt_db *db, const bt_uuid_t *uuid, + gatt_db_attribute_cb_t func, + void *user_data) +{ + gatt_db_foreach_service_in_range(db, uuid, func, user_data, 0x0001, + 0xffff); +} + +struct foreach_data { + gatt_db_attribute_cb_t func; + const bt_uuid_t *uuid; + void *user_data; + uint16_t start, end; +}; + +static void foreach_service_in_range(void *data, void *user_data) +{ + struct gatt_db_service *service = data; + struct foreach_data *foreach_data = user_data; + uint16_t svc_start; + bt_uuid_t uuid; + + svc_start = get_handle_at_index(service, 0); + + if (svc_start > foreach_data->end || svc_start < foreach_data->start) + return; + + if (foreach_data->uuid) { + gatt_db_attribute_get_service_uuid(service->attributes[0], + &uuid); + if (bt_uuid_cmp(&uuid, foreach_data->uuid)) + return; + } + + foreach_data->func(service->attributes[0], foreach_data->user_data); +} + +void gatt_db_foreach_service_in_range(struct gatt_db *db, + const bt_uuid_t *uuid, + gatt_db_attribute_cb_t func, + void *user_data, + uint16_t start_handle, + uint16_t end_handle) +{ + struct foreach_data data; + + if (!db || !func || start_handle > end_handle) + return; + + data.func = func; + data.uuid = uuid; + data.user_data = user_data; + data.start = start_handle; + data.end = end_handle; + + queue_foreach(db->services, foreach_service_in_range, &data); +} + +void gatt_db_service_foreach(struct gatt_db_attribute *attrib, + const bt_uuid_t *uuid, + gatt_db_attribute_cb_t func, + void *user_data) +{ + struct gatt_db_service *service; + struct gatt_db_attribute *attr; + uint16_t i; + + if (!attrib || !func) + return; + + service = attrib->service; + + for (i = 0; i < service->num_handles; i++) { + attr = service->attributes[i]; + if (!attr) + continue; + + if (uuid && bt_uuid_cmp(uuid, &attr->uuid)) + continue; + + func(attr, user_data); + } +} + +void gatt_db_service_foreach_char(struct gatt_db_attribute *attrib, + gatt_db_attribute_cb_t func, + void *user_data) +{ + gatt_db_service_foreach(attrib, &characteristic_uuid, func, user_data); +} + +void gatt_db_service_foreach_desc(struct gatt_db_attribute *attrib, + gatt_db_attribute_cb_t func, + void *user_data) +{ + struct gatt_db_service *service; + struct gatt_db_attribute *attr; + uint16_t i; + + if (!attrib || !func) + return; + + /* Return if this attribute is not a characteristic declaration */ + if (bt_uuid_cmp(&characteristic_uuid, &attrib->uuid)) + return; + + service = attrib->service; + + /* Start from the attribute following the value handle */ + i = attrib->handle - service->attributes[0]->handle + 2; + for (; i < service->num_handles; i++) { + attr = service->attributes[i]; + if (!attr) + continue; + + /* Return if we reached the end of this characteristic */ + if (!bt_uuid_cmp(&characteristic_uuid, &attr->uuid) || + !bt_uuid_cmp(&included_service_uuid, &attr->uuid)) + return; + + func(attr, user_data); + } +} + +void gatt_db_service_foreach_incl(struct gatt_db_attribute *attrib, + gatt_db_attribute_cb_t func, + void *user_data) +{ + gatt_db_service_foreach(attrib, &included_service_uuid, func, + user_data); +} + +static bool find_service_for_handle(const void *data, const void *user_data) +{ + const struct gatt_db_service *service = data; + uint16_t handle = PTR_TO_UINT(user_data); + uint16_t start, end; + + gatt_db_service_get_handles(service, &start, &end); + + return (start <= handle) && (handle <= end); +} + +struct gatt_db_attribute *gatt_db_get_attribute(struct gatt_db *db, + uint16_t handle) +{ + struct gatt_db_service *service; + uint16_t service_handle; + + if (!db || !handle) + return NULL; + + service = queue_find(db->services, find_service_for_handle, + UINT_TO_PTR(handle)); + if (!service) + return NULL; + + service_handle = service->attributes[0]->handle; + + /* + * We can safely get attribute from attributes array with offset, + * because find_service_for_handle() check if given handle is + * in service range. + */ + return service->attributes[handle - service_handle]; +} + +static bool find_service_with_uuid(const void *data, const void *user_data) +{ + const struct gatt_db_service *service = data; + const bt_uuid_t *uuid = user_data; + bt_uuid_t svc_uuid; + + gatt_db_attribute_get_service_uuid(service->attributes[0], &svc_uuid); + + return bt_uuid_cmp(uuid, &svc_uuid) == 0; +} + +struct gatt_db_attribute *gatt_db_get_service_with_uuid(struct gatt_db *db, + const bt_uuid_t *uuid) +{ + struct gatt_db_service *service; + + if (!db || !uuid) + return NULL; + + service = queue_find(db->services, find_service_with_uuid, uuid); + if (!service) + return NULL; + + return service->attributes[0]; +} + +const bt_uuid_t *gatt_db_attribute_get_type( + const struct gatt_db_attribute *attrib) +{ + if (!attrib) + return NULL; + + return &attrib->uuid; +} + +uint16_t gatt_db_attribute_get_handle(const struct gatt_db_attribute *attrib) +{ + if (!attrib) + return 0; + + return attrib->handle; +} + +bool gatt_db_attribute_get_service_uuid(const struct gatt_db_attribute *attrib, + bt_uuid_t *uuid) +{ + struct gatt_db_service *service; + + if (!attrib || !uuid) + return false; + + service = attrib->service; + + if (service->attributes[0]->value_len == sizeof(uint16_t)) { + uint16_t value; + + value = get_le16(service->attributes[0]->value); + bt_uuid16_create(uuid, value); + + return true; + } + + if (service->attributes[0]->value_len == sizeof(uint128_t)) { + uint128_t value; + + bswap_128(service->attributes[0]->value, &value); + bt_uuid128_create(uuid, value); + + return true; + } + + return false; +} + +bool gatt_db_attribute_get_service_handles( + const struct gatt_db_attribute *attrib, + uint16_t *start_handle, + uint16_t *end_handle) +{ + struct gatt_db_service *service; + + if (!attrib) + return false; + + service = attrib->service; + + gatt_db_service_get_handles(service, start_handle, end_handle); + + return true; +} + +bool gatt_db_attribute_get_service_data(const struct gatt_db_attribute *attrib, + uint16_t *start_handle, + uint16_t *end_handle, + bool *primary, + bt_uuid_t *uuid) +{ + struct gatt_db_service *service; + struct gatt_db_attribute *decl; + + if (!attrib) + return false; + + service = attrib->service; + decl = service->attributes[0]; + + gatt_db_service_get_handles(service, start_handle, end_handle); + + if (primary) + *primary = bt_uuid_cmp(&decl->uuid, &secondary_service_uuid); + + if (!uuid) + return true; + + /* + * The service declaration attribute value is the 16 or 128 bit service + * UUID. + */ + return le_to_uuid(decl->value, decl->value_len, uuid); +} + +bool gatt_db_attribute_get_char_data(const struct gatt_db_attribute *attrib, + uint16_t *handle, + uint16_t *value_handle, + uint8_t *properties, + bt_uuid_t *uuid) +{ + if (!attrib) + return false; + + if (bt_uuid_cmp(&characteristic_uuid, &attrib->uuid)) + return false; + + /* + * Characteristic declaration value: + * 1 octet: Characteristic properties + * 2 octets: Characteristic value handle + * 2 or 16 octets: characteristic UUID + */ + if (!attrib->value || (attrib->value_len != 5 && + attrib->value_len != 19)) + return false; + + if (handle) + *handle = attrib->handle; + + if (properties) + *properties = attrib->value[0]; + + if (value_handle) + *value_handle = get_le16(attrib->value + 1); + + if (!uuid) + return true; + + return le_to_uuid(attrib->value + 3, attrib->value_len - 3, uuid); +} + +bool gatt_db_attribute_get_incl_data(const struct gatt_db_attribute *attrib, + uint16_t *handle, + uint16_t *start_handle, + uint16_t *end_handle) +{ + if (!attrib) + return false; + + if (bt_uuid_cmp(&included_service_uuid, &attrib->uuid)) + return false; + + /* + * Include definition value: + * 2 octets: start handle of included service + * 2 octets: end handle of included service + * optional 2 octets: 16-bit Bluetooth UUID + */ + if (!attrib->value || attrib->value_len < 4 || attrib->value_len > 6) + return false; + + /* + * We only return the handles since the UUID can be easily obtained + * from the corresponding attribute. + */ + if (handle) + *handle = attrib->handle; + + if (start_handle) + *start_handle = get_le16(attrib->value); + + if (end_handle) + *end_handle = get_le16(attrib->value + 2); + + return true; +} + +uint32_t +gatt_db_attribute_get_permissions(const struct gatt_db_attribute *attrib) +{ + if (!attrib) + return 0; + + return attrib->permissions; +} + +static bool read_timeout(void *user_data) +{ + struct pending_read *p = user_data; + + p->timeout_id = 0; + + queue_remove(p->attrib->pending_reads, p); + + pending_read_result(p, -ETIMEDOUT, NULL, 0); + + return false; +} + +bool gatt_db_attribute_read(struct gatt_db_attribute *attrib, uint16_t offset, + uint8_t opcode, struct bt_att *att, + gatt_db_attribute_read_t func, void *user_data) +{ + uint8_t *value; + + if (!attrib || !func) + return false; + + if (attrib->read_func) { + struct pending_read *p; + + p = new0(struct pending_read, 1); + if (!p) + return false; + + p->attrib = attrib; + p->id = ++attrib->read_id; + p->timeout_id = timeout_add(ATTRIBUTE_TIMEOUT, read_timeout, + p, NULL); + p->func = func; + p->user_data = user_data; + + queue_push_tail(attrib->pending_reads, p); + + attrib->read_func(attrib, p->id, offset, opcode, att, + attrib->user_data); + return true; + } + + /* Check boundary if value is stored in the db */ + if (offset > attrib->value_len) { + func(attrib, BT_ATT_ERROR_INVALID_OFFSET, NULL, 0, user_data); + return true; + } + + /* Guard against invalid access if offset equals to value length */ + value = offset == attrib->value_len ? NULL : &attrib->value[offset]; + + func(attrib, 0, value, attrib->value_len - offset, user_data); + + return true; +} + +static bool find_pending(const void *a, const void *b) +{ + const struct pending_read *p = a; + unsigned int id = PTR_TO_UINT(b); + + return p->id == id; +} + +bool gatt_db_attribute_read_result(struct gatt_db_attribute *attrib, + unsigned int id, int err, + const uint8_t *value, size_t length) +{ + struct pending_read *p; + + if (!attrib || !id) + return false; + + p = queue_remove_if(attrib->pending_reads, find_pending, + UINT_TO_PTR(id)); + if (!p) + return false; + + pending_read_result(p, err, value, length); + + return true; +} + +static bool write_timeout(void *user_data) +{ + struct pending_write *p = user_data; + + p->timeout_id = 0; + + queue_remove(p->attrib->pending_writes, p); + + pending_write_result(p, -ETIMEDOUT); + + return false; +} + +bool gatt_db_attribute_write(struct gatt_db_attribute *attrib, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + gatt_db_attribute_write_t func, + void *user_data) +{ + if (!attrib || !func) + return false; + + if (attrib->write_func) { + struct pending_write *p; + + p = new0(struct pending_write, 1); + if (!p) + return false; + + p->attrib = attrib; + p->id = ++attrib->write_id; + p->timeout_id = timeout_add(ATTRIBUTE_TIMEOUT, write_timeout, + p, NULL); + p->func = func; + p->user_data = user_data; + + queue_push_tail(attrib->pending_writes, p); + + attrib->write_func(attrib, p->id, offset, value, len, opcode, + att, attrib->user_data); + return true; + } + + /* Nothing to write just skip */ + if (len == 0) + goto done; + + /* For values stored in db allocate on demand */ + if (!attrib->value || offset >= attrib->value_len || + len > (unsigned) (attrib->value_len - offset)) { + void *buf; + + buf = realloc(attrib->value, len + offset); + if (!buf) + return false; + + attrib->value = buf; + + /* Init data in the first allocation */ + if (!attrib->value_len) + memset(attrib->value, 0, offset); + + attrib->value_len = len + offset; + } + + memcpy(&attrib->value[offset], value, len); + +done: + func(attrib, 0, user_data); + + return true; +} + +bool gatt_db_attribute_write_result(struct gatt_db_attribute *attrib, + unsigned int id, int err) +{ + struct pending_write *p; + + if (!attrib || !id) + return false; + + p = queue_remove_if(attrib->pending_writes, find_pending, + UINT_TO_PTR(id)); + if (!p) + return false; + + pending_write_result(p, err); + + return true; +} + +bool gatt_db_attribute_reset(struct gatt_db_attribute *attrib) +{ + if (!attrib) + return false; + + if (!attrib->value || !attrib->value_len) + return true; + + free(attrib->value); + attrib->value = NULL; + attrib->value_len = 0; + + return true; +} diff --git a/bluez/gatt-db.h b/bluez/gatt-db.h new file mode 100644 index 0000000..74b37bc --- /dev/null +++ b/bluez/gatt-db.h @@ -0,0 +1,219 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 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 + * + */ + +struct gatt_db; +struct gatt_db_attribute; + +struct gatt_db *gatt_db_new(void); + +struct gatt_db *gatt_db_ref(struct gatt_db *db); +void gatt_db_unref(struct gatt_db *db); + +bool gatt_db_isempty(struct gatt_db *db); + +struct gatt_db_attribute *gatt_db_add_service(struct gatt_db *db, + const bt_uuid_t *uuid, + bool primary, + uint16_t num_handles); + +bool gatt_db_remove_service(struct gatt_db *db, + struct gatt_db_attribute *attrib); +bool gatt_db_clear(struct gatt_db *db); +bool gatt_db_clear_range(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle); + +struct gatt_db_attribute *gatt_db_insert_service(struct gatt_db *db, + uint16_t handle, + const bt_uuid_t *uuid, + bool primary, + uint16_t num_handles); + +typedef void (*gatt_db_read_t) (struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + uint8_t opcode, struct bt_att *att, + void *user_data); + +typedef void (*gatt_db_write_t) (struct gatt_db_attribute *attrib, + unsigned int id, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + void *user_data); + +struct gatt_db_attribute * +gatt_db_service_add_characteristic(struct gatt_db_attribute *attrib, + const bt_uuid_t *uuid, + uint32_t permissions, + uint8_t properties, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + void *user_data); + +struct gatt_db_attribute * +gatt_db_service_add_descriptor(struct gatt_db_attribute *attrib, + const bt_uuid_t *uuid, + uint32_t permissions, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + void *user_data); + +struct gatt_db_attribute * +gatt_db_service_add_included(struct gatt_db_attribute *attrib, + struct gatt_db_attribute *include); + +bool gatt_db_service_set_active(struct gatt_db_attribute *attrib, bool active); +bool gatt_db_service_get_active(struct gatt_db_attribute *attrib); + +bool gatt_db_service_set_claimed(struct gatt_db_attribute *attrib, + bool claimed); +bool gatt_db_service_get_claimed(struct gatt_db_attribute *attrib); + +typedef void (*gatt_db_attribute_cb_t)(struct gatt_db_attribute *attrib, + void *user_data); + +void gatt_db_read_by_group_type(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t type, + struct queue *queue); + +unsigned int gatt_db_find_by_type(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t *type, + gatt_db_attribute_cb_t func, + void *user_data); + +unsigned int gatt_db_find_by_type_value(struct gatt_db *db, + uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t *type, + const void *value, + size_t value_len, + gatt_db_attribute_cb_t func, + void *user_data); + +void gatt_db_read_by_type(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + const bt_uuid_t type, + struct queue *queue); + +void gatt_db_find_information(struct gatt_db *db, uint16_t start_handle, + uint16_t end_handle, + struct queue *queue); + + +void gatt_db_foreach_service(struct gatt_db *db, const bt_uuid_t *uuid, + gatt_db_attribute_cb_t func, + void *user_data); +void gatt_db_foreach_service_in_range(struct gatt_db *db, + const bt_uuid_t *uuid, + gatt_db_attribute_cb_t func, + void *user_data, + uint16_t start_handle, + uint16_t end_handle); + +void gatt_db_service_foreach(struct gatt_db_attribute *attrib, + const bt_uuid_t *uuid, + gatt_db_attribute_cb_t func, + void *user_data); +void gatt_db_service_foreach_char(struct gatt_db_attribute *attrib, + gatt_db_attribute_cb_t func, + void *user_data); +void gatt_db_service_foreach_desc(struct gatt_db_attribute *attrib, + gatt_db_attribute_cb_t func, + void *user_data); +void gatt_db_service_foreach_incl(struct gatt_db_attribute *attrib, + gatt_db_attribute_cb_t func, + void *user_data); + +typedef void (*gatt_db_destroy_func_t)(void *user_data); + +unsigned int gatt_db_register(struct gatt_db *db, + gatt_db_attribute_cb_t service_added, + gatt_db_attribute_cb_t service_removed, + void *user_data, + gatt_db_destroy_func_t destroy); +bool gatt_db_unregister(struct gatt_db *db, unsigned int id); + +struct gatt_db_attribute *gatt_db_get_attribute(struct gatt_db *db, + uint16_t handle); + +struct gatt_db_attribute *gatt_db_get_service_with_uuid(struct gatt_db *db, + const bt_uuid_t *uuid); + +const bt_uuid_t *gatt_db_attribute_get_type( + const struct gatt_db_attribute *attrib); + +uint16_t gatt_db_attribute_get_handle(const struct gatt_db_attribute *attrib); + +bool gatt_db_attribute_get_service_uuid(const struct gatt_db_attribute *attrib, + bt_uuid_t *uuid); + +bool gatt_db_attribute_get_service_handles( + const struct gatt_db_attribute *attrib, + uint16_t *start_handle, + uint16_t *end_handle); + +bool gatt_db_attribute_get_service_data(const struct gatt_db_attribute *attrib, + uint16_t *start_handle, + uint16_t *end_handle, + bool *primary, + bt_uuid_t *uuid); + +bool gatt_db_attribute_get_char_data(const struct gatt_db_attribute *attrib, + uint16_t *handle, + uint16_t *value_handle, + uint8_t *properties, + bt_uuid_t *uuid); + +bool gatt_db_attribute_get_incl_data(const struct gatt_db_attribute *attrib, + uint16_t *handle, + uint16_t *start_handle, + uint16_t *end_handle); + +uint32_t +gatt_db_attribute_get_permissions(const struct gatt_db_attribute *attrib); + +typedef void (*gatt_db_attribute_read_t) (struct gatt_db_attribute *attrib, + int err, const uint8_t *value, + size_t length, void *user_data); + +bool gatt_db_attribute_read(struct gatt_db_attribute *attrib, uint16_t offset, + uint8_t opcode, struct bt_att *att, + gatt_db_attribute_read_t func, void *user_data); + +bool gatt_db_attribute_read_result(struct gatt_db_attribute *attrib, + unsigned int id, int err, + const uint8_t *value, size_t length); + +typedef void (*gatt_db_attribute_write_t) (struct gatt_db_attribute *attrib, + int err, void *user_data); + +bool gatt_db_attribute_write(struct gatt_db_attribute *attrib, uint16_t offset, + const uint8_t *value, size_t len, + uint8_t opcode, struct bt_att *att, + gatt_db_attribute_write_t func, + void *user_data); + +bool gatt_db_attribute_write_result(struct gatt_db_attribute *attrib, + unsigned int id, int err); + +bool gatt_db_attribute_reset(struct gatt_db_attribute *attrib); diff --git a/bluez/gatt-helpers.c b/bluez/gatt-helpers.c new file mode 100644 index 0000000..b337697 --- /dev/null +++ b/bluez/gatt-helpers.c @@ -0,0 +1,1490 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 "queue.h" +#include "att.h" +#include "uuid.h" +#include "gatt-helpers.h" +#include "util.h" + +#ifndef MIN +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#endif + +struct bt_gatt_result { + uint8_t opcode; + void *pdu; + uint16_t pdu_len; + uint16_t data_len; + + void *op; /* Discovery operation data */ + + struct bt_gatt_result *next; +}; + +static struct bt_gatt_result *result_create(uint8_t opcode, const void *pdu, + uint16_t pdu_len, + uint16_t data_len, + void *op) +{ + struct bt_gatt_result *result; + + result = new0(struct bt_gatt_result, 1); + if (!result) + return NULL; + + result->pdu = malloc(pdu_len); + if (!result->pdu) { + free(result); + return NULL; + } + + result->opcode = opcode; + result->pdu_len = pdu_len; + result->data_len = data_len; + result->op = op; + + memcpy(result->pdu, pdu, pdu_len); + + return result; +} + +static void result_destroy(struct bt_gatt_result *result) +{ + struct bt_gatt_result *next; + + while (result) { + next = result->next; + + free(result->pdu); + free(result); + + result = next; + } +} + +static unsigned int result_element_count(struct bt_gatt_result *result) +{ + unsigned int count = 0; + struct bt_gatt_result *cur; + + cur = result; + + while (cur) { + count += cur->pdu_len / cur->data_len; + cur = cur->next; + } + + return count; +} + +unsigned int bt_gatt_result_service_count(struct bt_gatt_result *result) +{ + if (!result) + return 0; + + if (result->opcode != BT_ATT_OP_READ_BY_GRP_TYPE_RSP && + result->opcode != BT_ATT_OP_FIND_BY_TYPE_VAL_RSP) + return 0; + + return result_element_count(result); +} + +unsigned int bt_gatt_result_characteristic_count(struct bt_gatt_result *result) +{ + if (!result) + return 0; + + if (result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP) + return 0; + + /* + * Data length contains 7 or 21 octets: + * 2 octets: Attribute handle + * 1 octet: Characteristic properties + * 2 octets: Characteristic value handle + * 2 or 16 octets: characteristic UUID + */ + if (result->data_len != 21 && result->data_len != 7) + return 0; + + return result_element_count(result); +} + +unsigned int bt_gatt_result_descriptor_count(struct bt_gatt_result *result) +{ + if (!result) + return 0; + + if (result->opcode != BT_ATT_OP_FIND_INFO_RSP) + return 0; + + return result_element_count(result); +} + +unsigned int bt_gatt_result_included_count(struct bt_gatt_result *result) +{ + struct bt_gatt_result *cur; + unsigned int count = 0; + + if (!result) + return 0; + + if (result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP) + return 0; + + /* + * Data length can be of length 6 or 8 octets: + * 2 octets - include service handle + * 2 octets - start handle of included service + * 2 octets - end handle of included service + * 2 octets (optionally) - 16 bit Bluetooth UUID + */ + if (result->data_len != 6 && result->data_len != 8) + return 0; + + for (cur = result; cur; cur = cur->next) + if (cur->opcode == BT_ATT_OP_READ_BY_TYPE_RSP) + count += cur->pdu_len / cur->data_len; + + return count; +} + +bool bt_gatt_iter_init(struct bt_gatt_iter *iter, struct bt_gatt_result *result) +{ + if (!iter || !result) + return false; + + iter->result = result; + iter->pos = 0; + + return true; +} + +static const uint8_t bt_base_uuid[16] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB +}; + +static bool convert_uuid_le(const uint8_t *src, size_t len, uint8_t dst[16]) +{ + if (len == 16) { + bswap_128(src, dst); + return true; + } + + if (len != 2) + return false; + + memcpy(dst, bt_base_uuid, sizeof(bt_base_uuid)); + dst[2] = src[1]; + dst[3] = src[0]; + + return true; +} + +struct bt_gatt_request { + struct bt_att *att; + unsigned int id; + uint16_t end_handle; + int ref_count; + bt_uuid_t uuid; + uint16_t service_type; + struct bt_gatt_result *result_head; + struct bt_gatt_result *result_tail; + bt_gatt_request_callback_t callback; + void *user_data; + bt_gatt_destroy_func_t destroy; +}; + +static struct bt_gatt_result *result_append(uint8_t opcode, const void *pdu, + uint16_t pdu_len, + uint16_t data_len, + struct bt_gatt_request *op) +{ + struct bt_gatt_result *result; + + result = result_create(opcode, pdu, pdu_len, data_len, op); + if (!result) + return NULL; + + if (!op->result_head) + op->result_head = op->result_tail = result; + else { + op->result_tail->next = result; + op->result_tail = result; + } + + return result; +} + +bool bt_gatt_iter_next_included_service(struct bt_gatt_iter *iter, + uint16_t *handle, uint16_t *start_handle, + uint16_t *end_handle, uint8_t uuid[16]) +{ + struct bt_gatt_result *read_result; + struct bt_gatt_request *op; + const void *pdu_ptr; + int i = 0; + + if (!iter || !iter->result || !handle || !start_handle || !end_handle + || !uuid) + return false; + + + if (iter->result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP) + return false; + + /* UUID in discovery_op is set in read_by_type and service_discovery */ + op = iter->result->op; + if (op->uuid.type != BT_UUID_UNSPEC) + return false; + /* + * iter->result points to READ_BY_TYPE_RSP with data length containing: + * 2 octets - include service handle + * 2 octets - start handle of included service + * 2 octets - end handle of included service + * optional 2 octets - Bluetooth UUID + */ + if (iter->result->data_len != 8 && iter->result->data_len != 6) + return false; + + pdu_ptr = iter->result->pdu + iter->pos; + + /* This result contains 16 bit UUID */ + if (iter->result->data_len == 8) { + *handle = get_le16(pdu_ptr); + *start_handle = get_le16(pdu_ptr + 2); + *end_handle = get_le16(pdu_ptr + 4); + convert_uuid_le(pdu_ptr + 6, 2, uuid); + + iter->pos += iter->result->data_len; + + if (iter->pos == iter->result->pdu_len) { + iter->result = iter->result->next; + iter->pos = 0; + } + + return true; + } + + *handle = get_le16(pdu_ptr); + *start_handle = get_le16(pdu_ptr + 2); + *end_handle = get_le16(pdu_ptr + 4); + read_result = iter->result; + + /* + * Find READ_RSP with include service UUID. + * If number of current data set in READ_BY_TYPE_RSP is n, then we must + * go to n'th PDU next to current item->result + */ + for (read_result = read_result->next; read_result; i++) { + if (i >= (iter->pos / iter->result->data_len)) + break; + + read_result = read_result->next; + } + + if (!read_result) + return false; + + convert_uuid_le(read_result->pdu, read_result->data_len, uuid); + iter->pos += iter->result->data_len; + if (iter->pos == iter->result->pdu_len) { + iter->result = read_result->next; + iter->pos = 0; + } + + return true; +} + +bool bt_gatt_iter_next_service(struct bt_gatt_iter *iter, + uint16_t *start_handle, uint16_t *end_handle, + uint8_t uuid[16]) +{ + struct bt_gatt_request *op; + const void *pdu_ptr; + bt_uuid_t tmp; + + if (!iter || !iter->result || !start_handle || !end_handle || !uuid) + return false; + + op = iter->result->op; + pdu_ptr = iter->result->pdu + iter->pos; + + switch (iter->result->opcode) { + case BT_ATT_OP_READ_BY_GRP_TYPE_RSP: + *start_handle = get_le16(pdu_ptr); + *end_handle = get_le16(pdu_ptr + 2); + convert_uuid_le(pdu_ptr + 4, iter->result->data_len - 4, uuid); + break; + case BT_ATT_OP_FIND_BY_TYPE_VAL_RSP: + *start_handle = get_le16(pdu_ptr); + *end_handle = get_le16(pdu_ptr + 2); + + bt_uuid_to_uuid128(&op->uuid, &tmp); + memcpy(uuid, tmp.value.u128.data, 16); + break; + default: + return false; + } + + + iter->pos += iter->result->data_len; + if (iter->pos == iter->result->pdu_len) { + iter->result = iter->result->next; + iter->pos = 0; + } + + return true; +} + +bool bt_gatt_iter_next_characteristic(struct bt_gatt_iter *iter, + uint16_t *start_handle, uint16_t *end_handle, + uint16_t *value_handle, uint8_t *properties, + uint8_t uuid[16]) +{ + struct bt_gatt_request *op; + const void *pdu_ptr; + + if (!iter || !iter->result || !start_handle || !end_handle || + !value_handle || !properties || !uuid) + return false; + + if (iter->result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP) + return false; + + /* UUID in discovery_op is set in read_by_type and service_discovery */ + op = iter->result->op; + if (op->uuid.type != BT_UUID_UNSPEC) + return false; + /* + * Data length contains 7 or 21 octets: + * 2 octets: Attribute handle + * 1 octet: Characteristic properties + * 2 octets: Characteristic value handle + * 2 or 16 octets: characteristic UUID + */ + if (iter->result->data_len != 21 && iter->result->data_len != 7) + return false; + + pdu_ptr = iter->result->pdu + iter->pos; + + *start_handle = get_le16(pdu_ptr); + *properties = ((uint8_t *) pdu_ptr)[2]; + *value_handle = get_le16(pdu_ptr + 3); + convert_uuid_le(pdu_ptr + 5, iter->result->data_len - 5, uuid); + + iter->pos += iter->result->data_len; + if (iter->pos == iter->result->pdu_len) { + iter->result = iter->result->next; + iter->pos = 0; + } + + if (!iter->result) { + *end_handle = op->end_handle; + return true; + } + + *end_handle = get_le16(iter->result->pdu + iter->pos) - 1; + + return true; +} + +bool bt_gatt_iter_next_descriptor(struct bt_gatt_iter *iter, uint16_t *handle, + uint8_t uuid[16]) +{ + const void *pdu_ptr; + + if (!iter || !iter->result || !handle || !uuid) + return false; + + if (iter->result->opcode != BT_ATT_OP_FIND_INFO_RSP) + return false; + + pdu_ptr = iter->result->pdu + iter->pos; + + *handle = get_le16(pdu_ptr); + convert_uuid_le(pdu_ptr + 2, iter->result->data_len - 2, uuid); + + iter->pos += iter->result->data_len; + if (iter->pos == iter->result->pdu_len) { + iter->result = iter->result->next; + iter->pos = 0; + } + + return true; +} + +bool bt_gatt_iter_next_read_by_type(struct bt_gatt_iter *iter, + uint16_t *handle, uint16_t *length, + const uint8_t **value) +{ + struct bt_gatt_request *op; + const void *pdu_ptr; + + if (!iter || !iter->result || !handle || !length || !value) + return false; + + if (iter->result->opcode != BT_ATT_OP_READ_BY_TYPE_RSP) + return false; + + /* + * Check if UUID is set, otherwise results can contain characteristic + * discovery service or included service discovery results + */ + op = iter->result->op; + if (op->uuid.type == BT_UUID_UNSPEC) + return false; + + pdu_ptr = iter->result->pdu + iter->pos; + + *handle = get_le16(pdu_ptr); + *length = iter->result->data_len - 2; + *value = pdu_ptr + 2; + + iter->pos += iter->result->data_len; + if (iter->pos == iter->result->pdu_len) { + iter->result = iter->result->next; + iter->pos = 0; + } + + return true; +} + +struct mtu_op { + struct bt_att *att; + uint16_t client_rx_mtu; + bt_gatt_result_callback_t callback; + void *user_data; + bt_gatt_destroy_func_t destroy; +}; + +static void destroy_mtu_op(void *user_data) +{ + struct mtu_op *op = user_data; + + if (op->destroy) + op->destroy(op->user_data); + + free(op); +} + +static uint8_t process_error(const void *pdu, uint16_t length) +{ + const struct bt_att_pdu_error_rsp *error_pdu; + + if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp)) + return 0; + + error_pdu = pdu; + + return error_pdu->ecode; +} + +static void mtu_cb(uint8_t opcode, const void *pdu, uint16_t length, + void *user_data) +{ + struct mtu_op *op = user_data; + bool success = true; + uint8_t att_ecode = 0; + uint16_t server_rx_mtu; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_MTU_RSP || !pdu || length != 2) { + success = false; + goto done; + } + + server_rx_mtu = get_le16(pdu); + bt_att_set_mtu(op->att, MIN(op->client_rx_mtu, server_rx_mtu)); + +done: + if (op->callback) + op->callback(success, att_ecode, op->user_data); +} + +unsigned int bt_gatt_exchange_mtu(struct bt_att *att, uint16_t client_rx_mtu, + bt_gatt_result_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + struct mtu_op *op; + uint8_t pdu[2]; + unsigned int id; + + if (!att || !client_rx_mtu) + return false; + + op = new0(struct mtu_op, 1); + if (!op) + return false; + + op->att = att; + op->client_rx_mtu = client_rx_mtu; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + + put_le16(client_rx_mtu, pdu); + + id = bt_att_send(att, BT_ATT_OP_MTU_REQ, pdu, sizeof(pdu), mtu_cb, op, + destroy_mtu_op); + if (!id) + free(op); + + return id; +} + +static inline int get_uuid_len(const bt_uuid_t *uuid) +{ + if (!uuid) + return 0; + + return (uuid->type == BT_UUID16) ? 2 : 16; +} + +struct bt_gatt_request *bt_gatt_request_ref(struct bt_gatt_request *req) +{ + if (!req) + return NULL; + + __sync_fetch_and_add(&req->ref_count, 1); + + return req; +} + +void bt_gatt_request_unref(struct bt_gatt_request *req) +{ + if (!req) + return; + + if (__sync_sub_and_fetch(&req->ref_count, 1)) + return; + + bt_gatt_request_cancel(req); + + if (req->destroy) + req->destroy(req->user_data); + + result_destroy(req->result_head); + + free(req); +} + +void bt_gatt_request_cancel(struct bt_gatt_request *req) +{ + if (!req) + return; + + if (!req->id) + return; + + bt_att_cancel(req->att, req->id); + req->id = 0; +} + +static void async_req_unref(void *data) +{ + struct bt_gatt_request *req = data; + + bt_gatt_request_unref(req); +} + +static void discovery_op_complete(struct bt_gatt_request *op, bool success, + uint8_t ecode) +{ + if (op->callback) + op->callback(success, ecode, success ? op->result_head : NULL, + op->user_data); + + if (!op->id) + async_req_unref(op); + else + op->id = 0; + +} + +static void read_by_grp_type_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct bt_gatt_request *op = user_data; + bool success; + uint8_t att_ecode = 0; + struct bt_gatt_result *cur_result; + size_t data_length; + size_t list_length; + uint16_t last_end; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && + op->result_head) + goto success; + + goto done; + } + + /* PDU must contain at least the following (sans opcode): + * - Attr Data Length (1 octet) + * - Attr Data List (at least 6 octets): + * -- 2 octets: Attribute handle + * -- 2 octets: End group handle + * -- 2 or 16 octets: service UUID + */ + if (opcode != BT_ATT_OP_READ_BY_GRP_TYPE_RSP || !pdu || length < 7) { + success = false; + goto done; + } + + data_length = ((uint8_t *) pdu)[0]; + list_length = length - 1; + + if ((data_length != 6 && data_length != 20) || + (list_length % data_length)) { + success = false; + goto done; + } + + /* PDU is correctly formatted. Get the last end handle to process the + * next request and store the PDU. + */ + cur_result = result_append(opcode, pdu + 1, list_length, data_length, + op); + if (!cur_result) { + success = false; + goto done; + } + + last_end = get_le16(pdu + length - data_length + 2); + if (last_end < op->end_handle) { + uint8_t pdu[6]; + + put_le16(last_end + 1, pdu); + put_le16(op->end_handle, pdu + 2); + put_le16(op->service_type, pdu + 4); + + op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_GRP_TYPE_REQ, + pdu, sizeof(pdu), + read_by_grp_type_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto done; + } + + /* Some devices incorrectly return 0xffff as the end group handle when + * the read-by-group-type request is performed within a smaller range. + * Manually set the end group handle that we report in the result to the + * end handle in the original request. + */ + if (last_end == 0xffff && last_end != op->end_handle) + put_le16(op->end_handle, + cur_result->pdu + length - data_length + 1); + +success: + success = true; + +done: + discovery_op_complete(op, success, att_ecode); +} + +static void find_by_type_val_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct bt_gatt_request *op = user_data; + bool success; + uint8_t att_ecode = 0; + uint16_t last_end; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && + op->result_head) + goto success; + + goto done; + } + + /* PDU must contain 4 bytes and it must be a multiple of 4, where each + * 4 bytes contain the 16-bit attribute and group end handles. + */ + if (opcode != BT_ATT_OP_FIND_BY_TYPE_VAL_RSP || !pdu || !length || + length % 4) { + success = false; + goto done; + } + + if (!result_append(opcode, pdu, length, 4, op)) { + success = false; + goto done; + } + + /* + * Each data set contains: + * 2 octets with start handle + * 2 octets with end handle + * last_end is end handle of last data set + */ + last_end = get_le16(pdu + length - 2); + if (last_end < op->end_handle) { + uint8_t pdu[6 + get_uuid_len(&op->uuid)]; + + put_le16(last_end + 1, pdu); + put_le16(op->end_handle, pdu + 2); + put_le16(op->service_type, pdu + 4); + bt_uuid_to_le(&op->uuid, pdu + 6); + + op->id = bt_att_send(op->att, BT_ATT_OP_FIND_BY_TYPE_VAL_REQ, + pdu, sizeof(pdu), + find_by_type_val_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto done; + } + +success: + success = true; + +done: + discovery_op_complete(op, success, att_ecode); +} + +static struct bt_gatt_request *discover_services(struct bt_att *att, + bt_uuid_t *uuid, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy, + bool primary) +{ + struct bt_gatt_request *op; + + if (!att) + return NULL; + + op = new0(struct bt_gatt_request, 1); + if (!op) + return NULL; + + op->att = att; + op->end_handle = end; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + /* set service uuid to primary or secondary */ + op->service_type = primary ? GATT_PRIM_SVC_UUID : GATT_SND_SVC_UUID; + + /* If UUID is NULL, then discover all primary services */ + if (!uuid) { + uint8_t pdu[6]; + + put_le16(start, pdu); + put_le16(end, pdu + 2); + put_le16(op->service_type, pdu + 4); + + op->id = bt_att_send(att, BT_ATT_OP_READ_BY_GRP_TYPE_REQ, + pdu, sizeof(pdu), + read_by_grp_type_cb, + bt_gatt_request_ref(op), + async_req_unref); + } else { + uint8_t pdu[6 + get_uuid_len(uuid)]; + + if (uuid->type == BT_UUID_UNSPEC) { + free(op); + return NULL; + } + + /* Discover by UUID */ + op->uuid = *uuid; + + put_le16(start, pdu); + put_le16(end, pdu + 2); + put_le16(op->service_type, pdu + 4); + bt_uuid_to_le(&op->uuid, pdu + 6); + + op->id = bt_att_send(att, BT_ATT_OP_FIND_BY_TYPE_VAL_REQ, + pdu, sizeof(pdu), + find_by_type_val_cb, + bt_gatt_request_ref(op), + async_req_unref); + } + + if (!op->id) { + free(op); + return NULL; + } + + return bt_gatt_request_ref(op); +} + +struct bt_gatt_request *bt_gatt_discover_all_primary_services( + struct bt_att *att, bt_uuid_t *uuid, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + return bt_gatt_discover_primary_services(att, uuid, 0x0001, 0xffff, + callback, user_data, + destroy); +} + +struct bt_gatt_request *bt_gatt_discover_primary_services( + struct bt_att *att, bt_uuid_t *uuid, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + return discover_services(att, uuid, start, end, callback, user_data, + destroy, true); +} + +struct bt_gatt_request *bt_gatt_discover_secondary_services( + struct bt_att *att, bt_uuid_t *uuid, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + return discover_services(att, uuid, start, end, callback, user_data, + destroy, false); +} + +struct read_incl_data { + struct bt_gatt_request *op; + struct bt_gatt_result *result; + int pos; + int ref_count; +}; + +static struct read_incl_data *new_read_included(struct bt_gatt_result *res) +{ + struct read_incl_data *data; + + data = new0(struct read_incl_data, 1); + if (!data) + return NULL; + + data->op = bt_gatt_request_ref(res->op); + data->result = res; + + return data; +}; + +static struct read_incl_data *read_included_ref(struct read_incl_data *data) +{ + __sync_fetch_and_add(&data->ref_count, 1); + + return data; +} + +static void read_included_unref(void *data) +{ + struct read_incl_data *read_data = data; + + if (__sync_sub_and_fetch(&read_data->ref_count, 1)) + return; + + async_req_unref(read_data->op); + + free(read_data); +} + +static void discover_included_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data); + +static void read_included_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct read_incl_data *data = user_data; + struct bt_gatt_request *op = data->op; + uint8_t att_ecode = 0; + uint8_t read_pdu[2]; + bool success; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + goto done; + } + + if (opcode != BT_ATT_OP_READ_RSP || (!pdu && length)) { + success = false; + goto done; + } + + /* + * UUID should be in 128 bit format, as it couldn't be read in + * READ_BY_TYPE request + */ + if (length != 16) { + success = false; + goto done; + } + + if (!result_append(opcode, pdu, length, length, op)) { + success = false; + goto done; + } + + if (data->pos == data->result->pdu_len) { + uint16_t last_handle; + uint8_t pdu[6]; + + last_handle = get_le16(data->result->pdu + data->pos - + data->result->data_len); + if (last_handle == op->end_handle) { + success = true; + goto done; + } + + put_le16(last_handle + 1, pdu); + put_le16(op->end_handle, pdu + 2); + put_le16(GATT_INCLUDE_UUID, pdu + 4); + + op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ, + pdu, sizeof(pdu), + discover_included_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto done; + } + + memcpy(read_pdu, data->result->pdu + data->pos + 2, sizeof(uint16_t)); + + data->pos += data->result->data_len; + + if (bt_att_send(op->att, BT_ATT_OP_READ_REQ, read_pdu, sizeof(read_pdu), + read_included_cb, read_included_ref(data), + read_included_unref)) + return; + + read_included_unref(data); + success = false; + +done: + discovery_op_complete(op, success, att_ecode); +} + +static void read_included(struct read_incl_data *data) +{ + struct bt_gatt_request *op = data->op; + uint8_t pdu[2]; + + memcpy(pdu, data->result->pdu + 2, sizeof(uint16_t)); + + data->pos += data->result->data_len; + + if (bt_att_send(op->att, BT_ATT_OP_READ_REQ, pdu, sizeof(pdu), + read_included_cb, + read_included_ref(data), + read_included_unref)) + return; + + if (op->callback) + op->callback(false, 0, NULL, data->op->user_data); + + read_included_unref(data); +} + +static void discover_included_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct bt_gatt_request *op = user_data; + struct bt_gatt_result *cur_result; + uint8_t att_ecode = 0; + uint16_t last_handle; + size_t data_length; + bool success; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + att_ecode = process_error(pdu, length); + + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && + op->result_head) + goto done; + + success = false; + goto failed; + } + + if (opcode != BT_ATT_OP_READ_BY_TYPE_RSP || !pdu || length < 6) { + success = false; + goto failed; + } + + data_length = ((const uint8_t *) pdu)[0]; + + /* + * Check if PDU contains data sets with length declared in the beginning + * of frame and if this length is correct. + * Data set length may be 6 or 8 octets: + * 2 octets - include service handle + * 2 octets - start handle of included service + * 2 octets - end handle of included service + * optional 2 octets - Bluetooth UUID of included service + */ + if ((data_length != 8 && data_length != 6) || + (length - 1) % data_length) { + success = false; + goto failed; + } + + cur_result = result_append(opcode, pdu + 1, length - 1, data_length, + op); + if (!cur_result) { + success = false; + goto failed; + } + + if (data_length == 6) { + struct read_incl_data *data; + + data = new_read_included(cur_result); + if (!data) { + success = false; + goto failed; + } + + read_included(data); + return; + } + + last_handle = get_le16(pdu + length - data_length); + if (last_handle != op->end_handle) { + uint8_t pdu[6]; + + put_le16(last_handle + 1, pdu); + put_le16(op->end_handle, pdu + 2); + put_le16(GATT_INCLUDE_UUID, pdu + 4); + + op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ, + pdu, sizeof(pdu), + discover_included_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto failed; + } + +done: + success = true; + +failed: + discovery_op_complete(op, success, att_ecode); +} + +struct bt_gatt_request *bt_gatt_discover_included_services(struct bt_att *att, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + struct bt_gatt_request *op; + uint8_t pdu[6]; + + if (!att) + return false; + + op = new0(struct bt_gatt_request, 1); + if (!op) + return false; + + op->att = att; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + op->end_handle = end; + + put_le16(start, pdu); + put_le16(end, pdu + 2); + put_le16(GATT_INCLUDE_UUID, pdu + 4); + + op->id = bt_att_send(att, BT_ATT_OP_READ_BY_TYPE_REQ, pdu, sizeof(pdu), + discover_included_cb, bt_gatt_request_ref(op), + async_req_unref); + if (!op->id) { + free(op); + return NULL; + } + + return bt_gatt_request_ref(op); +} + +static void discover_chrcs_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct bt_gatt_request *op = user_data; + bool success; + uint8_t att_ecode = 0; + size_t data_length; + uint16_t last_handle; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && + op->result_head) + goto success; + + goto done; + } + + /* PDU must contain at least the following (sans opcode): + * - Attr Data Length (1 octet) + * - Attr Data List (at least 7 octets): + * -- 2 octets: Attribute handle + * -- 1 octet: Characteristic properties + * -- 2 octets: Characteristic value handle + * -- 2 or 16 octets: characteristic UUID + */ + if (opcode != BT_ATT_OP_READ_BY_TYPE_RSP || !pdu || length < 8) { + success = false; + goto done; + } + + data_length = ((uint8_t *) pdu)[0]; + + if ((data_length != 7 && data_length != 21) || + ((length - 1) % data_length)) { + success = false; + goto done; + } + + if (!result_append(opcode, pdu + 1, length - 1, + data_length, op)) { + success = false; + goto done; + } + last_handle = get_le16(pdu + length - data_length); + if (last_handle != op->end_handle) { + uint8_t pdu[6]; + + put_le16(last_handle + 1, pdu); + put_le16(op->end_handle, pdu + 2); + put_le16(GATT_CHARAC_UUID, pdu + 4); + + op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ, + pdu, sizeof(pdu), + discover_chrcs_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto done; + } + +success: + success = true; + +done: + discovery_op_complete(op, success, att_ecode); +} + +struct bt_gatt_request *bt_gatt_discover_characteristics(struct bt_att *att, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + struct bt_gatt_request *op; + uint8_t pdu[6]; + + if (!att) + return false; + + op = new0(struct bt_gatt_request, 1); + if (!op) + return false; + + op->att = att; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + op->end_handle = end; + + put_le16(start, pdu); + put_le16(end, pdu + 2); + put_le16(GATT_CHARAC_UUID, pdu + 4); + + op->id = bt_att_send(att, BT_ATT_OP_READ_BY_TYPE_REQ, pdu, sizeof(pdu), + discover_chrcs_cb, bt_gatt_request_ref(op), + async_req_unref); + if (!op->id) { + free(op); + return NULL; + } + + return bt_gatt_request_ref(op); +} + +static void read_by_type_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct bt_gatt_request *op = user_data; + bool success; + uint8_t att_ecode = 0; + size_t data_length; + uint16_t last_handle; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + att_ecode = process_error(pdu, length); + + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && + op->result_head) + success = true; + else + success = false; + + goto done; + } + + if (opcode != BT_ATT_OP_READ_BY_TYPE_RSP || !pdu) { + success = false; + att_ecode = 0; + goto done; + } + + data_length = ((uint8_t *) pdu)[0]; + if (((length - 1) % data_length)) { + success = false; + att_ecode = 0; + goto done; + } + + if (!result_append(opcode, pdu + 1, length - 1, data_length, op)) { + success = false; + att_ecode = 0; + goto done; + } + + last_handle = get_le16(pdu + length - data_length); + if (last_handle != op->end_handle) { + uint8_t pdu[4 + get_uuid_len(&op->uuid)]; + + put_le16(last_handle + 1, pdu); + put_le16(op->end_handle, pdu + 2); + bt_uuid_to_le(&op->uuid, pdu + 4); + + op->id = bt_att_send(op->att, BT_ATT_OP_READ_BY_TYPE_REQ, + pdu, sizeof(pdu), + read_by_type_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto done; + } + + success = true; + +done: + discovery_op_complete(op, success, att_ecode); +} + +bool bt_gatt_read_by_type(struct bt_att *att, uint16_t start, uint16_t end, + const bt_uuid_t *uuid, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + struct bt_gatt_request *op; + uint8_t pdu[4 + get_uuid_len(uuid)]; + + if (!att || !uuid || uuid->type == BT_UUID_UNSPEC) + return false; + + op = new0(struct bt_gatt_request, 1); + if (!op) + return false; + + op->att = att; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + op->end_handle = end; + op->uuid = *uuid; + + put_le16(start, pdu); + put_le16(end, pdu + 2); + bt_uuid_to_le(uuid, pdu + 4); + + op->id = bt_att_send(att, BT_ATT_OP_READ_BY_TYPE_REQ, pdu, sizeof(pdu), + read_by_type_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return true; + + free(op); + return false; +} + +static void discover_descs_cb(uint8_t opcode, const void *pdu, + uint16_t length, void *user_data) +{ + struct bt_gatt_request *op = user_data; + bool success; + uint8_t att_ecode = 0; + uint8_t format; + uint16_t last_handle; + size_t data_length; + + if (opcode == BT_ATT_OP_ERROR_RSP) { + success = false; + att_ecode = process_error(pdu, length); + + if (att_ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && + op->result_head) + goto success; + + goto done; + } + + /* The PDU should contain the following data (sans opcode): + * - Format (1 octet) + * - Attr Data List (at least 4 octets): + * -- 2 octets: Attribute handle + * -- 2 or 16 octets: UUID. + */ + if (opcode != BT_ATT_OP_FIND_INFO_RSP || !pdu || length < 5) { + success = false; + goto done; + } + + format = ((uint8_t *) pdu)[0]; + + if (format == 0x01) + data_length = 4; + else if (format == 0x02) + data_length = 18; + else { + success = false; + goto done; + } + + if ((length - 1) % data_length) { + success = false; + goto done; + } + + if (!result_append(opcode, pdu + 1, length - 1, data_length, op)) { + success = false; + goto done; + } + + last_handle = get_le16(pdu + length - data_length); + if (last_handle != op->end_handle) { + uint8_t pdu[4]; + + put_le16(last_handle + 1, pdu); + put_le16(op->end_handle, pdu + 2); + + op->id = bt_att_send(op->att, BT_ATT_OP_FIND_INFO_REQ, + pdu, sizeof(pdu), + discover_descs_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (op->id) + return; + + success = false; + goto done; + } + +success: + success = true; + +done: + discovery_op_complete(op, success, att_ecode); +} + +struct bt_gatt_request *bt_gatt_discover_descriptors(struct bt_att *att, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy) +{ + struct bt_gatt_request *op; + uint8_t pdu[4]; + + if (!att) + return false; + + op = new0(struct bt_gatt_request, 1); + if (!op) + return false; + + op->att = att; + op->callback = callback; + op->user_data = user_data; + op->destroy = destroy; + op->end_handle = end; + + put_le16(start, pdu); + put_le16(end, pdu + 2); + + op->id = bt_att_send(att, BT_ATT_OP_FIND_INFO_REQ, pdu, sizeof(pdu), + discover_descs_cb, + bt_gatt_request_ref(op), + async_req_unref); + if (!op->id) { + free(op); + return NULL; + } + + return bt_gatt_request_ref(op); +} diff --git a/bluez/gatt-helpers.h b/bluez/gatt-helpers.h new file mode 100644 index 0000000..dd9dd1c --- /dev/null +++ b/bluez/gatt-helpers.h @@ -0,0 +1,116 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 Google Inc. + * + * + * 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 + * + */ + +/* This file defines helpers for performing client-side procedures defined by + * the Generic Attribute Profile. + */ + +#include +#include + +struct bt_gatt_result; + +struct bt_gatt_iter { + struct bt_gatt_result *result; + uint16_t pos; +}; + +unsigned int bt_gatt_result_service_count(struct bt_gatt_result *result); +unsigned int bt_gatt_result_characteristic_count(struct bt_gatt_result *result); +unsigned int bt_gatt_result_descriptor_count(struct bt_gatt_result *result); +unsigned int bt_gatt_result_included_count(struct bt_gatt_result *result); + +bool bt_gatt_iter_init(struct bt_gatt_iter *iter, struct bt_gatt_result *result); +bool bt_gatt_iter_next_service(struct bt_gatt_iter *iter, + uint16_t *start_handle, uint16_t *end_handle, + uint8_t uuid[16]); +bool bt_gatt_iter_next_characteristic(struct bt_gatt_iter *iter, + uint16_t *start_handle, uint16_t *end_handle, + uint16_t *value_handle, uint8_t *properties, + uint8_t uuid[16]); +bool bt_gatt_iter_next_descriptor(struct bt_gatt_iter *iter, uint16_t *handle, + uint8_t uuid[16]); +bool bt_gatt_iter_next_included_service(struct bt_gatt_iter *iter, + uint16_t *handle, uint16_t *start_handle, + uint16_t *end_handle, uint8_t uuid[16]); +bool bt_gatt_iter_next_read_by_type(struct bt_gatt_iter *iter, + uint16_t *handle, uint16_t *length, + const uint8_t **value); + +typedef void (*bt_gatt_destroy_func_t)(void *user_data); + +typedef void (*bt_gatt_result_callback_t)(bool success, uint8_t att_ecode, + void *user_data); +typedef void (*bt_gatt_request_callback_t)(bool success, uint8_t att_ecode, + struct bt_gatt_result *result, + void *user_data); + +struct bt_gatt_request; + +struct bt_gatt_request *bt_gatt_request_ref(struct bt_gatt_request *req); +void bt_gatt_request_unref(struct bt_gatt_request *req); +void bt_gatt_request_cancel(struct bt_gatt_request *req); + +unsigned int bt_gatt_exchange_mtu(struct bt_att *att, uint16_t client_rx_mtu, + bt_gatt_result_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); + +struct bt_gatt_request *bt_gatt_discover_all_primary_services( + struct bt_att *att, bt_uuid_t *uuid, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); +struct bt_gatt_request *bt_gatt_discover_primary_services( + struct bt_att *att, bt_uuid_t *uuid, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); +struct bt_gatt_request *bt_gatt_discover_secondary_services( + struct bt_att *att, bt_uuid_t *uuid, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); +struct bt_gatt_request *bt_gatt_discover_included_services(struct bt_att *att, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); +struct bt_gatt_request *bt_gatt_discover_characteristics(struct bt_att *att, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); +struct bt_gatt_request *bt_gatt_discover_descriptors(struct bt_att *att, + uint16_t start, uint16_t end, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); + +bool bt_gatt_read_by_type(struct bt_att *att, uint16_t start, uint16_t end, + const bt_uuid_t *uuid, + bt_gatt_request_callback_t callback, + void *user_data, + bt_gatt_destroy_func_t destroy); diff --git a/bluez/io-mainloop.c b/bluez/io-mainloop.c new file mode 100644 index 0000000..752b05d --- /dev/null +++ b/bluez/io-mainloop.c @@ -0,0 +1,325 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 +#include +#include + +#include "mainloop.h" +#include "util.h" +#include "io.h" + +struct io { + int ref_count; + int fd; + uint32_t events; + bool close_on_destroy; + io_callback_func_t read_callback; + io_destroy_func_t read_destroy; + void *read_data; + io_callback_func_t write_callback; + io_destroy_func_t write_destroy; + void *write_data; + io_callback_func_t disconnect_callback; + io_destroy_func_t disconnect_destroy; + void *disconnect_data; +}; + +static struct io *io_ref(struct io *io) +{ + if (!io) + return NULL; + + __sync_fetch_and_add(&io->ref_count, 1); + + return io; +} + +static void io_unref(struct io *io) +{ + if (!io) + return; + + if (__sync_sub_and_fetch(&io->ref_count, 1)) + return; + + free(io); +} + +static void io_cleanup(void *user_data) +{ + struct io *io = user_data; + + if (io->write_destroy) + io->write_destroy(io->write_data); + + if (io->read_destroy) + io->read_destroy(io->read_data); + + if (io->disconnect_destroy) + io->disconnect_destroy(io->disconnect_data); + + if (io->close_on_destroy) + close(io->fd); + + io->fd = -1; +} + +static void io_callback(int fd, uint32_t events, void *user_data) +{ + struct io *io = user_data; + + io_ref(io); + + if ((events & (EPOLLRDHUP | EPOLLHUP | EPOLLERR))) { + io->read_callback = NULL; + io->write_callback = NULL; + + if (!io->disconnect_callback) { + mainloop_remove_fd(io->fd); + io_unref(io); + return; + } + + if (!io->disconnect_callback(io, io->disconnect_data)) { + if (io->disconnect_destroy) + io->disconnect_destroy(io->disconnect_data); + + io->disconnect_callback = NULL; + io->disconnect_destroy = NULL; + io->disconnect_data = NULL; + + io->events &= ~EPOLLRDHUP; + + mainloop_modify_fd(io->fd, io->events); + } + } + + if ((events & EPOLLIN) && io->read_callback) { + if (!io->read_callback(io, io->read_data)) { + if (io->read_destroy) + io->read_destroy(io->read_data); + + io->read_callback = NULL; + io->read_destroy = NULL; + io->read_data = NULL; + + io->events &= ~EPOLLIN; + + mainloop_modify_fd(io->fd, io->events); + } + } + + if ((events & EPOLLOUT) && io->write_callback) { + if (!io->write_callback(io, io->write_data)) { + if (io->write_destroy) + io->write_destroy(io->write_data); + + io->write_callback = NULL; + io->write_destroy = NULL; + io->write_data = NULL; + + io->events &= ~EPOLLOUT; + + mainloop_modify_fd(io->fd, io->events); + } + } + + io_unref(io); +} + +struct io *io_new(int fd) +{ + struct io *io; + + if (fd < 0) + return NULL; + + io = new0(struct io, 1); + if (!io) + return NULL; + + io->fd = fd; + io->events = 0; + io->close_on_destroy = false; + + if (mainloop_add_fd(io->fd, io->events, io_callback, + io, io_cleanup) < 0) { + free(io); + return NULL; + } + + return io_ref(io); +} + +void io_destroy(struct io *io) +{ + if (!io) + return; + + io->read_callback = NULL; + io->write_callback = NULL; + io->disconnect_callback = NULL; + + mainloop_remove_fd(io->fd); + + io_unref(io); +} + +int io_get_fd(struct io *io) +{ + if (!io) + return -ENOTCONN; + + return io->fd; +} + +bool io_set_close_on_destroy(struct io *io, bool do_close) +{ + if (!io) + return false; + + io->close_on_destroy = do_close; + + return true; +} + +bool io_set_read_handler(struct io *io, io_callback_func_t callback, + void *user_data, io_destroy_func_t destroy) +{ + uint32_t events; + + if (!io || io->fd < 0) + return false; + + if (io->read_destroy) + io->read_destroy(io->read_data); + + if (callback) + events = io->events | EPOLLIN; + else + events = io->events & ~EPOLLIN; + + io->read_callback = callback; + io->read_destroy = destroy; + io->read_data = user_data; + + if (events == io->events) + return true; + + if (mainloop_modify_fd(io->fd, events) < 0) + return false; + + io->events = events; + + return true; +} + +bool io_set_write_handler(struct io *io, io_callback_func_t callback, + void *user_data, io_destroy_func_t destroy) +{ + uint32_t events; + + if (!io || io->fd < 0) + return false; + + if (io->write_destroy) + io->write_destroy(io->write_data); + + if (callback) + events = io->events | EPOLLOUT; + else + events = io->events & ~EPOLLOUT; + + io->write_callback = callback; + io->write_destroy = destroy; + io->write_data = user_data; + + if (events == io->events) + return true; + + if (mainloop_modify_fd(io->fd, events) < 0) + return false; + + io->events = events; + + return true; +} + +bool io_set_disconnect_handler(struct io *io, io_callback_func_t callback, + void *user_data, io_destroy_func_t destroy) +{ + uint32_t events; + + if (!io || io->fd < 0) + return false; + + if (io->disconnect_destroy) + io->disconnect_destroy(io->disconnect_data); + + if (callback) + events = io->events | EPOLLRDHUP; + else + events = io->events & ~EPOLLRDHUP; + + io->disconnect_callback = callback; + io->disconnect_destroy = destroy; + io->disconnect_data = user_data; + + if (events == io->events) + return true; + + if (mainloop_modify_fd(io->fd, events) < 0) + return false; + + io->events = events; + + return true; +} + +ssize_t io_send(struct io *io, const struct iovec *iov, int iovcnt) +{ + ssize_t ret; + + if (!io || io->fd < 0) + return -ENOTCONN; + + do { + ret = writev(io->fd, iov, iovcnt); + } while (ret < 0 && errno == EINTR); + + if (ret < 0) + return -errno; + + return ret; +} + +bool io_shutdown(struct io *io) +{ + if (!io || io->fd < 0) + return false; + + return shutdown(io->fd, SHUT_RDWR) == 0; +} diff --git a/bluez/io.h b/bluez/io.h new file mode 100644 index 0000000..8bc1111 --- /dev/null +++ b/bluez/io.h @@ -0,0 +1,47 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 (*io_destroy_func_t)(void *data); + +struct io; + +struct io *io_new(int fd); +void io_destroy(struct io *io); + +int io_get_fd(struct io *io); +bool io_set_close_on_destroy(struct io *io, bool do_close); + +ssize_t io_send(struct io *io, const struct iovec *iov, int iovcnt); +bool io_shutdown(struct io *io); + +typedef bool (*io_callback_func_t)(struct io *io, void *user_data); + +bool io_set_read_handler(struct io *io, io_callback_func_t callback, + void *user_data, io_destroy_func_t destroy); +bool io_set_write_handler(struct io *io, io_callback_func_t callback, + void *user_data, io_destroy_func_t destroy); +bool io_set_disconnect_handler(struct io *io, io_callback_func_t callback, + void *user_data, io_destroy_func_t destroy); diff --git a/bluez/mainloop.c b/bluez/mainloop.c new file mode 100644 index 0000000..b769c8f --- /dev/null +++ b/bluez/mainloop.c @@ -0,0 +1,405 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2014 Intel Corporation + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mainloop.h" + +#define MAX_EPOLL_EVENTS 10 + +static int epoll_fd; +static int epoll_terminate; +static int exit_status; + +struct mainloop_data { + int fd; + uint32_t events; + mainloop_event_func callback; + mainloop_destroy_func destroy; + void *user_data; +}; + +#define MAX_MAINLOOP_ENTRIES 128 + +static struct mainloop_data *mainloop_list[MAX_MAINLOOP_ENTRIES]; + +struct timeout_data { + int fd; + mainloop_timeout_func callback; + mainloop_destroy_func destroy; + void *user_data; +}; + +struct signal_data { + int fd; + sigset_t mask; + mainloop_signal_func callback; + mainloop_destroy_func destroy; + void *user_data; +}; + +static struct signal_data *signal_data; + +void mainloop_init(void) +{ + unsigned int i; + + epoll_fd = epoll_create1(EPOLL_CLOEXEC); + + for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) + mainloop_list[i] = NULL; + + epoll_terminate = 0; +} + +void mainloop_quit(void) +{ + epoll_terminate = 1; +} + +void mainloop_exit_success(void) +{ + exit_status = EXIT_SUCCESS; + epoll_terminate = 1; +} + +void mainloop_exit_failure(void) +{ + exit_status = EXIT_FAILURE; + epoll_terminate = 1; +} + +static void signal_callback(int fd, uint32_t events, void *user_data) +{ + struct signal_data *data = user_data; + struct signalfd_siginfo si; + ssize_t result; + + if (events & (EPOLLERR | EPOLLHUP)) { + mainloop_quit(); + return; + } + + result = read(fd, &si, sizeof(si)); + if (result != sizeof(si)) + return; + + if (data->callback) + data->callback(si.ssi_signo, data->user_data); +} + +int mainloop_run(void) +{ + if (signal_data) { + if (sigprocmask(SIG_BLOCK, &signal_data->mask, NULL) < 0) + return EXIT_FAILURE; + + signal_data->fd = signalfd(-1, &signal_data->mask, + SFD_NONBLOCK | SFD_CLOEXEC); + if (signal_data->fd < 0) + return EXIT_FAILURE; + + if (mainloop_add_fd(signal_data->fd, EPOLLIN, + signal_callback, signal_data, NULL) < 0) { + close(signal_data->fd); + return EXIT_FAILURE; + } + } + + exit_status = EXIT_SUCCESS; + + while (!epoll_terminate) { + struct epoll_event events[MAX_EPOLL_EVENTS]; + int n, nfds; + + nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1); + if (nfds < 0) + continue; + + for (n = 0; n < nfds; n++) { + struct mainloop_data *data = events[n].data.ptr; + + data->callback(data->fd, events[n].events, + data->user_data); + } + } + + if (signal_data) { + mainloop_remove_fd(signal_data->fd); + close(signal_data->fd); + + if (signal_data->destroy) + signal_data->destroy(signal_data->user_data); + } + epoll_terminate = 0; + + + return exit_status; +} + +void mainloop_finish(void) +{ + unsigned int i; + + for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) { + struct mainloop_data *data = mainloop_list[i]; + + mainloop_list[i] = NULL; + + if (data) { + epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL); + + if (data->destroy) + data->destroy(data->user_data); + + free(data); + } + } + + close(epoll_fd); + epoll_fd = 0; +} + +int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback, + void *user_data, mainloop_destroy_func destroy) +{ + struct mainloop_data *data; + struct epoll_event ev; + int err; + + if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1 || !callback) + return -EINVAL; + + data = malloc(sizeof(*data)); + if (!data) + return -ENOMEM; + + memset(data, 0, sizeof(*data)); + data->fd = fd; + data->events = events; + data->callback = callback; + data->destroy = destroy; + data->user_data = user_data; + + memset(&ev, 0, sizeof(ev)); + ev.events = events; + ev.data.ptr = data; + + err = epoll_ctl(epoll_fd, EPOLL_CTL_ADD, data->fd, &ev); + if (err < 0) { + free(data); + return err; + } + + mainloop_list[fd] = data; + + return 0; +} + +int mainloop_modify_fd(int fd, uint32_t events) +{ + struct mainloop_data *data; + struct epoll_event ev; + int err; + + if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1) + return -EINVAL; + + data = mainloop_list[fd]; + if (!data) + return -ENXIO; + + memset(&ev, 0, sizeof(ev)); + ev.events = events; + ev.data.ptr = data; + + err = epoll_ctl(epoll_fd, EPOLL_CTL_MOD, data->fd, &ev); + if (err < 0) + return err; + + data->events = events; + + return 0; +} + +int mainloop_remove_fd(int fd) +{ + struct mainloop_data *data; + int err; + + if (fd < 0 || fd > MAX_MAINLOOP_ENTRIES - 1) + return -EINVAL; + + data = mainloop_list[fd]; + if (!data) + return -ENXIO; + + mainloop_list[fd] = NULL; + + err = epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL); + + if (data->destroy) + data->destroy(data->user_data); + + free(data); + + return err; +} + +static void timeout_destroy(void *user_data) +{ + struct timeout_data *data = user_data; + + close(data->fd); + data->fd = -1; + + if (data->destroy) + data->destroy(data->user_data); +} + +static void timeout_callback(int fd, uint32_t events, void *user_data) +{ + struct timeout_data *data = user_data; + uint64_t expired; + ssize_t result; + + if (events & (EPOLLERR | EPOLLHUP)) + return; + + result = read(data->fd, &expired, sizeof(expired)); + if (result != sizeof(expired)) + return; + + if (data->callback) + data->callback(data->fd, data->user_data); +} + +static inline int timeout_set(int fd, unsigned int msec) +{ + struct itimerspec itimer; + unsigned int sec = msec / 1000; + + memset(&itimer, 0, sizeof(itimer)); + itimer.it_interval.tv_sec = 0; + itimer.it_interval.tv_nsec = 0; + itimer.it_value.tv_sec = sec; + itimer.it_value.tv_nsec = (msec - (sec * 1000)) * 1000; + + return timerfd_settime(fd, 0, &itimer, NULL); +} + +int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback, + void *user_data, mainloop_destroy_func destroy) +{ + struct timeout_data *data; + + if (!callback) + return -EINVAL; + + data = malloc(sizeof(*data)); + if (!data) + return -ENOMEM; + + memset(data, 0, sizeof(*data)); + data->callback = callback; + data->destroy = destroy; + data->user_data = user_data; + + data->fd = timerfd_create(CLOCK_MONOTONIC, TFD_NONBLOCK | TFD_CLOEXEC); + if (data->fd < 0) { + free(data); + return -EIO; + } + + if (msec > 0) { + if (timeout_set(data->fd, msec) < 0) { + close(data->fd); + free(data); + return -EIO; + } + } + + if (mainloop_add_fd(data->fd, EPOLLIN | EPOLLONESHOT, + timeout_callback, data, timeout_destroy) < 0) { + close(data->fd); + free(data); + return -EIO; + } + + return data->fd; +} + +int mainloop_modify_timeout(int id, unsigned int msec) +{ + if (msec > 0) { + if (timeout_set(id, msec) < 0) + return -EIO; + } + + if (mainloop_modify_fd(id, EPOLLIN | EPOLLONESHOT) < 0) + return -EIO; + + return 0; +} + +int mainloop_remove_timeout(int id) +{ + return mainloop_remove_fd(id); +} + +int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback, + void *user_data, mainloop_destroy_func destroy) +{ + struct signal_data *data; + + if (!mask || !callback) + return -EINVAL; + + data = malloc(sizeof(*data)); + if (!data) + return -ENOMEM; + + memset(data, 0, sizeof(*data)); + data->callback = callback; + data->destroy = destroy; + data->user_data = user_data; + + data->fd = -1; + memcpy(&data->mask, mask, sizeof(sigset_t)); + + free(signal_data); + signal_data = data; + + return 0; +} diff --git a/bluez/mainloop.h b/bluez/mainloop.h new file mode 100644 index 0000000..bababcb --- /dev/null +++ b/bluez/mainloop.h @@ -0,0 +1,52 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2014 Intel Corporation + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * 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 (*mainloop_destroy_func) (void *user_data); + +typedef void (*mainloop_event_func) (int fd, uint32_t events, void *user_data); +typedef void (*mainloop_timeout_func) (int id, void *user_data); +typedef void (*mainloop_signal_func) (int signum, void *user_data); + +void mainloop_init(void); +void mainloop_quit(void); +void mainloop_exit_success(void); +void mainloop_exit_failure(void); +int mainloop_run(void); +void mainloop_finish(void); + +int mainloop_add_fd(int fd, uint32_t events, mainloop_event_func callback, + void *user_data, mainloop_destroy_func destroy); +int mainloop_modify_fd(int fd, uint32_t events); +int mainloop_remove_fd(int fd); + +int mainloop_add_timeout(unsigned int msec, mainloop_timeout_func callback, + void *user_data, mainloop_destroy_func destroy); +int mainloop_modify_timeout(int fd, unsigned int msec); +int mainloop_remove_timeout(int id); + +int mainloop_set_signal(sigset_t *mask, mainloop_signal_func callback, + void *user_data, mainloop_destroy_func destroy); diff --git a/bluez/queue.c b/bluez/queue.c new file mode 100644 index 0000000..f1795dc --- /dev/null +++ b/bluez/queue.c @@ -0,0 +1,417 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 "util.h" +#include "queue.h" + +struct queue { + int ref_count; + struct queue_entry *head; + struct queue_entry *tail; + unsigned int entries; +}; + +static struct queue *queue_ref(struct queue *queue) +{ + if (!queue) + return NULL; + + __sync_fetch_and_add(&queue->ref_count, 1); + + return queue; +} + +static void queue_unref(struct queue *queue) +{ + if (__sync_sub_and_fetch(&queue->ref_count, 1)) + return; + + free(queue); +} + +struct queue *queue_new(void) +{ + struct queue *queue; + + queue = new0(struct queue, 1); + if (!queue) + return NULL; + + queue->head = NULL; + queue->tail = NULL; + queue->entries = 0; + + return queue_ref(queue); +} + +void queue_destroy(struct queue *queue, queue_destroy_func_t destroy) +{ + if (!queue) + return; + + queue_remove_all(queue, NULL, NULL, destroy); + + queue_unref(queue); +} + +static struct queue_entry *queue_entry_ref(struct queue_entry *entry) +{ + if (!entry) + return NULL; + + __sync_fetch_and_add(&entry->ref_count, 1); + + return entry; +} + +static void queue_entry_unref(struct queue_entry *entry) +{ + if (__sync_sub_and_fetch(&entry->ref_count, 1)) + return; + + free(entry); +} + +static struct queue_entry *queue_entry_new(void *data) +{ + struct queue_entry *entry; + + entry = new0(struct queue_entry, 1); + if (!entry) + return NULL; + + entry->data = data; + + return queue_entry_ref(entry); +} + +bool queue_push_tail(struct queue *queue, void *data) +{ + struct queue_entry *entry; + + if (!queue) + return false; + + entry = queue_entry_new(data); + if (!entry) + return false; + + if (queue->tail) + queue->tail->next = entry; + + queue->tail = entry; + + if (!queue->head) + queue->head = entry; + + queue->entries++; + + return true; +} + +bool queue_push_head(struct queue *queue, void *data) +{ + struct queue_entry *entry; + + if (!queue) + return false; + + entry = queue_entry_new(data); + if (!entry) + return false; + + entry->next = queue->head; + + queue->head = entry; + + if (!queue->tail) + queue->tail = entry; + + queue->entries++; + + return true; +} + +bool queue_push_after(struct queue *queue, void *entry, void *data) +{ + struct queue_entry *qentry, *tmp, *new_entry; + + qentry = NULL; + + if (!queue) + return false; + + for (tmp = queue->head; tmp; tmp = tmp->next) { + if (tmp->data == entry) { + qentry = tmp; + break; + } + } + + if (!qentry) + return false; + + new_entry = queue_entry_new(data); + if (!new_entry) + return false; + + new_entry->next = qentry->next; + + if (!qentry->next) + queue->tail = new_entry; + + qentry->next = new_entry; + queue->entries++; + + return true; +} + +void *queue_pop_head(struct queue *queue) +{ + struct queue_entry *entry; + void *data; + + if (!queue || !queue->head) + return NULL; + + entry = queue->head; + + if (!queue->head->next) { + queue->head = NULL; + queue->tail = NULL; + } else + queue->head = queue->head->next; + + data = entry->data; + + queue_entry_unref(entry); + queue->entries--; + + return data; +} + +void *queue_peek_head(struct queue *queue) +{ + if (!queue || !queue->head) + return NULL; + + return queue->head->data; +} + +void *queue_peek_tail(struct queue *queue) +{ + if (!queue || !queue->tail) + return NULL; + + return queue->tail->data; +} + +void queue_foreach(struct queue *queue, queue_foreach_func_t function, + void *user_data) +{ + struct queue_entry *entry; + + if (!queue || !function) + return; + + entry = queue->head; + if (!entry) + return; + + queue_ref(queue); + while (entry && queue->head && queue->ref_count > 1) { + struct queue_entry *next; + + queue_entry_ref(entry); + + function(entry->data, user_data); + + next = entry->next; + + queue_entry_unref(entry); + + entry = next; + } + queue_unref(queue); +} + +static bool direct_match(const void *a, const void *b) +{ + return a == b; +} + +void *queue_find(struct queue *queue, queue_match_func_t function, + const void *match_data) +{ + struct queue_entry *entry; + + if (!queue) + return NULL; + + if (!function) + function = direct_match; + + for (entry = queue->head; entry; entry = entry->next) + if (function(entry->data, match_data)) + return entry->data; + + return NULL; +} + +bool queue_remove(struct queue *queue, void *data) +{ + struct queue_entry *entry, *prev; + + if (!queue) + return false; + + for (entry = queue->head, prev = NULL; entry; + prev = entry, entry = entry->next) { + if (entry->data != data) + continue; + + if (prev) + prev->next = entry->next; + else + queue->head = entry->next; + + if (!entry->next) + queue->tail = prev; + + queue_entry_unref(entry); + queue->entries--; + + return true; + } + + return false; +} + +void *queue_remove_if(struct queue *queue, queue_match_func_t function, + void *user_data) +{ + struct queue_entry *entry, *prev = NULL; + + if (!queue || !function) + return NULL; + + entry = queue->head; + + while (entry) { + if (function(entry->data, user_data)) { + void *data; + + if (prev) + prev->next = entry->next; + else + queue->head = entry->next; + + if (!entry->next) + queue->tail = prev; + + data = entry->data; + + queue_entry_unref(entry); + queue->entries--; + + return data; + } else { + prev = entry; + entry = entry->next; + } + } + + return NULL; +} + +unsigned int queue_remove_all(struct queue *queue, queue_match_func_t function, + void *user_data, queue_destroy_func_t destroy) +{ + struct queue_entry *entry; + unsigned int count = 0; + + if (!queue) + return 0; + + entry = queue->head; + + if (function) { + while (entry) { + void *data; + unsigned int entries = queue->entries; + + data = queue_remove_if(queue, function, user_data); + if (entries == queue->entries) + break; + + if (destroy) + destroy(data); + + count++; + } + } else { + queue->head = NULL; + queue->tail = NULL; + queue->entries = 0; + + while (entry) { + struct queue_entry *tmp = entry; + + entry = entry->next; + + if (destroy) + destroy(tmp->data); + + queue_entry_unref(tmp); + count++; + } + } + + return count; +} + +const struct queue_entry *queue_get_entries(struct queue *queue) +{ + if (!queue) + return NULL; + + return queue->head; +} + +unsigned int queue_length(struct queue *queue) +{ + if (!queue) + return 0; + + return queue->entries; +} + +bool queue_isempty(struct queue *queue) +{ + if (!queue) + return true; + + return queue->entries == 0; +} diff --git a/bluez/queue.h b/bluez/queue.h new file mode 100644 index 0000000..3bc8d2e --- /dev/null +++ b/bluez/queue.h @@ -0,0 +1,65 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 + +typedef void (*queue_destroy_func_t)(void *data); + +struct queue; + +struct queue_entry { + int ref_count; + void *data; + struct queue_entry *next; +}; + +struct queue *queue_new(void); +void queue_destroy(struct queue *queue, queue_destroy_func_t destroy); + +bool queue_push_tail(struct queue *queue, void *data); +bool queue_push_head(struct queue *queue, void *data); +bool queue_push_after(struct queue *queue, void *entry, void *data); +void *queue_pop_head(struct queue *queue); +void *queue_peek_head(struct queue *queue); +void *queue_peek_tail(struct queue *queue); + +typedef void (*queue_foreach_func_t)(void *data, void *user_data); + +void queue_foreach(struct queue *queue, queue_foreach_func_t function, + void *user_data); + +typedef bool (*queue_match_func_t)(const void *data, const void *match_data); + +void *queue_find(struct queue *queue, queue_match_func_t function, + const void *match_data); + +bool queue_remove(struct queue *queue, void *data); +void *queue_remove_if(struct queue *queue, queue_match_func_t function, + void *user_data); +unsigned int queue_remove_all(struct queue *queue, queue_match_func_t function, + void *user_data, queue_destroy_func_t destroy); + +const struct queue_entry *queue_get_entries(struct queue *queue); + +unsigned int queue_length(struct queue *queue); +bool queue_isempty(struct queue *queue); diff --git a/bluez/timeout-mainloop.c b/bluez/timeout-mainloop.c new file mode 100644 index 0000000..f099312 --- /dev/null +++ b/bluez/timeout-mainloop.c @@ -0,0 +1,85 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 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. + * + */ + +#include + +#include "mainloop.h" +#include "util.h" +#include "timeout.h" + +struct timeout_data { + int id; + timeout_func_t func; + timeout_destroy_func_t destroy; + unsigned int timeout; + void *user_data; +}; + +static void timeout_callback(int id, void *user_data) +{ + struct timeout_data *data = user_data; + + if (data->func(data->user_data) && + !mainloop_modify_timeout(data->id, data->timeout)) + return; + + mainloop_remove_timeout(data->id); +} + +static void timeout_destroy(void *user_data) +{ + struct timeout_data *data = user_data; + + if (data->destroy) + data->destroy(data->user_data); + + free(data); +} + +unsigned int timeout_add(unsigned int timeout, timeout_func_t func, + void *user_data, timeout_destroy_func_t destroy) +{ + struct timeout_data *data; + + data = new0(struct timeout_data, 1); + if (!data) + return 0; + + data->func = func; + data->user_data = user_data; + data->timeout = timeout; + data->destroy = destroy; + + data->id = mainloop_add_timeout(timeout, timeout_callback, data, + timeout_destroy); + if (data->id < 0) { + free(data); + return 0; + } + + return (unsigned int) data->id; +} + +void timeout_remove(unsigned int id) +{ + if (!id) + return; + + mainloop_remove_timeout((int) id); +} diff --git a/bluez/timeout.h b/bluez/timeout.h new file mode 100644 index 0000000..4930ce1 --- /dev/null +++ b/bluez/timeout.h @@ -0,0 +1,27 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2014 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. + * + */ + +#include + +typedef bool (*timeout_func_t)(void *user_data); +typedef void (*timeout_destroy_func_t)(void *user_data); + +unsigned int timeout_add(unsigned int timeout, timeout_func_t func, + void *user_data, timeout_destroy_func_t destroy); +void timeout_remove(unsigned int id); diff --git a/bluez/util.c b/bluez/util.c new file mode 100644 index 0000000..d7faea7 --- /dev/null +++ b/bluez/util.c @@ -0,0 +1,135 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" + +void util_debug(util_debug_func_t function, void *user_data, + const char *format, ...) +{ + char str[78]; + va_list ap; + + if (!function || !format) + return; + + va_start(ap, format); + vsnprintf(str, sizeof(str), format, ap); + va_end(ap); + + function(str, user_data); +} + +void util_hexdump(const char dir, const unsigned char *buf, size_t len, + util_debug_func_t function, void *user_data) +{ + static const char hexdigits[] = "0123456789abcdef"; + char str[68]; + size_t i; + + if (!function || !len) + return; + + str[0] = dir; + + for (i = 0; i < len; i++) { + str[((i % 16) * 3) + 1] = ' '; + str[((i % 16) * 3) + 2] = hexdigits[buf[i] >> 4]; + str[((i % 16) * 3) + 3] = hexdigits[buf[i] & 0xf]; + str[(i % 16) + 51] = isprint(buf[i]) ? buf[i] : '.'; + + if ((i + 1) % 16 == 0) { + str[49] = ' '; + str[50] = ' '; + str[67] = '\0'; + function(str, user_data); + str[0] = ' '; + } + } + + if (i % 16 > 0) { + size_t j; + for (j = (i % 16); j < 16; j++) { + str[(j * 3) + 1] = ' '; + str[(j * 3) + 2] = ' '; + str[(j * 3) + 3] = ' '; + str[j + 51] = ' '; + } + str[49] = ' '; + str[50] = ' '; + str[67] = '\0'; + function(str, user_data); + } +} + +/* Helper for getting the dirent type in case readdir returns DT_UNKNOWN */ +unsigned char util_get_dt(const char *parent, const char *name) +{ + char filename[PATH_MAX]; + struct stat st; + + snprintf(filename, sizeof(filename), "%s/%s", parent, name); + if (lstat(filename, &st) == 0 && S_ISDIR(st.st_mode)) + return DT_DIR; + + return DT_UNKNOWN; +} + +/* Helpers for bitfield operations */ + +/* Find unique id in range from 1 to max but no bigger then + * sizeof(int) * 8. ffs() is used since it is POSIX standard + */ +uint8_t util_get_uid(unsigned int *bitmap, uint8_t max) +{ + uint8_t id; + + id = ffs(~*bitmap); + + if (!id || id > max) + return 0; + + *bitmap |= 1 << (id - 1); + + return id; +} + +/* Clear id bit in bitmap */ +void util_clear_uid(unsigned int *bitmap, uint8_t id) +{ + if (!id) + return; + + *bitmap &= ~(1 << (id - 1)); +} diff --git a/bluez/util.h b/bluez/util.h new file mode 100644 index 0000000..30b7d92 --- /dev/null +++ b/bluez/util.h @@ -0,0 +1,157 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012-2014 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 +#include +#include + +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define le16_to_cpu(val) (val) +#define le32_to_cpu(val) (val) +#define le64_to_cpu(val) (val) +#define cpu_to_le16(val) (val) +#define cpu_to_le32(val) (val) +#define cpu_to_le64(val) (val) +#define be16_to_cpu(val) bswap_16(val) +#define be32_to_cpu(val) bswap_32(val) +#define be64_to_cpu(val) bswap_64(val) +#define cpu_to_be16(val) bswap_16(val) +#define cpu_to_be32(val) bswap_32(val) +#define cpu_to_be64(val) bswap_64(val) +#elif __BYTE_ORDER == __BIG_ENDIAN +#define le16_to_cpu(val) bswap_16(val) +#define le32_to_cpu(val) bswap_32(val) +#define le64_to_cpu(val) bswap_64(val) +#define cpu_to_le16(val) bswap_16(val) +#define cpu_to_le32(val) bswap_32(val) +#define cpu_to_le64(val) bswap_64(val) +#define be16_to_cpu(val) (val) +#define be32_to_cpu(val) (val) +#define be64_to_cpu(val) (val) +#define cpu_to_be16(val) (val) +#define cpu_to_be32(val) (val) +#define cpu_to_be64(val) (val) +#else +#error "Unknown byte order" +#endif + +#define get_unaligned(ptr) \ +__extension__ ({ \ + struct __attribute__((packed)) { \ + __typeof__(*(ptr)) __v; \ + } *__p = (__typeof__(__p)) (ptr); \ + __p->__v; \ +}) + +#define put_unaligned(val, ptr) \ +do { \ + struct __attribute__((packed)) { \ + __typeof__(*(ptr)) __v; \ + } *__p = (__typeof__(__p)) (ptr); \ + __p->__v = (val); \ +} while (0) + +#define PTR_TO_UINT(p) ((unsigned int) ((uintptr_t) (p))) +#define UINT_TO_PTR(u) ((void *) ((uintptr_t) (u))) + +#define PTR_TO_INT(p) ((int) ((intptr_t) (p))) +#define INT_TO_PTR(u) ((void *) ((intptr_t) (u))) + +#define new0(t, n) ((t*) calloc((n), sizeof(t))) +#define newa(t, n) ((t*) alloca(sizeof(t)*(n))) +#define malloc0(n) (calloc((n), 1)) + +typedef void (*util_debug_func_t)(const char *str, void *user_data); + +void util_debug(util_debug_func_t function, void *user_data, + const char *format, ...) + __attribute__((format(printf, 3, 4))); + +void util_hexdump(const char dir, const unsigned char *buf, size_t len, + util_debug_func_t function, void *user_data); + +unsigned char util_get_dt(const char *parent, const char *name); + +uint8_t util_get_uid(unsigned int *bitmap, uint8_t max); +void util_clear_uid(unsigned int *bitmap, uint8_t id); + +static inline uint16_t get_le16(const void *ptr) +{ + return le16_to_cpu(get_unaligned((const uint16_t *) ptr)); +} + +static inline uint16_t get_be16(const void *ptr) +{ + return be16_to_cpu(get_unaligned((const uint16_t *) ptr)); +} + +static inline uint32_t get_le32(const void *ptr) +{ + return le32_to_cpu(get_unaligned((const uint32_t *) ptr)); +} + +static inline uint32_t get_be32(const void *ptr) +{ + return be32_to_cpu(get_unaligned((const uint32_t *) ptr)); +} + +static inline uint64_t get_le64(const void *ptr) +{ + return le64_to_cpu(get_unaligned((const uint64_t *) ptr)); +} + +static inline uint64_t get_be64(const void *ptr) +{ + return be64_to_cpu(get_unaligned((const uint64_t *) ptr)); +} + +static inline void put_le16(uint16_t val, void *dst) +{ + put_unaligned(cpu_to_le16(val), (uint16_t *) dst); +} + +static inline void put_be16(uint16_t val, const void *ptr) +{ + put_unaligned(cpu_to_be16(val), (uint16_t *) ptr); +} + +static inline void put_le32(uint32_t val, void *dst) +{ + put_unaligned(cpu_to_le32(val), (uint32_t *) dst); +} + +static inline void put_be32(uint32_t val, void *dst) +{ + put_unaligned(cpu_to_be32(val), (uint32_t *) dst); +} + +static inline void put_le64(uint64_t val, void *dst) +{ + put_unaligned(cpu_to_le64(val), (uint64_t *) dst); +} + +static inline void put_be64(uint64_t val, void *dst) +{ + put_unaligned(cpu_to_be64(val), (uint64_t *) dst); +} diff --git a/bluez/uuid.c b/bluez/uuid.c new file mode 100644 index 0000000..6a10fbb --- /dev/null +++ b/bluez/uuid.c @@ -0,0 +1,311 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011 Nokia Corporation + * Copyright (C) 2011 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 + * + */ + +#include + +#include +#include +#include + +#include "uuid.h" + +static uint128_t bluetooth_base_uuid = { + .data = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB } +}; + +#define BASE_UUID16_OFFSET 2 +#define BASE_UUID32_OFFSET 0 + +static void bt_uuid16_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst) +{ + uint16_t be16; + + dst->value.u128 = bluetooth_base_uuid; + dst->type = BT_UUID128; + + /* + * No matter the system: 128-bit UUIDs should be stored + * as big-endian. 16-bit UUIDs are stored on host order. + */ + + be16 = htons(src->value.u16); + memcpy(&dst->value.u128.data[BASE_UUID16_OFFSET], &be16, sizeof(be16)); +} + +static void bt_uuid32_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst) +{ + uint32_t be32; + + dst->value.u128 = bluetooth_base_uuid; + dst->type = BT_UUID128; + + /* + * No matter the system: 128-bit UUIDs should be stored + * as big-endian. 32-bit UUIDs are stored on host order. + */ + + be32 = htonl(src->value.u32); + memcpy(&dst->value.u128.data[BASE_UUID32_OFFSET], &be32, sizeof(be32)); +} + +void bt_uuid_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst) +{ + switch (src->type) { + case BT_UUID128: + *dst = *src; + break; + case BT_UUID32: + bt_uuid32_to_uuid128(src, dst); + break; + case BT_UUID16: + bt_uuid16_to_uuid128(src, dst); + break; + case BT_UUID_UNSPEC: + default: + break; + } +} + +static int bt_uuid128_cmp(const bt_uuid_t *u1, const bt_uuid_t *u2) +{ + return memcmp(&u1->value.u128, &u2->value.u128, sizeof(uint128_t)); +} + +int bt_uuid16_create(bt_uuid_t *btuuid, uint16_t value) +{ + memset(btuuid, 0, sizeof(bt_uuid_t)); + btuuid->type = BT_UUID16; + btuuid->value.u16 = value; + + return 0; +} + +int bt_uuid32_create(bt_uuid_t *btuuid, uint32_t value) +{ + memset(btuuid, 0, sizeof(bt_uuid_t)); + btuuid->type = BT_UUID32; + btuuid->value.u32 = value; + + return 0; +} + +int bt_uuid128_create(bt_uuid_t *btuuid, uint128_t value) +{ + memset(btuuid, 0, sizeof(bt_uuid_t)); + btuuid->type = BT_UUID128; + btuuid->value.u128 = value; + + return 0; +} + +int bt_uuid_cmp(const bt_uuid_t *uuid1, const bt_uuid_t *uuid2) +{ + bt_uuid_t u1, u2; + + bt_uuid_to_uuid128(uuid1, &u1); + bt_uuid_to_uuid128(uuid2, &u2); + + return bt_uuid128_cmp(&u1, &u2); +} + +/* + * convert the UUID to string, copying a maximum of n characters. + */ +int bt_uuid_to_string(const bt_uuid_t *uuid, char *str, size_t n) +{ + if (!uuid) { + snprintf(str, n, "NULL"); + return -EINVAL; + } + + switch (uuid->type) { + case BT_UUID16: + snprintf(str, n, "%.4x", uuid->value.u16); + break; + case BT_UUID32: + snprintf(str, n, "%.8x", uuid->value.u32); + break; + case BT_UUID128: { + unsigned int data0; + unsigned short data1; + unsigned short data2; + unsigned short data3; + unsigned int data4; + unsigned short data5; + + const uint8_t *data = (uint8_t *) &uuid->value.u128; + + memcpy(&data0, &data[0], 4); + memcpy(&data1, &data[4], 2); + memcpy(&data2, &data[6], 2); + memcpy(&data3, &data[8], 2); + memcpy(&data4, &data[10], 4); + memcpy(&data5, &data[14], 2); + + snprintf(str, n, "%.8x-%.4x-%.4x-%.4x-%.8x%.4x", + ntohl(data0), ntohs(data1), + ntohs(data2), ntohs(data3), + ntohl(data4), ntohs(data5)); + } + break; + case BT_UUID_UNSPEC: + default: + snprintf(str, n, "Type of UUID (%x) unknown.", uuid->type); + return -EINVAL; /* Enum type of UUID not set */ + } + + return 0; +} + +static inline int is_uuid128(const char *string) +{ + return (strlen(string) == 36 && + string[8] == '-' && + string[13] == '-' && + string[18] == '-' && + string[23] == '-'); +} + +static inline int is_base_uuid128(const char *string) +{ + uint16_t uuid; + char dummy; + + if (!is_uuid128(string)) + return 0; + + return sscanf(string, + "0000%04hx-0000-1000-8000-00805%1[fF]9%1[bB]34%1[fF]%1[bB]", + &uuid, &dummy, &dummy, &dummy, &dummy) == 5; +} + +static inline int is_uuid32(const char *string) +{ + return (strlen(string) == 8 || strlen(string) == 10); +} + +static inline int is_uuid16(const char *string) +{ + return (strlen(string) == 4 || strlen(string) == 6); +} + +static int bt_string_to_uuid16(bt_uuid_t *uuid, const char *string) +{ + uint16_t u16; + char *endptr = NULL; + + u16 = strtol(string, &endptr, 16); + if (endptr && (*endptr == '\0' || *endptr == '-')) { + bt_uuid16_create(uuid, u16); + return 0; + } + + return -EINVAL; +} + +static int bt_string_to_uuid32(bt_uuid_t *uuid, const char *string) +{ + uint32_t u32; + char *endptr = NULL; + + u32 = strtol(string, &endptr, 16); + if (endptr && *endptr == '\0') { + bt_uuid32_create(uuid, u32); + return 0; + } + + return -EINVAL; +} + +static int bt_string_to_uuid128(bt_uuid_t *uuid, const char *string) +{ + uint32_t data0, data4; + uint16_t data1, data2, data3, data5; + uint128_t u128; + uint8_t *val = (uint8_t *) &u128; + + if (sscanf(string, "%08x-%04hx-%04hx-%04hx-%08x%04hx", + &data0, &data1, &data2, + &data3, &data4, &data5) != 6) + return -EINVAL; + + data0 = htonl(data0); + data1 = htons(data1); + data2 = htons(data2); + data3 = htons(data3); + data4 = htonl(data4); + data5 = htons(data5); + + memcpy(&val[0], &data0, 4); + memcpy(&val[4], &data1, 2); + memcpy(&val[6], &data2, 2); + memcpy(&val[8], &data3, 2); + memcpy(&val[10], &data4, 4); + memcpy(&val[14], &data5, 2); + + bt_uuid128_create(uuid, u128); + + return 0; +} + +int bt_string_to_uuid(bt_uuid_t *uuid, const char *string) +{ + if (is_base_uuid128(string)) + return bt_string_to_uuid16(uuid, string + 4); + else if (is_uuid128(string)) + return bt_string_to_uuid128(uuid, string); + else if (is_uuid32(string)) + return bt_string_to_uuid32(uuid, string); + else if (is_uuid16(string)) + return bt_string_to_uuid16(uuid, string); + + return -EINVAL; +} + +int bt_uuid_strcmp(const void *a, const void *b) +{ + return strcasecmp(a, b); +} + +int bt_uuid_to_le(const bt_uuid_t *src, void *dst) +{ + bt_uuid_t uuid; + + switch (src->type) { + case BT_UUID16: + bt_put_le16(src->value.u16, dst); + return 0; + case BT_UUID32: + bt_uuid_to_uuid128(src, &uuid); + /* Fallthrough */ + case BT_UUID128: + /* Convert from 128-bit BE to LE */ + bswap_128(&src->value.u128, dst); + return 0; + case BT_UUID_UNSPEC: + default: + return -EINVAL; + } +} diff --git a/bluez/uuid.h b/bluez/uuid.h new file mode 100644 index 0000000..2dcfe9e --- /dev/null +++ b/bluez/uuid.h @@ -0,0 +1,181 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011 Nokia Corporation + * Copyright (C) 2011 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 + * + */ + +#ifndef __BLUETOOTH_UUID_H +#define __BLUETOOTH_UUID_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define GENERIC_AUDIO_UUID "00001203-0000-1000-8000-00805f9b34fb" + +#define HSP_HS_UUID "00001108-0000-1000-8000-00805f9b34fb" +#define HSP_AG_UUID "00001112-0000-1000-8000-00805f9b34fb" + +#define HFP_HS_UUID "0000111e-0000-1000-8000-00805f9b34fb" +#define HFP_AG_UUID "0000111f-0000-1000-8000-00805f9b34fb" + +#define ADVANCED_AUDIO_UUID "0000110d-0000-1000-8000-00805f9b34fb" + +#define A2DP_SOURCE_UUID "0000110a-0000-1000-8000-00805f9b34fb" +#define A2DP_SINK_UUID "0000110b-0000-1000-8000-00805f9b34fb" + +#define AVRCP_REMOTE_UUID "0000110e-0000-1000-8000-00805f9b34fb" +#define AVRCP_TARGET_UUID "0000110c-0000-1000-8000-00805f9b34fb" + +#define PANU_UUID "00001115-0000-1000-8000-00805f9b34fb" +#define NAP_UUID "00001116-0000-1000-8000-00805f9b34fb" +#define GN_UUID "00001117-0000-1000-8000-00805f9b34fb" +#define BNEP_SVC_UUID "0000000f-0000-1000-8000-00805f9b34fb" + +#define PNPID_UUID "00002a50-0000-1000-8000-00805f9b34fb" +#define DEVICE_INFORMATION_UUID "0000180a-0000-1000-8000-00805f9b34fb" + +#define GATT_UUID "00001801-0000-1000-8000-00805f9b34fb" +#define IMMEDIATE_ALERT_UUID "00001802-0000-1000-8000-00805f9b34fb" +#define LINK_LOSS_UUID "00001803-0000-1000-8000-00805f9b34fb" +#define TX_POWER_UUID "00001804-0000-1000-8000-00805f9b34fb" +#define BATTERY_UUID "0000180f-0000-1000-8000-00805f9b34fb" +#define SCAN_PARAMETERS_UUID "00001813-0000-1000-8000-00805f9b34fb" + +#define SAP_UUID "0000112D-0000-1000-8000-00805f9b34fb" + +#define HEART_RATE_UUID "0000180d-0000-1000-8000-00805f9b34fb" +#define HEART_RATE_MEASUREMENT_UUID "00002a37-0000-1000-8000-00805f9b34fb" +#define BODY_SENSOR_LOCATION_UUID "00002a38-0000-1000-8000-00805f9b34fb" +#define HEART_RATE_CONTROL_POINT_UUID "00002a39-0000-1000-8000-00805f9b34fb" + +#define HEALTH_THERMOMETER_UUID "00001809-0000-1000-8000-00805f9b34fb" +#define TEMPERATURE_MEASUREMENT_UUID "00002a1c-0000-1000-8000-00805f9b34fb" +#define TEMPERATURE_TYPE_UUID "00002a1d-0000-1000-8000-00805f9b34fb" +#define INTERMEDIATE_TEMPERATURE_UUID "00002a1e-0000-1000-8000-00805f9b34fb" +#define MEASUREMENT_INTERVAL_UUID "00002a21-0000-1000-8000-00805f9b34fb" + +#define CYCLING_SC_UUID "00001816-0000-1000-8000-00805f9b34fb" +#define CSC_MEASUREMENT_UUID "00002a5b-0000-1000-8000-00805f9b34fb" +#define CSC_FEATURE_UUID "00002a5c-0000-1000-8000-00805f9b34fb" +#define SENSOR_LOCATION_UUID "00002a5d-0000-1000-8000-00805f9b34fb" +#define SC_CONTROL_POINT_UUID "00002a55-0000-1000-8000-00805f9b34fb" + +#define RFCOMM_UUID_STR "00000003-0000-1000-8000-00805f9b34fb" + +#define HDP_UUID "00001400-0000-1000-8000-00805f9b34fb" +#define HDP_SOURCE_UUID "00001401-0000-1000-8000-00805f9b34fb" +#define HDP_SINK_UUID "00001402-0000-1000-8000-00805f9b34fb" + +#define HID_UUID "00001124-0000-1000-8000-00805f9b34fb" + +#define DUN_GW_UUID "00001103-0000-1000-8000-00805f9b34fb" + +#define GAP_UUID "00001800-0000-1000-8000-00805f9b34fb" +#define PNP_UUID "00001200-0000-1000-8000-00805f9b34fb" + +#define SPP_UUID "00001101-0000-1000-8000-00805f9b34fb" + +#define OBEX_SYNC_UUID "00001104-0000-1000-8000-00805f9b34fb" +#define OBEX_OPP_UUID "00001105-0000-1000-8000-00805f9b34fb" +#define OBEX_FTP_UUID "00001106-0000-1000-8000-00805f9b34fb" +#define OBEX_PCE_UUID "0000112e-0000-1000-8000-00805f9b34fb" +#define OBEX_PSE_UUID "0000112f-0000-1000-8000-00805f9b34fb" +#define OBEX_PBAP_UUID "00001130-0000-1000-8000-00805f9b34fb" +#define OBEX_MAS_UUID "00001132-0000-1000-8000-00805f9b34fb" +#define OBEX_MNS_UUID "00001133-0000-1000-8000-00805f9b34fb" +#define OBEX_MAP_UUID "00001134-0000-1000-8000-00805f9b34fb" + +/* GATT UUIDs section */ +#define GATT_PRIM_SVC_UUID 0x2800 +#define GATT_SND_SVC_UUID 0x2801 +#define GATT_INCLUDE_UUID 0x2802 +#define GATT_CHARAC_UUID 0x2803 + +/* GATT Characteristic Types */ +#define GATT_CHARAC_DEVICE_NAME 0x2A00 +#define GATT_CHARAC_APPEARANCE 0x2A01 +#define GATT_CHARAC_PERIPHERAL_PRIV_FLAG 0x2A02 +#define GATT_CHARAC_RECONNECTION_ADDRESS 0x2A03 +#define GATT_CHARAC_PERIPHERAL_PREF_CONN 0x2A04 +#define GATT_CHARAC_SERVICE_CHANGED 0x2A05 +#define GATT_CHARAC_SYSTEM_ID 0x2A23 +#define GATT_CHARAC_MODEL_NUMBER_STRING 0x2A24 +#define GATT_CHARAC_SERIAL_NUMBER_STRING 0x2A25 +#define GATT_CHARAC_FIRMWARE_REVISION_STRING 0x2A26 +#define GATT_CHARAC_HARDWARE_REVISION_STRING 0x2A27 +#define GATT_CHARAC_SOFTWARE_REVISION_STRING 0x2A28 +#define GATT_CHARAC_MANUFACTURER_NAME_STRING 0x2A29 +#define GATT_CHARAC_PNP_ID 0x2A50 + +/* GATT Characteristic Descriptors */ +#define GATT_CHARAC_EXT_PROPER_UUID 0x2900 +#define GATT_CHARAC_USER_DESC_UUID 0x2901 +#define GATT_CLIENT_CHARAC_CFG_UUID 0x2902 +#define GATT_SERVER_CHARAC_CFG_UUID 0x2903 +#define GATT_CHARAC_FMT_UUID 0x2904 +#define GATT_CHARAC_AGREG_FMT_UUID 0x2905 +#define GATT_CHARAC_VALID_RANGE_UUID 0x2906 +#define GATT_EXTERNAL_REPORT_REFERENCE 0x2907 +#define GATT_REPORT_REFERENCE 0x2908 + +typedef struct { + enum { + BT_UUID_UNSPEC = 0, + BT_UUID16 = 16, + BT_UUID32 = 32, + BT_UUID128 = 128, + } type; + union { + uint16_t u16; + uint32_t u32; + uint128_t u128; + } value; +} bt_uuid_t; + +int bt_uuid_strcmp(const void *a, const void *b); + +int bt_uuid16_create(bt_uuid_t *btuuid, uint16_t value); +int bt_uuid32_create(bt_uuid_t *btuuid, uint32_t value); +int bt_uuid128_create(bt_uuid_t *btuuid, uint128_t value); + +int bt_uuid_cmp(const bt_uuid_t *uuid1, const bt_uuid_t *uuid2); +void bt_uuid_to_uuid128(const bt_uuid_t *src, bt_uuid_t *dst); + +#define MAX_LEN_UUID_STR 37 + +int bt_uuid_to_string(const bt_uuid_t *uuid, char *str, size_t n); +int bt_string_to_uuid(bt_uuid_t *uuid, const char *string); + +int bt_uuid_to_le(const bt_uuid_t *uuid, void *dst); + +static inline int bt_uuid_len(const bt_uuid_t *uuid) +{ + return uuid->type / 8; +} + +#ifdef __cplusplus +} +#endif + +#endif /* __BLUETOOTH_UUID_H */ diff --git a/dfu.c b/dfu.c index d59acac..be1f952 100644 --- a/dfu.c +++ b/dfu.c @@ -4,10 +4,36 @@ void dfu (const char *bdaddr, const char *type, const char *version, uint8_t * dat, size_t dat_sz, uint8_t * bin, size_t bin_sz) { + BLE *b; + uint8_t buf[32]; - bt_thing (bdaddr); + ble_init(); + + + do { + + b=ble_open (bdaddr); + + if (!b) + break; + + if (ble_register_notify(b)) break; + + ble_send_cp( + + ble_close (b); + return; + + +} while (0); + + + + ble_close (b); + exit(EXIT_FAILURE); + } diff --git a/dfu.h b/dfu.h index 54a6ac2..f85bc62 100644 --- a/dfu.h +++ b/dfu.h @@ -8,26 +8,32 @@ typedef enum { - BLE_DFU_RESP_VAL_SUCCESS = 1, /**< Success.*/ - BLE_DFU_RESP_VAL_INVALID_STATE, /**< Invalid state.*/ - BLE_DFU_RESP_VAL_NOT_SUPPORTED, /**< Operation not supported.*/ - BLE_DFU_RESP_VAL_DATA_SIZE, /**< Data size exceeds limit.*/ - BLE_DFU_RESP_VAL_CRC_ERROR, /**< CRC Error.*/ - BLE_DFU_RESP_VAL_OPER_FAILED /**< Operation failed.*/ + BLE_DFU_RESP_VAL_SUCCESS = 1, /**< Success.*/ + BLE_DFU_RESP_VAL_INVALID_STATE, /**< Invalid state.*/ + BLE_DFU_RESP_VAL_NOT_SUPPORTED, /**< Operation not supported.*/ + BLE_DFU_RESP_VAL_DATA_SIZE, /**< Data size exceeds limit.*/ + BLE_DFU_RESP_VAL_CRC_ERROR, /**< CRC Error.*/ + BLE_DFU_RESP_VAL_OPER_FAILED /**< Operation failed.*/ } ble_dfu_resp_val_t; enum { - OP_CODE_START_DFU = 1, /**< Value of the Op code field for 'Start DFU' command.*/ - OP_CODE_RECEIVE_INIT = 2, /**< Value of the Op code field for 'Initialize DFU parameters' command.*/ - OP_CODE_RECEIVE_FW = 3, /**< Value of the Op code field for 'Receive firmware image' command.*/ - OP_CODE_VALIDATE = 4, /**< Value of the Op code field for 'Validate firmware' command.*/ - OP_CODE_ACTIVATE_N_RESET = 5, /**< Value of the Op code field for 'Activate & Reset' command.*/ - OP_CODE_SYS_RESET = 6, /**< Value of the Op code field for 'Reset System' command.*/ - OP_CODE_IMAGE_SIZE_REQ = 7, /**< Value of the Op code field for 'Report received image size' command.*/ - OP_CODE_PKT_RCPT_NOTIF_REQ = 8, /**< Value of the Op code field for 'Request packet receipt notification.*/ - OP_CODE_RESPONSE = 16, /**< Value of the Op code field for 'Response.*/ - OP_CODE_PKT_RCPT_NOTIF = 17 /**< Value of the Op code field for 'Packets Receipt Notification'.*/ + OP_CODE_START_DFU = 1, /**< Value of the Op code field for 'Start DFU' command.*/ + OP_CODE_RECEIVE_INIT = 2, /**< Value of the Op code field for 'Initialize DFU parameters' command.*/ + OP_CODE_RECEIVE_FW = 3, /**< Value of the Op code field for 'Receive firmware image' command.*/ + OP_CODE_VALIDATE = 4, /**< Value of the Op code field for 'Validate firmware' command.*/ + OP_CODE_ACTIVATE_N_RESET = 5, /**< Value of the Op code field for 'Activate & Reset' command.*/ + OP_CODE_SYS_RESET = 6, /**< Value of the Op code field for 'Reset System' command.*/ + OP_CODE_IMAGE_SIZE_REQ = 7, /**< Value of the Op code field for 'Report received image size' command.*/ + OP_CODE_PKT_RCPT_NOTIF_REQ = 8, /**< Value of the Op code field for 'Request packet receipt notification.*/ + OP_CODE_RESPONSE = 16, /**< Value of the Op code field for 'Response.*/ + OP_CODE_PKT_RCPT_NOTIF = 17 /**< Value of the Op code field for 'Packets Receipt Notification'.*/ }; +enum { + DFU_MODE_SOFTDEVICE = 1, + DFU_MODE_BOOTLOADER = 2, + DFU_MODE_SD_BL = 3, + DFU_MODE_APPLICATION = 4 +}; diff --git a/manifest.c b/manifest.c index 795e833..dbda6ef 100644 --- a/manifest.c +++ b/manifest.c @@ -25,7 +25,6 @@ struct manifest * parse_manifest (const char *str) { json_object *json, *manifest; - enum json_type type; enum json_tokener_error jerr = json_tokener_success; struct manifest *m; diff --git a/manifest.h b/manifest.h index 5588535..89bdeb0 100644 --- a/manifest.h +++ b/manifest.h @@ -1,7 +1,7 @@ -struct manifest { - const char *type; - const char *dat_file; - const char *bin_file; - const char *dfu_version; +struct manifest +{ + const char *type; + const char *dat_file; + const char *bin_file; + const char *dfu_version; }; - diff --git a/nrfdfu.c b/nrfdfu.c index 96f99d9..ab58464 100644 --- a/nrfdfu.c +++ b/nrfdfu.c @@ -65,10 +65,11 @@ main (int argc, char *argv[]) bin_size = read_file_from_zip (zip, m->bin_file, &bin); - printf ("%d bytes init_data, %d bytes firmware\n", dat_size, bin_size); + printf ("%u bytes init_data, %u bytes firmware\n",(unsigned) dat_size, (unsigned) bin_size); dfu (bdaddr, m->type, m->dfu_version, dat, dat_size, bin, bin_size); +return EXIT_SUCCESS; } diff --git a/project.h b/project.h index 4d8853e..be211ee 100644 --- a/project.h +++ b/project.h @@ -16,10 +16,12 @@ //#include +#include "bluez/uuid.h" +#include "bluez/att.h" +#include "bluez/queue.h" +#include "bluez/gatt-db.h" + #include "manifest.h" #include "ble.h" #include "prototypes.h" - - - diff --git a/prototypes.h b/prototypes.h index 015fb69..ab6773c 100644 --- a/prototypes.h +++ b/prototypes.h @@ -8,7 +8,10 @@ extern void fatal_zip(struct zip *zip); extern struct zip *open_zip(const char *fn); extern size_t read_file_from_zip(struct zip *zip, const char *fn, void *_buf); /* ble.c */ -extern int bt_thing(const char *bdaddr); +extern void ble_close(BLE *ble); +extern void ble_init(void); +extern BLE *ble_open(const char *bdaddr); +extern int ble_register_notify(BLE *ble); /* manifest.c */ extern json_object *_json_object_object_get(json_object *obj, const char *name); extern struct manifest *parse_manifest(const char *str); diff --git a/util.c b/util.c index 554b4f9..9be342b 100644 --- a/util.c +++ b/util.c @@ -7,6 +7,7 @@ xmalloc (size_t s) char *ret = malloc (s); if (!ret) abort (); + return ret; } void * @@ -15,4 +16,5 @@ xrealloc (void *p, size_t s) char *ret = realloc (p, s); if (!ret) abort (); + return ret; } -- cgit v1.2.3