This patch set introduces tools/btgatt-server. This tool serves as a
standalone demo of combining shared/gatt-db, shared/gatt-server, and a
listening L2CAP socket on the ATT channel. The tool exposes the GAP and
GATT services, as well as a Heart Rate service. It's possible to hide/unhide
the latter to test Service Changed events.
The set also includes two small bug fixes for the way Service Changed
characteristics are handled by shared/gatt-client.
*v1: Fixed Luiz's comments:
- Style fixes.
- Make certain attribute values cached directly in gatt-db.
- Added an ATT error response encoding API and packed struct.
Arman Uguray (7):
shared/att: Add API for encoding ATT error response.
tools/btgatt-server: Introduce btgatt-server.
tools/btgatt-server: Accept incoming connection and initialize server.
tools/btgatt-server: Populate the database.
tools/btgatt-server: Add command support and the notify command.
tools/btgatt-server: Add Heart Rate service simulation.
tools/btgatt-server: Add "heart-rate" command.
.gitignore | 1 +
Makefile.tools | 8 +
src/shared/att-types.h | 12 +
src/shared/att.c | 58 ++-
src/shared/att.h | 3 +
src/shared/gatt-client.c | 8 +-
src/shared/gatt-helpers.c | 8 +-
src/shared/gatt-server.c | 153 ++++---
tools/btgatt-server.c | 1088 +++++++++++++++++++++++++++++++++++++++++++++
9 files changed, 1257 insertions(+), 82 deletions(-)
create mode 100644 tools/btgatt-server.c
--
2.1.0.rc2.206.gedb03e5
Hi Luiz,
> On Thu, Nov 13, 2014 at 8:10 AM, Luiz Augusto von Dentz <[email protected]> wrote:
> Hi Arman,
>
> On Thu, Nov 13, 2014 at 2:36 AM, Arman Uguray <[email protected]> wrote:
>> This patch adds bt_att_encode_error_rsp which allows encoding an ATT
>> protocol error response by correctly converting system errnos to ATT
>> protocol error codes. struct bt_att_pdu_error_rsp is introduced to
>> represent an error response PDU which can be sent directly over the
>> wire once properly encoded in network order.
>> ---
>> src/shared/att-types.h | 12 ++++
>> src/shared/att.c | 58 +++++++++++++++++-
>> src/shared/att.h | 3 +
>> src/shared/gatt-client.c | 8 ++-
>> src/shared/gatt-helpers.c | 8 ++-
>> src/shared/gatt-server.c | 153 +++++++++++++++++++++++-----------------------
>> 6 files changed, 160 insertions(+), 82 deletions(-)
>>
>> diff --git a/src/shared/att-types.h b/src/shared/att-types.h
>> index 24bf3da..3d8829a 100644
>> --- a/src/shared/att-types.h
>> +++ b/src/shared/att-types.h
>> @@ -23,6 +23,10 @@
>>
>> #include <stdint.h>
>>
>> +#ifndef __packed
>> +#define __packed __attribute__((packed))
>> +#endif
>> +
>> #define BT_ATT_DEFAULT_LE_MTU 23
>>
>> /* ATT protocol opcodes */
>> @@ -55,6 +59,14 @@
>> #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
>>
>> diff --git a/src/shared/att.c b/src/shared/att.c
>> index 6e1e538..e831078 100644
>> --- a/src/shared/att.c
>> +++ b/src/shared/att.c
>> @@ -35,13 +35,22 @@
>> #include "src/shared/timeout.h"
>> #include "lib/uuid.h"
>> #include "src/shared/att.h"
>> -#include "src/shared/att-types.h"
>>
>> #define ATT_MIN_PDU_LEN 1 /* At least 1 byte for the opcode. */
>> #define ATT_OP_CMD_MASK 0x40
>> #define ATT_OP_SIGNED_MASK 0x80
>> #define ATT_TIMEOUT_INTERVAL 30000 /* 30000 ms */
>>
>> +/*
>> + * Common Profile and Service Error Code descriptions (see Supplement to the
>> + * Bluetooth Core Specification, sections 1.2 and 2). The error codes within
>> + * 0xE0-0xFC are reserved for future use. The remaining 3 are defined as the
>> + * following:
>> + */
>> +#define BT_ERROR_CCC_IMPROPERLY_CONFIGURED 0xfd
>> +#define BT_ERROR_ALREADY_IN_PROGRESS 0xfe
>> +#define BT_ERROR_OUT_OF_RANGE 0xff
>> +
>> struct att_send_op;
>>
>> struct bt_att {
>> @@ -1218,3 +1227,50 @@ bool bt_att_unregister_all(struct bt_att *att)
>>
>> 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;
>> +}
>> +
>> +bool bt_att_encode_error_rsp(uint8_t opcode, uint16_t handle, int error,
>> + struct bt_att_pdu_error_rsp *pdu)
>> +{
>> + uint8_t ecode;
>> +
>> + if (!pdu)
>> + return false;
>> +
>> + ecode = att_ecode_from_error(error);
>> +
>> + memset(pdu, 0, sizeof(*pdu));
>> +
>> + pdu->opcode = opcode;
>> + put_le16(handle, &pdu->handle);
>> + pdu->ecode = ecode;
>> +
>> + return true;
>> +}
>> diff --git a/src/shared/att.h b/src/shared/att.h
>> index 1063021..a1a4821 100644
>> --- a/src/shared/att.h
>> +++ b/src/shared/att.h
>> @@ -76,3 +76,6 @@ unsigned int bt_att_register_disconnect(struct bt_att *att,
>> bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id);
>>
>> bool bt_att_unregister_all(struct bt_att *att);
>> +
>> +bool bt_att_encode_error_rsp(uint8_t opcode, uint16_t handle, int error,
>> + struct bt_att_pdu_error_rsp *pdu);
>
> Lets drop the encode part from the name, it should be implicit that it
> will encode before sending. Also Im not sure why you have chosen to
> pass the parameter as a struct, you could have passed separately by
> value so encoding does not affect the caller stack, also I was
> assuming that it would send the PDU after encoding it.
>
Ah, I see, so you're saying let's just have a bt_att_send_error_rsp
that encodes and sends it. Yeah, I like that idea better, I'll upload
this in a follow up.
In the meantime though, I don't think this is really blocking the
tools/btgatt-server patches, so if you're fine with the latest patch
set, please apply them and I'll send the revised version of this patch
seperately.
>> diff --git a/src/shared/gatt-client.c b/src/shared/gatt-client.c
>> index 30b271e..1f82048 100644
>> --- a/src/shared/gatt-client.c
>> +++ b/src/shared/gatt-client.c
>> @@ -1285,10 +1285,14 @@ static bool notify_data_write_ccc(struct notify_data *notify_data, bool enable,
>>
>> static uint8_t process_error(const void *pdu, uint16_t length)
>> {
>> - if (!pdu || length != 4)
>> + const struct bt_att_pdu_error_rsp *error_pdu;
>> +
>> + if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
>> return 0;
>>
>> - return ((uint8_t *) pdu)[3];
>> + error_pdu = pdu;
>> +
>> + return error_pdu->ecode;
>> }
>>
>> static void enable_ccc_callback(uint8_t opcode, const void *pdu,
>> diff --git a/src/shared/gatt-helpers.c b/src/shared/gatt-helpers.c
>> index 220d0eb..9c8a3ba 100644
>> --- a/src/shared/gatt-helpers.c
>> +++ b/src/shared/gatt-helpers.c
>> @@ -430,10 +430,14 @@ static void destroy_mtu_op(void *user_data)
>>
>> static uint8_t process_error(const void *pdu, uint16_t length)
>> {
>> - if (!pdu || length != 4)
>> + const struct bt_att_pdu_error_rsp *error_pdu;
>> +
>> + if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
>> return 0;
>>
>> - return ((uint8_t *) pdu)[3];
>> + error_pdu = pdu;
>> +
>> + return error_pdu->ecode;
>> }
>>
>> static void mtu_cb(uint8_t opcode, const void *pdu, uint16_t length,
>> diff --git a/src/shared/gatt-server.c b/src/shared/gatt-server.c
>> index 2ca318a..3262992 100644
>> --- a/src/shared/gatt-server.c
>> +++ b/src/shared/gatt-server.c
>> @@ -22,6 +22,7 @@
>> */
>>
>> #include <sys/uio.h>
>> +#include <errno.h>
>>
>> #include "src/shared/att.h"
>> #include "lib/uuid.h"
>> @@ -29,7 +30,6 @@
>> #include "src/shared/gatt-db.h"
>> #include "src/shared/gatt-server.h"
>> #include "src/shared/gatt-helpers.h"
>> -#include "src/shared/att-types.h"
>> #include "src/shared/util.h"
>>
>> #ifndef MAX
>> @@ -133,22 +133,6 @@ static void bt_gatt_server_free(struct bt_gatt_server *server)
>> free(server);
>> }
>>
>> -static uint8_t att_ecode_from_error(int err)
>> -{
>> - if (err < 0 || err > UINT8_MAX)
>> - return 0xff;
>> -
>> - return err;
>> -}
>> -
>> -static void encode_error_rsp(uint8_t opcode, uint16_t handle, uint8_t ecode,
>> - uint8_t pdu[4])
>> -{
>> - pdu[0] = opcode;
>> - pdu[3] = ecode;
>> - put_le16(handle, pdu + 1);
>> -}
>> -
>> static bool get_uuid_le(const uint8_t *uuid, size_t len, bt_uuid_t *out_uuid)
>> {
>> uint128_t u128;
>> @@ -315,8 +299,9 @@ static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
>>
>> error:
>> rsp_opcode = BT_ATT_OP_ERROR_RSP;
>> - rsp_len = 4;
>> - encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
>> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
>> + bt_att_encode_error_rsp(opcode, ehandle, ecode,
>> + (struct bt_att_pdu_error_rsp *) rsp_pdu);
>>
>> done:
>> queue_destroy(q, NULL);
>> @@ -355,13 +340,13 @@ static void read_by_type_read_complete_cb(struct gatt_db_attribute *attr,
>>
>> /* Terminate the operation if there was an error */
>> if (err) {
>> - uint8_t pdu[4];
>> - uint8_t att_ecode = att_ecode_from_error(err);
>> + struct bt_att_pdu_error_rsp error_pdu;
>>
>> - encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, handle, att_ecode,
>> - pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
>> - NULL, NULL);
>> + bt_att_encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, handle, err,
>> + &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> + NULL, NULL, NULL);
>> async_read_op_destroy(op);
>> return;
>> }
>> @@ -437,8 +422,9 @@ static void process_read_by_type(struct async_read_op *op)
>> error:
>> ehandle = gatt_db_attribute_get_handle(attr);
>> rsp_opcode = BT_ATT_OP_ERROR_RSP;
>> - rsp_len = 4;
>> - encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, ehandle, ecode, op->pdu);
>> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
>> + bt_att_encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, ehandle, ecode,
>> + (struct bt_att_pdu_error_rsp *) op->pdu);
>>
>> done:
>> bt_att_send(server->att, rsp_opcode, op->pdu, rsp_len, NULL,
>> @@ -452,7 +438,7 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
>> struct bt_gatt_server *server = user_data;
>> uint16_t start, end;
>> bt_uuid_t type;
>> - uint8_t rsp_pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>> uint16_t ehandle = 0;
>> uint8_t ecode;
>> struct queue *q = NULL;
>> @@ -524,9 +510,10 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
>> return;
>>
>> error:
>> - encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
>> + bt_att_encode_error_rsp(opcode, ehandle, ecode, &error_pdu);
>> queue_destroy(q, NULL);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> NULL, NULL, NULL);
>> }
>>
>> @@ -665,8 +652,9 @@ static void find_info_cb(uint8_t opcode, const void *pdu,
>>
>> error:
>> rsp_opcode = BT_ATT_OP_ERROR_RSP;
>> - rsp_len = 4;
>> - encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
>> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
>> + bt_att_encode_error_rsp(opcode, ehandle, ecode,
>> + (struct bt_att_pdu_error_rsp *) rsp_pdu);
>>
>> done:
>> queue_destroy(q, NULL);
>> @@ -697,11 +685,11 @@ static void write_complete_cb(struct gatt_db_attribute *attr, int err,
>> handle = gatt_db_attribute_get_handle(attr);
>>
>> if (err) {
>> - uint8_t rsp_pdu[4];
>> - uint8_t att_ecode = att_ecode_from_error(err);
>> + struct bt_att_pdu_error_rsp error_pdu;
>>
>> - encode_error_rsp(op->opcode, handle, att_ecode, rsp_pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
>> + bt_att_encode_error_rsp(op->opcode, handle, err, &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> NULL, NULL, NULL);
>> } else {
>> bt_att_send(server->att, BT_ATT_OP_WRITE_RSP, NULL, 0,
>> @@ -717,7 +705,7 @@ static void write_cb(uint8_t opcode, const void *pdu,
>> struct bt_gatt_server *server = user_data;
>> struct gatt_db_attribute *attr;
>> uint16_t handle = 0;
>> - uint8_t rsp_pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>> struct async_write_op *op = NULL;
>> uint8_t ecode;
>> uint32_t perm;
>> @@ -777,8 +765,9 @@ error:
>> if (opcode == BT_ATT_OP_WRITE_CMD)
>> return;
>>
>> - encode_error_rsp(opcode, handle, ecode, rsp_pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
>> + bt_att_encode_error_rsp(opcode, handle, ecode, &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> NULL, NULL, NULL);
>> }
>>
>> @@ -824,12 +813,12 @@ static void read_complete_cb(struct gatt_db_attribute *attr, int err,
>> handle = gatt_db_attribute_get_handle(attr);
>>
>> if (err) {
>> - uint8_t pdu[4];
>> - uint8_t att_ecode = att_ecode_from_error(err);
>> + struct bt_att_pdu_error_rsp error_pdu;
>>
>> - encode_error_rsp(op->opcode, handle, att_ecode, pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
>> - NULL, NULL);
>> + bt_att_encode_error_rsp(op->opcode, handle, err, &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> + NULL, NULL, NULL);
>> async_read_op_destroy(op);
>> return;
>> }
>> @@ -846,7 +835,7 @@ static void handle_read_req(struct bt_gatt_server *server, uint8_t opcode,
>> uint16_t handle,
>> uint16_t offset)
>> {
>> - uint8_t error_pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>> struct gatt_db_attribute *attr;
>> uint8_t ecode;
>> uint32_t perm;
>> @@ -898,9 +887,10 @@ error:
>> if (op)
>> async_read_op_destroy(op);
>>
>> - encode_error_rsp(opcode, handle, ecode, error_pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, error_pdu, 4, NULL, NULL,
>> - NULL);
>> + bt_att_encode_error_rsp(opcode, handle, ecode, &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> + NULL, NULL, NULL);
>> }
>>
>> static void read_cb(uint8_t opcode, const void *pdu,
>> @@ -910,11 +900,13 @@ static void read_cb(uint8_t opcode, const void *pdu,
>> uint16_t handle;
>>
>> if (length != 2) {
>> - uint8_t pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>>
>> - encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
>> - NULL, NULL);
>> + bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
>> + &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> + NULL, NULL, NULL);
>> return;
>> }
>>
>> @@ -930,11 +922,13 @@ static void read_blob_cb(uint8_t opcode, const void *pdu,
>> uint16_t handle, offset;
>>
>> if (length != 4) {
>> - uint8_t pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>>
>> - encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
>> - NULL, NULL);
>> + bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
>> + &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu),
>> + NULL, NULL, NULL);
>> return;
>> }
>>
>> @@ -1031,8 +1025,9 @@ error:
>> prep_write_data_destroy(prep_data);
>>
>> rsp_opcode = BT_ATT_OP_ERROR_RSP;
>> - rsp_len = 4;
>> - encode_error_rsp(opcode, handle, ecode, rsp_pdu);
>> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
>> + bt_att_encode_error_rsp(opcode, handle, ecode,
>> + (struct bt_att_pdu_error_rsp *) rsp_pdu);
>>
>> done:
>> bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
>> @@ -1040,30 +1035,29 @@ done:
>> }
>>
>> static void exec_next_prep_write(struct bt_gatt_server *server,
>> - uint16_t ehandle, uint8_t att_ecode);
>> + uint16_t ehandle, int err);
>>
>> static void exec_write_complete_cb(struct gatt_db_attribute *attr, int err,
>> void *user_data)
>> {
>> struct bt_gatt_server *server = user_data;
>> uint16_t handle = gatt_db_attribute_get_handle(attr);
>> - uint16_t att_ecode = att_ecode_from_error(err);
>>
>> - exec_next_prep_write(server, handle, att_ecode);
>> + exec_next_prep_write(server, handle, err);
>> }
>>
>> static void exec_next_prep_write(struct bt_gatt_server *server,
>> - uint16_t ehandle, uint8_t att_ecode)
>> + uint16_t ehandle, int err)
>> {
>> struct prep_write_data *next = NULL;
>> uint8_t rsp_opcode = BT_ATT_OP_EXEC_WRITE_RSP;
>> - uint8_t error_pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>> uint8_t *rsp_pdu = NULL;
>> uint16_t rsp_len = 0;
>> struct gatt_db_attribute *attr;
>> bool status;
>>
>> - if (att_ecode)
>> + if (err)
>> goto error;
>>
>> next = queue_pop_head(server->prep_queue);
>> @@ -1072,7 +1066,7 @@ static void exec_next_prep_write(struct bt_gatt_server *server,
>>
>> attr = gatt_db_get_attribute(server->db, next->handle);
>> if (!attr) {
>> - att_ecode = BT_ATT_ERROR_UNLIKELY;
>> + err = BT_ATT_ERROR_UNLIKELY;
>> goto error;
>> }
>>
>> @@ -1086,14 +1080,15 @@ static void exec_next_prep_write(struct bt_gatt_server *server,
>> if (status)
>> return;
>>
>> - att_ecode = BT_ATT_ERROR_UNLIKELY;
>> + err = BT_ATT_ERROR_UNLIKELY;
>>
>> error:
>> rsp_opcode = BT_ATT_OP_ERROR_RSP;
>> - rsp_len = 4;
>> - rsp_pdu = error_pdu;
>> - encode_error_rsp(BT_ATT_OP_EXEC_WRITE_REQ, ehandle, att_ecode, rsp_pdu);
>> + rsp_len = sizeof(error_pdu);
>> + bt_att_encode_error_rsp(BT_ATT_OP_EXEC_WRITE_REQ, ehandle, err,
>> + &error_pdu);
>>
>> + rsp_pdu = (uint8_t *) &error_pdu;
>> done:
>> bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
>> NULL);
>> @@ -1106,7 +1101,7 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
>> uint8_t flags;
>> uint8_t ecode;
>> uint8_t rsp_opcode;
>> - uint8_t error_pdu[4];
>> + struct bt_att_pdu_error_rsp error_pdu;
>> uint8_t *rsp_pdu = NULL;
>> uint16_t rsp_len = 0;
>> bool write;
>> @@ -1143,9 +1138,10 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
>>
>> error:
>> rsp_opcode = BT_ATT_OP_ERROR_RSP;
>> - rsp_len = 4;
>> - rsp_pdu = error_pdu;
>> - encode_error_rsp(opcode, 0, ecode, rsp_pdu);
>> + rsp_len = sizeof(error_pdu);
>> + bt_att_encode_error_rsp(opcode, 0, ecode, &error_pdu);
>> +
>> + rsp_pdu = (uint8_t *) &error_pdu;
>>
>> done:
>> bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
>> @@ -1158,12 +1154,15 @@ static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
>> struct bt_gatt_server *server = user_data;
>> uint16_t client_rx_mtu;
>> uint16_t final_mtu;
>> - uint8_t rsp_pdu[4];
>> + uint8_t rsp_pdu[2];
>>
>> if (length != 2) {
>> - encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, rsp_pdu);
>> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu,
>> - sizeof(rsp_pdu), NULL, NULL, NULL);
>> + struct bt_att_pdu_error_rsp error_pdu;
>> +
>> + bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
>> + &error_pdu);
>> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
>> + sizeof(error_pdu), NULL, NULL, NULL);
>> return;
>> }
>>
>> --
>> 2.1.0.rc2.206.gedb03e5
>>
>> --
>> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
>> the body of a message to [email protected]
>> More majordomo info at http://vger.kernel.org/majordomo-info.html
>
>
>
> --
> Luiz Augusto von Dentz
> --
> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
> the body of a message to [email protected]
> More majordomo info at http://vger.kernel.org/majordomo-info.html
Thanks,
Arman
Hi Arman,
On Thu, Nov 13, 2014 at 2:36 AM, Arman Uguray <[email protected]> wrote:
> This patch adds bt_att_encode_error_rsp which allows encoding an ATT
> protocol error response by correctly converting system errnos to ATT
> protocol error codes. struct bt_att_pdu_error_rsp is introduced to
> represent an error response PDU which can be sent directly over the
> wire once properly encoded in network order.
> ---
> src/shared/att-types.h | 12 ++++
> src/shared/att.c | 58 +++++++++++++++++-
> src/shared/att.h | 3 +
> src/shared/gatt-client.c | 8 ++-
> src/shared/gatt-helpers.c | 8 ++-
> src/shared/gatt-server.c | 153 +++++++++++++++++++++++-----------------------
> 6 files changed, 160 insertions(+), 82 deletions(-)
>
> diff --git a/src/shared/att-types.h b/src/shared/att-types.h
> index 24bf3da..3d8829a 100644
> --- a/src/shared/att-types.h
> +++ b/src/shared/att-types.h
> @@ -23,6 +23,10 @@
>
> #include <stdint.h>
>
> +#ifndef __packed
> +#define __packed __attribute__((packed))
> +#endif
> +
> #define BT_ATT_DEFAULT_LE_MTU 23
>
> /* ATT protocol opcodes */
> @@ -55,6 +59,14 @@
> #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
>
> diff --git a/src/shared/att.c b/src/shared/att.c
> index 6e1e538..e831078 100644
> --- a/src/shared/att.c
> +++ b/src/shared/att.c
> @@ -35,13 +35,22 @@
> #include "src/shared/timeout.h"
> #include "lib/uuid.h"
> #include "src/shared/att.h"
> -#include "src/shared/att-types.h"
>
> #define ATT_MIN_PDU_LEN 1 /* At least 1 byte for the opcode. */
> #define ATT_OP_CMD_MASK 0x40
> #define ATT_OP_SIGNED_MASK 0x80
> #define ATT_TIMEOUT_INTERVAL 30000 /* 30000 ms */
>
> +/*
> + * Common Profile and Service Error Code descriptions (see Supplement to the
> + * Bluetooth Core Specification, sections 1.2 and 2). The error codes within
> + * 0xE0-0xFC are reserved for future use. The remaining 3 are defined as the
> + * following:
> + */
> +#define BT_ERROR_CCC_IMPROPERLY_CONFIGURED 0xfd
> +#define BT_ERROR_ALREADY_IN_PROGRESS 0xfe
> +#define BT_ERROR_OUT_OF_RANGE 0xff
> +
> struct att_send_op;
>
> struct bt_att {
> @@ -1218,3 +1227,50 @@ bool bt_att_unregister_all(struct bt_att *att)
>
> 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;
> +}
> +
> +bool bt_att_encode_error_rsp(uint8_t opcode, uint16_t handle, int error,
> + struct bt_att_pdu_error_rsp *pdu)
> +{
> + uint8_t ecode;
> +
> + if (!pdu)
> + return false;
> +
> + ecode = att_ecode_from_error(error);
> +
> + memset(pdu, 0, sizeof(*pdu));
> +
> + pdu->opcode = opcode;
> + put_le16(handle, &pdu->handle);
> + pdu->ecode = ecode;
> +
> + return true;
> +}
> diff --git a/src/shared/att.h b/src/shared/att.h
> index 1063021..a1a4821 100644
> --- a/src/shared/att.h
> +++ b/src/shared/att.h
> @@ -76,3 +76,6 @@ unsigned int bt_att_register_disconnect(struct bt_att *att,
> bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id);
>
> bool bt_att_unregister_all(struct bt_att *att);
> +
> +bool bt_att_encode_error_rsp(uint8_t opcode, uint16_t handle, int error,
> + struct bt_att_pdu_error_rsp *pdu);
Lets drop the encode part from the name, it should be implicit that it
will encode before sending. Also Im not sure why you have chosen to
pass the parameter as a struct, you could have passed separately by
value so encoding does not affect the caller stack, also I was
assuming that it would send the PDU after encoding it.
> diff --git a/src/shared/gatt-client.c b/src/shared/gatt-client.c
> index 30b271e..1f82048 100644
> --- a/src/shared/gatt-client.c
> +++ b/src/shared/gatt-client.c
> @@ -1285,10 +1285,14 @@ static bool notify_data_write_ccc(struct notify_data *notify_data, bool enable,
>
> static uint8_t process_error(const void *pdu, uint16_t length)
> {
> - if (!pdu || length != 4)
> + const struct bt_att_pdu_error_rsp *error_pdu;
> +
> + if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
> return 0;
>
> - return ((uint8_t *) pdu)[3];
> + error_pdu = pdu;
> +
> + return error_pdu->ecode;
> }
>
> static void enable_ccc_callback(uint8_t opcode, const void *pdu,
> diff --git a/src/shared/gatt-helpers.c b/src/shared/gatt-helpers.c
> index 220d0eb..9c8a3ba 100644
> --- a/src/shared/gatt-helpers.c
> +++ b/src/shared/gatt-helpers.c
> @@ -430,10 +430,14 @@ static void destroy_mtu_op(void *user_data)
>
> static uint8_t process_error(const void *pdu, uint16_t length)
> {
> - if (!pdu || length != 4)
> + const struct bt_att_pdu_error_rsp *error_pdu;
> +
> + if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
> return 0;
>
> - return ((uint8_t *) pdu)[3];
> + error_pdu = pdu;
> +
> + return error_pdu->ecode;
> }
>
> static void mtu_cb(uint8_t opcode, const void *pdu, uint16_t length,
> diff --git a/src/shared/gatt-server.c b/src/shared/gatt-server.c
> index 2ca318a..3262992 100644
> --- a/src/shared/gatt-server.c
> +++ b/src/shared/gatt-server.c
> @@ -22,6 +22,7 @@
> */
>
> #include <sys/uio.h>
> +#include <errno.h>
>
> #include "src/shared/att.h"
> #include "lib/uuid.h"
> @@ -29,7 +30,6 @@
> #include "src/shared/gatt-db.h"
> #include "src/shared/gatt-server.h"
> #include "src/shared/gatt-helpers.h"
> -#include "src/shared/att-types.h"
> #include "src/shared/util.h"
>
> #ifndef MAX
> @@ -133,22 +133,6 @@ static void bt_gatt_server_free(struct bt_gatt_server *server)
> free(server);
> }
>
> -static uint8_t att_ecode_from_error(int err)
> -{
> - if (err < 0 || err > UINT8_MAX)
> - return 0xff;
> -
> - return err;
> -}
> -
> -static void encode_error_rsp(uint8_t opcode, uint16_t handle, uint8_t ecode,
> - uint8_t pdu[4])
> -{
> - pdu[0] = opcode;
> - pdu[3] = ecode;
> - put_le16(handle, pdu + 1);
> -}
> -
> static bool get_uuid_le(const uint8_t *uuid, size_t len, bt_uuid_t *out_uuid)
> {
> uint128_t u128;
> @@ -315,8 +299,9 @@ static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
>
> error:
> rsp_opcode = BT_ATT_OP_ERROR_RSP;
> - rsp_len = 4;
> - encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
> + bt_att_encode_error_rsp(opcode, ehandle, ecode,
> + (struct bt_att_pdu_error_rsp *) rsp_pdu);
>
> done:
> queue_destroy(q, NULL);
> @@ -355,13 +340,13 @@ static void read_by_type_read_complete_cb(struct gatt_db_attribute *attr,
>
> /* Terminate the operation if there was an error */
> if (err) {
> - uint8_t pdu[4];
> - uint8_t att_ecode = att_ecode_from_error(err);
> + struct bt_att_pdu_error_rsp error_pdu;
>
> - encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, handle, att_ecode,
> - pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
> - NULL, NULL);
> + bt_att_encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, handle, err,
> + &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> + NULL, NULL, NULL);
> async_read_op_destroy(op);
> return;
> }
> @@ -437,8 +422,9 @@ static void process_read_by_type(struct async_read_op *op)
> error:
> ehandle = gatt_db_attribute_get_handle(attr);
> rsp_opcode = BT_ATT_OP_ERROR_RSP;
> - rsp_len = 4;
> - encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, ehandle, ecode, op->pdu);
> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
> + bt_att_encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, ehandle, ecode,
> + (struct bt_att_pdu_error_rsp *) op->pdu);
>
> done:
> bt_att_send(server->att, rsp_opcode, op->pdu, rsp_len, NULL,
> @@ -452,7 +438,7 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
> struct bt_gatt_server *server = user_data;
> uint16_t start, end;
> bt_uuid_t type;
> - uint8_t rsp_pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
> uint16_t ehandle = 0;
> uint8_t ecode;
> struct queue *q = NULL;
> @@ -524,9 +510,10 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
> return;
>
> error:
> - encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
> + bt_att_encode_error_rsp(opcode, ehandle, ecode, &error_pdu);
> queue_destroy(q, NULL);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> NULL, NULL, NULL);
> }
>
> @@ -665,8 +652,9 @@ static void find_info_cb(uint8_t opcode, const void *pdu,
>
> error:
> rsp_opcode = BT_ATT_OP_ERROR_RSP;
> - rsp_len = 4;
> - encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
> + bt_att_encode_error_rsp(opcode, ehandle, ecode,
> + (struct bt_att_pdu_error_rsp *) rsp_pdu);
>
> done:
> queue_destroy(q, NULL);
> @@ -697,11 +685,11 @@ static void write_complete_cb(struct gatt_db_attribute *attr, int err,
> handle = gatt_db_attribute_get_handle(attr);
>
> if (err) {
> - uint8_t rsp_pdu[4];
> - uint8_t att_ecode = att_ecode_from_error(err);
> + struct bt_att_pdu_error_rsp error_pdu;
>
> - encode_error_rsp(op->opcode, handle, att_ecode, rsp_pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
> + bt_att_encode_error_rsp(op->opcode, handle, err, &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> NULL, NULL, NULL);
> } else {
> bt_att_send(server->att, BT_ATT_OP_WRITE_RSP, NULL, 0,
> @@ -717,7 +705,7 @@ static void write_cb(uint8_t opcode, const void *pdu,
> struct bt_gatt_server *server = user_data;
> struct gatt_db_attribute *attr;
> uint16_t handle = 0;
> - uint8_t rsp_pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
> struct async_write_op *op = NULL;
> uint8_t ecode;
> uint32_t perm;
> @@ -777,8 +765,9 @@ error:
> if (opcode == BT_ATT_OP_WRITE_CMD)
> return;
>
> - encode_error_rsp(opcode, handle, ecode, rsp_pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
> + bt_att_encode_error_rsp(opcode, handle, ecode, &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> NULL, NULL, NULL);
> }
>
> @@ -824,12 +813,12 @@ static void read_complete_cb(struct gatt_db_attribute *attr, int err,
> handle = gatt_db_attribute_get_handle(attr);
>
> if (err) {
> - uint8_t pdu[4];
> - uint8_t att_ecode = att_ecode_from_error(err);
> + struct bt_att_pdu_error_rsp error_pdu;
>
> - encode_error_rsp(op->opcode, handle, att_ecode, pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
> - NULL, NULL);
> + bt_att_encode_error_rsp(op->opcode, handle, err, &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> + NULL, NULL, NULL);
> async_read_op_destroy(op);
> return;
> }
> @@ -846,7 +835,7 @@ static void handle_read_req(struct bt_gatt_server *server, uint8_t opcode,
> uint16_t handle,
> uint16_t offset)
> {
> - uint8_t error_pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
> struct gatt_db_attribute *attr;
> uint8_t ecode;
> uint32_t perm;
> @@ -898,9 +887,10 @@ error:
> if (op)
> async_read_op_destroy(op);
>
> - encode_error_rsp(opcode, handle, ecode, error_pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, error_pdu, 4, NULL, NULL,
> - NULL);
> + bt_att_encode_error_rsp(opcode, handle, ecode, &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> + NULL, NULL, NULL);
> }
>
> static void read_cb(uint8_t opcode, const void *pdu,
> @@ -910,11 +900,13 @@ static void read_cb(uint8_t opcode, const void *pdu,
> uint16_t handle;
>
> if (length != 2) {
> - uint8_t pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
>
> - encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
> - NULL, NULL);
> + bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
> + &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> + NULL, NULL, NULL);
> return;
> }
>
> @@ -930,11 +922,13 @@ static void read_blob_cb(uint8_t opcode, const void *pdu,
> uint16_t handle, offset;
>
> if (length != 4) {
> - uint8_t pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
>
> - encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
> - NULL, NULL);
> + bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
> + &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu),
> + NULL, NULL, NULL);
> return;
> }
>
> @@ -1031,8 +1025,9 @@ error:
> prep_write_data_destroy(prep_data);
>
> rsp_opcode = BT_ATT_OP_ERROR_RSP;
> - rsp_len = 4;
> - encode_error_rsp(opcode, handle, ecode, rsp_pdu);
> + rsp_len = sizeof(struct bt_att_pdu_error_rsp);
> + bt_att_encode_error_rsp(opcode, handle, ecode,
> + (struct bt_att_pdu_error_rsp *) rsp_pdu);
>
> done:
> bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
> @@ -1040,30 +1035,29 @@ done:
> }
>
> static void exec_next_prep_write(struct bt_gatt_server *server,
> - uint16_t ehandle, uint8_t att_ecode);
> + uint16_t ehandle, int err);
>
> static void exec_write_complete_cb(struct gatt_db_attribute *attr, int err,
> void *user_data)
> {
> struct bt_gatt_server *server = user_data;
> uint16_t handle = gatt_db_attribute_get_handle(attr);
> - uint16_t att_ecode = att_ecode_from_error(err);
>
> - exec_next_prep_write(server, handle, att_ecode);
> + exec_next_prep_write(server, handle, err);
> }
>
> static void exec_next_prep_write(struct bt_gatt_server *server,
> - uint16_t ehandle, uint8_t att_ecode)
> + uint16_t ehandle, int err)
> {
> struct prep_write_data *next = NULL;
> uint8_t rsp_opcode = BT_ATT_OP_EXEC_WRITE_RSP;
> - uint8_t error_pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
> uint8_t *rsp_pdu = NULL;
> uint16_t rsp_len = 0;
> struct gatt_db_attribute *attr;
> bool status;
>
> - if (att_ecode)
> + if (err)
> goto error;
>
> next = queue_pop_head(server->prep_queue);
> @@ -1072,7 +1066,7 @@ static void exec_next_prep_write(struct bt_gatt_server *server,
>
> attr = gatt_db_get_attribute(server->db, next->handle);
> if (!attr) {
> - att_ecode = BT_ATT_ERROR_UNLIKELY;
> + err = BT_ATT_ERROR_UNLIKELY;
> goto error;
> }
>
> @@ -1086,14 +1080,15 @@ static void exec_next_prep_write(struct bt_gatt_server *server,
> if (status)
> return;
>
> - att_ecode = BT_ATT_ERROR_UNLIKELY;
> + err = BT_ATT_ERROR_UNLIKELY;
>
> error:
> rsp_opcode = BT_ATT_OP_ERROR_RSP;
> - rsp_len = 4;
> - rsp_pdu = error_pdu;
> - encode_error_rsp(BT_ATT_OP_EXEC_WRITE_REQ, ehandle, att_ecode, rsp_pdu);
> + rsp_len = sizeof(error_pdu);
> + bt_att_encode_error_rsp(BT_ATT_OP_EXEC_WRITE_REQ, ehandle, err,
> + &error_pdu);
>
> + rsp_pdu = (uint8_t *) &error_pdu;
> done:
> bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
> NULL);
> @@ -1106,7 +1101,7 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
> uint8_t flags;
> uint8_t ecode;
> uint8_t rsp_opcode;
> - uint8_t error_pdu[4];
> + struct bt_att_pdu_error_rsp error_pdu;
> uint8_t *rsp_pdu = NULL;
> uint16_t rsp_len = 0;
> bool write;
> @@ -1143,9 +1138,10 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
>
> error:
> rsp_opcode = BT_ATT_OP_ERROR_RSP;
> - rsp_len = 4;
> - rsp_pdu = error_pdu;
> - encode_error_rsp(opcode, 0, ecode, rsp_pdu);
> + rsp_len = sizeof(error_pdu);
> + bt_att_encode_error_rsp(opcode, 0, ecode, &error_pdu);
> +
> + rsp_pdu = (uint8_t *) &error_pdu;
>
> done:
> bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
> @@ -1158,12 +1154,15 @@ static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
> struct bt_gatt_server *server = user_data;
> uint16_t client_rx_mtu;
> uint16_t final_mtu;
> - uint8_t rsp_pdu[4];
> + uint8_t rsp_pdu[2];
>
> if (length != 2) {
> - encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, rsp_pdu);
> - bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu,
> - sizeof(rsp_pdu), NULL, NULL, NULL);
> + struct bt_att_pdu_error_rsp error_pdu;
> +
> + bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
> + &error_pdu);
> + bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
> + sizeof(error_pdu), NULL, NULL, NULL);
> return;
> }
>
> --
> 2.1.0.rc2.206.gedb03e5
>
> --
> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
> the body of a message to [email protected]
> More majordomo info at http://vger.kernel.org/majordomo-info.html
--
Luiz Augusto von Dentz
This patch adds a Heart Rate service to the database which simulates a
fake Heart Rate service implementation. The service can be enabled by
default by passing the '-r' flag to the executable.
---
tools/btgatt-server.c | 211 ++++++++++++++++++++++++++++++++++++++++++++++++--
1 file changed, 204 insertions(+), 7 deletions(-)
diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
index 4fcf10e..76ebc9a 100644
--- a/tools/btgatt-server.c
+++ b/tools/btgatt-server.c
@@ -23,6 +23,7 @@
#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>
+#include <time.h>
#include <stdlib.h>
#include <getopt.h>
#include <unistd.h>
@@ -38,12 +39,18 @@
#include "src/shared/util.h"
#include "src/shared/att.h"
#include "src/shared/queue.h"
+#include "src/shared/timeout.h"
#include "src/shared/gatt-db.h"
#include "src/shared/gatt-server.h"
-#define ATT_CID 4
+#define UUID_GAP 0x1800
+#define UUID_GATT 0x1801
+#define UUID_HEART_RATE 0x180d
+#define UUID_HEART_RATE_MSRMT 0x2a37
+#define UUID_HEART_RATE_BODY 0x2a38
+#define UUID_HEART_RATE_CTRL 0x2a39
-#define UUID_GAP 0x1800
+#define ATT_CID 4
#define PRLOG(...) \
do { \
@@ -77,6 +84,13 @@ struct server {
size_t name_len;
bool svc_chngd_enabled;
+
+ uint16_t hr_msrmt_handle;
+ uint16_t hr_energy_expended;
+ bool hr_visible;
+ bool hr_msrmt_enabled;
+ int hr_ee_count;
+ unsigned int hr_timeout_id;
};
static void print_prompt(void)
@@ -248,6 +262,127 @@ done:
gatt_db_attribute_write_result(attrib, id, ecode);
}
+static void hr_msrmt_ccc_read_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t value[2];
+
+ value[0] = server->hr_msrmt_enabled ? 0x01 : 0x00;
+ value[1] = 0x00;
+
+ gatt_db_attribute_read_result(attrib, id, 0, value, 2);
+}
+
+static bool hr_msrmt_cb(void *user_data)
+{
+ struct server *server = user_data;
+ bool expended_present = !(server->hr_ee_count % 10);
+ uint16_t len = 2;
+ uint8_t pdu[4];
+ uint32_t cur_ee;
+
+ pdu[0] = 0x06;
+ pdu[1] = 90 + (rand() % 40);
+
+ if (expended_present) {
+ pdu[0] |= 0x08;
+ put_le16(server->hr_energy_expended, pdu + 2);
+ len += 2;
+ }
+
+ bt_gatt_server_send_notification(server->gatt,
+ server->hr_msrmt_handle,
+ pdu, len);
+
+
+ cur_ee = server->hr_energy_expended;
+ server->hr_energy_expended = MIN(UINT16_MAX, cur_ee + 10);
+ server->hr_ee_count++;
+
+ return true;
+}
+
+static void update_hr_msrmt_simulation(struct server *server)
+{
+ if (!server->hr_msrmt_enabled || !server->hr_visible) {
+ timeout_remove(server->hr_timeout_id);
+ return;
+ }
+
+ server->hr_timeout_id = timeout_add(1000, hr_msrmt_cb, server, NULL);
+}
+
+static void hr_msrmt_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t ecode = 0;
+
+ if (!value || len != 2) {
+ ecode = BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN;
+ goto done;
+ }
+
+ if (offset) {
+ ecode = BT_ATT_ERROR_INVALID_OFFSET;
+ goto done;
+ }
+
+ if (value[0] == 0x00)
+ server->hr_msrmt_enabled = false;
+ else if (value[0] == 0x01) {
+ if (server->hr_msrmt_enabled) {
+ PRLOG("HR Measurement Already Enabled\n");
+ goto done;
+ }
+
+ server->hr_msrmt_enabled = true;
+ } else
+ ecode = 0x80;
+
+ PRLOG("HR: Measurement Enabled: %s\n",
+ server->hr_msrmt_enabled ? "true" : "false");
+
+ update_hr_msrmt_simulation(server);
+
+done:
+ gatt_db_attribute_write_result(attrib, id, ecode);
+}
+
+static void hr_control_point_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t ecode = 0;
+
+ if (!value || len != 1) {
+ ecode = BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN;
+ goto done;
+ }
+
+ if (offset) {
+ ecode = BT_ATT_ERROR_INVALID_OFFSET;
+ goto done;
+ }
+
+ if (value[0] == 1) {
+ PRLOG("HR: Energy Expended value reset\n");
+ server->hr_energy_expended = 0;
+ }
+
+done:
+ gatt_db_attribute_write_result(attrib, id, ecode);
+}
+
static void confirm_write(struct gatt_db_attribute *attr, int err,
void *user_data)
{
@@ -315,7 +450,7 @@ static void populate_gatt_service(struct server *server)
struct gatt_db_attribute *service;
/* Add the GATT service */
- bt_uuid16_create(&uuid, 0x1801);
+ bt_uuid16_create(&uuid, UUID_GATT);
service = gatt_db_add_service(server->db, &uuid, true, 4);
bt_uuid16_create(&uuid, GATT_CHARAC_SERVICE_CHANGED);
@@ -333,13 +468,64 @@ static void populate_gatt_service(struct server *server)
gatt_db_service_set_active(service, true);
}
+static void populate_hr_service(struct server *server)
+{
+ bt_uuid_t uuid;
+ struct gatt_db_attribute *service, *hr_msrmt, *body;
+ uint8_t body_loc = 1; /* "Chest" */
+
+ /* Add Heart Rate Service */
+ bt_uuid16_create(&uuid, UUID_HEART_RATE);
+ service = gatt_db_add_service(server->db, &uuid, true, 8);
+
+ /* HR Measurement Characteristic */
+ bt_uuid16_create(&uuid, UUID_HEART_RATE_MSRMT);
+ hr_msrmt = gatt_db_service_add_characteristic(service, &uuid,
+ BT_ATT_PERM_NONE,
+ BT_GATT_CHRC_PROP_NOTIFY,
+ NULL, NULL, NULL);
+ server->hr_msrmt_handle = gatt_db_attribute_get_handle(hr_msrmt);
+
+ bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
+ gatt_db_service_add_descriptor(service, &uuid,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ hr_msrmt_ccc_read_cb,
+ hr_msrmt_ccc_write_cb, server);
+
+ /*
+ * Body Sensor Location Characteristic. Make reads obtain the value from
+ * the database.
+ */
+ bt_uuid16_create(&uuid, UUID_HEART_RATE_BODY);
+ body = gatt_db_service_add_characteristic(service, &uuid,
+ BT_ATT_PERM_READ,
+ BT_GATT_CHRC_PROP_READ,
+ NULL, NULL, server);
+ gatt_db_attribute_write(body, 0, (void *) &body_loc, sizeof(body_loc),
+ BT_ATT_OP_WRITE_REQ,
+ NULL, confirm_write,
+ NULL);
+
+ /* HR Control Point Characteristic */
+ bt_uuid16_create(&uuid, UUID_HEART_RATE_CTRL);
+ gatt_db_service_add_characteristic(service, &uuid,
+ BT_ATT_PERM_WRITE,
+ BT_GATT_CHRC_PROP_WRITE,
+ NULL, hr_control_point_write_cb,
+ server);
+
+ if (server->hr_visible)
+ gatt_db_service_set_active(service, true);
+}
+
static void populate_db(struct server *server)
{
populate_gap_service(server);
populate_gatt_service(server);
+ populate_hr_service(server);
}
-static struct server *server_create(int fd, uint16_t mtu)
+static struct server *server_create(int fd, uint16_t mtu, bool hr_visible)
{
struct server *server;
struct bt_att *att;
@@ -390,15 +576,19 @@ static struct server *server_create(int fd, uint16_t mtu)
goto fail;
}
+ server->hr_visible = hr_visible;
+
if (verbose) {
bt_att_set_debug(att, att_debug_cb, "att: ", NULL);
bt_gatt_server_set_debug(server->gatt, gatt_debug_cb,
"server: ", NULL);
}
+ /* Random seed for generating fake Heart Rate measurements */
+ srand(time(NULL));
+
/* bt_gatt_server already holds a reference */
bt_att_unref(att);
-
populate_db(server);
return server;
@@ -414,6 +604,7 @@ fail:
static void server_destroy(struct server *server)
{
+ timeout_remove(server->hr_timeout_id);
bt_gatt_server_unref(server->gatt);
gatt_db_destroy(server->db);
}
@@ -429,6 +620,7 @@ static void usage(void)
"\t-s, --security-level <sec>\tSet security level (low|"
"medium|high)\n"
"\t-v, --verbose\t\t\tEnable extra logging\n"
+ "\t-r, --heart-rate\t\tEnable Heart Rate service"
"\t-h, --help\t\t\tDisplay help\n");
}
@@ -437,6 +629,7 @@ static struct option main_options[] = {
{ "mtu", 1, 0, 'm' },
{ "security-level", 1, 0, 's' },
{ "verbose", 0, 0, 'v' },
+ { "heart-rate", 0, 0, 'r' },
{ "help", 0, 0, 'h' },
{ }
};
@@ -722,9 +915,10 @@ int main(int argc, char *argv[])
int dev_id = -1;
int fd;
sigset_t mask;
+ bool hr_visible = false;
struct server *server;
- while ((opt = getopt_long(argc, argv, "+hvs:m:i:",
+ while ((opt = getopt_long(argc, argv, "+hvrs:m:i:",
main_options, NULL)) != -1) {
switch (opt) {
case 'h':
@@ -733,6 +927,9 @@ int main(int argc, char *argv[])
case 'v':
verbose = true;
break;
+ case 'r':
+ hr_visible = true;
+ break;
case 's':
if (strcmp(optarg, "low") == 0)
sec = BT_SECURITY_LOW;
@@ -800,7 +997,7 @@ int main(int argc, char *argv[])
mainloop_init();
- server = server_create(fd, mtu);
+ server = server_create(fd, mtu, hr_visible);
if (!server) {
close(fd);
return EXIT_FAILURE;
--
2.1.0.rc2.206.gedb03e5
This patch adds the "heart-rate" command which can be used to hide
or unhide the Heart Rate service. This is useful for testing service
changed events.
---
tools/btgatt-server.c | 60 +++++++++++++++++++++++++++++++++++++++++++++++++--
1 file changed, 58 insertions(+), 2 deletions(-)
diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
index 76ebc9a..13f4a25 100644
--- a/tools/btgatt-server.c
+++ b/tools/btgatt-server.c
@@ -83,8 +83,10 @@ struct server {
uint8_t *device_name;
size_t name_len;
+ uint16_t gatt_svc_chngd_handle;
bool svc_chngd_enabled;
+ uint16_t hr_handle;
uint16_t hr_msrmt_handle;
uint16_t hr_energy_expended;
bool hr_visible;
@@ -447,17 +449,19 @@ static void populate_gap_service(struct server *server)
static void populate_gatt_service(struct server *server)
{
bt_uuid_t uuid;
- struct gatt_db_attribute *service;
+ struct gatt_db_attribute *service, *svc_chngd;
/* Add the GATT service */
bt_uuid16_create(&uuid, UUID_GATT);
service = gatt_db_add_service(server->db, &uuid, true, 4);
bt_uuid16_create(&uuid, GATT_CHARAC_SERVICE_CHANGED);
- gatt_db_service_add_characteristic(service, &uuid, BT_ATT_PERM_READ,
+ svc_chngd = gatt_db_service_add_characteristic(service, &uuid,
+ BT_ATT_PERM_READ,
BT_GATT_CHRC_PROP_READ | BT_GATT_CHRC_PROP_INDICATE,
gatt_service_changed_cb,
NULL, server);
+ server->gatt_svc_chngd_handle = gatt_db_attribute_get_handle(svc_chngd);
bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
gatt_db_service_add_descriptor(service, &uuid,
@@ -477,6 +481,7 @@ static void populate_hr_service(struct server *server)
/* Add Heart Rate Service */
bt_uuid16_create(&uuid, UUID_HEART_RATE);
service = gatt_db_add_service(server->db, &uuid, true, 8);
+ server->hr_handle = gatt_db_attribute_get_handle(service);
/* HR Measurement Characteristic */
bt_uuid16_create(&uuid, UUID_HEART_RATE_MSRMT);
@@ -822,6 +827,56 @@ done:
free(value);
}
+static void heart_rate_usage(void)
+{
+ printf("Usage: heart-rate on|off\n");
+}
+
+static void cmd_heart_rate(struct server *server, char *cmd_str)
+{
+ bool enable;
+ uint8_t pdu[4];
+ struct gatt_db_attribute *attr;
+
+ if (!cmd_str) {
+ heart_rate_usage();
+ return;
+ }
+
+ if (strcmp(cmd_str, "on") == 0)
+ enable = true;
+ else if (strcmp(cmd_str, "off") == 0)
+ enable = false;
+ else {
+ heart_rate_usage();
+ return;
+ }
+
+ if (enable == server->hr_visible) {
+ printf("Heart Rate Service already %s\n",
+ enable ? "visible" : "hidden");
+ return;
+ }
+
+ server->hr_visible = enable;
+ attr = gatt_db_get_attribute(server->db, server->hr_handle);
+ gatt_db_service_set_active(attr, server->hr_visible);
+ update_hr_msrmt_simulation(server);
+
+ if (!server->svc_chngd_enabled)
+ return;
+
+ put_le16(server->hr_handle, pdu);
+ put_le16(server->hr_handle + 7, pdu + 2);
+
+ server->hr_msrmt_enabled = false;
+ update_hr_msrmt_simulation(server);
+
+ bt_gatt_server_send_indication(server->gatt,
+ server->gatt_svc_chngd_handle,
+ pdu, 4, conf_cb, NULL, NULL);
+}
+
static void cmd_help(struct server *server, char *cmd_str);
typedef void (*command_func_t)(struct server *server, char *cmd_str);
@@ -833,6 +888,7 @@ static struct {
} command[] = {
{ "help", cmd_help, "\tDisplay help message" },
{ "notify", cmd_notify, "\tSend handle-value notification" },
+ { "heart-rate", cmd_heart_rate, "\tHide/Unhide Heart Rate Service" },
{ }
};
--
2.1.0.rc2.206.gedb03e5
This patch adds a command REPL and introduces the "notify" command which
allows sending notifications and indications.
---
tools/btgatt-server.c | 230 ++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 230 insertions(+)
diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
index 545c8c4..4fcf10e 100644
--- a/tools/btgatt-server.c
+++ b/tools/btgatt-server.c
@@ -26,6 +26,7 @@
#include <stdlib.h>
#include <getopt.h>
#include <unistd.h>
+#include <errno.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
@@ -501,6 +502,217 @@ fail:
return -1;
}
+static void notify_usage(void)
+{
+ printf("Usage: notify [options] <value_handle> <value>\n"
+ "Options:\n"
+ "\t -i, --indicate\tSend indication\n"
+ "e.g.:\n"
+ "\tnotify 0x0001 00 01 00\n");
+}
+
+static struct option notify_options[] = {
+ { "indicate", 0, 0, 'i' },
+ { }
+};
+
+static bool parse_args(char *str, int expected_argc, char **argv, int *argc)
+{
+ char **ap;
+
+ for (ap = argv; (*ap = strsep(&str, " \t")) != NULL;) {
+ if (**ap == '\0')
+ continue;
+
+ (*argc)++;
+ ap++;
+
+ if (*argc > expected_argc)
+ return false;
+ }
+
+ return true;
+}
+
+static void conf_cb(void *user_data)
+{
+ PRLOG("Received confirmation\n");
+}
+
+static void cmd_notify(struct server *server, char *cmd_str)
+{
+ int opt, i;
+ char *argvbuf[516];
+ char **argv = argvbuf;
+ int argc = 1;
+ uint16_t handle;
+ char *endptr = NULL;
+ int length;
+ uint8_t *value = NULL;
+ bool indicate = false;
+
+ if (!parse_args(cmd_str, 514, argv + 1, &argc)) {
+ printf("Too many arguments\n");
+ notify_usage();
+ return;
+ }
+
+ optind = 0;
+ argv[0] = "notify";
+ while ((opt = getopt_long(argc, argv, "+i", notify_options,
+ NULL)) != -1) {
+ switch (opt) {
+ case 'i':
+ indicate = true;
+ break;
+ default:
+ notify_usage();
+ return;
+ }
+ }
+
+ argc -= optind;
+ argv += optind;
+
+ if (argc < 1) {
+ notify_usage();
+ return;
+ }
+
+ handle = strtol(argv[0], &endptr, 16);
+ if (!endptr || *endptr != '\0' || !handle) {
+ printf("Invalid handle: %s\n", argv[0]);
+ return;
+ }
+
+ length = argc - 1;
+
+ if (length > 0) {
+ if (length > UINT16_MAX) {
+ printf("Value too long\n");
+ return;
+ }
+
+ value = malloc(length);
+ if (!value) {
+ printf("Failed to construct value\n");
+ return;
+ }
+
+ for (i = 1; i < argc; i++) {
+ if (strlen(argv[i]) != 2) {
+ printf("Invalid value byte: %s\n",
+ argv[i]);
+ goto done;
+ }
+
+ value[i-1] = strtol(argv[i], &endptr, 16);
+ if (endptr == argv[i] || *endptr != '\0'
+ || errno == ERANGE) {
+ printf("Invalid value byte: %s\n",
+ argv[i]);
+ goto done;
+ }
+ }
+ }
+
+ if (indicate) {
+ if (!bt_gatt_server_send_indication(server->gatt, handle,
+ value, length,
+ conf_cb, NULL, NULL))
+ printf("Failed to initiate indication\n");
+ } else if (!bt_gatt_server_send_notification(server->gatt, handle,
+ value, length))
+ printf("Failed to initiate notification\n");
+
+done:
+ free(value);
+}
+
+static void cmd_help(struct server *server, char *cmd_str);
+
+typedef void (*command_func_t)(struct server *server, char *cmd_str);
+
+static struct {
+ char *cmd;
+ command_func_t func;
+ char *doc;
+} command[] = {
+ { "help", cmd_help, "\tDisplay help message" },
+ { "notify", cmd_notify, "\tSend handle-value notification" },
+ { }
+};
+
+static void cmd_help(struct server *server, char *cmd_str)
+{
+ int i;
+
+ printf("Commands:\n");
+ for (i = 0; command[i].cmd; i++)
+ printf("\t%-15s\t%s\n", command[i].cmd, command[i].doc);
+}
+
+static void prompt_read_cb(int fd, uint32_t events, void *user_data)
+{
+ ssize_t read;
+ size_t len = 0;
+ char *line = NULL;
+ char *cmd = NULL, *args;
+ struct server *server = user_data;
+ int i;
+
+ if (events & (EPOLLRDHUP | EPOLLHUP | EPOLLERR)) {
+ mainloop_quit();
+ return;
+ }
+
+ if ((read = getline(&line, &len, stdin)) == -1)
+ return;
+
+ if (read <= 1) {
+ cmd_help(server, NULL);
+ print_prompt();
+ return;
+ }
+
+ line[read-1] = '\0';
+ args = line;
+
+ while ((cmd = strsep(&args, " \t")))
+ if (*cmd != '\0')
+ break;
+
+ if (!cmd)
+ goto failed;
+
+ for (i = 0; command[i].cmd; i++) {
+ if (strcmp(command[i].cmd, cmd) == 0)
+ break;
+ }
+
+ if (command[i].cmd)
+ command[i].func(server, args);
+ else
+ fprintf(stderr, "Unknown command: %s\n", line);
+
+failed:
+ print_prompt();
+
+ free(line);
+}
+
+static void signal_cb(int signum, void *user_data)
+{
+ switch (signum) {
+ case SIGINT:
+ case SIGTERM:
+ mainloop_quit();
+ break;
+ default:
+ break;
+ }
+}
+
int main(int argc, char *argv[])
{
int opt;
@@ -509,6 +721,7 @@ int main(int argc, char *argv[])
bdaddr_t src_addr;
int dev_id = -1;
int fd;
+ sigset_t mask;
struct server *server;
while ((opt = getopt_long(argc, argv, "+hvs:m:i:",
@@ -593,8 +806,25 @@ int main(int argc, char *argv[])
return EXIT_FAILURE;
}
+ if (mainloop_add_fd(fileno(stdin),
+ EPOLLIN | EPOLLRDHUP | EPOLLHUP | EPOLLERR,
+ prompt_read_cb, server, NULL) < 0) {
+ fprintf(stderr, "Failed to initialize console\n");
+ server_destroy(server);
+
+ return EXIT_FAILURE;
+ }
+
printf("Running GATT server\n");
+ sigemptyset(&mask);
+ sigaddset(&mask, SIGINT);
+ sigaddset(&mask, SIGTERM);
+
+ mainloop_set_signal(&mask, signal_cb, NULL, NULL);
+
+ print_prompt();
+
mainloop_run();
printf("\n\nShutting down...\n");
--
2.1.0.rc2.206.gedb03e5
This patch introduces tools/btgatt-server, which is a command-line tool
for testing and debugging shared/gatt-server.
---
.gitignore | 1 +
Makefile.tools | 8 +++
tools/btgatt-server.c | 139 ++++++++++++++++++++++++++++++++++++++++++++++++++
3 files changed, 148 insertions(+)
create mode 100644 tools/btgatt-server.c
diff --git a/.gitignore b/.gitignore
index 164cc97..82304b0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -75,6 +75,7 @@ tools/3dsp
tools/obexctl
tools/gatt-service
tools/btgatt-client
+tools/btgatt-server
tools/mcaptest
test/sap_client.pyc
test/bluezutils.pyc
diff --git a/Makefile.tools b/Makefile.tools
index 75a6faa..4bd683b 100644
--- a/Makefile.tools
+++ b/Makefile.tools
@@ -207,6 +207,14 @@ EXTRA_DIST += tools/hciattach.1 tools/hciconfig.1 \
tools/sdptool.1 tools/ciptool.1 tools/bccmd.1
endif
+if TOOLS
+bin_PROGRAMS += tools/btgatt-server
+
+tools_btgatt_server_SOURCES = tools/btgatt-server.c src/uuid-helper.c
+tools_btgatt_server_LDADD = lib/libbluetooth-internal.la \
+ src/libshared-mainloop.la
+endif
+
if HID2HCI
udevdir = @UDEV_DIR@
diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
new file mode 100644
index 0000000..4508de2
--- /dev/null
+++ b/tools/btgatt-server.c
@@ -0,0 +1,139 @@
+/*
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2014 Google Inc.
+ *
+ *
+ * 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.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/hci_lib.h>
+#include <bluetooth/l2cap.h>
+#include "lib/uuid.h"
+
+static bool verbose = false;
+
+static void usage(void)
+{
+ printf("btgatt-server\n");
+ printf("Usage:\n\tbtgatt-server [options]\n");
+
+ printf("Options:\n"
+ "\t-i, --index <id>\t\tSpecify adapter index, e.g. hci0\n"
+ "\t-m, --mtu <mtu>\t\t\tThe ATT MTU to use\n"
+ "\t-s, --security-level <sec>\tSet security level (low|"
+ "medium|high)\n"
+ "\t-v, --verbose\t\t\tEnable extra logging\n"
+ "\t-h, --help\t\t\tDisplay help\n");
+}
+
+static struct option main_options[] = {
+ { "index", 1, 0, 'i' },
+ { "mtu", 1, 0, 'm' },
+ { "security-level", 1, 0, 's' },
+ { "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;
+ bdaddr_t src_addr;
+ int dev_id = -1;
+
+ while ((opt = getopt_long(argc, argv, "+hvs:m:i:",
+ main_options, NULL)) != -1) {
+ switch (opt) {
+ case 'h':
+ usage();
+ return EXIT_SUCCESS;
+ case 'v':
+ verbose = true;
+ break;
+ case 's':
+ if (strcmp(optarg, "low") == 0)
+ sec = BT_SECURITY_LOW;
+ else if (strcmp(optarg, "medium") == 0)
+ sec = BT_SECURITY_MEDIUM;
+ else if (strcmp(optarg, "high") == 0)
+ sec = BT_SECURITY_HIGH;
+ else {
+ fprintf(stderr, "Invalid security level\n");
+ return EXIT_FAILURE;
+ }
+ break;
+ case 'm': {
+ int arg;
+
+ arg = atoi(optarg);
+ if (arg <= 0) {
+ fprintf(stderr, "Invalid MTU: %d\n", arg);
+ return EXIT_FAILURE;
+ }
+
+ if (arg > UINT16_MAX) {
+ fprintf(stderr, "MTU too large: %d\n", arg);
+ return EXIT_FAILURE;
+ }
+
+ mtu = (uint16_t)arg;
+ 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;
+ }
+ }
+
+ argc -= optind;
+ argv -= optind;
+ optind = 0;
+
+ if (argc) {
+ usage();
+ 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;
+ }
+
+ /* TODO: Set up mainloop and listening LE socket */
+
+ return 0;
+}
--
2.1.0.rc2.206.gedb03e5
This patch adds code that listens for incoming connections and creates a
bt_gatt_server and bt_att structure once a connection is accepted.
---
tools/btgatt-server.c | 211 +++++++++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 209 insertions(+), 2 deletions(-)
diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
index 4508de2..e185413 100644
--- a/tools/btgatt-server.c
+++ b/tools/btgatt-server.c
@@ -25,6 +25,7 @@
#include <stdint.h>
#include <stdlib.h>
#include <getopt.h>
+#include <unistd.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
@@ -32,8 +33,131 @@
#include <bluetooth/l2cap.h>
#include "lib/uuid.h"
+#include "monitor/mainloop.h"
+#include "src/shared/util.h"
+#include "src/shared/att.h"
+#include "src/shared/queue.h"
+#include "src/shared/gatt-db.h"
+#include "src/shared/gatt-server.h"
+
+#define ATT_CID 4
+
+#define PRLOG(...) \
+ do { \
+ printf(__VA_ARGS__); \
+ print_prompt(); \
+ } while (0)
+
+#define COLOR_OFF "\x1B[0m"
+#define COLOR_RED "\x1B[0;91m"
+#define COLOR_GREEN "\x1B[0;92m"
+#define COLOR_YELLOW "\x1B[0;93m"
+#define COLOR_BLUE "\x1B[0;94m"
+#define COLOR_MAGENTA "\x1B[0;95m"
+#define COLOR_BOLDGRAY "\x1B[1;30m"
+#define COLOR_BOLDWHITE "\x1B[1;37m"
+
static bool verbose = false;
+struct server {
+ int fd;
+ struct gatt_db *db;
+ struct bt_gatt_server *gatt;
+};
+
+static void print_prompt(void)
+{
+ printf(COLOR_BLUE "[GATT server]" COLOR_OFF "# ");
+ fflush(stdout);
+}
+
+static void att_disconnect_cb(void *user_data)
+{
+ printf("Device disconnected\n");
+
+ mainloop_quit();
+}
+
+static void att_debug_cb(const char *str, void *user_data)
+{
+ const char *prefix = user_data;
+
+ PRLOG(COLOR_BOLDGRAY "%s" COLOR_BOLDWHITE "%s\n" COLOR_OFF, prefix,
+ str);
+}
+
+static void gatt_debug_cb(const char *str, void *user_data)
+{
+ const char *prefix = user_data;
+
+ PRLOG(COLOR_GREEN "%s%s\n" COLOR_OFF, prefix, str);
+}
+
+static struct server *server_create(int fd, uint16_t mtu)
+{
+ struct server *server;
+ struct bt_att *att;
+
+ server = new0(struct server, 1);
+ if (!server) {
+ fprintf(stderr, "Failed to allocate memory for server\n");
+ return NULL;
+ }
+
+ att = bt_att_new(fd);
+ if (!att) {
+ fprintf(stderr, "Failed to initialze ATT transport layer\n");
+ goto fail;
+ }
+
+ if (!bt_att_set_close_on_unref(att, true)) {
+ fprintf(stderr, "Failed to set up ATT transport layer\n");
+ goto fail;
+ }
+
+ if (!bt_att_register_disconnect(att, att_disconnect_cb, NULL, NULL)) {
+ fprintf(stderr, "Failed to set ATT disconnect handler\n");
+ goto fail;
+ }
+
+ server->fd = fd;
+ server->db = gatt_db_new();
+ if (!server->db) {
+ fprintf(stderr, "Failed to create GATT database\n");
+ goto fail;
+ }
+
+ server->gatt = bt_gatt_server_new(server->db, att, mtu);
+ if (!server->gatt) {
+ fprintf(stderr, "Failed to create GATT server\n");
+ goto fail;
+ }
+
+ if (verbose) {
+ bt_att_set_debug(att, att_debug_cb, "att: ", NULL);
+ bt_gatt_server_set_debug(server->gatt, gatt_debug_cb,
+ "server: ", NULL);
+ }
+
+ /* bt_gatt_server already holds a reference */
+ bt_att_unref(att);
+
+ return server;
+
+fail:
+ gatt_db_destroy(server->db);
+ bt_att_unref(att);
+ free(server);
+
+ return NULL;
+}
+
+static void server_destroy(struct server *server)
+{
+ bt_gatt_server_unref(server->gatt);
+ gatt_db_destroy(server->db);
+}
+
static void usage(void)
{
printf("btgatt-server\n");
@@ -57,6 +181,67 @@ static struct option main_options[] = {
{ }
};
+static int l2cap_le_att_listen_and_accept(bdaddr_t *src, int sec)
+{
+ int sk, nsk;
+ struct sockaddr_l2 srcaddr, addr;
+ socklen_t optlen;
+ struct bt_security btsec;
+ char ba[18];
+
+ sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+ if (sk < 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(sk, (struct sockaddr *)&srcaddr, sizeof(srcaddr)) < 0) {
+ perror("Failed to bind L2CAP socket");
+ goto fail;
+ }
+
+ /* Set the security level */
+ memset(&btsec, 0, sizeof(btsec));
+ btsec.level = sec;
+ if (setsockopt(sk, SOL_BLUETOOTH, BT_SECURITY, &btsec,
+ sizeof(btsec)) != 0) {
+ fprintf(stderr, "Failed to set L2CAP security level\n");
+ goto fail;
+ }
+
+ if (listen(sk, 10) < 0) {
+ perror("Listening on socket failed");
+ goto fail;
+ }
+
+ printf("Started listening on ATT channel. Waiting for connections\n");
+
+ memset(&addr, 0, sizeof(addr));
+ optlen = sizeof(addr);
+ nsk = accept(sk, (struct sockaddr *)&addr, &optlen);
+ if (nsk < 0) {
+ perror("Accept failed");
+ goto fail;
+ }
+
+ ba2str(&addr.l2_bdaddr, ba);
+ printf("Connect from %s\n", ba);
+ close(sk);
+
+ return nsk;
+
+fail:
+ close(sk);
+ return -1;
+}
+
int main(int argc, char *argv[])
{
int opt;
@@ -64,6 +249,8 @@ int main(int argc, char *argv[])
uint16_t mtu = 0;
bdaddr_t src_addr;
int dev_id = -1;
+ int fd;
+ struct server *server;
while ((opt = getopt_long(argc, argv, "+hvs:m:i:",
main_options, NULL)) != -1) {
@@ -133,7 +320,27 @@ int main(int argc, char *argv[])
return EXIT_FAILURE;
}
- /* TODO: Set up mainloop and listening LE socket */
+ fd = l2cap_le_att_listen_and_accept(&src_addr, sec);
+ if (fd < 0) {
+ fprintf(stderr, "Failed to accept L2CAP ATT connection\n");
+ return EXIT_FAILURE;
+ }
+
+ mainloop_init();
+
+ server = server_create(fd, mtu);
+ if (!server) {
+ close(fd);
+ return EXIT_FAILURE;
+ }
+
+ printf("Running GATT server\n");
+
+ mainloop_run();
+
+ printf("\n\nShutting down...\n");
+
+ server_destroy(server);
- return 0;
+ return EXIT_SUCCESS;
}
--
2.1.0.rc2.206.gedb03e5
This patch populates the GATT database with the GAP and GATT services.
---
tools/btgatt-server.c | 259 ++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 259 insertions(+)
diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
index e185413..545c8c4 100644
--- a/tools/btgatt-server.c
+++ b/tools/btgatt-server.c
@@ -42,12 +42,18 @@
#define ATT_CID 4
+#define UUID_GAP 0x1800
+
#define PRLOG(...) \
do { \
printf(__VA_ARGS__); \
print_prompt(); \
} while (0)
+#ifndef MIN
+#define MIN(a, b) ((a) < (b) ? (a) : (b))
+#endif
+
#define COLOR_OFF "\x1B[0m"
#define COLOR_RED "\x1B[0;91m"
#define COLOR_GREEN "\x1B[0;92m"
@@ -57,12 +63,19 @@
#define COLOR_BOLDGRAY "\x1B[1;30m"
#define COLOR_BOLDWHITE "\x1B[1;37m"
+static const char test_device_name[] = "Very Long Test Device Name For Testing "
+ "ATT Protocol Operations On GATT Server";
static bool verbose = false;
struct server {
int fd;
struct gatt_db *db;
struct bt_gatt_server *gatt;
+
+ uint8_t *device_name;
+ size_t name_len;
+
+ bool svc_chngd_enabled;
};
static void print_prompt(void)
@@ -93,10 +106,243 @@ static void gatt_debug_cb(const char *str, void *user_data)
PRLOG(COLOR_GREEN "%s%s\n" COLOR_OFF, prefix, str);
}
+static void gap_device_name_read_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t error = 0;
+ size_t len = 0;
+ const uint8_t *value = NULL;
+
+ PRLOG("GAP Device Name Read called\n");
+
+ len = server->name_len;
+
+ if (offset > len) {
+ error = BT_ATT_ERROR_INVALID_OFFSET;
+ goto done;
+ }
+
+ len -= offset;
+ value = len ? &server->device_name[offset] : NULL;
+
+done:
+ gatt_db_attribute_read_result(attrib, id, error, value, len);
+}
+
+static void gap_device_name_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t error = 0;
+
+ PRLOG("GAP Device Name Write called\n");
+
+ /* Implement this as a variable length attribute value. */
+ if (offset > server->name_len) {
+ error = BT_ATT_ERROR_INVALID_OFFSET;
+ goto done;
+ }
+
+ if (offset + len != server->name_len) {
+ uint8_t *name;
+
+ name = realloc(server->device_name, offset + len);
+ if (!name) {
+ error = BT_ATT_ERROR_INSUFFICIENT_RESOURCES;
+ goto done;
+ }
+
+ memcpy(name, server->device_name,
+ MIN(offset + len, server->name_len));
+ server->device_name = name;
+ server->name_len = offset + len;
+ }
+
+ if (value)
+ memcpy(server->device_name + offset, value, len);
+
+done:
+ gatt_db_attribute_write_result(attrib, id, error);
+}
+
+static void gap_device_name_ext_prop_read_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ uint8_t value[2];
+
+ PRLOG("Device Name Extended Properties Read called\n");
+
+ value[0] = BT_GATT_CHRC_EXT_PROP_RELIABLE_WRITE;
+ value[1] = 0;
+
+ gatt_db_attribute_read_result(attrib, id, 0, value, sizeof(value));
+}
+
+static void gatt_service_changed_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ PRLOG("Service Changed Read called\n");
+
+ gatt_db_attribute_read_result(attrib, id, 0, NULL, 0);
+}
+
+static void gatt_svc_chngd_ccc_read_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t value[2];
+
+ PRLOG("Service Changed CCC Read called\n");
+
+ value[0] = server->svc_chngd_enabled ? 0x02 : 0x00;
+ value[1] = 0x00;
+
+ gatt_db_attribute_read_result(attrib, id, 0, value, sizeof(value));
+}
+
+static void gatt_svc_chngd_ccc_write_cb(struct gatt_db_attribute *attrib,
+ unsigned int id, uint16_t offset,
+ const uint8_t *value, size_t len,
+ uint8_t opcode, bdaddr_t *bdaddr,
+ void *user_data)
+{
+ struct server *server = user_data;
+ uint8_t ecode = 0;
+
+ PRLOG("Service Changed CCC Write called\n");
+
+ if (!value || len != 2) {
+ ecode = BT_ATT_ERROR_INVALID_ATTRIBUTE_VALUE_LEN;
+ goto done;
+ }
+
+ if (offset) {
+ ecode = BT_ATT_ERROR_INVALID_OFFSET;
+ goto done;
+ }
+
+ if (value[0] == 0x00)
+ server->svc_chngd_enabled = false;
+ else if (value[0] == 0x02)
+ server->svc_chngd_enabled = true;
+ else
+ ecode = 0x80;
+
+ PRLOG("Service Changed Enabled: %s\n",
+ server->svc_chngd_enabled ? "true" : "false");
+
+done:
+ gatt_db_attribute_write_result(attrib, id, ecode);
+}
+
+static void confirm_write(struct gatt_db_attribute *attr, int err,
+ void *user_data)
+{
+ if (!err)
+ return;
+
+ fprintf(stderr, "Error caching attribute %p - err: %d\n", attr, err);
+ exit(1);
+}
+
+static void populate_gap_service(struct server *server)
+{
+ bt_uuid_t uuid;
+ struct gatt_db_attribute *service, *tmp;
+ uint16_t appearance;
+
+ /* Add the GAP service */
+ bt_uuid16_create(&uuid, UUID_GAP);
+ service = gatt_db_add_service(server->db, &uuid, true, 6);
+
+ /*
+ * Device Name characteristic. Make the value dynamically read and
+ * written via callbacks.
+ */
+ bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME);
+ gatt_db_service_add_characteristic(service, &uuid,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ BT_GATT_CHRC_PROP_READ,
+ gap_device_name_read_cb,
+ gap_device_name_write_cb,
+ server);
+
+ bt_uuid16_create(&uuid, GATT_CHARAC_EXT_PROPER_UUID);
+ gatt_db_service_add_descriptor(service, &uuid, BT_ATT_PERM_READ,
+ gap_device_name_ext_prop_read_cb,
+ NULL, server);
+
+ /*
+ * Appearance characteristic. Reads and writes should obtain the value
+ * from the database.
+ */
+ bt_uuid16_create(&uuid, GATT_CHARAC_APPEARANCE);
+ tmp = gatt_db_service_add_characteristic(service, &uuid,
+ BT_ATT_PERM_READ,
+ BT_GATT_CHRC_PROP_READ,
+ NULL, NULL, server);
+
+ /*
+ * Write the appearance value to the database, since we're not using a
+ * callback.
+ */
+ put_le16(128, &appearance);
+ gatt_db_attribute_write(tmp, 0, (void *) &appearance,
+ sizeof(appearance),
+ BT_ATT_OP_WRITE_REQ,
+ NULL, confirm_write,
+ NULL);
+
+ gatt_db_service_set_active(service, true);
+}
+
+static void populate_gatt_service(struct server *server)
+{
+ bt_uuid_t uuid;
+ struct gatt_db_attribute *service;
+
+ /* Add the GATT service */
+ bt_uuid16_create(&uuid, 0x1801);
+ service = gatt_db_add_service(server->db, &uuid, true, 4);
+
+ bt_uuid16_create(&uuid, GATT_CHARAC_SERVICE_CHANGED);
+ gatt_db_service_add_characteristic(service, &uuid, BT_ATT_PERM_READ,
+ BT_GATT_CHRC_PROP_READ | BT_GATT_CHRC_PROP_INDICATE,
+ gatt_service_changed_cb,
+ NULL, server);
+
+ bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
+ gatt_db_service_add_descriptor(service, &uuid,
+ BT_ATT_PERM_READ | BT_ATT_PERM_WRITE,
+ gatt_svc_chngd_ccc_read_cb,
+ gatt_svc_chngd_ccc_write_cb, server);
+
+ gatt_db_service_set_active(service, true);
+}
+
+static void populate_db(struct server *server)
+{
+ populate_gap_service(server);
+ populate_gatt_service(server);
+}
+
static struct server *server_create(int fd, uint16_t mtu)
{
struct server *server;
struct bt_att *att;
+ size_t name_len = strlen(test_device_name);
server = new0(struct server, 1);
if (!server) {
@@ -120,6 +366,16 @@ static struct server *server_create(int fd, uint16_t mtu)
goto fail;
}
+ server->name_len = name_len + 1;
+ server->device_name = malloc(name_len + 1);
+ if (!server->device_name) {
+ fprintf(stderr, "Failed to allocate memory for device name\n");
+ goto fail;
+ }
+
+ memcpy(server->device_name, test_device_name, name_len);
+ server->device_name[name_len] = '\0';
+
server->fd = fd;
server->db = gatt_db_new();
if (!server->db) {
@@ -142,10 +398,13 @@ static struct server *server_create(int fd, uint16_t mtu)
/* bt_gatt_server already holds a reference */
bt_att_unref(att);
+ populate_db(server);
+
return server;
fail:
gatt_db_destroy(server->db);
+ free(server->device_name);
bt_att_unref(att);
free(server);
--
2.1.0.rc2.206.gedb03e5
This patch adds bt_att_encode_error_rsp which allows encoding an ATT
protocol error response by correctly converting system errnos to ATT
protocol error codes. struct bt_att_pdu_error_rsp is introduced to
represent an error response PDU which can be sent directly over the
wire once properly encoded in network order.
---
src/shared/att-types.h | 12 ++++
src/shared/att.c | 58 +++++++++++++++++-
src/shared/att.h | 3 +
src/shared/gatt-client.c | 8 ++-
src/shared/gatt-helpers.c | 8 ++-
src/shared/gatt-server.c | 153 +++++++++++++++++++++++-----------------------
6 files changed, 160 insertions(+), 82 deletions(-)
diff --git a/src/shared/att-types.h b/src/shared/att-types.h
index 24bf3da..3d8829a 100644
--- a/src/shared/att-types.h
+++ b/src/shared/att-types.h
@@ -23,6 +23,10 @@
#include <stdint.h>
+#ifndef __packed
+#define __packed __attribute__((packed))
+#endif
+
#define BT_ATT_DEFAULT_LE_MTU 23
/* ATT protocol opcodes */
@@ -55,6 +59,14 @@
#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
diff --git a/src/shared/att.c b/src/shared/att.c
index 6e1e538..e831078 100644
--- a/src/shared/att.c
+++ b/src/shared/att.c
@@ -35,13 +35,22 @@
#include "src/shared/timeout.h"
#include "lib/uuid.h"
#include "src/shared/att.h"
-#include "src/shared/att-types.h"
#define ATT_MIN_PDU_LEN 1 /* At least 1 byte for the opcode. */
#define ATT_OP_CMD_MASK 0x40
#define ATT_OP_SIGNED_MASK 0x80
#define ATT_TIMEOUT_INTERVAL 30000 /* 30000 ms */
+/*
+ * Common Profile and Service Error Code descriptions (see Supplement to the
+ * Bluetooth Core Specification, sections 1.2 and 2). The error codes within
+ * 0xE0-0xFC are reserved for future use. The remaining 3 are defined as the
+ * following:
+ */
+#define BT_ERROR_CCC_IMPROPERLY_CONFIGURED 0xfd
+#define BT_ERROR_ALREADY_IN_PROGRESS 0xfe
+#define BT_ERROR_OUT_OF_RANGE 0xff
+
struct att_send_op;
struct bt_att {
@@ -1218,3 +1227,50 @@ bool bt_att_unregister_all(struct bt_att *att)
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;
+}
+
+bool bt_att_encode_error_rsp(uint8_t opcode, uint16_t handle, int error,
+ struct bt_att_pdu_error_rsp *pdu)
+{
+ uint8_t ecode;
+
+ if (!pdu)
+ return false;
+
+ ecode = att_ecode_from_error(error);
+
+ memset(pdu, 0, sizeof(*pdu));
+
+ pdu->opcode = opcode;
+ put_le16(handle, &pdu->handle);
+ pdu->ecode = ecode;
+
+ return true;
+}
diff --git a/src/shared/att.h b/src/shared/att.h
index 1063021..a1a4821 100644
--- a/src/shared/att.h
+++ b/src/shared/att.h
@@ -76,3 +76,6 @@ unsigned int bt_att_register_disconnect(struct bt_att *att,
bool bt_att_unregister_disconnect(struct bt_att *att, unsigned int id);
bool bt_att_unregister_all(struct bt_att *att);
+
+bool bt_att_encode_error_rsp(uint8_t opcode, uint16_t handle, int error,
+ struct bt_att_pdu_error_rsp *pdu);
diff --git a/src/shared/gatt-client.c b/src/shared/gatt-client.c
index 30b271e..1f82048 100644
--- a/src/shared/gatt-client.c
+++ b/src/shared/gatt-client.c
@@ -1285,10 +1285,14 @@ static bool notify_data_write_ccc(struct notify_data *notify_data, bool enable,
static uint8_t process_error(const void *pdu, uint16_t length)
{
- if (!pdu || length != 4)
+ const struct bt_att_pdu_error_rsp *error_pdu;
+
+ if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
return 0;
- return ((uint8_t *) pdu)[3];
+ error_pdu = pdu;
+
+ return error_pdu->ecode;
}
static void enable_ccc_callback(uint8_t opcode, const void *pdu,
diff --git a/src/shared/gatt-helpers.c b/src/shared/gatt-helpers.c
index 220d0eb..9c8a3ba 100644
--- a/src/shared/gatt-helpers.c
+++ b/src/shared/gatt-helpers.c
@@ -430,10 +430,14 @@ static void destroy_mtu_op(void *user_data)
static uint8_t process_error(const void *pdu, uint16_t length)
{
- if (!pdu || length != 4)
+ const struct bt_att_pdu_error_rsp *error_pdu;
+
+ if (!pdu || length != sizeof(struct bt_att_pdu_error_rsp))
return 0;
- return ((uint8_t *) pdu)[3];
+ error_pdu = pdu;
+
+ return error_pdu->ecode;
}
static void mtu_cb(uint8_t opcode, const void *pdu, uint16_t length,
diff --git a/src/shared/gatt-server.c b/src/shared/gatt-server.c
index 2ca318a..3262992 100644
--- a/src/shared/gatt-server.c
+++ b/src/shared/gatt-server.c
@@ -22,6 +22,7 @@
*/
#include <sys/uio.h>
+#include <errno.h>
#include "src/shared/att.h"
#include "lib/uuid.h"
@@ -29,7 +30,6 @@
#include "src/shared/gatt-db.h"
#include "src/shared/gatt-server.h"
#include "src/shared/gatt-helpers.h"
-#include "src/shared/att-types.h"
#include "src/shared/util.h"
#ifndef MAX
@@ -133,22 +133,6 @@ static void bt_gatt_server_free(struct bt_gatt_server *server)
free(server);
}
-static uint8_t att_ecode_from_error(int err)
-{
- if (err < 0 || err > UINT8_MAX)
- return 0xff;
-
- return err;
-}
-
-static void encode_error_rsp(uint8_t opcode, uint16_t handle, uint8_t ecode,
- uint8_t pdu[4])
-{
- pdu[0] = opcode;
- pdu[3] = ecode;
- put_le16(handle, pdu + 1);
-}
-
static bool get_uuid_le(const uint8_t *uuid, size_t len, bt_uuid_t *out_uuid)
{
uint128_t u128;
@@ -315,8 +299,9 @@ static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
error:
rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
+ rsp_len = sizeof(struct bt_att_pdu_error_rsp);
+ bt_att_encode_error_rsp(opcode, ehandle, ecode,
+ (struct bt_att_pdu_error_rsp *) rsp_pdu);
done:
queue_destroy(q, NULL);
@@ -355,13 +340,13 @@ static void read_by_type_read_complete_cb(struct gatt_db_attribute *attr,
/* Terminate the operation if there was an error */
if (err) {
- uint8_t pdu[4];
- uint8_t att_ecode = att_ecode_from_error(err);
+ struct bt_att_pdu_error_rsp error_pdu;
- encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, handle, att_ecode,
- pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
- NULL, NULL);
+ bt_att_encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, handle, err,
+ &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
+ NULL, NULL, NULL);
async_read_op_destroy(op);
return;
}
@@ -437,8 +422,9 @@ static void process_read_by_type(struct async_read_op *op)
error:
ehandle = gatt_db_attribute_get_handle(attr);
rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, ehandle, ecode, op->pdu);
+ rsp_len = sizeof(struct bt_att_pdu_error_rsp);
+ bt_att_encode_error_rsp(BT_ATT_OP_READ_BY_TYPE_REQ, ehandle, ecode,
+ (struct bt_att_pdu_error_rsp *) op->pdu);
done:
bt_att_send(server->att, rsp_opcode, op->pdu, rsp_len, NULL,
@@ -452,7 +438,7 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
struct bt_gatt_server *server = user_data;
uint16_t start, end;
bt_uuid_t type;
- uint8_t rsp_pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
uint16_t ehandle = 0;
uint8_t ecode;
struct queue *q = NULL;
@@ -524,9 +510,10 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
return;
error:
- encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
+ bt_att_encode_error_rsp(opcode, ehandle, ecode, &error_pdu);
queue_destroy(q, NULL);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
NULL, NULL, NULL);
}
@@ -665,8 +652,9 @@ static void find_info_cb(uint8_t opcode, const void *pdu,
error:
rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
+ rsp_len = sizeof(struct bt_att_pdu_error_rsp);
+ bt_att_encode_error_rsp(opcode, ehandle, ecode,
+ (struct bt_att_pdu_error_rsp *) rsp_pdu);
done:
queue_destroy(q, NULL);
@@ -697,11 +685,11 @@ static void write_complete_cb(struct gatt_db_attribute *attr, int err,
handle = gatt_db_attribute_get_handle(attr);
if (err) {
- uint8_t rsp_pdu[4];
- uint8_t att_ecode = att_ecode_from_error(err);
+ struct bt_att_pdu_error_rsp error_pdu;
- encode_error_rsp(op->opcode, handle, att_ecode, rsp_pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
+ bt_att_encode_error_rsp(op->opcode, handle, err, &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
NULL, NULL, NULL);
} else {
bt_att_send(server->att, BT_ATT_OP_WRITE_RSP, NULL, 0,
@@ -717,7 +705,7 @@ static void write_cb(uint8_t opcode, const void *pdu,
struct bt_gatt_server *server = user_data;
struct gatt_db_attribute *attr;
uint16_t handle = 0;
- uint8_t rsp_pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
struct async_write_op *op = NULL;
uint8_t ecode;
uint32_t perm;
@@ -777,8 +765,9 @@ error:
if (opcode == BT_ATT_OP_WRITE_CMD)
return;
- encode_error_rsp(opcode, handle, ecode, rsp_pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
+ bt_att_encode_error_rsp(opcode, handle, ecode, &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
NULL, NULL, NULL);
}
@@ -824,12 +813,12 @@ static void read_complete_cb(struct gatt_db_attribute *attr, int err,
handle = gatt_db_attribute_get_handle(attr);
if (err) {
- uint8_t pdu[4];
- uint8_t att_ecode = att_ecode_from_error(err);
+ struct bt_att_pdu_error_rsp error_pdu;
- encode_error_rsp(op->opcode, handle, att_ecode, pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
- NULL, NULL);
+ bt_att_encode_error_rsp(op->opcode, handle, err, &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
+ NULL, NULL, NULL);
async_read_op_destroy(op);
return;
}
@@ -846,7 +835,7 @@ static void handle_read_req(struct bt_gatt_server *server, uint8_t opcode,
uint16_t handle,
uint16_t offset)
{
- uint8_t error_pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
struct gatt_db_attribute *attr;
uint8_t ecode;
uint32_t perm;
@@ -898,9 +887,10 @@ error:
if (op)
async_read_op_destroy(op);
- encode_error_rsp(opcode, handle, ecode, error_pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, error_pdu, 4, NULL, NULL,
- NULL);
+ bt_att_encode_error_rsp(opcode, handle, ecode, &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
+ NULL, NULL, NULL);
}
static void read_cb(uint8_t opcode, const void *pdu,
@@ -910,11 +900,13 @@ static void read_cb(uint8_t opcode, const void *pdu,
uint16_t handle;
if (length != 2) {
- uint8_t pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
- encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
- NULL, NULL);
+ bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
+ &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
+ NULL, NULL, NULL);
return;
}
@@ -930,11 +922,13 @@ static void read_blob_cb(uint8_t opcode, const void *pdu,
uint16_t handle, offset;
if (length != 4) {
- uint8_t pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
- encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, pdu, 4, NULL,
- NULL, NULL);
+ bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
+ &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu),
+ NULL, NULL, NULL);
return;
}
@@ -1031,8 +1025,9 @@ error:
prep_write_data_destroy(prep_data);
rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(opcode, handle, ecode, rsp_pdu);
+ rsp_len = sizeof(struct bt_att_pdu_error_rsp);
+ bt_att_encode_error_rsp(opcode, handle, ecode,
+ (struct bt_att_pdu_error_rsp *) rsp_pdu);
done:
bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
@@ -1040,30 +1035,29 @@ done:
}
static void exec_next_prep_write(struct bt_gatt_server *server,
- uint16_t ehandle, uint8_t att_ecode);
+ uint16_t ehandle, int err);
static void exec_write_complete_cb(struct gatt_db_attribute *attr, int err,
void *user_data)
{
struct bt_gatt_server *server = user_data;
uint16_t handle = gatt_db_attribute_get_handle(attr);
- uint16_t att_ecode = att_ecode_from_error(err);
- exec_next_prep_write(server, handle, att_ecode);
+ exec_next_prep_write(server, handle, err);
}
static void exec_next_prep_write(struct bt_gatt_server *server,
- uint16_t ehandle, uint8_t att_ecode)
+ uint16_t ehandle, int err)
{
struct prep_write_data *next = NULL;
uint8_t rsp_opcode = BT_ATT_OP_EXEC_WRITE_RSP;
- uint8_t error_pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
uint8_t *rsp_pdu = NULL;
uint16_t rsp_len = 0;
struct gatt_db_attribute *attr;
bool status;
- if (att_ecode)
+ if (err)
goto error;
next = queue_pop_head(server->prep_queue);
@@ -1072,7 +1066,7 @@ static void exec_next_prep_write(struct bt_gatt_server *server,
attr = gatt_db_get_attribute(server->db, next->handle);
if (!attr) {
- att_ecode = BT_ATT_ERROR_UNLIKELY;
+ err = BT_ATT_ERROR_UNLIKELY;
goto error;
}
@@ -1086,14 +1080,15 @@ static void exec_next_prep_write(struct bt_gatt_server *server,
if (status)
return;
- att_ecode = BT_ATT_ERROR_UNLIKELY;
+ err = BT_ATT_ERROR_UNLIKELY;
error:
rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- rsp_pdu = error_pdu;
- encode_error_rsp(BT_ATT_OP_EXEC_WRITE_REQ, ehandle, att_ecode, rsp_pdu);
+ rsp_len = sizeof(error_pdu);
+ bt_att_encode_error_rsp(BT_ATT_OP_EXEC_WRITE_REQ, ehandle, err,
+ &error_pdu);
+ rsp_pdu = (uint8_t *) &error_pdu;
done:
bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
NULL);
@@ -1106,7 +1101,7 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
uint8_t flags;
uint8_t ecode;
uint8_t rsp_opcode;
- uint8_t error_pdu[4];
+ struct bt_att_pdu_error_rsp error_pdu;
uint8_t *rsp_pdu = NULL;
uint16_t rsp_len = 0;
bool write;
@@ -1143,9 +1138,10 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
error:
rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- rsp_pdu = error_pdu;
- encode_error_rsp(opcode, 0, ecode, rsp_pdu);
+ rsp_len = sizeof(error_pdu);
+ bt_att_encode_error_rsp(opcode, 0, ecode, &error_pdu);
+
+ rsp_pdu = (uint8_t *) &error_pdu;
done:
bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
@@ -1158,12 +1154,15 @@ static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
struct bt_gatt_server *server = user_data;
uint16_t client_rx_mtu;
uint16_t final_mtu;
- uint8_t rsp_pdu[4];
+ uint8_t rsp_pdu[2];
if (length != 2) {
- encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU, rsp_pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu,
- sizeof(rsp_pdu), NULL, NULL, NULL);
+ struct bt_att_pdu_error_rsp error_pdu;
+
+ bt_att_encode_error_rsp(opcode, 0, BT_ATT_ERROR_INVALID_PDU,
+ &error_pdu);
+ bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, &error_pdu,
+ sizeof(error_pdu), NULL, NULL, NULL);
return;
}
--
2.1.0.rc2.206.gedb03e5