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