From c75ea8ea48ff5404a10c63c9cfed835ac281ab53 Mon Sep 17 00:00:00 2001 From: majonezz Date: Fri, 20 Oct 2023 21:31:26 +0200 Subject: [PATCH] Initial entry --- README.md | 58 +- att-types.h | 147 ++ att.c | 1532 ++++++++++++++++++ att.h | 93 ++ bluetooth.c | 1456 ++++++++++++++++++ bluetooth.h | 403 +++++ client.c | 249 +++ client.h | 54 + config.h | 133 ++ crypto.c | 701 +++++++++ crypto.h | 61 + design_notes.h | 112 ++ gatt-client.c | 3069 ++++++++++++++++++++++++++++++++++++ gatt-client.h | 135 ++ gatt-db.c | 1765 +++++++++++++++++++++ gatt-db.h | 236 +++ gatt-helpers.c | 1580 +++++++++++++++++++ gatt-helpers.h | 116 ++ hci.c | 4012 ++++++++++++++++++++++++++++++++++++++++++++++++ hci.h | 2449 +++++++++++++++++++++++++++++ hci_lib.h | 242 +++ io-mainloop.c | 423 +++++ io.h | 47 + json.c | 887 +++++++++++ json.h | 94 ++ l2cap.h | 279 ++++ main.c | 936 +++++++++++ mainloop.c | 481 ++++++ mainloop.h | 51 + makefile | 21 + mqtt.c | 441 ++++++ mqtt.h | 55 + queue.c | 575 +++++++ queue.h | 65 + timeout-glib.c | 86 ++ timeout.h | 27 + util.c | 178 +++ util.h | 158 ++ uuid.c | 324 ++++ uuid.h | 181 +++ 40 files changed, 23910 insertions(+), 2 deletions(-) create mode 100644 att-types.h create mode 100644 att.c create mode 100644 att.h create mode 100644 bluetooth.c create mode 100644 bluetooth.h create mode 100644 client.c create mode 100644 client.h create mode 100644 config.h create mode 100644 crypto.c create mode 100644 crypto.h create mode 100644 design_notes.h create mode 100644 gatt-client.c create mode 100644 gatt-client.h create mode 100644 gatt-db.c create mode 100644 gatt-db.h create mode 100644 gatt-helpers.c create mode 100644 gatt-helpers.h create mode 100644 hci.c create mode 100644 hci.h create mode 100644 hci_lib.h create mode 100644 io-mainloop.c create mode 100644 io.h create mode 100644 json.c create mode 100644 json.h create mode 100644 l2cap.h create mode 100644 main.c create mode 100644 mainloop.c create mode 100644 mainloop.h create mode 100644 makefile create mode 100644 mqtt.c create mode 100644 mqtt.h create mode 100644 queue.c create mode 100644 queue.h create mode 100644 timeout-glib.c create mode 100644 timeout.h create mode 100644 util.c create mode 100644 util.h create mode 100644 uuid.c create mode 100644 uuid.h diff --git a/README.md b/README.md index 04f04ab..e15256e 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,56 @@ -# solarlife -Get data from cheap solar charge controller and pass it to MQTT broker +#Solarlife + +This is a simple approach to read, parse and pass data from cheap solar charge controllers like Lumiax and Volt to MQTT broker. +You can pass data to home automation software like Home Assistant, Openhab and others. + +Parameters that you will be able to get are for example: + + -battery voltage + -battery current + -battery power + -battery percentage capability + -solar voltage + -solar current + -solar power + -load voltage + -load current + -load power + -and many more... + + +Based on: + https://github.com/majki09/lumiax_solar_bt/tree/main + https://github.com/gbrault/gattclient/tree/master + https://ctlsys.com/support/how_to_compute_the_modbus_rtu_message_crc/ + https://github.com/kokke/tiny-MQTT-c + https://github.com/rpz80/json + Lumiax Modbus Communication Protocol V3.9-1.pdf + +Depends on: + libglib-2.0 + +Tested on VM running Ubuntu 21.04 and Fritz-Box 6690 Cable router. + +To use this app you will need a Bluetooth adapter with low energy capability and compatible solar charger with Bluetooth. +Generally speaking all of charging controllers that uses "Solar Life" app for Android should work. + + +TODO: + -Fix bugs + -Pass commands to controller (load on/off, clock setup, etc.) + +Example output: + +root@ubuntu:/usr/src/solarlife# ./solarlife -d 04:7f:0e:51:21:4c -a test.mqtt.com +Connecting to BT device... Done +MQTT: connected to server. +Service Added - UUID: 00001801-0000-1000-8000-00805f9b34fb start: 0x0001 end: 0x0003 +Service Added - UUID: 00001800-0000-1000-8000-00805f9b34fb start: 0x0004 end: 0x000e +Service Added - UUID: 0000ff00-0000-1000-8000-00805f9b34fb start: 0x000f end: 0x001a +GATT discovery procedures complete +Registering notify handler with id: 2 +Registered notify handler! +{"PV_rated_voltage":50,"PV_rated_current":10,"PV_rated_power_l":130,"PV_rated_power_h":0,"battery_rated_voltage":17,"battery_rated_current":10,"battery_rated_power_l":130,"battery_rated_power_h":0,"load_rated_voltage":17,"load_rated_current":10,"load_rated_power_l":130,"load_rated_power_h":0,"slave_id":1,"running_days":6,"sys_voltage":12,"battery_status":3,"charge_status":32,"discharge_status":0,"env_temperature":13,"sys_temperature":6,"undervoltage_times":255,"fullycharged_times":1,"overvoltage_prot_times":0,"overcurrent_prot_times":0,"shortcircuit_prot_times":0,"opencircuit_prot_times":0,"hw_prot_times":0,"charge_overtemp_prot_times":0,"discharge_overtemp_prot_times":0,"battery_remaining_capacity":16,"battery_voltage":11.13,"battery_current":0,"battery_power_lo":0,"battery_power_hi":0,"load_voltage":0,"load_current":0,"load_power_l":0,"load_power_h":0,"solar_voltage":0.2,"solar_current":0,"solar_power_l":0,"solar_power_h":0,"daily_production":0.01,"total_production_l":0.82,"total_production_h":0,"daily_consumption":0.02,"total_consumption_l":0.83,"total_consumption_h":0,"lighttime_daily":695,"monthly_production_l":0,"monthly_production_h":0,"yearly_production_l":0,"yearly_production_h":0,"timestamp":"2023-10-20T19:15:58Z"} +MQTT server acknowledged data + + diff --git a/att-types.h b/att-types.h new file mode 100644 index 0000000..d474495 --- /dev/null +++ b/att-types.h @@ -0,0 +1,147 @@ +/* + * + * 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_SECURITY_AUTO 0 +#define BT_ATT_SECURITY_LOW 1 +#define BT_ATT_SECURITY_MEDIUM 2 +#define BT_ATT_SECURITY_HIGH 3 + +#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 + +/* + * 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 + +/* + * 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_READ_ENCRYPT 0x04 +#define BT_ATT_PERM_WRITE_ENCRYPT 0x08 +#define BT_ATT_PERM_ENCRYPT (BT_ATT_PERM_READ_ENCRYPT | \ + BT_ATT_PERM_WRITE_ENCRYPT) +#define BT_ATT_PERM_READ_AUTHEN 0x10 +#define BT_ATT_PERM_WRITE_AUTHEN 0x20 +#define BT_ATT_PERM_AUTHEN (BT_ATT_PERM_READ_AUTHEN | \ + BT_ATT_PERM_WRITE_AUTHEN) +#define BT_ATT_PERM_AUTHOR 0x40 +#define BT_ATT_PERM_NONE 0x80 + +/* 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 +#define BT_GATT_CHRC_EXT_PROP_ENC_READ 0x04 +#define BT_GATT_CHRC_EXT_PROP_ENC_WRITE 0x08 +#define BT_GATT_CHRC_EXT_PROP_ENC (BT_GATT_CHRC_EXT_PROP_ENC_READ | \ + BT_GATT_CHRC_EXT_PROP_ENC_WRITE) +#define BT_GATT_CHRC_EXT_PROP_AUTH_READ 0x10 +#define BT_GATT_CHRC_EXT_PROP_AUTH_WRITE 0x20 +#define BT_GATT_CHRC_EXT_PROP_AUTH (BT_GATT_CHRC_EXT_PROP_AUTH_READ | \ + BT_GATT_CHRC_EXT_PROP_AUTH_WRITE) diff --git a/att.c b/att.c new file mode 100644 index 0000000..51a3692 --- /dev/null +++ b/att.c @@ -0,0 +1,1532 @@ +/** + * @file att.c + * @brief att protocol implementation + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + * @see gatt-helpers.c for pdu creation + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include + +#include "io.h" +#include "queue.h" +#include "util.h" +#include "timeout.h" +#include "bluetooth.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 */ + +/* Length of signature in write signed packet */ +#define BT_ATT_SIGNATURE_LEN 12 + +struct att_send_op; + +/** + * ATT structure (protocol context) + */ +struct bt_att { + /// reference counter incremented by bt_att_ref, decremented by bt_att_unref + int ref_count; + /// socket + int fd; + /// io structure for low level i/o (read and write) + struct io *io; + /// true if an l2cap socket + bool io_on_l2cap; + /// i/o seurity level: Only used for non-L2CAP + int io_sec_level; + /// Queued ATT protocol requests + struct queue *req_queue; + /// Pending request state + struct att_send_op *pending_req; + /// Queued ATT protocol indications + struct queue *ind_queue; + /// Pending indication state + struct att_send_op *pending_ind; + /// Queue of PDUs ready to send + struct queue *write_queue; + /// true if already engaged in write operation + bool writer_active; + /// List of registered callbacks + struct queue *notify_list; + /// List of disconnect handlers + struct queue *disconn_list; + /// There's a pending incoming request + bool in_req; + /// buffer pointer + uint8_t *buf; + /// actual number of bytes for pdu ATT exchange + uint16_t mtu; + /// IDs for "send" ops + unsigned int next_send_id; + /// IDs for registered callbacks + unsigned int next_reg_id; + /// timeout function for callback + bt_att_timeout_func_t timeout_callback; + /// timeout function to manage data context (house keeping) + bt_att_destroy_func_t timeout_destroy; + /// + void *timeout_data; + /// debug callback + bt_att_debug_func_t debug_callback; + /// data management function for debug + bt_att_destroy_func_t debug_destroy; + /// user pointer for debug + void *debug_data; + /// crypto structure + //struct bt_crypto *crypto; + /// true, requires key signature + bool ext_signed; + /// local key structure pointer + struct sign_info *local_sign; + /// remote key structure pointer + 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; +}; + +/** + * @brief destroy att send operation + * calls the destroy callback with user_data as an argument + * free pdu data + * + * @param data att_send_op pointer + */ +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 bool change_security(struct bt_att *att, uint8_t ecode) +{ + int security; + + security = bt_att_get_security(att); + if (security != BT_ATT_SECURITY_AUTO) + return false; + + if (ecode == BT_ATT_ERROR_INSUFFICIENT_ENCRYPTION && + security < BT_ATT_SECURITY_MEDIUM) + security = BT_ATT_SECURITY_MEDIUM; + else if (ecode == BT_ATT_ERROR_AUTHENTICATION && + security < BT_ATT_SECURITY_HIGH) + security = BT_ATT_SECURITY_HIGH; + else + return false; + + return bt_att_set_security(att, security); +} + +static bool handle_error_rsp(struct bt_att *att, uint8_t *pdu, + ssize_t pdu_len, uint8_t *opcode) +{ + const struct bt_att_pdu_error_rsp *rsp; + struct att_send_op *op = att->pending_req; + + if (pdu_len != sizeof(*rsp)) { + *opcode = 0; + return false; + } + + rsp = (void *) pdu; + + *opcode = rsp->opcode; + + /* Attempt to change security */ + if (!change_security(att, rsp->ecode)) + return false; + + util_debug(att->debug_callback, att->debug_data, + "Retrying operation %p", op); + + att->pending_req = NULL; + + /* Push operation back to request queue */ + return queue_push_head(att->req_queue, op); +} + +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) { + /* Return if error response cause a retry */ + if (handle_error_rsp(att, pdu, pdu_len, &req_opcode)) { + wakeup_writer(att); + return; + } + } 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) +{ + struct bt_att_pdu_error_rsp pdu; + + pdu.opcode = opcode; + pdu.handle = 0x0000; + pdu.ecode = 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) && !att->ext_signed) { + 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, bool ext_signed) +{ + struct bt_att *att; + + if (fd < 0) + return NULL; + + att = new0(struct bt_att, 1); + if (!att) + return NULL; + + att->fd = fd; + att->ext_signed = ext_signed; + 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 */ + //if (!ext_signed) + // 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; +} + +/** + * encode & send an att message + * If (op-code type == ATT_OP_TYPE_REQ) process the response with callback function + * see att_opcode_type_table (att.c) for opcode type definition + * + * @param att structure of the communication channel + * @param opcode att message op-code + * @param pdu protocol data unit buffer + * @param length size of pdu + * @param callback callback function depending on opcode to process response + * @param user_data request data when relevant + * @param destroy function to manage user_data + * + * @return att message sequence number or 0 if error + */ +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_security(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_security(struct bt_att *att, int level) +{ + struct bt_security sec; + + if (!att || level < BT_ATT_SECURITY_AUTO || + level > BT_ATT_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; +} +*/ \ No newline at end of file diff --git a/att.h b/att.h new file mode 100644 index 0000000..3122080 --- /dev/null +++ b/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, bool ext_signed); + +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_security(struct bt_att *att); +bool bt_att_set_security(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/bluetooth.c b/bluetooth.c new file mode 100644 index 0000000..dbbb174 --- /dev/null +++ b/bluetooth.c @@ -0,0 +1,1456 @@ +/** + * @file bluetooth.c + * @brief various bluetooth functions + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include "bluetooth.h" +#include "hci.h" + +void baswap(bdaddr_t *dst, const bdaddr_t *src) +{ + register unsigned char *d = (unsigned char *) dst; + register const unsigned char *s = (const unsigned char *) src; + register int i; + + for (i = 0; i < 6; i++) + d[i] = s[5-i]; +} + +char *batostr(const bdaddr_t *ba) +{ + char *str = bt_malloc(18); + if (!str) + return NULL; + + sprintf(str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X", + ba->b[0], ba->b[1], ba->b[2], + ba->b[3], ba->b[4], ba->b[5]); + + return str; +} + +bdaddr_t *strtoba(const char *str) +{ + bdaddr_t b; + bdaddr_t *ba = bt_malloc(sizeof(*ba)); + + if (ba) { + str2ba(str, &b); + baswap(ba, &b); + } + + return ba; +} + +int ba2str(const bdaddr_t *ba, char *str) +{ + return sprintf(str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X", + ba->b[5], ba->b[4], ba->b[3], ba->b[2], ba->b[1], ba->b[0]); +} + +int str2ba(const char *str, bdaddr_t *ba) +{ + int i; + + if (bachk(str) < 0) { + memset(ba, 0, sizeof(*ba)); + return -1; + } + + for (i = 5; i >= 0; i--, str += 3) + ba->b[i] = strtol(str, NULL, 16); + + return 0; +} + +int ba2oui(const bdaddr_t *ba, char *str) +{ + return sprintf(str, "%2.2X-%2.2X-%2.2X", ba->b[5], ba->b[4], ba->b[3]); +} + +int bachk(const char *str) +{ + if (!str) + return -1; + + if (strlen(str) != 17) + return -1; + + while (*str) { + if (!isxdigit(*str++)) + return -1; + + if (!isxdigit(*str++)) + return -1; + + if (*str == 0) + break; + + if (*str++ != ':') + return -1; + } + + return 0; +} + +int baprintf(const char *format, ...) +{ + va_list ap; + int len; + + va_start(ap, format); + len = vprintf(format, ap); + va_end(ap); + + return len; +} + +int bafprintf(FILE *stream, const char *format, ...) +{ + va_list ap; + int len; + + va_start(ap, format); + len = vfprintf(stream, format, ap); + va_end(ap); + + return len; +} + +int basprintf(char *str, const char *format, ...) +{ + va_list ap; + int len; + + va_start(ap, format); + len = vsnprintf(str, (~0U) >> 1, format, ap); + va_end(ap); + + return len; +} + +int basnprintf(char *str, size_t size, const char *format, ...) +{ + va_list ap; + int len; + + va_start(ap, format); + len = vsnprintf(str, size, format, ap); + va_end(ap); + + return len; +} + +void *bt_malloc(size_t size) +{ + return malloc(size); +} + +void bt_free(void *ptr) +{ + free(ptr); +} + +/* Bluetooth error codes to Unix errno mapping */ +int bt_error(uint16_t code) +{ + switch (code) { + case 0: + return 0; + case HCI_UNKNOWN_COMMAND: + return EBADRQC; + case HCI_NO_CONNECTION: + return ENOTCONN; + case HCI_HARDWARE_FAILURE: + return EIO; + case HCI_PAGE_TIMEOUT: + return EHOSTDOWN; + case HCI_AUTHENTICATION_FAILURE: + return EACCES; + case HCI_PIN_OR_KEY_MISSING: + return EINVAL; + case HCI_MEMORY_FULL: + return ENOMEM; + case HCI_CONNECTION_TIMEOUT: + return ETIMEDOUT; + case HCI_MAX_NUMBER_OF_CONNECTIONS: + case HCI_MAX_NUMBER_OF_SCO_CONNECTIONS: + return EMLINK; + case HCI_ACL_CONNECTION_EXISTS: + return EALREADY; + case HCI_COMMAND_DISALLOWED: + case HCI_TRANSACTION_COLLISION: + case HCI_ROLE_SWITCH_PENDING: + return EBUSY; + case HCI_REJECTED_LIMITED_RESOURCES: + case HCI_REJECTED_PERSONAL: + case HCI_QOS_REJECTED: + return ECONNREFUSED; + case HCI_HOST_TIMEOUT: + return ETIMEDOUT; + case HCI_UNSUPPORTED_FEATURE: + case HCI_QOS_NOT_SUPPORTED: + case HCI_PAIRING_NOT_SUPPORTED: + case HCI_CLASSIFICATION_NOT_SUPPORTED: + case HCI_UNSUPPORTED_LMP_PARAMETER_VALUE: + case HCI_PARAMETER_OUT_OF_RANGE: + case HCI_QOS_UNACCEPTABLE_PARAMETER: + return EOPNOTSUPP; + case HCI_INVALID_PARAMETERS: + case HCI_SLOT_VIOLATION: + return EINVAL; + case HCI_OE_USER_ENDED_CONNECTION: + case HCI_OE_LOW_RESOURCES: + case HCI_OE_POWER_OFF: + return ECONNRESET; + case HCI_CONNECTION_TERMINATED: + return ECONNABORTED; + case HCI_REPEATED_ATTEMPTS: + return ELOOP; + case HCI_REJECTED_SECURITY: + case HCI_PAIRING_NOT_ALLOWED: + case HCI_INSUFFICIENT_SECURITY: + return EACCES; + case HCI_UNSUPPORTED_REMOTE_FEATURE: + return EPROTONOSUPPORT; + case HCI_SCO_OFFSET_REJECTED: + return ECONNREFUSED; + case HCI_UNKNOWN_LMP_PDU: + case HCI_INVALID_LMP_PARAMETERS: + case HCI_LMP_ERROR_TRANSACTION_COLLISION: + case HCI_LMP_PDU_NOT_ALLOWED: + case HCI_ENCRYPTION_MODE_NOT_ACCEPTED: + return EPROTO; + default: + return ENOSYS; + } +} + +const char *bt_compidtostr(int compid) +{ + switch (compid) { + case 0: + return "Ericsson Technology Licensing"; + case 1: + return "Nokia Mobile Phones"; + case 2: + return "Intel Corp."; + case 3: + return "IBM Corp."; + case 4: + return "Toshiba Corp."; + case 5: + return "3Com"; + case 6: + return "Microsoft"; + case 7: + return "Lucent"; + case 8: + return "Motorola"; + case 9: + return "Infineon Technologies AG"; + case 10: + return "Cambridge Silicon Radio"; + case 11: + return "Silicon Wave"; + case 12: + return "Digianswer A/S"; + case 13: + return "Texas Instruments Inc."; + case 14: + return "Ceva, Inc. (formerly Parthus Technologies, Inc.)"; + case 15: + return "Broadcom Corporation"; + case 16: + return "Mitel Semiconductor"; + case 17: + return "Widcomm, Inc"; + case 18: + return "Zeevo, Inc."; + case 19: + return "Atmel Corporation"; + case 20: + return "Mitsubishi Electric Corporation"; + case 21: + return "RTX Telecom A/S"; + case 22: + return "KC Technology Inc."; + case 23: + return "NewLogic"; + case 24: + return "Transilica, Inc."; + case 25: + return "Rohde & Schwarz GmbH & Co. KG"; + case 26: + return "TTPCom Limited"; + case 27: + return "Signia Technologies, Inc."; + case 28: + return "Conexant Systems Inc."; + case 29: + return "Qualcomm"; + case 30: + return "Inventel"; + case 31: + return "AVM Berlin"; + case 32: + return "BandSpeed, Inc."; + case 33: + return "Mansella Ltd"; + case 34: + return "NEC Corporation"; + case 35: + return "WavePlus Technology Co., Ltd."; + case 36: + return "Alcatel"; + case 37: + return "NXP Semiconductors (formerly Philips Semiconductors)"; + case 38: + return "C Technologies"; + case 39: + return "Open Interface"; + case 40: + return "R F Micro Devices"; + case 41: + return "Hitachi Ltd"; + case 42: + return "Symbol Technologies, Inc."; + case 43: + return "Tenovis"; + case 44: + return "Macronix International Co. Ltd."; + case 45: + return "GCT Semiconductor"; + case 46: + return "Norwood Systems"; + case 47: + return "MewTel Technology Inc."; + case 48: + return "ST Microelectronics"; + case 49: + return "Synopsis"; + case 50: + return "Red-M (Communications) Ltd"; + case 51: + return "Commil Ltd"; + case 52: + return "Computer Access Technology Corporation (CATC)"; + case 53: + return "Eclipse (HQ Espana) S.L."; + case 54: + return "Renesas Electronics Corporation"; + case 55: + return "Mobilian Corporation"; + case 56: + return "Terax"; + case 57: + return "Integrated System Solution Corp."; + case 58: + return "Matsushita Electric Industrial Co., Ltd."; + case 59: + return "Gennum Corporation"; + case 60: + return "BlackBerry Limited (formerly Research In Motion)"; + case 61: + return "IPextreme, Inc."; + case 62: + return "Systems and Chips, Inc."; + case 63: + return "Bluetooth SIG, Inc."; + case 64: + return "Seiko Epson Corporation"; + case 65: + return "Integrated Silicon Solution Taiwan, Inc."; + case 66: + return "CONWISE Technology Corporation Ltd"; + case 67: + return "PARROT SA"; + case 68: + return "Socket Mobile"; + case 69: + return "Atheros Communications, Inc."; + case 70: + return "MediaTek, Inc."; + case 71: + return "Bluegiga"; + case 72: + return "Marvell Technology Group Ltd."; + case 73: + return "3DSP Corporation"; + case 74: + return "Accel Semiconductor Ltd."; + case 75: + return "Continental Automotive Systems"; + case 76: + return "Apple, Inc."; + case 77: + return "Staccato Communications, Inc."; + case 78: + return "Avago Technologies"; + case 79: + return "APT Licensing Ltd."; + case 80: + return "SiRF Technology"; + case 81: + return "Tzero Technologies, Inc."; + case 82: + return "J&M Corporation"; + case 83: + return "Free2move AB"; + case 84: + return "3DiJoy Corporation"; + case 85: + return "Plantronics, Inc."; + case 86: + return "Sony Ericsson Mobile Communications"; + case 87: + return "Harman International Industries, Inc."; + case 88: + return "Vizio, Inc."; + case 89: + return "Nordic Semiconductor ASA"; + case 90: + return "EM Microelectronic-Marin SA"; + case 91: + return "Ralink Technology Corporation"; + case 92: + return "Belkin International, Inc."; + case 93: + return "Realtek Semiconductor Corporation"; + case 94: + return "Stonestreet One, LLC"; + case 95: + return "Wicentric, Inc."; + case 96: + return "RivieraWaves S.A.S"; + case 97: + return "RDA Microelectronics"; + case 98: + return "Gibson Guitars"; + case 99: + return "MiCommand Inc."; + case 100: + return "Band XI International, LLC"; + case 101: + return "Hewlett-Packard Company"; + case 102: + return "9Solutions Oy"; + case 103: + return "GN Netcom A/S"; + case 104: + return "General Motors"; + case 105: + return "A&D Engineering, Inc."; + case 106: + return "MindTree Ltd."; + case 107: + return "Polar Electro OY"; + case 108: + return "Beautiful Enterprise Co., Ltd."; + case 109: + return "BriarTek, Inc."; + case 110: + return "Summit Data Communications, Inc."; + case 111: + return "Sound ID"; + case 112: + return "Monster, LLC"; + case 113: + return "connectBlue AB"; + case 114: + return "ShangHai Super Smart Electronics Co. Ltd."; + case 115: + return "Group Sense Ltd."; + case 116: + return "Zomm, LLC"; + case 117: + return "Samsung Electronics Co. Ltd."; + case 118: + return "Creative Technology Ltd."; + case 119: + return "Laird Technologies"; + case 120: + return "Nike, Inc."; + case 121: + return "lesswire AG"; + case 122: + return "MStar Semiconductor, Inc."; + case 123: + return "Hanlynn Technologies"; + case 124: + return "A & R Cambridge"; + case 125: + return "Seers Technology Co. Ltd"; + case 126: + return "Sports Tracking Technologies Ltd."; + case 127: + return "Autonet Mobile"; + case 128: + return "DeLorme Publishing Company, Inc."; + case 129: + return "WuXi Vimicro"; + case 130: + return "Sennheiser Communications A/S"; + case 131: + return "TimeKeeping Systems, Inc."; + case 132: + return "Ludus Helsinki Ltd."; + case 133: + return "BlueRadios, Inc."; + case 134: + return "equinox AG"; + case 135: + return "Garmin International, Inc."; + case 136: + return "Ecotest"; + case 137: + return "GN ReSound A/S"; + case 138: + return "Jawbone"; + case 139: + return "Topcorn Positioning Systems, LLC"; + case 140: + return "Gimbal Inc. (formerly Qualcomm Labs, Inc. and Qualcomm Retail Solutions, Inc.)"; + case 141: + return "Zscan Software"; + case 142: + return "Quintic Corp."; + case 143: + return "Stollman E+V GmbH"; + case 144: + return "Funai Electric Co., Ltd."; + case 145: + return "Advanced PANMOBIL Systems GmbH & Co. KG"; + case 146: + return "ThinkOptics, Inc."; + case 147: + return "Universal Electronics, Inc."; + case 148: + return "Airoha Technology Corp."; + case 149: + return "NEC Lighting, Ltd."; + case 150: + return "ODM Technology, Inc."; + case 151: + return "ConnecteDevice Ltd."; + case 152: + return "zer01.tv GmbH"; + case 153: + return "i.Tech Dynamic Global Distribution Ltd."; + case 154: + return "Alpwise"; + case 155: + return "Jiangsu Toppower Automotive Electronics Co., Ltd."; + case 156: + return "Colorfy, Inc."; + case 157: + return "Geoforce Inc."; + case 158: + return "Bose Corporation"; + case 159: + return "Suunto Oy"; + case 160: + return "Kensington Computer Products Group"; + case 161: + return "SR-Medizinelektronik"; + case 162: + return "Vertu Corporation Limited"; + case 163: + return "Meta Watch Ltd."; + case 164: + return "LINAK A/S"; + case 165: + return "OTL Dynamics LLC"; + case 166: + return "Panda Ocean Inc."; + case 167: + return "Visteon Corporation"; + case 168: + return "ARP Devices Limited"; + case 169: + return "Magneti Marelli S.p.A"; + case 170: + return "CAEN RFID srl"; + case 171: + return "Ingenieur-Systemgruppe Zahn GmbH"; + case 172: + return "Green Throttle Games"; + case 173: + return "Peter Systemtechnik GmbH"; + case 174: + return "Omegawave Oy"; + case 175: + return "Cinetix"; + case 176: + return "Passif Semiconductor Corp"; + case 177: + return "Saris Cycling Group, Inc"; + case 178: + return "Bekey A/S"; + case 179: + return "Clarinox Technologies Pty. Ltd."; + case 180: + return "BDE Technology Co., Ltd."; + case 181: + return "Swirl Networks"; + case 182: + return "Meso international"; + case 183: + return "TreLab Ltd"; + case 184: + return "Qualcomm Innovation Center, Inc. (QuIC)"; + case 185: + return "Johnson Controls, Inc."; + case 186: + return "Starkey Laboratories Inc."; + case 187: + return "S-Power Electronics Limited"; + case 188: + return "Ace Sensor Inc"; + case 189: + return "Aplix Corporation"; + case 190: + return "AAMP of America"; + case 191: + return "Stalmart Technology Limited"; + case 192: + return "AMICCOM Electronics Corporation"; + case 193: + return "Shenzhen Excelsecu Data Technology Co.,Ltd"; + case 194: + return "Geneq Inc."; + case 195: + return "adidas AG"; + case 196: + return "LG Electronics"; + case 197: + return "Onset Computer Corporation"; + case 198: + return "Selfly BV"; + case 199: + return "Quuppa Oy."; + case 200: + return "GeLo Inc"; + case 201: + return "Evluma"; + case 202: + return "MC10"; + case 203: + return "Binauric SE"; + case 204: + return "Beats Electronics"; + case 205: + return "Microchip Technology Inc."; + case 206: + return "Elgato Systems GmbH"; + case 207: + return "ARCHOS SA"; + case 208: + return "Dexcom, Inc."; + case 209: + return "Polar Electro Europe B.V."; + case 210: + return "Dialog Semiconductor B.V."; + case 211: + return "Taixingbang Technology (HK) Co,. LTD."; + case 212: + return "Kawantech"; + case 213: + return "Austco Communication Systems"; + case 214: + return "Timex Group USA, Inc."; + case 215: + return "Qualcomm Technologies, Inc."; + case 216: + return "Qualcomm Connected Experiences, Inc."; + case 217: + return "Voyetra Turtle Beach"; + case 218: + return "txtr GmbH"; + case 219: + return "Biosentronics"; + case 220: + return "Procter & Gamble"; + case 221: + return "Hosiden Corporation"; + case 222: + return "Muzik LLC"; + case 223: + return "Misfit Wearables Corp"; + case 224: + return "Google"; + case 225: + return "Danlers Ltd"; + case 226: + return "Semilink Inc"; + case 227: + return "inMusic Brands, Inc"; + case 228: + return "L.S. Research Inc."; + case 229: + return "Eden Software Consultants Ltd."; + case 230: + return "Freshtemp"; + case 231: + return "KS Technologies"; + case 232: + return "ACTS Technologies"; + case 233: + return "Vtrack Systems"; + case 234: + return "Nielsen-Kellerman Company"; + case 235: + return "Server Technology, Inc."; + case 236: + return "BioResearch Associates"; + case 237: + return "Jolly Logic, LLC"; + case 238: + return "Above Average Outcomes, Inc."; + case 239: + return "Bitsplitters GmbH"; + case 240: + return "PayPal, Inc."; + case 241: + return "Witron Technology Limited"; + case 242: + return "Aether Things Inc. (formerly Morse Project Inc.)"; + case 243: + return "Kent Displays Inc."; + case 244: + return "Nautilus Inc."; + case 245: + return "Smartifier Oy"; + case 246: + return "Elcometer Limited"; + case 247: + return "VSN Technologies Inc."; + case 248: + return "AceUni Corp., Ltd."; + case 249: + return "StickNFind"; + case 250: + return "Crystal Code AB"; + case 251: + return "KOUKAAM a.s."; + case 252: + return "Delphi Corporation"; + case 253: + return "ValenceTech Limited"; + case 254: + return "Reserved"; + case 255: + return "Typo Products, LLC"; + case 256: + return "TomTom International BV"; + case 257: + return "Fugoo, Inc"; + case 258: + return "Keiser Corporation"; + case 259: + return "Bang & Olufsen A/S"; + case 260: + return "PLUS Locations Systems Pty Ltd"; + case 261: + return "Ubiquitous Computing Technology Corporation"; + case 262: + return "Innovative Yachtter Solutions"; + case 263: + return "William Demant Holding A/S"; + case 264: + return "Chicony Electronics Co., Ltd."; + case 265: + return "Atus BV"; + case 266: + return "Codegate Ltd."; + case 267: + return "ERi, Inc."; + case 268: + return "Transducers Direct, LLC"; + case 269: + return "Fujitsu Ten Limited"; + case 270: + return "Audi AG"; + case 271: + return "HiSilicon Technologies Co., Ltd."; + case 272: + return "Nippon Seiki Co., Ltd."; + case 273: + return "Steelseries ApS"; + case 274: + return "vyzybl Inc."; + case 275: + return "Openbrain Technologies, Co., Ltd."; + case 276: + return "Xensr"; + case 277: + return "e.solutions"; + case 278: + return "1OAK Technologies"; + case 279: + return "Wimoto Technologies Inc"; + case 280: + return "Radius Networks, Inc."; + case 281: + return "Wize Technology Co., Ltd."; + case 282: + return "Qualcomm Labs, Inc."; + case 283: + return "Aruba Networks"; + case 284: + return "Baidu"; + case 285: + return "Arendi AG"; + case 286: + return "Skoda Auto a.s."; + case 287: + return "Volkswagon AG"; + case 288: + return "Porsche AG"; + case 289: + return "Sino Wealth Electronic Ltd."; + case 290: + return "AirTurn, Inc."; + case 291: + return "Kinsa, Inc."; + case 292: + return "HID Global"; + case 293: + return "SEAT es"; + case 294: + return "Promethean Ltd."; + case 295: + return "Salutica Allied Solutions"; + case 296: + return "GPSI Group Pty Ltd"; + case 297: + return "Nimble Devices Oy"; + case 298: + return "Changzhou Yongse Infotech Co., Ltd"; + case 299: + return "SportIQ"; + case 300: + return "TEMEC Instruments B.V."; + case 301: + return "Sony Corporation"; + case 302: + return "ASSA ABLOY"; + case 303: + return "Clarion Co., Ltd."; + case 304: + return "Warehouse Innovations"; + case 305: + return "Cypress Semiconductor Corporation"; + case 306: + return "MADS Inc"; + case 307: + return "Blue Maestro Limited"; + case 308: + return "Resolution Products, Inc."; + case 309: + return "Airewear LLC"; + case 310: + return "Seed Labs, Inc. (formerly ETC sp. z.o.o.)"; + case 311: + return "Prestigio Plaza Ltd."; + case 312: + return "NTEO Inc."; + case 313: + return "Focus Systems Corporation"; + case 314: + return "Tencent Holdings Limited"; + case 315: + return "Allegion"; + case 316: + return "Murata Manufacuring Co., Ltd."; + case 317: + return "WirelessWERX"; + case 318: + return "Nod, Inc."; + case 319: + return "B&B Manufacturing Company"; + case 320: + return "Alpine Electronics (China) Co., Ltd"; + case 321: + return "FedEx Services"; + case 322: + return "Grape Systems Inc."; + case 323: + return "Bkon Connect"; + case 324: + return "Lintech GmbH"; + case 325: + return "Novatel Wireless"; + case 326: + return "Ciright"; + case 327: + return "Mighty Cast, Inc."; + case 328: + return "Ambimat Electronics"; + case 329: + return "Perytons Ltd."; + case 330: + return "Tivoli Audio, LLC"; + case 331: + return "Master Lock"; + case 332: + return "Mesh-Net Ltd"; + case 333: + return "Huizhou Desay SV Automotive CO., LTD."; + case 334: + return "Tangerine, Inc."; + case 335: + return "B&W Group Ltd."; + case 336: + return "Pioneer Corporation"; + case 337: + return "OnBeep"; + case 338: + return "Vernier Software & Technology"; + case 339: + return "ROL Ergo"; + case 340: + return "Pebble Technology"; + case 341: + return "NETATMO"; + case 342: + return "Accumulate AB"; + case 343: + return "Anhui Huami Information Technology Co., Ltd."; + case 344: + return "Inmite s.r.o."; + case 345: + return "ChefSteps, Inc."; + case 346: + return "micas AG"; + case 347: + return "Biomedical Research Ltd."; + case 348: + return "Pitius Tec S.L."; + case 349: + return "Estimote, Inc."; + case 350: + return "Unikey Technologies, Inc."; + case 351: + return "Timer Cap Co."; + case 352: + return "AwoX"; + case 353: + return "yikes"; + case 354: + return "MADSGlobal NZ Ltd."; + case 355: + return "PCH International"; + case 356: + return "Qingdao Yeelink Information Technology Co., Ltd."; + case 357: + return "Milwaukee Tool (formerly Milwaukee Electric Tools)"; + case 358: + return "MISHIK Pte Ltd"; + case 359: + return "Bayer HealthCare"; + case 360: + return "Spicebox LLC"; + case 361: + return "emberlight"; + case 362: + return "Cooper-Atkins Corporation"; + case 363: + return "Qblinks"; + case 364: + return "MYSPHERA"; + case 365: + return "LifeScan Inc"; + case 366: + return "Volantic AB"; + case 367: + return "Podo Labs, Inc"; + case 368: + return "Roche Diabetes Care AG"; + case 369: + return "Amazon Fulfillment Service"; + case 370: + return "Connovate Technology Private Limited"; + case 371: + return "Kocomojo, LLC"; + case 372: + return "Everykey LLC"; + case 373: + return "Dynamic Controls"; + case 374: + return "SentriLock"; + case 375: + return "I-SYST inc."; + case 376: + return "CASIO COMPUTER CO., LTD."; + case 377: + return "LAPIS Semiconductor Co., Ltd."; + case 378: + return "Telemonitor, Inc."; + case 379: + return "taskit GmbH"; + case 380: + return "Daimler AG"; + case 381: + return "BatAndCat"; + case 382: + return "BluDotz Ltd"; + case 383: + return "XTel ApS"; + case 384: + return "Gigaset Communications GmbH"; + case 385: + return "Gecko Health Innovations, Inc."; + case 386: + return "HOP Ubiquitous"; + case 387: + return "To Be Assigned"; + case 388: + return "Nectar"; + case 389: + return "bel'apps LLC"; + case 390: + return "CORE Lighting Ltd"; + case 391: + return "Seraphim Sense Ltd"; + case 392: + return "Unico RBC"; + case 393: + return "Physical Enterprises Inc."; + case 394: + return "Able Trend Technology Limited"; + case 395: + return "Konica Minolta, Inc."; + case 396: + return "Wilo SE"; + case 397: + return "Extron Design Services"; + case 398: + return "Fitbit, Inc."; + case 399: + return "Fireflies Systems"; + case 400: + return "Intelletto Technologies Inc."; + case 401: + return "FDK CORPORATION"; + case 402: + return "Cloudleaf, Inc"; + case 403: + return "Maveric Automation LLC"; + case 404: + return "Acoustic Stream Corporation"; + case 405: + return "Zuli"; + case 406: + return "Paxton Access Ltd"; + case 407: + return "WiSilica Inc"; + case 408: + return "Vengit Limited"; + case 409: + return "SALTO SYSTEMS S.L."; + case 410: + return "TRON Forum (formerly T-Engine Forum)"; + case 411: + return "CUBETECH s.r.o."; + case 412: + return "Cokiya Incorporated"; + case 413: + return "CVS Health"; + case 414: + return "Ceruus"; + case 415: + return "Strainstall Ltd"; + case 416: + return "Channel Enterprises (HK) Ltd."; + case 417: + return "FIAMM"; + case 418: + return "GIGALANE.CO.,LTD"; + case 419: + return "EROAD"; + case 420: + return "Mine Safety Appliances"; + case 421: + return "Icon Health and Fitness"; + case 422: + return "Asandoo GmbH"; + case 423: + return "ENERGOUS CORPORATION"; + case 424: + return "Taobao"; + case 425: + return "Canon Inc."; + case 426: + return "Geophysical Technology Inc."; + case 427: + return "Facebook, Inc."; + case 428: + return "Nipro Diagnostics, Inc."; + case 429: + return "FlightSafety International"; + case 430: + return "Earlens Corporation"; + case 431: + return "Sunrise Micro Devices, Inc."; + case 432: + return "Star Micronics Co., Ltd."; + case 433: + return "Netizens Sp. z o.o."; + case 434: + return "Nymi Inc."; + case 435: + return "Nytec, Inc."; + case 436: + return "Trineo Sp. z o.o."; + case 437: + return "Nest Labs Inc."; + case 438: + return "LM Technologies Ltd"; + case 439: + return "General Electric Company"; + case 440: + return "i+D3 S.L."; + case 441: + return "HANA Micron"; + case 442: + return "Stages Cycling LLC"; + case 443: + return "Cochlear Bone Anchored Solutions AB"; + case 444: + return "SenionLab AB"; + case 445: + return "Syszone Co., Ltd"; + case 446: + return "Pulsate Mobile Ltd."; + case 447: + return "Hong Kong HunterSun Electronic Limited"; + case 448: + return "pironex GmbH"; + case 449: + return "BRADATECH Corp."; + case 450: + return "Transenergooil AG"; + case 451: + return "Bunch"; + case 452: + return "DME Microelectronics"; + case 453: + return "Bitcraze AB"; + case 454: + return "HASWARE Inc."; + case 455: + return "Abiogenix Inc."; + case 456: + return "Poly-Control ApS"; + case 457: + return "Avi-on"; + case 458: + return "Laerdal Medical AS"; + case 459: + return "Fetch My Pet"; + case 460: + return "Sam Labs Ltd."; + case 461: + return "Chengdu Synwing Technology Ltd"; + case 462: + return "HOUWA SYSTEM DESIGN, k.k."; + case 463: + return "BSH"; + case 464: + return "Primus Inter Pares Ltd"; + case 465: + return "August"; + case 466: + return "Gill Electronics"; + case 467: + return "Sky Wave Design"; + case 468: + return "Newlab S.r.l."; + case 469: + return "ELAD srl"; + case 470: + return "G-wearables inc."; + case 471: + return "Squadrone Systems Inc."; + case 472: + return "Code Corporation"; + case 473: + return "Savant Systems LLC"; + case 474: + return "Logitech International SA"; + case 475: + return "Innblue Consulting"; + case 476: + return "iParking Ltd."; + case 477: + return "Koninklijke Philips Electronics N.V."; + case 478: + return "Minelab Electronics Pty Limited"; + case 479: + return "Bison Group Ltd."; + case 480: + return "Widex A/S"; + case 481: + return "Jolla Ltd"; + case 482: + return "Lectronix, Inc."; + case 483: + return "Caterpillar Inc"; + case 484: + return "Freedom Innovations"; + case 485: + return "Dynamic Devices Ltd"; + case 486: + return "Technology Solutions (UK) Ltd"; + case 487: + return "IPS Group Inc."; + case 488: + return "STIR"; + case 489: + return "Sano, Inc"; + case 490: + return "Advanced Application Design, Inc."; + case 491: + return "AutoMap LLC"; + case 492: + return "Spreadtrum Communications Shanghai Ltd"; + case 493: + return "CuteCircuit LTD"; + case 494: + return "Valeo Service"; + case 495: + return "Fullpower Technologies, Inc."; + case 496: + return "KloudNation"; + case 497: + return "Zebra Technologies Corporation"; + case 498: + return "Itron, Inc."; + case 499: + return "The University of Tokyo"; + case 500: + return "UTC Fire and Security"; + case 501: + return "Cool Webthings Limited"; + case 502: + return "DJO Global"; + case 503: + return "Gelliner Limited"; + case 504: + return "Anyka (Guangzhou) Microelectronics Technology Co, LTD"; + case 505: + return "Medtronic, Inc."; + case 506: + return "Gozio, Inc."; + case 507: + return "Form Lifting, LLC"; + case 508: + return "Wahoo Fitness, LLC"; + case 509: + return "Kontakt Micro-Location Sp. z o.o."; + case 510: + return "Radio System Corporation"; + case 511: + return "Freescale Semiconductor, Inc."; + case 512: + return "Verifone Systems PTe Ltd. Taiwan Branch"; + case 513: + return "AR Timing"; + case 514: + return "Rigado LLC"; + case 515: + return "Kemppi Oy"; + case 516: + return "Tapcentive Inc."; + case 517: + return "Smartbotics Inc."; + case 518: + return "Otter Products, LLC"; + case 519: + return "STEMP Inc."; + case 520: + return "LumiGeek LLC"; + case 521: + return "InvisionHeart Inc."; + case 522: + return "Macnica Inc."; + case 523: + return "Jaguar Land Rover Limited"; + case 524: + return "CoroWare Technologies, Inc"; + case 525: + return "Simplo Technology Co., LTD"; + case 526: + return "Omron Healthcare Co., LTD"; + case 527: + return "Comodule GMBH"; + case 528: + return "ikeGPS"; + case 529: + return "Telink Semiconductor Co. Ltd"; + case 530: + return "Interplan Co., Ltd"; + case 531: + return "Wyler AG"; + case 532: + return "IK Multimedia Production srl"; + case 533: + return "Lukoton Experience Oy"; + case 534: + return "MTI Ltd"; + case 535: + return "Tech4home, Lda"; + case 536: + return "Hiotech AB"; + case 537: + return "DOTT Limited"; + case 538: + return "Blue Speck Labs, LLC"; + case 539: + return "Cisco Systems Inc"; + case 540: + return "Mobicomm Inc"; + case 541: + return "Edamic"; + case 542: + return "Goodnet Ltd"; + case 543: + return "Luster Leaf Products Inc"; + case 544: + return "Manus Machina BV"; + case 545: + return "Mobiquity Networks Inc"; + case 546: + return "Praxis Dynamics"; + case 547: + return "Philip Morris Products S.A."; + case 548: + return "Comarch SA"; + case 549: + return "Nestl Nespresso S.A."; + case 550: + return "Merlinia A/S"; + case 551: + return "LifeBEAM Technologies"; + case 552: + return "Twocanoes Labs, LLC"; + case 553: + return "Muoverti Limited"; + case 554: + return "Stamer Musikanlagen GMBH"; + case 555: + return "Tesla Motors"; + case 556: + return "Pharynks Corporation"; + case 557: + return "Lupine"; + case 558: + return "Siemens AG"; + case 559: + return "Huami (Shanghai) Culture Communication CO., LTD"; + case 560: + return "Foster Electric Company, Ltd"; + case 561: + return "ETA SA"; + case 562: + return "x-Senso Solutions Kft"; + case 563: + return "Shenzhen SuLong Communication Ltd"; + case 564: + return "FengFan (BeiJing) Technology Co, Ltd"; + case 565: + return "Qrio Inc"; + case 566: + return "Pitpatpet Ltd"; + case 567: + return "MSHeli s.r.l."; + case 568: + return "Trakm8 Ltd"; + case 569: + return "JIN CO, Ltd"; + case 570: + return "Alatech Technology"; + case 571: + return "Beijing CarePulse Electronic Technology Co, Ltd"; + case 572: + return "Awarepoint"; + case 573: + return "ViCentra B.V."; + case 574: + return "Raven Industries"; + case 575: + return "WaveWare Technologies"; + case 576: + return "Argenox Technologies"; + case 577: + return "Bragi GmbH"; + case 578: + return "16Lab Inc"; + case 579: + return "Masimo Corp"; + case 580: + return "Iotera Inc."; + case 581: + return "Endress+Hauser"; + case 582: + return "ACKme Networks, Inc."; + case 583: + return "FiftyThree Inc."; + case 584: + return "Parker Hannifin Corp"; + case 585: + return "Transcranial Ltd"; + case 586: + return "Uwatec AG"; + case 587: + return "Orlan LLC"; + case 588: + return "Blue Clover Devices"; + case 65535: + return "internal use"; + default: + return "not assigned"; + } +} diff --git a/bluetooth.h b/bluetooth.h new file mode 100644 index 0000000..852a6b2 --- /dev/null +++ b/bluetooth.h @@ -0,0 +1,403 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef __BLUETOOTH_H +#define __BLUETOOTH_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include +#include + +#ifndef AF_BLUETOOTH +#define AF_BLUETOOTH 31 +#define PF_BLUETOOTH AF_BLUETOOTH +#endif + +#define BTPROTO_L2CAP 0 +#define BTPROTO_HCI 1 +#define BTPROTO_SCO 2 +#define BTPROTO_RFCOMM 3 +#define BTPROTO_BNEP 4 +#define BTPROTO_CMTP 5 +#define BTPROTO_HIDP 6 +#define BTPROTO_AVDTP 7 + +#define SOL_HCI 0 +#define SOL_L2CAP 6 +#define SOL_SCO 17 +#define SOL_RFCOMM 18 + +#ifndef SOL_BLUETOOTH +#define SOL_BLUETOOTH 274 +#endif + +#define BT_SECURITY 4 +struct bt_security { + uint8_t level; + uint8_t key_size; +}; +#define BT_SECURITY_SDP 0 +#define BT_SECURITY_LOW 1 +#define BT_SECURITY_MEDIUM 2 +#define BT_SECURITY_HIGH 3 + +#define BT_DEFER_SETUP 7 + +#define BT_FLUSHABLE 8 + +#define BT_FLUSHABLE_OFF 0 +#define BT_FLUSHABLE_ON 1 + +#define BT_POWER 9 +struct bt_power { + uint8_t force_active; +}; +#define BT_POWER_FORCE_ACTIVE_OFF 0 +#define BT_POWER_FORCE_ACTIVE_ON 1 + +#define BT_CHANNEL_POLICY 10 + +/* BR/EDR only (default policy) + * AMP controllers cannot be used. + * Channel move requests from the remote device are denied. + * If the L2CAP channel is currently using AMP, move the channel to BR/EDR. + */ +#define BT_CHANNEL_POLICY_BREDR_ONLY 0 + +/* BR/EDR Preferred + * Allow use of AMP controllers. + * If the L2CAP channel is currently on AMP, move it to BR/EDR. + * Channel move requests from the remote device are allowed. + */ +#define BT_CHANNEL_POLICY_BREDR_PREFERRED 1 + +/* AMP Preferred + * Allow use of AMP controllers + * If the L2CAP channel is currently on BR/EDR and AMP controller + * resources are available, initiate a channel move to AMP. + * Channel move requests from the remote device are allowed. + * If the L2CAP socket has not been connected yet, try to create + * and configure the channel directly on an AMP controller rather + * than BR/EDR. + */ +#define BT_CHANNEL_POLICY_AMP_PREFERRED 2 + +#define BT_VOICE 11 +struct bt_voice { + uint16_t setting; +}; + +#define BT_SNDMTU 12 +#define BT_RCVMTU 13 + +#define BT_VOICE_TRANSPARENT 0x0003 +#define BT_VOICE_CVSD_16BIT 0x0060 + +/* Connection and socket states */ +enum { + BT_CONNECTED = 1, /* Equal to TCP_ESTABLISHED to make net code happy */ + BT_OPEN, + BT_BOUND, + BT_LISTEN, + BT_CONNECT, + BT_CONNECT2, + BT_CONFIG, + BT_DISCONN, + BT_CLOSED +}; + +/* Byte order conversions */ +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define htobs(d) (d) +#define htobl(d) (d) +#define htobll(d) (d) +#define btohs(d) (d) +#define btohl(d) (d) +#define btohll(d) (d) +#elif __BYTE_ORDER == __BIG_ENDIAN +#define htobs(d) bswap_16(d) +#define htobl(d) bswap_32(d) +#define htobll(d) bswap_64(d) +#define btohs(d) bswap_16(d) +#define btohl(d) bswap_32(d) +#define btohll(d) bswap_64(d) +#else +#error "Unknown byte order" +#endif + +/* Bluetooth unaligned access */ +#define bt_get_unaligned(ptr) \ +__extension__ ({ \ + struct __attribute__((packed)) { \ + __typeof__(*(ptr)) __v; \ + } *__p = (__typeof__(__p)) (ptr); \ + __p->__v; \ +}) + +#define bt_put_unaligned(val, ptr) \ +do { \ + struct __attribute__((packed)) { \ + __typeof__(*(ptr)) __v; \ + } *__p = (__typeof__(__p)) (ptr); \ + __p->__v = (val); \ +} while(0) + +#if __BYTE_ORDER == __LITTLE_ENDIAN +static inline uint64_t bt_get_le64(const void *ptr) +{ + return bt_get_unaligned((const uint64_t *) ptr); +} + +static inline uint64_t bt_get_be64(const void *ptr) +{ + return bswap_64(bt_get_unaligned((const uint64_t *) ptr)); +} + +static inline uint32_t bt_get_le32(const void *ptr) +{ + return bt_get_unaligned((const uint32_t *) ptr); +} + +static inline uint32_t bt_get_be32(const void *ptr) +{ + return bswap_32(bt_get_unaligned((const uint32_t *) ptr)); +} + +static inline uint16_t bt_get_le16(const void *ptr) +{ + return bt_get_unaligned((const uint16_t *) ptr); +} + +static inline uint16_t bt_get_be16(const void *ptr) +{ + return bswap_16(bt_get_unaligned((const uint16_t *) ptr)); +} + +static inline void bt_put_le64(uint64_t val, const void *ptr) +{ + bt_put_unaligned(val, (uint64_t *) ptr); +} + +static inline void bt_put_be64(uint64_t val, const void *ptr) +{ + bt_put_unaligned(bswap_64(val), (uint64_t *) ptr); +} + +static inline void bt_put_le32(uint32_t val, const void *ptr) +{ + bt_put_unaligned(val, (uint32_t *) ptr); +} + +static inline void bt_put_be32(uint32_t val, const void *ptr) +{ + bt_put_unaligned(bswap_32(val), (uint32_t *) ptr); +} + +static inline void bt_put_le16(uint16_t val, const void *ptr) +{ + bt_put_unaligned(val, (uint16_t *) ptr); +} + +static inline void bt_put_be16(uint16_t val, const void *ptr) +{ + bt_put_unaligned(bswap_16(val), (uint16_t *) ptr); +} + +#elif __BYTE_ORDER == __BIG_ENDIAN +static inline uint64_t bt_get_le64(const void *ptr) +{ + return bswap_64(bt_get_unaligned((const uint64_t *) ptr)); +} + +static inline uint64_t bt_get_be64(const void *ptr) +{ + return bt_get_unaligned((const uint64_t *) ptr); +} + +static inline uint32_t bt_get_le32(const void *ptr) +{ + return bswap_32(bt_get_unaligned((const uint32_t *) ptr)); +} + +static inline uint32_t bt_get_be32(const void *ptr) +{ + return bt_get_unaligned((const uint32_t *) ptr); +} + +static inline uint16_t bt_get_le16(const void *ptr) +{ + return bswap_16(bt_get_unaligned((const uint16_t *) ptr)); +} + +static inline uint16_t bt_get_be16(const void *ptr) +{ + return bt_get_unaligned((const uint16_t *) ptr); +} + +static inline void bt_put_le64(uint64_t val, const void *ptr) +{ + bt_put_unaligned(bswap_64(val), (uint64_t *) ptr); +} + +static inline void bt_put_be64(uint64_t val, const void *ptr) +{ + bt_put_unaligned(val, (uint64_t *) ptr); +} + +static inline void bt_put_le32(uint32_t val, const void *ptr) +{ + bt_put_unaligned(bswap_32(val), (uint32_t *) ptr); +} + +static inline void bt_put_be32(uint32_t val, const void *ptr) +{ + bt_put_unaligned(val, (uint32_t *) ptr); +} + +static inline void bt_put_le16(uint16_t val, const void *ptr) +{ + bt_put_unaligned(bswap_16(val), (uint16_t *) ptr); +} + +static inline void bt_put_be16(uint16_t val, const void *ptr) +{ + bt_put_unaligned(val, (uint16_t *) ptr); +} +#else +#error "Unknown byte order" +#endif + +/* BD Address */ +typedef struct { + uint8_t b[6]; +} __attribute__((packed)) bdaddr_t; + +/* BD Address type */ +#define BDADDR_BREDR 0x00 +#define BDADDR_LE_PUBLIC 0x01 +#define BDADDR_LE_RANDOM 0x02 + +#define BDADDR_ANY (&(bdaddr_t) {{0, 0, 0, 0, 0, 0}}) +#define BDADDR_ALL (&(bdaddr_t) {{0xff, 0xff, 0xff, 0xff, 0xff, 0xff}}) +#define BDADDR_LOCAL (&(bdaddr_t) {{0, 0, 0, 0xff, 0xff, 0xff}}) + +/* Copy, swap, convert BD Address */ +static inline int bacmp(const bdaddr_t *ba1, const bdaddr_t *ba2) +{ + return memcmp(ba1, ba2, sizeof(bdaddr_t)); +} +static inline void bacpy(bdaddr_t *dst, const bdaddr_t *src) +{ + memcpy(dst, src, sizeof(bdaddr_t)); +} + +void baswap(bdaddr_t *dst, const bdaddr_t *src); +bdaddr_t *strtoba(const char *str); +char *batostr(const bdaddr_t *ba); +int ba2str(const bdaddr_t *ba, char *str); +int str2ba(const char *str, bdaddr_t *ba); +int ba2oui(const bdaddr_t *ba, char *oui); +int bachk(const char *str); + +int baprintf(const char *format, ...); +int bafprintf(FILE *stream, const char *format, ...); +int basprintf(char *str, const char *format, ...); +int basnprintf(char *str, size_t size, const char *format, ...); + +void *bt_malloc(size_t size); +void bt_free(void *ptr); + +int bt_error(uint16_t code); +const char *bt_compidtostr(int id); + +typedef struct { + uint8_t data[16]; +} uint128_t; + +static inline void bswap_128(const void *src, void *dst) +{ + const uint8_t *s = (const uint8_t *) src; + uint8_t *d = (uint8_t *) dst; + int i; + + for (i = 0; i < 16; i++) + d[15 - i] = s[i]; +} + +#if __BYTE_ORDER == __BIG_ENDIAN + +#define ntoh64(x) (x) + +static inline void ntoh128(const uint128_t *src, uint128_t *dst) +{ + memcpy(dst, src, sizeof(uint128_t)); +} + +static inline void btoh128(const uint128_t *src, uint128_t *dst) +{ + bswap_128(src, dst); +} + +#else + +static inline uint64_t ntoh64(uint64_t n) +{ + uint64_t h; + uint64_t tmp = ntohl(n & 0x00000000ffffffff); + + h = ntohl(n >> 32); + h |= tmp << 32; + + return h; +} + +static inline void ntoh128(const uint128_t *src, uint128_t *dst) +{ + bswap_128(src, dst); +} + +static inline void btoh128(const uint128_t *src, uint128_t *dst) +{ + memcpy(dst, src, sizeof(uint128_t)); +} + +#endif + +#define hton64(x) ntoh64(x) +#define hton128(x, y) ntoh128(x, y) +#define htob128(x, y) btoh128(x, y) + +#ifdef __cplusplus +} +#endif + +#endif /* __BLUETOOTH_H */ diff --git a/client.c b/client.c new file mode 100644 index 0000000..687c196 --- /dev/null +++ b/client.c @@ -0,0 +1,249 @@ +#include "client.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +/* Helper functions: */ +static void _dummy_connect (client_t* s, int f, const char* b) { (void) s; (void) f; (void) b; } +static void _dummy_recv_data(client_t* s, int f, char* d, int l) { (void) s; (void) f; (void) d; (void) l; } + +static void _change_state(client_t* psClnt, conn_state_t new_state) +{ + require(psClnt != 0); + + psClnt->state = new_state; + psClnt->last_active = time(0); +} + + + +/* + Implementation of exported interface begins here +*/ + +void client_init(client_t* psClnt, char* dst_addr, uint16_t dst_port, char* rxbuf, uint32_t rxbufsize) +{ + require(psClnt != 0); + require(dst_addr != 0); + require(rxbuf != 0); + + strcpy(psClnt->addr, dst_addr); + psClnt->port = dst_port; + psClnt->sockfd = 0; + psClnt->rxbuf = rxbuf; + psClnt->rxbufsz = rxbufsize; + psClnt->client_connected = (void*)_dummy_connect; + psClnt->client_disconnected = (void*)_dummy_connect; + psClnt->client_new_data = (void*)_dummy_recv_data; + + /* + Set the socket I/O mode: In this case FIONBIO + enables or disables the blocking mode for the + socket based on the numerical value of iMode. + If iMode = 0, blocking is enabled; + If iMode != 0, non-blocking mode is enabled. + */ + int iMode = 0; + ioctl(psClnt->sockfd, FIONBIO, &iMode); + + /* + // WINDOWS + DWORD timeout = SOCKET_READ_TIMEOUT_SEC * 1000; + setsockopt(socket, SOL_SOCKET, SO_RCVTIMEO, (const char*)&timeout, sizeof(timeout)); + */ + + _change_state(psClnt, CREATED); + + /* Ignore SIGPIPE - "broken pipe" / disconnected socket */ + signal(SIGPIPE, SIG_IGN); +} + +int client_set_callback(client_t* psClnt, cb_type eTyp, void* funcptr) +{ + require(psClnt != 0); + require(funcptr != 0); + + int success = 1; + + switch(eTyp) + { + case CB_ON_CONNECTION: psClnt->client_connected = funcptr; break; + case CB_ON_DISCONNECT: psClnt->client_disconnected = funcptr; break; + case CB_RECEIVED_DATA: psClnt->client_new_data = funcptr; break; + default: success = 0; /* unknown callback-type */ break; + } + + return success; +} + +int client_send(client_t* psClnt, char* data, uint32_t nbytes) +{ + require(psClnt != 0); + require(data != 0); + + psClnt->last_active = time(0); +// printf("MQTT: sending %u bytes.\n", nbytes); + + int success = send(psClnt->sockfd, data, nbytes, 0); + + if (success < 0) + { + perror("send"); + client_disconnect(psClnt); + } + + return success; +} + +int client_recv(client_t* psClnt, uint32_t timeout_us) +{ + require(psClnt != 0); + + struct timeval tv; + tv.tv_sec = 0; + while (timeout_us >= 1000000) + { + timeout_us -= 1000000; + tv.tv_sec += 1; + } + tv.tv_usec = timeout_us; /* Not init'ing this can cause strange errors */ + setsockopt(psClnt->sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv,sizeof(struct timeval)); + + int nbytes = recv(psClnt->sockfd, psClnt->rxbuf, psClnt->rxbufsz, 0); + if (nbytes <= 0) + { + /* got error or connection closed by server? */ + if ( (nbytes == -1) /* nothing for us */ + || (nbytes == EAGAIN) /* try again later */ + || (nbytes == EWOULDBLOCK)) /* same as above */ + { + /* do nothing */ + } + else + { + /* connection lost / reset by peer */ + perror("recv"); + client_disconnect(psClnt); + } + } + else + { + psClnt->client_new_data(psClnt, psClnt->rxbuf, nbytes); + psClnt->last_active = time(0); + } + return nbytes; +} + +int client_state(client_t* psClnt) +{ + return ((psClnt != 0) ? psClnt->state : 0); +} + +void client_poll(client_t* psClnt, uint32_t timeout_us) +{ + /* 10usec timeout is a reasonable minimum I think */ + timeout_us = ((timeout_us >= 10) ? timeout_us : 10); + + require(psClnt != 0); + + switch (psClnt->state) + { + case CREATED: + { + _change_state(psClnt, DISCONNECTED); + } break; + + case CONNECTED: + { + client_recv(psClnt, timeout_us); +/* + static time_t timeLastMsg = 0; + if ((time(0) - timeLastMsg) > 0) + { + timeLastMsg = time(0); + client_send(psClnt, "hej mor!", 8); + } +*/ + } break; + + case DISCONNECTED: + { + /* ~1 second cooldown */ + if (time(0) > psClnt->last_active) + { + client_connect(psClnt); + } + } break; + } + +} + + +void client_disconnect(client_t* psClnt) +{ + require(psClnt != 0); + + shutdown(psClnt->sockfd, 2); + close(psClnt->sockfd); + + _change_state(psClnt, DISCONNECTED); + psClnt->client_disconnected(psClnt); +} + +int client_connect(client_t* psClnt) +{ + require(psClnt != 0); + + int success = 0; + + struct sockaddr_in serv_addr; + struct hostent* server; + + psClnt->sockfd = socket(AF_INET, SOCK_STREAM, 0); + if (psClnt->sockfd < 0) + { + perror("socket"); + } + else + { + server = gethostbyname(psClnt->addr); + if (server == NULL) + { + fprintf(stderr,"ERROR, no such host\n"); + } + else + { + memset(&serv_addr, 0, sizeof(serv_addr)); + serv_addr.sin_family = AF_INET; + memcpy(&serv_addr.sin_addr.s_addr, server->h_addr, server->h_length); + serv_addr.sin_port = htons(psClnt->port); + + if (connect(psClnt->sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) + { + perror("connect"); + /* Clean up by calling close() on socket before allocating a new socket. */ + client_disconnect(psClnt); + } + else + { + _change_state(psClnt, CONNECTED); + psClnt->client_connected(psClnt); + success = 1; + } + } + } + + return success; +} + diff --git a/client.h b/client.h new file mode 100644 index 0000000..6efd625 --- /dev/null +++ b/client.h @@ -0,0 +1,54 @@ +#include +#include + +#define NCONNECTIONS 1 +#define BUFFER_SIZE_BYTES 8000 + +/* Assertion macro */ +#define require(predicate) assert((predicate)) + + +/* Socket connection state */ +typedef enum +{ + CREATED, + CONNECTED, + DISCONNECTED, +} conn_state_t; + +/* Type definitions */ +typedef enum +{ + CB_ON_CONNECTION, + CB_ON_DISCONNECT, + CB_RECEIVED_DATA, +} cb_type; + +typedef struct +{ + int sockfd; + char* rxbuf; + uint32_t rxbufsz; + conn_state_t state; + uint16_t port; + char addr[32]; + time_t last_active; + + /* Callbacks */ + void (*client_connected) (void* psClnt); /* new connection established */ + void (*client_disconnected)(void* psClnt); /* a connection was closed */ + void (*client_new_data) (void* psClnt, char* data, int nbytes); /* data has been received */ +} client_t; + + + +void client_init(client_t* psClnt, char* dst_addr, uint16_t dst_port, char* rxbuf, uint32_t rxbufsize); +int client_set_callback(client_t* psClnt, cb_type eTyp, void* funcptr); +int client_send(client_t* psClnt, char* data, uint32_t nbytes); +int client_recv(client_t* psClnt, uint32_t timeout_us); +void client_poll(client_t* psClnt, uint32_t timeout_us); +void client_disconnect(client_t* psClnt); +int client_connect(client_t* psClnt); +int client_state(client_t* psClnt); + + diff --git a/config.h b/config.h new file mode 100644 index 0000000..f6838e3 --- /dev/null +++ b/config.h @@ -0,0 +1,133 @@ +/* config.h. Generated from config.h.in by configure. */ +/* config.h.in. Generated from configure.ac by autoheader. */ + +/* Directory for the Android daemon storage files */ +#define ANDROID_STORAGEDIR "/var/lib/bluetooth/android" + +/* Directory for the configuration files */ +#define CONFIGDIR "/usr/local/etc/bluetooth" + +/* Define to 1 if you have the header file. */ +#define HAVE_DLFCN_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_INTTYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_LINUX_IF_ALG_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_LINUX_TYPES_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_MEMORY_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_READLINE_READLINE_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDINT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STDLIB_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRINGS_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_STRING_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_STAT_H 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_SYS_TYPES_H 1 + +/* Define to 1 if you have the udev_hwdb_new() function. */ +#define HAVE_UDEV_HWDB_NEW 1 + +/* Define to 1 if you have the header file. */ +#define HAVE_UNISTD_H 1 + +/* Define to the sub-directory in which libtool stores uninstalled libraries. + */ +#define LT_OBJDIR ".libs/" + +/* Define if threading support is required */ +/* #undef NEED_THREADS */ + +/* Name of package */ +#define PACKAGE "bluez" + +/* Define to the address where bug reports for this package should be sent. */ +#define PACKAGE_BUGREPORT "" + +/* Define to the full name of this package. */ +#define PACKAGE_NAME "bluez" + +/* Define to the full name and version of this package. */ +#define PACKAGE_STRING "bluez 5.31" + +/* Define to the one symbol short name of this package. */ +#define PACKAGE_TARNAME "bluez" + +/* Define to the home page for this package. */ +#define PACKAGE_URL "" + +/* Define to the version of this package. */ +#define PACKAGE_VERSION "5.31" + +/* Define to 1 if you have the ANSI C header files. */ +#define STDC_HEADERS 1 + +/* Directory for the storage files */ +#define STORAGEDIR "/var/lib/bluetooth" + +/* Enable extensions on AIX 3, Interix. */ +#ifndef _ALL_SOURCE +# define _ALL_SOURCE 1 +#endif +/* Enable GNU extensions on systems that have them. */ +#ifndef _GNU_SOURCE +# define _GNU_SOURCE 1 +#endif +/* Enable threading extensions on Solaris. */ +#ifndef _POSIX_PTHREAD_SEMANTICS +# define _POSIX_PTHREAD_SEMANTICS 1 +#endif +/* Enable extensions on HP NonStop. */ +#ifndef _TANDEM_SOURCE +# define _TANDEM_SOURCE 1 +#endif +/* Enable general extensions on Solaris. */ +#ifndef __EXTENSIONS__ +# define __EXTENSIONS__ 1 +#endif + + +/* Version number of package */ +#define VERSION "5.31" + +/* Define to 1 if on MINIX. */ +/* #undef _MINIX */ + +/* Define to 2 if the system does not provide POSIX.1 features except with + this defined. */ +/* #undef _POSIX_1_SOURCE */ + +/* Define to 1 if you need to in order for `stat' and other things to work. */ +/* #undef _POSIX_SOURCE */ + +/* Define to the equivalent of the C99 'restrict' keyword, or to + nothing if this is not supported. Do not define if restrict is + supported directly. */ +#define restrict __restrict +/* Work around a bug in Sun C++: it does not support _Restrict or + __restrict__, even though the corresponding Sun C compiler ends up with + "#define restrict _Restrict" or "#define restrict __restrict__" in the + previous line. Perhaps some future version of Sun C++ will work with + restrict; if so, hopefully it defines __RESTRICT like Sun C does. */ +#if defined __SUNPRO_CC && !defined __RESTRICT +# define _Restrict +# define __restrict__ +#endif diff --git a/crypto.c b/crypto.c new file mode 100644 index 0000000..4886f0c --- /dev/null +++ b/crypto.c @@ -0,0 +1,701 @@ +/** + * @file crypto.c + * @brief crypting gatt messages + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#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; +}; + +/** + * open the pseudo random os generator, returns the associated file descriptor + * + * @return pseudo-random generator associated file descriptor + */ +static int urandom_setup(void) +{ + int fd; + + fd = open("/dev/urandom", O_RDONLY); + if (fd < 0) + return -1; + + return fd; +} + +/** + * + * @return + */ +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/crypto.h b/crypto.h new file mode 100644 index 0000000..9ba5803 --- /dev/null +++ b/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/design_notes.h b/design_notes.h new file mode 100644 index 0000000..e1fcf09 --- /dev/null +++ b/design_notes.h @@ -0,0 +1,112 @@ +/** + * @file design_notes.h + * @brief notes about btgattclient software design + * + * We discuss about various topics concerning btgattclient software design and architecture: + * - epoll: a technic, specific to Linux, to manage in a single loop various i/o events associated with files (incl. standard i/o) and sockets + * - tbc: some more details to come + * + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * + */ +/* + * design_notes.h + * + * Created on: 30 juil. 2015 + * Author: gilbert + */ + +#ifndef DESIGN_NOTES_H_ +#define DESIGN_NOTES_H_ +#include +/** + * controls the epoll instance associated with epfd + * + * @param epfd the epoll context structure + * @param op specifies the operation to be taken against the file associated with fd + * @param fd file descriptor can be a socket + * @param event epool event further constrains the behavior of the operation + * @return 0 success <0 error + */ +int epoll_ctl_doc (int epfd, + int op, + int fd, + struct epoll_event *event); + +/** + * @brief epool event structure 4th parameter of epoll_ctl(...) + */ +struct epoll_event_doc { + /// and EPOLL_EVENT value (might be ored) + int events; + union { + void *ptr; + int fd; + int u32; + long u64; + } data; +}; + +/** + * @brief EPOLL_CTL_XXX possible values + */ +enum EPOLL_CTL_DOC { + /// EPOLL_CTL_ADD: Add a monitor on the file associated with the file descriptor fd to the epoll instance associated with epfd, per the events defined in event. + EPOLL_CTL_ADD_DOC=EPOLL_CTL_ADD, + /// EPOLL_CTL_DEL: Remove a monitor on the file associated with the file descriptor fd from the epoll instance associated with epfd. + EPOLL_CTL_DEL_DOC=EPOLL_CTL_DEL, + /// EPOLL_CTL_MOD: Modify an existing monitor of fd with the updated events specified by event. + EPOLL_CTL_MOD_DOC=EPOLL_CTL_MOD +}; + +/** @brief EPOLL_EVENTS possible values + * + + The events field in the epoll_event structure lists which events to monitor on the given file descriptor. + Multiple events can be bitwise-ORed together. + + Edge- Versus Level-Triggered Events + + If the EPOLLET value is set in the events field of the event parameter passed to epoll_ctl( ), + the watch on fd is edge-triggered, as opposed to level-triggered. + + Consider the following events between a producer and a consumer communicating over a Unix pipe: + + The producer writes 1 KB of data onto a pipe. + + The consumer performs an epoll_wait( ) on the pipe, waiting for the pipe to contain data, and thus be readable. + + With a level-triggered watch, the call to epoll_wait( ) in step 2 will return immediately, + showing that the pipe is ready to read. + + With an edge-triggered watch,this call will not return until after step 1 occurs. + That is, even if the pipe is readable at the invocation of epoll_wait( ), + the call will not return until the data is written onto the pipe. + + Level-triggered is the default behavior. It is how poll( ) and select( ) behave, + and it is what most developers expect. Edge-triggered behavior requires a different approach + to programming, commonly utilizing nonblocking I/O, and careful checking for EAGAIN. + + + */ +enum EPOLL_EVENTS_DOC { + /// EPOLLERR: An error condition occurred on the file. This event is always monitored, even if it's not specified. + EPOLLERR_DOC=EPOLLERR, + /// EPOLLET: Enables edge-triggered behavior for the monitor of the file (see the upcoming section "Edge- Versus Level-Triggered Events"). The default behavior is level-triggered. + EPOLLET_DOC=EPOLLET, + /// EPOLLHUP: A hangup occurred on the file. This event is always monitored, even if it's not specified. + EPOLLHUP_DOC=EPOLLHUP, + /// EPOLLIN: The file is available to be read from without blocking + EPOLLIN_DOC=EPOLLIN, + /// EPOLLONESHOT: After an event is generated and read, the file is automatically no longer monitored. A new event mask must be specified via EPOLL_CTL_MOD to reenable the watch. + EPOLLONESHOT_DOC=EPOLLONESHOT, + /// EPOLLOUT: The file is available to be written to without blocking. + EPOLLOUT_DOC=EPOLLOUT, + /// EPOLLPRI: There is urgent out-of-band data available to read. + EPOLLPRI_DOC=EPOLLPRI +}; + + + +#endif /* DESIGN_NOTES_H_ */ diff --git a/gatt-client.c b/gatt-client.c new file mode 100644 index 0000000..083a933 --- /dev/null +++ b/gatt-client.c @@ -0,0 +1,3069 @@ +/** + * @file gatt-client.c + * @brief gatt client protocol core functions + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 "bluetooth.h" +#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 + +/** + * @brief bluetooth GATT client structure + * + * main data structure to manage the GATT client state + */ +struct bt_gatt_client { + struct bt_att *att; + /**< ATT client structure pointer */ + int ref_count; + /**< count of references */ + 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; + struct queue *long_write_queue; + /**< Queue of long write requests. An error during "prepare write" + * requests can result in a cancel through "execute write". To prevent + * cancellation 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. + */ + bool in_long_write; + + unsigned int reliable_write_session_id; + struct queue *notify_list; + /**< List of registered disconnect/notification/indication callbacks */ + struct queue *notify_chrcs; + int next_reg_id; + unsigned int disc_id; + /**< Handle of the GATT Service + * 0 if not present on the remote peripheral. + */ + unsigned int notify_id; + /**< + * Handle of the GATT Service Changed characteristic + * 0 if not present on the remote peripheral. + */ + unsigned int ind_id; + /**< + * Handles of the GATT indication. + * 0 if 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; + struct queue *pending_requests; + /**< 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. + */ + unsigned int next_request_id; + struct bt_gatt_request *discovery_req; + unsigned int mtu_req_id; +}; + +/** + * @brief Request data structure + * + * Structure to manage request queries + */ +struct request { + struct bt_gatt_client *client; + /**< GATT client context */ + bool long_write; + /**< true if the request is a long write */ + bool prep_write; + /**< true if the request is a preparation write request */ + bool removed; + /**< request still in queue if true */ + int ref_count; + /**< number of references to this data structure */ + unsigned int id; + /**< request id: a unique number incremented by each client request */ + unsigned int att_id; + /**< att message sequence number */ + void *data; + /**< reference to the data exchanged during the request */ + void (*destroy)(void *); + /**< function called when data structure need to be released (ref_count reaches 0) */ +}; + +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_insert_characteristic(op->cur_svc, + chrc_data->value_handle, + &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; + + /* + * check for descriptors presence, before initializing the + * desc_handle and avoid integer overflow during desc_handle + * intialization. + */ + if (chrc_data->value_handle >= chrc_data->end_handle) { + free(chrc_data); + continue; + } + desc_start = chrc_data->value_handle + 1; + + 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_insert_descriptor(op->cur_svc, handle, + &uuid, 0, NULL, NULL, + NULL); + if (!attr) + goto failed; + + if (gatt_db_attribute_get_handle(attr) != handle) + goto failed; + } + +next: + if (!discover_descs(op, &discovering)) + goto failed; + + if (discovering) + return; + + /* 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) { + gatt_db_clear_range(client->db, start, end); + 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 store service"); + success = false; + goto done; + } + } + + /* Skip if service already active */ + if (!gatt_db_service_get_active(attr)) + 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) { + success = true; + 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) { + gatt_db_clear_range(client->db, start, end); + 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; + } + } + + /* Skip if service already active */ + if (!gatt_db_service_get_active(attr)) + queue_push_tail(op->pending_svcs, attr); + } + +secondary: + /* + * Version 4.2 [Vol 1, Part A] page 101: + * A secondary service is a service that provides auxiliary + * functionality of a device and is referenced from at least one + * primary service on the device. + */ + if (queue_isempty(op->pending_svcs)) + goto done; + + /* 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 || client->ready) + return; + + bt_gatt_client_ref(client); + client->ready = success; + 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); + + /* + * BLUETOOTH SPECIFICATION Version 4.2 [Vol 3, Part G] page 546 + * If the Error Response is sent by the server with the Error + * Code set to RequestNot Supported , the Attribute Opcode is + * not supported and the default MTU shall be used. + */ + if (att_ecode == BT_ATT_ERROR_REQUEST_NOT_SUPPORTED) + goto discover; + + 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)); + +discover: + 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 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 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 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; +} + +static unsigned int register_notify(struct bt_gatt_client *client, + uint16_t 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; + + /* Check if a characteristic ref count has been started already */ + chrc = queue_find(client->notify_chrcs, match_notify_chrc_value_handle, + UINT_TO_PTR(handle)); + + if (!chrc) { + /* + * Create an entry if the characteristic is known and has a CCC + * descriptor. + */ + chrc = notify_chrc_create(client, 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; +} + +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_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; + 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 bool register_service_changed(struct bt_gatt_client *client) +{ + bt_uuid_t uuid; + struct gatt_db_attribute *attr = NULL; + + bt_uuid16_create(&uuid, SVC_CHNGD_UUID); + + if (client->svc_chngd_ind_id) + return true; + + gatt_db_find_by_type(client->db, 0x0001, 0xffff, &uuid, + get_first_attribute, &attr); + if (!attr) + return true; + + /* + * Register an indication handler for the "Service Changed" + * characteristic and report ready only if the handler is registered + * successfully. + */ + client->svc_chngd_ind_id = register_notify(client, + gatt_db_attribute_get_handle(attr), + service_changed_register_cb, + service_changed_cb, + client, NULL); + + return client->svc_chngd_ind_id ? true : false; +} + +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; + + 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; + } + + if (register_service_changed(client)) + 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 init_complete(struct discovery_op *op, bool success, + uint8_t att_ecode) +{ + struct bt_gatt_client *client = op->client; + + client->in_init = false; + + if (!success) + goto fail; + + if (register_service_changed(client)) + goto done; + + 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 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); +} + +struct gatt_db *bt_gatt_client_get_db(struct bt_gatt_client *client) +{ + if (!client || !client->db) + return NULL; + + return client->db; +} + +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); +} + +static bool cancel_request(struct request *req) +{ + req->removed = true; + + if (req->long_write) + return cancel_long_write_req(req->client, req); + + if (req->prep_write) + return cancel_prep_write_session(req->client, req); + + return bt_att_cancel(req->client->att, req->att_id); +} + +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; + + return cancel_request(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, + (queue_destroy_func_t) 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); +} + +/** + * + * @param client + * @param value_handle + * @param callback + * @param user_data + * @param destroy + * @return + */ +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_security(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; + + bt_gatt_client_ref(op->client); + + if (op->callback) + op->callback(success, op->reliable_error, att_ecode, + op->user_data); + + start_next_long_write(op->client); + + bt_gatt_client_unref(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; + + bt_gatt_client_ref(op->client); + + if (op->callback) + op->callback(success, reliable_error, att_ecode, op->user_data); + + start_next_long_write(op->client); + + bt_gatt_client_unref(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, 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 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; +} + +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) +{ + if (!client || !client->db || !chrc_value_handle || !callback) + return 0; + + if (!bt_gatt_client_is_ready(client) || client->in_svc_chngd) + return 0; + + return register_notify(client, chrc_value_handle, callback, notify, + user_data, destroy); +} + +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_security(struct bt_gatt_client *client, int level) +{ + if (!client) + return false; + + return bt_att_set_security(client->att, level); +} + +int bt_gatt_client_get_security(struct bt_gatt_client *client) +{ + if (!client) + return -1; + + return bt_att_get_security(client->att); +} diff --git a/gatt-client.h b/gatt-client.h new file mode 100644 index 0000000..befa43f --- /dev/null +++ b/gatt-client.h @@ -0,0 +1,135 @@ +/* + * + * 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); +struct gatt_db *bt_gatt_client_get_db(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, + 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_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_security(struct bt_gatt_client *client, int level); +int bt_gatt_client_get_security(struct bt_gatt_client *client); diff --git a/gatt-db.c b/gatt-db.c new file mode 100644 index 0000000..a0c933c --- /dev/null +++ b/gatt-db.c @@ -0,0 +1,1765 @@ +/** + * @file gatt-db.c + * @brief manage the services / characteristics data model + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 "bluetooth.h" +#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, + uint16_t handle, + 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->handle = handle; + 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, + uint16_t handle, + 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, handle, 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 struct gatt_db_service *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 service; + + if (end >= cur_start && end <= cur_end) + return service; + + if (end < cur_start) + return NULL; + + *after = service; + services_entry = services_entry->next; + } + + return NULL; +} + +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; + + service = find_insert_loc(db, handle, handle + num_handles - 1, &after); + if (service) { + const bt_uuid_t *type; + bt_uuid_t value; + + if (primary) + type = &primary_service_uuid; + else + type = &secondary_service_uuid; + + gatt_db_attribute_get_service_uuid(service->attributes[0], + &value); + + /* Check if service match */ + if (!bt_uuid_cmp(&service->attributes[0]->uuid, type) && + !bt_uuid_cmp(&value, uuid) && + service->num_handles == num_handles && + service->attributes[0]->handle == handle) + return service->attributes[0]; + + return NULL; + } + + service = gatt_db_service_create(uuid, handle, 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; +} + +static struct gatt_db_attribute * +service_insert_characteristic(struct gatt_db_service *service, + uint16_t handle, + 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) +{ + uint8_t value[MAX_CHAR_DECL_VALUE_LEN]; + uint16_t len = 0; + int i; + + /* Check if handle is in within service range */ + if (handle && handle <= service->attributes[0]->handle) + return NULL; + + /* + * It is not possible to allocate last handle for a Characteristic + * since it would not have space for its value: + * 3.3.2 Characteristic Value Declaration + * The Characteristic Value declaration contains the value of the + * characteristic. It is the first Attribute after the characteristic + * declaration. All characteristic definitions shall have a + * Characteristic Value declaration. + */ + if (handle == UINT16_MAX) + return NULL; + + i = get_attribute_index(service, 1); + if (!i) + return NULL; + + if (!handle) + handle = get_handle_at_index(service, i - 1) + 2; + + value[0] = properties; + len += sizeof(properties); + + /* We set handle of characteristic value, which will be added next */ + put_le16(handle, &value[1]); + len += sizeof(uint16_t); + len += uuid_to_le(uuid, &value[3]); + + service->attributes[i] = new_attribute(service, handle - 1, + &characteristic_uuid, + value, len); + if (!service->attributes[i]) + return NULL; + + i++; + + service->attributes[i] = new_attribute(service, handle, 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 service->attributes[i]; +} + +struct gatt_db_attribute * +gatt_db_service_insert_characteristic(struct gatt_db_attribute *attrib, + uint16_t handle, + 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) +{ + if (!attrib || !handle) + return NULL; + + return service_insert_characteristic(attrib->service, handle, uuid, + permissions, properties, + read_func, write_func, + 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) +{ + if (!attrib) + return NULL; + + return service_insert_characteristic(attrib->service, 0, uuid, + permissions, properties, + read_func, write_func, + user_data); +} + +static struct gatt_db_attribute * +service_insert_descriptor(struct gatt_db_service *service, + uint16_t handle, + const bt_uuid_t *uuid, + uint32_t permissions, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + void *user_data) +{ + int i; + + i = get_attribute_index(service, 0); + if (!i) + return NULL; + + /* Check if handle is in within service range */ + if (handle && handle <= service->attributes[0]->handle) + return NULL; + + if (!handle) + handle = get_handle_at_index(service, i - 1) + 1; + + service->attributes[i] = new_attribute(service, handle, uuid, NULL, 0); + if (!service->attributes[i]) + return NULL; + + set_attribute_data(service->attributes[i], read_func, write_func, + permissions, user_data); + + return service->attributes[i]; +} + +struct gatt_db_attribute * +gatt_db_service_insert_descriptor(struct gatt_db_attribute *attrib, + uint16_t handle, + const bt_uuid_t *uuid, + uint32_t permissions, + gatt_db_read_t read_func, + gatt_db_write_t write_func, + void *user_data) +{ + if (!attrib || !handle) + return NULL; + + return service_insert_descriptor(attrib->service, handle, uuid, + permissions, read_func, write_func, + 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) +{ + if (!attrib) + return NULL; + + return service_insert_descriptor(attrib->service, 0, uuid, + permissions, read_func, write_func, + user_data); +} + +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, 0, + &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 */ + for (i = 0; i < service->num_handles; i++) { + if (service->attributes[i] == attrib) { + i += 2; + break; + } + } + + 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; + int i; + + if (!db || !handle) + return NULL; + + service = queue_find(db->services, find_service_for_handle, + UINT_TO_PTR(handle)); + if (!service) + return NULL; + + for (i = 0; i < service->num_handles; i++) { + if (!service->attributes[i]) + continue; + + if (service->attributes[i]->handle == handle) + return service->attributes[i]; + } + + return NULL; +} + +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/gatt-db.h b/gatt-db.h new file mode 100644 index 0000000..96cceb9 --- /dev/null +++ b/gatt-db.h @@ -0,0 +1,236 @@ +/* + * + * 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_insert_characteristic(struct gatt_db_attribute *attrib, + uint16_t handle, + 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_insert_descriptor(struct gatt_db_attribute *attrib, + uint16_t handle, + 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/gatt-helpers.c b/gatt-helpers.c new file mode 100644 index 0000000..0e12f37 --- /dev/null +++ b/gatt-helpers.c @@ -0,0 +1,1580 @@ +/** + * @file gatt-helpers.c + * @brief collection of functions to facilitate gatt implementation + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "queue.h" +#include "att.h" +#include "bluetooth.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 start_handle; + 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; +} + +/** + * + * @param opcode + * @param pdu + * @param length + * @param user_data + */ +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); +} + +/** + * Exchange MTU trigger + * + * \msc + * hscale = "2"; + * "GATT Client", "gatt-helpers.c","att.c" ; + * + * "GATT Client"=>"gatt-helpers.c" [label="bt_gatt_exchange_mtu(...)", URL="\ref bt_gatt_exchange_mtu", ID=1]; + * "gatt-helpers.c"=>"att.c" [label="bt_att_send(...)", URL="\ref bt_att_send", ID=2]; + * "gatt-helpers.c"<<="att.c" [label="mtu_cb(...)", URL="\ref mtu_cb", ID=3]; + * "GATT Client"<<="gatt-helpers.c" [label="callback(...)", URL="\ref exchange_mtu_cb", ID=4]; + * \endmsc + * + *
    + *
  1. \ref bt_gatt_exchange_mtu is called by a GATT Client with the thereafter Parameters building the mtu_op data structure (\ref gatt_client_init in btgattclient.c case) + *
  2. \ref bt_att_send is called with \ref BT_ATT_OP_MTU_REQ op code and passing the mtu_op data structure + *
  3. after receiving the response message \ref mtu_cb is called with the opcode and the mtu_op data structure + *
  4. \ref mtu_cb call the user callback function (\ref exchange_mtu_cb in the btgattclient.c case) + *
+ * + * @param att pointer to bt_att data structure + * @param client_rx_mtu mtu client size + * @param callback user callback to call from mtu_cb (set in mtu_op internal data structure) + * @param user_data pointer for user, set in the mtu_op internal data structure + * @param destroy housekeeping data management function set in the mtu_op internal data structure + * @return sequence number or 0 if error + */ +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) +{ + /* Reset success if there is some result to report */ + if (ecode == BT_ATT_ERROR_ATTRIBUTE_NOT_FOUND && op->result_head) + success = true; + + 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); + 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 handle is lower from previous start handle then it is smth + * wrong. Let's stop search, otherwise we might enter infinite loop. + */ + if (last_end < op->start_handle) { + success = false; + goto done; + } + + op->start_handle = last_end + 1; + + if (last_end < op->end_handle) { + uint8_t pdu[6]; + + put_le16(op->start_handle, 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 = 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); + 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 handle is lower from previous start handle then it is smth + * wrong. Let's stop search, otherwise we might enter infinite loop. + */ + if (last_end < op->start_handle) { + success = false; + goto done; + } + + op->start_handle = last_end + 1; + + if (last_end < op->end_handle) { + uint8_t pdu[6 + get_uuid_len(&op->uuid)]; + + put_le16(op->start_handle, 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 = false; + +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->start_handle = start; + 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); + 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 is lower from previous start handle then it is smth + * wrong. Let's stop search, otherwise we might enter infinite loop. + */ + if (last_handle < op->start_handle) { + success = false; + goto failed; + } + + op->start_handle = last_handle + 1; + if (last_handle != op->end_handle) { + uint8_t pdu[6]; + + put_le16(op->start_handle, 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; + } + + 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->start_handle = start; + 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); + 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 is lower from previous start handle then it is smth + * wrong. Let's stop search, otherwise we might enter infinite loop. + */ + if (last_handle < op->start_handle) { + success = false; + goto done; + } + + op->start_handle = last_handle + 1; + + if (last_handle != op->end_handle) { + uint8_t pdu[6]; + + put_le16(op->start_handle, 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 = 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->start_handle = start; + 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); + 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 is lower from previous start handle then it is smth + * wrong. Let's stop search, otherwise we might enter infinite loop. + */ + if (last_handle < op->start_handle) { + success = false; + goto done; + } + + op->start_handle = last_handle + 1; + + if (last_handle != op->end_handle) { + uint8_t pdu[4 + get_uuid_len(&op->uuid)]; + + put_le16(op->start_handle, 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->start_handle = start; + 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); + 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 is lower from previous start handle then it is smth + * wrong. Let's stop search, otherwise we might enter infinite loop. + */ + if (last_handle < op->start_handle) { + success = false; + goto done; + } + + op->start_handle = last_handle + 1; + + if (last_handle != op->end_handle) { + uint8_t pdu[4]; + + put_le16(op->start_handle, 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; + } + + 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->start_handle = start; + 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/gatt-helpers.h b/gatt-helpers.h new file mode 100644 index 0000000..dd9dd1c --- /dev/null +++ b/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/hci.c b/hci.c new file mode 100644 index 0000000..8b48685 --- /dev/null +++ b/hci.c @@ -0,0 +1,4012 @@ +/** + + * @file hci.c + * @brief hci functions + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "bluetooth.h" +#include "hci.h" +#include "hci_lib.h" + +#ifndef MIN +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#endif + +typedef struct { + char *str; + unsigned int val; +} hci_map; + +/** + * lookup the numeric code val into the hci_map m and return the matching string + * + * @param m pointer to the hci_map + * @param val code to match + * @return string matching the code (val) from the hci_map + */ +static char *hci_bit2str(hci_map *m, unsigned int val) +{ + char *str = malloc(120); + char *ptr = str; + + if (!str) + return NULL; + + *ptr = 0; + while (m->str) { + if ((unsigned int) m->val & val) + ptr += sprintf(ptr, "%s ", m->str); + m++; + } + return str; +} + +/** + * ored bits corresponding to comma separated commands matching the hci_map array + * + * @param map pointer to the hci_map array + * @param str commands, separated by commas for match in the hci_map array + * @param val ored bits (from the hci_map) of maching commands + * + * @return 1 val is set, 0 + */static int hci_str2bit(hci_map *map, char *str, unsigned int *val) +{ + char *t, *ptr; + hci_map *m; + int set; + + if (!str || !(str = ptr = strdup(str))) + return 0; + + *val = set = 0; + + while ((t = strsep(&ptr, ","))) { + for (m = map; m->str; m++) { + if (!strcasecmp(m->str, t)) { + *val |= (unsigned int) m->val; + set = 1; + } + } + } + free(str); + + return set; +} + + /** + * lookup the hci_map and find entry corresponding to val and return the corresponding string + * + * @param m hci_map pointer + * @param val val to match in the hci_map + * @return string mapped to the matching value or NULL if no matching + */ +static char *hci_uint2str(hci_map *m, unsigned int val) +{ + char *str = malloc(50); + char *ptr = str; + + if (!str) + return NULL; + + *ptr = 0; + while (m->str) { + if ((unsigned int) m->val == val) { + ptr += sprintf(ptr, "%s", m->str); + break; + } + m++; + } + return str; +} + +/** + * lookup a command string in the hci_map array and return the corresponding code + * + * @param map pointer to the hci_map array + * @param str command to match + * @param val returned corresponding code + * + * @return 1 val is set 0 if no matching string + */ +static int hci_str2uint(hci_map *map, char *str, unsigned int *val) +{ + char *t, *ptr; + hci_map *m; + int set = 0; + + if (!str) + return 0; + + str = ptr = strdup(str); + + while ((t = strsep(&ptr, ","))) { + for (m = map; m->str; m++) { + if (!strcasecmp(m->str,t)) { + *val = (unsigned int) m->val; + set = 1; + break; + } + } + } + free(str); + + return set; +} + +/** + * return a string corresponding to the bus code (see HCI bus types in hci.h) + * + * @param bus bus code + * @return string corresponding to the bus code (NULL if error) + */ +char *hci_bustostr(int bus) +{ + switch (bus) { + case HCI_VIRTUAL: + return "VIRTUAL"; + case HCI_USB: + return "USB"; + case HCI_PCCARD: + return "PCCARD"; + case HCI_UART: + return "UART"; + case HCI_RS232: + return "RS232"; + case HCI_PCI: + return "PCI"; + case HCI_SDIO: + return "SDIO"; + default: + return "UNKNOWN"; + } +} + +/** + * return the bus string according to the bus code (type) + * + * @param type bus code + * @return bus string (NULL if error) + */ +char *hci_dtypetostr(int type) +{ + return hci_bustostr(type & 0x0f); +} + +/** + * return the bluetooth type string give the code (see HCI controller types in hci.h) + * + * @param type HCI controller types + * @return corresponding string (NULL if error) + */ +char *hci_typetostr(int type) +{ + switch (type) { + case HCI_BREDR: + return "BR/EDR"; + case HCI_AMP: + return "AMP"; + default: + return "UNKNOWN"; + } +} + +/* HCI dev flags mapping */ +static hci_map dev_flags_map[] = { + { "UP", HCI_UP }, + { "INIT", HCI_INIT }, + { "RUNNING", HCI_RUNNING }, + { "RAW", HCI_RAW }, + { "PSCAN", HCI_PSCAN }, + { "ISCAN", HCI_ISCAN }, + { "INQUIRY", HCI_INQUIRY }, + { "AUTH", HCI_AUTH }, + { "ENCRYPT", HCI_ENCRYPT }, + { NULL } +}; + +/** + * return the HCI device flags string given its code + * + * @param flags HCI device flags code + * @return HCI device flags string (NULL if error) + */ +char *hci_dflagstostr(uint32_t flags) +{ + char *str = bt_malloc(50); + char *ptr = str; + hci_map *m = dev_flags_map; + + if (!str) + return NULL; + + *ptr = 0; + + if (!hci_test_bit(HCI_UP, &flags)) + ptr += sprintf(ptr, "DOWN "); + + while (m->str) { + if (hci_test_bit(m->val, &flags)) + ptr += sprintf(ptr, "%s ", m->str); + m++; + } + return str; +} + +/* HCI packet type mapping */ +static hci_map pkt_type_map[] = { + { "DM1", HCI_DM1 }, + { "DM3", HCI_DM3 }, + { "DM5", HCI_DM5 }, + { "DH1", HCI_DH1 }, + { "DH3", HCI_DH3 }, + { "DH5", HCI_DH5 }, + { "HV1", HCI_HV1 }, + { "HV2", HCI_HV2 }, + { "HV3", HCI_HV3 }, + { "2-DH1", HCI_2DH1 }, + { "2-DH3", HCI_2DH3 }, + { "2-DH5", HCI_2DH5 }, + { "3-DH1", HCI_3DH1 }, + { "3-DH3", HCI_3DH3 }, + { "3-DH5", HCI_3DH5 }, + { NULL } +}; + +/* SCO (synchronous connection orientated) links */ +static hci_map sco_ptype_map[] = { + { "HV1", 0x0001 }, + { "HV2", 0x0002 }, + { "HV3", 0x0004 }, + { "EV3", HCI_EV3 }, + { "EV4", HCI_EV4 }, + { "EV5", HCI_EV5 }, + { "2-EV3", HCI_2EV3 }, + { "2-EV5", HCI_2EV5 }, + { "3-EV3", HCI_3EV3 }, + { "3-EV5", HCI_3EV5 }, + { NULL } +}; + +/** + * return the HCI packet type string given the code + * + * @param ptype HCI packet type + * @return HCI packet type string (NULL if error) + */ +char *hci_ptypetostr(unsigned int ptype) +{ + return hci_bit2str(pkt_type_map, ptype); +} + +/** + * return the HCI packet type code given the string + * + * @param str HCI packet type string + * @param val HCI packet type code + * @return 1 if found 0 error + */ +int hci_strtoptype(char *str, unsigned int *val) +{ + return hci_str2bit(pkt_type_map, str, val); +} + +/** + * return the SCO (synchronous connection orientated) links string given the code + * + * @param ptype SCO type code + * @return SCO string (NULL if error) + */ +char *hci_scoptypetostr(unsigned int ptype) +{ + return hci_bit2str(sco_ptype_map, ptype); +} + +/** + * return the SCO (synchronous connection orientated) links code given the string + * + * @param str SCO (synchronous connection orientated) links string + * @param val SCO (synchronous connection orientated) links code + * @return 1 match found 0 error + */ +int hci_strtoscoptype(char *str, unsigned int *val) +{ + return hci_str2bit(sco_ptype_map, str, val); +} + +/* Link policy mapping */ +static hci_map link_policy_map[] = { + { "NONE", 0 }, + { "RSWITCH", HCI_LP_RSWITCH }, + { "HOLD", HCI_LP_HOLD }, + { "SNIFF", HCI_LP_SNIFF }, + { "PARK", HCI_LP_PARK }, + { NULL } +}; + +/** + * return the Link policy mapping string given the code + * + * @param lp Link policy mapping code + * @return Link policy mapping string (NULL if error) + */ +char *hci_lptostr(unsigned int lp) +{ + return hci_bit2str(link_policy_map, lp); +} + +/** + * return Link policy mapping code given the string + * + * @param str Link policy mapping string + * @param val Link policy mapping code + * @return 1 match found 0 error + */ +int hci_strtolp(char *str, unsigned int *val) +{ + return hci_str2bit(link_policy_map, str, val); +} + +/* Link mode mapping */ +static hci_map link_mode_map[] = { + { "NONE", 0 }, + { "ACCEPT", HCI_LM_ACCEPT }, + { "MASTER", HCI_LM_MASTER }, + { "AUTH", HCI_LM_AUTH }, + { "ENCRYPT", HCI_LM_ENCRYPT }, + { "TRUSTED", HCI_LM_TRUSTED }, + { "RELIABLE", HCI_LM_RELIABLE }, + { "SECURE", HCI_LM_SECURE }, + { NULL } +}; + +/** + * return the Link mode mapping string given the code + * + * @param lm Link mode mapping code + * @return Link mode mapping string (NULL if error) + */ +char *hci_lmtostr(unsigned int lm) +{ + char *s, *str = bt_malloc(50); + if (!str) + return NULL; + + *str = 0; + if (!(lm & HCI_LM_MASTER)) + strcpy(str, "SLAVE "); + + s = hci_bit2str(link_mode_map, lm); + if (!s) { + bt_free(str); + return NULL; + } + + strcat(str, s); + free(s); + return str; +} + +/** + * retrun Link mode mapping code given the tring + * + * @param str Link mode mapping string + * @param val Link mode mapping code + * @return 1 match found, 0 error + */ +int hci_strtolm(char *str, unsigned int *val) +{ + return hci_str2bit(link_mode_map, str, val); +} + +/* Command mapping */ +static hci_map commands_map[] = { + { "Inquiry", 0 }, + { "Inquiry Cancel", 1 }, + { "Periodic Inquiry Mode", 2 }, + { "Exit Periodic Inquiry Mode", 3 }, + { "Create Connection", 4 }, + { "Disconnect", 5 }, + { "Add SCO Connection", 6 }, + { "Cancel Create Connection", 7 }, + + { "Accept Connection Request", 8 }, + { "Reject Connection Request", 9 }, + { "Link Key Request Reply", 10 }, + { "Link Key Request Negative Reply", 11 }, + { "PIN Code Request Reply", 12 }, + { "PIN Code Request Negative Reply", 13 }, + { "Change Connection Packet Type", 14 }, + { "Authentication Requested", 15 }, + + { "Set Connection Encryption", 16 }, + { "Change Connection Link Key", 17 }, + { "Master Link Key", 18 }, + { "Remote Name Request", 19 }, + { "Cancel Remote Name Request", 20 }, + { "Read Remote Supported Features", 21 }, + { "Read Remote Extended Features", 22 }, + { "Read Remote Version Information", 23 }, + + { "Read Clock Offset", 24 }, + { "Read LMP Handle", 25 }, + { "Reserved", 26 }, + { "Reserved", 27 }, + { "Reserved", 28 }, + { "Reserved", 29 }, + { "Reserved", 30 }, + { "Reserved", 31 }, + + { "Reserved", 32 }, + { "Hold Mode", 33 }, + { "Sniff Mode", 34 }, + { "Exit Sniff Mode", 35 }, + { "Park State", 36 }, + { "Exit Park State", 37 }, + { "QoS Setup", 38 }, + { "Role Discovery", 39 }, + + { "Switch Role", 40 }, + { "Read Link Policy Settings", 41 }, + { "Write Link Policy Settings", 42 }, + { "Read Default Link Policy Settings", 43 }, + { "Write Default Link Policy Settings", 44 }, + { "Flow Specification", 45 }, + { "Set Event Mask", 46 }, + { "Reset", 47 }, + + { "Set Event Filter", 48 }, + { "Flush", 49 }, + { "Read PIN Type", 50 }, + { "Write PIN Type", 51 }, + { "Create New Unit Key", 52 }, + { "Read Stored Link Key", 53 }, + { "Write Stored Link Key", 54 }, + { "Delete Stored Link Key", 55 }, + + { "Write Local Name", 56 }, + { "Read Local Name", 57 }, + { "Read Connection Accept Timeout", 58 }, + { "Write Connection Accept Timeout", 59 }, + { "Read Page Timeout", 60 }, + { "Write Page Timeout", 61 }, + { "Read Scan Enable", 62 }, + { "Write Scan Enable", 63 }, + + { "Read Page Scan Activity", 64 }, + { "Write Page Scan Activity", 65 }, + { "Read Inquiry Scan Activity", 66 }, + { "Write Inquiry Scan Activity", 67 }, + { "Read Authentication Enable", 68 }, + { "Write Authentication Enable", 69 }, + { "Read Encryption Mode", 70 }, + { "Write Encryption Mode", 71 }, + + { "Read Class Of Device", 72 }, + { "Write Class Of Device", 73 }, + { "Read Voice Setting", 74 }, + { "Write Voice Setting", 75 }, + { "Read Automatic Flush Timeout", 76 }, + { "Write Automatic Flush Timeout", 77 }, + { "Read Num Broadcast Retransmissions", 78 }, + { "Write Num Broadcast Retransmissions", 79 }, + + { "Read Hold Mode Activity", 80 }, + { "Write Hold Mode Activity", 81 }, + { "Read Transmit Power Level", 82 }, + { "Read Synchronous Flow Control Enable", 83 }, + { "Write Synchronous Flow Control Enable", 84 }, + { "Set Host Controller To Host Flow Control", 85 }, + { "Host Buffer Size", 86 }, + { "Host Number Of Completed Packets", 87 }, + + { "Read Link Supervision Timeout", 88 }, + { "Write Link Supervision Timeout", 89 }, + { "Read Number of Supported IAC", 90 }, + { "Read Current IAC LAP", 91 }, + { "Write Current IAC LAP", 92 }, + { "Read Page Scan Period Mode", 93 }, + { "Write Page Scan Period Mode", 94 }, + { "Read Page Scan Mode", 95 }, + + { "Write Page Scan Mode", 96 }, + { "Set AFH Channel Classification", 97 }, + { "Reserved", 98 }, + { "Reserved", 99 }, + { "Read Inquiry Scan Type", 100 }, + { "Write Inquiry Scan Type", 101 }, + { "Read Inquiry Mode", 102 }, + { "Write Inquiry Mode", 103 }, + + { "Read Page Scan Type", 104 }, + { "Write Page Scan Type", 105 }, + { "Read AFH Channel Assessment Mode", 106 }, + { "Write AFH Channel Assessment Mode", 107 }, + { "Reserved", 108 }, + { "Reserved", 109 }, + { "Reserved", 110 }, + { "Reserved", 111 }, + + { "Reserved", 112 }, + { "Reserved", 113 }, + { "Reserved", 114 }, + { "Read Local Version Information", 115 }, + { "Read Local Supported Commands", 116 }, + { "Read Local Supported Features", 117 }, + { "Read Local Extended Features", 118 }, + { "Read Buffer Size", 119 }, + + { "Read Country Code", 120 }, + { "Read BD ADDR", 121 }, + { "Read Failed Contact Counter", 122 }, + { "Reset Failed Contact Counter", 123 }, + { "Get Link Quality", 124 }, + { "Read RSSI", 125 }, + { "Read AFH Channel Map", 126 }, + { "Read BD Clock", 127 }, + + { "Read Loopback Mode", 128 }, + { "Write Loopback Mode", 129 }, + { "Enable Device Under Test Mode", 130 }, + { "Setup Synchronous Connection", 131 }, + { "Accept Synchronous Connection", 132 }, + { "Reject Synchronous Connection", 133 }, + { "Reserved", 134 }, + { "Reserved", 135 }, + + { "Read Extended Inquiry Response", 136 }, + { "Write Extended Inquiry Response", 137 }, + { "Refresh Encryption Key", 138 }, + { "Reserved", 139 }, + { "Sniff Subrating", 140 }, + { "Read Simple Pairing Mode", 141 }, + { "Write Simple Pairing Mode", 142 }, + { "Read Local OOB Data", 143 }, + + { "Read Inquiry Response Transmit Power Level", 144 }, + { "Write Inquiry Transmit Power Level", 145 }, + { "Read Default Erroneous Data Reporting", 146 }, + { "Write Default Erroneous Data Reporting", 147 }, + { "Reserved", 148 }, + { "Reserved", 149 }, + { "Reserved", 150 }, + { "IO Capability Request Reply", 151 }, + + { "User Confirmation Request Reply", 152 }, + { "User Confirmation Request Negative Reply", 153 }, + { "User Passkey Request Reply", 154 }, + { "User Passkey Request Negative Reply", 155 }, + { "Remote OOB Data Request Reply", 156 }, + { "Write Simple Pairing Debug Mode", 157 }, + { "Enhanced Flush", 158 }, + { "Remote OOB Data Request Negative Reply", 159 }, + + { "Reserved", 160 }, + { "Reserved", 161 }, + { "Send Keypress Notification", 162 }, + { "IO Capability Request Negative Reply", 163 }, + { "Read Encryption Key Size", 164 }, + { "Reserved", 165 }, + { "Reserved", 166 }, + { "Reserved", 167 }, + + { "Create Physical Link", 168 }, + { "Accept Physical Link", 169 }, + { "Disconnect Physical Link", 170 }, + { "Create Logical Link", 171 }, + { "Accept Logical Link", 172 }, + { "Disconnect Logical Link", 173 }, + { "Logical Link Cancel", 174 }, + { "Flow Specification Modify", 175 }, + + { "Read Logical Link Accept Timeout", 176 }, + { "Write Logical Link Accept Timeout", 177 }, + { "Set Event Mask Page 2", 178 }, + { "Read Location Data", 179 }, + { "Write Location Data", 180 }, + { "Read Local AMP Info", 181 }, + { "Read Local AMP_ASSOC", 182 }, + { "Write Remote AMP_ASSOC", 183 }, + + { "Read Flow Control Mode", 184 }, + { "Write Flow Control Mode", 185 }, + { "Read Data Block Size", 186 }, + { "Reserved", 187 }, + { "Reserved", 188 }, + { "Enable AMP Receiver Reports", 189 }, + { "AMP Test End", 190 }, + { "AMP Test Command", 191 }, + + { "Read Enhanced Transmit Power Level", 192 }, + { "Reserved", 193 }, + { "Read Best Effort Flush Timeout", 194 }, + { "Write Best Effort Flush Timeout", 195 }, + { "Short Range Mode", 196 }, + { "Read LE Host Support", 197 }, + { "Write LE Host Support", 198 }, + { "Reserved", 199 }, + + { "LE Set Event Mask", 200 }, + { "LE Read Buffer Size", 201 }, + { "LE Read Local Supported Features", 202 }, + { "Reserved", 203 }, + { "LE Set Random Address", 204 }, + { "LE Set Advertising Parameters", 205 }, + { "LE Read Advertising Channel TX Power", 206 }, + { "LE Set Advertising Data", 207 }, + + { "LE Set Scan Response Data", 208 }, + { "LE Set Advertise Enable", 209 }, + { "LE Set Scan Parameters", 210 }, + { "LE Set Scan Enable", 211 }, + { "LE Create Connection", 212 }, + { "LE Create Connection Cancel", 213 }, + { "LE Read White List Size", 214 }, + { "LE Clear White List", 215 }, + + { "LE Add Device To White List", 216 }, + { "LE Remove Device From White List", 217 }, + { "LE Connection Update", 218 }, + { "LE Set Host Channel Classification", 219 }, + { "LE Read Channel Map", 220 }, + { "LE Read Remote Used Features", 221 }, + { "LE Encrypt", 222 }, + { "LE Rand", 223 }, + + { "LE Start Encryption", 224 }, + { "LE Long Term Key Request Reply", 225 }, + { "LE Long Term Key Request Negative Reply", 226 }, + { "LE Read Supported States", 227 }, + { "LE Receiver Test", 228 }, + { "LE Transmitter Test", 229 }, + { "LE Test End", 230 }, + { "Reserved", 231 }, + + { NULL } +}; + +/** + * translate a command code into an english sentence + * + * use the commands_map table which is an array of structures { "english sentence", command code } + * + * @param cmd command code value + * @return english sentence corresponding to the command code (NULL if error) + */ +char *hci_cmdtostr(unsigned int cmd) +{ + return hci_uint2str(commands_map, cmd); +} + +/** + * translate a byte array of bit "commands" into a series of corresponding english strings which is returned + * + * @param commands an array of bytes corresponding to a list of encoded commands + * @param pref prefix to add to str (if pref !=NULL pref will be the first concatenated string) + * @param width + * @return NULL if error else str which is a string of concatenated commands separated by /0 + */ +char *hci_commandstostr(uint8_t *commands, char *pref, int width) +{ + unsigned int maxwidth = width - 3; + hci_map *m; + char *off, *ptr, *str; + int size = 10; + + m = commands_map; + + while (m->str) { + if (commands[m->val / 8] & (1 << (m->val % 8))) + size += strlen(m->str) + (pref ? strlen(pref) : 0) + 3; + m++; + } + + str = bt_malloc(size); + if (!str) + return NULL; + + ptr = str; *ptr = '\0'; + + if (pref) + ptr += sprintf(ptr, "%s", pref); + + off = ptr; + + m = commands_map; + + while (m->str) { + if (commands[m->val / 8] & (1 << (m->val % 8))) { + if (strlen(off) + strlen(m->str) > maxwidth) { + ptr += sprintf(ptr, "\n%s", pref ? pref : ""); + off = ptr; + } + ptr += sprintf(ptr, "'%s' ", m->str); + } + m++; + } + + return str; +} + +/* Version mapping */ +static hci_map ver_map[] = { + { "1.0b", 0x00 }, + { "1.1", 0x01 }, + { "1.2", 0x02 }, + { "2.0", 0x03 }, + { "2.1", 0x04 }, + { "3.0", 0x05 }, + { "4.0", 0x06 }, + { "4.1", 0x07 }, + { "4.2", 0x08 }, + { NULL } +}; + +/** + * translate a version code into a mapping string + * + * @param ver version code + * @return mapping string (NULL if error) + */ +char *hci_vertostr(unsigned int ver) +{ + return hci_uint2str(ver_map, ver); +} + +/** + * return the version code given the string + * + * @param str version string + * @param ver version code + * @return 1 of match found, 0 error + */ +int hci_strtover(char *str, unsigned int *ver) +{ + return hci_str2uint(ver_map, str, ver); +} + +/** + * Link Manager Protocol version (same as hci) string given the code + * + * @param ver version code + * @return mapping string (NULL if error) + */ +char *lmp_vertostr(unsigned int ver) +{ + return hci_uint2str(ver_map, ver); +} + +/** + * Link Manager Protocol code given the string + * + * @param str version string + * @param ver version code + * @return 1 of match found, 0 error + */ +int lmp_strtover(char *str, unsigned int *ver) +{ + return hci_str2uint(ver_map, str, ver); +} + +static hci_map pal_map[] = { + { "3.0", 0x01 }, + { NULL } +}; + +/** + * pal version string given code + * + * @param ver pal version code + * @return pal version string or NULL if error + */ +char *pal_vertostr(unsigned int ver) +{ + return hci_uint2str(pal_map, ver); +} + +/** + * pal version code given string + * + * @param str pal version string + * @param ver pal version code + * @return 1 pal version code is set 0 error + */ +int pal_strtover(char *str, unsigned int *ver) +{ + return hci_str2uint(pal_map, str, ver); +} + +/* LMP features mapping */ +static hci_map lmp_features_map[8][9] = { + { /* Byte 0 */ + { "<3-slot packets>", LMP_3SLOT }, /* Bit 0 */ + { "<5-slot packets>", LMP_5SLOT }, /* Bit 1 */ + { "", LMP_ENCRYPT }, /* Bit 2 */ + { "", LMP_SOFFSET }, /* Bit 3 */ + { "", LMP_TACCURACY }, /* Bit 4 */ + { "", LMP_RSWITCH }, /* Bit 5 */ + { "", LMP_HOLD }, /* Bit 6 */ + { "", LMP_SNIFF }, /* Bit 7 */ + { NULL } + }, + { /* Byte 1 */ + { "", LMP_PARK }, /* Bit 0 */ + { "", LMP_RSSI }, /* Bit 1 */ + { "", LMP_QUALITY }, /* Bit 2 */ + { "", LMP_SCO }, /* Bit 3 */ + { "", LMP_HV2 }, /* Bit 4 */ + { "", LMP_HV3 }, /* Bit 5 */ + { "", LMP_ULAW }, /* Bit 6 */ + { "", LMP_ALAW }, /* Bit 7 */ + { NULL } + }, + { /* Byte 2 */ + { "", LMP_CVSD }, /* Bit 0 */ + { "", LMP_PSCHEME }, /* Bit 1 */ + { "", LMP_PCONTROL }, /* Bit 2 */ + { "", LMP_TRSP_SCO }, /* Bit 3 */ + { "",LMP_BCAST_ENC }, /* Bit 7 */ + { NULL } + }, + { /* Byte 3 */ + { "", 0x01 }, /* Bit 0 */ + { "", LMP_EDR_ACL_2M }, /* Bit 1 */ + { "", LMP_EDR_ACL_3M }, /* Bit 2 */ + { "", LMP_ENH_ISCAN }, /* Bit 3 */ + { "", LMP_ILACE_ISCAN }, /* Bit 4 */ + { "", LMP_ILACE_PSCAN }, /* Bit 5 */ + { "",LMP_RSSI_INQ }, /* Bit 6 */ + { "", LMP_ESCO }, /* Bit 7 */ + { NULL } + }, + { /* Byte 4 */ + { "", LMP_EV4 }, /* Bit 0 */ + { "", LMP_EV5 }, /* Bit 1 */ + { "", 0x04 }, /* Bit 2 */ + { "", LMP_AFH_CAP_SLV }, /* Bit 3 */ + { "", LMP_AFH_CLS_SLV }, /* Bit 4 */ + { "
", LMP_NO_BREDR }, /* Bit 5 */ + { "", LMP_LE }, /* Bit 6 */ + { "<3-slot EDR ACL>", LMP_EDR_3SLOT }, /* Bit 7 */ + { NULL } + }, + { /* Byte 5 */ + { "<5-slot EDR ACL>", LMP_EDR_5SLOT }, /* Bit 0 */ + { "", LMP_SNIFF_SUBR }, /* Bit 1 */ + { "", LMP_PAUSE_ENC }, /* Bit 2 */ + { "", LMP_AFH_CAP_MST }, /* Bit 3 */ + { "",LMP_AFH_CLS_MST }, /* Bit 4 */ + { "", LMP_EDR_ESCO_2M }, /* Bit 5 */ + { "", LMP_EDR_ESCO_3M }, /* Bit 6 */ + { "<3-slot EDR eSCO>", LMP_EDR_3S_ESCO }, /* Bit 7 */ + { NULL } + }, + { /* Byte 6 */ + { "", LMP_EXT_INQ }, /* Bit 0 */ + { "", LMP_LE_BREDR }, /* Bit 1 */ + { "", 0x04 }, /* Bit 2 */ + { "", LMP_SIMPLE_PAIR }, /* Bit 3 */ + { "", LMP_ENCAPS_PDU }, /* Bit 4 */ + { "", LMP_ERR_DAT_REP }, /* Bit 5 */ + { "", LMP_NFLUSH_PKTS }, /* Bit 6 */ + { "", 0x80 }, /* Bit 7 */ + { NULL } + }, + { /* Byte 7 */ + { "", LMP_LSTO }, /* Bit 1 */ + { "", LMP_INQ_TX_PWR }, /* Bit 1 */ + { "", LMP_EPC }, /* Bit 2 */ + { "", 0x08 }, /* Bit 3 */ + { "", 0x10 }, /* Bit 4 */ + { "", 0x20 }, /* Bit 5 */ + { "", 0x40 }, /* Bit 6 */ + { "",LMP_EXT_FEAT }, /* Bit 7 */ + { NULL } + }, +}; + +/** + * Link Manager Protocol feature string given code + * + * @param features Link Manager Protocol feature code + * @param pref prefix + * @param width + * @return Link Manager Protocol string or NULL if error + */ +char *lmp_featurestostr(uint8_t *features, char *pref, int width) +{ + unsigned int maxwidth = width - 1; + char *off, *ptr, *str; + int i, size = 10; + + for (i = 0; i < 8; i++) { + hci_map *m = lmp_features_map[i]; + + while (m->str) { + if (m->val & features[i]) + size += strlen(m->str) + + (pref ? strlen(pref) : 0) + 1; + m++; + } + } + + str = bt_malloc(size); + if (!str) + return NULL; + + ptr = str; *ptr = '\0'; + + if (pref) + ptr += sprintf(ptr, "%s", pref); + + off = ptr; + + for (i = 0; i < 8; i++) { + hci_map *m = lmp_features_map[i]; + + while (m->str) { + if (m->val & features[i]) { + if (strlen(off) + strlen(m->str) > maxwidth) { + ptr += sprintf(ptr, "\n%s", + pref ? pref : ""); + off = ptr; + } + ptr += sprintf(ptr, "%s ", m->str); + } + m++; + } + } + + return str; +} + +/* HCI functions that do not require open device */ + +/** + * return the bt device number + * + * @param flag + * @param func + * @param arg + * @return + */ +int hci_for_each_dev(int flag, int (*func)(int dd, int dev_id, long arg), + long arg) +{ + struct hci_dev_list_req *dl; + struct hci_dev_req *dr; + int dev_id = -1; + int i, sk, err = 0; + + sk = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI); + if (sk < 0) + return -1; + + dl = malloc(HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl)); + if (!dl) { + err = errno; + goto done; + } + + memset(dl, 0, HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl)); + + dl->dev_num = HCI_MAX_DEV; + dr = dl->dev_req; + + if (ioctl(sk, HCIGETDEVLIST, (void *) dl) < 0) { + err = errno; + goto free; + } + + for (i = 0; i < dl->dev_num; i++, dr++) { + if (hci_test_bit(flag, &dr->dev_opt)) + if (!func || func(sk, dr->dev_id, arg)) { + dev_id = dr->dev_id; + break; + } + } + + if (dev_id < 0) + err = ENODEV; + +free: + free(dl); + +done: + close(sk); + errno = err; + + return dev_id; +} + +/** + * + * @param dd + * @param dev_id + * @param arg + * @return + */ +static int __other_bdaddr(int dd, int dev_id, long arg) +{ + struct hci_dev_info di = { .dev_id = dev_id }; + + if (ioctl(dd, HCIGETDEVINFO, (void *) &di)) + return 0; + + if (hci_test_bit(HCI_RAW, &di.flags)) + return 0; + + return bacmp((bdaddr_t *) arg, &di.bdaddr); +} + +/** + * + * @param dd + * @param dev_id + * @param arg + * @return + */ +static int __same_bdaddr(int dd, int dev_id, long arg) +{ + struct hci_dev_info di = { .dev_id = dev_id }; + + if (ioctl(dd, HCIGETDEVINFO, (void *) &di)) + return 0; + + return !bacmp((bdaddr_t *) arg, &di.bdaddr); +} + +/** + * + * + * @param bdaddr + * @return + */ +int hci_get_route(bdaddr_t *bdaddr) +{ + int dev_id; + + dev_id = hci_for_each_dev(HCI_UP, __other_bdaddr, + (long) (bdaddr ? bdaddr : BDADDR_ANY)); + if (dev_id < 0) + dev_id = hci_for_each_dev(HCI_UP, __same_bdaddr, + (long) (bdaddr ? bdaddr : BDADDR_ANY)); + + return dev_id; +} + +/** + * return the bt device number + * + * @param str string should be hci + * @return bt device number (-1 if error) + */ +int hci_devid(const char *str) +{ + bdaddr_t ba; + int id = -1; + + if (!strncmp(str, "hci", 3) && strlen(str) >= 4) { + id = atoi(str + 3); + if (hci_devba(id, &ba) < 0) + return -1; + } else { + errno = ENODEV; + str2ba(str, &ba); + id = hci_for_each_dev(HCI_UP, __same_bdaddr, (long) &ba); + } + + return id; +} + +/** + * get device info given device id + * + * @param dev_id device id + * @param di device info structure if ok + * @return 0 info structure set else error + */ +int hci_devinfo(int dev_id, struct hci_dev_info *di) +{ + int dd, err, ret; + + dd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI); + if (dd < 0) + return dd; + + memset(di, 0, sizeof(struct hci_dev_info)); + + di->dev_id = dev_id; + ret = ioctl(dd, HCIGETDEVINFO, (void *) di); + + err = errno; + close(dd); + errno = err; + + return ret; +} + +/** + * + * + * @param dev_id + * @param bdaddr + * @return 0 = OK, -1 = Error + */ +int hci_devba(int dev_id, bdaddr_t *bdaddr) +{ + struct hci_dev_info di; + + memset(&di, 0, sizeof(di)); + + if (hci_devinfo(dev_id, &di)) + return -1; + + if (!hci_test_bit(HCI_UP, &di.flags)) { + errno = ENETDOWN; + return -1; + } + + bacpy(bdaddr, &di.bdaddr); + + return 0; +} + +/** + * + * + * @param dev_id + * @param len + * @param nrsp + * @param lap + * @param ii + * @param flags + * @return + */ +int hci_inquiry(int dev_id, int len, int nrsp, const uint8_t *lap, + inquiry_info **ii, long flags) +{ + struct hci_inquiry_req *ir; + uint8_t num_rsp = nrsp; + void *buf; + int dd, size, err, ret = -1; + + if (nrsp <= 0) { + num_rsp = 0; + nrsp = 255; + } + + if (dev_id < 0) { + dev_id = hci_get_route(NULL); + if (dev_id < 0) { + errno = ENODEV; + return -1; + } + } + + dd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI); + if (dd < 0) + return dd; + + buf = malloc(sizeof(*ir) + (sizeof(inquiry_info) * (nrsp))); + if (!buf) + goto done; + + ir = buf; + ir->dev_id = dev_id; + ir->num_rsp = num_rsp; + ir->length = len; + ir->flags = flags; + + if (lap) { + memcpy(ir->lap, lap, 3); + } else { + ir->lap[0] = 0x33; + ir->lap[1] = 0x8b; + ir->lap[2] = 0x9e; + } + + ret = ioctl(dd, HCIINQUIRY, (unsigned long) buf); + if (ret < 0) + goto free; + + size = sizeof(inquiry_info) * ir->num_rsp; + + if (!*ii) + *ii = malloc(size); + + if (*ii) { + memcpy((void *) *ii, buf + sizeof(*ir), size); + ret = ir->num_rsp; + } else + ret = -1; + +free: + free(buf); + +done: + err = errno; + close(dd); + errno = err; + + return ret; +} + +/** + * Open HCI device. + * + * @param dev_id integer device address + * @return device descriptor (dd): a bluetooth socket + */ +int hci_open_dev(int dev_id) +{ + struct sockaddr_hci a; + int dd, err; + + /* Check for valid device id */ + if (dev_id < 0) { + errno = ENODEV; + return -1; + } + + /* Create HCI socket */ + dd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI); + if (dd < 0) + return dd; + + /* Bind socket to the HCI device */ + memset(&a, 0, sizeof(a)); + a.hci_family = AF_BLUETOOTH; + a.hci_dev = dev_id; + if (bind(dd, (struct sockaddr *) &a, sizeof(a)) < 0) + goto failed; + + return dd; + +failed: + err = errno; + close(dd); + errno = err; + + return -1; +} + +/** + * Close the device + * + * @param dd Device descriptor returned by hci_open_dev. + * @return 0 if OK or -1 if failure + */ +int hci_close_dev(int dd) +{ + return close(dd); +} + +/* HCI functions that require open device */ + +/** + * + * + * @param dd Device descriptor returned by hci_open_dev. + * @param ogf + * @param ocf + * @param plen + * @param param + * @return + */ +int hci_send_cmd(int dd, uint16_t ogf, uint16_t ocf, uint8_t plen, void *param) +{ + uint8_t type = HCI_COMMAND_PKT; + hci_command_hdr hc; + struct iovec iv[3]; + int ivn; + + hc.opcode = htobs(cmd_opcode_pack(ogf, ocf)); + hc.plen= plen; + + iv[0].iov_base = &type; + iv[0].iov_len = 1; + iv[1].iov_base = &hc; + iv[1].iov_len = HCI_COMMAND_HDR_SIZE; + ivn = 2; + + if (plen) { + iv[2].iov_base = param; + iv[2].iov_len = plen; + ivn = 3; + } + + while (writev(dd, iv, ivn) < 0) { + if (errno == EAGAIN || errno == EINTR) + continue; + return -1; + } + return 0; +} + +/** + * + * + * @param dd + * @param r + * @param to + * @return + */ +int hci_send_req(int dd, struct hci_request *r, int to) +{ + unsigned char buf[HCI_MAX_EVENT_SIZE], *ptr; + uint16_t opcode = htobs(cmd_opcode_pack(r->ogf, r->ocf)); + struct hci_filter nf, of; + socklen_t olen; + hci_event_hdr *hdr; + int err, try; + + olen = sizeof(of); + if (getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &olen) < 0) + return -1; + + hci_filter_clear(&nf); + hci_filter_set_ptype(HCI_EVENT_PKT, &nf); + hci_filter_set_event(EVT_CMD_STATUS, &nf); + hci_filter_set_event(EVT_CMD_COMPLETE, &nf); + hci_filter_set_event(EVT_LE_META_EVENT, &nf); + hci_filter_set_event(r->event, &nf); + hci_filter_set_opcode(opcode, &nf); + if (setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf)) < 0) + return -1; + + if (hci_send_cmd(dd, r->ogf, r->ocf, r->clen, r->cparam) < 0) + goto failed; + + try = 10; + while (try--) { + evt_cmd_complete *cc; + evt_cmd_status *cs; + evt_remote_name_req_complete *rn; + evt_le_meta_event *me; + remote_name_req_cp *cp; + int len; + + if (to) { + struct pollfd p; + int n; + + p.fd = dd; p.events = POLLIN; + while ((n = poll(&p, 1, to)) < 0) { + if (errno == EAGAIN || errno == EINTR) + continue; + goto failed; + } + + if (!n) { + errno = ETIMEDOUT; + goto failed; + } + + to -= 10; + if (to < 0) + to = 0; + + } + + while ((len = read(dd, buf, sizeof(buf))) < 0) { + if (errno == EAGAIN || errno == EINTR) + continue; + goto failed; + } + + hdr = (void *) (buf + 1); + ptr = buf + (1 + HCI_EVENT_HDR_SIZE); + len -= (1 + HCI_EVENT_HDR_SIZE); + + switch (hdr->evt) { + case EVT_CMD_STATUS: + cs = (void *) ptr; + + if (cs->opcode != opcode) + continue; + + if (r->event != EVT_CMD_STATUS) { + if (cs->status) { + errno = EIO; + goto failed; + } + break; + } + + r->rlen = MIN(len, r->rlen); + memcpy(r->rparam, ptr, r->rlen); + goto done; + + case EVT_CMD_COMPLETE: + cc = (void *) ptr; + + if (cc->opcode != opcode) + continue; + + ptr += EVT_CMD_COMPLETE_SIZE; + len -= EVT_CMD_COMPLETE_SIZE; + + r->rlen = MIN(len, r->rlen); + memcpy(r->rparam, ptr, r->rlen); + goto done; + + case EVT_REMOTE_NAME_REQ_COMPLETE: + if (hdr->evt != r->event) + break; + + rn = (void *) ptr; + cp = r->cparam; + + if (bacmp(&rn->bdaddr, &cp->bdaddr)) + continue; + + r->rlen = MIN(len, r->rlen); + memcpy(r->rparam, ptr, r->rlen); + goto done; + + case EVT_LE_META_EVENT: + me = (void *) ptr; + + if (me->subevent != r->event) + continue; + + len -= 1; + r->rlen = MIN(len, r->rlen); + memcpy(r->rparam, me->data, r->rlen); + goto done; + + default: + if (hdr->evt != r->event) + break; + + r->rlen = MIN(len, r->rlen); + memcpy(r->rparam, ptr, r->rlen); + goto done; + } + } + errno = ETIMEDOUT; + +failed: + err = errno; + setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of)); + errno = err; + return -1; + +done: + setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of)); + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param ptype + * @param clkoffset + * @param rswitch + * @param handle + * @param to + * @return + */ +int hci_create_connection(int dd, const bdaddr_t *bdaddr, uint16_t ptype, + uint16_t clkoffset, uint8_t rswitch, + uint16_t *handle, int to) +{ + evt_conn_complete rp; + create_conn_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, bdaddr); + cp.pkt_type = ptype; + cp.pscan_rep_mode = 0x02; + cp.clock_offset = clkoffset; + cp.role_switch = rswitch; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_CREATE_CONN; + rq.event = EVT_CONN_COMPLETE; + rq.cparam = &cp; + rq.clen = CREATE_CONN_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_CONN_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *handle = rp.handle; + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param reason + * @param to + * @return + */ +int hci_disconnect(int dd, uint16_t handle, uint8_t reason, int to) +{ + evt_disconn_complete rp; + disconnect_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.reason = reason; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_DISCONNECT; + rq.event = EVT_DISCONN_COMPLETE; + rq.cparam = &cp; + rq.clen = DISCONNECT_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_DISCONN_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param type + * @param to + * @return + */ +int hci_le_add_white_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to) +{ + struct hci_request rq; + le_add_device_to_white_list_cp cp; + uint8_t status; + + memset(&cp, 0, sizeof(cp)); + cp.bdaddr_type = type; + bacpy(&cp.bdaddr, bdaddr); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_ADD_DEVICE_TO_WHITE_LIST; + rq.cparam = &cp; + rq.clen = LE_ADD_DEVICE_TO_WHITE_LIST_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param type + * @param to + * @return + */ +int hci_le_rm_white_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to) +{ + struct hci_request rq; + le_remove_device_from_white_list_cp cp; + uint8_t status; + + memset(&cp, 0, sizeof(cp)); + cp.bdaddr_type = type; + bacpy(&cp.bdaddr, bdaddr); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST; + rq.cparam = &cp; + rq.clen = LE_REMOVE_DEVICE_FROM_WHITE_LIST_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param size + * @param to + * @return + */ +int hci_le_read_white_list_size(int dd, uint8_t *size, int to) +{ + struct hci_request rq; + le_read_white_list_size_rp rp; + + memset(&rp, 0, sizeof(rp)); + memset(&rq, 0, sizeof(rq)); + + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_READ_WHITE_LIST_SIZE; + rq.rparam = &rp; + rq.rlen = LE_READ_WHITE_LIST_SIZE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (size) + *size = rp.size; + + return 0; +} + +/** + * + * + * @param dd + * @param to + * @return + */ +int hci_le_clear_white_list(int dd, int to) +{ + struct hci_request rq; + uint8_t status; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_CLEAR_WHITE_LIST; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param type + * @param peer_irk + * @param local_irk + * @param to + * @return + */ +int hci_le_add_resolving_list(int dd, const bdaddr_t *bdaddr, uint8_t type, + uint8_t *peer_irk, uint8_t *local_irk, int to) +{ + struct hci_request rq; + le_add_device_to_resolv_list_cp cp; + uint8_t status; + + memset(&cp, 0, sizeof(cp)); + cp.bdaddr_type = type; + bacpy(&cp.bdaddr, bdaddr); + if (peer_irk) + memcpy(cp.peer_irk, peer_irk, 16); + if (local_irk) + memcpy(cp.local_irk, local_irk, 16); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_ADD_DEVICE_TO_RESOLV_LIST; + rq.cparam = &cp; + rq.clen = LE_ADD_DEVICE_TO_RESOLV_LIST_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param type + * @param to + * @return + */ +int hci_le_rm_resolving_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to) +{ + struct hci_request rq; + le_remove_device_from_resolv_list_cp cp; + uint8_t status; + + memset(&cp, 0, sizeof(cp)); + cp.bdaddr_type = type; + bacpy(&cp.bdaddr, bdaddr); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_REMOVE_DEVICE_FROM_RESOLV_LIST; + rq.cparam = &cp; + rq.clen = LE_REMOVE_DEVICE_FROM_RESOLV_LIST_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param to + * @return + */ +int hci_le_clear_resolving_list(int dd, int to) +{ + struct hci_request rq; + uint8_t status; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_CLEAR_RESOLV_LIST; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param size + * @param to + * @return + */ +int hci_le_read_resolving_list_size(int dd, uint8_t *size, int to) +{ + struct hci_request rq; + le_read_resolv_list_size_rp rp; + + memset(&rp, 0, sizeof(rp)); + memset(&rq, 0, sizeof(rq)); + + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_READ_RESOLV_LIST_SIZE; + rq.rparam = &rp; + rq.rlen = LE_READ_RESOLV_LIST_SIZE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (size) + *size = rp.size; + + return 0; +} + +/** + * + * + * @param dd + * @param enable + * @param to + * @return + */ +int hci_le_set_address_resolution_enable(int dd, uint8_t enable, int to) +{ + struct hci_request rq; + le_set_address_resolution_enable_cp cp; + uint8_t status; + + memset(&cp, 0, sizeof(cp)); + cp.enable = enable; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_SET_ADDRESS_RESOLUTION_ENABLE; + rq.cparam = &cp; + rq.clen = LE_SET_ADDRESS_RESOLUTION_ENABLE_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * @param dd + * @param len + * @param name + * @param to + * @return + */ +int hci_read_local_name(int dd, int len, char *name, int to) +{ + read_local_name_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_LOCAL_NAME; + rq.rparam = &rp; + rq.rlen = READ_LOCAL_NAME_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + rp.name[247] = '\0'; + strncpy(name, (char *) rp.name, len); + return 0; +} + +/** + * + * + * @param dd + * @param name + * @param to + * @return + */ +int hci_write_local_name(int dd, const char *name, int to) +{ + change_local_name_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + strncpy((char *) cp.name, name, sizeof(cp.name)); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_CHANGE_LOCAL_NAME; + rq.cparam = &cp; + rq.clen = CHANGE_LOCAL_NAME_CP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param pscan_rep_mode + * @param clkoffset + * @param len + * @param name + * @param to + * @return + */ +int hci_read_remote_name_with_clock_offset(int dd, const bdaddr_t *bdaddr, + uint8_t pscan_rep_mode, + uint16_t clkoffset, + int len, char *name, int to) +{ + evt_remote_name_req_complete rn; + remote_name_req_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, bdaddr); + cp.pscan_rep_mode = pscan_rep_mode; + cp.clock_offset = clkoffset; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_REMOTE_NAME_REQ; + rq.cparam = &cp; + rq.clen = REMOTE_NAME_REQ_CP_SIZE; + rq.event = EVT_REMOTE_NAME_REQ_COMPLETE; + rq.rparam = &rn; + rq.rlen = EVT_REMOTE_NAME_REQ_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rn.status) { + errno = EIO; + return -1; + } + + rn.name[247] = '\0'; + strncpy(name, (char *) rn.name, len); + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param len + * @param name + * @param to + * @return + */ +int hci_read_remote_name(int dd, const bdaddr_t *bdaddr, int len, char *name, + int to) +{ + return hci_read_remote_name_with_clock_offset(dd, bdaddr, 0x02, 0x0000, + len, name, to); +} + +/** + * + * + * @param dd + * @param bdaddr + * @param to + * @return + */ +int hci_read_remote_name_cancel(int dd, const bdaddr_t *bdaddr, int to) +{ + remote_name_req_cancel_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, bdaddr); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_REMOTE_NAME_REQ_CANCEL; + rq.cparam = &cp; + rq.clen = REMOTE_NAME_REQ_CANCEL_CP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param ver + * @param to + * @return + */ +int hci_read_remote_version(int dd, uint16_t handle, struct hci_version *ver, + int to) +{ + evt_read_remote_version_complete rp; + read_remote_version_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_READ_REMOTE_VERSION; + rq.event = EVT_READ_REMOTE_VERSION_COMPLETE; + rq.cparam = &cp; + rq.clen = READ_REMOTE_VERSION_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_READ_REMOTE_VERSION_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + ver->manufacturer = btohs(rp.manufacturer); + ver->lmp_ver = rp.lmp_ver; + ver->lmp_subver = btohs(rp.lmp_subver); + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param features + * @param to + * @return + */ +int hci_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to) +{ + evt_read_remote_features_complete rp; + read_remote_features_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_READ_REMOTE_FEATURES; + rq.event = EVT_READ_REMOTE_FEATURES_COMPLETE; + rq.cparam = &cp; + rq.clen = READ_REMOTE_FEATURES_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (features) + memcpy(features, rp.features, 8); + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param page + * @param max_page + * @param features + * @param to + * @return + */ +int hci_read_remote_ext_features(int dd, uint16_t handle, uint8_t page, + uint8_t *max_page, uint8_t *features, + int to) +{ + evt_read_remote_ext_features_complete rp; + read_remote_ext_features_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.page_num = page; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_READ_REMOTE_EXT_FEATURES; + rq.event = EVT_READ_REMOTE_EXT_FEATURES_COMPLETE; + rq.cparam = &cp; + rq.clen = READ_REMOTE_EXT_FEATURES_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (max_page) + *max_page = rp.max_page_num; + + if (features) + memcpy(features, rp.features, 8); + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param clkoffset + * @param to + * @return + */ +int hci_read_clock_offset(int dd, uint16_t handle, uint16_t *clkoffset, int to) +{ + evt_read_clock_offset_complete rp; + read_clock_offset_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_READ_CLOCK_OFFSET; + rq.event = EVT_READ_CLOCK_OFFSET_COMPLETE; + rq.cparam = &cp; + rq.clen = READ_CLOCK_OFFSET_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *clkoffset = rp.clock_offset; + return 0; +} + +/** + * + * + * @param dd + * @param ver + * @param to + * @return + */ +int hci_read_local_version(int dd, struct hci_version *ver, int to) +{ + read_local_version_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_INFO_PARAM; + rq.ocf = OCF_READ_LOCAL_VERSION; + rq.rparam = &rp; + rq.rlen = READ_LOCAL_VERSION_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + ver->manufacturer = btohs(rp.manufacturer); + ver->hci_ver = rp.hci_ver; + ver->hci_rev = btohs(rp.hci_rev); + ver->lmp_ver = rp.lmp_ver; + ver->lmp_subver = btohs(rp.lmp_subver); + return 0; +} + +/** + * + * + * @param dd + * @param commands + * @param to + * @return + */ +int hci_read_local_commands(int dd, uint8_t *commands, int to) +{ + read_local_commands_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_INFO_PARAM; + rq.ocf = OCF_READ_LOCAL_COMMANDS; + rq.rparam = &rp; + rq.rlen = READ_LOCAL_COMMANDS_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (commands) + memcpy(commands, rp.commands, 64); + + return 0; +} + +/** + * + * + * @param dd + * @param features + * @param to + * @return + */ +int hci_read_local_features(int dd, uint8_t *features, int to) +{ + read_local_features_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_INFO_PARAM; + rq.ocf = OCF_READ_LOCAL_FEATURES; + rq.rparam = &rp; + rq.rlen = READ_LOCAL_FEATURES_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (features) + memcpy(features, rp.features, 8); + + return 0; +} + +/** + * + * + * @param dd + * @param page + * @param max_page + * @param features + * @param to + * @return + */ +int hci_read_local_ext_features(int dd, uint8_t page, uint8_t *max_page, + uint8_t *features, int to) +{ + read_local_ext_features_cp cp; + read_local_ext_features_rp rp; + struct hci_request rq; + + cp.page_num = page; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_INFO_PARAM; + rq.ocf = OCF_READ_LOCAL_EXT_FEATURES; + rq.cparam = &cp; + rq.clen = READ_LOCAL_EXT_FEATURES_CP_SIZE; + rq.rparam = &rp; + rq.rlen = READ_LOCAL_EXT_FEATURES_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (max_page) + *max_page = rp.max_page_num; + + if (features) + memcpy(features, rp.features, 8); + + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param to + * @return + */ +int hci_read_bd_addr(int dd, bdaddr_t *bdaddr, int to) +{ + read_bd_addr_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_INFO_PARAM; + rq.ocf = OCF_READ_BD_ADDR; + rq.rparam = &rp; + rq.rlen = READ_BD_ADDR_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (bdaddr) + bacpy(bdaddr, &rp.bdaddr); + + return 0; +} + +/** + * + * + * @param dd + * @param cls + * @param to + * @return + */ +int hci_read_class_of_dev(int dd, uint8_t *cls, int to) +{ + read_class_of_dev_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_CLASS_OF_DEV; + rq.rparam = &rp; + rq.rlen = READ_CLASS_OF_DEV_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + memcpy(cls, rp.dev_class, 3); + return 0; +} + +/** + * + * + * @param dd + * @param cls + * @param to + * @return + */ +int hci_write_class_of_dev(int dd, uint32_t cls, int to) +{ + write_class_of_dev_cp cp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + cp.dev_class[0] = cls & 0xff; + cp.dev_class[1] = (cls >> 8) & 0xff; + cp.dev_class[2] = (cls >> 16) & 0xff; + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_CLASS_OF_DEV; + rq.cparam = &cp; + rq.clen = WRITE_CLASS_OF_DEV_CP_SIZE; + return hci_send_req(dd, &rq, to); +} + +/** + * + * + * @param dd + * @param vs + * @param to + * @return + */ +int hci_read_voice_setting(int dd, uint16_t *vs, int to) +{ + read_voice_setting_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_VOICE_SETTING; + rq.rparam = &rp; + rq.rlen = READ_VOICE_SETTING_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *vs = rp.voice_setting; + return 0; +} + +/** + * + * + * @param dd + * @param vs + * @param to + * @return + */ +int hci_write_voice_setting(int dd, uint16_t vs, int to) +{ + write_voice_setting_cp cp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + cp.voice_setting = vs; + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_VOICE_SETTING; + rq.cparam = &cp; + rq.clen = WRITE_VOICE_SETTING_CP_SIZE; + + return hci_send_req(dd, &rq, to); +} + +/** + * + * + * @param dd + * @param num_iac + * @param lap + * @param to + * @return + */ +int hci_read_current_iac_lap(int dd, uint8_t *num_iac, uint8_t *lap, int to) +{ + read_current_iac_lap_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_CURRENT_IAC_LAP; + rq.rparam = &rp; + rq.rlen = READ_CURRENT_IAC_LAP_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *num_iac = rp.num_current_iac; + memcpy(lap, rp.lap, rp.num_current_iac * 3); + return 0; +} + +/** + * + * + * @param dd + * @param num_iac + * @param lap + * @param to + * @return + */ +int hci_write_current_iac_lap(int dd, uint8_t num_iac, uint8_t *lap, int to) +{ + write_current_iac_lap_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.num_current_iac = num_iac; + memcpy(&cp.lap, lap, num_iac * 3); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_CURRENT_IAC_LAP; + rq.cparam = &cp; + rq.clen = num_iac * 3 + 1; + + return hci_send_req(dd, &rq, to); +} + +/** + * + * + * @param dd + * @param bdaddr + * @param all + * @param to + * @return + */ +int hci_read_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t all, int to) +{ + read_stored_link_key_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, bdaddr); + cp.read_all = all; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_STORED_LINK_KEY; + rq.cparam = &cp; + rq.clen = READ_STORED_LINK_KEY_CP_SIZE; + + return hci_send_req(dd, &rq, to); +} + +/** + * + * + * @param dd + * @param bdaddr + * @param key + * @param to + * @return + */ +int hci_write_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t *key, int to) +{ + unsigned char cp[WRITE_STORED_LINK_KEY_CP_SIZE + 6 + 16]; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp[0] = 1; + bacpy((bdaddr_t *) (cp + 1), bdaddr); + memcpy(cp + 7, key, 16); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_STORED_LINK_KEY; + rq.cparam = &cp; + rq.clen = WRITE_STORED_LINK_KEY_CP_SIZE + 6 + 16; + + return hci_send_req(dd, &rq, to); +} + +/** + * + * + * @param dd + * @param bdaddr + * @param all + * @param to + * @return + */ +int hci_delete_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t all, int to) +{ + delete_stored_link_key_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, bdaddr); + cp.delete_all = all; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_DELETE_STORED_LINK_KEY; + rq.cparam = &cp; + rq.clen = DELETE_STORED_LINK_KEY_CP_SIZE; + + return hci_send_req(dd, &rq, to); +} + +/** + * + * + * @param dd + * @param handle + * @param to + * @return + */ +int hci_authenticate_link(int dd, uint16_t handle, int to) +{ + auth_requested_cp cp; + evt_auth_complete rp; + struct hci_request rq; + + cp.handle = handle; + + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_AUTH_REQUESTED; + rq.event = EVT_AUTH_COMPLETE; + rq.cparam = &cp; + rq.clen = AUTH_REQUESTED_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_AUTH_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param encrypt + * @param to + * @return + */ +int hci_encrypt_link(int dd, uint16_t handle, uint8_t encrypt, int to) +{ + set_conn_encrypt_cp cp; + evt_encrypt_change rp; + struct hci_request rq; + + cp.handle = handle; + cp.encrypt = encrypt; + + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_SET_CONN_ENCRYPT; + rq.event = EVT_ENCRYPT_CHANGE; + rq.cparam = &cp; + rq.clen = SET_CONN_ENCRYPT_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_ENCRYPT_CHANGE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param to + * @return + */ +int hci_change_link_key(int dd, uint16_t handle, int to) +{ + change_conn_link_key_cp cp; + evt_change_conn_link_key_complete rp; + struct hci_request rq; + + cp.handle = handle; + + rq.ogf = OGF_LINK_CTL; + rq.ocf = OCF_CHANGE_CONN_LINK_KEY; + rq.event = EVT_CHANGE_CONN_LINK_KEY_COMPLETE; + rq.cparam = &cp; + rq.clen = CHANGE_CONN_LINK_KEY_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_CHANGE_CONN_LINK_KEY_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param bdaddr + * @param role + * @param to + * @return + */ +int hci_switch_role(int dd, bdaddr_t *bdaddr, uint8_t role, int to) +{ + switch_role_cp cp; + evt_role_change rp; + struct hci_request rq; + + bacpy(&cp.bdaddr, bdaddr); + cp.role = role; + rq.ogf = OGF_LINK_POLICY; + rq.ocf = OCF_SWITCH_ROLE; + rq.cparam = &cp; + rq.clen = SWITCH_ROLE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_ROLE_CHANGE_SIZE; + rq.event = EVT_ROLE_CHANGE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param max_interval + * @param min_interval + * @param to + * @return + */ +int hci_park_mode(int dd, uint16_t handle, uint16_t max_interval, + uint16_t min_interval, int to) +{ + park_mode_cp cp; + evt_mode_change rp; + struct hci_request rq; + + memset(&cp, 0, sizeof (cp)); + cp.handle = handle; + cp.max_interval = max_interval; + cp.min_interval = min_interval; + + memset(&rq, 0, sizeof (rq)); + rq.ogf = OGF_LINK_POLICY; + rq.ocf = OCF_PARK_MODE; + rq.event = EVT_MODE_CHANGE; + rq.cparam = &cp; + rq.clen = PARK_MODE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_MODE_CHANGE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param to + * @return + */ +int hci_exit_park_mode(int dd, uint16_t handle, int to) +{ + exit_park_mode_cp cp; + evt_mode_change rp; + struct hci_request rq; + + memset(&cp, 0, sizeof (cp)); + cp.handle = handle; + + memset (&rq, 0, sizeof (rq)); + rq.ogf = OGF_LINK_POLICY; + rq.ocf = OCF_EXIT_PARK_MODE; + rq.event = EVT_MODE_CHANGE; + rq.cparam = &cp; + rq.clen = EXIT_PARK_MODE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_MODE_CHANGE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param type + * @param to + * @return + */ +int hci_read_inquiry_scan_type(int dd, uint8_t *type, int to) +{ + read_inquiry_scan_type_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_INQUIRY_SCAN_TYPE; + rq.rparam = &rp; + rq.rlen = READ_INQUIRY_SCAN_TYPE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *type = rp.type; + return 0; +} + +/** + * + * + * @param dd + * @param type + * @param to + * @return + */ +int hci_write_inquiry_scan_type(int dd, uint8_t type, int to) +{ + write_inquiry_scan_type_cp cp; + write_inquiry_scan_type_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.type = type; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_INQUIRY_SCAN_TYPE; + rq.cparam = &cp; + rq.clen = WRITE_INQUIRY_SCAN_TYPE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_INQUIRY_SCAN_TYPE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param mode + * @param to + * @return + */ +int hci_read_inquiry_mode(int dd, uint8_t *mode, int to) +{ + read_inquiry_mode_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_INQUIRY_MODE; + rq.rparam = &rp; + rq.rlen = READ_INQUIRY_MODE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *mode = rp.mode; + return 0; +} + +/** + * + * + * @param dd + * @param mode + * @param to + * @return + */ +int hci_write_inquiry_mode(int dd, uint8_t mode, int to) +{ + write_inquiry_mode_cp cp; + write_inquiry_mode_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.mode = mode; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_INQUIRY_MODE; + rq.cparam = &cp; + rq.clen = WRITE_INQUIRY_MODE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_INQUIRY_MODE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param mode + * @param to + * @return + */ +int hci_read_afh_mode(int dd, uint8_t *mode, int to) +{ + read_afh_mode_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_AFH_MODE; + rq.rparam = &rp; + rq.rlen = READ_AFH_MODE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *mode = rp.mode; + return 0; +} + +/** + * + * + * @param dd + * @param mode + * @param to + * @return + */ +int hci_write_afh_mode(int dd, uint8_t mode, int to) +{ + write_afh_mode_cp cp; + write_afh_mode_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.mode = mode; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_AFH_MODE; + rq.cparam = &cp; + rq.clen = WRITE_AFH_MODE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_AFH_MODE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param fec + * @param data + * @param to + * @return + */ +int hci_read_ext_inquiry_response(int dd, uint8_t *fec, uint8_t *data, int to) +{ + read_ext_inquiry_response_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_EXT_INQUIRY_RESPONSE; + rq.rparam = &rp; + rq.rlen = READ_EXT_INQUIRY_RESPONSE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *fec = rp.fec; + memcpy(data, rp.data, HCI_MAX_EIR_LENGTH); + + return 0; +} + +/** + * + * + * @param dd + * @param fec + * @param data + * @param to + * @return + */ +int hci_write_ext_inquiry_response(int dd, uint8_t fec, uint8_t *data, int to) +{ + write_ext_inquiry_response_cp cp; + write_ext_inquiry_response_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.fec = fec; + memcpy(cp.data, data, HCI_MAX_EIR_LENGTH); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_EXT_INQUIRY_RESPONSE; + rq.cparam = &cp; + rq.clen = WRITE_EXT_INQUIRY_RESPONSE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_EXT_INQUIRY_RESPONSE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param mode + * @param to + * @return + */ +int hci_read_simple_pairing_mode(int dd, uint8_t *mode, int to) +{ + read_simple_pairing_mode_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_SIMPLE_PAIRING_MODE; + rq.rparam = &rp; + rq.rlen = READ_SIMPLE_PAIRING_MODE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *mode = rp.mode; + return 0; +} + +/** + * + * + * @param dd + * @param mode + * @param to + * @return + */ +int hci_write_simple_pairing_mode(int dd, uint8_t mode, int to) +{ + write_simple_pairing_mode_cp cp; + write_simple_pairing_mode_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.mode = mode; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_SIMPLE_PAIRING_MODE; + rq.cparam = &cp; + rq.clen = WRITE_SIMPLE_PAIRING_MODE_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_SIMPLE_PAIRING_MODE_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param hash + * @param randomizer + * @param to + * @return + */ +int hci_read_local_oob_data(int dd, uint8_t *hash, uint8_t *randomizer, int to) +{ + read_local_oob_data_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_LOCAL_OOB_DATA; + rq.rparam = &rp; + rq.rlen = READ_LOCAL_OOB_DATA_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + memcpy(hash, rp.hash, 16); + memcpy(randomizer, rp.randomizer, 16); + return 0; +} + +/** + * + * + * @param dd + * @param level + * @param to + * @return + */ +int hci_read_inq_response_tx_power_level(int dd, int8_t *level, int to) +{ + read_inq_response_tx_power_level_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_INQ_RESPONSE_TX_POWER_LEVEL; + rq.rparam = &rp; + rq.rlen = READ_INQ_RESPONSE_TX_POWER_LEVEL_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *level = rp.level; + return 0; +} + +/** + * + * + * @param dd + * @param level + * @param to + * @return + */ +int hci_read_inquiry_transmit_power_level(int dd, int8_t *level, int to) +{ + return hci_read_inq_response_tx_power_level(dd, level, to); +} + +/** + * + * + * @param dd + * @param level + * @param to + * @return + */ +int hci_write_inquiry_transmit_power_level(int dd, int8_t level, int to) +{ + write_inquiry_transmit_power_level_cp cp; + write_inquiry_transmit_power_level_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.level = level; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_INQUIRY_TRANSMIT_POWER_LEVEL; + rq.cparam = &cp; + rq.clen = WRITE_INQUIRY_TRANSMIT_POWER_LEVEL_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_INQUIRY_TRANSMIT_POWER_LEVEL_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param type + * @param level + * @param to + * @return + */ +int hci_read_transmit_power_level(int dd, uint16_t handle, uint8_t type, + int8_t *level, int to) +{ + read_transmit_power_level_cp cp; + read_transmit_power_level_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.type = type; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_TRANSMIT_POWER_LEVEL; + rq.cparam = &cp; + rq.clen = READ_TRANSMIT_POWER_LEVEL_CP_SIZE; + rq.rparam = &rp; + rq.rlen = READ_TRANSMIT_POWER_LEVEL_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *level = rp.level; + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param policy + * @param to + * @return + */ +int hci_read_link_policy(int dd, uint16_t handle, uint16_t *policy, int to) +{ + read_link_policy_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_POLICY; + rq.ocf = OCF_READ_LINK_POLICY; + rq.cparam = &handle; + rq.clen = 2; + rq.rparam = &rp; + rq.rlen = READ_LINK_POLICY_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *policy = rp.policy; + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param policy + * @param to + * @return + */ +int hci_write_link_policy(int dd, uint16_t handle, uint16_t policy, int to) +{ + write_link_policy_cp cp; + write_link_policy_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.policy = policy; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LINK_POLICY; + rq.ocf = OCF_WRITE_LINK_POLICY; + rq.cparam = &cp; + rq.clen = WRITE_LINK_POLICY_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_LINK_POLICY_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param timeout + * @param to + * @return + */ +int hci_read_link_supervision_timeout(int dd, uint16_t handle, + uint16_t *timeout, int to) +{ + read_link_supervision_timeout_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_READ_LINK_SUPERVISION_TIMEOUT; + rq.cparam = &handle; + rq.clen = 2; + rq.rparam = &rp; + rq.rlen = READ_LINK_SUPERVISION_TIMEOUT_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *timeout = rp.timeout; + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param timeout + * @param to + * @return + */ +int hci_write_link_supervision_timeout(int dd, uint16_t handle, + uint16_t timeout, int to) +{ + write_link_supervision_timeout_cp cp; + write_link_supervision_timeout_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.timeout = timeout; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_WRITE_LINK_SUPERVISION_TIMEOUT; + rq.cparam = &cp; + rq.clen = WRITE_LINK_SUPERVISION_TIMEOUT_CP_SIZE; + rq.rparam = &rp; + rq.rlen = WRITE_LINK_SUPERVISION_TIMEOUT_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param map + * @param to + * @return + */ +int hci_set_afh_classification(int dd, uint8_t *map, int to) +{ + set_afh_classification_cp cp; + set_afh_classification_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + memcpy(cp.map, map, 10); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_HOST_CTL; + rq.ocf = OCF_SET_AFH_CLASSIFICATION; + rq.cparam = &cp; + rq.clen = SET_AFH_CLASSIFICATION_CP_SIZE; + rq.rparam = &rp; + rq.rlen = SET_AFH_CLASSIFICATION_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param link_quality + * @param to + * @return + */ +int hci_read_link_quality(int dd, uint16_t handle, uint8_t *link_quality, + int to) +{ + read_link_quality_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_STATUS_PARAM; + rq.ocf = OCF_READ_LINK_QUALITY; + rq.cparam = &handle; + rq.clen = 2; + rq.rparam = &rp; + rq.rlen = READ_LINK_QUALITY_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *link_quality = rp.link_quality; + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param rssi + * @param to + * @return + */ +int hci_read_rssi(int dd, uint16_t handle, int8_t *rssi, int to) +{ + read_rssi_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_STATUS_PARAM; + rq.ocf = OCF_READ_RSSI; + rq.cparam = &handle; + rq.clen = 2; + rq.rparam = &rp; + rq.rlen = READ_RSSI_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *rssi = rp.rssi; + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param mode + * @param map + * @param to + * @return + */ +int hci_read_afh_map(int dd, uint16_t handle, uint8_t *mode, uint8_t *map, + int to) +{ + read_afh_map_rp rp; + struct hci_request rq; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_STATUS_PARAM; + rq.ocf = OCF_READ_AFH_MAP; + rq.cparam = &handle; + rq.clen = 2; + rq.rparam = &rp; + rq.rlen = READ_AFH_MAP_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *mode = rp.mode; + memcpy(map, rp.map, 10); + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param which + * @param clock + * @param accuracy + * @param to + * @return + */ +int hci_read_clock(int dd, uint16_t handle, uint8_t which, uint32_t *clock, + uint16_t *accuracy, int to) +{ + read_clock_cp cp; + read_clock_rp rp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.which_clock = which; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_STATUS_PARAM; + rq.ocf = OCF_READ_CLOCK; + rq.cparam = &cp; + rq.clen = READ_CLOCK_CP_SIZE; + rq.rparam = &rp; + rq.rlen = READ_CLOCK_RP_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + *clock = rp.clock; + *accuracy = rp.accuracy; + return 0; +} + +/** + * + * + * @param dd + * @param enable + * @param filter_dup + * @param to + * @return + */ +int hci_le_set_scan_enable(int dd, uint8_t enable, uint8_t filter_dup, int to) +{ + struct hci_request rq; + le_set_scan_enable_cp scan_cp; + uint8_t status; + + memset(&scan_cp, 0, sizeof(scan_cp)); + scan_cp.enable = enable; + scan_cp.filter_dup = filter_dup; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_SET_SCAN_ENABLE; + rq.cparam = &scan_cp; + rq.clen = LE_SET_SCAN_ENABLE_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param type + * @param interval + * @param window + * @param own_type + * @param filter + * @param to + * @return + */ +int hci_le_set_scan_parameters(int dd, uint8_t type, + uint16_t interval, uint16_t window, + uint8_t own_type, uint8_t filter, int to) +{ + struct hci_request rq; + le_set_scan_parameters_cp param_cp; + uint8_t status; + + memset(¶m_cp, 0, sizeof(param_cp)); + param_cp.type = type; + param_cp.interval = interval; + param_cp.window = window; + param_cp.own_bdaddr_type = own_type; + param_cp.filter = filter; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_SET_SCAN_PARAMETERS; + rq.cparam = ¶m_cp; + rq.clen = LE_SET_SCAN_PARAMETERS_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param enable + * @param to + * @return + */ +int hci_le_set_advertise_enable(int dd, uint8_t enable, int to) +{ + struct hci_request rq; + le_set_advertise_enable_cp adv_cp; + uint8_t status; + + memset(&adv_cp, 0, sizeof(adv_cp)); + adv_cp.enable = enable; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_SET_ADVERTISE_ENABLE; + rq.cparam = &adv_cp; + rq.clen = LE_SET_ADVERTISE_ENABLE_CP_SIZE; + rq.rparam = &status; + rq.rlen = 1; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param interval + * @param window + * @param initiator_filter + * @param peer_bdaddr_type + * @param peer_bdaddr + * @param own_bdaddr_type + * @param min_interval + * @param max_interval + * @param latency + * @param supervision_timeout + * @param min_ce_length + * @param max_ce_length + * @param handle + * @param to + * @return + */ +int hci_le_create_conn(int dd, uint16_t interval, uint16_t window, + uint8_t initiator_filter, uint8_t peer_bdaddr_type, + bdaddr_t peer_bdaddr, uint8_t own_bdaddr_type, + uint16_t min_interval, uint16_t max_interval, + uint16_t latency, uint16_t supervision_timeout, + uint16_t min_ce_length, uint16_t max_ce_length, + uint16_t *handle, int to) +{ + struct hci_request rq; + le_create_connection_cp create_conn_cp; + evt_le_connection_complete conn_complete_rp; + + memset(&create_conn_cp, 0, sizeof(create_conn_cp)); + create_conn_cp.interval = interval; + create_conn_cp.window = window; + create_conn_cp.initiator_filter = initiator_filter; + create_conn_cp.peer_bdaddr_type = peer_bdaddr_type; + create_conn_cp.peer_bdaddr = peer_bdaddr; + create_conn_cp.own_bdaddr_type = own_bdaddr_type; + create_conn_cp.min_interval = min_interval; + create_conn_cp.max_interval = max_interval; + create_conn_cp.latency = latency; + create_conn_cp.supervision_timeout = supervision_timeout; + create_conn_cp.min_ce_length = min_ce_length; + create_conn_cp.max_ce_length = max_ce_length; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_CREATE_CONN; + rq.event = EVT_LE_CONN_COMPLETE; + rq.cparam = &create_conn_cp; + rq.clen = LE_CREATE_CONN_CP_SIZE; + rq.rparam = &conn_complete_rp; + rq.rlen = EVT_CONN_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (conn_complete_rp.status) { + errno = EIO; + return -1; + } + + if (handle) + *handle = conn_complete_rp.handle; + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param min_interval + * @param max_interval + * @param latency + * @param supervision_timeout + * @param to + * @return + */ +int hci_le_conn_update(int dd, uint16_t handle, uint16_t min_interval, + uint16_t max_interval, uint16_t latency, + uint16_t supervision_timeout, int to) +{ + evt_le_connection_update_complete evt; + le_connection_update_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + cp.min_interval = min_interval; + cp.max_interval = max_interval; + cp.latency = latency; + cp.supervision_timeout = supervision_timeout; + cp.min_ce_length = htobs(0x0001); + cp.max_ce_length = htobs(0x0001); + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_CONN_UPDATE; + rq.cparam = &cp; + rq.clen = LE_CONN_UPDATE_CP_SIZE; + rq.event = EVT_LE_CONN_UPDATE_COMPLETE; + rq.rparam = &evt; + rq.rlen = sizeof(evt); + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (evt.status) { + errno = EIO; + return -1; + } + + return 0; +} + +/** + * + * + * @param dd + * @param handle + * @param features + * @param to + * @return + */ +int hci_le_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to) +{ + evt_le_read_remote_used_features_complete rp; + le_read_remote_used_features_cp cp; + struct hci_request rq; + + memset(&cp, 0, sizeof(cp)); + cp.handle = handle; + + memset(&rq, 0, sizeof(rq)); + rq.ogf = OGF_LE_CTL; + rq.ocf = OCF_LE_READ_REMOTE_USED_FEATURES; + rq.event = EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE; + rq.cparam = &cp; + rq.clen = LE_READ_REMOTE_USED_FEATURES_CP_SIZE; + rq.rparam = &rp; + rq.rlen = EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE_SIZE; + + if (hci_send_req(dd, &rq, to) < 0) + return -1; + + if (rp.status) { + errno = EIO; + return -1; + } + + if (features) + memcpy(features, rp.features, 8); + + return 0; +} diff --git a/hci.h b/hci.h new file mode 100644 index 0000000..4ce77f7 --- /dev/null +++ b/hci.h @@ -0,0 +1,2449 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef __HCI_H +#define __HCI_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#define HCI_MAX_DEV 16 + +#define HCI_MAX_ACL_SIZE (1492 + 4) +#define HCI_MAX_SCO_SIZE 255 +#define HCI_MAX_EVENT_SIZE 260 +#define HCI_MAX_FRAME_SIZE (HCI_MAX_ACL_SIZE + 4) + +/* HCI dev events */ +#define HCI_DEV_REG 1 +#define HCI_DEV_UNREG 2 +#define HCI_DEV_UP 3 +#define HCI_DEV_DOWN 4 +#define HCI_DEV_SUSPEND 5 +#define HCI_DEV_RESUME 6 + +/* HCI bus types */ +#define HCI_VIRTUAL 0 +#define HCI_USB 1 +#define HCI_PCCARD 2 +#define HCI_UART 3 +#define HCI_RS232 4 +#define HCI_PCI 5 +#define HCI_SDIO 6 + +/* HCI controller types */ +#define HCI_BREDR 0x00 +#define HCI_AMP 0x01 + +/* HCI device flags */ +enum { + HCI_UP, + HCI_INIT, + HCI_RUNNING, + + HCI_PSCAN, + HCI_ISCAN, + HCI_AUTH, + HCI_ENCRYPT, + HCI_INQUIRY, + + HCI_RAW, +}; + +/* LE address type */ +enum { + LE_PUBLIC_ADDRESS = 0x00, + LE_RANDOM_ADDRESS = 0x01 +}; + +/* HCI ioctl defines */ +#define HCIDEVUP _IOW('H', 201, int) +#define HCIDEVDOWN _IOW('H', 202, int) +#define HCIDEVRESET _IOW('H', 203, int) +#define HCIDEVRESTAT _IOW('H', 204, int) + +#define HCIGETDEVLIST _IOR('H', 210, int) +#define HCIGETDEVINFO _IOR('H', 211, int) +#define HCIGETCONNLIST _IOR('H', 212, int) +#define HCIGETCONNINFO _IOR('H', 213, int) +#define HCIGETAUTHINFO _IOR('H', 215, int) + +#define HCISETRAW _IOW('H', 220, int) +#define HCISETSCAN _IOW('H', 221, int) +#define HCISETAUTH _IOW('H', 222, int) +#define HCISETENCRYPT _IOW('H', 223, int) +#define HCISETPTYPE _IOW('H', 224, int) +#define HCISETLINKPOL _IOW('H', 225, int) +#define HCISETLINKMODE _IOW('H', 226, int) +#define HCISETACLMTU _IOW('H', 227, int) +#define HCISETSCOMTU _IOW('H', 228, int) + +#define HCIBLOCKADDR _IOW('H', 230, int) +#define HCIUNBLOCKADDR _IOW('H', 231, int) + +#define HCIINQUIRY _IOR('H', 240, int) + +#ifndef __NO_HCI_DEFS + +/* HCI Packet types */ +#define HCI_COMMAND_PKT 0x01 +#define HCI_ACLDATA_PKT 0x02 +#define HCI_SCODATA_PKT 0x03 +#define HCI_EVENT_PKT 0x04 +#define HCI_VENDOR_PKT 0xff + +/* HCI Packet types */ +#define HCI_2DH1 0x0002 +#define HCI_3DH1 0x0004 +#define HCI_DM1 0x0008 +#define HCI_DH1 0x0010 +#define HCI_2DH3 0x0100 +#define HCI_3DH3 0x0200 +#define HCI_DM3 0x0400 +#define HCI_DH3 0x0800 +#define HCI_2DH5 0x1000 +#define HCI_3DH5 0x2000 +#define HCI_DM5 0x4000 +#define HCI_DH5 0x8000 + +#define HCI_HV1 0x0020 +#define HCI_HV2 0x0040 +#define HCI_HV3 0x0080 + +#define HCI_EV3 0x0008 +#define HCI_EV4 0x0010 +#define HCI_EV5 0x0020 +#define HCI_2EV3 0x0040 +#define HCI_3EV3 0x0080 +#define HCI_2EV5 0x0100 +#define HCI_3EV5 0x0200 + +#define SCO_PTYPE_MASK (HCI_HV1 | HCI_HV2 | HCI_HV3) +#define ACL_PTYPE_MASK (HCI_DM1 | HCI_DH1 | HCI_DM3 | HCI_DH3 | HCI_DM5 | HCI_DH5) + +/* HCI Error codes */ +#define HCI_UNKNOWN_COMMAND 0x01 +#define HCI_NO_CONNECTION 0x02 +#define HCI_HARDWARE_FAILURE 0x03 +#define HCI_PAGE_TIMEOUT 0x04 +#define HCI_AUTHENTICATION_FAILURE 0x05 +#define HCI_PIN_OR_KEY_MISSING 0x06 +#define HCI_MEMORY_FULL 0x07 +#define HCI_CONNECTION_TIMEOUT 0x08 +#define HCI_MAX_NUMBER_OF_CONNECTIONS 0x09 +#define HCI_MAX_NUMBER_OF_SCO_CONNECTIONS 0x0a +#define HCI_ACL_CONNECTION_EXISTS 0x0b +#define HCI_COMMAND_DISALLOWED 0x0c +#define HCI_REJECTED_LIMITED_RESOURCES 0x0d +#define HCI_REJECTED_SECURITY 0x0e +#define HCI_REJECTED_PERSONAL 0x0f +#define HCI_HOST_TIMEOUT 0x10 +#define HCI_UNSUPPORTED_FEATURE 0x11 +#define HCI_INVALID_PARAMETERS 0x12 +#define HCI_OE_USER_ENDED_CONNECTION 0x13 +#define HCI_OE_LOW_RESOURCES 0x14 +#define HCI_OE_POWER_OFF 0x15 +#define HCI_CONNECTION_TERMINATED 0x16 +#define HCI_REPEATED_ATTEMPTS 0x17 +#define HCI_PAIRING_NOT_ALLOWED 0x18 +#define HCI_UNKNOWN_LMP_PDU 0x19 +#define HCI_UNSUPPORTED_REMOTE_FEATURE 0x1a +#define HCI_SCO_OFFSET_REJECTED 0x1b +#define HCI_SCO_INTERVAL_REJECTED 0x1c +#define HCI_AIR_MODE_REJECTED 0x1d +#define HCI_INVALID_LMP_PARAMETERS 0x1e +#define HCI_UNSPECIFIED_ERROR 0x1f +#define HCI_UNSUPPORTED_LMP_PARAMETER_VALUE 0x20 +#define HCI_ROLE_CHANGE_NOT_ALLOWED 0x21 +#define HCI_LMP_RESPONSE_TIMEOUT 0x22 +#define HCI_LMP_ERROR_TRANSACTION_COLLISION 0x23 +#define HCI_LMP_PDU_NOT_ALLOWED 0x24 +#define HCI_ENCRYPTION_MODE_NOT_ACCEPTED 0x25 +#define HCI_UNIT_LINK_KEY_USED 0x26 +#define HCI_QOS_NOT_SUPPORTED 0x27 +#define HCI_INSTANT_PASSED 0x28 +#define HCI_PAIRING_NOT_SUPPORTED 0x29 +#define HCI_TRANSACTION_COLLISION 0x2a +#define HCI_QOS_UNACCEPTABLE_PARAMETER 0x2c +#define HCI_QOS_REJECTED 0x2d +#define HCI_CLASSIFICATION_NOT_SUPPORTED 0x2e +#define HCI_INSUFFICIENT_SECURITY 0x2f +#define HCI_PARAMETER_OUT_OF_RANGE 0x30 +#define HCI_ROLE_SWITCH_PENDING 0x32 +#define HCI_SLOT_VIOLATION 0x34 +#define HCI_ROLE_SWITCH_FAILED 0x35 +#define HCI_EIR_TOO_LARGE 0x36 +#define HCI_SIMPLE_PAIRING_NOT_SUPPORTED 0x37 +#define HCI_HOST_BUSY_PAIRING 0x38 + +/* ACL flags */ +#define ACL_START_NO_FLUSH 0x00 +#define ACL_CONT 0x01 +#define ACL_START 0x02 +#define ACL_ACTIVE_BCAST 0x04 +#define ACL_PICO_BCAST 0x08 + +/* Baseband links */ +#define SCO_LINK 0x00 +#define ACL_LINK 0x01 +#define ESCO_LINK 0x02 + +/* LMP features */ +#define LMP_3SLOT 0x01 +#define LMP_5SLOT 0x02 +#define LMP_ENCRYPT 0x04 +#define LMP_SOFFSET 0x08 +#define LMP_TACCURACY 0x10 +#define LMP_RSWITCH 0x20 +#define LMP_HOLD 0x40 +#define LMP_SNIFF 0x80 + +#define LMP_PARK 0x01 +#define LMP_RSSI 0x02 +#define LMP_QUALITY 0x04 +#define LMP_SCO 0x08 +#define LMP_HV2 0x10 +#define LMP_HV3 0x20 +#define LMP_ULAW 0x40 +#define LMP_ALAW 0x80 + +#define LMP_CVSD 0x01 +#define LMP_PSCHEME 0x02 +#define LMP_PCONTROL 0x04 +#define LMP_TRSP_SCO 0x08 +#define LMP_BCAST_ENC 0x80 + +#define LMP_EDR_ACL_2M 0x02 +#define LMP_EDR_ACL_3M 0x04 +#define LMP_ENH_ISCAN 0x08 +#define LMP_ILACE_ISCAN 0x10 +#define LMP_ILACE_PSCAN 0x20 +#define LMP_RSSI_INQ 0x40 +#define LMP_ESCO 0x80 + +#define LMP_EV4 0x01 +#define LMP_EV5 0x02 +#define LMP_AFH_CAP_SLV 0x08 +#define LMP_AFH_CLS_SLV 0x10 +#define LMP_NO_BREDR 0x20 +#define LMP_LE 0x40 +#define LMP_EDR_3SLOT 0x80 + +#define LMP_EDR_5SLOT 0x01 +#define LMP_SNIFF_SUBR 0x02 +#define LMP_PAUSE_ENC 0x04 +#define LMP_AFH_CAP_MST 0x08 +#define LMP_AFH_CLS_MST 0x10 +#define LMP_EDR_ESCO_2M 0x20 +#define LMP_EDR_ESCO_3M 0x40 +#define LMP_EDR_3S_ESCO 0x80 + +#define LMP_EXT_INQ 0x01 +#define LMP_LE_BREDR 0x02 +#define LMP_SIMPLE_PAIR 0x08 +#define LMP_ENCAPS_PDU 0x10 +#define LMP_ERR_DAT_REP 0x20 +#define LMP_NFLUSH_PKTS 0x40 + +#define LMP_LSTO 0x01 +#define LMP_INQ_TX_PWR 0x02 +#define LMP_EPC 0x04 +#define LMP_EXT_FEAT 0x80 + +/* Extended LMP features */ +#define LMP_HOST_SSP 0x01 +#define LMP_HOST_LE 0x02 +#define LMP_HOST_LE_BREDR 0x04 + +/* Link policies */ +#define HCI_LP_RSWITCH 0x0001 +#define HCI_LP_HOLD 0x0002 +#define HCI_LP_SNIFF 0x0004 +#define HCI_LP_PARK 0x0008 + +/* Link mode */ +#define HCI_LM_ACCEPT 0x8000 +#define HCI_LM_MASTER 0x0001 +#define HCI_LM_AUTH 0x0002 +#define HCI_LM_ENCRYPT 0x0004 +#define HCI_LM_TRUSTED 0x0008 +#define HCI_LM_RELIABLE 0x0010 +#define HCI_LM_SECURE 0x0020 + +/* Link Key types */ +#define HCI_LK_COMBINATION 0x00 +#define HCI_LK_LOCAL_UNIT 0x01 +#define HCI_LK_REMOTE_UNIT 0x02 +#define HCI_LK_DEBUG_COMBINATION 0x03 +#define HCI_LK_UNAUTH_COMBINATION 0x04 +#define HCI_LK_AUTH_COMBINATION 0x05 +#define HCI_LK_CHANGED_COMBINATION 0x06 +#define HCI_LK_INVALID 0xFF + +/* ----- HCI Commands ----- */ + +/* Link Control */ +#define OGF_LINK_CTL 0x01 + +#define OCF_INQUIRY 0x0001 +typedef struct { + uint8_t lap[3]; + uint8_t length; /* 1.28s units */ + uint8_t num_rsp; +} __attribute__ ((packed)) inquiry_cp; +#define INQUIRY_CP_SIZE 5 + +typedef struct { + uint8_t status; + bdaddr_t bdaddr; +} __attribute__ ((packed)) status_bdaddr_rp; +#define STATUS_BDADDR_RP_SIZE 7 + +#define OCF_INQUIRY_CANCEL 0x0002 + +#define OCF_PERIODIC_INQUIRY 0x0003 +typedef struct { + uint16_t max_period; /* 1.28s units */ + uint16_t min_period; /* 1.28s units */ + uint8_t lap[3]; + uint8_t length; /* 1.28s units */ + uint8_t num_rsp; +} __attribute__ ((packed)) periodic_inquiry_cp; +#define PERIODIC_INQUIRY_CP_SIZE 9 + +#define OCF_EXIT_PERIODIC_INQUIRY 0x0004 + +#define OCF_CREATE_CONN 0x0005 +typedef struct { + bdaddr_t bdaddr; + uint16_t pkt_type; + uint8_t pscan_rep_mode; + uint8_t pscan_mode; + uint16_t clock_offset; + uint8_t role_switch; +} __attribute__ ((packed)) create_conn_cp; +#define CREATE_CONN_CP_SIZE 13 + +#define OCF_DISCONNECT 0x0006 +typedef struct { + uint16_t handle; + uint8_t reason; +} __attribute__ ((packed)) disconnect_cp; +#define DISCONNECT_CP_SIZE 3 + +#define OCF_ADD_SCO 0x0007 +typedef struct { + uint16_t handle; + uint16_t pkt_type; +} __attribute__ ((packed)) add_sco_cp; +#define ADD_SCO_CP_SIZE 4 + +#define OCF_CREATE_CONN_CANCEL 0x0008 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) create_conn_cancel_cp; +#define CREATE_CONN_CANCEL_CP_SIZE 6 + +#define OCF_ACCEPT_CONN_REQ 0x0009 +typedef struct { + bdaddr_t bdaddr; + uint8_t role; +} __attribute__ ((packed)) accept_conn_req_cp; +#define ACCEPT_CONN_REQ_CP_SIZE 7 + +#define OCF_REJECT_CONN_REQ 0x000A +typedef struct { + bdaddr_t bdaddr; + uint8_t reason; +} __attribute__ ((packed)) reject_conn_req_cp; +#define REJECT_CONN_REQ_CP_SIZE 7 + +#define OCF_LINK_KEY_REPLY 0x000B +typedef struct { + bdaddr_t bdaddr; + uint8_t link_key[16]; +} __attribute__ ((packed)) link_key_reply_cp; +#define LINK_KEY_REPLY_CP_SIZE 22 + +#define OCF_LINK_KEY_NEG_REPLY 0x000C + +#define OCF_PIN_CODE_REPLY 0x000D +typedef struct { + bdaddr_t bdaddr; + uint8_t pin_len; + uint8_t pin_code[16]; +} __attribute__ ((packed)) pin_code_reply_cp; +#define PIN_CODE_REPLY_CP_SIZE 23 + +#define OCF_PIN_CODE_NEG_REPLY 0x000E + +#define OCF_SET_CONN_PTYPE 0x000F +typedef struct { + uint16_t handle; + uint16_t pkt_type; +} __attribute__ ((packed)) set_conn_ptype_cp; +#define SET_CONN_PTYPE_CP_SIZE 4 + +#define OCF_AUTH_REQUESTED 0x0011 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) auth_requested_cp; +#define AUTH_REQUESTED_CP_SIZE 2 + +#define OCF_SET_CONN_ENCRYPT 0x0013 +typedef struct { + uint16_t handle; + uint8_t encrypt; +} __attribute__ ((packed)) set_conn_encrypt_cp; +#define SET_CONN_ENCRYPT_CP_SIZE 3 + +#define OCF_CHANGE_CONN_LINK_KEY 0x0015 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) change_conn_link_key_cp; +#define CHANGE_CONN_LINK_KEY_CP_SIZE 2 + +#define OCF_MASTER_LINK_KEY 0x0017 +typedef struct { + uint8_t key_flag; +} __attribute__ ((packed)) master_link_key_cp; +#define MASTER_LINK_KEY_CP_SIZE 1 + +#define OCF_REMOTE_NAME_REQ 0x0019 +typedef struct { + bdaddr_t bdaddr; + uint8_t pscan_rep_mode; + uint8_t pscan_mode; + uint16_t clock_offset; +} __attribute__ ((packed)) remote_name_req_cp; +#define REMOTE_NAME_REQ_CP_SIZE 10 + +#define OCF_REMOTE_NAME_REQ_CANCEL 0x001A +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) remote_name_req_cancel_cp; +#define REMOTE_NAME_REQ_CANCEL_CP_SIZE 6 + +#define OCF_READ_REMOTE_FEATURES 0x001B +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) read_remote_features_cp; +#define READ_REMOTE_FEATURES_CP_SIZE 2 + +#define OCF_READ_REMOTE_EXT_FEATURES 0x001C +typedef struct { + uint16_t handle; + uint8_t page_num; +} __attribute__ ((packed)) read_remote_ext_features_cp; +#define READ_REMOTE_EXT_FEATURES_CP_SIZE 3 + +#define OCF_READ_REMOTE_VERSION 0x001D +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) read_remote_version_cp; +#define READ_REMOTE_VERSION_CP_SIZE 2 + +#define OCF_READ_CLOCK_OFFSET 0x001F +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) read_clock_offset_cp; +#define READ_CLOCK_OFFSET_CP_SIZE 2 + +#define OCF_READ_LMP_HANDLE 0x0020 + +#define OCF_SETUP_SYNC_CONN 0x0028 +typedef struct { + uint16_t handle; + uint32_t tx_bandwith; + uint32_t rx_bandwith; + uint16_t max_latency; + uint16_t voice_setting; + uint8_t retrans_effort; + uint16_t pkt_type; +} __attribute__ ((packed)) setup_sync_conn_cp; +#define SETUP_SYNC_CONN_CP_SIZE 17 + +#define OCF_ACCEPT_SYNC_CONN_REQ 0x0029 +typedef struct { + bdaddr_t bdaddr; + uint32_t tx_bandwith; + uint32_t rx_bandwith; + uint16_t max_latency; + uint16_t voice_setting; + uint8_t retrans_effort; + uint16_t pkt_type; +} __attribute__ ((packed)) accept_sync_conn_req_cp; +#define ACCEPT_SYNC_CONN_REQ_CP_SIZE 21 + +#define OCF_REJECT_SYNC_CONN_REQ 0x002A +typedef struct { + bdaddr_t bdaddr; + uint8_t reason; +} __attribute__ ((packed)) reject_sync_conn_req_cp; +#define REJECT_SYNC_CONN_REQ_CP_SIZE 7 + +#define OCF_IO_CAPABILITY_REPLY 0x002B +typedef struct { + bdaddr_t bdaddr; + uint8_t capability; + uint8_t oob_data; + uint8_t authentication; +} __attribute__ ((packed)) io_capability_reply_cp; +#define IO_CAPABILITY_REPLY_CP_SIZE 9 + +#define OCF_USER_CONFIRM_REPLY 0x002C +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) user_confirm_reply_cp; +#define USER_CONFIRM_REPLY_CP_SIZE 6 + +#define OCF_USER_CONFIRM_NEG_REPLY 0x002D + +#define OCF_USER_PASSKEY_REPLY 0x002E +typedef struct { + bdaddr_t bdaddr; + uint32_t passkey; +} __attribute__ ((packed)) user_passkey_reply_cp; +#define USER_PASSKEY_REPLY_CP_SIZE 10 + +#define OCF_USER_PASSKEY_NEG_REPLY 0x002F + +#define OCF_REMOTE_OOB_DATA_REPLY 0x0030 +typedef struct { + bdaddr_t bdaddr; + uint8_t hash[16]; + uint8_t randomizer[16]; +} __attribute__ ((packed)) remote_oob_data_reply_cp; +#define REMOTE_OOB_DATA_REPLY_CP_SIZE 38 + +#define OCF_REMOTE_OOB_DATA_NEG_REPLY 0x0033 + +#define OCF_IO_CAPABILITY_NEG_REPLY 0x0034 +typedef struct { + bdaddr_t bdaddr; + uint8_t reason; +} __attribute__ ((packed)) io_capability_neg_reply_cp; +#define IO_CAPABILITY_NEG_REPLY_CP_SIZE 7 + +#define OCF_CREATE_PHYSICAL_LINK 0x0035 +typedef struct { + uint8_t handle; + uint8_t key_length; + uint8_t key_type; + uint8_t key[32]; +} __attribute__ ((packed)) create_physical_link_cp; +#define CREATE_PHYSICAL_LINK_CP_SIZE 35 + +#define OCF_ACCEPT_PHYSICAL_LINK 0x0036 +typedef struct { + uint8_t handle; + uint8_t key_length; + uint8_t key_type; + uint8_t key[32]; +} __attribute__ ((packed)) accept_physical_link_cp; +#define ACCEPT_PHYSICAL_LINK_CP_SIZE 35 + +#define OCF_DISCONNECT_PHYSICAL_LINK 0x0037 +typedef struct { + uint8_t handle; + uint8_t reason; +} __attribute__ ((packed)) disconnect_physical_link_cp; +#define DISCONNECT_PHYSICAL_LINK_CP_SIZE 2 + +#define OCF_CREATE_LOGICAL_LINK 0x0038 +typedef struct { + uint8_t handle; + uint8_t tx_flow[16]; + uint8_t rx_flow[16]; +} __attribute__ ((packed)) create_logical_link_cp; +#define CREATE_LOGICAL_LINK_CP_SIZE 33 + +#define OCF_ACCEPT_LOGICAL_LINK 0x0039 + +#define OCF_DISCONNECT_LOGICAL_LINK 0x003A +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) disconnect_logical_link_cp; +#define DISCONNECT_LOGICAL_LINK_CP_SIZE 2 + +#define OCF_LOGICAL_LINK_CANCEL 0x003B +typedef struct { + uint8_t handle; + uint8_t tx_flow_id; +} __attribute__ ((packed)) cancel_logical_link_cp; +#define LOGICAL_LINK_CANCEL_CP_SIZE 2 +typedef struct { + uint8_t status; + uint8_t handle; + uint8_t tx_flow_id; +} __attribute__ ((packed)) cancel_logical_link_rp; +#define LOGICAL_LINK_CANCEL_RP_SIZE 3 + +#define OCF_FLOW_SPEC_MODIFY 0x003C + +/* Link Policy */ +#define OGF_LINK_POLICY 0x02 + +#define OCF_HOLD_MODE 0x0001 +typedef struct { + uint16_t handle; + uint16_t max_interval; + uint16_t min_interval; +} __attribute__ ((packed)) hold_mode_cp; +#define HOLD_MODE_CP_SIZE 6 + +#define OCF_SNIFF_MODE 0x0003 +typedef struct { + uint16_t handle; + uint16_t max_interval; + uint16_t min_interval; + uint16_t attempt; + uint16_t timeout; +} __attribute__ ((packed)) sniff_mode_cp; +#define SNIFF_MODE_CP_SIZE 10 + +#define OCF_EXIT_SNIFF_MODE 0x0004 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) exit_sniff_mode_cp; +#define EXIT_SNIFF_MODE_CP_SIZE 2 + +#define OCF_PARK_MODE 0x0005 +typedef struct { + uint16_t handle; + uint16_t max_interval; + uint16_t min_interval; +} __attribute__ ((packed)) park_mode_cp; +#define PARK_MODE_CP_SIZE 6 + +#define OCF_EXIT_PARK_MODE 0x0006 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) exit_park_mode_cp; +#define EXIT_PARK_MODE_CP_SIZE 2 + +#define OCF_QOS_SETUP 0x0007 +typedef struct { + uint8_t service_type; /* 1 = best effort */ + uint32_t token_rate; /* Byte per seconds */ + uint32_t peak_bandwidth; /* Byte per seconds */ + uint32_t latency; /* Microseconds */ + uint32_t delay_variation; /* Microseconds */ +} __attribute__ ((packed)) hci_qos; +#define HCI_QOS_CP_SIZE 17 +typedef struct { + uint16_t handle; + uint8_t flags; /* Reserved */ + hci_qos qos; +} __attribute__ ((packed)) qos_setup_cp; +#define QOS_SETUP_CP_SIZE (3 + HCI_QOS_CP_SIZE) + +#define OCF_ROLE_DISCOVERY 0x0009 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) role_discovery_cp; +#define ROLE_DISCOVERY_CP_SIZE 2 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t role; +} __attribute__ ((packed)) role_discovery_rp; +#define ROLE_DISCOVERY_RP_SIZE 4 + +#define OCF_SWITCH_ROLE 0x000B +typedef struct { + bdaddr_t bdaddr; + uint8_t role; +} __attribute__ ((packed)) switch_role_cp; +#define SWITCH_ROLE_CP_SIZE 7 + +#define OCF_READ_LINK_POLICY 0x000C +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) read_link_policy_cp; +#define READ_LINK_POLICY_CP_SIZE 2 +typedef struct { + uint8_t status; + uint16_t handle; + uint16_t policy; +} __attribute__ ((packed)) read_link_policy_rp; +#define READ_LINK_POLICY_RP_SIZE 5 + +#define OCF_WRITE_LINK_POLICY 0x000D +typedef struct { + uint16_t handle; + uint16_t policy; +} __attribute__ ((packed)) write_link_policy_cp; +#define WRITE_LINK_POLICY_CP_SIZE 4 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) write_link_policy_rp; +#define WRITE_LINK_POLICY_RP_SIZE 3 + +#define OCF_READ_DEFAULT_LINK_POLICY 0x000E + +#define OCF_WRITE_DEFAULT_LINK_POLICY 0x000F + +#define OCF_FLOW_SPECIFICATION 0x0010 + +#define OCF_SNIFF_SUBRATING 0x0011 +typedef struct { + uint16_t handle; + uint16_t max_latency; + uint16_t min_remote_timeout; + uint16_t min_local_timeout; +} __attribute__ ((packed)) sniff_subrating_cp; +#define SNIFF_SUBRATING_CP_SIZE 8 + +/* Host Controller and Baseband */ +#define OGF_HOST_CTL 0x03 + +#define OCF_SET_EVENT_MASK 0x0001 +typedef struct { + uint8_t mask[8]; +} __attribute__ ((packed)) set_event_mask_cp; +#define SET_EVENT_MASK_CP_SIZE 8 + +#define OCF_RESET 0x0003 + +#define OCF_SET_EVENT_FLT 0x0005 +typedef struct { + uint8_t flt_type; + uint8_t cond_type; + uint8_t condition[0]; +} __attribute__ ((packed)) set_event_flt_cp; +#define SET_EVENT_FLT_CP_SIZE 2 + +/* Filter types */ +#define FLT_CLEAR_ALL 0x00 +#define FLT_INQ_RESULT 0x01 +#define FLT_CONN_SETUP 0x02 +/* INQ_RESULT Condition types */ +#define INQ_RESULT_RETURN_ALL 0x00 +#define INQ_RESULT_RETURN_CLASS 0x01 +#define INQ_RESULT_RETURN_BDADDR 0x02 +/* CONN_SETUP Condition types */ +#define CONN_SETUP_ALLOW_ALL 0x00 +#define CONN_SETUP_ALLOW_CLASS 0x01 +#define CONN_SETUP_ALLOW_BDADDR 0x02 +/* CONN_SETUP Conditions */ +#define CONN_SETUP_AUTO_OFF 0x01 +#define CONN_SETUP_AUTO_ON 0x02 + +#define OCF_FLUSH 0x0008 + +#define OCF_READ_PIN_TYPE 0x0009 +typedef struct { + uint8_t status; + uint8_t pin_type; +} __attribute__ ((packed)) read_pin_type_rp; +#define READ_PIN_TYPE_RP_SIZE 2 + +#define OCF_WRITE_PIN_TYPE 0x000A +typedef struct { + uint8_t pin_type; +} __attribute__ ((packed)) write_pin_type_cp; +#define WRITE_PIN_TYPE_CP_SIZE 1 + +#define OCF_CREATE_NEW_UNIT_KEY 0x000B + +#define OCF_READ_STORED_LINK_KEY 0x000D +typedef struct { + bdaddr_t bdaddr; + uint8_t read_all; +} __attribute__ ((packed)) read_stored_link_key_cp; +#define READ_STORED_LINK_KEY_CP_SIZE 7 +typedef struct { + uint8_t status; + uint16_t max_keys; + uint16_t num_keys; +} __attribute__ ((packed)) read_stored_link_key_rp; +#define READ_STORED_LINK_KEY_RP_SIZE 5 + +#define OCF_WRITE_STORED_LINK_KEY 0x0011 +typedef struct { + uint8_t num_keys; + /* variable length part */ +} __attribute__ ((packed)) write_stored_link_key_cp; +#define WRITE_STORED_LINK_KEY_CP_SIZE 1 +typedef struct { + uint8_t status; + uint8_t num_keys; +} __attribute__ ((packed)) write_stored_link_key_rp; +#define READ_WRITE_LINK_KEY_RP_SIZE 2 + +#define OCF_DELETE_STORED_LINK_KEY 0x0012 +typedef struct { + bdaddr_t bdaddr; + uint8_t delete_all; +} __attribute__ ((packed)) delete_stored_link_key_cp; +#define DELETE_STORED_LINK_KEY_CP_SIZE 7 +typedef struct { + uint8_t status; + uint16_t num_keys; +} __attribute__ ((packed)) delete_stored_link_key_rp; +#define DELETE_STORED_LINK_KEY_RP_SIZE 3 + +#define HCI_MAX_NAME_LENGTH 248 + +#define OCF_CHANGE_LOCAL_NAME 0x0013 +typedef struct { + uint8_t name[HCI_MAX_NAME_LENGTH]; +} __attribute__ ((packed)) change_local_name_cp; +#define CHANGE_LOCAL_NAME_CP_SIZE 248 + +#define OCF_READ_LOCAL_NAME 0x0014 +typedef struct { + uint8_t status; + uint8_t name[HCI_MAX_NAME_LENGTH]; +} __attribute__ ((packed)) read_local_name_rp; +#define READ_LOCAL_NAME_RP_SIZE 249 + +#define OCF_READ_CONN_ACCEPT_TIMEOUT 0x0015 +typedef struct { + uint8_t status; + uint16_t timeout; +} __attribute__ ((packed)) read_conn_accept_timeout_rp; +#define READ_CONN_ACCEPT_TIMEOUT_RP_SIZE 3 + +#define OCF_WRITE_CONN_ACCEPT_TIMEOUT 0x0016 +typedef struct { + uint16_t timeout; +} __attribute__ ((packed)) write_conn_accept_timeout_cp; +#define WRITE_CONN_ACCEPT_TIMEOUT_CP_SIZE 2 + +#define OCF_READ_PAGE_TIMEOUT 0x0017 +typedef struct { + uint8_t status; + uint16_t timeout; +} __attribute__ ((packed)) read_page_timeout_rp; +#define READ_PAGE_TIMEOUT_RP_SIZE 3 + +#define OCF_WRITE_PAGE_TIMEOUT 0x0018 +typedef struct { + uint16_t timeout; +} __attribute__ ((packed)) write_page_timeout_cp; +#define WRITE_PAGE_TIMEOUT_CP_SIZE 2 + +#define OCF_READ_SCAN_ENABLE 0x0019 +typedef struct { + uint8_t status; + uint8_t enable; +} __attribute__ ((packed)) read_scan_enable_rp; +#define READ_SCAN_ENABLE_RP_SIZE 2 + +#define OCF_WRITE_SCAN_ENABLE 0x001A + #define SCAN_DISABLED 0x00 + #define SCAN_INQUIRY 0x01 + #define SCAN_PAGE 0x02 + +#define OCF_READ_PAGE_ACTIVITY 0x001B +typedef struct { + uint8_t status; + uint16_t interval; + uint16_t window; +} __attribute__ ((packed)) read_page_activity_rp; +#define READ_PAGE_ACTIVITY_RP_SIZE 5 + +#define OCF_WRITE_PAGE_ACTIVITY 0x001C +typedef struct { + uint16_t interval; + uint16_t window; +} __attribute__ ((packed)) write_page_activity_cp; +#define WRITE_PAGE_ACTIVITY_CP_SIZE 4 + +#define OCF_READ_INQ_ACTIVITY 0x001D +typedef struct { + uint8_t status; + uint16_t interval; + uint16_t window; +} __attribute__ ((packed)) read_inq_activity_rp; +#define READ_INQ_ACTIVITY_RP_SIZE 5 + +#define OCF_WRITE_INQ_ACTIVITY 0x001E +typedef struct { + uint16_t interval; + uint16_t window; +} __attribute__ ((packed)) write_inq_activity_cp; +#define WRITE_INQ_ACTIVITY_CP_SIZE 4 + +#define OCF_READ_AUTH_ENABLE 0x001F + +#define OCF_WRITE_AUTH_ENABLE 0x0020 + #define AUTH_DISABLED 0x00 + #define AUTH_ENABLED 0x01 + +#define OCF_READ_ENCRYPT_MODE 0x0021 + +#define OCF_WRITE_ENCRYPT_MODE 0x0022 + #define ENCRYPT_DISABLED 0x00 + #define ENCRYPT_P2P 0x01 + #define ENCRYPT_BOTH 0x02 + +#define OCF_READ_CLASS_OF_DEV 0x0023 +typedef struct { + uint8_t status; + uint8_t dev_class[3]; +} __attribute__ ((packed)) read_class_of_dev_rp; +#define READ_CLASS_OF_DEV_RP_SIZE 4 + +#define OCF_WRITE_CLASS_OF_DEV 0x0024 +typedef struct { + uint8_t dev_class[3]; +} __attribute__ ((packed)) write_class_of_dev_cp; +#define WRITE_CLASS_OF_DEV_CP_SIZE 3 + +#define OCF_READ_VOICE_SETTING 0x0025 +typedef struct { + uint8_t status; + uint16_t voice_setting; +} __attribute__ ((packed)) read_voice_setting_rp; +#define READ_VOICE_SETTING_RP_SIZE 3 + +#define OCF_WRITE_VOICE_SETTING 0x0026 +typedef struct { + uint16_t voice_setting; +} __attribute__ ((packed)) write_voice_setting_cp; +#define WRITE_VOICE_SETTING_CP_SIZE 2 + +#define OCF_READ_AUTOMATIC_FLUSH_TIMEOUT 0x0027 + +#define OCF_WRITE_AUTOMATIC_FLUSH_TIMEOUT 0x0028 + +#define OCF_READ_NUM_BROADCAST_RETRANS 0x0029 + +#define OCF_WRITE_NUM_BROADCAST_RETRANS 0x002A + +#define OCF_READ_HOLD_MODE_ACTIVITY 0x002B + +#define OCF_WRITE_HOLD_MODE_ACTIVITY 0x002C + +#define OCF_READ_TRANSMIT_POWER_LEVEL 0x002D +typedef struct { + uint16_t handle; + uint8_t type; +} __attribute__ ((packed)) read_transmit_power_level_cp; +#define READ_TRANSMIT_POWER_LEVEL_CP_SIZE 3 +typedef struct { + uint8_t status; + uint16_t handle; + int8_t level; +} __attribute__ ((packed)) read_transmit_power_level_rp; +#define READ_TRANSMIT_POWER_LEVEL_RP_SIZE 4 + +#define OCF_READ_SYNC_FLOW_ENABLE 0x002E + +#define OCF_WRITE_SYNC_FLOW_ENABLE 0x002F + +#define OCF_SET_CONTROLLER_TO_HOST_FC 0x0031 + +#define OCF_HOST_BUFFER_SIZE 0x0033 +typedef struct { + uint16_t acl_mtu; + uint8_t sco_mtu; + uint16_t acl_max_pkt; + uint16_t sco_max_pkt; +} __attribute__ ((packed)) host_buffer_size_cp; +#define HOST_BUFFER_SIZE_CP_SIZE 7 + +#define OCF_HOST_NUM_COMP_PKTS 0x0035 +typedef struct { + uint8_t num_hndl; + /* variable length part */ +} __attribute__ ((packed)) host_num_comp_pkts_cp; +#define HOST_NUM_COMP_PKTS_CP_SIZE 1 + +#define OCF_READ_LINK_SUPERVISION_TIMEOUT 0x0036 +typedef struct { + uint8_t status; + uint16_t handle; + uint16_t timeout; +} __attribute__ ((packed)) read_link_supervision_timeout_rp; +#define READ_LINK_SUPERVISION_TIMEOUT_RP_SIZE 5 + +#define OCF_WRITE_LINK_SUPERVISION_TIMEOUT 0x0037 +typedef struct { + uint16_t handle; + uint16_t timeout; +} __attribute__ ((packed)) write_link_supervision_timeout_cp; +#define WRITE_LINK_SUPERVISION_TIMEOUT_CP_SIZE 4 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) write_link_supervision_timeout_rp; +#define WRITE_LINK_SUPERVISION_TIMEOUT_RP_SIZE 3 + +#define OCF_READ_NUM_SUPPORTED_IAC 0x0038 + +#define MAX_IAC_LAP 0x40 +#define OCF_READ_CURRENT_IAC_LAP 0x0039 +typedef struct { + uint8_t status; + uint8_t num_current_iac; + uint8_t lap[MAX_IAC_LAP][3]; +} __attribute__ ((packed)) read_current_iac_lap_rp; +#define READ_CURRENT_IAC_LAP_RP_SIZE 2+3*MAX_IAC_LAP + +#define OCF_WRITE_CURRENT_IAC_LAP 0x003A +typedef struct { + uint8_t num_current_iac; + uint8_t lap[MAX_IAC_LAP][3]; +} __attribute__ ((packed)) write_current_iac_lap_cp; +#define WRITE_CURRENT_IAC_LAP_CP_SIZE 1+3*MAX_IAC_LAP + +#define OCF_READ_PAGE_SCAN_PERIOD_MODE 0x003B + +#define OCF_WRITE_PAGE_SCAN_PERIOD_MODE 0x003C + +#define OCF_READ_PAGE_SCAN_MODE 0x003D + +#define OCF_WRITE_PAGE_SCAN_MODE 0x003E + +#define OCF_SET_AFH_CLASSIFICATION 0x003F +typedef struct { + uint8_t map[10]; +} __attribute__ ((packed)) set_afh_classification_cp; +#define SET_AFH_CLASSIFICATION_CP_SIZE 10 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) set_afh_classification_rp; +#define SET_AFH_CLASSIFICATION_RP_SIZE 1 + +#define OCF_READ_INQUIRY_SCAN_TYPE 0x0042 +typedef struct { + uint8_t status; + uint8_t type; +} __attribute__ ((packed)) read_inquiry_scan_type_rp; +#define READ_INQUIRY_SCAN_TYPE_RP_SIZE 2 + +#define OCF_WRITE_INQUIRY_SCAN_TYPE 0x0043 +typedef struct { + uint8_t type; +} __attribute__ ((packed)) write_inquiry_scan_type_cp; +#define WRITE_INQUIRY_SCAN_TYPE_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_inquiry_scan_type_rp; +#define WRITE_INQUIRY_SCAN_TYPE_RP_SIZE 1 + +#define OCF_READ_INQUIRY_MODE 0x0044 +typedef struct { + uint8_t status; + uint8_t mode; +} __attribute__ ((packed)) read_inquiry_mode_rp; +#define READ_INQUIRY_MODE_RP_SIZE 2 + +#define OCF_WRITE_INQUIRY_MODE 0x0045 +typedef struct { + uint8_t mode; +} __attribute__ ((packed)) write_inquiry_mode_cp; +#define WRITE_INQUIRY_MODE_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_inquiry_mode_rp; +#define WRITE_INQUIRY_MODE_RP_SIZE 1 + +#define OCF_READ_PAGE_SCAN_TYPE 0x0046 + +#define OCF_WRITE_PAGE_SCAN_TYPE 0x0047 + #define PAGE_SCAN_TYPE_STANDARD 0x00 + #define PAGE_SCAN_TYPE_INTERLACED 0x01 + +#define OCF_READ_AFH_MODE 0x0048 +typedef struct { + uint8_t status; + uint8_t mode; +} __attribute__ ((packed)) read_afh_mode_rp; +#define READ_AFH_MODE_RP_SIZE 2 + +#define OCF_WRITE_AFH_MODE 0x0049 +typedef struct { + uint8_t mode; +} __attribute__ ((packed)) write_afh_mode_cp; +#define WRITE_AFH_MODE_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_afh_mode_rp; +#define WRITE_AFH_MODE_RP_SIZE 1 + +#define HCI_MAX_EIR_LENGTH 240 + +#define OCF_READ_EXT_INQUIRY_RESPONSE 0x0051 +typedef struct { + uint8_t status; + uint8_t fec; + uint8_t data[HCI_MAX_EIR_LENGTH]; +} __attribute__ ((packed)) read_ext_inquiry_response_rp; +#define READ_EXT_INQUIRY_RESPONSE_RP_SIZE 242 + +#define OCF_WRITE_EXT_INQUIRY_RESPONSE 0x0052 +typedef struct { + uint8_t fec; + uint8_t data[HCI_MAX_EIR_LENGTH]; +} __attribute__ ((packed)) write_ext_inquiry_response_cp; +#define WRITE_EXT_INQUIRY_RESPONSE_CP_SIZE 241 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_ext_inquiry_response_rp; +#define WRITE_EXT_INQUIRY_RESPONSE_RP_SIZE 1 + +#define OCF_REFRESH_ENCRYPTION_KEY 0x0053 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) refresh_encryption_key_cp; +#define REFRESH_ENCRYPTION_KEY_CP_SIZE 2 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) refresh_encryption_key_rp; +#define REFRESH_ENCRYPTION_KEY_RP_SIZE 1 + +#define OCF_READ_SIMPLE_PAIRING_MODE 0x0055 +typedef struct { + uint8_t status; + uint8_t mode; +} __attribute__ ((packed)) read_simple_pairing_mode_rp; +#define READ_SIMPLE_PAIRING_MODE_RP_SIZE 2 + +#define OCF_WRITE_SIMPLE_PAIRING_MODE 0x0056 +typedef struct { + uint8_t mode; +} __attribute__ ((packed)) write_simple_pairing_mode_cp; +#define WRITE_SIMPLE_PAIRING_MODE_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_simple_pairing_mode_rp; +#define WRITE_SIMPLE_PAIRING_MODE_RP_SIZE 1 + +#define OCF_READ_LOCAL_OOB_DATA 0x0057 +typedef struct { + uint8_t status; + uint8_t hash[16]; + uint8_t randomizer[16]; +} __attribute__ ((packed)) read_local_oob_data_rp; +#define READ_LOCAL_OOB_DATA_RP_SIZE 33 + +#define OCF_READ_INQ_RESPONSE_TX_POWER_LEVEL 0x0058 +typedef struct { + uint8_t status; + int8_t level; +} __attribute__ ((packed)) read_inq_response_tx_power_level_rp; +#define READ_INQ_RESPONSE_TX_POWER_LEVEL_RP_SIZE 2 + +#define OCF_READ_INQUIRY_TRANSMIT_POWER_LEVEL 0x0058 +typedef struct { + uint8_t status; + int8_t level; +} __attribute__ ((packed)) read_inquiry_transmit_power_level_rp; +#define READ_INQUIRY_TRANSMIT_POWER_LEVEL_RP_SIZE 2 + +#define OCF_WRITE_INQUIRY_TRANSMIT_POWER_LEVEL 0x0059 +typedef struct { + int8_t level; +} __attribute__ ((packed)) write_inquiry_transmit_power_level_cp; +#define WRITE_INQUIRY_TRANSMIT_POWER_LEVEL_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_inquiry_transmit_power_level_rp; +#define WRITE_INQUIRY_TRANSMIT_POWER_LEVEL_RP_SIZE 1 + +#define OCF_READ_DEFAULT_ERROR_DATA_REPORTING 0x005A +typedef struct { + uint8_t status; + uint8_t reporting; +} __attribute__ ((packed)) read_default_error_data_reporting_rp; +#define READ_DEFAULT_ERROR_DATA_REPORTING_RP_SIZE 2 + +#define OCF_WRITE_DEFAULT_ERROR_DATA_REPORTING 0x005B +typedef struct { + uint8_t reporting; +} __attribute__ ((packed)) write_default_error_data_reporting_cp; +#define WRITE_DEFAULT_ERROR_DATA_REPORTING_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_default_error_data_reporting_rp; +#define WRITE_DEFAULT_ERROR_DATA_REPORTING_RP_SIZE 1 + +#define OCF_ENHANCED_FLUSH 0x005F +typedef struct { + uint16_t handle; + uint8_t type; +} __attribute__ ((packed)) enhanced_flush_cp; +#define ENHANCED_FLUSH_CP_SIZE 3 + +#define OCF_SEND_KEYPRESS_NOTIFY 0x0060 +typedef struct { + bdaddr_t bdaddr; + uint8_t type; +} __attribute__ ((packed)) send_keypress_notify_cp; +#define SEND_KEYPRESS_NOTIFY_CP_SIZE 7 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) send_keypress_notify_rp; +#define SEND_KEYPRESS_NOTIFY_RP_SIZE 1 + +#define OCF_READ_LOGICAL_LINK_ACCEPT_TIMEOUT 0x0061 +typedef struct { + uint8_t status; + uint16_t timeout; +} __attribute__ ((packed)) read_log_link_accept_timeout_rp; +#define READ_LOGICAL_LINK_ACCEPT_TIMEOUT_RP_SIZE 3 + +#define OCF_WRITE_LOGICAL_LINK_ACCEPT_TIMEOUT 0x0062 +typedef struct { + uint16_t timeout; +} __attribute__ ((packed)) write_log_link_accept_timeout_cp; +#define WRITE_LOGICAL_LINK_ACCEPT_TIMEOUT_CP_SIZE 2 + +#define OCF_SET_EVENT_MASK_PAGE_2 0x0063 + +#define OCF_READ_LOCATION_DATA 0x0064 + +#define OCF_WRITE_LOCATION_DATA 0x0065 + +#define OCF_READ_FLOW_CONTROL_MODE 0x0066 + +#define OCF_WRITE_FLOW_CONTROL_MODE 0x0067 + +#define OCF_READ_ENHANCED_TRANSMIT_POWER_LEVEL 0x0068 +typedef struct { + uint8_t status; + uint16_t handle; + int8_t level_gfsk; + int8_t level_dqpsk; + int8_t level_8dpsk; +} __attribute__ ((packed)) read_enhanced_transmit_power_level_rp; +#define READ_ENHANCED_TRANSMIT_POWER_LEVEL_RP_SIZE 6 + +#define OCF_READ_BEST_EFFORT_FLUSH_TIMEOUT 0x0069 +typedef struct { + uint8_t status; + uint32_t timeout; +} __attribute__ ((packed)) read_best_effort_flush_timeout_rp; +#define READ_BEST_EFFORT_FLUSH_TIMEOUT_RP_SIZE 5 + +#define OCF_WRITE_BEST_EFFORT_FLUSH_TIMEOUT 0x006A +typedef struct { + uint16_t handle; + uint32_t timeout; +} __attribute__ ((packed)) write_best_effort_flush_timeout_cp; +#define WRITE_BEST_EFFORT_FLUSH_TIMEOUT_CP_SIZE 6 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_best_effort_flush_timeout_rp; +#define WRITE_BEST_EFFORT_FLUSH_TIMEOUT_RP_SIZE 1 + +#define OCF_READ_LE_HOST_SUPPORTED 0x006C +typedef struct { + uint8_t status; + uint8_t le; + uint8_t simul; +} __attribute__ ((packed)) read_le_host_supported_rp; +#define READ_LE_HOST_SUPPORTED_RP_SIZE 3 + +#define OCF_WRITE_LE_HOST_SUPPORTED 0x006D +typedef struct { + uint8_t le; + uint8_t simul; +} __attribute__ ((packed)) write_le_host_supported_cp; +#define WRITE_LE_HOST_SUPPORTED_CP_SIZE 2 + +/* Informational Parameters */ +#define OGF_INFO_PARAM 0x04 + +#define OCF_READ_LOCAL_VERSION 0x0001 +typedef struct { + uint8_t status; + uint8_t hci_ver; + uint16_t hci_rev; + uint8_t lmp_ver; + uint16_t manufacturer; + uint16_t lmp_subver; +} __attribute__ ((packed)) read_local_version_rp; +#define READ_LOCAL_VERSION_RP_SIZE 9 + +#define OCF_READ_LOCAL_COMMANDS 0x0002 +typedef struct { + uint8_t status; + uint8_t commands[64]; +} __attribute__ ((packed)) read_local_commands_rp; +#define READ_LOCAL_COMMANDS_RP_SIZE 65 + +#define OCF_READ_LOCAL_FEATURES 0x0003 +typedef struct { + uint8_t status; + uint8_t features[8]; +} __attribute__ ((packed)) read_local_features_rp; +#define READ_LOCAL_FEATURES_RP_SIZE 9 + +#define OCF_READ_LOCAL_EXT_FEATURES 0x0004 +typedef struct { + uint8_t page_num; +} __attribute__ ((packed)) read_local_ext_features_cp; +#define READ_LOCAL_EXT_FEATURES_CP_SIZE 1 +typedef struct { + uint8_t status; + uint8_t page_num; + uint8_t max_page_num; + uint8_t features[8]; +} __attribute__ ((packed)) read_local_ext_features_rp; +#define READ_LOCAL_EXT_FEATURES_RP_SIZE 11 + +#define OCF_READ_BUFFER_SIZE 0x0005 +typedef struct { + uint8_t status; + uint16_t acl_mtu; + uint8_t sco_mtu; + uint16_t acl_max_pkt; + uint16_t sco_max_pkt; +} __attribute__ ((packed)) read_buffer_size_rp; +#define READ_BUFFER_SIZE_RP_SIZE 8 + +#define OCF_READ_COUNTRY_CODE 0x0007 + +#define OCF_READ_BD_ADDR 0x0009 +typedef struct { + uint8_t status; + bdaddr_t bdaddr; +} __attribute__ ((packed)) read_bd_addr_rp; +#define READ_BD_ADDR_RP_SIZE 7 + +#define OCF_READ_DATA_BLOCK_SIZE 0x000A +typedef struct { + uint8_t status; + uint16_t max_acl_len; + uint16_t data_block_len; + uint16_t num_blocks; +} __attribute__ ((packed)) read_data_block_size_rp; + +/* Status params */ +#define OGF_STATUS_PARAM 0x05 + +#define OCF_READ_FAILED_CONTACT_COUNTER 0x0001 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t counter; +} __attribute__ ((packed)) read_failed_contact_counter_rp; +#define READ_FAILED_CONTACT_COUNTER_RP_SIZE 4 + +#define OCF_RESET_FAILED_CONTACT_COUNTER 0x0002 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) reset_failed_contact_counter_rp; +#define RESET_FAILED_CONTACT_COUNTER_RP_SIZE 3 + +#define OCF_READ_LINK_QUALITY 0x0003 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t link_quality; +} __attribute__ ((packed)) read_link_quality_rp; +#define READ_LINK_QUALITY_RP_SIZE 4 + +#define OCF_READ_RSSI 0x0005 +typedef struct { + uint8_t status; + uint16_t handle; + int8_t rssi; +} __attribute__ ((packed)) read_rssi_rp; +#define READ_RSSI_RP_SIZE 4 + +#define OCF_READ_AFH_MAP 0x0006 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t mode; + uint8_t map[10]; +} __attribute__ ((packed)) read_afh_map_rp; +#define READ_AFH_MAP_RP_SIZE 14 + +#define OCF_READ_CLOCK 0x0007 +typedef struct { + uint16_t handle; + uint8_t which_clock; +} __attribute__ ((packed)) read_clock_cp; +#define READ_CLOCK_CP_SIZE 3 +typedef struct { + uint8_t status; + uint16_t handle; + uint32_t clock; + uint16_t accuracy; +} __attribute__ ((packed)) read_clock_rp; +#define READ_CLOCK_RP_SIZE 9 + +#define OCF_READ_LOCAL_AMP_INFO 0x0009 +typedef struct { + uint8_t status; + uint8_t amp_status; + uint32_t total_bandwidth; + uint32_t max_guaranteed_bandwidth; + uint32_t min_latency; + uint32_t max_pdu_size; + uint8_t controller_type; + uint16_t pal_caps; + uint16_t max_amp_assoc_length; + uint32_t max_flush_timeout; + uint32_t best_effort_flush_timeout; +} __attribute__ ((packed)) read_local_amp_info_rp; +#define READ_LOCAL_AMP_INFO_RP_SIZE 31 + +#define OCF_READ_LOCAL_AMP_ASSOC 0x000A +typedef struct { + uint8_t handle; + uint16_t length_so_far; + uint16_t assoc_length; +} __attribute__ ((packed)) read_local_amp_assoc_cp; +#define READ_LOCAL_AMP_ASSOC_CP_SIZE 5 +typedef struct { + uint8_t status; + uint8_t handle; + uint16_t length; + uint8_t fragment[HCI_MAX_NAME_LENGTH]; +} __attribute__ ((packed)) read_local_amp_assoc_rp; +#define READ_LOCAL_AMP_ASSOC_RP_SIZE 252 + +#define OCF_WRITE_REMOTE_AMP_ASSOC 0x000B +typedef struct { + uint8_t handle; + uint16_t length_so_far; + uint16_t remaining_length; + uint8_t fragment[HCI_MAX_NAME_LENGTH]; +} __attribute__ ((packed)) write_remote_amp_assoc_cp; +#define WRITE_REMOTE_AMP_ASSOC_CP_SIZE 253 +typedef struct { + uint8_t status; + uint8_t handle; +} __attribute__ ((packed)) write_remote_amp_assoc_rp; +#define WRITE_REMOTE_AMP_ASSOC_RP_SIZE 2 + +/* Testing commands */ +#define OGF_TESTING_CMD 0x3e + +#define OCF_READ_LOOPBACK_MODE 0x0001 + +#define OCF_WRITE_LOOPBACK_MODE 0x0002 + +#define OCF_ENABLE_DEVICE_UNDER_TEST_MODE 0x0003 + +#define OCF_WRITE_SIMPLE_PAIRING_DEBUG_MODE 0x0004 +typedef struct { + uint8_t mode; +} __attribute__ ((packed)) write_simple_pairing_debug_mode_cp; +#define WRITE_SIMPLE_PAIRING_DEBUG_MODE_CP_SIZE 1 +typedef struct { + uint8_t status; +} __attribute__ ((packed)) write_simple_pairing_debug_mode_rp; +#define WRITE_SIMPLE_PAIRING_DEBUG_MODE_RP_SIZE 1 + +/* LE commands */ +#define OGF_LE_CTL 0x08 + +#define OCF_LE_SET_EVENT_MASK 0x0001 +typedef struct { + uint8_t mask[8]; +} __attribute__ ((packed)) le_set_event_mask_cp; +#define LE_SET_EVENT_MASK_CP_SIZE 8 + +#define OCF_LE_READ_BUFFER_SIZE 0x0002 +typedef struct { + uint8_t status; + uint16_t pkt_len; + uint8_t max_pkt; +} __attribute__ ((packed)) le_read_buffer_size_rp; +#define LE_READ_BUFFER_SIZE_RP_SIZE 4 + +#define OCF_LE_READ_LOCAL_SUPPORTED_FEATURES 0x0003 +typedef struct { + uint8_t status; + uint8_t features[8]; +} __attribute__ ((packed)) le_read_local_supported_features_rp; +#define LE_READ_LOCAL_SUPPORTED_FEATURES_RP_SIZE 9 + +#define OCF_LE_SET_RANDOM_ADDRESS 0x0005 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) le_set_random_address_cp; +#define LE_SET_RANDOM_ADDRESS_CP_SIZE 6 + +#define OCF_LE_SET_ADVERTISING_PARAMETERS 0x0006 +typedef struct { + uint16_t min_interval; + uint16_t max_interval; + uint8_t advtype; + uint8_t own_bdaddr_type; + uint8_t direct_bdaddr_type; + bdaddr_t direct_bdaddr; + uint8_t chan_map; + uint8_t filter; +} __attribute__ ((packed)) le_set_advertising_parameters_cp; +#define LE_SET_ADVERTISING_PARAMETERS_CP_SIZE 15 + +#define OCF_LE_READ_ADVERTISING_CHANNEL_TX_POWER 0x0007 +typedef struct { + uint8_t status; + int8_t level; +} __attribute__ ((packed)) le_read_advertising_channel_tx_power_rp; +#define LE_READ_ADVERTISING_CHANNEL_TX_POWER_RP_SIZE 2 + +#define OCF_LE_SET_ADVERTISING_DATA 0x0008 +typedef struct { + uint8_t length; + uint8_t data[31]; +} __attribute__ ((packed)) le_set_advertising_data_cp; +#define LE_SET_ADVERTISING_DATA_CP_SIZE 32 + +#define OCF_LE_SET_SCAN_RESPONSE_DATA 0x0009 +typedef struct { + uint8_t length; + uint8_t data[31]; +} __attribute__ ((packed)) le_set_scan_response_data_cp; +#define LE_SET_SCAN_RESPONSE_DATA_CP_SIZE 32 + +#define OCF_LE_SET_ADVERTISE_ENABLE 0x000A +typedef struct { + uint8_t enable; +} __attribute__ ((packed)) le_set_advertise_enable_cp; +#define LE_SET_ADVERTISE_ENABLE_CP_SIZE 1 + +#define OCF_LE_SET_SCAN_PARAMETERS 0x000B +typedef struct { + uint8_t type; + uint16_t interval; + uint16_t window; + uint8_t own_bdaddr_type; + uint8_t filter; +} __attribute__ ((packed)) le_set_scan_parameters_cp; +#define LE_SET_SCAN_PARAMETERS_CP_SIZE 7 + +#define OCF_LE_SET_SCAN_ENABLE 0x000C +typedef struct { + uint8_t enable; + uint8_t filter_dup; +} __attribute__ ((packed)) le_set_scan_enable_cp; +#define LE_SET_SCAN_ENABLE_CP_SIZE 2 + +#define OCF_LE_CREATE_CONN 0x000D +typedef struct { + uint16_t interval; + uint16_t window; + uint8_t initiator_filter; + uint8_t peer_bdaddr_type; + bdaddr_t peer_bdaddr; + uint8_t own_bdaddr_type; + uint16_t min_interval; + uint16_t max_interval; + uint16_t latency; + uint16_t supervision_timeout; + uint16_t min_ce_length; + uint16_t max_ce_length; +} __attribute__ ((packed)) le_create_connection_cp; +#define LE_CREATE_CONN_CP_SIZE 25 + +#define OCF_LE_CREATE_CONN_CANCEL 0x000E + +#define OCF_LE_READ_WHITE_LIST_SIZE 0x000F +typedef struct { + uint8_t status; + uint8_t size; +} __attribute__ ((packed)) le_read_white_list_size_rp; +#define LE_READ_WHITE_LIST_SIZE_RP_SIZE 2 + +#define OCF_LE_CLEAR_WHITE_LIST 0x0010 + +#define OCF_LE_ADD_DEVICE_TO_WHITE_LIST 0x0011 +typedef struct { + uint8_t bdaddr_type; + bdaddr_t bdaddr; +} __attribute__ ((packed)) le_add_device_to_white_list_cp; +#define LE_ADD_DEVICE_TO_WHITE_LIST_CP_SIZE 7 + +#define OCF_LE_REMOVE_DEVICE_FROM_WHITE_LIST 0x0012 +typedef struct { + uint8_t bdaddr_type; + bdaddr_t bdaddr; +} __attribute__ ((packed)) le_remove_device_from_white_list_cp; +#define LE_REMOVE_DEVICE_FROM_WHITE_LIST_CP_SIZE 7 + +#define OCF_LE_CONN_UPDATE 0x0013 +typedef struct { + uint16_t handle; + uint16_t min_interval; + uint16_t max_interval; + uint16_t latency; + uint16_t supervision_timeout; + uint16_t min_ce_length; + uint16_t max_ce_length; +} __attribute__ ((packed)) le_connection_update_cp; +#define LE_CONN_UPDATE_CP_SIZE 14 + +#define OCF_LE_SET_HOST_CHANNEL_CLASSIFICATION 0x0014 +typedef struct { + uint8_t map[5]; +} __attribute__ ((packed)) le_set_host_channel_classification_cp; +#define LE_SET_HOST_CHANNEL_CLASSIFICATION_CP_SIZE 5 + +#define OCF_LE_READ_CHANNEL_MAP 0x0015 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) le_read_channel_map_cp; +#define LE_READ_CHANNEL_MAP_CP_SIZE 2 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t map[5]; +} __attribute__ ((packed)) le_read_channel_map_rp; +#define LE_READ_CHANNEL_MAP_RP_SIZE 8 + +#define OCF_LE_READ_REMOTE_USED_FEATURES 0x0016 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) le_read_remote_used_features_cp; +#define LE_READ_REMOTE_USED_FEATURES_CP_SIZE 2 + +#define OCF_LE_ENCRYPT 0x0017 +typedef struct { + uint8_t key[16]; + uint8_t plaintext[16]; +} __attribute__ ((packed)) le_encrypt_cp; +#define LE_ENCRYPT_CP_SIZE 32 +typedef struct { + uint8_t status; + uint8_t data[16]; +} __attribute__ ((packed)) le_encrypt_rp; +#define LE_ENCRYPT_RP_SIZE 17 + +#define OCF_LE_RAND 0x0018 +typedef struct { + uint8_t status; + uint64_t random; +} __attribute__ ((packed)) le_rand_rp; +#define LE_RAND_RP_SIZE 9 + +#define OCF_LE_START_ENCRYPTION 0x0019 +typedef struct { + uint16_t handle; + uint64_t random; + uint16_t diversifier; + uint8_t key[16]; +} __attribute__ ((packed)) le_start_encryption_cp; +#define LE_START_ENCRYPTION_CP_SIZE 28 + +#define OCF_LE_LTK_REPLY 0x001A +typedef struct { + uint16_t handle; + uint8_t key[16]; +} __attribute__ ((packed)) le_ltk_reply_cp; +#define LE_LTK_REPLY_CP_SIZE 18 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) le_ltk_reply_rp; +#define LE_LTK_REPLY_RP_SIZE 3 + +#define OCF_LE_LTK_NEG_REPLY 0x001B +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) le_ltk_neg_reply_cp; +#define LE_LTK_NEG_REPLY_CP_SIZE 2 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) le_ltk_neg_reply_rp; +#define LE_LTK_NEG_REPLY_RP_SIZE 3 + +#define OCF_LE_READ_SUPPORTED_STATES 0x001C +typedef struct { + uint8_t status; + uint64_t states; +} __attribute__ ((packed)) le_read_supported_states_rp; +#define LE_READ_SUPPORTED_STATES_RP_SIZE 9 + +#define OCF_LE_RECEIVER_TEST 0x001D +typedef struct { + uint8_t frequency; +} __attribute__ ((packed)) le_receiver_test_cp; +#define LE_RECEIVER_TEST_CP_SIZE 1 + +#define OCF_LE_TRANSMITTER_TEST 0x001E +typedef struct { + uint8_t frequency; + uint8_t length; + uint8_t payload; +} __attribute__ ((packed)) le_transmitter_test_cp; +#define LE_TRANSMITTER_TEST_CP_SIZE 3 + +#define OCF_LE_TEST_END 0x001F +typedef struct { + uint8_t status; + uint16_t num_pkts; +} __attribute__ ((packed)) le_test_end_rp; +#define LE_TEST_END_RP_SIZE 3 + +#define OCF_LE_ADD_DEVICE_TO_RESOLV_LIST 0x0027 +typedef struct { + uint8_t bdaddr_type; + bdaddr_t bdaddr; + uint8_t peer_irk[16]; + uint8_t local_irk[16]; +} __attribute__ ((packed)) le_add_device_to_resolv_list_cp; +#define LE_ADD_DEVICE_TO_RESOLV_LIST_CP_SIZE 39 + +#define OCF_LE_REMOVE_DEVICE_FROM_RESOLV_LIST 0x0028 +typedef struct { + uint8_t bdaddr_type; + bdaddr_t bdaddr; +} __attribute__ ((packed)) le_remove_device_from_resolv_list_cp; +#define LE_REMOVE_DEVICE_FROM_RESOLV_LIST_CP_SIZE 7 + +#define OCF_LE_CLEAR_RESOLV_LIST 0x0029 + +#define OCF_LE_READ_RESOLV_LIST_SIZE 0x002A +typedef struct { + uint8_t status; + uint8_t size; +} __attribute__ ((packed)) le_read_resolv_list_size_rp; +#define LE_READ_RESOLV_LIST_SIZE_RP_SIZE 2 + +#define OCF_LE_SET_ADDRESS_RESOLUTION_ENABLE 0x002D +typedef struct { + uint8_t enable; +} __attribute__ ((packed)) le_set_address_resolution_enable_cp; +#define LE_SET_ADDRESS_RESOLUTION_ENABLE_CP_SIZE 1 + +/* Vendor specific commands */ +#define OGF_VENDOR_CMD 0x3f + +/* ---- HCI Events ---- */ + +#define EVT_INQUIRY_COMPLETE 0x01 + +#define EVT_INQUIRY_RESULT 0x02 +typedef struct { + bdaddr_t bdaddr; + uint8_t pscan_rep_mode; + uint8_t pscan_period_mode; + uint8_t pscan_mode; + uint8_t dev_class[3]; + uint16_t clock_offset; +} __attribute__ ((packed)) inquiry_info; +#define INQUIRY_INFO_SIZE 14 + +#define EVT_CONN_COMPLETE 0x03 +typedef struct { + uint8_t status; + uint16_t handle; + bdaddr_t bdaddr; + uint8_t link_type; + uint8_t encr_mode; +} __attribute__ ((packed)) evt_conn_complete; +#define EVT_CONN_COMPLETE_SIZE 11 + +#define EVT_CONN_REQUEST 0x04 +typedef struct { + bdaddr_t bdaddr; + uint8_t dev_class[3]; + uint8_t link_type; +} __attribute__ ((packed)) evt_conn_request; +#define EVT_CONN_REQUEST_SIZE 10 + +#define EVT_DISCONN_COMPLETE 0x05 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t reason; +} __attribute__ ((packed)) evt_disconn_complete; +#define EVT_DISCONN_COMPLETE_SIZE 4 + +#define EVT_AUTH_COMPLETE 0x06 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) evt_auth_complete; +#define EVT_AUTH_COMPLETE_SIZE 3 + +#define EVT_REMOTE_NAME_REQ_COMPLETE 0x07 +typedef struct { + uint8_t status; + bdaddr_t bdaddr; + uint8_t name[HCI_MAX_NAME_LENGTH]; +} __attribute__ ((packed)) evt_remote_name_req_complete; +#define EVT_REMOTE_NAME_REQ_COMPLETE_SIZE 255 + +#define EVT_ENCRYPT_CHANGE 0x08 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t encrypt; +} __attribute__ ((packed)) evt_encrypt_change; +#define EVT_ENCRYPT_CHANGE_SIZE 4 + +#define EVT_CHANGE_CONN_LINK_KEY_COMPLETE 0x09 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) evt_change_conn_link_key_complete; +#define EVT_CHANGE_CONN_LINK_KEY_COMPLETE_SIZE 3 + +#define EVT_MASTER_LINK_KEY_COMPLETE 0x0A +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t key_flag; +} __attribute__ ((packed)) evt_master_link_key_complete; +#define EVT_MASTER_LINK_KEY_COMPLETE_SIZE 4 + +#define EVT_READ_REMOTE_FEATURES_COMPLETE 0x0B +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t features[8]; +} __attribute__ ((packed)) evt_read_remote_features_complete; +#define EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE 11 + +#define EVT_READ_REMOTE_VERSION_COMPLETE 0x0C +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t lmp_ver; + uint16_t manufacturer; + uint16_t lmp_subver; +} __attribute__ ((packed)) evt_read_remote_version_complete; +#define EVT_READ_REMOTE_VERSION_COMPLETE_SIZE 8 + +#define EVT_QOS_SETUP_COMPLETE 0x0D +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t flags; /* Reserved */ + hci_qos qos; +} __attribute__ ((packed)) evt_qos_setup_complete; +#define EVT_QOS_SETUP_COMPLETE_SIZE (4 + HCI_QOS_CP_SIZE) + +#define EVT_CMD_COMPLETE 0x0E +typedef struct { + uint8_t ncmd; + uint16_t opcode; +} __attribute__ ((packed)) evt_cmd_complete; +#define EVT_CMD_COMPLETE_SIZE 3 + +#define EVT_CMD_STATUS 0x0F +typedef struct { + uint8_t status; + uint8_t ncmd; + uint16_t opcode; +} __attribute__ ((packed)) evt_cmd_status; +#define EVT_CMD_STATUS_SIZE 4 + +#define EVT_HARDWARE_ERROR 0x10 +typedef struct { + uint8_t code; +} __attribute__ ((packed)) evt_hardware_error; +#define EVT_HARDWARE_ERROR_SIZE 1 + +#define EVT_FLUSH_OCCURRED 0x11 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) evt_flush_occured; +#define EVT_FLUSH_OCCURRED_SIZE 2 + +#define EVT_ROLE_CHANGE 0x12 +typedef struct { + uint8_t status; + bdaddr_t bdaddr; + uint8_t role; +} __attribute__ ((packed)) evt_role_change; +#define EVT_ROLE_CHANGE_SIZE 8 + +#define EVT_NUM_COMP_PKTS 0x13 +typedef struct { + uint8_t num_hndl; + /* variable length part */ +} __attribute__ ((packed)) evt_num_comp_pkts; +#define EVT_NUM_COMP_PKTS_SIZE 1 + +#define EVT_MODE_CHANGE 0x14 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t mode; + uint16_t interval; +} __attribute__ ((packed)) evt_mode_change; +#define EVT_MODE_CHANGE_SIZE 6 + +#define EVT_RETURN_LINK_KEYS 0x15 +typedef struct { + uint8_t num_keys; + /* variable length part */ +} __attribute__ ((packed)) evt_return_link_keys; +#define EVT_RETURN_LINK_KEYS_SIZE 1 + +#define EVT_PIN_CODE_REQ 0x16 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) evt_pin_code_req; +#define EVT_PIN_CODE_REQ_SIZE 6 + +#define EVT_LINK_KEY_REQ 0x17 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) evt_link_key_req; +#define EVT_LINK_KEY_REQ_SIZE 6 + +#define EVT_LINK_KEY_NOTIFY 0x18 +typedef struct { + bdaddr_t bdaddr; + uint8_t link_key[16]; + uint8_t key_type; +} __attribute__ ((packed)) evt_link_key_notify; +#define EVT_LINK_KEY_NOTIFY_SIZE 23 + +#define EVT_LOOPBACK_COMMAND 0x19 + +#define EVT_DATA_BUFFER_OVERFLOW 0x1A +typedef struct { + uint8_t link_type; +} __attribute__ ((packed)) evt_data_buffer_overflow; +#define EVT_DATA_BUFFER_OVERFLOW_SIZE 1 + +#define EVT_MAX_SLOTS_CHANGE 0x1B +typedef struct { + uint16_t handle; + uint8_t max_slots; +} __attribute__ ((packed)) evt_max_slots_change; +#define EVT_MAX_SLOTS_CHANGE_SIZE 3 + +#define EVT_READ_CLOCK_OFFSET_COMPLETE 0x1C +typedef struct { + uint8_t status; + uint16_t handle; + uint16_t clock_offset; +} __attribute__ ((packed)) evt_read_clock_offset_complete; +#define EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE 5 + +#define EVT_CONN_PTYPE_CHANGED 0x1D +typedef struct { + uint8_t status; + uint16_t handle; + uint16_t ptype; +} __attribute__ ((packed)) evt_conn_ptype_changed; +#define EVT_CONN_PTYPE_CHANGED_SIZE 5 + +#define EVT_QOS_VIOLATION 0x1E +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) evt_qos_violation; +#define EVT_QOS_VIOLATION_SIZE 2 + +#define EVT_PSCAN_REP_MODE_CHANGE 0x20 +typedef struct { + bdaddr_t bdaddr; + uint8_t pscan_rep_mode; +} __attribute__ ((packed)) evt_pscan_rep_mode_change; +#define EVT_PSCAN_REP_MODE_CHANGE_SIZE 7 + +#define EVT_FLOW_SPEC_COMPLETE 0x21 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t flags; + uint8_t direction; + hci_qos qos; +} __attribute__ ((packed)) evt_flow_spec_complete; +#define EVT_FLOW_SPEC_COMPLETE_SIZE (5 + HCI_QOS_CP_SIZE) + +#define EVT_INQUIRY_RESULT_WITH_RSSI 0x22 +typedef struct { + bdaddr_t bdaddr; + uint8_t pscan_rep_mode; + uint8_t pscan_period_mode; + uint8_t dev_class[3]; + uint16_t clock_offset; + int8_t rssi; +} __attribute__ ((packed)) inquiry_info_with_rssi; +#define INQUIRY_INFO_WITH_RSSI_SIZE 14 +typedef struct { + bdaddr_t bdaddr; + uint8_t pscan_rep_mode; + uint8_t pscan_period_mode; + uint8_t pscan_mode; + uint8_t dev_class[3]; + uint16_t clock_offset; + int8_t rssi; +} __attribute__ ((packed)) inquiry_info_with_rssi_and_pscan_mode; +#define INQUIRY_INFO_WITH_RSSI_AND_PSCAN_MODE_SIZE 15 + +#define EVT_READ_REMOTE_EXT_FEATURES_COMPLETE 0x23 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t page_num; + uint8_t max_page_num; + uint8_t features[8]; +} __attribute__ ((packed)) evt_read_remote_ext_features_complete; +#define EVT_READ_REMOTE_EXT_FEATURES_COMPLETE_SIZE 13 + +#define EVT_SYNC_CONN_COMPLETE 0x2C +typedef struct { + uint8_t status; + uint16_t handle; + bdaddr_t bdaddr; + uint8_t link_type; + uint8_t trans_interval; + uint8_t retrans_window; + uint16_t rx_pkt_len; + uint16_t tx_pkt_len; + uint8_t air_mode; +} __attribute__ ((packed)) evt_sync_conn_complete; +#define EVT_SYNC_CONN_COMPLETE_SIZE 17 + +#define EVT_SYNC_CONN_CHANGED 0x2D +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t trans_interval; + uint8_t retrans_window; + uint16_t rx_pkt_len; + uint16_t tx_pkt_len; +} __attribute__ ((packed)) evt_sync_conn_changed; +#define EVT_SYNC_CONN_CHANGED_SIZE 9 + +#define EVT_SNIFF_SUBRATING 0x2E +typedef struct { + uint8_t status; + uint16_t handle; + uint16_t max_tx_latency; + uint16_t max_rx_latency; + uint16_t min_remote_timeout; + uint16_t min_local_timeout; +} __attribute__ ((packed)) evt_sniff_subrating; +#define EVT_SNIFF_SUBRATING_SIZE 11 + +#define EVT_EXTENDED_INQUIRY_RESULT 0x2F +typedef struct { + bdaddr_t bdaddr; + uint8_t pscan_rep_mode; + uint8_t pscan_period_mode; + uint8_t dev_class[3]; + uint16_t clock_offset; + int8_t rssi; + uint8_t data[HCI_MAX_EIR_LENGTH]; +} __attribute__ ((packed)) extended_inquiry_info; +#define EXTENDED_INQUIRY_INFO_SIZE 254 + +#define EVT_ENCRYPTION_KEY_REFRESH_COMPLETE 0x30 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) evt_encryption_key_refresh_complete; +#define EVT_ENCRYPTION_KEY_REFRESH_COMPLETE_SIZE 3 + +#define EVT_IO_CAPABILITY_REQUEST 0x31 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) evt_io_capability_request; +#define EVT_IO_CAPABILITY_REQUEST_SIZE 6 + +#define EVT_IO_CAPABILITY_RESPONSE 0x32 +typedef struct { + bdaddr_t bdaddr; + uint8_t capability; + uint8_t oob_data; + uint8_t authentication; +} __attribute__ ((packed)) evt_io_capability_response; +#define EVT_IO_CAPABILITY_RESPONSE_SIZE 9 + +#define EVT_USER_CONFIRM_REQUEST 0x33 +typedef struct { + bdaddr_t bdaddr; + uint32_t passkey; +} __attribute__ ((packed)) evt_user_confirm_request; +#define EVT_USER_CONFIRM_REQUEST_SIZE 10 + +#define EVT_USER_PASSKEY_REQUEST 0x34 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) evt_user_passkey_request; +#define EVT_USER_PASSKEY_REQUEST_SIZE 6 + +#define EVT_REMOTE_OOB_DATA_REQUEST 0x35 +typedef struct { + bdaddr_t bdaddr; +} __attribute__ ((packed)) evt_remote_oob_data_request; +#define EVT_REMOTE_OOB_DATA_REQUEST_SIZE 6 + +#define EVT_SIMPLE_PAIRING_COMPLETE 0x36 +typedef struct { + uint8_t status; + bdaddr_t bdaddr; +} __attribute__ ((packed)) evt_simple_pairing_complete; +#define EVT_SIMPLE_PAIRING_COMPLETE_SIZE 7 + +#define EVT_LINK_SUPERVISION_TIMEOUT_CHANGED 0x38 +typedef struct { + uint16_t handle; + uint16_t timeout; +} __attribute__ ((packed)) evt_link_supervision_timeout_changed; +#define EVT_LINK_SUPERVISION_TIMEOUT_CHANGED_SIZE 4 + +#define EVT_ENHANCED_FLUSH_COMPLETE 0x39 +typedef struct { + uint16_t handle; +} __attribute__ ((packed)) evt_enhanced_flush_complete; +#define EVT_ENHANCED_FLUSH_COMPLETE_SIZE 2 + +#define EVT_USER_PASSKEY_NOTIFY 0x3B +typedef struct { + bdaddr_t bdaddr; + uint32_t passkey; +} __attribute__ ((packed)) evt_user_passkey_notify; +#define EVT_USER_PASSKEY_NOTIFY_SIZE 10 + +#define EVT_KEYPRESS_NOTIFY 0x3C +typedef struct { + bdaddr_t bdaddr; + uint8_t type; +} __attribute__ ((packed)) evt_keypress_notify; +#define EVT_KEYPRESS_NOTIFY_SIZE 7 + +#define EVT_REMOTE_HOST_FEATURES_NOTIFY 0x3D +typedef struct { + bdaddr_t bdaddr; + uint8_t features[8]; +} __attribute__ ((packed)) evt_remote_host_features_notify; +#define EVT_REMOTE_HOST_FEATURES_NOTIFY_SIZE 14 + +#define EVT_LE_META_EVENT 0x3E +typedef struct { + uint8_t subevent; + uint8_t data[0]; +} __attribute__ ((packed)) evt_le_meta_event; +#define EVT_LE_META_EVENT_SIZE 1 + +#define EVT_LE_CONN_COMPLETE 0x01 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t role; + uint8_t peer_bdaddr_type; + bdaddr_t peer_bdaddr; + uint16_t interval; + uint16_t latency; + uint16_t supervision_timeout; + uint8_t master_clock_accuracy; +} __attribute__ ((packed)) evt_le_connection_complete; +#define EVT_LE_CONN_COMPLETE_SIZE 18 + +#define EVT_LE_ADVERTISING_REPORT 0x02 +typedef struct { + uint8_t evt_type; + uint8_t bdaddr_type; + bdaddr_t bdaddr; + uint8_t length; + uint8_t data[0]; +} __attribute__ ((packed)) le_advertising_info; +#define LE_ADVERTISING_INFO_SIZE 9 + +#define EVT_LE_CONN_UPDATE_COMPLETE 0x03 +typedef struct { + uint8_t status; + uint16_t handle; + uint16_t interval; + uint16_t latency; + uint16_t supervision_timeout; +} __attribute__ ((packed)) evt_le_connection_update_complete; +#define EVT_LE_CONN_UPDATE_COMPLETE_SIZE 9 + +#define EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE 0x04 +typedef struct { + uint8_t status; + uint16_t handle; + uint8_t features[8]; +} __attribute__ ((packed)) evt_le_read_remote_used_features_complete; +#define EVT_LE_READ_REMOTE_USED_FEATURES_COMPLETE_SIZE 11 + +#define EVT_LE_LTK_REQUEST 0x05 +typedef struct { + uint16_t handle; + uint64_t random; + uint16_t diversifier; +} __attribute__ ((packed)) evt_le_long_term_key_request; +#define EVT_LE_LTK_REQUEST_SIZE 12 + +#define EVT_PHYSICAL_LINK_COMPLETE 0x40 +typedef struct { + uint8_t status; + uint8_t handle; +} __attribute__ ((packed)) evt_physical_link_complete; +#define EVT_PHYSICAL_LINK_COMPLETE_SIZE 2 + +#define EVT_CHANNEL_SELECTED 0x41 + +#define EVT_DISCONNECT_PHYSICAL_LINK_COMPLETE 0x42 +typedef struct { + uint8_t status; + uint8_t handle; + uint8_t reason; +} __attribute__ ((packed)) evt_disconn_physical_link_complete; +#define EVT_DISCONNECT_PHYSICAL_LINK_COMPLETE_SIZE 3 + +#define EVT_PHYSICAL_LINK_LOSS_EARLY_WARNING 0x43 +typedef struct { + uint8_t handle; + uint8_t reason; +} __attribute__ ((packed)) evt_physical_link_loss_warning; +#define EVT_PHYSICAL_LINK_LOSS_WARNING_SIZE 2 + +#define EVT_PHYSICAL_LINK_RECOVERY 0x44 +typedef struct { + uint8_t handle; +} __attribute__ ((packed)) evt_physical_link_recovery; +#define EVT_PHYSICAL_LINK_RECOVERY_SIZE 1 + +#define EVT_LOGICAL_LINK_COMPLETE 0x45 +typedef struct { + uint8_t status; + uint16_t log_handle; + uint8_t handle; + uint8_t tx_flow_id; +} __attribute__ ((packed)) evt_logical_link_complete; +#define EVT_LOGICAL_LINK_COMPLETE_SIZE 5 + +#define EVT_DISCONNECT_LOGICAL_LINK_COMPLETE 0x46 + +#define EVT_FLOW_SPEC_MODIFY_COMPLETE 0x47 +typedef struct { + uint8_t status; + uint16_t handle; +} __attribute__ ((packed)) evt_flow_spec_modify_complete; +#define EVT_FLOW_SPEC_MODIFY_COMPLETE_SIZE 3 + +#define EVT_NUMBER_COMPLETED_BLOCKS 0x48 +typedef struct { + uint16_t handle; + uint16_t num_cmplt_pkts; + uint16_t num_cmplt_blks; +} __attribute__ ((packed)) cmplt_handle; +typedef struct { + uint16_t total_num_blocks; + uint8_t num_handles; + cmplt_handle handles[0]; +} __attribute__ ((packed)) evt_num_completed_blocks; + +#define EVT_AMP_STATUS_CHANGE 0x4D +typedef struct { + uint8_t status; + uint8_t amp_status; +} __attribute__ ((packed)) evt_amp_status_change; +#define EVT_AMP_STATUS_CHANGE_SIZE 2 + +#define EVT_TESTING 0xFE + +#define EVT_VENDOR 0xFF + +/* Internal events generated by BlueZ stack */ +#define EVT_STACK_INTERNAL 0xFD +typedef struct { + uint16_t type; + uint8_t data[0]; +} __attribute__ ((packed)) evt_stack_internal; +#define EVT_STACK_INTERNAL_SIZE 2 + +#define EVT_SI_DEVICE 0x01 +typedef struct { + uint16_t event; + uint16_t dev_id; +} __attribute__ ((packed)) evt_si_device; +#define EVT_SI_DEVICE_SIZE 4 + +/* -------- HCI Packet structures -------- */ +#define HCI_TYPE_LEN 1 + +typedef struct { + uint16_t opcode; /* OCF & OGF */ + uint8_t plen; +} __attribute__ ((packed)) hci_command_hdr; +#define HCI_COMMAND_HDR_SIZE 3 + +typedef struct { + uint8_t evt; + uint8_t plen; +} __attribute__ ((packed)) hci_event_hdr; +#define HCI_EVENT_HDR_SIZE 2 + +typedef struct { + uint16_t handle; /* Handle & Flags(PB, BC) */ + uint16_t dlen; +} __attribute__ ((packed)) hci_acl_hdr; +#define HCI_ACL_HDR_SIZE 4 + +typedef struct { + uint16_t handle; + uint8_t dlen; +} __attribute__ ((packed)) hci_sco_hdr; +#define HCI_SCO_HDR_SIZE 3 + +typedef struct { + uint16_t device; + uint16_t type; + uint16_t plen; +} __attribute__ ((packed)) hci_msg_hdr; +#define HCI_MSG_HDR_SIZE 6 + +/* Command opcode pack/unpack */ +#define cmd_opcode_pack(ogf, ocf) (uint16_t)((ocf & 0x03ff)|(ogf << 10)) +#define cmd_opcode_ogf(op) (op >> 10) +#define cmd_opcode_ocf(op) (op & 0x03ff) + +/* ACL handle and flags pack/unpack */ +#define acl_handle_pack(h, f) (uint16_t)((h & 0x0fff)|(f << 12)) +#define acl_handle(h) (h & 0x0fff) +#define acl_flags(h) (h >> 12) + +#endif /* _NO_HCI_DEFS */ + +/* HCI Socket options */ +#define HCI_DATA_DIR 1 +#define HCI_FILTER 2 +#define HCI_TIME_STAMP 3 + +/* HCI CMSG flags */ +#define HCI_CMSG_DIR 0x0001 +#define HCI_CMSG_TSTAMP 0x0002 + +struct sockaddr_hci { + sa_family_t hci_family; + unsigned short hci_dev; + unsigned short hci_channel; +}; +#define HCI_DEV_NONE 0xffff + +#define HCI_CHANNEL_RAW 0 +#define HCI_CHANNEL_USER 1 +#define HCI_CHANNEL_MONITOR 2 +#define HCI_CHANNEL_CONTROL 3 + +struct hci_filter { + uint32_t type_mask; + uint32_t event_mask[2]; + uint16_t opcode; +}; + +#define HCI_FLT_TYPE_BITS 31 +#define HCI_FLT_EVENT_BITS 63 +#define HCI_FLT_OGF_BITS 63 +#define HCI_FLT_OCF_BITS 127 + +/* Ioctl requests structures */ +struct hci_dev_stats { + uint32_t err_rx; + uint32_t err_tx; + uint32_t cmd_tx; + uint32_t evt_rx; + uint32_t acl_tx; + uint32_t acl_rx; + uint32_t sco_tx; + uint32_t sco_rx; + uint32_t byte_rx; + uint32_t byte_tx; +}; + +struct hci_dev_info { + uint16_t dev_id; + char name[8]; + + bdaddr_t bdaddr; + + uint32_t flags; + uint8_t type; + + uint8_t features[8]; + + uint32_t pkt_type; + uint32_t link_policy; + uint32_t link_mode; + + uint16_t acl_mtu; + uint16_t acl_pkts; + uint16_t sco_mtu; + uint16_t sco_pkts; + + struct hci_dev_stats stat; +}; + +struct hci_conn_info { + uint16_t handle; + bdaddr_t bdaddr; + uint8_t type; + uint8_t out; + uint16_t state; + uint32_t link_mode; +}; + +struct hci_dev_req { + uint16_t dev_id; + uint32_t dev_opt; +}; + +struct hci_dev_list_req { + uint16_t dev_num; + struct hci_dev_req dev_req[0]; /* hci_dev_req structures */ +}; + +struct hci_conn_list_req { + uint16_t dev_id; + uint16_t conn_num; + struct hci_conn_info conn_info[0]; +}; + +struct hci_conn_info_req { + bdaddr_t bdaddr; + uint8_t type; + struct hci_conn_info conn_info[0]; +}; + +struct hci_auth_info_req { + bdaddr_t bdaddr; + uint8_t type; +}; + +struct hci_inquiry_req { + uint16_t dev_id; + uint16_t flags; + uint8_t lap[3]; + uint8_t length; + uint8_t num_rsp; +}; +#define IREQ_CACHE_FLUSH 0x0001 + +#ifdef __cplusplus +} +#endif + +#endif /* __HCI_H */ diff --git a/hci_lib.h b/hci_lib.h new file mode 100644 index 0000000..55aeb17 --- /dev/null +++ b/hci_lib.h @@ -0,0 +1,242 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifndef __HCI_LIB_H +#define __HCI_LIB_H + +#ifdef __cplusplus +extern "C" { +#endif + +struct hci_request { + uint16_t ogf; + uint16_t ocf; + int event; + void *cparam; + int clen; + void *rparam; + int rlen; +}; + +struct hci_version { + uint16_t manufacturer; + uint8_t hci_ver; + uint16_t hci_rev; + uint8_t lmp_ver; + uint16_t lmp_subver; +}; + +int hci_open_dev(int dev_id); +int hci_close_dev(int dd); +int hci_send_cmd(int dd, uint16_t ogf, uint16_t ocf, uint8_t plen, void *param); +int hci_send_req(int dd, struct hci_request *req, int timeout); + +int hci_create_connection(int dd, const bdaddr_t *bdaddr, uint16_t ptype, uint16_t clkoffset, uint8_t rswitch, uint16_t *handle, int to); +int hci_disconnect(int dd, uint16_t handle, uint8_t reason, int to); + +int hci_inquiry(int dev_id, int len, int num_rsp, const uint8_t *lap, inquiry_info **ii, long flags); +int hci_devinfo(int dev_id, struct hci_dev_info *di); +int hci_devba(int dev_id, bdaddr_t *bdaddr); +int hci_devid(const char *str); + +int hci_read_local_name(int dd, int len, char *name, int to); +int hci_write_local_name(int dd, const char *name, int to); +int hci_read_remote_name(int dd, const bdaddr_t *bdaddr, int len, char *name, int to); +int hci_read_remote_name_with_clock_offset(int dd, const bdaddr_t *bdaddr, uint8_t pscan_rep_mode, uint16_t clkoffset, int len, char *name, int to); +int hci_read_remote_name_cancel(int dd, const bdaddr_t *bdaddr, int to); +int hci_read_remote_version(int dd, uint16_t handle, struct hci_version *ver, int to); +int hci_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to); +int hci_read_remote_ext_features(int dd, uint16_t handle, uint8_t page, uint8_t *max_page, uint8_t *features, int to); +int hci_read_clock_offset(int dd, uint16_t handle, uint16_t *clkoffset, int to); +int hci_read_local_version(int dd, struct hci_version *ver, int to); +int hci_read_local_commands(int dd, uint8_t *commands, int to); +int hci_read_local_features(int dd, uint8_t *features, int to); +int hci_read_local_ext_features(int dd, uint8_t page, uint8_t *max_page, uint8_t *features, int to); +int hci_read_bd_addr(int dd, bdaddr_t *bdaddr, int to); +int hci_read_class_of_dev(int dd, uint8_t *cls, int to); +int hci_write_class_of_dev(int dd, uint32_t cls, int to); +int hci_read_voice_setting(int dd, uint16_t *vs, int to); +int hci_write_voice_setting(int dd, uint16_t vs, int to); +int hci_read_current_iac_lap(int dd, uint8_t *num_iac, uint8_t *lap, int to); +int hci_write_current_iac_lap(int dd, uint8_t num_iac, uint8_t *lap, int to); +int hci_read_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t all, int to); +int hci_write_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t *key, int to); +int hci_delete_stored_link_key(int dd, bdaddr_t *bdaddr, uint8_t all, int to); +int hci_authenticate_link(int dd, uint16_t handle, int to); +int hci_encrypt_link(int dd, uint16_t handle, uint8_t encrypt, int to); +int hci_change_link_key(int dd, uint16_t handle, int to); +int hci_switch_role(int dd, bdaddr_t *bdaddr, uint8_t role, int to); +int hci_park_mode(int dd, uint16_t handle, uint16_t max_interval, uint16_t min_interval, int to); +int hci_exit_park_mode(int dd, uint16_t handle, int to); +int hci_read_inquiry_scan_type(int dd, uint8_t *type, int to); +int hci_write_inquiry_scan_type(int dd, uint8_t type, int to); +int hci_read_inquiry_mode(int dd, uint8_t *mode, int to); +int hci_write_inquiry_mode(int dd, uint8_t mode, int to); +int hci_read_afh_mode(int dd, uint8_t *mode, int to); +int hci_write_afh_mode(int dd, uint8_t mode, int to); +int hci_read_ext_inquiry_response(int dd, uint8_t *fec, uint8_t *data, int to); +int hci_write_ext_inquiry_response(int dd, uint8_t fec, uint8_t *data, int to); +int hci_read_simple_pairing_mode(int dd, uint8_t *mode, int to); +int hci_write_simple_pairing_mode(int dd, uint8_t mode, int to); +int hci_read_local_oob_data(int dd, uint8_t *hash, uint8_t *randomizer, int to); +int hci_read_inq_response_tx_power_level(int dd, int8_t *level, int to); +int hci_read_inquiry_transmit_power_level(int dd, int8_t *level, int to); +int hci_write_inquiry_transmit_power_level(int dd, int8_t level, int to); +int hci_read_transmit_power_level(int dd, uint16_t handle, uint8_t type, int8_t *level, int to); +int hci_read_link_policy(int dd, uint16_t handle, uint16_t *policy, int to); +int hci_write_link_policy(int dd, uint16_t handle, uint16_t policy, int to); +int hci_read_link_supervision_timeout(int dd, uint16_t handle, uint16_t *timeout, int to); +int hci_write_link_supervision_timeout(int dd, uint16_t handle, uint16_t timeout, int to); +int hci_set_afh_classification(int dd, uint8_t *map, int to); +int hci_read_link_quality(int dd, uint16_t handle, uint8_t *link_quality, int to); +int hci_read_rssi(int dd, uint16_t handle, int8_t *rssi, int to); +int hci_read_afh_map(int dd, uint16_t handle, uint8_t *mode, uint8_t *map, int to); +int hci_read_clock(int dd, uint16_t handle, uint8_t which, uint32_t *clock, uint16_t *accuracy, int to); + +int hci_le_set_scan_enable(int dev_id, uint8_t enable, uint8_t filter_dup, int to); +int hci_le_set_scan_parameters(int dev_id, uint8_t type, uint16_t interval, + uint16_t window, uint8_t own_type, + uint8_t filter, int to); +int hci_le_set_advertise_enable(int dev_id, uint8_t enable, int to); +int hci_le_create_conn(int dd, uint16_t interval, uint16_t window, + uint8_t initiator_filter, uint8_t peer_bdaddr_type, + bdaddr_t peer_bdaddr, uint8_t own_bdaddr_type, + uint16_t min_interval, uint16_t max_interval, + uint16_t latency, uint16_t supervision_timeout, + uint16_t min_ce_length, uint16_t max_ce_length, + uint16_t *handle, int to); +int hci_le_conn_update(int dd, uint16_t handle, uint16_t min_interval, + uint16_t max_interval, uint16_t latency, + uint16_t supervision_timeout, int to); +int hci_le_add_white_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to); +int hci_le_rm_white_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to); +int hci_le_read_white_list_size(int dd, uint8_t *size, int to); +int hci_le_clear_white_list(int dd, int to); +int hci_le_add_resolving_list(int dd, const bdaddr_t *bdaddr, uint8_t type, + uint8_t *peer_irk, uint8_t *local_irk, int to); +int hci_le_rm_resolving_list(int dd, const bdaddr_t *bdaddr, uint8_t type, int to); +int hci_le_clear_resolving_list(int dd, int to); +int hci_le_read_resolving_list_size(int dd, uint8_t *size, int to); +int hci_le_set_address_resolution_enable(int dev_id, uint8_t enable, int to); +int hci_le_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to); + +int hci_for_each_dev(int flag, int(*func)(int dd, int dev_id, long arg), long arg); +int hci_get_route(bdaddr_t *bdaddr); + +char *hci_bustostr(int bus); +char *hci_typetostr(int type); +char *hci_dtypetostr(int type); +char *hci_dflagstostr(uint32_t flags); +char *hci_ptypetostr(unsigned int ptype); +int hci_strtoptype(char *str, unsigned int *val); +char *hci_scoptypetostr(unsigned int ptype); +int hci_strtoscoptype(char *str, unsigned int *val); +char *hci_lptostr(unsigned int ptype); +int hci_strtolp(char *str, unsigned int *val); +char *hci_lmtostr(unsigned int ptype); +int hci_strtolm(char *str, unsigned int *val); + +char *hci_cmdtostr(unsigned int cmd); +char *hci_commandstostr(uint8_t *commands, char *pref, int width); + +char *hci_vertostr(unsigned int ver); +int hci_strtover(char *str, unsigned int *ver); +char *lmp_vertostr(unsigned int ver); +int lmp_strtover(char *str, unsigned int *ver); +char *pal_vertostr(unsigned int ver); +int pal_strtover(char *str, unsigned int *ver); + +char *lmp_featurestostr(uint8_t *features, char *pref, int width); + +static inline void hci_set_bit(int nr, void *addr) +{ + *((uint32_t *) addr + (nr >> 5)) |= (1 << (nr & 31)); +} + +static inline void hci_clear_bit(int nr, void *addr) +{ + *((uint32_t *) addr + (nr >> 5)) &= ~(1 << (nr & 31)); +} + +static inline int hci_test_bit(int nr, void *addr) +{ + return *((uint32_t *) addr + (nr >> 5)) & (1 << (nr & 31)); +} + +/* HCI filter tools */ +static inline void hci_filter_clear(struct hci_filter *f) +{ + memset(f, 0, sizeof(*f)); +} +static inline void hci_filter_set_ptype(int t, struct hci_filter *f) +{ + hci_set_bit((t == HCI_VENDOR_PKT) ? 0 : (t & HCI_FLT_TYPE_BITS), &f->type_mask); +} +static inline void hci_filter_clear_ptype(int t, struct hci_filter *f) +{ + hci_clear_bit((t == HCI_VENDOR_PKT) ? 0 : (t & HCI_FLT_TYPE_BITS), &f->type_mask); +} +static inline int hci_filter_test_ptype(int t, struct hci_filter *f) +{ + return hci_test_bit((t == HCI_VENDOR_PKT) ? 0 : (t & HCI_FLT_TYPE_BITS), &f->type_mask); +} +static inline void hci_filter_all_ptypes(struct hci_filter *f) +{ + memset((void *) &f->type_mask, 0xff, sizeof(f->type_mask)); +} +static inline void hci_filter_set_event(int e, struct hci_filter *f) +{ + hci_set_bit((e & HCI_FLT_EVENT_BITS), &f->event_mask); +} +static inline void hci_filter_clear_event(int e, struct hci_filter *f) +{ + hci_clear_bit((e & HCI_FLT_EVENT_BITS), &f->event_mask); +} +static inline int hci_filter_test_event(int e, struct hci_filter *f) +{ + return hci_test_bit((e & HCI_FLT_EVENT_BITS), &f->event_mask); +} +static inline void hci_filter_all_events(struct hci_filter *f) +{ + memset((void *) f->event_mask, 0xff, sizeof(f->event_mask)); +} +static inline void hci_filter_set_opcode(int opcode, struct hci_filter *f) +{ + f->opcode = opcode; +} +static inline void hci_filter_clear_opcode(struct hci_filter *f) +{ + f->opcode = 0; +} +static inline int hci_filter_test_opcode(int opcode, struct hci_filter *f) +{ + return (f->opcode == opcode); +} + +#ifdef __cplusplus +} +#endif + +#endif /* __HCI_LIB_H */ diff --git a/io-mainloop.c b/io-mainloop.c new file mode 100644 index 0000000..16dcd1a --- /dev/null +++ b/io-mainloop.c @@ -0,0 +1,423 @@ +/** + * @file io-mainloop.c + * @brief set of function to manage io part of the mainloop feature + * @see mainloop.c + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include + +#include "mainloop.h" +#include "util.h" +#include "io.h" + +/** + * @brief data structure to manage io + */ +struct io { + int ref_count; /**< number of references to the data structure */ + int fd; /**< file descriptor */ + uint32_t events; /**< epoll events (might be ored) */ + bool close_on_destroy; /**< do you need to close the underlying socket on destroy? */ + io_callback_func_t read_callback; /**< read call back */ + io_destroy_func_t read_destroy; /**< data management for read */ + void *read_data; /**< user pointer for read data */ + io_callback_func_t write_callback; /**< write call back*/ + io_destroy_func_t write_destroy; /**< data management for write */ + void *write_data; /**< user pointer for write data */ + io_callback_func_t disconnect_callback; /**< call back on disconnect */ + io_destroy_func_t disconnect_destroy; /**< data management at disconnect */ + void *disconnect_data; /**< user pointer for disconnect */ +}; + +/** + * Increment &io->ref_count + * @param io point to io data structure + * @return + */ +static struct io *io_ref(struct io *io) +{ + if (!io) + return NULL; + + __sync_fetch_and_add(&io->ref_count, 1); + + return io; +} + +/** + * Decrement &io->ref_count and free io data structure when count reaches 0 + * @param io point to io data structure + * @return + */ +static void io_unref(struct io *io) +{ + if (!io) + return; + + if (__sync_sub_and_fetch(&io->ref_count, 1)) + return; + + free(io); +} + +/** + * io data structure house keeping + * @param user_data pointer to io data structure + */ +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; +} + +/** + * depending on events epoll event and read, write or disconnect prepare for the appropriate call back calling mainloop_modify_fd + * + * epoll event value (some 'ored' combination allowed) EPOLLRDHUP | EPOLLHUP | EPOLLERR | EPOLLIN | EPOLLOUT ) + * + * @param fd file descriptor data structure (can be socket) + * @param events epoll event + * @param user_data pointer to io data structure + */ +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); +} + +/** + * create a new io data structure + * + * @param fd file descriptor (includes socket) + * @return NULL if error or io data structure + */ +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); +} + +/** + * remove the fd from the 'mainloop' and reset all io call back, unref the io data structure + * + * @param io pointer to the io data structure + */ +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); +} + +/** + * return the fd (file descriptor, can be a socket) attached to the io data structure + * + * @param io pointer to io data structure + * @return + */ +int io_get_fd(struct io *io) +{ + if (!io) + return -ENOTCONN; + + return io->fd; +} + +/** + * + * @param io + * @param do_close + * @return + */ +bool io_set_close_on_destroy(struct io *io, bool do_close) +{ + if (!io) + return false; + + io->close_on_destroy = do_close; + + return true; +} + +/** + * + * @param io + * @param callback + * @param user_data + * @param destroy + * @return + */ +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; +} + +/** + * + * @param io + * @param callback + * @param user_data + * @param destroy + * @return + */ +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; +} + +/** + * + * @param io + * @param callback + * @param user_data + * @param destroy + * @return + */ +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; +} + +/** + * send data to the underlying socket (actual write) + * + * @param io io structure which describe the underlying socket and its context + * @param iov pointer to pdu, pdu length structure + * @param iovcnt number of pdu to send (iov is an array) + * @return The return value is a count of bytes written, or -errno indicating an error. + */ +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; +} + +/** + * + * @param io + * @return + */ +bool io_shutdown(struct io *io) +{ + if (!io || io->fd < 0) + return false; + + return shutdown(io->fd, SHUT_RDWR) == 0; +} diff --git a/io.h b/io.h new file mode 100644 index 0000000..8bc1111 --- /dev/null +++ b/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/json.c b/json.c new file mode 100644 index 0000000..6e8840c --- /dev/null +++ b/json.c @@ -0,0 +1,887 @@ +#include +#include +#include +#include +#include +#include "json.h" + + +/* + read and traverse +*/ + +static void skipWhiteSpaces(const char** pstr) +{ + while (isspace(**pstr)) + ++*pstr; +} + +static void fillError(const char* str, char* errorBuf, int errorBufSize) +{ + static const char errorPrefix[] = "Parse error here: "; + static const int errorPrefixLen = sizeof(errorPrefix); + + snprintf(errorBuf, errorBufSize, "%s", errorPrefix); + snprintf(errorBuf + errorPrefixLen - 1, errorBufSize - errorPrefixLen, "%s", str); +} + +static int parseChar(const char **source, char c) +{ + skipWhiteSpaces(source); + if (!**source || tolower(**source) != tolower(c)) + return -1; + + ++*source; + return 0; +} + +static int createStringCopy(const char *source, int sourceLen, char **target) +{ + if (sourceLen == -1) + sourceLen = strlen(source); + + *target = malloc(sourceLen + 1); + + if (!*target) + return -1; + + memcpy(*target, source, sourceLen); + (*target)[sourceLen] = '\0'; + + return 0; +} + + +static int parseQuotedString(const char **source, char **target) +{ + const char *begin; + int strLen; + + skipWhiteSpaces(source); + if (parseChar(source, '"') != 0) + return -1; + + begin = *source; + while (**source != '"' && **source) + ++*source; + + if (!**source) + return -1; + + strLen = *source - begin; + if (createStringCopy(begin, strLen, target) != 0) + return -1; + + ++*source; + + return 0; +} + +static int matchString(const char **source, const char *pattern) +{ + while (*pattern) + { + if (parseChar(source, *pattern) != 0) + return -1; + ++pattern; + } + + return 0; +} + +static int reallocObject(struct JsonVal *val) +{ + if (!(val->u.object.keys = realloc( + val->u.object.keys, + (val->u.object.len + 1) * sizeof(*val->u.object.keys)))) + { + return -1; + } + + if (!(val->u.object.values = realloc( + val->u.object.values, + (val->u.object.len + 1) * sizeof(*val->u.object.values)))) + { + return -1; + } + + val->u.object.len++; + return 0; +} + +static int reallocArray(struct JsonVal *val) +{ + if (!(val->u.array.values = realloc( + val->u.array.values, + (val->u.array.len + 1) * sizeof(*val->u.array.values)))) + { + return -1; + } + + val->u.array.len++; + return 0; +} + +static int parseObject(const char **source, struct JsonVal *value); +static int parseValue(const char **source, struct JsonVal *val); + +static int parseArrayEntry(const char **source, struct JsonVal *val) +{ + if(reallocArray(val) != 0) + return -1; + + skipWhiteSpaces(source); + return parseValue(source, val->u.array.values + val->u.array.len - 1); +} + +static int parseArray(const char **source, struct JsonVal *value) +{ + int finished = 0; + + value->type = jsonArrayT; + value->u.array.values = NULL; + value->u.array.len = 0; + + if (parseChar(source, '[') != 0) + return -1; + + skipWhiteSpaces(source); + if (**source == ']') + { + ++*source; + return 0; + } + + while (**source) + { + skipWhiteSpaces(source); + if (parseArrayEntry(source, value) != 0) + return -1; + skipWhiteSpaces(source); + if (**source == ',') + { + ++*source; + continue; + } + if (**source == ']') + { + ++*source; + finished = 1; + break; + } + return -1; + } + + return finished ? 0 : -1; +} + +static int parseString(const char **source, struct JsonVal *value) +{ + value->type = jsonStringT; + return parseQuotedString(source, &value->u.string); +} + +static int parseTrue(const char **source, struct JsonVal *value) +{ + value->type = jsonTrueT; + return matchString(source, "true"); +} + +static int parseFalse(const char **source, struct JsonVal *value) +{ + value->type = jsonFalseT; + return matchString(source, "false"); +} + +static int parseNum(const char **source, struct JsonVal *value) +{ + value->type = jsonNumberT; + value->u.number = strtold(*source, (char**)source); + + return 0; +} + +static int parseNull(const char **source, struct JsonVal *value) +{ + value->type = jsonNullT; + return matchString(source, "null"); +} + +static int (*getParser(char c))(const char **, struct JsonVal*) +{ + switch (tolower(c)) + { + case '[': + return &parseArray; + case '{': + return &parseObject; + case '"': + return &parseString; + case 'f': + return &parseFalse; + case 't': + return &parseTrue; + default: + if (isdigit(c)) + return &parseNum; + else if (tolower(c) == 'n') + return &parseNull; + return NULL; + } + + return NULL; +} + +static int parseValue(const char **source, struct JsonVal *val) +{ + int (*valueParser)(const char **str, struct JsonVal *val) = getParser(**source); + + if (valueParser == NULL) + return -1; + + return valueParser(source, val); +} + +static int parseKeyValue(const char **source, struct JsonVal *val) +{ + assert(val->type == jsonObjectT); + if(reallocObject(val) != 0) + return -1; + + if (parseQuotedString(source, val->u.object.keys + val->u.object.len - 1) != 0) + return -1; + + if (parseChar(source, ':') != 0) + return -1; + + skipWhiteSpaces(source); + return parseValue(source, val->u.object.values + val->u.object.len - 1); +} + +static int parseObject(const char **source, struct JsonVal *value) +{ + int finished = 0; + + value->type = jsonObjectT; + value->u.object.keys = NULL; + value->u.object.values = NULL; + value->u.object.len = 0; + + skipWhiteSpaces(source); + + if (parseChar(source, '{') != 0) + return -1; + + skipWhiteSpaces(source); + if (**source == '}') + { + ++*source; + return 0; + } + + while (**source) + { + skipWhiteSpaces(source); + if (parseKeyValue(source, value) != 0) + return -1; + skipWhiteSpaces(source); + if (**source == ',') + { + ++*source; + continue; + } + if (**source == '}') + { + ++*source; + finished = 1; + break; + } + return -1; + } + + return finished ? 0 : -1; +} + +struct JsonVal jsonParseString(const char *str, char *errorBuf, int errorBufSize) +{ + struct JsonVal result; + const char *_str = str; + + result = (struct JsonVal){jsonNullT}; + if (parseObject(&_str, &result) != 0) + fillError(_str, errorBuf, errorBufSize); + else + memset(errorBuf, 0, errorBufSize); + + return result; +} + +int JsonVal_isString(const struct JsonVal *val) +{ + return val->type == jsonStringT; +} + +int JsonVal_isNumber(const struct JsonVal *val) +{ + return val->type == jsonNumberT; +} + +int JsonVal_isObject(const struct JsonVal *val) +{ + return val->type == jsonObjectT; +} + +int JsonVal_isArray(const struct JsonVal *val) +{ + return val->type == jsonArrayT; +} + +int JsonVal_isTrue(const struct JsonVal *val) +{ + return val->type == jsonTrueT; +} + +int JsonVal_isFalse(const struct JsonVal *val) +{ + return val->type == jsonFalseT; +} + +int JsonVal_isNull(const struct JsonVal *val) +{ + return val->type == jsonNullT; +} + +int JsonVal_arrayLen(struct JsonVal *val) +{ + assert(val->type == jsonArrayT); + if (val->type != jsonArrayT) + return -1; + + return val->u.array.len; +} + +struct JsonVal *JsonVal_arrayAt(struct JsonVal *val, int index) +{ + assert(val->type == jsonArrayT); + if (val->type != jsonArrayT || index >= val->u.array.len) + return NULL; + + return &val->u.array.values[index]; +} + +struct JsonVal *JsonVal_getObjectValueByKey(const struct JsonVal *val, const char *key) +{ + int i; + + assert(val->type == jsonObjectT); + if (val->type != jsonObjectT) + return NULL; + + for (i = 0; i < val->u.object.len; ++i) + { + if (strcmp(key, val->u.object.keys[i]) == 0) + return &val->u.object.values[i]; + } + + return NULL; +} + +void JsonVal_forEachArrayElement( + const struct JsonVal *val, + void *ctx, + void (*action)(void *, const struct JsonVal *)) +{ + int i; + + assert(val->type == jsonArrayT); + if (val->type != jsonArrayT) + return; + + for (i = 0; i < val->u.array.len; ++i) + action(ctx, &val->u.array.values[i]); +} + +void JsonVal_forEachObjectElement( + const struct JsonVal *val, + void *ctx, + void (*action)(void *, const char *key, const struct JsonVal *)) +{ + int i; + + assert(val->type == jsonObjectT); + if (val->type != jsonObjectT) + return; + + for (i = 0; i < val->u.object.len; ++i) + action(ctx, val->u.object.keys[i], &val->u.object.values[i]); +} + +struct JsonVal jsonCreateObject() +{ + struct JsonVal result; + + result.type = jsonObjectT; + memset(&result.u, 0, sizeof(result.u)); + return result; +} + +static struct JsonVal *prepareNewObjectVal(struct JsonVal *val, const char *key) +{ + struct JsonVal *newValPtr; + + assert(val->type == jsonObjectT); + if (val->type != jsonObjectT) + return NULL; + + if (reallocObject(val) != 0) + return NULL; + + if (createStringCopy(key, strlen(key), val->u.object.keys + val->u.object.len - 1) != 0) + return NULL; + + newValPtr = val->u.object.values + val->u.object.len - 1; + memset(newValPtr, 0, sizeof(*newValPtr)); + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddString(struct JsonVal *val, const char *key, const char *value) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + if (createStringCopy(value, strlen(value), &newValPtr->u.string) != 0) + return NULL; + + newValPtr->type = jsonStringT; + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddNumber(struct JsonVal *val, const char *key, long double number) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + newValPtr->u.number = number; + newValPtr->type = jsonNumberT; + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddObject(struct JsonVal *val, const char *key) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonObjectT; + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddArray(struct JsonVal *val, const char *key) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonArrayT; + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddTrue(struct JsonVal *val, const char *key) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonTrueT; + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddFalse(struct JsonVal *val, const char *key) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonFalseT; + + return newValPtr; +} + +struct JsonVal *JsonVal_objectAddNull(struct JsonVal *val, const char *key) +{ + struct JsonVal *newValPtr = prepareNewObjectVal(val, key); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonNullT; + + return newValPtr; +} + +static struct JsonVal *prepareNewArrayVal(struct JsonVal *val) +{ + struct JsonVal *newValPtr; + + assert(val->type == jsonArrayT); + if (val->type != jsonArrayT) + return NULL; + + if (reallocArray(val) != 0) + return NULL; + + newValPtr = val->u.array.values + val->u.array.len - 1; + memset(newValPtr, 0, sizeof(*newValPtr)); + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddString(struct JsonVal *val, const char *value) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + if (createStringCopy(value, strlen(value), &newValPtr->u.string) != 0) + return NULL; + + newValPtr->type = jsonStringT; + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddNumber(struct JsonVal *val, long double number) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonNumberT; + newValPtr->u.number = number; + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddObject(struct JsonVal *val) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonObjectT; + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddArray(struct JsonVal *val) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonArrayT; + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddTrue(struct JsonVal *val) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonTrueT; + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddFalse(struct JsonVal *val) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonFalseT; + + return newValPtr; +} + +struct JsonVal *JsonVal_arrayAddNull(struct JsonVal *val) +{ + struct JsonVal *newValPtr = prepareNewArrayVal(val); + + if (newValPtr == NULL) + return NULL; + + newValPtr->type = jsonNullT; + + return newValPtr; +} + +/* + write +*/ + +struct Buf +{ + char *buf; + int spaceLeft; +}; + +static const int kWriteBufSize = 1024; + +static int writeToBuf(void *ctx, void *source, int len) +{ + struct Buf *target = (struct Buf *)ctx; + int bytesToWrite = len >= target->spaceLeft ? target->spaceLeft : len; + + memcpy(target->buf, source, bytesToWrite); + target->spaceLeft -= bytesToWrite; + target->buf += bytesToWrite; + + return bytesToWrite; +} + +static void writeChar( + char c, void *ctx, + char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + writeBuf[0] = c; + writeFunc(ctx, writeBuf, 1); +} + +static void writeString( + const char *s, void *ctx, + char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + int writeLen = strlen(s); + int bytesToWrite; + + while (writeLen > 0) + { + bytesToWrite = writeLen < kWriteBufSize ? writeLen : kWriteBufSize; + writeFunc(ctx, (void *)s, bytesToWrite); + writeLen -= bytesToWrite; + s += bytesToWrite; + } +} + +static void writeObject( + const struct JsonVal *val, + void *ctx, char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)); + +static void writeValue( + const struct JsonVal *val, + void *ctx, char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)); + +static void writeQuotedString( + const char *s, void *ctx, + char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + writeBuf[0] = '"'; + writeFunc(ctx, writeBuf, 1); + + writeString(s, ctx, writeBuf, writeFunc); + + writeBuf[0] = '"'; + writeFunc(ctx, writeBuf, 1); +} + +static void writeNumber( + long double num, + void *ctx, char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + char *dot; + char *end; + + sprintf(writeBuf, "%.5Lf", num); + dot = strchr(writeBuf, '.'); + end = writeBuf + strlen(writeBuf) - 1; + + for (; end != dot; --end) + if (*end != '0') + break; + + if (end == dot) + *end = '\0'; + else + *(++end) ='\0'; + + writeString(writeBuf, ctx, NULL, writeFunc); +} + +static void writeArray( + const struct JsonVal *val, + void *ctx, char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + int i; + + writeChar('[', ctx, writeBuf, writeFunc); + + for (i = 0; i < val->u.array.len; ++i) + { + writeValue(&val->u.array.values[i], ctx, writeBuf, writeFunc); + if (i != val->u.array.len - 1) + writeChar(',', ctx, writeBuf, writeFunc); + } + + writeChar(']', ctx, writeBuf, writeFunc); +} + +static void writeTrue(void *ctx, char *writeBuf, int (*writeFunc)(void *ctx, void *buf, int len)) +{ + writeString("true", ctx, writeBuf, writeFunc); +} + +static void writeFalse(void *ctx, char *writeBuf, int (*writeFunc)(void *ctx, void *buf, int len)) +{ + writeString("false", ctx, writeBuf, writeFunc); +} + +static void writeNull(void *ctx, char *writeBuf, int (*writeFunc)(void *ctx, void *buf, int len)) +{ + writeString("null", ctx, writeBuf, writeFunc); +} + +static void writeValue( + const struct JsonVal *val, + void *ctx, char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + switch (val->type) + { + case jsonStringT: + writeQuotedString(val->u.string, ctx, writeBuf, writeFunc); + break; + case jsonNumberT: + writeNumber(val->u.number, ctx, writeBuf, writeFunc); + break; + case jsonObjectT: + writeObject(val, ctx, writeBuf, writeFunc); + break; + case jsonArrayT: + writeArray(val, ctx, writeBuf, writeFunc); + break; + case jsonTrueT: + writeTrue(ctx, writeBuf, writeFunc); + break; + case jsonFalseT: + writeFalse(ctx, writeBuf, writeFunc); + break; + case jsonNullT: + writeNull(ctx, writeBuf, writeFunc); + break; + } +} + +static void writeObject( + const struct JsonVal *val, + void *ctx, char *writeBuf, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + int i; + + writeChar('{', ctx, writeBuf, writeFunc); + + for (i = 0; i < val->u.object.len; ++i) + { + writeQuotedString(val->u.object.keys[i], ctx, writeBuf, writeFunc); + writeChar(':', ctx, writeBuf, writeFunc); + writeValue(&val->u.object.values[i], ctx, writeBuf, writeFunc); + + if (i != val->u.object.len - 1) + writeChar(',', ctx, writeBuf, writeFunc); + } + + writeChar('}', ctx, writeBuf, writeFunc); +} + +struct CountingContext +{ + void *userCtx; + int (*userWriteFunc)(void *ctx, void *buf, int len); + int totalWrittenBytes; +}; + +static int countingWriteWrapper(void *ctx, void *buf, int len) +{ + struct CountingContext *countingCtx = (struct CountingContext *)ctx; + countingCtx->totalWrittenBytes += len; + + return countingCtx->userWriteFunc(countingCtx->userCtx, buf, len); +} + +int JsonVal_write( + const struct JsonVal *val, void *ctx, + int (*writeFunc)(void *ctx, void *buf, int len)) +{ + char writeBuf[kWriteBufSize]; + struct CountingContext countingCtx = {ctx, writeFunc, 0}; + + writeObject(val, &countingCtx, writeBuf, &countingWriteWrapper); + writeBuf[0] = '\0'; + countingWriteWrapper(&countingCtx, writeBuf, 1); + + return countingCtx.totalWrittenBytes; +} + +int JsonVal_writeString(const struct JsonVal *val, char *buf, int len) +{ + struct Buf ctx = {buf, len}; + return JsonVal_write(val, &ctx, &writeToBuf); +} + +void JsonVal_destroy(struct JsonVal *val) +{ + int i; + + switch (val->type) + { + case jsonStringT: + free(val->u.string); + break; + case jsonObjectT: + for (i = 0; i < val->u.object.len; ++i) + { + JsonVal_destroy(&val->u.object.values[i]); + free(val->u.object.keys[i]); + } + free(val->u.object.values); + free(val->u.object.keys); + break; + case jsonArrayT: + for (i = 0; i < val->u.array.len; ++i) + JsonVal_destroy(&val->u.array.values[i]); + free(val->u.array.values); + break; + default: + break; + } +} diff --git a/json.h b/json.h new file mode 100644 index 0000000..c2087af --- /dev/null +++ b/json.h @@ -0,0 +1,94 @@ +#pragma once + +#include + +enum JsonType +{ + jsonStringT, + jsonNumberT, + jsonObjectT, + jsonArrayT, + jsonTrueT, + jsonFalseT, + jsonNullT, +}; + +struct JsonVal +{ + enum JsonType type; + union + { + char *string; + long double number; + struct { + char **keys; + struct JsonVal *values; + int len; + } object; + struct { + struct JsonVal *values; + int len; + } array; + } u; +}; + + +struct JsonVal jsonParseString(const char *str, char *errorBuf, int errorBufSize); + +/*creates empty (top-level) object*/ +struct JsonVal jsonCreateObject(); + +/*check type functions*/ +int JsonVal_isString(const struct JsonVal *val); +int JsonVal_isNumber(const struct JsonVal *val); +int JsonVal_isObject(const struct JsonVal *val); +int JsonVal_isArray(const struct JsonVal *val); +int JsonVal_isTrue(const struct JsonVal *val); +int JsonVal_isFalse(const struct JsonVal *val); +int JsonVal_isNull(const struct JsonVal *val); + +/*add subvalues to existing values. return pointers to the newly created values.*/ +struct JsonVal *JsonVal_objectAddString(struct JsonVal *val, const char *key, const char *value); +struct JsonVal *JsonVal_objectAddNumber(struct JsonVal *val, const char *key, long double number); +struct JsonVal *JsonVal_objectAddObject(struct JsonVal *val, const char *key); +struct JsonVal *JsonVal_objectAddArray(struct JsonVal *val, const char *key); +struct JsonVal *JsonVal_objectAddTrue(struct JsonVal *val, const char *key); +struct JsonVal *JsonVal_objectAddFalse(struct JsonVal *val, const char *key); +struct JsonVal *JsonVal_objectAddNull(struct JsonVal *val, const char *key); + +struct JsonVal *JsonVal_arrayAddString(struct JsonVal *val, const char *value); +struct JsonVal *JsonVal_arrayAddNumber(struct JsonVal *val, long double number); +struct JsonVal *JsonVal_arrayAddObject(struct JsonVal *val); +struct JsonVal *JsonVal_arrayAddArray(struct JsonVal *val); +struct JsonVal *JsonVal_arrayAddTrue(struct JsonVal *val); +struct JsonVal *JsonVal_arrayAddFalse(struct JsonVal *val); +struct JsonVal *JsonVal_arrayAddNull(struct JsonVal *val); + +/*access functions*/ +int JsonVal_arrayLen(struct JsonVal *val); +struct JsonVal *JsonVal_arrayAt(struct JsonVal* val, int index); + +struct JsonVal *JsonVal_getObjectValueByKey(const struct JsonVal *val, const char *key); + +void JsonVal_forEachArrayElement( + const struct JsonVal *val, + void *ctx, + void (*action)(void *, const struct JsonVal *)); + +void JsonVal_forEachObjectElement( + const struct JsonVal *val, + void *ctx, + void (*action)(void *, const char *key, const struct JsonVal *)); + +/*write functions*/ +int JsonVal_write( + const struct JsonVal *val, + /*User context. Will be passed to the writeFunc callback.*/ + void *ctx, + /*user supplied write callback function.*/ + int (*writeFunc)(void *ctx, void *buf, int len)); + +/*returns total bytes required*/ +int JsonVal_writeString(const struct JsonVal *val, char *buf, int len); + +void JsonVal_destroy(struct JsonVal *val); diff --git a/l2cap.h b/l2cap.h new file mode 100644 index 0000000..5ce94c4 --- /dev/null +++ b/l2cap.h @@ -0,0 +1,279 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2000-2001 Qualcomm Incorporated + * Copyright (C) 2002-2003 Maxim Krasnyansky + * Copyright (C) 2002-2010 Marcel Holtmann + * Copyright (c) 2012 Code Aurora Forum. All rights reserved. + * + * + * 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 __L2CAP_H +#define __L2CAP_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* L2CAP defaults */ +#define L2CAP_DEFAULT_MTU 672 +#define L2CAP_DEFAULT_FLUSH_TO 0xFFFF + +/* L2CAP socket address */ +struct sockaddr_l2 { + sa_family_t l2_family; + unsigned short l2_psm; + bdaddr_t l2_bdaddr; + unsigned short l2_cid; + uint8_t l2_bdaddr_type; +}; + +/* L2CAP socket options */ +#define L2CAP_OPTIONS 0x01 +struct l2cap_options { + uint16_t omtu; + uint16_t imtu; + uint16_t flush_to; + uint8_t mode; + uint8_t fcs; + uint8_t max_tx; + uint16_t txwin_size; +}; + +#define L2CAP_CONNINFO 0x02 +struct l2cap_conninfo { + uint16_t hci_handle; + uint8_t dev_class[3]; +}; + +#define L2CAP_LM 0x03 +#define L2CAP_LM_MASTER 0x0001 +#define L2CAP_LM_AUTH 0x0002 +#define L2CAP_LM_ENCRYPT 0x0004 +#define L2CAP_LM_TRUSTED 0x0008 +#define L2CAP_LM_RELIABLE 0x0010 +#define L2CAP_LM_SECURE 0x0020 + +/* L2CAP command codes */ +#define L2CAP_COMMAND_REJ 0x01 +#define L2CAP_CONN_REQ 0x02 +#define L2CAP_CONN_RSP 0x03 +#define L2CAP_CONF_REQ 0x04 +#define L2CAP_CONF_RSP 0x05 +#define L2CAP_DISCONN_REQ 0x06 +#define L2CAP_DISCONN_RSP 0x07 +#define L2CAP_ECHO_REQ 0x08 +#define L2CAP_ECHO_RSP 0x09 +#define L2CAP_INFO_REQ 0x0a +#define L2CAP_INFO_RSP 0x0b +#define L2CAP_CREATE_REQ 0x0c +#define L2CAP_CREATE_RSP 0x0d +#define L2CAP_MOVE_REQ 0x0e +#define L2CAP_MOVE_RSP 0x0f +#define L2CAP_MOVE_CFM 0x10 +#define L2CAP_MOVE_CFM_RSP 0x11 + +/* L2CAP extended feature mask */ +#define L2CAP_FEAT_FLOWCTL 0x00000001 +#define L2CAP_FEAT_RETRANS 0x00000002 +#define L2CAP_FEAT_BIDIR_QOS 0x00000004 +#define L2CAP_FEAT_ERTM 0x00000008 +#define L2CAP_FEAT_STREAMING 0x00000010 +#define L2CAP_FEAT_FCS 0x00000020 +#define L2CAP_FEAT_EXT_FLOW 0x00000040 +#define L2CAP_FEAT_FIXED_CHAN 0x00000080 +#define L2CAP_FEAT_EXT_WINDOW 0x00000100 +#define L2CAP_FEAT_UCD 0x00000200 + +/* L2CAP fixed channels */ +#define L2CAP_FC_L2CAP 0x02 +#define L2CAP_FC_CONNLESS 0x04 +#define L2CAP_FC_A2MP 0x08 + +/* L2CAP structures */ +typedef struct { + uint16_t len; + uint16_t cid; +} __attribute__ ((packed)) l2cap_hdr; +#define L2CAP_HDR_SIZE 4 + +typedef struct { + uint8_t code; + uint8_t ident; + uint16_t len; +} __attribute__ ((packed)) l2cap_cmd_hdr; +#define L2CAP_CMD_HDR_SIZE 4 + +typedef struct { + uint16_t reason; +} __attribute__ ((packed)) l2cap_cmd_rej; +#define L2CAP_CMD_REJ_SIZE 2 + +typedef struct { + uint16_t psm; + uint16_t scid; +} __attribute__ ((packed)) l2cap_conn_req; +#define L2CAP_CONN_REQ_SIZE 4 + +typedef struct { + uint16_t dcid; + uint16_t scid; + uint16_t result; + uint16_t status; +} __attribute__ ((packed)) l2cap_conn_rsp; +#define L2CAP_CONN_RSP_SIZE 8 + +/* connect result */ +#define L2CAP_CR_SUCCESS 0x0000 +#define L2CAP_CR_PEND 0x0001 +#define L2CAP_CR_BAD_PSM 0x0002 +#define L2CAP_CR_SEC_BLOCK 0x0003 +#define L2CAP_CR_NO_MEM 0x0004 + +/* connect status */ +#define L2CAP_CS_NO_INFO 0x0000 +#define L2CAP_CS_AUTHEN_PEND 0x0001 +#define L2CAP_CS_AUTHOR_PEND 0x0002 + +typedef struct { + uint16_t dcid; + uint16_t flags; + uint8_t data[0]; +} __attribute__ ((packed)) l2cap_conf_req; +#define L2CAP_CONF_REQ_SIZE 4 + +typedef struct { + uint16_t scid; + uint16_t flags; + uint16_t result; + uint8_t data[0]; +} __attribute__ ((packed)) l2cap_conf_rsp; +#define L2CAP_CONF_RSP_SIZE 6 + +#define L2CAP_CONF_SUCCESS 0x0000 +#define L2CAP_CONF_UNACCEPT 0x0001 +#define L2CAP_CONF_REJECT 0x0002 +#define L2CAP_CONF_UNKNOWN 0x0003 +#define L2CAP_CONF_PENDING 0x0004 +#define L2CAP_CONF_EFS_REJECT 0x0005 + +typedef struct { + uint8_t type; + uint8_t len; + uint8_t val[0]; +} __attribute__ ((packed)) l2cap_conf_opt; +#define L2CAP_CONF_OPT_SIZE 2 + +#define L2CAP_CONF_MTU 0x01 +#define L2CAP_CONF_FLUSH_TO 0x02 +#define L2CAP_CONF_QOS 0x03 +#define L2CAP_CONF_RFC 0x04 +#define L2CAP_CONF_FCS 0x05 +#define L2CAP_CONF_EFS 0x06 +#define L2CAP_CONF_EWS 0x07 + +#define L2CAP_CONF_MAX_SIZE 22 + +#define L2CAP_MODE_BASIC 0x00 +#define L2CAP_MODE_RETRANS 0x01 +#define L2CAP_MODE_FLOWCTL 0x02 +#define L2CAP_MODE_ERTM 0x03 +#define L2CAP_MODE_STREAMING 0x04 + +#define L2CAP_SERVTYPE_NOTRAFFIC 0x00 +#define L2CAP_SERVTYPE_BESTEFFORT 0x01 +#define L2CAP_SERVTYPE_GUARANTEED 0x02 + +typedef struct { + uint16_t dcid; + uint16_t scid; +} __attribute__ ((packed)) l2cap_disconn_req; +#define L2CAP_DISCONN_REQ_SIZE 4 + +typedef struct { + uint16_t dcid; + uint16_t scid; +} __attribute__ ((packed)) l2cap_disconn_rsp; +#define L2CAP_DISCONN_RSP_SIZE 4 + +typedef struct { + uint16_t type; +} __attribute__ ((packed)) l2cap_info_req; +#define L2CAP_INFO_REQ_SIZE 2 + +typedef struct { + uint16_t type; + uint16_t result; + uint8_t data[0]; +} __attribute__ ((packed)) l2cap_info_rsp; +#define L2CAP_INFO_RSP_SIZE 4 + +/* info type */ +#define L2CAP_IT_CL_MTU 0x0001 +#define L2CAP_IT_FEAT_MASK 0x0002 + +/* info result */ +#define L2CAP_IR_SUCCESS 0x0000 +#define L2CAP_IR_NOTSUPP 0x0001 + +typedef struct { + uint16_t psm; + uint16_t scid; + uint8_t id; +} __attribute__ ((packed)) l2cap_create_req; +#define L2CAP_CREATE_REQ_SIZE 5 + +typedef struct { + uint16_t dcid; + uint16_t scid; + uint16_t result; + uint16_t status; +} __attribute__ ((packed)) l2cap_create_rsp; +#define L2CAP_CREATE_RSP_SIZE 8 + +typedef struct { + uint16_t icid; + uint8_t id; +} __attribute__ ((packed)) l2cap_move_req; +#define L2CAP_MOVE_REQ_SIZE 3 + +typedef struct { + uint16_t icid; + uint16_t result; +} __attribute__ ((packed)) l2cap_move_rsp; +#define L2CAP_MOVE_RSP_SIZE 4 + +typedef struct { + uint16_t icid; + uint16_t result; +} __attribute__ ((packed)) l2cap_move_cfm; +#define L2CAP_MOVE_CFM_SIZE 4 + +typedef struct { + uint16_t icid; +} __attribute__ ((packed)) l2cap_move_cfm_rsp; +#define L2CAP_MOVE_CFM_RSP_SIZE 2 + +#ifdef __cplusplus +} +#endif + +#endif /* __L2CAP_H */ diff --git a/main.c b/main.c new file mode 100644 index 0000000..dc85335 --- /dev/null +++ b/main.c @@ -0,0 +1,936 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "mqtt.h" +#include "client.h" +#include +#include "bluetooth.h" +#include "hci.h" +#include "hci_lib.h" +#include "l2cap.h" +#include "uuid.h" +#include +#include "mainloop.h" +#include "util.h" +#include "att.h" +#include "queue.h" +#include "gatt-db.h" +#include "gatt-client.h" +#include "json.h" + +#define ATT_CID 4 +#define PRLOG(...) \ + printf(__VA_ARGS__); + +#define JSONOUTLEN 4096 +#define MQTT_SEND_PERIOD_S 10 + +static uint16_t mqtt_port = 1883; +static uint16_t send_interval = MQTT_SEND_PERIOD_S; +static bool verbose = false; +static int handler_registered=0; +struct client *cli; +uint8_t rcv_buf[256]; +char jsonout[JSONOUTLEN]; +struct JsonVal top; +uint8_t rcv_ptr=0, frame_len=0; +uint16_t addr_base=0; +char mqtt_buffer[BUFFER_SIZE_BYTES]; +client_t c; +//int is_subscriber = 1; +int keepalive_sec = 4; +int nbytes; + +struct client { + /// socket + int fd; + /// pointer to a bt_att structure + struct bt_att *att; + /// pointer to a gatt_db structure + struct gatt_db *db; + /// pointer to a bt_gatt_client structure + struct bt_gatt_client *gatt; + /// session id + unsigned int reliable_session_id; +}; + + + +static void got_connection(client_t* psClnt) +{ + uint8_t buf[128]; + require(psClnt != 0); + if(!psClnt->sockfd) return; + + printf("MQTT: connected to server.\n"); + nbytes = mqtt_encode_connect_msg2(buf, 0x02, keepalive_sec, (uint8_t*)"Solarlife", 9); + + client_send(&c, (char*)buf, nbytes); +} + +static void lost_connection(client_t* psClnt) +{ + require(psClnt != 0); + printf("MQTT: disconnected from server.\n"); + mainloop_quit(); +} + +static void got_data(client_t* psClnt, unsigned char* data, int nbytes) +{ + require(psClnt != 0); + + uint8_t ctrl = data[0] >> 4; + if (ctrl == CTRL_PUBACK) { + printf("MQTT server acknowledged data\n"); + } +} + + + + +int16_t b2int (uint8_t *ptr) { + + return ptr[1] | ptr[0] << 8; + +} + + +int b2int2 (uint8_t *ptr) { + + return ptr[0] | ptr[1] << 8; + +} + + +uint16_t ModRTU_CRC(uint8_t* buf, int len) +{ + uint16_t crc = 0xFFFF; + + for (int pos = 0; pos < len; pos++) { + crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc + + for (int i = 8; i != 0; i--) { // Loop over each bit + if ((crc & 0x0001) != 0) { // If the LSB is set + crc >>= 1; // Shift right and XOR 0xA001 + crc ^= 0xA001; + } + else // Else LSB is not set + crc >>= 1; // Just shift right + } + } + // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes) + return crc; +} + + +static void write_cb(bool success, uint8_t att_ecode, void *user_data) +{ +/* + if (success) { + PRLOG("\nWrite successful\n"); + } else { + PRLOG("\nWrite failed: %s (0x%02x)\n", + ecode_to_string(att_ecode), att_ecode); + } +*/ +} + + + +int send_modbus_req(uint8_t eq_id, uint8_t fct_code,uint16_t addr, uint16_t len) { + + uint8_t buf[10]; + + + buf[0]=eq_id; + buf[1]=fct_code; + buf[2]=addr>>8; + buf[3]=addr&0xff; + buf[4]=len>>8; + buf[5]=len&0xff; + uint16_t crc = ModRTU_CRC(buf,6); + buf[7]=crc>>8; + buf[6]=crc&0xff; + addr_base = addr; + + rcv_ptr = 0; //reset pointer + top = jsonCreateObject(); + + if (!bt_gatt_client_write_value(cli->gatt, 0x14, buf, 8, write_cb, NULL, NULL)) { + printf("Failed to initiate write procedure\n"); + return 1; + } + + return 0; + + } + + +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); + + PRLOG("%s - UUID: %s start: 0x%04x end: 0x%04x\n", str, uuid_str, + start, end); +} + + + + +static void register_notify_cb(uint16_t att_ecode, void *user_data) +{ + if (att_ecode) { + PRLOG("Failed to register notify handler " + "- error code: 0x%02x\n", att_ecode); + return; + } + + PRLOG("Registered notify handler!\n"); + handler_registered = 1; +} + +void parse_val(uint8_t *buf, uint16_t addr) { + + //printf("Parsing value at address %04x\n",addr); + uint8_t ptr = addr*2+3; + addr+=addr_base; + + switch (addr) { + + case 0x3000: + JsonVal_objectAddNumber(&top, "PV_rated_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x3001: + JsonVal_objectAddNumber(&top, "PV_rated_current", (float)b2int(buf+ptr)/100); + break; + case 0x3002: + JsonVal_objectAddNumber(&top, "PV_rated_power_l", (float)b2int(buf+ptr)/100); + break; + case 0x3003: + JsonVal_objectAddNumber(&top, "PV_rated_power_h", (float)b2int(buf+ptr)/100); + break; + + case 0x3004: + JsonVal_objectAddNumber(&top, "battery_rated_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x3005: + JsonVal_objectAddNumber(&top, "battery_rated_current", (float)b2int(buf+ptr)/100); + break; + case 0x3006: + JsonVal_objectAddNumber(&top, "battery_rated_power_l", (float)b2int(buf+ptr)/100); + break; + case 0x3007: + JsonVal_objectAddNumber(&top, "battery_rated_power_h", (float)b2int(buf+ptr)/100); + break; + + case 0x3008: + JsonVal_objectAddNumber(&top, "load_rated_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x3009: + JsonVal_objectAddNumber(&top, "load_rated_current", (float)b2int(buf+ptr)/100); + break; + case 0x300a: + JsonVal_objectAddNumber(&top, "load_rated_power_l", (float)b2int(buf+ptr)/100); + break; + case 0x300b: + JsonVal_objectAddNumber(&top, "load_rated_power_h", (float)b2int(buf+ptr)/100); + break; + + + case 0x3030: + JsonVal_objectAddNumber(&top, "slave_id", (int)b2int(buf+ptr)); + break; + case 0x3031: + JsonVal_objectAddNumber(&top, "running_days", (int)b2int(buf+ptr)); + break; + case 0x3032: + JsonVal_objectAddNumber(&top, "sys_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x3033: + JsonVal_objectAddNumber(&top, "battery_status", b2int(buf+ptr)); + break; + case 0x3034: + JsonVal_objectAddNumber(&top, "charge_status", b2int(buf+ptr)); + break; + case 0x3035: + JsonVal_objectAddNumber(&top, "discharge_status", b2int(buf+ptr)); + break; + case 0x3036: + JsonVal_objectAddNumber(&top, "env_temperature", (float)b2int(buf+ptr)/100); + break; + case 0x3037: + JsonVal_objectAddNumber(&top, "sys_temperature", (float)b2int(buf+ptr)/100); + break; + case 0x3038: + JsonVal_objectAddNumber(&top, "undervoltage_times", b2int(buf+ptr)); + break; + case 0x3039: + JsonVal_objectAddNumber(&top, "fullycharged_times", b2int(buf+ptr)); + break; + case 0x303a: + JsonVal_objectAddNumber(&top, "overvoltage_prot_times", b2int(buf+ptr)); + break; + case 0x303b: + JsonVal_objectAddNumber(&top, "overcurrent_prot_times", b2int(buf+ptr)); + break; + case 0x303c: + JsonVal_objectAddNumber(&top, "shortcircuit_prot_times", b2int(buf+ptr)); + break; + case 0x303d: + JsonVal_objectAddNumber(&top, "opencircuit_prot_times", b2int(buf+ptr)); + break; + case 0x303e: + JsonVal_objectAddNumber(&top, "hw_prot_times", b2int(buf+ptr)); + break; + case 0x303f: + JsonVal_objectAddNumber(&top, "charge_overtemp_prot_times", b2int(buf+ptr)); + break; + case 0x3040: + JsonVal_objectAddNumber(&top, "discharge_overtemp_prot_times", b2int(buf+ptr)); + break; + case 0x3045: + JsonVal_objectAddNumber(&top, "battery_remaining_capacity", b2int(buf+ptr)); + break; + case 0x3046: + JsonVal_objectAddNumber(&top, "battery_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x3047: + JsonVal_objectAddNumber(&top, "battery_current", (float)b2int(buf+ptr)/100); + //JsonVal_objectAddNumber(&top, "battery_current", b2int(buf+ptr)/100); + //printf("current: %d\n",b2int(buf+ptr)); + + break; + case 0x3048: + JsonVal_objectAddNumber(&top, "battery_power_lo", (float)b2int(buf+ptr)/100); + break; + case 0x3049: + JsonVal_objectAddNumber(&top, "battery_power_hi", (float)b2int(buf+ptr)/100); + break; + case 0x304a: + JsonVal_objectAddNumber(&top, "load_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x304b: + JsonVal_objectAddNumber(&top, "load_current", (float)b2int(buf+ptr)/100); + break; + case 0x304c: + JsonVal_objectAddNumber(&top, "load_power_l", (float)b2int(buf+ptr)/100); + break; + case 0x304d: + JsonVal_objectAddNumber(&top, "load_power_h", (float)b2int(buf+ptr)/100); + break; + case 0x304e: + JsonVal_objectAddNumber(&top, "solar_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x304f: + JsonVal_objectAddNumber(&top, "solar_current", (float)b2int(buf+ptr)/100); + break; + case 0x3050: + JsonVal_objectAddNumber(&top, "solar_power_l", (float)b2int(buf+ptr)/100); + break; + case 0x3051: + JsonVal_objectAddNumber(&top, "solar_power_h", (float)b2int(buf+ptr)/100); + break; + case 0x3052: + JsonVal_objectAddNumber(&top, "daily_production", (float)b2int(buf+ptr)/100); + break; + case 0x3053: + JsonVal_objectAddNumber(&top, "total_production_l", (float)b2int(buf+ptr)/100); + break; + case 0x3054: + JsonVal_objectAddNumber(&top, "total_production_h", (float)b2int(buf+ptr)/100); + break; + case 0x3055: + JsonVal_objectAddNumber(&top, "daily_consumption", (float)b2int(buf+ptr)/100); + break; + case 0x3056: + JsonVal_objectAddNumber(&top, "total_consumption_l", (float)b2int(buf+ptr)/100); + break; + case 0x3057: + JsonVal_objectAddNumber(&top, "total_consumption_h", (float)b2int(buf+ptr)/100); + break; + case 0x3058: + JsonVal_objectAddNumber(&top, "lighttime_daily", b2int(buf+ptr)); + break; + case 0x305d: + JsonVal_objectAddNumber(&top, "monthly_production_l", (float)b2int(buf+ptr)/100); + break; + case 0x305e: + JsonVal_objectAddNumber(&top, "monthly_production_h", (float)b2int(buf+ptr)/100); + break; + case 0x305f: + JsonVal_objectAddNumber(&top, "yearly_production_l", (float)b2int(buf+ptr)/100); + break; + case 0x3060: + JsonVal_objectAddNumber(&top, "yearly_production_h", (float)b2int(buf+ptr)/100); + break; + + case 0x9017: + JsonVal_objectAddNumber(&top, "rtc_second", b2int(buf+ptr)); + break; + case 0x9018: + JsonVal_objectAddNumber(&top, "rtc_minute", b2int(buf+ptr)); + break; + case 0x9019: + JsonVal_objectAddNumber(&top, "rtc_hour", b2int(buf+ptr)); + break; + case 0x901a: + JsonVal_objectAddNumber(&top, "rtc_day", b2int(buf+ptr)); + break; + case 0x901b: + JsonVal_objectAddNumber(&top, "rtc_month", b2int(buf+ptr)); + break; + case 0x901c: + JsonVal_objectAddNumber(&top, "rtc_year", b2int(buf+ptr)); + break; + case 0x901d: + JsonVal_objectAddNumber(&top, "baud_rate", b2int(buf+ptr)); + break; + case 0x901f: + JsonVal_objectAddNumber(&top, "password", b2int(buf+ptr)); + break; + case 0x9021: + JsonVal_objectAddNumber(&top, "battery_type", b2int(buf+ptr)); + break; + case 0x9022: + JsonVal_objectAddNumber(&top, "lvp_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x9023: + JsonVal_objectAddNumber(&top, "lvr_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x9024: + JsonVal_objectAddNumber(&top, "boost_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x9025: + JsonVal_objectAddNumber(&top, "equalizing_voltage", (float)b2int(buf+ptr)/100); + break; + case 0x9026: + JsonVal_objectAddNumber(&top, "floating_voltage", (float)b2int(buf+ptr)/100); + break; + + default: + break; + } + +} + +static void notify_cb(uint16_t value_handle, const uint8_t *value, + uint16_t length, void *user_data) +{ + + memcpy(rcv_buf+rcv_ptr, value, length); + rcv_ptr+=length; + + if (rcv_ptr==rcv_buf[2]+5) { + + uint16_t crc_r = b2int2(rcv_buf+rcv_ptr-2); + uint16_t crc = ModRTU_CRC(rcv_buf, rcv_ptr-2); + if((crc==crc_r) && addr_base) { //CRC check passed + for (int ii = 0; ii<(rcv_buf[2]-2)/2; ii++) { + parse_val(rcv_buf,ii); + + } + time_t now; + time(&now); + char buf[sizeof "2011-10-08T07:07:09Z"]; + strftime(buf, sizeof buf, "%FT%TZ", gmtime(&now)); + JsonVal_objectAddString(&top, "timestamp", buf); + JsonVal_writeString(&top, jsonout, JSONOUTLEN); + puts(jsonout); + char buf2[8000]; + nbytes = mqtt_encode_publish_msg((uint8_t*)buf2, (uint8_t*)"Solarlife",9 , 1, 10, (uint8_t*)jsonout, strlen(jsonout)); + if(c.sockfd) { + client_send(&c, buf2, nbytes); + client_poll(&c, 1000000); + } + JsonVal_destroy(&top); + //if(addr_base<0x9000) { + // send_modbus_req(0x01, 0x03, 0x9000, 0x40); + // addr_base = 0; + //} + + } + } + + +} + + + + +static void ready_cb(bool success, uint8_t att_ecode, void *user_data) +{ + struct client *cli = user_data; + + if (!success) { + PRLOG("GATT discovery procedures failed - error code: 0x%02x\n", + att_ecode); + return; + } + + PRLOG("GATT discovery procedures complete\n"); + + unsigned char buf[] = {01,00}; + if (!bt_gatt_client_write_value(cli->gatt, 0x12, buf, sizeof(buf), + write_cb, + NULL, NULL)) + printf("Failed to initiate write procedure\n"); + + unsigned int id = bt_gatt_client_register_notify(cli->gatt, 0x11, + register_notify_cb, + notify_cb, NULL, NULL); + if (!id) { + printf("Failed to register notify handler\n"); + } + + PRLOG("Registering notify handler with id: %u\n", id); + + +} + + + +static void service_removed_cb(struct gatt_db_attribute *attr, void *user_data) +{ + log_service_event(attr, "Service Removed"); +} + + +static void service_added_cb(struct gatt_db_attribute *attr, void *user_data) +{ + log_service_event(attr, "Service Added"); +} + + +static void att_disconnect_cb(int err, void *user_data) +{ + printf("Device disconnected: %s\n", strerror(err)); + mainloop_quit(); +} + +void send_mqtt_ping(void) { + + unsigned char buf[100]; + nbytes = mqtt_encode_ping_msg(buf); + if ((nbytes != 0) && (c.sockfd)) + { + client_send(&c, (char*)buf, nbytes); + client_poll(&c, 0); + } + + +} + +static void signal_cb(int signum, void *user_data) +{ + + static int sec_counter; + switch (signum) { + case SIGINT: + case SIGTERM: + mainloop_quit(); + break; + case SIGALRM: + { + //if(addr_base) send_modbus_req(0x01, 0x03, 0x9000, 0x40); + //else { + // send_modbus_req(0x01, 0x04, 0x3000, 0x7c); + // addr_base=0; + //} + //uint16_t len = form_modbus_msg(buff,0x01, 0x03, 0x9000, 0x40); + //send_modbus_req(0x01, 0x04, 0x3000, 0x7c); + //send_modbus_req(0x01, 0x04, 0x3047, 0x20); + send_mqtt_ping(); + if(handler_registered) { + if (++sec_counter>send_interval) { + send_modbus_req(0x01, 0x04, 0x3000, 0x7c); + sec_counter=0; + } + + } + + + alarm(1); + break; + } + default: + break; + } +} + + + +static void client_destroy(struct client *cli) +{ + bt_gatt_client_unref(cli->gatt); + bt_att_unref(cli->att); + free(cli); +} + + + +static struct client *client_create(int fd, uint16_t mtu) +{ + struct client *cli; + + cli = new0(struct client, 1); + if (!cli) { + fprintf(stderr, "Failed to allocate memory for client\n"); + return NULL; + } + + cli->att = bt_att_new(fd, false); + if (!cli->att) { + fprintf(stderr, "Failed to initialze ATT transport layer\n"); + bt_att_unref(cli->att); + free(cli); + return NULL; + } + + if (!bt_att_set_close_on_unref(cli->att, true)) { + fprintf(stderr, "Failed to set up ATT transport layer\n"); + bt_att_unref(cli->att); + free(cli); + return NULL; + } + + if (!bt_att_register_disconnect(cli->att, att_disconnect_cb, NULL, + NULL)) { + fprintf(stderr, "Failed to set ATT disconnect handler\n"); + bt_att_unref(cli->att); + free(cli); + return NULL; + } + + cli->fd = fd; + cli->db = gatt_db_new(); + if (!cli->db) { + fprintf(stderr, "Failed to create GATT database\n"); + bt_att_unref(cli->att); + free(cli); + return NULL; + } + + cli->gatt = bt_gatt_client_new(cli->db, cli->att, mtu); + if (!cli->gatt) { + fprintf(stderr, "Failed to create GATT client\n"); + gatt_db_unref(cli->db); + bt_att_unref(cli->att); + free(cli); + return NULL; + } + + gatt_db_register(cli->db, service_added_cb, service_removed_cb, + NULL, NULL); + + bt_gatt_client_set_ready_handler(cli->gatt, ready_cb, cli, NULL); + + /* bt_gatt_client already holds a reference */ + gatt_db_unref(cli->db); + + return cli; +} + + +static int l2cap_le_att_connect(bdaddr_t *src, bdaddr_t *dst, uint8_t dst_type, + int sec) +{ + int sock; + struct sockaddr_l2 srcaddr, dstaddr; + struct bt_security btsec; + + + sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP); + if (sock < 0) { + perror("Failed to create L2CAP socket"); + return -1; + } + + /* Set up source address */ + memset(&srcaddr, 0, sizeof(srcaddr)); + srcaddr.l2_family = AF_BLUETOOTH; + srcaddr.l2_cid = htobs(ATT_CID); + srcaddr.l2_bdaddr_type = 0; + bacpy(&srcaddr.l2_bdaddr, src); + + if (bind(sock, (struct sockaddr *)&srcaddr, sizeof(srcaddr)) < 0) { + perror("Failed to bind L2CAP socket"); + close(sock); + return -1; + } + + /* Set the security level */ + memset(&btsec, 0, sizeof(btsec)); + btsec.level = sec; + if (setsockopt(sock, SOL_BLUETOOTH, BT_SECURITY, &btsec, + sizeof(btsec)) != 0) { + fprintf(stderr, "Failed to set L2CAP security level\n"); + close(sock); + return -1; + } + + /* Set up destination address */ + memset(&dstaddr, 0, sizeof(dstaddr)); + dstaddr.l2_family = AF_BLUETOOTH; + dstaddr.l2_cid = htobs(ATT_CID); + dstaddr.l2_bdaddr_type = dst_type; + bacpy(&dstaddr.l2_bdaddr, dst); + + printf("Connecting to BT device..."); + fflush(stdout); + + if (connect(sock, (struct sockaddr *) &dstaddr, sizeof(dstaddr)) < 0) { + perror(" Failed to connect to BT device"); + close(sock); + return -1; + } + + printf(" Done\n"); + + return sock; +} + +void inthandler(int dummy) +{ + (void)dummy; + + uint8_t buf[128]; + int nbytes = mqtt_encode_disconnect_msg(buf); + if (nbytes != 0) + { + client_send(&c, (char*)buf, nbytes); + } + + client_poll(&c, 10000000); + + exit(0); +} + + + +static void usage(char *argv[]) +{ + printf("%s\n",argv[0]); + printf("Usage:\n\t%s [options]\n",argv[0]); + + printf("Options:\n" + "\t-i, --index \t\tSpecify adapter index, e.g. hci0\n" + "\t-d, --dest \t\tSpecify the destination mac address of the Solarlife device\n" + "\t-a, --host \t\tSpecify the MQTT broker address\n" + "\t-p, --port \t\t\tMQTT broker port, default: %d\n" + "\t-t, --interval \t\t\tSet data sending interval in seconds, default: %d s\n" + "\t-v, --verbose\t\t\tEnable extra logging\n" + "\t-h, --help\t\t\tDisplay help\n",mqtt_port, send_interval); + + printf("Example:\n" + "%s -d C4:BE:84:70:29:04 -a test.mosquitto.org -t 10\n",argv[0]); +} + + + +static struct option main_options[] = { + { "index", 1, 0, 'i' }, + { "dest", 1, 0, 'd' }, + { "host", 1, 0, 'a' }, + { "port", 1, 0, 'p' }, + { "interval", 1, 0, 't' }, + { "verbose", 0, 0, 'v' }, + { "help", 0, 0, 'h' }, + { } +}; + + + + + + + +int main(int argc, char *argv[]) +{ + + int opt; + int sec = BT_SECURITY_LOW; + uint16_t mtu = 0; + uint8_t dst_type = BDADDR_LE_PUBLIC; + bool dst_addr_given = false; + bdaddr_t src_addr, dst_addr; + int dev_id = -1; + int fd; + sigset_t mask; + char mqtt_host[100]; + + + //str2ba("04:7f:0e:54:61:0c", &dst_addr); + + + + while ((opt = getopt_long(argc, argv, "+hva:t:d:i:p:", + main_options, NULL)) != -1) { + switch (opt) { + case 'h': + usage(argv); + return EXIT_SUCCESS; + case 'v': + verbose = true; + break; + case 'a': + strncpy(mqtt_host,optarg,sizeof(mqtt_host)); + break; + + case 'p': { + int arg; + + arg = atoi(optarg); + if (arg <= 0) { + fprintf(stderr, "Invalid MQTT port: %d\n", arg); + return EXIT_FAILURE; + } + + if (arg > UINT16_MAX) { + fprintf(stderr, "MQTT port too large: %d\n", arg); + return EXIT_FAILURE; + } + + mqtt_port = (uint16_t)arg; + break; + } + + + case 't': { + int arg; + + arg = atoi(optarg); + if (arg <= 0) { + fprintf(stderr, "Invalid interval: %d s\n", arg); + return EXIT_FAILURE; + } + + if (arg > UINT16_MAX) { + fprintf(stderr, "Interval too large: %d s\n", arg); + return EXIT_FAILURE; + } + if (arg < 1) { + fprintf(stderr, "Interval too small: %d s\n", arg); + return EXIT_FAILURE; + } + + send_interval = (uint16_t)arg; + break; + } + + case 'd': + if (str2ba(optarg, &dst_addr) < 0) { + fprintf(stderr, "Invalid Solarlife address: %s\n", + optarg); + return EXIT_FAILURE; + } + + dst_addr_given = true; + break; + + case 'i': + dev_id = hci_devid(optarg); + if (dev_id < 0) { + perror("Invalid adapter"); + return EXIT_FAILURE; + } + + break; + default: + //fprintf(stderr, "Invalid option: %c\n", opt); + return EXIT_FAILURE; + } + } + + if (!argc) { + usage(argv); + return EXIT_SUCCESS; + } + + argc -= optind; + argv += optind; + optind = 0; + + if (argc) { + usage(argv); + return EXIT_SUCCESS; + } + + + + + + + + + if (dev_id == -1) + bacpy(&src_addr, BDADDR_ANY); + else if (hci_devba(dev_id, &src_addr) < 0) { + perror("Adapter not available"); + return EXIT_FAILURE; + } + + if (!dst_addr_given) { + fprintf(stderr, "Destination BT mac address required! Use -h option to print usage.\n"); + return EXIT_FAILURE; + } + + + /* create the mainloop resources */ + + + mainloop_init(); + + fd = l2cap_le_att_connect(&src_addr, &dst_addr, dst_type, sec); + if (fd < 0) + return EXIT_FAILURE; + + + cli = client_create(fd, mtu); + if (!cli) { + close(fd); + return EXIT_FAILURE; + } + + if(strlen(mqtt_host)) { + client_init(&c, mqtt_host, mqtt_port, mqtt_buffer, BUFFER_SIZE_BYTES); + assert(client_set_callback(&c, CB_RECEIVED_DATA, got_data) == 1); + assert(client_set_callback(&c, CB_ON_CONNECTION, got_connection) == 1); + assert(client_set_callback(&c, CB_ON_DISCONNECT, lost_connection) == 1); + signal(SIGINT, inthandler); + client_connect(&c); + } else { + printf("MQTT Broker address not given, will not send data.\n"); + } + alarm(1); + + + + sigemptyset(&mask); + sigaddset(&mask, SIGINT); + sigaddset(&mask, SIGTERM); + sigaddset(&mask, SIGALRM); + + + /* add handler for process interrupted (SIGINT) or terminated (SIGTERM)*/ + mainloop_set_signal(&mask, signal_cb, NULL, NULL); + + mainloop_run(); + + printf("\n\nShutting down...\n"); + if(c.sockfd) client_disconnect(&c); + client_destroy(cli); + + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/mainloop.c b/mainloop.c new file mode 100644 index 0000000..577e172 --- /dev/null +++ b/mainloop.c @@ -0,0 +1,481 @@ +/** + * @file mainloop.c + * @brief set of function to manage the epool loop + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#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; + +/** + * @brief mainloop file descriptor event data structure + */ +struct mainloop_data { + /// socket or file descriptor, incl. standard i/o + int fd; + /// epoll event @see EPOLL_EVENTS_DOC + uint32_t events; + /// call back function(int fd, uint32_t events, void *user_data); + mainloop_event_func callback; + /// data management call back function(void *user_data); + mainloop_destroy_func destroy; + /// pointer to a user specific data structure + void *user_data; +}; + +#define MAX_MAINLOOP_ENTRIES 128 + +/** + * @brief array of file descriptor event stub + */ +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; + +/** + * create the epoll resource (epoll_fd global variable) + * initialize mainloop_list (global variable) event table + * set epoll_terminate to 0 (mainloop_run looping) + */ +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; +} + +/** + * set epoll_terminate to 1 (mainloop_run exit looping) + */ +void mainloop_quit(void) +{ + epoll_terminate = 1; +} + +/** + * set exit_status to EXIT_SUCCESS + * set epoll_terminate to 1 (mainloop_run exit looping) + */ +void mainloop_exit_success(void) +{ + exit_status = EXIT_SUCCESS; + epoll_terminate = 1; +} + +/** + * set exit_status to EXIT_FAILURE + * set epoll_terminate to 1 (mainloop_run exit looping) + */ +void mainloop_exit_failure(void) +{ + exit_status = EXIT_FAILURE; + epoll_terminate = 1; +} + +/** + * + * @param fd + * @param events + * @param user_data + */ +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); +} + +/** + * main loop wait for epoll events + * to exit the loop, set epoll_terminate to a <>0 value + * @see mainloop_exit_failure + * @see mainloop_exit_success + * + * @return exit_status EXIT_SUCCESS or EXIT_FAILURE + */ +int mainloop_run(void) +{ + unsigned int i; + + 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); + } + + 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; + + return exit_status; +} + +/** + * trigger an event to be processed by the mainloop_run function + * and create a mainloop_list entry @see EPOLL_EVENTS_DOC for events description + * + * @param fd "file descriptor" source of the event (socket) + * @param events event flags EPOOL type like EPOLLIN, EPOLLOUT... + * @param callback function to call back by the event processor + * @param user_data associated data + * @param destroy management function to unallocate user_data + * @return 0 success else <0 error + */ +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; +} + +/** + * trigger an epoll event for an existing mainloop socket (exisiting mainloop_list[fd]) + * epool event "events" = events, "data.ptr" = mainloop_list[fd] + * + * @param fd socket + * @param events EPOLL event like EPOLLIN, EPOLLOUT... + * @return 0==Success <0 error + */ +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); +} + +/** + * set mainloop signal handler (signal_data) usally SIGINT and SIGTERM handler + * signal_data is a global variable + * + * @param mask events to filter + * @param callback function call triggered by filtered events prototype callback(int signum, void *user_data) + * @param user_data user data to pass to callback + * @param destroy user data storage management + * @return 0 success else <0 error + */ +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/mainloop.h b/mainloop.h new file mode 100644 index 0000000..b83caab --- /dev/null +++ b/mainloop.h @@ -0,0 +1,51 @@ +/* + * + * 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); + +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/makefile b/makefile new file mode 100644 index 0000000..0f9e217 --- /dev/null +++ b/makefile @@ -0,0 +1,21 @@ +CC=gcc +CFLAGS=-I/usr/include/glib-2.0 -I/usr/lib/x86_64-linux-gnu/glib-2.0/include -Wall +LIBS=-lglib-2.0 +CC_FRITZ=/usr/src/freetz-ng/toolchain/target/bin/i686-linux-uclibc-gcc-5.5.0 +CFLAGS_FRITZ=-I/usr/src/freetz-ng/toolchain/target/i686-linux-uclibc/include/glib-2.0 -I/usr/src/freetz-ng/toolchain/target/i686-linux-uclibc/lib/glib-2.0/include -Wall --static -s +LIBS_FRITZ=-L/usr/src/freetz-ng/toolchain/target/i686-linux-uclibc/lib -lglib-2.0 +APPNAME=solarlife + + +all: + $(CC) $(CFLAGS) *.c -o $(APPNAME) $(LIBS) + + +fritz: + $(CC_FRITZ) $(CFLAGS_FRITZ) *.c -o $(APPNAME) $(LIBS_FRITZ) + +.PHONY: clean + +clean: + rm -f $(APPNAME) + diff --git a/mqtt.c b/mqtt.c new file mode 100644 index 0000000..b5f0255 --- /dev/null +++ b/mqtt.c @@ -0,0 +1,441 @@ +#include "mqtt.h" +#include +#include + + + + +static int mqtt_encode_length(unsigned int nbytes, uint8_t* au8data) +{ + int nbytes_encoded = 0; + if (nbytes < 0x10000000) /* max MQTT packet size */ + { + unsigned int x = nbytes; + do + { + uint8_t u8digit = x % 128; + x /= 128; /* >> 7 */ + if (x != 0) + u8digit |= 0x80; + au8data[nbytes_encoded++] = u8digit; + } + while (x > 0); + } + return nbytes_encoded; +} + + + +static int mqtt_decode_length(uint8_t* au8data) +{ + int nbytes = 0; + int multiplier = 1; + unsigned int array_idx = 0; + uint8_t u8digit; + do + { + u8digit = au8data[array_idx++]; + nbytes += (u8digit & 127) * multiplier; + multiplier *= 128; + } + while ( (++array_idx < 4) + && ((u8digit & 128) != 0)); + return nbytes; +} + + +static int mqtt_encode_msg(uint8_t* pu8dst, uint8_t u8ctrl_type, uint8_t u8flgs, uint8_t** apu8data_in, uint32_t* au32input_len, uint32_t u32nargs, uint32_t u32input_len) +{ + int nbytes_encoded = 0; + if ( (pu8dst != 0) + && (u8ctrl_type > 0) /* cmd type = [1:14] .. 0 + 15 are reserved */ + && (u8ctrl_type < 15) + && (u8flgs < 16) /* flag are 4 bits wide, [0:15] */ + && ( (u32nargs == 0) /* Allow for msg's without payload */ + || ( (apu8data_in != 0) /* ... otherwise, check the args */ + && (au32input_len != 0)))) + { + pu8dst[0] = (u8ctrl_type << 4) | (u8flgs); + + int idx = mqtt_encode_length(u32input_len, &pu8dst[1]); + uint32_t i, j; + for (i = 0; i < u32nargs; ++i) + { + for (j = 0; j < au32input_len[i]; ++j, ++nbytes_encoded) + { + pu8dst[idx + nbytes_encoded + 1] = apu8data_in[i][j]; + } + } + nbytes_encoded += (1 + idx); + } + return nbytes_encoded; +} + + +int mqtt_decode_msg(uint8_t* pu8src, uint8_t* pu8ctrl_type, uint8_t* pu8flgs, uint8_t* pu8data_out, uint32_t* pu32output_len) +{ + int nbytes_decoded = 0; + if ( (pu8src != 0) + && (pu8ctrl_type != 0) + && (pu8flgs != 0) + && (pu8data_out != 0) + && (pu32output_len != 0)) + { + *pu8ctrl_type = ((*pu8src) >> 4); + *pu8flgs = ((*pu8src) & 0x0F); + int idx = 1; + int nbytes = mqtt_decode_length(&pu8src[idx]); + //printf("nbytes = %d\n", nbytes); + if (nbytes < 0x80) /* 0 <= nbytes < 128 */ + { + idx += 1; + } + else if (nbytes < 0x4000) /* 128 <= nbytes < 16384 */ + { + idx += 2; + } + else if (nbytes < 0x200000) /* 16384 <= nbytes < 2097152 */ + { + idx += 3; + } + else + { + idx += 4; + assert(nbytes < 0x10000000); + } + /* Copy decoded data to output-buffer */ + int i; + for (i = 0; i < nbytes; ++i) + { + pu8data_out[i] = pu8src[idx + i]; + } + //memcpy(pu8data_out, &pu8src[idx], nbytes); + *pu32output_len = nbytes; + nbytes_decoded = nbytes + idx; + } + return nbytes_decoded; +} + + + +/* Advanced connect: More options available */ +int mqtt_encode_connect_msg2(uint8_t* pu8dst, uint8_t u8conn_flgs, uint16_t u16keepalive, uint8_t* pu8clientid, uint16_t u16clientid_len) +{ + int nbytes_encoded = 0; + if ( (pu8dst != 0) + && (pu8clientid != 0)) + { + const uint32_t u32hdr_len = 12; + uint8_t u8keepalive_msb = (u16keepalive & 0xFF00) >> 8; /* Bug if on Big-Endian machine */ + uint8_t u8keepalive_lsb = (u16keepalive & 0x00FF); + uint8_t u8clientid_len_msb = (u16clientid_len & 0xFF00) >> 8; /* Bug if on Big-Endian machine */ + uint8_t u8clientid_len_lsb = (u16clientid_len & 0x00FF); + uint8_t au8conn_buf[u32hdr_len]; + /* Variable part of header for CONNECT msg */ + int idx = 0; + au8conn_buf[idx++] = 0x00; /* protocol version length U16 */ + au8conn_buf[idx++] = 0x04; /* 4 bytes long (MQTT) */ + au8conn_buf[idx++] = 'M'; + au8conn_buf[idx++] = 'Q'; + au8conn_buf[idx++] = 'T'; + au8conn_buf[idx++] = 'T'; + au8conn_buf[idx++] = 0x04; /* 4 == MQTT version 3.1.1 */ + au8conn_buf[idx++] = u8conn_flgs; + au8conn_buf[idx++] = u8keepalive_msb; + au8conn_buf[idx++] = u8keepalive_lsb; + au8conn_buf[idx++] = u8clientid_len_msb; + au8conn_buf[idx++] = u8clientid_len_lsb; + + uint8_t* buffers[] = { au8conn_buf, pu8clientid }; + uint32_t sizes[] = { u32hdr_len, u16clientid_len }; + + nbytes_encoded = mqtt_encode_msg(pu8dst, CTRL_CONNECT, 0, buffers, sizes, 2, u32hdr_len + u16clientid_len); + } + return nbytes_encoded; +} + + +/* Simple connect: No username/password, no QoS etc. */ +int mqtt_encode_connect_msg(uint8_t* pu8dst, uint8_t* pu8clientid, uint16_t u16clientid_len) /* u8conn_flgs = 2, u16keepalive = 60 */ +{ + return mqtt_encode_connect_msg2(pu8dst, 0x02, 60, pu8clientid, u16clientid_len); +} + +int mqtt_encode_disconnect_msg(uint8_t* pu8dst) +{ + return mqtt_encode_msg(pu8dst, CTRL_DISCONNECT, 0x02, 0, 0, 0, 0); +} + +int mqtt_encode_ping_msg(uint8_t* pu8dst) +{ + return mqtt_encode_msg(pu8dst, CTRL_PINGREQ, 0x02, 0, 0, 0, 0); +} + + +int mqtt_encode_publish_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id, uint8_t* pu8payload, uint32_t u32data_len) +{ + int nbytes_encoded = 0; + if (pu8topic != 0) + { + uint32_t u32msg_len = sizeof(uint16_t) + u16topic_len + sizeof(uint16_t) + u32data_len; + uint8_t u8topic_len_msb = (u16topic_len & 0xFF00) >> 8; /* Bug if on Big-Endian machine */ + uint8_t u8topic_len_lsb = (u16topic_len & 0x00FF); + uint8_t u8msg_id_msb = (u16msg_id & 0xFF00) >> 8; /* Bug if on Big-Endian machine */ + uint8_t u8msg_id_lsb = (u16msg_id & 0x00FF); + uint8_t au8topic_len_buf[sizeof(uint16_t)] = { u8topic_len_msb, u8topic_len_lsb }; + uint8_t au8msg_id_buf[sizeof(uint16_t)] = { u8msg_id_msb, u8msg_id_lsb }; + uint8_t* buffers[] = { au8topic_len_buf, pu8topic, au8msg_id_buf, pu8payload }; + uint32_t sizes[] = { sizeof(uint16_t), u16topic_len, sizeof(uint16_t), u32data_len }; + nbytes_encoded = mqtt_encode_msg(pu8dst, CTRL_PUBLISH, u8qos << 1, buffers, sizes, 4, u32msg_len); + } + return nbytes_encoded; +} + + + + +static int encode_pubsub_msg2(uint8_t* pu8dst, uint8_t u8ctrl, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id) +{ + int nbytes_encoded = 0; + if ( (apu8topic != 0) + && (u32nargs < MSG_SUB_MAXNTOPICS)) + { + uint8_t topicsizes[sizeof(uint16_t)][MSG_SUB_MAXNTOPICS]; + uint32_t sizes[1 + (3 * MSG_SUB_MAXNTOPICS)]; /* for each topic a topic-len, the topic itself, and the qos -- hence 3 * max(subs_pr_msg) */ + uint8_t* buffers[1 + (3 * MSG_SUB_MAXNTOPICS)]; /* msgid, [topic-len + topic + qos]+ */ + uint8_t u8msg_id_msb = (u16msg_id & 0xFF00) >> 8; /* Bug if on Big-Endian machine */ + uint8_t u8msg_id_lsb = (u16msg_id & 0x00FF); + uint8_t au8msg_id_buf[sizeof(uint16_t)] = { u8msg_id_msb, u8msg_id_lsb }; + int bufidx = 0; + buffers[bufidx] = au8msg_id_buf; + sizes[bufidx++] = sizeof(uint16_t); + uint32_t u32msg_len = sizeof(uint16_t); /* msgid */ + uint8_t u8highest_qos = 0; + uint32_t i; + for (i = 0; i < u32nargs; ++i) + { + u32msg_len += sizeof(uint16_t) + au16topic_len[i] + sizeof(uint8_t); /* topic-len + topic + qos */ + uint8_t u8topic_len_msb = (au16topic_len[i] & 0xFF00) >> 8; /* Bug if on Big-Endian machine */ + uint8_t u8topic_len_lsb = (au16topic_len[i] & 0x00FF); + topicsizes[i][0] = u8topic_len_msb; + topicsizes[i][1] = u8topic_len_lsb; + buffers[bufidx] = topicsizes[i]; + sizes[bufidx++] = sizeof(uint16_t); + buffers[bufidx] = apu8topic[i]; + sizes[bufidx++] = au16topic_len[i]; + buffers[bufidx] = &au8qos[i]; + sizes[bufidx++] = sizeof(uint8_t); + /* Send MQTT SUBSCRIBE msg with highest QoS of all subscribtions */ + if (au8qos[i] > u8highest_qos) + { + u8highest_qos = au8qos[i]; + } + } + nbytes_encoded = mqtt_encode_msg(pu8dst, u8ctrl, u8highest_qos << 1, buffers, sizes, bufidx, u32msg_len); + } + return nbytes_encoded; +} + + + +int mqtt_encode_subscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id) +{ + return encode_pubsub_msg2(pu8dst, CTRL_SUBSCRIBE, &pu8topic, &u16topic_len, &u8qos, 1, u16msg_id); +} + +int mqtt_encode_unsubscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id) +{ + return encode_pubsub_msg2(pu8dst, CTRL_UNSUBSCRIBE, &pu8topic, &u16topic_len, &u8qos, 1, u16msg_id); +} +int mqtt_encode_subscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id) +{ + return encode_pubsub_msg2(pu8dst, CTRL_SUBSCRIBE, apu8topic, au16topic_len, au8qos, u32nargs, u16msg_id); +} + + +int mqtt_encode_unsubscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id) +{ + return encode_pubsub_msg2(pu8dst, CTRL_UNSUBSCRIBE, apu8topic, au16topic_len, au8qos, u32nargs, u16msg_id); +} + + + + +int mqtt_decode_connack_msg(uint8_t* pu8src, uint32_t u32nbytes) +{ + return ( (pu8src != 0) + && (u32nbytes >= 4) + && (pu8src[0] == 0x20) /* 0x20 : CTRL_CONNACK << 4 */ + && (pu8src[1] == 0x02) /* 0x02 : bytes after fixed header */ + // && (pu8src[3] == 0x00) /* 0x00 : 3rd byte is reserved */ + && (pu8src[3] == 0x00)); /* 0x00 : Connection Accepted */ +} + + +int mqtt_decode_pingresp_msg(uint8_t* pu8src, uint32_t u32nbytes) +{ + return ( (pu8src != 0) + && (u32nbytes >= 2) + && (pu8src[0] == 0xd0) /* 0xD0 : CTRL_PINGRESP << 4 */ + && (pu8src[1] == 0x00)); /* 0x00 : bytes after fixed header */ +} + +int mqtt_decode_puback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id_out) +{ + int success = 0; + if ( (pu8src != 0) + && (u32nbytes >= 4) + && (pu8src[0] == 0x40) /* 0x40 : CTRL_PUBACK << 4 */ + && (pu8src[1] == 0x02) /* 0x02 : bytes after fixed header */ + && (pu16msg_id_out != 0)) + { + *pu16msg_id_out = (pu8src[2] << 8) | pu8src[3]; + success = 1; + } + return success; +} + + +int mqtt_decode_suback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id_out) +{ + int success = 0; + if ( (pu8src != 0) + && (u32nbytes >= 4) + && (pu8src[0] == 0x90) /* 0x90 : CTRL_SUBACK << 4 */ + && (pu8src[1] == 0x02) /* 0x02 : bytes after fixed header */ + && (pu16msg_id_out != 0)) + { + *pu16msg_id_out = (pu8src[2] << 8) | pu8src[3]; + success = 1; + } + return success; +} + +int mqtt_decode_publish_msg(uint8_t* pu8src, uint32_t u32nbytes, uint8_t* pu8qos, uint16_t* pu16msg_id_out, uint16_t* pu16topic_len, uint8_t** ppu8topic, uint8_t** ppu8payload) +{ + int success = 0; + if ( (pu8src != 0) + && (u32nbytes >= 6) + && (pu8src[0] >> 4 == CTRL_PUBLISH) + && (pu16msg_id_out != 0) + && (pu16topic_len != 0) + && (ppu8topic != 0) + && (ppu8payload != 0) ) + { + *pu8qos = (pu8src[0] >> 1) & 3; + uint16_t u16topic_len = (pu8src[2] << 8) | pu8src[3]; + *pu16topic_len = u16topic_len; + *ppu8topic = &pu8src[4]; + *pu16msg_id_out = (pu8src[4 + u16topic_len] << 8) | pu8src[5 + u16topic_len]; + *ppu8payload = &pu8src[6 + u16topic_len]; + success = 1; + } + return success; +} + + + +#if defined(TEST) && (TEST == 1) + +int main(void) +{ + // int mqtt_encode_connect_msg2(uint8_t* pu8dst, uint8_t u8conn_flgs, uint16_t u16keepalive, uint8_t* pu8clientid, uint16_t u16clientid_len) + uint8_t buf[128]; + int nbytes; + int i; + + nbytes = mqtt_encode_connect_msg2(buf, 0x02, 60, (uint8_t*)"DIGI", 4); + printf("con: "); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + nbytes = mqtt_encode_connect_msg(buf, (uint8_t*)"DIGI", 4); + printf("con: "); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + nbytes = mqtt_encode_disconnect_msg(buf); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + nbytes = mqtt_encode_ping_msg(buf); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + nbytes = mqtt_encode_publish_msg(buf, (uint8_t*)"a/b", 3, 1, 32767, (uint8_t*)"payload", 8); +//nbytes = mqtt_encode_publish_msg(buf, (uint8_t*)"a/b", 3, 1, 10, 0, 0); + printf("pub: "); + for (i = 0; i < nbytes; ++i) + { + if (buf[i] >= 'A' && buf[i] <= 'z') + printf("%c ", buf[i]); + else + printf("0x%.02x ", buf[i]); + } + printf("\n"); + + { + uint16_t msg_id, topic_len; + uint8_t qos; + uint8_t* topic; + uint8_t* payload; + int rc = mqtt_decode_publish_msg(buf, nbytes, &qos, &msg_id, &topic_len, &topic, &payload); + printf("nbytes = %u\n", nbytes); + printf("decode pub msg = %d:\n", rc); + printf(" qos = %u\n", qos); + printf(" msg id = %u\n", msg_id); + printf(" topic-len = %u\n", topic_len); + printf(" topic = '%s'\n", topic); + printf(" payload = '%s'\n", payload); + } + + nbytes = mqtt_encode_subscribe_msg(buf, (uint8_t*)"a/b", 3, 1, 32767); + printf("sub: "); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + uint8_t* tpcs[] = { (uint8_t*)"a/b" }; + uint16_t tpclens[] = { 3 }; + uint8_t tpcqos[] = { 1 }; + nbytes = mqtt_encode_subscribe_msg2(buf, tpcs, tpclens, tpcqos, 1, 32767); + printf("sub: "); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + nbytes = mqtt_encode_unsubscribe_msg(buf, (uint8_t*)"a/b", 3, 1, 32767); + printf("unsub: "); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + nbytes = mqtt_encode_unsubscribe_msg2(buf, tpcs, tpclens, tpcqos, 1, 32767); + printf("unsub: "); + for (i = 0; i < nbytes; ++i) + printf("0x%.02x ", buf[i]); + printf("\n"); + + + + uint8_t au8connack_msg[] = { 0x20, 0x02, 0x00, 0x00 }; + printf("connack(msg) = %d \n", mqtt_decode_connack_msg(au8connack_msg, sizeof(au8connack_msg))); + + uint8_t au8pingresp_msg[] = { 0xd0, 0x00 }; + printf("pingresp(msg) = %d \n", mqtt_decode_pingresp_msg(au8pingresp_msg, sizeof(au8pingresp_msg))); + + uint16_t u16msg_id; + uint8_t au8puback_msg[] = { 0x40, 0x02, 0x7f, 0xff }; + printf("puback(msg) = %d \n", mqtt_decode_puback_msg(au8puback_msg, sizeof(au8puback_msg), &u16msg_id)); + + + + return 0; +} + +#endif + diff --git a/mqtt.h b/mqtt.h new file mode 100644 index 0000000..674d08a --- /dev/null +++ b/mqtt.h @@ -0,0 +1,55 @@ +#include + +/* Max number of topics one can subscribe to in a single SUBSCRIBE message */ +#define MSG_SUB_MAXNTOPICS 8 + +/* Control Command Types */ +enum +{ + CTRL_RESERVED, /* 0 */ + CTRL_CONNECT, /* 1 */ + CTRL_CONNACK, /* 2 */ + CTRL_PUBLISH, /* 3 */ + CTRL_PUBACK, /* 4 */ + CTRL_PUBREC, /* 5 */ + CTRL_PUBREL, /* 6 */ + CTRL_PUBCOMP, /* 7 */ + CTRL_SUBSCRIBE, /* 8 */ + CTRL_SUBACK, /* 9 */ + CTRL_UNSUBSCRIBE, /* 10 */ + CTRL_UNSUBACK, /* 11 */ + CTRL_PINGREQ, /* 12 */ + CTRL_PINGRESP, /* 13 */ + CTRL_DISCONNECT, /* 14 */ +}; + +enum +{ + QOS_AT_MOST_ONCE, /* 0 - "Fire and Forget" */ + QOS_AT_LEAST_ONCE, /* 1 - ARQ until ACK */ + QOS_EXACTLY_ONCE, /* 2 - Transactional delivery */ +}; + + + +/* Simple connect: No username/password, no QoS etc. */ +int mqtt_encode_connect_msg(uint8_t* pu8dst, uint8_t* pu8clientid, uint16_t u16clientid_len); /* u8conn_flgs = 2, u16keepalive = 60 */ +/* Advanced connect: More options available */ +int mqtt_encode_connect_msg2(uint8_t* pu8dst, uint8_t u8conn_flgs, uint16_t u16keepalive, uint8_t* pu8clientid, uint16_t u16clientid_len); +int mqtt_encode_disconnect_msg(uint8_t* pu8dst); +int mqtt_encode_ping_msg(uint8_t* pu8dst); +int mqtt_encode_publish_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id, uint8_t* pu8payload, uint32_t u32data_len); +int mqtt_encode_subscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id); +int mqtt_encode_subscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id); +int mqtt_encode_unsubscribe_msg(uint8_t* pu8dst, uint8_t* pu8topic, uint16_t u16topic_len, uint8_t u8qos, uint16_t u16msg_id); +int mqtt_encode_unsubscribe_msg2(uint8_t* pu8dst, uint8_t** apu8topic, uint16_t* au16topic_len, uint8_t* au8qos, uint32_t u32nargs, uint16_t u16msg_id); + +int mqtt_decode_connack_msg(uint8_t* pu8src, uint32_t u32nbytes); +int mqtt_decode_pingresp_msg(uint8_t* pu8src, uint32_t u32nbytes); +int mqtt_decode_puback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id); +int mqtt_decode_suback_msg(uint8_t* pu8src, uint32_t u32nbytes, uint16_t* pu16msg_id_out); + +int mqtt_decode_msg(uint8_t* pu8src, uint8_t* pu8ctrl_type, uint8_t* pu8flgs, uint8_t* pu8data_out, uint32_t* pu32output_len); +int mqtt_decode_publish_msg(uint8_t* pu8src, uint32_t u32nbytes, uint8_t* pu8qos, uint16_t* pu16msg_id_out, uint16_t* pu16topic_len, uint8_t** ppu8topic, uint8_t** ppu8payload); + + diff --git a/queue.c b/queue.c new file mode 100644 index 0000000..079d38e --- /dev/null +++ b/queue.c @@ -0,0 +1,575 @@ +/** + * @file queue.c + * @brief set of functions to manage queues + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include "util.h" +#include "queue.h" + +struct queue { + int ref_count; + struct queue_entry *head; + struct queue_entry *tail; + unsigned int entries; +}; + +/** + * increment &queue->ref_count + * + * @param queue pointer to the queue structure + * @return + */ +static struct queue *queue_ref(struct queue *queue) +{ + if (!queue) + return NULL; + + __sync_fetch_and_add(&queue->ref_count, 1); + + return queue; +} + +/** + * decrement &queue->ref_count and free queue structure if ref_count == 0 + * + * @param queue pointer to the queue structure + */ +static void queue_unref(struct queue *queue) +{ + if (__sync_sub_and_fetch(&queue->ref_count, 1)) + return; + + free(queue); +} + +/** + * create a new queue structure + * + * @return queue reference + */ +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); +} + +/** + * destroy all queue structure elements + * + * @param queue pointer to the queue structure + * @param destroy function making data deallocation + */ +void queue_destroy(struct queue *queue, queue_destroy_func_t destroy) +{ + if (!queue) + return; + + queue_remove_all(queue, NULL, NULL, destroy); + + queue_unref(queue); +} + +/** + * increment &entry->ref_count + * + * @param entry + * @return entry + */ +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; +} + +/** + * decrement &entry->ref_count and free entry structure if ref_count == 0 + * + * @param entry + */ +static void queue_entry_unref(struct queue_entry *entry) +{ + if (__sync_sub_and_fetch(&entry->ref_count, 1)) + return; + + free(entry); +} + +/** + * create a new queue, set queue->data to data, increment queue->ref_count + * + * @param data + * @return new queue pointer + */ +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); +} + +/** + * push a queue entry allocated with data and set it at the tail of queue + * + * @param queue queue pointer where to allocate data + * @param data data to queue + * @return + */ +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; +} + +/** + * push a queue entry allocated with data and set it at the head of queue + * + * @param queue queue pointer where to allocate data + * @param data data to queue + * @return + */ +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; +} + +/** + * push a queue entry allocated with data and set it after the entry + * + * @param queue queue pointer where to allocate data + * @param entry element queue where to put the data after + * @param data data to queue + * @return + */ +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; +} + +/** + * pop data from the head of queue + * + * @param queue queue pointer + */ +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; +} + +/** + * peek data from the head of the queue + * + * @param queue queue pointer + */ +void *queue_peek_head(struct queue *queue) +{ + if (!queue || !queue->head) + return NULL; + + return queue->head->data; +} + +/** + * peek data from the tail of the queue + * + * @param queue queue pointer + */ +void *queue_peek_tail(struct queue *queue) +{ + if (!queue || !queue->tail) + return NULL; + + return queue->tail->data; +} + +/** + * iterator for the queue + * + * @param queue queue pointer + * @param function function(void *data, void *user_data) to call for each element + * @param user_data user pointer to pass to function + */ +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); +} + +/** + * compare a with b + * + * @param a first parameter + * @param b second parameter + * + * @return true if both element match + */ +static bool direct_match(const void *a, const void *b) +{ + return a == b; +} + +/** + * + * + * @param queue queue pointer + * @param function function call to make comparison + * @param match_data data to match with queue entry + * + * @return found entry and return pointer to user data (queued data) or NULL if not found + */ +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; +} + +/** + * remove queue element holding data + * + * @param queue queue pointer + * @param data data pointer + * + * @return false if not found, true if element removed + */ +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; +} + +/** + * remove element match by function(void *data, void *match_data) where match_data = user_data + * + * @param queue queue pointer + * @param function function call to make comparison + * @param user_data data to match queue element in function + * + * @return NULL if not found or queue entry matching criteria + */ +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; +} + +/** + * remove all queue element + * + * @param queue queue pointer + * @param function function used to match data deallocation + * @param user_data data pointer used in deallocation function + * @param destroy function for data deallocation + * + * @return count of element destroyed (0 if none) + */ +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; +} + +/** + * return queue head pointer + * + * @param queue queue pointer + * + * @return queue head + */ +const struct queue_entry *queue_get_entries(struct queue *queue) +{ + if (!queue) + return NULL; + + return queue->head; +} + +/** + * return queue length + * + * @param queue queue pointer + * + * @return return 0 or queue elements count + */ +unsigned int queue_length(struct queue *queue) +{ + if (!queue) + return 0; + + return queue->entries; +} + +/** + * test if queue is empty + * + * @param queue queue pointer + * @return + */ +bool queue_isempty(struct queue *queue) +{ + if (!queue) + return true; + + return queue->entries == 0; +} diff --git a/queue.h b/queue.h new file mode 100644 index 0000000..3bc8d2e --- /dev/null +++ b/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/timeout-glib.c b/timeout-glib.c new file mode 100644 index 0000000..eb36e19 --- /dev/null +++ b/timeout-glib.c @@ -0,0 +1,86 @@ +/** + * @file timeout-glib.c + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 "timeout.h" + +#include + +struct timeout_data { + timeout_func_t func; + timeout_destroy_func_t destroy; + void *user_data; +}; + +static gboolean timeout_callback(gpointer user_data) +{ + struct timeout_data *data = user_data; + + if (data->func(data->user_data)) + return TRUE; + + return FALSE; +} + +static void timeout_destroy(gpointer user_data) +{ + struct timeout_data *data = user_data; + + if (data->destroy) + data->destroy(data->user_data); + + g_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; + guint id; + + data = g_try_new0(struct timeout_data, 1); + if (!data) + return 0; + + data->func = func; + data->destroy = destroy; + data->user_data = user_data; + + id = g_timeout_add_full(G_PRIORITY_DEFAULT, timeout, timeout_callback, + data, timeout_destroy); + if (!id) + g_free(data); + + return id; +} + +void timeout_remove(unsigned int id) +{ + GSource *source = g_main_context_find_source_by_id(NULL, id); + + if (source) + g_source_destroy(source); +} diff --git a/timeout.h b/timeout.h new file mode 100644 index 0000000..4930ce1 --- /dev/null +++ b/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/util.c b/util.c new file mode 100644 index 0000000..9bc418a --- /dev/null +++ b/util.c @@ -0,0 +1,178 @@ +/** + * @file util.c + * @brief set of utility functions + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" + +/** + * create a str debug message using format and then call function(str,user_data) + * + * @param function function to call + * @param user_data data for the "function" + * @param format format string template of str string + */ +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); +} + +/** + * hexadecimal dump utility: create the str hex string and then call function(str,user_data) + * + * @param dir first char of str + * @param buf buffer to convert to hex (str) + * @param len size of buffer (should be less than or equal to 16) + * @param function function to call with (str,user_data) + * @param user_data pointer to pass to function + */ +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 + * + * @param parent prefix to build full path + * @param name suffix to builld full path + * @return DT_UNKNOWN or DT_DIR if parent/name is a DIR + */ +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 + * + * @param bitmap map of bits + * @param max max bit position + * @return + */ +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 + * + * @param bitmap map of bits + * @param id bit rank to clear + */ +void util_clear_uid(unsigned int *bitmap, uint8_t id) +{ + if (!id) + return; + + *bitmap &= ~(1 << (id - 1)); +} diff --git a/util.h b/util.h new file mode 100644 index 0000000..260a5f8 --- /dev/null +++ b/util.h @@ -0,0 +1,158 @@ +/* + * + * 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 + +#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/uuid.c b/uuid.c new file mode 100644 index 0000000..ce6bb9f --- /dev/null +++ b/uuid.c @@ -0,0 +1,324 @@ +/** + * @file uuid.c + * @brief uuid collection of functions + * @author Gilbert Brault + * @copyright Gilbert Brault 2015 + * the original work comes from bluez v5.39 + * value add: documenting main features + * + */ +/* + * + * 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 + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include + +#include "bluetooth.h" +#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_uuid32_to_uuid128(src, &uuid); + 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/uuid.h b/uuid.h new file mode 100644 index 0000000..2dcfe9e --- /dev/null +++ b/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 */