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.
*v2: Change bt_att_encode_error_rsp to bt_att_send_error_rsp.
Arman Uguray (7):
shared/att: Add API for sending 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 | 60 ++-
src/shared/att.h | 3 +
src/shared/gatt-client.c | 8 +-
src/shared/gatt-helpers.c | 8 +-
src/shared/gatt-server.c | 208 +++------
tools/btgatt-server.c | 1088 +++++++++++++++++++++++++++++++++++++++++++++
9 files changed, 1242 insertions(+), 154 deletions(-)
create mode 100644 tools/btgatt-server.c
--
2.1.0.rc2.206.gedb03e5
Hi Michael,
> On Fri, Nov 14, 2014 at 11:44 AM, Michael Janssen <[email protected]> wrote:
> Hi Arman,
>
> On Thu, Nov 13, 2014 at 6:49 PM, Arman Uguray <[email protected]> wrote:
>> 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;
>
> You don't need this memcpy, realloc will copy the contents of the old buffer.
I guess you're right, for some reason I assumed that the new pointer
may not point to the beginning of the old memory block pointed to by
the previous pointer. Anyway, I tested it and you're right, so fixed
it for v3.
> Is it reasonable to expect / deal with value being NULL and len being
> non-zero here?
>
No, that shouldn't ever happen. It's one of the guarantees of
shared/gatt-server and such a case would be a bug that we would need
to fix in gatt-server.
>> + }
>> +
>> + 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
>>
>> --
>> 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
>
>
> --
> Michael Janssen
Hi Luiz,
> On Fri, Nov 14, 2014 at 8:32 AM, Arman Uguray <[email protected]> wrote:
> Hi Luiz,
>
>> On Fri, Nov 14, 2014 at 5:31 AM, Luiz Augusto von Dentz <[email protected]> wrote:
>> Hi Arman,
>>
>> On Fri, Nov 14, 2014 at 4:49 AM, Arman Uguray <[email protected]> wrote:
>>> 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 one fails to link which is quite strange since if I continue with
>> the next patch apparently fix it but it doesn't actually touch
>> anything to change the build so maybe it is a symbol resolution thing.
>>
>
> Yeah, I've hit this before but ignored it once it "fixed itself". I
> can make that patch compile by adding lib/uuid.h and lib/uuid.c to the
> libshared sources (since they all internally depend on it, I guess it
> makes sense to put these there?) but I'm really curious why the
> following patch fixes the issue.
>
Changing the order of the static libraries in the
tools_btgatt_server_LDADD statement fixed the issue (i.e. first
src/libshared-mainloop.la followed by lib/libbluetooth-internal.la).
>>
>> --
>> Luiz Augusto von Dentz
>
> Cheers,
> Arman
Cheers,
Arman
Hi Arman,
On Thu, Nov 13, 2014 at 6:49 PM, Arman Uguray <[email protected]> wrote:
> 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;
You don't need this memcpy, realloc will copy the contents of the old buffer.
Is it reasonable to expect / deal with value being NULL and len being
non-zero here?
> + }
> +
> + 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
>
> --
> 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
--
Michael Janssen
Hi Luiz,
> On Fri, Nov 14, 2014 at 5:31 AM, Luiz Augusto von Dentz <[email protected]> wrote:
> Hi Arman,
>
> On Fri, Nov 14, 2014 at 4:49 AM, Arman Uguray <[email protected]> wrote:
>> 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 one fails to link which is quite strange since if I continue with
> the next patch apparently fix it but it doesn't actually touch
> anything to change the build so maybe it is a symbol resolution thing.
>
Yeah, I've hit this before but ignored it once it "fixed itself". I
can make that patch compile by adding lib/uuid.h and lib/uuid.c to the
libshared sources (since they all internally depend on it, I guess it
makes sense to put these there?) but I'm really curious why the
following patch fixes the issue.
>
> --
> Luiz Augusto von Dentz
Cheers,
Arman
Hi Arman,
On Fri, Nov 14, 2014 at 4:49 AM, Arman Uguray <[email protected]> wrote:
> 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.
>
> *v2: Change bt_att_encode_error_rsp to bt_att_send_error_rsp.
>
> Arman Uguray (7):
> shared/att: Add API for sending 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 | 60 ++-
> src/shared/att.h | 3 +
> src/shared/gatt-client.c | 8 +-
> src/shared/gatt-helpers.c | 8 +-
> src/shared/gatt-server.c | 208 +++------
> tools/btgatt-server.c | 1088 +++++++++++++++++++++++++++++++++++++++++++++
> 9 files changed, 1242 insertions(+), 154 deletions(-)
> create mode 100644 tools/btgatt-server.c
>
> --
> 2.1.0.rc2.206.gedb03e5
Ive aplied the first one, please fix the build for 2/7 and 3/7 if
possible. Btw I notice that sometimes you do casts without spaces in
between, please fix that as well.
--
Luiz Augusto von Dentz
Hi Arman,
On Fri, Nov 14, 2014 at 4:49 AM, Arman Uguray <[email protected]> wrote:
> 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 one fails to link which is quite strange since if I continue with
the next patch apparently fix it but it doesn't actually touch
anything to change the build so maybe it is a symbol resolution thing.
--
Luiz Augusto von Dentz
Hi Arman,
On Fri, Nov 14, 2014 at 4:49 AM, Arman Uguray <[email protected]> wrote:
> 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
Shouldn't it be placed under experimental together with btgatt-client?
> 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
Btw, this is breaking the build if applied separately:
tools/btgatt-server.c: In function ‘main’:
tools/btgatt-server.c:64:11: error: variable ‘mtu’ set but not used
[-Werror=unused-but-set-variable]
uint16_t mtu = 0;
^
tools/btgatt-server.c:63:6: error: variable ‘sec’ set but not used
[-Werror=unused-but-set-variable]
int sec = BT_SECURITY_LOW;
^
--
Luiz Augusto von Dentz
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 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 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 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 adds bt_att_send_error_rsp which sends an ATT protocol error
response PDU over the underlying link based on the parameters that are
passed to it. This is a useful function since it reduces a lot of
repeated code in shared/gatt-server that does the same thing.
The "error" parameter is expressed as a signed integer (in line with the
new shared/gatt-db APIs). bt_att_send_error_rsp correctly maps this
(including possible UNIX errnos) to ATT protocol error codes. In
addition, struct bt_att_pdu_error_rsp is introduced to represent a
packed 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 | 60 ++++++++++++-
src/shared/att.h | 3 +
src/shared/gatt-client.c | 8 +-
src/shared/gatt-helpers.c | 8 +-
src/shared/gatt-server.c | 208 +++++++++++++---------------------------------
6 files changed, 145 insertions(+), 154 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..2bc7682 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 {
@@ -1139,6 +1148,55 @@ bool bt_att_cancel_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;
+}
+
+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,
diff --git a/src/shared/att.h b/src/shared/att.h
index 1063021..99b5a5b 100644
--- a/src/shared/att.h
+++ b/src/shared/att.h
@@ -63,6 +63,9 @@ unsigned int bt_att_send(struct bt_att *att, uint8_t opcode,
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,
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..2ef9269 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;
@@ -247,7 +231,6 @@ static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
uint16_t mtu = bt_att_get_mtu(server->att);
uint8_t rsp_pdu[mtu];
uint16_t rsp_len;
- uint8_t rsp_opcode;
uint8_t ecode = 0;
uint16_t ehandle = 0;
struct queue *q = NULL;
@@ -309,19 +292,17 @@ static void read_by_grp_type_cb(uint8_t opcode, const void *pdu,
goto error;
}
- rsp_opcode = BT_ATT_OP_READ_BY_GRP_TYPE_RSP;
+ queue_destroy(q, NULL);
- goto done;
+ bt_att_send(server->att, BT_ATT_OP_READ_BY_GRP_TYPE_RSP,
+ rsp_pdu, rsp_len,
+ NULL, NULL, NULL);
-error:
- rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
+ return;
-done:
+error:
queue_destroy(q, NULL);
- bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len,
- NULL, NULL, NULL);
+ bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
}
static void async_read_op_destroy(struct async_read_op *op)
@@ -355,13 +336,8 @@ 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);
-
- 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_send_error_rsp(server->att, BT_ATT_OP_READ_BY_TYPE_REQ,
+ handle, err);
async_read_op_destroy(op);
return;
}
@@ -397,19 +373,19 @@ done:
static void process_read_by_type(struct async_read_op *op)
{
struct bt_gatt_server *server = op->server;
- uint8_t rsp_opcode;
- uint8_t rsp_len;
uint8_t ecode;
- uint16_t ehandle;
struct gatt_db_attribute *attr;
uint32_t perm;
attr = queue_pop_head(op->db_data);
if (op->done || !attr) {
- rsp_opcode = BT_ATT_OP_READ_BY_TYPE_RSP;
- rsp_len = op->pdu_len;
- goto done;
+ bt_att_send(server->att, BT_ATT_OP_READ_BY_TYPE_RSP, op->pdu,
+ op->pdu_len,
+ NULL, NULL,
+ NULL);
+ async_read_op_destroy(op);
+ return;
}
if (!gatt_db_attribute_get_permissions(attr, &perm)) {
@@ -435,14 +411,8 @@ static void process_read_by_type(struct async_read_op *op)
ecode = BT_ATT_ERROR_UNLIKELY;
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);
-
-done:
- bt_att_send(server->att, rsp_opcode, op->pdu, rsp_len, NULL,
- NULL, NULL);
+ bt_att_send_error_rsp(server->att, BT_ATT_OP_READ_BY_TYPE_REQ,
+ gatt_db_attribute_get_handle(attr), ecode);
async_read_op_destroy(op);
}
@@ -452,7 +422,6 @@ 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];
uint16_t ehandle = 0;
uint8_t ecode;
struct queue *q = NULL;
@@ -524,10 +493,8 @@ static void read_by_type_cb(uint8_t opcode, const void *pdu,
return;
error:
- encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
+ bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
queue_destroy(q, NULL);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
- NULL, NULL, NULL);
}
static void put_uuid_le(const bt_uuid_t *src, void *dst)
@@ -612,7 +579,6 @@ static void find_info_cb(uint8_t opcode, const void *pdu,
uint16_t mtu = bt_att_get_mtu(server->att);
uint8_t rsp_pdu[mtu];
uint16_t rsp_len;
- uint8_t rsp_opcode;
uint8_t ecode = 0;
uint16_t ehandle = 0;
struct queue *q = NULL;
@@ -659,19 +625,16 @@ static void find_info_cb(uint8_t opcode, const void *pdu,
goto error;
}
- rsp_opcode = BT_ATT_OP_FIND_INFO_RSP;
+ bt_att_send(server->att, BT_ATT_OP_FIND_INFO_RSP, rsp_pdu, rsp_len,
+ NULL, NULL, NULL);
+ queue_destroy(q, NULL);
- goto done;
+ return;
error:
- rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(opcode, ehandle, ecode, rsp_pdu);
-
-done:
+ bt_att_send_error_rsp(server->att, opcode, ehandle, ecode);
queue_destroy(q, NULL);
- bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len,
- NULL, NULL, NULL);
+
}
static void async_write_op_destroy(struct async_write_op *op)
@@ -696,17 +659,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);
-
- encode_error_rsp(op->opcode, handle, att_ecode, rsp_pdu);
- bt_att_send(server->att, BT_ATT_OP_ERROR_RSP, rsp_pdu, 4,
- NULL, NULL, NULL);
- } else {
+ if (err)
+ bt_att_send_error_rsp(server->att, op->opcode, handle, err);
+ else
bt_att_send(server->att, BT_ATT_OP_WRITE_RSP, NULL, 0,
NULL, NULL, NULL);
- }
async_write_op_destroy(op);
}
@@ -717,7 +674,6 @@ 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 async_write_op *op = NULL;
uint8_t ecode;
uint32_t perm;
@@ -777,9 +733,7 @@ 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,
- NULL, NULL, NULL);
+ bt_att_send_error_rsp(server->att, opcode, handle, ecode);
}
static uint8_t get_read_rsp_opcode(uint8_t opcode)
@@ -824,12 +778,7 @@ 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);
-
- 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_send_error_rsp(server->att, op->opcode, handle, err);
async_read_op_destroy(op);
return;
}
@@ -846,7 +795,6 @@ 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 gatt_db_attribute *attr;
uint8_t ecode;
uint32_t perm;
@@ -898,9 +846,7 @@ 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_send_error_rsp(server->att, opcode, handle, ecode);
}
static void read_cb(uint8_t opcode, const void *pdu,
@@ -910,11 +856,8 @@ static void read_cb(uint8_t opcode, const void *pdu,
uint16_t handle;
if (length != 2) {
- uint8_t pdu[4];
-
- 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_send_error_rsp(server->att, opcode, 0,
+ BT_ATT_ERROR_INVALID_PDU);
return;
}
@@ -930,11 +873,8 @@ static void read_blob_cb(uint8_t opcode, const void *pdu,
uint16_t handle, offset;
if (length != 4) {
- uint8_t pdu[4];
-
- 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_send_error_rsp(server->att, opcode, 0,
+ BT_ATT_ERROR_INVALID_PDU);
return;
}
@@ -952,9 +892,6 @@ static void prep_write_cb(uint8_t opcode, const void *pdu,
uint16_t handle = 0;
uint16_t offset;
struct gatt_db_attribute *attr;
- uint8_t rsp_opcode;
- uint8_t rsp_pdu[MAX(4, length)];
- uint16_t rsp_len;
uint8_t ecode;
uint32_t perm;
@@ -1019,60 +956,50 @@ static void prep_write_cb(uint8_t opcode, const void *pdu,
queue_push_tail(server->prep_queue, prep_data);
- /* Create the response */
- rsp_len = length;
- rsp_opcode = BT_ATT_OP_PREP_WRITE_RSP;
- memcpy(rsp_pdu, pdu, rsp_len);
-
- goto done;
+ bt_att_send(server->att, BT_ATT_OP_PREP_WRITE_RSP, pdu, length, NULL,
+ NULL, NULL);
+ return;
error:
if (prep_data)
prep_write_data_destroy(prep_data);
- rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- encode_error_rsp(opcode, handle, ecode, rsp_pdu);
+ bt_att_send_error_rsp(server->att, opcode, handle, ecode);
-done:
- bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
- NULL);
}
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];
- 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);
- if (!next)
- goto done;
+ if (!next) {
+ bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
+ NULL, NULL, NULL);
+ return;
+ }
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,17 +1013,11 @@ 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);
-
-done:
- bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
- NULL);
+ bt_att_send_error_rsp(server->att, BT_ATT_OP_EXEC_WRITE_REQ,
+ ehandle, err);
}
static void exec_write_cb(uint8_t opcode, const void *pdu,
@@ -1105,10 +1026,6 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
struct bt_gatt_server *server = user_data;
uint8_t flags;
uint8_t ecode;
- uint8_t rsp_opcode;
- uint8_t error_pdu[4];
- uint8_t *rsp_pdu = NULL;
- uint16_t rsp_len = 0;
bool write;
if (length != 1) {
@@ -1133,8 +1050,9 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
if (!write) {
queue_remove_all(server->prep_queue, NULL, NULL,
prep_write_data_destroy);
- rsp_opcode = BT_ATT_OP_EXEC_WRITE_RSP;
- goto done;
+ bt_att_send(server->att, BT_ATT_OP_EXEC_WRITE_RSP, NULL, 0,
+ NULL, NULL, NULL);
+ return;
}
exec_next_prep_write(server, 0, 0);
@@ -1142,14 +1060,7 @@ static void exec_write_cb(uint8_t opcode, const void *pdu,
return;
error:
- rsp_opcode = BT_ATT_OP_ERROR_RSP;
- rsp_len = 4;
- rsp_pdu = error_pdu;
- encode_error_rsp(opcode, 0, ecode, rsp_pdu);
-
-done:
- bt_att_send(server->att, rsp_opcode, rsp_pdu, rsp_len, NULL, NULL,
- NULL);
+ bt_att_send_error_rsp(server->att, opcode, 0, ecode);
}
static void exchange_mtu_cb(uint8_t opcode, const void *pdu,
@@ -1158,12 +1069,11 @@ 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);
+ bt_att_send_error_rsp(server->att, opcode, 0,
+ BT_ATT_ERROR_INVALID_PDU);
return;
}
--
2.1.0.rc2.206.gedb03e5