Implementation of the LE Advertisement D-Bus API.
Things that are missing still:
* Semantics for multiple Advertisements (currently any more than one will fail)
* TX Power property parsing / interface
* Setting other Flags in the advertisement.
v1 -> v2:
* Cleanup based on comments from Arman
* Add g_dbus_proxy_set_read_watch function to avoid ObjectManager dependency
* Consolidate script updates into single test script
v2 -> v3:
* Change "ManufacturerSpecificData" to "ManufacturerData"
* Add advertisement-api.txt to EXTRA_DIST
* Add Release() callback to org.bluez.LEAdvertisement1
* Remove IncludePower from the LEAdvertisement1 API
v3 -> v4:
* Rename "g_dbus_proxy_set_read_watch" to "g_dbus_proxy_set_ready_watch"
v4 -> v5:
* Rebase
* Use g_dbus_client_set_proxy_handlers instead of g_dbus_proxy_set_ready_watch
* Add MGMT interface calls to actually advertise.
v5 -> v6:
* Minor patch cruft cleanup
* Rename btd_advertising_manager to btd_advertising and fix filename change.
v6 -> v7:
* Rebase for included patches
* Rename ad structure to bt_ad
* Fixes for bt_ad_generate bugs
* Correctly error for invalid length
* Improvements so advertisement-example works out of the box
Michael Janssen (10):
shared: add bt_ad structure
core/advertising: use bt_ad
core/advertising: Parse ServiceUUIDs
core/advertising: Parse SolicitUUIDs
core/advertising: Parse ManufacturerSpecificData
core/advertising: Parse ServiceData
Update TODO for LE Advertising
shared/ad: implement bt_ad_generate
core/advertising: Add and Remove AD using MGMT
test: improvements to advertising-example
Makefile.am | 1 +
TODO | 20 +-
src/advertising.c | 310 +++++++++++++++++++++++-
src/shared/ad.c | 582 +++++++++++++++++++++++++++++++++++++++++++++
src/shared/ad.h | 60 +++++
test/advertisement-example | 12 +-
6 files changed, 958 insertions(+), 27 deletions(-)
create mode 100644 src/shared/ad.c
create mode 100644 src/shared/ad.h
--
2.2.0.rc0.207.ga3a616c
Hi Michael,
> On Tue, Mar 31, 2015 at 10:24 AM, Michael Janssen <[email protected]> wrote:
> Implements the function to provide a raw version of the advertising data
> packet for passing to the kernel.
> ---
> src/shared/ad.c | 244 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
> 1 file changed, 240 insertions(+), 4 deletions(-)
>
> diff --git a/src/shared/ad.c b/src/shared/ad.c
> index 8e38d26..be8e8dc 100644
> --- a/src/shared/ad.c
> +++ b/src/shared/ad.c
> @@ -22,6 +22,21 @@
> #include "src/shared/queue.h"
> #include "src/shared/util.h"
>
> +#define MAX_ADV_DATA_LEN 31
> +
> +#define LE_AD_TYPE_UUID16_ALL 0x03 /* 16-bit service UUIDs, all */
> +#define LE_AD_TYPE_UUID32_ALL 0x05 /* 16-bit service UUIDs, all */
> +#define LE_AD_TYPE_UUID128_ALL 0x07 /* 16-bit service UUIDs, all */
> +#define LE_AD_TYPE_SOLICIT16 0x14 /* 16-bit service solic. UUIDs */
> +#define LE_AD_TYPE_SOLICIT32 0x1F /* 32-bit service solic. UUIDs */
> +#define LE_AD_TYPE_SOLICIT128 0x15 /* 128-bit service solic. UUIDs */
> +#define LE_AD_TYPE_SVC_DATA16 0x16 /* Service Data - 16-bit UUID */
> +#define LE_AD_TYPE_SVC_DATA32 0x20 /* Service Data - 32-bit UUID */
> +#define LE_AD_TYPE_SVC_DATA128 0x21 /* Service Data - 128-bit UUID */
> +#define LE_AD_TYPE_PUB_TRGT_ADDR 0x17 /* Public Target Address */
> +#define LE_AD_TYPE_RND_TRGT_ADDR 0x18 /* Random Target Address */
> +#define LE_AD_TYPE_MANUF_DATA 0xFF /* Manufacturer Specific Data */
> +
It's probably better to use the macros in src/eir.h here. If anything
is missing, let's just add new definitions to src/eir.h instead of
here.
> struct bt_ad {
> int ref_count;
> struct queue *service_uuids;
> @@ -136,10 +151,231 @@ void bt_ad_unref(struct bt_ad *ad)
> free(ad);
> }
>
> +static uint8_t uuid_list_length(struct queue *uuid_queue)
> +{
> + bool uuid16_included = false;
> + bool uuid32_included = false;
> + bool uuid128_included = false;
> + uint8_t length = 0;
> + const struct queue_entry *entry;
> +
> + entry = queue_get_entries(uuid_queue);
> +
> + while (entry) {
> + bt_uuid_t *uuid = entry->data;
> + uint8_t uuid_len = bt_uuid_len(uuid);
> +
> + length += uuid_len;
> +
> + if (uuid_len == 2)
I'd do "if (uuid->type == BT_UUID16)" etc.
> + uuid16_included = true;
> + else if (uuid_len == 4)
> + uuid32_included = true;
> + else
> + uuid128_included = true;
> +
> + entry = entry->next;
> + }
> +
> + if (uuid16_included)
> + length += 2;
> +
> + if (uuid32_included)
> + length += 2;
> +
> + if (uuid128_included)
> + length += 2;
> +
> + return length;
> +}
> +
> +static uint8_t mfg_data_length(struct queue *manuf_data)
> +{
> + uint8_t length = 0;
> + const struct queue_entry *entry;
> +
> + entry = queue_get_entries(manuf_data);
> +
> + while (entry) {
> + struct manufacturer_tagged_data *data = entry->data;
> +
> + length += 2 + sizeof(uint16_t) + data->len;
> +
> + entry = entry->next;
> + }
> +
> + return length;
> +}
> +
> +static uint8_t uuid_data_length(struct queue *tagged_data)
> +{
> + uint8_t length = 0;
> + const struct queue_entry *entry;
> +
> + entry = queue_get_entries(tagged_data);
> +
> + while (entry) {
> + struct uuid_tagged_data *data = entry->data;
> +
> + length += 2 + bt_uuid_len(&data->uuid) + data->len;
> +
> + entry = entry->next;
> + }
> +
> + return length;
> +}
> +
> +static uint8_t calculate_length(struct bt_ad *ad)
> +{
> + uint8_t length = 0;
> +
> + length += uuid_list_length(ad->service_uuids);
> +
> + length += uuid_list_length(ad->solicit_uuids);
> +
> + length += mfg_data_length(ad->manufacturer_data);
> +
> + length += uuid_data_length(ad->service_data);
> +
> + return length;
> +}
> +
> +static void serialize_uuids(struct queue *uuids, uint8_t uuid_type,
> + uint8_t ad_type, uint8_t *buf,
> + uint8_t *pos)
> +{
> + const struct queue_entry *entry = queue_get_entries(uuids);
> + bool added = false;
> + uint8_t length_pos = 0;
> +
> + while (entry) {
> + bt_uuid_t *uuid = entry->data;
> +
> + if (uuid->type == uuid_type) {
> + if (!added) {
> + length_pos = (*pos)++;
> + buf[(*pos)++] = ad_type;
> + added = true;
> + }
> +
> + if (uuid_type != BT_UUID32)
> + bt_uuid_to_le(uuid, buf + *pos);
> + else
> + bt_put_le32(uuid->value.u32, buf + *pos);
> +
> + *pos += bt_uuid_len(uuid);
> + }
> +
> + entry = entry->next;
> + }
> +
> + if (added)
> + buf[length_pos] = *pos - length_pos - 1;
> +}
> +
> +static void serialize_service_uuids(struct queue *uuids, uint8_t *buf,
> + uint8_t *pos)
> +{
> + serialize_uuids(uuids, BT_UUID16, LE_AD_TYPE_UUID16_ALL, buf, pos);
> +
> + serialize_uuids(uuids, BT_UUID32, LE_AD_TYPE_UUID32_ALL, buf, pos);
> +
> + serialize_uuids(uuids, BT_UUID128, LE_AD_TYPE_UUID128_ALL, buf, pos);
> +}
> +
> +static void serialize_solicit_uuids(struct queue *uuids, uint8_t *buf,
> + uint8_t *pos)
> +{
> + serialize_uuids(uuids, BT_UUID16, LE_AD_TYPE_SOLICIT16, buf, pos);
> +
> + serialize_uuids(uuids, BT_UUID32, LE_AD_TYPE_SOLICIT32, buf, pos);
> +
> + serialize_uuids(uuids, BT_UUID128, LE_AD_TYPE_SOLICIT128, buf, pos);
> +}
> +
> +static void serialize_manuf_data(struct queue *manuf_data, uint8_t *buf,
> + uint8_t *pos)
> +{
> + const struct queue_entry *entry = queue_get_entries(manuf_data);
> +
> + while (entry) {
> + struct manufacturer_tagged_data *data = entry->data;
> +
> + buf[(*pos)++] = data->len + 2 + 1;
> +
> + buf[(*pos)++] = LE_AD_TYPE_MANUF_DATA;
> +
> + bt_put_le16(data->manufacturer_id, buf + (*pos));
> +
> + *pos += 2;
> +
> + memcpy(buf + *pos, data->data, data->len);
> +
> + *pos += data->len;
> +
> + entry = entry->next;
> + }
> +}
> +
> +static void serialize_service_data(struct queue *service_data, uint8_t *buf,
> + uint8_t *pos)
> +{
> + const struct queue_entry *entry = queue_get_entries(service_data);
> +
> + while (entry) {
> + struct uuid_tagged_data *data = entry->data;
> + int uuid_len = bt_uuid_len(&data->uuid);
> +
> + buf[(*pos)++] = uuid_len + data->len + 1;
> +
> + switch (uuid_len) {
> + case 2:
> + buf[(*pos)++] = LE_AD_TYPE_SVC_DATA16;
> + break;
> + case 4:
> + buf[(*pos)++] = LE_AD_TYPE_SVC_DATA32;
> + break;
> + case 16:
> + buf[(*pos)++] = LE_AD_TYPE_SVC_DATA128;
> + break;
> + }
> +
> + if (uuid_len != 4)
> + bt_uuid_to_le(&data->uuid, buf + *pos);
> + else
> + bt_put_le32(data->uuid.value.u32, buf + *pos);
> +
> + *pos += uuid_len;
> +
> + memcpy(buf + *pos, data->data, data->len);
> +
> + *pos += data->len;
> +
> + entry = entry->next;
> + }
> +}
> +
> uint8_t *bt_ad_generate(struct bt_ad *ad, uint8_t *length)
> {
> - /* TODO: implement */
> - return NULL;
> + uint8_t *adv_data;
> + uint8_t pos = 0;
> +
> + *length = calculate_length(ad);
> +
> + if (*length > MAX_ADV_DATA_LEN)
> + return NULL;
> +
> + adv_data = malloc0(*length);
> +
> + serialize_service_uuids(ad->service_uuids, adv_data, &pos);
> +
> + serialize_solicit_uuids(ad->solicit_uuids, adv_data, &pos);
> +
> + serialize_manuf_data(ad->manufacturer_data, adv_data, &pos);
> +
> + serialize_service_data(ad->service_data, adv_data, &pos);
> +
> + return adv_data;
> }
>
> static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
> @@ -153,7 +389,7 @@ static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
> if (!new_uuid)
> return false;
>
> - bt_uuid_to_uuid128(uuid, new_uuid);
> + *new_uuid = *uuid;
>
> queue_push_tail(queue, new_uuid);
>
> @@ -301,7 +537,7 @@ bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void *data,
> if (!new_data)
> return false;
>
> - bt_uuid_to_uuid128(uuid, &new_data->uuid);
> + new_data->uuid = *uuid;
>
> new_data->data = malloc(len);
> if (!new_data) {
> --
> 2.2.0.rc0.207.ga3a616c
>
> --
> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
> the body of a message to [email protected]
> More majordomo info at http://vger.kernel.org/majordomo-info.html
Thanks,
Arman
Improves the advertising-example in two ways:
It uses 16-bit service UUIDs so that it does not overflow the
Advertisement Data
It turns the adapter's power on so that it works out of the box.
---
test/advertisement-example | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/test/advertisement-example b/test/advertisement-example
index 98aeafa..6e47391 100644
--- a/test/advertisement-example
+++ b/test/advertisement-example
@@ -113,11 +113,10 @@ class TestAdvertisement(Advertisement):
def __init__(self, bus, index):
Advertisement.__init__(self, bus, index, 'broadcast')
- self.add_service_uuid('0000180D-0000-1000-8000-00805F9B34FB')
- self.add_service_uuid('0000180F-0000-1000-8000-00805F9B34FB')
+ self.add_service_uuid('180D')
+ self.add_service_uuid('180F')
self.add_manufacturer_data(0xffff, [0x00, 0x01, 0x02, 0x03, 0x04])
- self.add_service_data('00009999-0000-1000-8000-00805F9B34FB',
- [0x00, 0x01, 0x02, 0x03, 0x04])
+ self.add_service_data('9999', [0x00, 0x01, 0x02, 0x03, 0x04])
def register_ad_cb():
@@ -153,6 +152,11 @@ def main():
print 'LEAdvertisingManager1 interface not found'
return
+ adapter_props = dbus.Interface(bus.get_object(BLUEZ_SERVICE_NAME, adapter),
+ "org.freedesktop.DBus.Properties");
+
+ adapter_props.Set("org.bluez.Adapter1", "Powered", dbus.Boolean(1))
+
ad_manager = dbus.Interface(bus.get_object(BLUEZ_SERVICE_NAME, adapter),
LE_ADVERTISING_MANAGER_IFACE)
--
2.2.0.rc0.207.ga3a616c
Parse the ManufacturerSpecificData property of the LEAdvertisement1
---
src/advertising.c | 57 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 57 insertions(+)
diff --git a/src/advertising.c b/src/advertising.c
index d736ea9..8d6c13e 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -244,6 +244,58 @@ fail:
return false;
}
+static bool parse_advertising_manufacturer_data(GDBusProxy *proxy,
+ struct bt_ad *data)
+{
+ DBusMessageIter iter, entries;
+
+ if (!g_dbus_proxy_get_property(proxy, "ManufacturerData", &iter))
+ return true;
+
+ if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY)
+ return false;
+
+ dbus_message_iter_recurse(&iter, &entries);
+
+ bt_ad_clear_manufacturer_data(data);
+
+ while (dbus_message_iter_get_arg_type(&entries)
+ == DBUS_TYPE_DICT_ENTRY) {
+ DBusMessageIter value, entry;
+ uint16_t manuf_id;
+ uint8_t *manuf_data;
+ int len;
+
+ dbus_message_iter_recurse(&entries, &entry);
+ dbus_message_iter_get_basic(&entry, &manuf_id);
+
+ dbus_message_iter_next(&entry);
+ if (dbus_message_iter_get_arg_type(&entry) != DBUS_TYPE_ARRAY)
+ goto fail;
+
+ dbus_message_iter_recurse(&entry, &value);
+
+ if (dbus_message_iter_get_arg_type(&value) != DBUS_TYPE_BYTE)
+ goto fail;
+
+ dbus_message_iter_get_fixed_array(&value, &manuf_data, &len);
+
+ DBG("Adding ManufacturerData for %04x", manuf_id);
+
+ if (!bt_ad_add_manufacturer_data(data, manuf_id, manuf_data,
+ len))
+ goto fail;
+
+ dbus_message_iter_next(&entries);
+ }
+
+ return true;
+
+fail:
+ bt_ad_clear_manufacturer_data(data);
+ return false;
+}
+
static void refresh_advertisement(struct advertisement *ad)
{
DBG("Refreshing advertisement: %s", ad->path);
@@ -266,6 +318,11 @@ static bool parse_advertisement(struct advertisement *ad)
return false;
}
+ if (!parse_advertising_manufacturer_data(ad->proxy, ad->data)) {
+ error("Property \"ManufacturerData\" failed to parse");
+ return false;
+ }
+
/* TODO: parse the rest of the properties */
refresh_advertisement(ad);
--
2.2.0.rc0.207.ga3a616c
Updates the advertising manager to add and remove the LE Advertising Data
using MGMT interface commands.
---
src/advertising.c | 104 +++++++++++++++++++++++++++++++++++++++++++++---------
1 file changed, 88 insertions(+), 16 deletions(-)
diff --git a/src/advertising.c b/src/advertising.c
index 74b9cbd..300f440 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -26,6 +26,7 @@
#include <gdbus/gdbus.h>
#include "lib/bluetooth.h"
+#include "lib/mgmt.h"
#include "lib/sdp.h"
#include "adapter.h"
@@ -33,6 +34,7 @@
#include "error.h"
#include "log.h"
#include "src/shared/ad.h"
+#include "src/shared/mgmt.h"
#include "src/shared/queue.h"
#include "src/shared/util.h"
@@ -42,6 +44,8 @@
struct btd_advertising {
struct btd_adapter *adapter;
struct queue *ads;
+ struct mgmt *mgmt;
+ uint16_t mgmt_index;
};
#define AD_TYPE_BROADCAST 0
@@ -56,6 +60,7 @@ struct advertisement {
DBusMessage *reg;
uint8_t type; /* Advertising type */
struct bt_ad *data;
+ uint8_t instance;
};
static bool match_advertisement_path(const void *a, const void *b)
@@ -123,10 +128,15 @@ static void advertisement_destroy(void *data)
static void advertisement_remove(void *data)
{
struct advertisement *ad = data;
+ struct mgmt_cp_remove_advertising cp;
g_dbus_client_set_disconnect_watch(ad->client, NULL, NULL);
- /* TODO: mgmt API call to remove advert */
+ cp.instance = ad->instance;
+
+ mgmt_send(ad->manager->mgmt, MGMT_OP_REMOVE_ADVERTISING,
+ ad->manager->mgmt_index, sizeof(cp), &cp, NULL, NULL,
+ NULL);
queue_remove(ad->manager->ads, ad);
@@ -351,43 +361,90 @@ fail:
return false;
}
-static void refresh_advertisement(struct advertisement *ad)
+static void add_advertising_callback(uint8_t status, uint16_t length,
+ const void *param, void *user_data)
+{
+ struct advertisement *ad = user_data;
+ const struct mgmt_rp_add_advertising *rp = param;
+
+ if (status || !param) {
+ error("Failed to add advertising MGMT");
+ return;
+ }
+
+ ad->instance = rp->instance;
+}
+
+static DBusMessage *refresh_advertisement(struct advertisement *ad)
{
+ struct mgmt_cp_add_advertising *cp;
+ uint8_t adv_data_len, param_len;
+ uint8_t *adv_data;
+
DBG("Refreshing advertisement: %s", ad->path);
+
+ adv_data = bt_ad_generate(ad->data, &adv_data_len);
+
+ if (!adv_data) {
+ error("Advertising data couldn't be generated.");
+
+ return g_dbus_create_error(ad->reg, ERROR_INTERFACE
+ ".InvalidLength",
+ "Advertising data too long.");
+ }
+
+ param_len = sizeof(struct mgmt_cp_add_advertising) + adv_data_len;
+
+ cp = malloc0(param_len);
+
+ if (!cp) {
+ error("Couldn't allocate for MGMT!");
+
+ return btd_error_failed(ad->reg, "Failed");
+ }
+
+ cp->instance = ad->instance;
+ cp->adv_data_len = adv_data_len;
+ memcpy(cp->data, adv_data, adv_data_len);
+
+ mgmt_send(ad->manager->mgmt, MGMT_OP_ADD_ADVERTISING,
+ ad->manager->mgmt_index, param_len, cp,
+ add_advertising_callback, ad, NULL);
+
+ return NULL;
}
-static bool parse_advertisement(struct advertisement *ad)
+static DBusMessage *parse_advertisement(struct advertisement *ad)
{
if (!parse_advertising_type(ad->proxy, &ad->type)) {
error("Failed to read \"Type\" property of advertisement");
- return false;
+ goto fail;
}
if (!parse_advertising_service_uuids(ad->proxy, ad->data)) {
error("Property \"ServiceUUIDs\" failed to parse");
- return false;
+ goto fail;
}
if (!parse_advertising_solicit_uuids(ad->proxy, ad->data)) {
error("Property \"SolicitUUIDs\" failed to parse");
- return false;
+ goto fail;
}
if (!parse_advertising_manufacturer_data(ad->proxy, ad->data)) {
error("Property \"ManufacturerData\" failed to parse");
- return false;
+ goto fail;
}
if (!parse_advertising_service_data(ad->proxy, ad->data)) {
error("Property \"ServiceData\" failed to parse");
- return false;
+ goto fail;
}
- /* TODO: parse the rest of the properties */
+ return refresh_advertisement(ad);
- refresh_advertisement(ad);
-
- return true;
+fail:
+ return btd_error_failed(ad->reg, "Failed to parse advertisement.");
}
static void advertisement_proxy_added(GDBusProxy *proxy, void *data)
@@ -395,11 +452,14 @@ static void advertisement_proxy_added(GDBusProxy *proxy, void *data)
struct advertisement *ad = data;
DBusMessage *reply;
- if (!parse_advertisement(ad)) {
- error("Failed to parse advertisement");
+ reply = parse_advertisement(ad);
+
+ if (reply) {
+ /* Failed to publish for some reason, remove. */
+ queue_remove(ad->manager->ads, ad);
+
+ g_idle_add(advertisement_free_idle_cb, ad);
- reply = btd_error_failed(ad->reg,
- "Failed to register advertisement");
goto done;
}
@@ -430,6 +490,8 @@ static struct advertisement *advertisement_create(DBusConnection *conn,
if (!ad)
return NULL;
+ ad->instance = 1;
+
ad->client = g_dbus_client_new_full(conn, sender, path, path);
if (!ad->client)
goto fail;
@@ -567,6 +629,16 @@ advertising_manager_create(struct btd_adapter *adapter)
manager->adapter = adapter;
+ manager->mgmt = mgmt_new_default();
+
+ if (!manager->mgmt) {
+ error("Failed to access management interface");
+ free(manager);
+ return NULL;
+ }
+
+ manager->mgmt_index = btd_adapter_get_index(adapter);
+
if (!g_dbus_register_interface(btd_get_dbus_connection(),
adapter_get_path(adapter),
LE_ADVERTISING_MGR_IFACE,
--
2.2.0.rc0.207.ga3a616c
Parse the ServiceData property of LEAdvertisement1
---
src/advertising.c | 60 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 60 insertions(+)
diff --git a/src/advertising.c b/src/advertising.c
index 8d6c13e..74b9cbd 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -296,6 +296,61 @@ fail:
return false;
}
+static bool parse_advertising_service_data(GDBusProxy *proxy,
+ struct bt_ad *data)
+{
+ DBusMessageIter iter, entries;
+
+ if (!g_dbus_proxy_get_property(proxy, "ServiceData", &iter))
+ return true;
+
+ if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY)
+ return false;
+
+ dbus_message_iter_recurse(&iter, &entries);
+
+ bt_ad_clear_service_data(data);
+
+ while (dbus_message_iter_get_arg_type(&entries)
+ == DBUS_TYPE_DICT_ENTRY) {
+ DBusMessageIter value, entry;
+ const char *uuid_str;
+ bt_uuid_t uuid;
+ uint8_t *service_data;
+ int len;
+
+ dbus_message_iter_recurse(&entries, &entry);
+ dbus_message_iter_get_basic(&entry, &uuid_str);
+
+ if (bt_string_to_uuid(&uuid, uuid_str) < 0)
+ goto fail;
+
+ dbus_message_iter_next(&entry);
+ if (dbus_message_iter_get_arg_type(&entry) != DBUS_TYPE_ARRAY)
+ goto fail;
+
+ dbus_message_iter_recurse(&entry, &value);
+
+ if (dbus_message_iter_get_arg_type(&value) != DBUS_TYPE_BYTE)
+ goto fail;
+
+ dbus_message_iter_get_fixed_array(&value, &service_data, &len);
+
+ DBG("Adding ServiceData for %s", uuid_str);
+
+ if (!bt_ad_add_service_data(data, &uuid, service_data, len))
+ goto fail;
+
+ dbus_message_iter_next(&entries);
+ }
+
+ return true;
+
+fail:
+ bt_ad_clear_service_data(data);
+ return false;
+}
+
static void refresh_advertisement(struct advertisement *ad)
{
DBG("Refreshing advertisement: %s", ad->path);
@@ -323,6 +378,11 @@ static bool parse_advertisement(struct advertisement *ad)
return false;
}
+ if (!parse_advertising_service_data(ad->proxy, ad->data)) {
+ error("Property \"ServiceData\" failed to parse");
+ return false;
+ }
+
/* TODO: parse the rest of the properties */
refresh_advertisement(ad);
--
2.2.0.rc0.207.ga3a616c
The bt_ad structure provides an abstraction for easy translation of defined
Advertisement Data fields into the resulting raw bytes needed by the Bluetooth
HCI LE Set Advertising Data command.
---
Makefile.am | 1 +
src/shared/ad.c | 346 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
src/shared/ad.h | 60 ++++++++++
3 files changed, 407 insertions(+)
create mode 100644 src/shared/ad.c
create mode 100644 src/shared/ad.h
diff --git a/Makefile.am b/Makefile.am
index 676d929..e144428 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -111,6 +111,7 @@ shared_sources = src/shared/io.h src/shared/timeout.h \
src/shared/uhid.h src/shared/uhid.c \
src/shared/pcap.h src/shared/pcap.c \
src/shared/btsnoop.h src/shared/btsnoop.c \
+ src/shared/ad.h src/shared/ad.c \
src/shared/att-types.h \
src/shared/att.h src/shared/att.c \
src/shared/gatt-helpers.h src/shared/gatt-helpers.c \
diff --git a/src/shared/ad.c b/src/shared/ad.c
new file mode 100644
index 0000000..8e38d26
--- /dev/null
+++ b/src/shared/ad.c
@@ -0,0 +1,346 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2015 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.
+ *
+ */
+
+#include "src/shared/ad.h"
+
+#include "src/shared/queue.h"
+#include "src/shared/util.h"
+
+struct bt_ad {
+ int ref_count;
+ struct queue *service_uuids;
+ struct queue *manufacturer_data;
+ struct queue *solicit_uuids;
+ struct queue *service_data;
+};
+
+struct uuid_tagged_data {
+ bt_uuid_t uuid;
+ uint8_t *data;
+ size_t len;
+};
+
+struct manufacturer_tagged_data {
+ uint16_t manufacturer_id;
+ uint8_t *data;
+ size_t len;
+};
+
+struct bt_ad *bt_ad_new(void)
+{
+ struct bt_ad *ad;
+
+ ad = new0(struct bt_ad, 1);
+ if (!ad)
+ return NULL;
+
+ ad->service_uuids = queue_new();
+ if (!ad->service_uuids)
+ goto fail;
+
+ ad->manufacturer_data = queue_new();
+ if (!ad->manufacturer_data)
+ goto fail;
+
+ ad->solicit_uuids = queue_new();
+ if (!ad->solicit_uuids)
+ goto fail;
+
+ ad->service_data = queue_new();
+ if (!ad->service_data)
+ goto fail;
+
+ return bt_ad_ref(ad);
+
+fail:
+ queue_destroy(ad->service_uuids, NULL);
+ queue_destroy(ad->manufacturer_data, NULL);
+ queue_destroy(ad->solicit_uuids, NULL);
+ queue_destroy(ad->service_data, NULL);
+
+ free(ad);
+
+ return NULL;
+}
+
+struct bt_ad *bt_ad_ref(struct bt_ad *ad)
+{
+ ad->ref_count++;
+ return ad;
+}
+
+static void uuid_tagged_destroy(void *data)
+{
+ struct uuid_tagged_data *tagged = data;
+
+ free(tagged->data);
+ free(tagged);
+}
+
+static bool uuid_tagged_match(const void *data, const void *elem)
+{
+ const struct uuid_tagged_data *tagged = elem;
+ const bt_uuid_t *uuid = data;
+
+ return !bt_uuid_cmp(&tagged->uuid, uuid);
+}
+
+static void manuf_tagged_destroy(void *data)
+{
+ struct manufacturer_tagged_data *tagged = data;
+
+ free(tagged->data);
+ free(tagged);
+}
+
+static bool manuf_tagged_match(const void *data, const void *elem)
+{
+ const struct manufacturer_tagged_data *tagged = elem;
+ uint16_t manuf_id = PTR_TO_UINT(elem);
+
+ return tagged->manufacturer_id == manuf_id;
+}
+
+void bt_ad_unref(struct bt_ad *ad)
+{
+ if (!ad)
+ return;
+
+ if (__sync_sub_and_fetch(&ad->ref_count, 1))
+ return;
+
+ queue_destroy(ad->service_uuids, free);
+
+ queue_destroy(ad->manufacturer_data, manuf_tagged_destroy);
+
+ queue_destroy(ad->solicit_uuids, free);
+
+ queue_destroy(ad->service_data, uuid_tagged_destroy);
+
+ free(ad);
+}
+
+uint8_t *bt_ad_generate(struct bt_ad *ad, uint8_t *length)
+{
+ /* TODO: implement */
+ return NULL;
+}
+
+static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
+{
+ bt_uuid_t *new_uuid;
+
+ if (!queue)
+ return false;
+
+ new_uuid = new0(bt_uuid_t, 1);
+ if (!new_uuid)
+ return false;
+
+ bt_uuid_to_uuid128(uuid, new_uuid);
+
+ queue_push_tail(queue, new_uuid);
+
+ return true;
+}
+
+static bool uuid_match(const void *data, const void *elem)
+{
+ const bt_uuid_t *match_uuid = data;
+ const bt_uuid_t *uuid = elem;
+
+ return bt_uuid_cmp(match_uuid, uuid);
+}
+
+static bool queue_remove_uuid(struct queue *queue, bt_uuid_t *uuid)
+{
+ bt_uuid_t *removed;
+
+ if (!queue || !uuid)
+ return false;
+
+ removed = queue_remove_if(queue, uuid_match, uuid);
+
+ if (removed) {
+ free(removed);
+ return true;
+ }
+
+ return false;
+}
+
+bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
+{
+ if (!ad)
+ return false;
+
+ return queue_add_uuid(ad->service_uuids, uuid);
+}
+
+bool bt_ad_remove_service_uuid(struct bt_ad *ad, bt_uuid_t *uuid)
+{
+ if (!ad)
+ return false;
+
+ return queue_remove_uuid(ad->service_uuids, uuid);
+}
+
+void bt_ad_clear_service_uuid(struct bt_ad *ad)
+{
+ queue_destroy(ad->service_uuids, free);
+
+ ad->service_uuids = queue_new();
+}
+
+bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_id,
+ void *data, size_t len)
+{
+ struct manufacturer_tagged_data *new_data;
+
+ if (!ad)
+ return false;
+
+ new_data = new0(struct manufacturer_tagged_data, 1);
+ if (!new_data)
+ return false;
+
+ new_data->manufacturer_id = manufacturer_id;
+
+ new_data->data = malloc(len);
+ if (!new_data->data) {
+ free(new_data);
+ return false;
+ }
+
+ memcpy(new_data->data, data, len);
+
+ new_data->len = len;
+
+ if (queue_push_tail(ad->manufacturer_data, new_data))
+ return true;
+
+ manuf_tagged_destroy(new_data);
+
+ return false;
+}
+
+bool bt_ad_remove_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_id)
+{
+ struct manufacturer_tagged_data *data;
+
+ if (!ad)
+ return false;
+
+ data = queue_remove_if(ad->manufacturer_data, manuf_tagged_match,
+ UINT_TO_PTR(manufacturer_id));
+
+ if (!data)
+ return false;
+
+ manuf_tagged_destroy(data);
+
+ return true;
+}
+
+void bt_ad_clear_manufacturer_data(struct bt_ad *ad)
+{
+ queue_destroy(ad->manufacturer_data, manuf_tagged_destroy);
+
+ ad->manufacturer_data = queue_new();
+}
+
+bool bt_ad_add_solicit_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
+{
+ if (!ad)
+ return false;
+
+ return queue_add_uuid(ad->solicit_uuids, uuid);
+}
+
+bool bt_ad_remove_solicit_uuid(struct bt_ad *ad,
+ bt_uuid_t *uuid)
+{
+ if (!ad)
+ return false;
+
+ return queue_remove_uuid(ad->solicit_uuids, uuid);
+}
+
+void bt_ad_clear_solicit_uuid(struct bt_ad *ad)
+{
+ queue_destroy(ad->solicit_uuids, free);
+
+ ad->solicit_uuids = queue_new();
+}
+
+bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void *data,
+ size_t len)
+{
+ struct uuid_tagged_data *new_data;
+
+ if (!ad)
+ return false;
+
+ new_data = new0(struct uuid_tagged_data, 1);
+ if (!new_data)
+ return false;
+
+ bt_uuid_to_uuid128(uuid, &new_data->uuid);
+
+ new_data->data = malloc(len);
+ if (!new_data) {
+ free(new_data);
+ return false;
+ }
+
+ memcpy(new_data->data, data, len);
+
+ new_data->len = len;
+
+ if (queue_push_tail(ad->service_data, new_data))
+ return true;
+
+ uuid_tagged_destroy(new_data);
+
+ return false;
+}
+
+bool bt_ad_remove_service_data(struct bt_ad *ad, bt_uuid_t *uuid)
+{
+ struct uuid_tagged_data *data;
+
+ if (!ad)
+ return false;
+
+ data = queue_remove_if(ad->service_data, uuid_tagged_match, uuid);
+
+ if (!data)
+ return false;
+
+ uuid_tagged_destroy(data);
+
+ return true;
+}
+
+void bt_ad_clear_service_data(struct bt_ad *ad)
+{
+ queue_destroy(ad->service_data, uuid_tagged_destroy);
+
+ ad->service_data = queue_new();
+}
diff --git a/src/shared/ad.h b/src/shared/ad.h
new file mode 100644
index 0000000..0e41da0
--- /dev/null
+++ b/src/shared/ad.h
@@ -0,0 +1,60 @@
+/*
+ *
+ * BlueZ - Bluetooth protocol stack for Linux
+ *
+ * Copyright (C) 2015 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.
+ *
+ */
+
+#include <inttypes.h>
+#include <stdbool.h>
+
+#include "lib/bluetooth.h"
+#include "lib/uuid.h"
+
+struct bt_ad;
+
+struct bt_ad *bt_ad_new(void);
+
+struct bt_ad *bt_ad_ref(struct bt_ad *ad);
+
+void bt_ad_unref(struct bt_ad *ad);
+
+uint8_t *bt_ad_generate(struct bt_ad *ad, uint8_t *length);
+
+bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid);
+
+bool bt_ad_remove_service_uuid(struct bt_ad *ad, bt_uuid_t *uuid);
+
+void bt_ad_clear_service_uuid(struct bt_ad *ad);
+
+bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_data,
+ void *data, size_t len);
+
+bool bt_ad_remove_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_id);
+
+void bt_ad_clear_manufacturer_data(struct bt_ad *ad);
+
+bool bt_ad_add_solicit_uuid(struct bt_ad *ad, const bt_uuid_t *uuid);
+
+bool bt_ad_remove_solicit_uuid(struct bt_ad *ad, bt_uuid_t *uuid);
+
+void bt_ad_clear_solicit_uuid(struct bt_ad *ad);
+
+bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void *data,
+ size_t len);
+
+bool bt_ad_remove_service_data(struct bt_ad *ad, bt_uuid_t *uuid);
+
+void bt_ad_clear_service_data(struct bt_ad *ad);
--
2.2.0.rc0.207.ga3a616c
Parse the ServiceUUIDs property of the LEAdvertisement1 object.
---
src/advertising.c | 46 +++++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 45 insertions(+), 1 deletion(-)
diff --git a/src/advertising.c b/src/advertising.c
index 275e2d7..9867e1e 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -166,6 +166,45 @@ static bool parse_advertising_type(GDBusProxy *proxy, uint8_t *type)
return false;
}
+static bool parse_advertising_service_uuids(GDBusProxy *proxy,
+ struct bt_ad *data)
+{
+ DBusMessageIter iter, ariter;
+
+ if (!g_dbus_proxy_get_property(proxy, "ServiceUUIDs", &iter))
+ return true;
+
+ if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY)
+ return false;
+
+ dbus_message_iter_recurse(&iter, &ariter);
+
+ bt_ad_clear_service_uuid(data);
+
+ while (dbus_message_iter_get_arg_type(&ariter) == DBUS_TYPE_STRING) {
+ const char *uuid_str;
+ bt_uuid_t uuid;
+
+ dbus_message_iter_get_basic(&ariter, &uuid_str);
+
+ DBG("Adding ServiceUUID: %s", uuid_str);
+
+ if (bt_string_to_uuid(&uuid, uuid_str) < 0)
+ goto fail;
+
+ if (!bt_ad_add_service_uuid(data, &uuid))
+ goto fail;
+
+ dbus_message_iter_next(&ariter);
+ }
+
+ return true;
+
+fail:
+ bt_ad_clear_service_uuid(data);
+ return false;
+}
+
static void refresh_advertisement(struct advertisement *ad)
{
DBG("Refreshing advertisement: %s", ad->path);
@@ -178,7 +217,12 @@ static bool parse_advertisement(struct advertisement *ad)
return false;
}
- /* TODO: parse the remaining properties into a shared structure */
+ if (!parse_advertising_service_uuids(ad->proxy, ad->data)) {
+ error("Property \"ServiceUUIDs\" failed to parse");
+ return false;
+ }
+
+ /* TODO: parse the rest of the properties */
refresh_advertisement(ad);
--
2.2.0.rc0.207.ga3a616c
---
TODO | 20 ++++++++++----------
1 file changed, 10 insertions(+), 10 deletions(-)
diff --git a/TODO b/TODO
index 65e19ee..2945392 100644
--- a/TODO
+++ b/TODO
@@ -64,16 +64,16 @@ General
Low Energy
==========
-- Advertising management. Adapter interface needs to be changed to manage
- connection modes, adapter type and advertising policy. See Volume 3,
- Part C, section 9.3. If Attribute Server is enabled the LE capable
- adapter shall to start advertising. Further investigation is necessary
- to define which connectable mode needs to be supported: Non-connectable,
- directed connectable and undirected connectable. Basically, two connectable
- scenarios shall be addressed:
- 1. GATT client is disconnected, but intends to become a Peripheral to
- receive indications/notifications.
- 2. GATT server intends to accept connections.
+- Connection modes. Adapter interface needs to be changed to manage
+ connection modes and adapter type. See Volume 3, Part C, section 9.3.
+ 1. Mode management: Peripheral / Central
+
+ Priority: Medium
+ Complexity: C2
+
+- Advertising data. The D-Bus interface needs to be updated to enable setting
+ scan response data, and to read the advertising and scan response data which
+ has been broadcast from other LE devices.
Priority: Medium
Complexity: C2
--
2.2.0.rc0.207.ga3a616c
Parse the SolicitUUIDs property of the LEAdvertisement1 object.
---
src/advertising.c | 44 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 44 insertions(+)
diff --git a/src/advertising.c b/src/advertising.c
index 9867e1e..d736ea9 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -205,6 +205,45 @@ fail:
return false;
}
+static bool parse_advertising_solicit_uuids(GDBusProxy *proxy,
+ struct bt_ad *data)
+{
+ DBusMessageIter iter, ariter;
+
+ if (!g_dbus_proxy_get_property(proxy, "SolicitUUIDs", &iter))
+ return true;
+
+ if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_ARRAY)
+ return false;
+
+ dbus_message_iter_recurse(&iter, &ariter);
+
+ bt_ad_clear_solicit_uuid(data);
+
+ while (dbus_message_iter_get_arg_type(&ariter) == DBUS_TYPE_STRING) {
+ const char *uuid_str;
+ bt_uuid_t uuid;
+
+ dbus_message_iter_get_basic(&ariter, &uuid_str);
+
+ DBG("Adding SolicitUUID: %s", uuid_str);
+
+ if (bt_string_to_uuid(&uuid, uuid_str) < 0)
+ goto fail;
+
+ if (!bt_ad_add_solicit_uuid(data, &uuid))
+ goto fail;
+
+ dbus_message_iter_next(&ariter);
+ }
+
+ return true;
+
+fail:
+ bt_ad_clear_solicit_uuid(data);
+ return false;
+}
+
static void refresh_advertisement(struct advertisement *ad)
{
DBG("Refreshing advertisement: %s", ad->path);
@@ -222,6 +261,11 @@ static bool parse_advertisement(struct advertisement *ad)
return false;
}
+ if (!parse_advertising_solicit_uuids(ad->proxy, ad->data)) {
+ error("Property \"SolicitUUIDs\" failed to parse");
+ return false;
+ }
+
/* TODO: parse the rest of the properties */
refresh_advertisement(ad);
--
2.2.0.rc0.207.ga3a616c
Implements the function to provide a raw version of the advertising data
packet for passing to the kernel.
---
src/shared/ad.c | 244 +++++++++++++++++++++++++++++++++++++++++++++++++++++++-
1 file changed, 240 insertions(+), 4 deletions(-)
diff --git a/src/shared/ad.c b/src/shared/ad.c
index 8e38d26..be8e8dc 100644
--- a/src/shared/ad.c
+++ b/src/shared/ad.c
@@ -22,6 +22,21 @@
#include "src/shared/queue.h"
#include "src/shared/util.h"
+#define MAX_ADV_DATA_LEN 31
+
+#define LE_AD_TYPE_UUID16_ALL 0x03 /* 16-bit service UUIDs, all */
+#define LE_AD_TYPE_UUID32_ALL 0x05 /* 16-bit service UUIDs, all */
+#define LE_AD_TYPE_UUID128_ALL 0x07 /* 16-bit service UUIDs, all */
+#define LE_AD_TYPE_SOLICIT16 0x14 /* 16-bit service solic. UUIDs */
+#define LE_AD_TYPE_SOLICIT32 0x1F /* 32-bit service solic. UUIDs */
+#define LE_AD_TYPE_SOLICIT128 0x15 /* 128-bit service solic. UUIDs */
+#define LE_AD_TYPE_SVC_DATA16 0x16 /* Service Data - 16-bit UUID */
+#define LE_AD_TYPE_SVC_DATA32 0x20 /* Service Data - 32-bit UUID */
+#define LE_AD_TYPE_SVC_DATA128 0x21 /* Service Data - 128-bit UUID */
+#define LE_AD_TYPE_PUB_TRGT_ADDR 0x17 /* Public Target Address */
+#define LE_AD_TYPE_RND_TRGT_ADDR 0x18 /* Random Target Address */
+#define LE_AD_TYPE_MANUF_DATA 0xFF /* Manufacturer Specific Data */
+
struct bt_ad {
int ref_count;
struct queue *service_uuids;
@@ -136,10 +151,231 @@ void bt_ad_unref(struct bt_ad *ad)
free(ad);
}
+static uint8_t uuid_list_length(struct queue *uuid_queue)
+{
+ bool uuid16_included = false;
+ bool uuid32_included = false;
+ bool uuid128_included = false;
+ uint8_t length = 0;
+ const struct queue_entry *entry;
+
+ entry = queue_get_entries(uuid_queue);
+
+ while (entry) {
+ bt_uuid_t *uuid = entry->data;
+ uint8_t uuid_len = bt_uuid_len(uuid);
+
+ length += uuid_len;
+
+ if (uuid_len == 2)
+ uuid16_included = true;
+ else if (uuid_len == 4)
+ uuid32_included = true;
+ else
+ uuid128_included = true;
+
+ entry = entry->next;
+ }
+
+ if (uuid16_included)
+ length += 2;
+
+ if (uuid32_included)
+ length += 2;
+
+ if (uuid128_included)
+ length += 2;
+
+ return length;
+}
+
+static uint8_t mfg_data_length(struct queue *manuf_data)
+{
+ uint8_t length = 0;
+ const struct queue_entry *entry;
+
+ entry = queue_get_entries(manuf_data);
+
+ while (entry) {
+ struct manufacturer_tagged_data *data = entry->data;
+
+ length += 2 + sizeof(uint16_t) + data->len;
+
+ entry = entry->next;
+ }
+
+ return length;
+}
+
+static uint8_t uuid_data_length(struct queue *tagged_data)
+{
+ uint8_t length = 0;
+ const struct queue_entry *entry;
+
+ entry = queue_get_entries(tagged_data);
+
+ while (entry) {
+ struct uuid_tagged_data *data = entry->data;
+
+ length += 2 + bt_uuid_len(&data->uuid) + data->len;
+
+ entry = entry->next;
+ }
+
+ return length;
+}
+
+static uint8_t calculate_length(struct bt_ad *ad)
+{
+ uint8_t length = 0;
+
+ length += uuid_list_length(ad->service_uuids);
+
+ length += uuid_list_length(ad->solicit_uuids);
+
+ length += mfg_data_length(ad->manufacturer_data);
+
+ length += uuid_data_length(ad->service_data);
+
+ return length;
+}
+
+static void serialize_uuids(struct queue *uuids, uint8_t uuid_type,
+ uint8_t ad_type, uint8_t *buf,
+ uint8_t *pos)
+{
+ const struct queue_entry *entry = queue_get_entries(uuids);
+ bool added = false;
+ uint8_t length_pos = 0;
+
+ while (entry) {
+ bt_uuid_t *uuid = entry->data;
+
+ if (uuid->type == uuid_type) {
+ if (!added) {
+ length_pos = (*pos)++;
+ buf[(*pos)++] = ad_type;
+ added = true;
+ }
+
+ if (uuid_type != BT_UUID32)
+ bt_uuid_to_le(uuid, buf + *pos);
+ else
+ bt_put_le32(uuid->value.u32, buf + *pos);
+
+ *pos += bt_uuid_len(uuid);
+ }
+
+ entry = entry->next;
+ }
+
+ if (added)
+ buf[length_pos] = *pos - length_pos - 1;
+}
+
+static void serialize_service_uuids(struct queue *uuids, uint8_t *buf,
+ uint8_t *pos)
+{
+ serialize_uuids(uuids, BT_UUID16, LE_AD_TYPE_UUID16_ALL, buf, pos);
+
+ serialize_uuids(uuids, BT_UUID32, LE_AD_TYPE_UUID32_ALL, buf, pos);
+
+ serialize_uuids(uuids, BT_UUID128, LE_AD_TYPE_UUID128_ALL, buf, pos);
+}
+
+static void serialize_solicit_uuids(struct queue *uuids, uint8_t *buf,
+ uint8_t *pos)
+{
+ serialize_uuids(uuids, BT_UUID16, LE_AD_TYPE_SOLICIT16, buf, pos);
+
+ serialize_uuids(uuids, BT_UUID32, LE_AD_TYPE_SOLICIT32, buf, pos);
+
+ serialize_uuids(uuids, BT_UUID128, LE_AD_TYPE_SOLICIT128, buf, pos);
+}
+
+static void serialize_manuf_data(struct queue *manuf_data, uint8_t *buf,
+ uint8_t *pos)
+{
+ const struct queue_entry *entry = queue_get_entries(manuf_data);
+
+ while (entry) {
+ struct manufacturer_tagged_data *data = entry->data;
+
+ buf[(*pos)++] = data->len + 2 + 1;
+
+ buf[(*pos)++] = LE_AD_TYPE_MANUF_DATA;
+
+ bt_put_le16(data->manufacturer_id, buf + (*pos));
+
+ *pos += 2;
+
+ memcpy(buf + *pos, data->data, data->len);
+
+ *pos += data->len;
+
+ entry = entry->next;
+ }
+}
+
+static void serialize_service_data(struct queue *service_data, uint8_t *buf,
+ uint8_t *pos)
+{
+ const struct queue_entry *entry = queue_get_entries(service_data);
+
+ while (entry) {
+ struct uuid_tagged_data *data = entry->data;
+ int uuid_len = bt_uuid_len(&data->uuid);
+
+ buf[(*pos)++] = uuid_len + data->len + 1;
+
+ switch (uuid_len) {
+ case 2:
+ buf[(*pos)++] = LE_AD_TYPE_SVC_DATA16;
+ break;
+ case 4:
+ buf[(*pos)++] = LE_AD_TYPE_SVC_DATA32;
+ break;
+ case 16:
+ buf[(*pos)++] = LE_AD_TYPE_SVC_DATA128;
+ break;
+ }
+
+ if (uuid_len != 4)
+ bt_uuid_to_le(&data->uuid, buf + *pos);
+ else
+ bt_put_le32(data->uuid.value.u32, buf + *pos);
+
+ *pos += uuid_len;
+
+ memcpy(buf + *pos, data->data, data->len);
+
+ *pos += data->len;
+
+ entry = entry->next;
+ }
+}
+
uint8_t *bt_ad_generate(struct bt_ad *ad, uint8_t *length)
{
- /* TODO: implement */
- return NULL;
+ uint8_t *adv_data;
+ uint8_t pos = 0;
+
+ *length = calculate_length(ad);
+
+ if (*length > MAX_ADV_DATA_LEN)
+ return NULL;
+
+ adv_data = malloc0(*length);
+
+ serialize_service_uuids(ad->service_uuids, adv_data, &pos);
+
+ serialize_solicit_uuids(ad->solicit_uuids, adv_data, &pos);
+
+ serialize_manuf_data(ad->manufacturer_data, adv_data, &pos);
+
+ serialize_service_data(ad->service_data, adv_data, &pos);
+
+ return adv_data;
}
static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
@@ -153,7 +389,7 @@ static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
if (!new_uuid)
return false;
- bt_uuid_to_uuid128(uuid, new_uuid);
+ *new_uuid = *uuid;
queue_push_tail(queue, new_uuid);
@@ -301,7 +537,7 @@ bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void *data,
if (!new_data)
return false;
- bt_uuid_to_uuid128(uuid, &new_data->uuid);
+ new_data->uuid = *uuid;
new_data->data = malloc(len);
if (!new_data) {
--
2.2.0.rc0.207.ga3a616c
Start using the newly introduced struct bt_ad API.
---
src/advertising.c | 11 +++++++++--
1 file changed, 9 insertions(+), 2 deletions(-)
diff --git a/src/advertising.c b/src/advertising.c
index 04492f7..275e2d7 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -32,6 +32,7 @@
#include "dbus-common.h"
#include "error.h"
#include "log.h"
+#include "src/shared/ad.h"
#include "src/shared/queue.h"
#include "src/shared/util.h"
@@ -54,6 +55,7 @@ struct advertisement {
GDBusProxy *proxy;
DBusMessage *reg;
uint8_t type; /* Advertising type */
+ struct bt_ad *data;
};
static bool match_advertisement_path(const void *a, const void *b)
@@ -73,8 +75,9 @@ static void advertisement_free(void *data)
g_dbus_client_unref(ad->client);
}
- if (ad->proxy)
- g_dbus_proxy_unref(ad->proxy);
+ bt_ad_unref(ad->data);
+
+ g_dbus_proxy_unref(ad->proxy);
if (ad->owner)
g_free(ad->owner);
@@ -244,6 +247,10 @@ static struct advertisement *advertisement_create(DBusConnection *conn,
ad->reg = dbus_message_ref(msg);
+ ad->data = bt_ad_new();
+ if (!ad->data)
+ goto fail;
+
return ad;
fail:
--
2.2.0.rc0.207.ga3a616c
Hi Michael,
On Tue, Mar 31, 2015 at 8:23 PM, Michael Janssen <[email protected]> wrote:
> The bt_ad structure provides an abstraction for easy translation of defined
> Advertisement Data fields into the resulting raw bytes needed by the Bluetooth
> HCI LE Set Advertising Data command.
> ---
> Makefile.am | 1 +
> src/shared/ad.c | 346 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++
> src/shared/ad.h | 60 ++++++++++
> 3 files changed, 407 insertions(+)
> create mode 100644 src/shared/ad.c
> create mode 100644 src/shared/ad.h
>
> diff --git a/Makefile.am b/Makefile.am
> index 676d929..e144428 100644
> --- a/Makefile.am
> +++ b/Makefile.am
> @@ -111,6 +111,7 @@ shared_sources = src/shared/io.h src/shared/timeout.h \
> src/shared/uhid.h src/shared/uhid.c \
> src/shared/pcap.h src/shared/pcap.c \
> src/shared/btsnoop.h src/shared/btsnoop.c \
> + src/shared/ad.h src/shared/ad.c \
> src/shared/att-types.h \
> src/shared/att.h src/shared/att.c \
> src/shared/gatt-helpers.h src/shared/gatt-helpers.c \
> diff --git a/src/shared/ad.c b/src/shared/ad.c
> new file mode 100644
> index 0000000..8e38d26
> --- /dev/null
> +++ b/src/shared/ad.c
> @@ -0,0 +1,346 @@
> +/*
> + *
> + * BlueZ - Bluetooth protocol stack for Linux
> + *
> + * Copyright (C) 2015 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.
> + *
> + */
> +
> +#include "src/shared/ad.h"
> +
> +#include "src/shared/queue.h"
> +#include "src/shared/util.h"
> +
> +struct bt_ad {
> + int ref_count;
> + struct queue *service_uuids;
> + struct queue *manufacturer_data;
> + struct queue *solicit_uuids;
> + struct queue *service_data;
Did you thought about using a single queue and carrying a type in the
struct data? Perhaps it would be simpler, but in the other hand it may
consume more memory per entry but usually we don't have many entries.
> +};
> +
> +struct uuid_tagged_data {
> + bt_uuid_t uuid;
> + uint8_t *data;
> + size_t len;
> +};
> +
> +struct manufacturer_tagged_data {
> + uint16_t manufacturer_id;
> + uint8_t *data;
> + size_t len;
> +};
I would remove tagged term from these structs.
> +void bt_ad_clear_service_uuid(struct bt_ad *ad)
> +{
For public functions please add a NULL check first.
> + queue_destroy(ad->service_uuids, free);
> +
> + ad->service_uuids = queue_new();
I would prefer queue_remove_all instead of queue_destroy+queue_new to cleanup.
> +bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t manufacturer_id,
> + void *data, size_t len)
> +{
> + struct manufacturer_tagged_data *new_data;
> +
> + if (!ad)
> + return false;
> +
> + new_data = new0(struct manufacturer_tagged_data, 1);
> + if (!new_data)
> + return false;
> +
> + new_data->manufacturer_id = manufacturer_id;
> +
> + new_data->data = malloc(len);
> + if (!new_data->data) {
> + free(new_data);
> + return false;
> + }
> +
> + memcpy(new_data->data, data, len);
How about having a memdup?
--
Luiz Augusto von Dentz
Hi Michael,
On Tuesday 31 of March 2015 10:23:55 Michael Janssen wrote:
> The bt_ad structure provides an abstraction for easy translation of defined
> Advertisement Data fields into the resulting raw bytes needed by the
> Bluetooth HCI LE Set Advertising Data command.
> ---
> Makefile.am | 1 +
> src/shared/ad.c | 346
> ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/shared/ad.h |
> 60 ++++++++++
> 3 files changed, 407 insertions(+)
> create mode 100644 src/shared/ad.c
> create mode 100644 src/shared/ad.h
>
> diff --git a/Makefile.am b/Makefile.am
> index 676d929..e144428 100644
> --- a/Makefile.am
> +++ b/Makefile.am
> @@ -111,6 +111,7 @@ shared_sources = src/shared/io.h src/shared/timeout.h \
> src/shared/uhid.h src/shared/uhid.c \
> src/shared/pcap.h src/shared/pcap.c \
> src/shared/btsnoop.h src/shared/btsnoop.c \
> + src/shared/ad.h src/shared/ad.c \
> src/shared/att-types.h \
> src/shared/att.h src/shared/att.c \
> src/shared/gatt-helpers.h src/shared/gatt-helpers.c \
> diff --git a/src/shared/ad.c b/src/shared/ad.c
> new file mode 100644
> index 0000000..8e38d26
> --- /dev/null
> +++ b/src/shared/ad.c
> @@ -0,0 +1,346 @@
> +/*
> + *
> + * BlueZ - Bluetooth protocol stack for Linux
> + *
> + * Copyright (C) 2015 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.
> + *
> + */
> +
> +#include "src/shared/ad.h"
> +
> +#include "src/shared/queue.h"
> +#include "src/shared/util.h"
> +
> +struct bt_ad {
> + int ref_count;
> + struct queue *service_uuids;
> + struct queue *manufacturer_data;
> + struct queue *solicit_uuids;
> + struct queue *service_data;
> +};
> +
> +struct uuid_tagged_data {
> + bt_uuid_t uuid;
> + uint8_t *data;
> + size_t len;
> +};
> +
> +struct manufacturer_tagged_data {
> + uint16_t manufacturer_id;
> + uint8_t *data;
> + size_t len;
> +};
> +
> +struct bt_ad *bt_ad_new(void)
> +{
> + struct bt_ad *ad;
> +
> + ad = new0(struct bt_ad, 1);
> + if (!ad)
> + return NULL;
> +
> + ad->service_uuids = queue_new();
> + if (!ad->service_uuids)
> + goto fail;
> +
> + ad->manufacturer_data = queue_new();
> + if (!ad->manufacturer_data)
> + goto fail;
> +
> + ad->solicit_uuids = queue_new();
> + if (!ad->solicit_uuids)
> + goto fail;
> +
> + ad->service_data = queue_new();
> + if (!ad->service_data)
> + goto fail;
> +
> + return bt_ad_ref(ad);
> +
> +fail:
> + queue_destroy(ad->service_uuids, NULL);
> + queue_destroy(ad->manufacturer_data, NULL);
> + queue_destroy(ad->solicit_uuids, NULL);
> + queue_destroy(ad->service_data, NULL);
> +
> + free(ad);
> +
> + return NULL;
> +}
> +
> +struct bt_ad *bt_ad_ref(struct bt_ad *ad)
> +{
> + ad->ref_count++;
> + return ad;
> +}
> +
> +static void uuid_tagged_destroy(void *data)
> +{
> + struct uuid_tagged_data *tagged = data;
> +
> + free(tagged->data);
> + free(tagged);
> +}
> +
> +static bool uuid_tagged_match(const void *data, const void *elem)
> +{
> + const struct uuid_tagged_data *tagged = elem;
> + const bt_uuid_t *uuid = data;
> +
> + return !bt_uuid_cmp(&tagged->uuid, uuid);
> +}
> +
> +static void manuf_tagged_destroy(void *data)
> +{
> + struct manufacturer_tagged_data *tagged = data;
> +
> + free(tagged->data);
> + free(tagged);
> +}
> +
> +static bool manuf_tagged_match(const void *data, const void *elem)
> +{
> + const struct manufacturer_tagged_data *tagged = elem;
> + uint16_t manuf_id = PTR_TO_UINT(elem);
> +
> + return tagged->manufacturer_id == manuf_id;
> +}
> +
> +void bt_ad_unref(struct bt_ad *ad)
> +{
> + if (!ad)
> + return;
> +
> + if (__sync_sub_and_fetch(&ad->ref_count, 1))
> + return;
> +
> + queue_destroy(ad->service_uuids, free);
> +
> + queue_destroy(ad->manufacturer_data, manuf_tagged_destroy);
> +
> + queue_destroy(ad->solicit_uuids, free);
> +
> + queue_destroy(ad->service_data, uuid_tagged_destroy);
> +
> + free(ad);
> +}
> +
> +uint8_t *bt_ad_generate(struct bt_ad *ad, uint8_t *length)
> +{
> + /* TODO: implement */
> + return NULL;
> +}
> +
> +static bool queue_add_uuid(struct queue *queue, const bt_uuid_t *uuid)
> +{
> + bt_uuid_t *new_uuid;
> +
> + if (!queue)
> + return false;
> +
> + new_uuid = new0(bt_uuid_t, 1);
> + if (!new_uuid)
> + return false;
> +
> + bt_uuid_to_uuid128(uuid, new_uuid);
> +
> + queue_push_tail(queue, new_uuid);
> +
> + return true;
> +}
> +
> +static bool uuid_match(const void *data, const void *elem)
> +{
> + const bt_uuid_t *match_uuid = data;
> + const bt_uuid_t *uuid = elem;
> +
> + return bt_uuid_cmp(match_uuid, uuid);
> +}
> +
> +static bool queue_remove_uuid(struct queue *queue, bt_uuid_t *uuid)
> +{
> + bt_uuid_t *removed;
> +
> + if (!queue || !uuid)
> + return false;
> +
> + removed = queue_remove_if(queue, uuid_match, uuid);
> +
> + if (removed) {
> + free(removed);
> + return true;
> + }
> +
> + return false;
> +}
> +
> +bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
> +{
> + if (!ad)
> + return false;
> +
> + return queue_add_uuid(ad->service_uuids, uuid);
> +}
> +
> +bool bt_ad_remove_service_uuid(struct bt_ad *ad, bt_uuid_t *uuid)
> +{
> + if (!ad)
> + return false;
> +
> + return queue_remove_uuid(ad->service_uuids, uuid);
> +}
> +
> +void bt_ad_clear_service_uuid(struct bt_ad *ad)
> +{
> + queue_destroy(ad->service_uuids, free);
> +
> + ad->service_uuids = queue_new();
> +}
> +
> +bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t
> manufacturer_id, + void *data, size_t len)
> +{
> + struct manufacturer_tagged_data *new_data;
> +
> + if (!ad)
> + return false;
> +
> + new_data = new0(struct manufacturer_tagged_data, 1);
> + if (!new_data)
> + return false;
> +
> + new_data->manufacturer_id = manufacturer_id;
> +
> + new_data->data = malloc(len);
> + if (!new_data->data) {
> + free(new_data);
> + return false;
> + }
> +
> + memcpy(new_data->data, data, len);
> +
> + new_data->len = len;
> +
> + if (queue_push_tail(ad->manufacturer_data, new_data))
> + return true;
> +
> + manuf_tagged_destroy(new_data);
> +
> + return false;
> +}
> +
> +bool bt_ad_remove_manufacturer_data(struct bt_ad *ad, uint16_t
> manufacturer_id) +{
> + struct manufacturer_tagged_data *data;
> +
> + if (!ad)
> + return false;
> +
> + data = queue_remove_if(ad->manufacturer_data, manuf_tagged_match,
> + UINT_TO_PTR(manufacturer_id));
> +
> + if (!data)
> + return false;
> +
> + manuf_tagged_destroy(data);
> +
> + return true;
> +}
> +
> +void bt_ad_clear_manufacturer_data(struct bt_ad *ad)
> +{
> + queue_destroy(ad->manufacturer_data, manuf_tagged_destroy);
> +
> + ad->manufacturer_data = queue_new();
> +}
> +
> +bool bt_ad_add_solicit_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
> +{
> + if (!ad)
> + return false;
> +
> + return queue_add_uuid(ad->solicit_uuids, uuid);
> +}
> +
> +bool bt_ad_remove_solicit_uuid(struct bt_ad *ad,
> + bt_uuid_t *uuid)
> +{
> + if (!ad)
> + return false;
> +
> + return queue_remove_uuid(ad->solicit_uuids, uuid);
> +}
> +
> +void bt_ad_clear_solicit_uuid(struct bt_ad *ad)
> +{
> + queue_destroy(ad->solicit_uuids, free);
> +
> + ad->solicit_uuids = queue_new();
> +}
> +
> +bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void
> *data, + size_t len)
> +{
> + struct uuid_tagged_data *new_data;
> +
> + if (!ad)
> + return false;
> +
> + new_data = new0(struct uuid_tagged_data, 1);
> + if (!new_data)
> + return false;
> +
> + bt_uuid_to_uuid128(uuid, &new_data->uuid);
> +
> + new_data->data = malloc(len);
> + if (!new_data) {
Should be if (!new_data->data)
> + free(new_data);
> + return false;
> + }
> +
> + memcpy(new_data->data, data, len);
> +
> + new_data->len = len;
> +
> + if (queue_push_tail(ad->service_data, new_data))
> + return true;
> +
> + uuid_tagged_destroy(new_data);
> +
> + return false;
> +}
> +
> +bool bt_ad_remove_service_data(struct bt_ad *ad, bt_uuid_t *uuid)
> +{
> + struct uuid_tagged_data *data;
> +
> + if (!ad)
> + return false;
> +
> + data = queue_remove_if(ad->service_data, uuid_tagged_match, uuid);
> +
> + if (!data)
> + return false;
> +
> + uuid_tagged_destroy(data);
> +
> + return true;
> +}
> +
> +void bt_ad_clear_service_data(struct bt_ad *ad)
> +{
> + queue_destroy(ad->service_data, uuid_tagged_destroy);
> +
> + ad->service_data = queue_new();
> +}
> diff --git a/src/shared/ad.h b/src/shared/ad.h
> new file mode 100644
> index 0000000..0e41da0
> --- /dev/null
> +++ b/src/shared/ad.h
> @@ -0,0 +1,60 @@
> +/*
> + *
> + * BlueZ - Bluetooth protocol stack for Linux
> + *
> + * Copyright (C) 2015 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.
> + *
> + */
> +
> +#include <inttypes.h>
> +#include <stdbool.h>
> +
> +#include "lib/bluetooth.h"
> +#include "lib/uuid.h"
> +
> +struct bt_ad;
> +
> +struct bt_ad *bt_ad_new(void);
> +
> +struct bt_ad *bt_ad_ref(struct bt_ad *ad);
> +
> +void bt_ad_unref(struct bt_ad *ad);
> +
> +uint8_t *bt_ad_generate(struct bt_ad *ad, uint8_t *length);
> +
> +bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid);
> +
> +bool bt_ad_remove_service_uuid(struct bt_ad *ad, bt_uuid_t *uuid);
> +
> +void bt_ad_clear_service_uuid(struct bt_ad *ad);
> +
> +bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t
> manufacturer_data, + void *data, size_t len);
> +
> +bool bt_ad_remove_manufacturer_data(struct bt_ad *ad, uint16_t
> manufacturer_id); +
> +void bt_ad_clear_manufacturer_data(struct bt_ad *ad);
> +
> +bool bt_ad_add_solicit_uuid(struct bt_ad *ad, const bt_uuid_t *uuid);
> +
> +bool bt_ad_remove_solicit_uuid(struct bt_ad *ad, bt_uuid_t *uuid);
> +
> +void bt_ad_clear_solicit_uuid(struct bt_ad *ad);
> +
> +bool bt_ad_add_service_data(struct bt_ad *ad, const bt_uuid_t *uuid, void
> *data, + size_t len);
> +
> +bool bt_ad_remove_service_data(struct bt_ad *ad, bt_uuid_t *uuid);
> +
> +void bt_ad_clear_service_data(struct bt_ad *ad);
And more general note: most of the queue_* (ie. push and new) functions can
fail so that should probably be checked and it seems to be missing in multiple
places.
Eventually we might go with just aborting if small allocation fails but for
now those should be checked against.
--
BR
Szymon Janc