[PATCH v2 1/9] shared/ad: Add support for arbritary type

[PATCH v2 1/9] shared/ad: Add support for arbritary type

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds APIs to include data of arbritary type as long as it is not
of a type which blacklisted due to being handled already or it is
considered to safe to be set by an application.
---
 src/shared/ad.c | 231 ++++++++++++++++++++++++++++++++++++++++++++++++++++----
 src/shared/ad.h |  60 +++++++++++++++
 2 files changed, 278 insertions(+), 13 deletions(-)

diff --git a/src/shared/ad.c b/src/shared/ad.c
index 255794dc4..f0e62cef1 100644
--- a/src/shared/ad.c
+++ b/src/shared/ad.c
@@ -37,6 +37,7 @@ struct bt_ad {
 	struct queue *manufacturer_data;
 	struct queue *solicit_uuids;
 	struct queue *service_data;
+	struct queue *data;
 };
 
 struct bt_ad *bt_ad_new(void)
@@ -48,6 +49,7 @@ struct bt_ad *bt_ad_new(void)
 	ad->manufacturer_data = queue_new();
 	ad->solicit_uuids = queue_new();
 	ad->service_data = queue_new();
+	ad->data = queue_new();
 	ad->appearance = UINT16_MAX;
 
 	return bt_ad_ref(ad);
@@ -94,6 +96,14 @@ static bool manuf_match(const void *data, const void *elem)
 	return manuf->manufacturer_id == manuf_id;
 }
 
+static void data_destroy(void *data)
+{
+	struct bt_ad_data *ad = data;
+
+	free(ad->data);
+	free(ad);
+}
+
 void bt_ad_unref(struct bt_ad *ad)
 {
 	if (!ad)
@@ -110,6 +120,8 @@ void bt_ad_unref(struct bt_ad *ad)
 
 	queue_destroy(ad->service_data, uuid_destroy);
 
+	queue_destroy(ad->data, data_destroy);
+
 	free(ad->name);
 
 	free(ad);
@@ -203,6 +215,24 @@ static size_t name_length(const char *name, size_t *pos)
 	return len;
 }
 
+static size_t data_length(struct queue *queue)
+{
+	size_t length = 0;
+	const struct queue_entry *entry;
+
+	entry = queue_get_entries(queue);
+
+	while (entry) {
+		struct bt_ad_data *data = entry->data;
+
+		length += 1 + 1 + data->len;
+
+		entry = entry->next;
+	}
+
+	return length;
+}
+
 static size_t calculate_length(struct bt_ad *ad)
 {
 	size_t length = 0;
@@ -219,6 +249,8 @@ static size_t calculate_length(struct bt_ad *ad)
 
 	length += ad->appearance != UINT16_MAX ? 4 : 0;
 
+	length += data_length(ad->data);
+
 	return length;
 }
 
@@ -258,21 +290,21 @@ static void serialize_uuids(struct queue *uuids, uint8_t uuid_type,
 static void serialize_service_uuids(struct queue *uuids, uint8_t *buf,
 								uint8_t *pos)
 {
-	serialize_uuids(uuids, BT_UUID16, EIR_UUID16_ALL, buf, pos);
+	serialize_uuids(uuids, BT_UUID16, BT_AD_UUID16_ALL, buf, pos);
 
-	serialize_uuids(uuids, BT_UUID32, EIR_UUID32_ALL, buf, pos);
+	serialize_uuids(uuids, BT_UUID32, BT_AD_UUID32_ALL, buf, pos);
 
-	serialize_uuids(uuids, BT_UUID128, EIR_UUID128_ALL, buf, pos);
+	serialize_uuids(uuids, BT_UUID128, BT_AD_UUID128_ALL, buf, pos);
 }
 
 static void serialize_solicit_uuids(struct queue *uuids, uint8_t *buf,
 								uint8_t *pos)
 {
-	serialize_uuids(uuids, BT_UUID16, EIR_SOLICIT16, buf, pos);
+	serialize_uuids(uuids, BT_UUID16, BT_AD_SOLICIT16, buf, pos);
 
-	serialize_uuids(uuids, BT_UUID32, EIR_SOLICIT32, buf, pos);
+	serialize_uuids(uuids, BT_UUID32, BT_AD_SOLICIT32, buf, pos);
 
-	serialize_uuids(uuids, BT_UUID128, EIR_SOLICIT128, buf, pos);
+	serialize_uuids(uuids, BT_UUID128, BT_AD_SOLICIT128, buf, pos);
 }
 
 static void serialize_manuf_data(struct queue *manuf_data, uint8_t *buf,
@@ -285,7 +317,7 @@ static void serialize_manuf_data(struct queue *manuf_data, uint8_t *buf,
 
 		buf[(*pos)++] = data->len + 2 + 1;
 
-		buf[(*pos)++] = EIR_MANUFACTURER_DATA;
+		buf[(*pos)++] = BT_AD_MANUFACTURER_DATA;
 
 		bt_put_le16(data->manufacturer_id, buf + (*pos));
 
@@ -312,13 +344,13 @@ static void serialize_service_data(struct queue *service_data, uint8_t *buf,
 
 		switch (uuid_len) {
 		case 2:
-			buf[(*pos)++] = EIR_SVC_DATA16;
+			buf[(*pos)++] = BT_AD_SERVICE_DATA16;
 			break;
 		case 4:
-			buf[(*pos)++] = EIR_SVC_DATA32;
+			buf[(*pos)++] = BT_AD_SERVICE_DATA32;
 			break;
 		case 16:
-			buf[(*pos)++] = EIR_SVC_DATA128;
+			buf[(*pos)++] = BT_AD_SERVICE_DATA128;
 			break;
 		}
 
@@ -340,14 +372,14 @@ static void serialize_service_data(struct queue *service_data, uint8_t *buf,
 static void serialize_name(const char *name, uint8_t *buf, uint8_t *pos)
 {
 	int len;
-	uint8_t type = EIR_NAME_COMPLETE;
+	uint8_t type = BT_AD_NAME_COMPLETE;
 
 	if (!name)
 		return;
 
 	len = strlen(name);
 	if (len > MAX_ADV_DATA_LEN - (*pos + 2)) {
-		type = EIR_NAME_SHORT;
+		type = BT_AD_NAME_SHORT;
 		len = MAX_ADV_DATA_LEN - (*pos + 2);
 	}
 
@@ -364,12 +396,30 @@ static void serialize_appearance(uint16_t value, uint8_t *buf, uint8_t *pos)
 		return;
 
 	buf[(*pos)++] = sizeof(value) + 1;
-	buf[(*pos)++] = EIR_GAP_APPEARANCE;
+	buf[(*pos)++] = BT_AD_GAP_APPEARANCE;
 
 	bt_put_le16(value, buf + (*pos));
 	*pos += 2;
 }
 
+static void serialize_data(struct queue *queue, uint8_t *buf, uint8_t *pos)
+{
+	const struct queue_entry *entry = queue_get_entries(queue);
+
+	while (entry) {
+		struct bt_ad_data *data = entry->data;
+
+		buf[(*pos)++] = data->len + 1;
+		buf[(*pos)++] = data->type;
+
+		memcpy(buf + *pos, data->data, data->len);
+
+		*pos += data->len;
+
+		entry = entry->next;
+	}
+}
+
 uint8_t *bt_ad_generate(struct bt_ad *ad, size_t *length)
 {
 	uint8_t *adv_data;
@@ -399,6 +449,8 @@ uint8_t *bt_ad_generate(struct bt_ad *ad, size_t *length)
 
 	serialize_appearance(ad->appearance, adv_data, &pos);
 
+	serialize_data(ad->data, adv_data, &pos);
+
 	return adv_data;
 }
 
@@ -756,3 +808,156 @@ void bt_ad_clear_appearance(struct bt_ad *ad)
 
 	ad->appearance = UINT16_MAX;
 }
+
+static bool data_type_match(const void *data, const void *user_data)
+{
+	const struct bt_ad_data *a = data;
+	const uint8_t type = PTR_TO_UINT(user_data);
+
+	return a->type == type;
+}
+
+static uint8_t type_blacklist[] = {
+	BT_AD_FLAGS,
+	BT_AD_UUID16_SOME,
+	BT_AD_UUID16_ALL,
+	BT_AD_UUID32_SOME,
+	BT_AD_UUID32_ALL,
+	BT_AD_UUID128_SOME,
+	BT_AD_UUID128_ALL,
+	BT_AD_NAME_SHORT,
+	BT_AD_NAME_COMPLETE,
+	BT_AD_TX_POWER,
+	BT_AD_CLASS_OF_DEV,
+	BT_AD_SSP_HASH,
+	BT_AD_SSP_RANDOMIZER,
+	BT_AD_DEVICE_ID,
+	BT_AD_SMP_TK,
+	BT_AD_SMP_OOB_FLAGS,
+	BT_AD_SLAVE_CONN_INTERVAL,
+	BT_AD_SOLICIT16,
+	BT_AD_SOLICIT128,
+	BT_AD_SERVICE_DATA16,
+	BT_AD_PUBLIC_ADDRESS,
+	BT_AD_RANDOM_ADDRESS,
+	BT_AD_GAP_APPEARANCE,
+	BT_AD_ADVERTISING_INTERVAL,
+	BT_AD_LE_DEVICE_ADDRESS,
+	BT_AD_LE_ROLE,
+	BT_AD_SSP_HASH_P256,
+	BT_AD_SSP_RANDOMIZER_P256,
+	BT_AD_SOLICIT32,
+	BT_AD_SERVICE_DATA32,
+	BT_AD_SERVICE_DATA128,
+	BT_AD_LE_SC_CONFIRM_VALUE,
+	BT_AD_LE_SC_RANDOM_VALUE,
+	BT_AD_LE_SUPPORTED_FEATURES,
+	BT_AD_CHANNEL_MAP_UPDATE_IND,
+	BT_AD_MESH_PROV,
+	BT_AD_MESH_DATA,
+	BT_AD_MESH_BEACON,
+	BT_AD_3D_INFO_DATA,
+	BT_AD_MANUFACTURER_DATA,
+};
+
+bool bt_ad_add_data(struct bt_ad *ad, uint8_t type, void *data, size_t len)
+{
+	struct bt_ad_data *new_data;
+	size_t i;
+
+	if (!ad)
+		return false;
+
+	if (len > (MAX_ADV_DATA_LEN - 2))
+		return false;
+
+	new_data = queue_find(ad->data, data_type_match, UINT_TO_PTR(type));
+	if (new_data) {
+		if (new_data->len == len && !memcmp(new_data->data, data, len))
+			return false;
+		new_data->data = realloc(new_data->data, len);
+		memcpy(new_data->data, data, len);
+		new_data->len = len;
+		return true;
+	}
+
+	for (i = 0; i < sizeof(type_blacklist); i++) {
+		if (type == type_blacklist[i])
+			return false;
+	}
+
+	new_data = new0(struct bt_ad_data, 1);
+	new_data->type = type;
+	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->data, new_data))
+		return true;
+
+	data_destroy(new_data);
+
+	return false;
+}
+
+static bool data_match(const void *data, const void *user_data)
+{
+	const struct bt_ad_data *d1 = data;
+	const struct bt_ad_data *d2 = user_data;
+
+	if (d1->type != d2->type)
+		return false;
+
+	if (d1->len != d2->len)
+		return false;
+
+	return !memcmp(d1->data, d2->data, d1->len);
+}
+
+bool bt_ad_has_data(struct bt_ad *ad, const struct bt_ad_data *data)
+{
+	if (!ad)
+		return false;
+
+	if (!data)
+		return !queue_isempty(ad->data);
+
+	return queue_find(ad->data, data_match, data);
+}
+
+void bt_ad_foreach_data(struct bt_ad *ad, bt_ad_func_t func, void *user_data)
+{
+	if (!ad)
+		return;
+
+	queue_foreach(ad->data, func, user_data);
+}
+
+bool bt_ad_remove_data(struct bt_ad *ad, uint8_t type)
+{
+	struct bt_ad_data *data;
+
+	if (!ad)
+		return false;
+
+	data = queue_remove_if(ad->data, data_type_match, UINT_TO_PTR(type));
+	if (!data)
+		return false;
+
+	data_destroy(data);
+
+	return true;
+}
+
+void bt_ad_clear_data(struct bt_ad *ad)
+{
+	if (!ad)
+		return;
+
+	queue_remove_all(ad->data, NULL, NULL, data_destroy);
+}
diff --git a/src/shared/ad.h b/src/shared/ad.h
index f0e3b8127..8d705323b 100644
--- a/src/shared/ad.h
+++ b/src/shared/ad.h
@@ -27,6 +27,50 @@
 #include "lib/bluetooth.h"
 #include "lib/uuid.h"
 
+#define BT_AD_FLAGS			0x01
+#define BT_AD_UUID16_SOME		0x02
+#define BT_AD_UUID16_ALL		0x03
+#define BT_AD_UUID32_SOME		0x04
+#define BT_AD_UUID32_ALL		0x05
+#define BT_AD_UUID128_SOME		0x06
+#define BT_AD_UUID128_ALL		0x07
+#define BT_AD_NAME_SHORT		0x08
+#define BT_AD_NAME_COMPLETE		0x09
+#define BT_AD_TX_POWER			0x0a
+#define BT_AD_CLASS_OF_DEV		0x0d
+#define BT_AD_SSP_HASH			0x0e
+#define BT_AD_SSP_RANDOMIZER		0x0f
+#define BT_AD_DEVICE_ID			0x10
+#define BT_AD_SMP_TK			0x10
+#define BT_AD_SMP_OOB_FLAGS		0x11
+#define BT_AD_SLAVE_CONN_INTERVAL	0x12
+#define BT_AD_SOLICIT16			0x14
+#define BT_AD_SOLICIT128		0x15
+#define BT_AD_SERVICE_DATA16		0x16
+#define BT_AD_PUBLIC_ADDRESS		0x17
+#define BT_AD_RANDOM_ADDRESS		0x18
+#define BT_AD_GAP_APPEARANCE		0x19
+#define BT_AD_ADVERTISING_INTERVAL	0x1a
+#define BT_AD_LE_DEVICE_ADDRESS		0x1b
+#define BT_AD_LE_ROLE			0x1c
+#define BT_AD_SSP_HASH_P256		0x1d
+#define BT_AD_SSP_RANDOMIZER_P256	0x1e
+#define BT_AD_SOLICIT32			0x1f
+#define BT_AD_SERVICE_DATA32		0x20
+#define BT_AD_SERVICE_DATA128		0x21
+#define BT_AD_LE_SC_CONFIRM_VALUE	0x22
+#define BT_AD_LE_SC_RANDOM_VALUE	0x23
+#define BT_AD_URI			0x24
+#define BT_AD_INDOOR_POSITIONING	0x25
+#define BT_AD_TRANSPORT_DISCOVERY	0x26
+#define BT_AD_LE_SUPPORTED_FEATURES	0x27
+#define BT_AD_CHANNEL_MAP_UPDATE_IND	0x28
+#define BT_AD_MESH_PROV			0x29
+#define BT_AD_MESH_DATA			0x2a
+#define BT_AD_MESH_BEACON		0x2b
+#define BT_AD_3D_INFO_DATA		0x3d
+#define BT_AD_MANUFACTURER_DATA		0xff
+
 typedef void (*bt_ad_func_t)(void *data, void *user_data);
 
 struct bt_ad;
@@ -39,6 +83,12 @@ struct bt_ad_manufacturer_data {
 
 struct bt_ad_service_data {
 	bt_uuid_t uuid;
+	size_t len;
+	void *data;
+};
+
+struct bt_ad_data {
+	uint8_t type;
 	uint8_t *data;
 	size_t len;
 };
@@ -96,3 +146,13 @@ void bt_ad_clear_name(struct bt_ad *ad);
 bool bt_ad_add_appearance(struct bt_ad *ad, uint16_t appearance);
 
 void bt_ad_clear_appearance(struct bt_ad *ad);
+
+bool bt_ad_add_data(struct bt_ad *ad, uint8_t type, void *data, size_t len);
+
+bool bt_ad_has_data(struct bt_ad *ad, const struct bt_ad_data *data);
+
+void bt_ad_foreach_data(struct bt_ad *ad, bt_ad_func_t func, void *user_data);
+
+bool bt_ad_remove_data(struct bt_ad *ad, uint8_t type);
+
+void bt_ad_clear_data(struct bt_ad *ad);
-- 
2.14.3

[PATCH v2 2/9] doc/device-api: Add AdvertisingData property

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds AdvertisingData which exposes data being advertised which is
may be useful for profiles not handled by bluetoothd.
---
 doc/device-api.txt | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/doc/device-api.txt b/doc/device-api.txt
index 1b448eef1..65d8fee37 100644
--- a/doc/device-api.txt
+++ b/doc/device-api.txt
@@ -251,3 +251,19 @@ Properties	string Address [readonly]
 		array{byte} AdvertisingFlags [readonly, experimental]
 
 			The Advertising Data Flags of the remote device.
+
+		dict AdvertisingData [readonly, experimental]
+
+			The Advertising Data of the remote device. Keys are
+			are 8 bits AD Type followed by data as byte array.
+
+			Note: Only types considered safe to be handled by
+			application are exposed.
+
+			Possible values:
+				<type> <byte array>
+				...
+
+			Example:
+				<Transport Discovery> <Organization Flags...>
+				0x26                   0x01         0x01...
-- 
2.14.3

[PATCH v2 3/9] device: Add implementation of AdvertisingData

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds the implementation of AdvertisingData property:

[CHG] Device 00:1B:DC:07:31:88 AdvertisingData Key: 0x26
[CHG] Device 00:1B:DC:07:31:88 AdvertisingData Value:
  01 01 00                                         ...
---
 src/adapter.c |  3 +++
 src/device.c  | 67 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++-
 src/device.h  |  2 ++
 src/eir.c     | 27 ++++++++++++++++++++++++
 src/eir.h     |  8 +++++++
 5 files changed, 106 insertions(+), 1 deletion(-)

diff --git a/src/adapter.c b/src/adapter.c
index 932b2a34d..c1c792aa3 100644
--- a/src/adapter.c
+++ b/src/adapter.c
@@ -6092,6 +6092,9 @@ static void update_found_devices(struct btd_adapter *adapter,
 	if (eir_data.sd_list)
 		device_set_service_data(dev, eir_data.sd_list, duplicate);
 
+	if (eir_data.data_list)
+		device_set_data(dev, eir_data.data_list, duplicate);
+
 	if (bdaddr_type != BDADDR_BREDR)
 		device_set_flags(dev, eir_data.flags);
 
diff --git a/src/device.c b/src/device.c
index f693b7023..38e23211f 100644
--- a/src/device.c
+++ b/src/device.c
@@ -1276,6 +1276,46 @@ dev_property_service_data_exist(const GDBusPropertyTable *property,
 	return bt_ad_has_service_data(device->ad, NULL);
 }
 
+static void append_advertising_data(void *data, void *user_data)
+{
+	struct bt_ad_data *ad = data;
+	DBusMessageIter *dict = user_data;
+
+	g_dbus_dict_append_basic_array(dict,
+				DBUS_TYPE_BYTE, &ad->type,
+				DBUS_TYPE_BYTE, &ad->data, ad->len);
+}
+
+static gboolean
+dev_property_get_advertising_data(const GDBusPropertyTable *property,
+					DBusMessageIter *iter, void *data)
+{
+	struct btd_device *device = data;
+	DBusMessageIter dict;
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,
+					DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
+					DBUS_TYPE_BYTE_AS_STRING
+					DBUS_TYPE_VARIANT_AS_STRING
+					DBUS_DICT_ENTRY_END_CHAR_AS_STRING,
+					&dict);
+
+	bt_ad_foreach_data(device->ad, append_advertising_data, &dict);
+
+	dbus_message_iter_close_container(iter, &dict);
+
+	return TRUE;
+}
+
+static gboolean
+dev_property_advertising_data_exist(const GDBusPropertyTable *property,
+								void *data)
+{
+	struct btd_device *device = data;
+
+	return bt_ad_has_data(device->ad, NULL);
+}
+
 static gboolean disconnect_all(gpointer user_data)
 {
 	struct btd_device *device = user_data;
@@ -1681,6 +1721,29 @@ void device_set_service_data(struct btd_device *dev, GSList *list,
 	g_slist_foreach(list, add_service_data, dev);
 }
 
+static void add_data(void *data, void *user_data)
+{
+	struct eir_ad *ad = data;
+	struct btd_device *dev = user_data;
+
+	if (!bt_ad_add_data(dev->ad, ad->type, ad->data, ad->len))
+		return;
+
+	if (ad->type == EIR_TRANSPORT_DISCOVERY)
+		g_dbus_emit_property_changed(dbus_conn, dev->path,
+						DEVICE_INTERFACE,
+						"AdvertisingData");
+}
+
+void device_set_data(struct btd_device *dev, GSList *list,
+							bool duplicate)
+{
+	if (duplicate)
+		bt_ad_clear_data(dev->ad);
+
+	g_slist_foreach(list, add_data, dev);
+}
+
 static struct btd_service *find_connectable_service(struct btd_device *dev,
 							const char *uuid)
 {
@@ -2673,7 +2736,9 @@ static const GDBusPropertyTable device_properties[] = {
 	{ "AdvertisingFlags", "ay", dev_property_get_flags, NULL,
 					dev_property_flags_exist,
 					G_DBUS_PROPERTY_FLAG_EXPERIMENTAL},
-
+	{ "AdvertisingData", "a{yv}", dev_property_get_advertising_data,
+				NULL, dev_property_advertising_data_exist,
+				G_DBUS_PROPERTY_FLAG_EXPERIMENTAL },
 	{ }
 };
 
diff --git a/src/device.h b/src/device.h
index b90f9273a..06b100499 100644
--- a/src/device.h
+++ b/src/device.h
@@ -83,6 +83,8 @@ void device_set_manufacturer_data(struct btd_device *dev, GSList *list,
 							bool duplicate);
 void device_set_service_data(struct btd_device *dev, GSList *list,
 							bool duplicate);
+void device_set_data(struct btd_device *dev, GSList *list,
+							bool duplicate);
 void device_probe_profile(gpointer a, gpointer b);
 void device_remove_profile(gpointer a, gpointer b);
 struct btd_adapter *device_get_adapter(struct btd_device *device);
diff --git a/src/eir.c b/src/eir.c
index c984fa5a7..25d961dad 100644
--- a/src/eir.c
+++ b/src/eir.c
@@ -51,6 +51,14 @@ static void sd_free(void *data)
 	g_free(sd);
 }
 
+static void data_free(void *data)
+{
+	struct eir_ad *ad = data;
+
+	g_free(ad->data);
+	g_free(ad);
+}
+
 void eir_data_free(struct eir_data *eir)
 {
 	g_slist_free_full(eir->services, free);
@@ -65,6 +73,8 @@ void eir_data_free(struct eir_data *eir)
 	eir->msd_list = NULL;
 	g_slist_free_full(eir->sd_list, sd_free);
 	eir->sd_list = NULL;
+	g_slist_free_full(eir->data_list, data_free);
+	eir->data_list = NULL;
 }
 
 static void eir_parse_uuid16(struct eir_data *eir, const void *data,
@@ -226,6 +236,20 @@ static void eir_parse_uuid128_data(struct eir_data *eir, const uint8_t *data,
 	eir_parse_sd(eir, &service, data + 16, len - 16);
 }
 
+static void eir_parse_data(struct eir_data *eir, uint8_t type,
+					const uint8_t *data, uint8_t len)
+{
+	struct eir_ad *ad;
+
+	ad = g_malloc(sizeof(*ad));
+	ad->type = type;
+	ad->len = len;
+	ad->data = g_malloc(len);
+	memcpy(ad->data, data, len);
+
+	eir->data_list = g_slist_append(eir->data_list, ad);
+}
+
 void eir_parse(struct eir_data *eir, const uint8_t *eir_data, uint8_t eir_len)
 {
 	uint16_t len = 0;
@@ -346,6 +370,9 @@ void eir_parse(struct eir_data *eir, const uint8_t *eir_data, uint8_t eir_len)
 			eir_parse_msd(eir, data, data_len);
 			break;
 
+		default:
+			eir_parse_data(eir, eir_data[1], data, data_len);
+			break;
 		}
 
 		eir_data += field_len + 1;
diff --git a/src/eir.h b/src/eir.h
index 219ee794b..c868177a6 100644
--- a/src/eir.h
+++ b/src/eir.h
@@ -49,6 +49,7 @@
 #define EIR_SOLICIT32               0x1F  /* LE: Solicit UUIDs, 32-bit */
 #define EIR_SVC_DATA32              0x20  /* LE: Service data, 32-bit UUID */
 #define EIR_SVC_DATA128             0x21  /* LE: Service data, 128-bit UUID */
+#define EIR_TRANSPORT_DISCOVERY     0x26  /* Transport Discovery Service */
 #define EIR_MANUFACTURER_DATA       0xFF  /* Manufacturer Specific Data */
 
 /* Flags Descriptions */
@@ -75,6 +76,12 @@ struct eir_sd {
 	uint8_t data_len;
 };
 
+struct eir_ad {
+	uint8_t type;
+	uint8_t len;
+	void *data;
+};
+
 struct eir_data {
 	GSList *services;
 	unsigned int flags;
@@ -92,6 +99,7 @@ struct eir_data {
 	uint16_t did_source;
 	GSList *msd_list;
 	GSList *sd_list;
+	GSList *data_list;
 };
 
 void eir_data_free(struct eir_data *eir);
-- 
2.14.3

[PATCH v2 4/9] doc/advertising-api: Add Data property

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

Data property can be used to add advertising data with arbitrary/profile
specific type.
---
 doc/advertising-api.txt | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/doc/advertising-api.txt b/doc/advertising-api.txt
index 15e64d38a..eef98ad91 100644
--- a/doc/advertising-api.txt
+++ b/doc/advertising-api.txt
@@ -61,6 +61,23 @@ Properties	string Type
 			Service Data elements to include. The keys are the
 			UUID to associate with the data.
 
+		dict Data [Experimental]
+
+			Advertising Type to include in the Advertising
+			Data. Key is the advertising type and value is the
+			data as byte array.
+
+			Note: Types already handled by other properties shall
+			not be used.
+
+			Possible values:
+				<type> <byte array>
+				...
+
+			Example:
+				<Transport Discovery> <Organization Flags...>
+				0x26                   0x01         0x01...
+
 		array{string} Includes
 
 			List of features to be included in the advertising
-- 
2.14.3

[PATCH v2 5/9] advertising: Add Data implementation

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds the support for Data property.
---
 src/advertising.c | 59 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/src/advertising.c b/src/advertising.c
index 0cb6eac3a..441d0a235 100644
--- a/src/advertising.c
+++ b/src/advertising.c
@@ -583,6 +583,64 @@ static bool parse_timeout(DBusMessageIter *iter,
 	return true;
 }
 
+static bool parse_data(DBusMessageIter *iter, struct btd_adv_client *client)
+{
+	DBusMessageIter entries;
+
+	if (!iter) {
+		bt_ad_clear_data(client->data);
+		return true;
+	}
+
+	if (dbus_message_iter_get_arg_type(iter) != DBUS_TYPE_ARRAY)
+		return false;
+
+	dbus_message_iter_recurse(iter, &entries);
+
+	bt_ad_clear_data(client->data);
+
+	while (dbus_message_iter_get_arg_type(&entries)
+						== DBUS_TYPE_DICT_ENTRY) {
+		DBusMessageIter value, entry, array;
+		uint8_t type;
+		uint8_t *data;
+		int len;
+
+		dbus_message_iter_recurse(&entries, &entry);
+		dbus_message_iter_get_basic(&entry, &type);
+
+		dbus_message_iter_next(&entry);
+
+		if (dbus_message_iter_get_arg_type(&entry) != DBUS_TYPE_VARIANT)
+			goto fail;
+
+		dbus_message_iter_recurse(&entry, &value);
+
+		if (dbus_message_iter_get_arg_type(&value) != DBUS_TYPE_ARRAY)
+			goto fail;
+
+		dbus_message_iter_recurse(&value, &array);
+
+		if (dbus_message_iter_get_arg_type(&array) != DBUS_TYPE_BYTE)
+			goto fail;
+
+		dbus_message_iter_get_fixed_array(&array, &data, &len);
+
+		DBG("Adding Data for type 0x%02x len %u", type, len);
+
+		if (!bt_ad_add_data(client->data, type, data, len))
+			goto fail;
+
+		dbus_message_iter_next(&entries);
+	}
+
+	return true;
+
+fail:
+	bt_ad_clear_data(client->data);
+	return false;
+}
+
 static struct adv_parser {
 	const char *name;
 	bool (*func)(DBusMessageIter *iter, struct btd_adv_client *client);
@@ -597,6 +655,7 @@ static struct adv_parser {
 	{ "Appearance", parse_appearance },
 	{ "Duration", parse_duration },
 	{ "Timeout", parse_timeout },
+	{ "Data", parse_data },
 	{ },
 };
 
-- 
2.14.3

[PATCH v2 6/9] monitor: Add support for Transport Discovery AD

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds support for decoding Transport Discovery Data:

@ MGMT Command: Add Advertising (0x003e) plen 16
        Instance: 1
        Flags: 0x00000001
          Switch into Connectable mode
        Duration: 0
        Timeout: 0
        Advertising data length: 5
        Transport Discovery Data
          Organization: Bluetooth SIG (0x01)
          Flags: 0x01
            Role: 0x01
              Seeker Only
            Transport Data Incomplete: False (0x00)
            Transport State: 0x00
              Off
          Length: 0
          Data:
        Scan response length: 0
---
 monitor/packet.c | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 55 insertions(+)

diff --git a/monitor/packet.c b/monitor/packet.c
index b800a2d75..c00e9efa4 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -3545,6 +3545,57 @@ static void print_mesh_data(const uint8_t *data, uint8_t len)
 	packet_hexdump(data + 1, len - 1);
 }
 
+static void print_transport_data(const uint8_t *data, uint8_t len)
+{
+	print_field("Transport Discovery Data");
+
+	if (len < 3)
+		return;
+
+	print_field("  Organization: %s (0x%02x)",
+			data[0] == 0x01 ? "Bluetooth SIG" : "RFU", data[0]);
+	print_field("  Flags: 0x%2.2x", data[1]);
+	print_field("    Role: 0x%2.2x", data[1] & 0x03);
+
+	switch (data[1] & 0x03) {
+	case 0x00:
+		print_field("      Not Specified");
+		break;
+	case 0x01:
+		print_field("      Seeker Only");
+		break;
+	case 0x02:
+		print_field("      Provider Only");
+		break;
+	case 0x03:
+		print_field("      Both Seeker an Provider");
+		break;
+	}
+
+	print_field("    Transport Data Incomplete: %s (0x%2.2x)",
+			data[1] & 0x04 ? "True" : "False", data[1] & 0x04);
+
+	print_field("    Transport State: 0x%2.2x", data[1] & 0x18);
+
+	switch (data[1] & 0x18) {
+	case 0x00:
+		print_field("      Off");
+		break;
+	case 0x08:
+		print_field("      On");
+		break;
+	case 0x10:
+		print_field("      Temporary Unavailable");
+		break;
+	case 0x18:
+		print_field("      RFU");
+		break;
+	}
+
+	print_field("  Length: %u", data[2]);
+	print_hex_field("  Data", data + 3, len - 3);
+}
+
 static void print_eir(const uint8_t *eir, uint8_t eir_len, bool le)
 {
 	uint16_t len = 0;
@@ -3744,6 +3795,10 @@ static void print_eir(const uint8_t *eir, uint8_t eir_len, bool le)
 			print_randomizer_p256(data);
 			break;
 
+		case BT_EIR_TRANSPORT_DISCOVERY:
+			print_transport_data(data, data_len);
+			break;
+
 		case BT_EIR_3D_INFO_DATA:
 			print_hex_field("3D Information Data", data, data_len);
 			if (data_len < 2)
-- 
2.14.3

[PATCH v2 7/9] client: Add support for setting advertising Data property

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds data command to advertise menu which can be used to set
an arbitrary/profile specific advertising type and data:

[bluetooth]# data 0x26 0x01 0x01 0x00
---
 client/advertising.c | 158 +++++++++++++++++++++++++++++++++++++--------------
 client/advertising.h |   2 +
 client/main.c        |  13 +++++
 3 files changed, 130 insertions(+), 43 deletions(-)

diff --git a/client/advertising.c b/client/advertising.c
index 152a22a56..045133aa3 100644
--- a/client/advertising.c
+++ b/client/advertising.c
@@ -54,6 +54,11 @@ struct manufacturer_data {
 	struct ad_data data;
 };
 
+struct data {
+	uint8_t type;
+	struct ad_data data;
+};
+
 static struct ad {
 	bool registered;
 	char *type;
@@ -65,6 +70,7 @@ static struct ad {
 	size_t uuids_len;
 	struct service_data service;
 	struct manufacturer_data manufacturer;
+	struct data data;
 	bool tx_power;
 	bool name;
 	bool appearance;
@@ -373,12 +379,35 @@ static gboolean get_timeout(const GDBusPropertyTable *property,
 	return TRUE;
 }
 
+static gboolean data_exists(const GDBusPropertyTable *property, void *data)
+{
+	return ad.data.type != 0;
+}
+
+static gboolean get_data(const GDBusPropertyTable *property,
+					DBusMessageIter *iter, void *user_data)
+{
+	DBusMessageIter dict;
+	struct ad_data *data = &ad.data.data;
+	uint8_t *val = data->data;
+
+	dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY, "{yv}", &dict);
+
+	g_dbus_dict_append_basic_array(&dict, DBUS_TYPE_BYTE, &ad.data.type,
+					DBUS_TYPE_BYTE, &val, data->len);
+
+	dbus_message_iter_close_container(iter, &dict);
+
+	return TRUE;
+}
+
 static const GDBusPropertyTable ad_props[] = {
 	{ "Type", "s", get_type },
 	{ "ServiceUUIDs", "as", get_uuids, NULL, uuids_exists },
 	{ "ServiceData", "a{sv}", get_service_data, NULL, service_data_exists },
 	{ "ManufacturerData", "a{qv}", get_manufacturer_data, NULL,
 						manufacturer_data_exists },
+	{ "Data", "a{yv}", get_data, NULL, data_exists },
 	{ "Includes", "as", get_includes, NULL, includes_exists },
 	{ "LocalName", "s", get_local_name, NULL, local_name_exits },
 	{ "Appearance", "q", get_appearance, NULL, appearance_exits },
@@ -508,46 +537,55 @@ static void ad_clear_service(void)
 	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
 }
 
-void ad_advertise_service(DBusConnection *conn, int argc, char *argv[])
+static bool ad_add_data(struct ad_data *data, int argc, char *argv[])
 {
 	unsigned int i;
-	struct ad_data *data;
 
-	if (argc < 2 || !strlen(argv[1])) {
-		if (ad.service.uuid) {
-			print_uuid(ad.service.uuid);
-			bt_shell_hexdump(ad.service.data.data,
-						ad.service.data.len);
-		}
-		return bt_shell_noninteractive_quit(EXIT_SUCCESS);
-	}
+	memset(data, 0, sizeof(*data));
 
-	ad_clear_service();
-
-	ad.service.uuid = g_strdup(argv[1]);
-	data = &ad.service.data;
-
-	for (i = 2; i < (unsigned int) argc; i++) {
+	for (i = 0; i < (unsigned int) argc; i++) {
 		long int val;
 		char *endptr = NULL;
 
 		if (i >= G_N_ELEMENTS(data->data)) {
 			bt_shell_printf("Too much data\n");
-			ad_clear_service();
-			return;
+			return false;
 		}
 
 		val = strtol(argv[i], &endptr, 0);
 		if (!endptr || *endptr != '\0' || val > UINT8_MAX) {
 			bt_shell_printf("Invalid value at index %d\n", i);
-			ad_clear_service();
-			return;
+			return false;
 		}
 
 		data->data[data->len] = val;
 		data->len++;
 	}
 
+	return true;
+}
+
+void ad_advertise_service(DBusConnection *conn, int argc, char *argv[])
+{
+	struct ad_data data;
+
+	if (argc < 2 || !strlen(argv[1])) {
+		if (ad.service.uuid) {
+			print_uuid(ad.service.uuid);
+			bt_shell_hexdump(ad.service.data.data,
+						ad.service.data.len);
+		}
+		return bt_shell_noninteractive_quit(EXIT_SUCCESS);
+	}
+
+	if (!ad_add_data(&data, argc - 2, argv + 2))
+		return bt_shell_noninteractive_quit(EXIT_FAILURE);
+
+	ad_clear_service();
+
+	ad.service.uuid = g_strdup(argv[1]);
+	ad.service.data = data;
+
 	g_dbus_emit_property_changed(conn, AD_PATH, AD_IFACE, "ServiceData");
 
 	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
@@ -573,10 +611,9 @@ static void ad_clear_manufacturer(void)
 
 void ad_advertise_manufacturer(DBusConnection *conn, int argc, char *argv[])
 {
-	unsigned int i;
 	char *endptr = NULL;
 	long int val;
-	struct ad_data *data;
+	struct ad_data data;
 
 	if (argc < 2 || !strlen(argv[1])) {
 		if (ad.manufacturer.data.len) {
@@ -589,34 +626,18 @@ void ad_advertise_manufacturer(DBusConnection *conn, int argc, char *argv[])
 		return bt_shell_noninteractive_quit(EXIT_SUCCESS);
 	}
 
-	ad_clear_manufacturer();
-
 	val = strtol(argv[1], &endptr, 0);
 	if (!endptr || *endptr != '\0' || val > UINT16_MAX) {
 		bt_shell_printf("Invalid manufacture id\n");
 		return bt_shell_noninteractive_quit(EXIT_FAILURE);
 	}
 
-	ad.manufacturer.id = val;
-	data = &ad.manufacturer.data;
-
-	for (i = 2; i < (unsigned int) argc; i++) {
-		if (i >= G_N_ELEMENTS(data->data)) {
-			bt_shell_printf("Too much data\n");
-			ad_clear_manufacturer();
-			return bt_shell_noninteractive_quit(EXIT_FAILURE);
-		}
-
-		val = strtol(argv[i], &endptr, 0);
-		if (!endptr || *endptr != '\0' || val > UINT8_MAX) {
-			bt_shell_printf("Invalid value at index %d\n", i);
-			ad_clear_manufacturer();
-			return bt_shell_noninteractive_quit(EXIT_FAILURE);
-		}
+	if (!ad_add_data(&data, argc - 2, argv + 2))
+		return bt_shell_noninteractive_quit(EXIT_FAILURE);
 
-		data->data[data->len] = val;
-		data->len++;
-	}
+	ad_clear_manufacturer();
+	ad.manufacturer.id = val;
+	ad.manufacturer.data = data;
 
 	g_dbus_emit_property_changed(conn, AD_PATH, AD_IFACE,
 							"ManufacturerData");
@@ -636,6 +657,57 @@ void ad_disable_manufacturer(DBusConnection *conn)
 	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
 }
 
+static void ad_clear_data(void)
+{
+	memset(&ad.manufacturer, 0, sizeof(ad.manufacturer));
+
+	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
+}
+
+void ad_advertise_data(DBusConnection *conn, int argc, char *argv[])
+{
+	char *endptr = NULL;
+	long int val;
+	struct ad_data data;
+
+	if (argc < 2 || !strlen(argv[1])) {
+		if (ad.manufacturer.data.len) {
+			bt_shell_printf("Type: 0x%02x\n", ad.data.type);
+			bt_shell_hexdump(ad.data.data.data, ad.data.data.len);
+		}
+
+		return bt_shell_noninteractive_quit(EXIT_SUCCESS);
+	}
+
+	val = strtol(argv[1], &endptr, 0);
+	if (!endptr || *endptr != '\0' || val > UINT8_MAX) {
+		bt_shell_printf("Invalid type\n");
+		return bt_shell_noninteractive_quit(EXIT_FAILURE);
+	}
+
+	if (!ad_add_data(&data, argc - 2, argv + 2))
+		return bt_shell_noninteractive_quit(EXIT_FAILURE);
+
+	ad_clear_data();
+	ad.data.type = val;
+	ad.data.data = data;
+
+	g_dbus_emit_property_changed(conn, AD_PATH, AD_IFACE, "Data");
+
+	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
+}
+
+void ad_disable_data(DBusConnection *conn)
+{
+	if (!ad.data.type && !ad.data.data.len)
+		return bt_shell_noninteractive_quit(EXIT_SUCCESS);
+
+	ad_clear_data();
+	g_dbus_emit_property_changed(conn, AD_PATH, AD_IFACE, "Data");
+
+	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
+}
+
 void ad_advertise_tx_power(DBusConnection *conn, dbus_bool_t *value)
 {
 	if (!value) {
diff --git a/client/advertising.h b/client/advertising.h
index b73d33b13..12b4d69c1 100644
--- a/client/advertising.h
+++ b/client/advertising.h
@@ -37,3 +37,5 @@ void ad_advertise_local_name(DBusConnection *conn, const char *name);
 void ad_advertise_local_appearance(DBusConnection *conn, long int *value);
 void ad_advertise_duration(DBusConnection *conn, long int *value);
 void ad_advertise_timeout(DBusConnection *conn, long int *value);
+void ad_advertise_data(DBusConnection *conn, int argc, char *argv[]);
+void ad_disable_data(DBusConnection *conn);
diff --git a/client/main.c b/client/main.c
index dd85a1c85..ebd7c195c 100644
--- a/client/main.c
+++ b/client/main.c
@@ -2185,6 +2185,11 @@ static void cmd_advertise_manufacturer(int argc, char *argv[])
 	ad_advertise_manufacturer(dbus_conn, argc, argv);
 }
 
+static void cmd_advertise_data(int argc, char *argv[])
+{
+	ad_advertise_data(dbus_conn, argc, argv);
+}
+
 static void cmd_advertise_tx_power(int argc, char *argv[])
 {
 	dbus_bool_t powered;
@@ -2302,6 +2307,11 @@ static void ad_clear_manufacturer(void)
 	ad_disable_manufacturer(dbus_conn);
 }
 
+static void ad_clear_data(void)
+{
+	ad_disable_data(dbus_conn);
+}
+
 static void ad_clear_tx_power(void)
 {
 	dbus_bool_t powered = false;
@@ -2337,6 +2347,7 @@ static const struct clear_entry ad_clear[] = {
 	{ "uuids",		ad_clear_uuids },
 	{ "service",		ad_clear_service },
 	{ "manufacturer",	ad_clear_manufacturer },
+	{ "data",		ad_clear_data },
 	{ "tx-power",		ad_clear_tx_power },
 	{ "name",		ad_clear_name },
 	{ "appearance",		ad_clear_appearance },
@@ -2367,6 +2378,8 @@ static const struct bt_shell_menu advertise_menu = {
 	{ "manufacturer", "[id] [data=xx xx ...]",
 			cmd_advertise_manufacturer,
 			"Set/Get advertise manufacturer data" },
+	{ "data", "[type] [data=xx xx ...]", cmd_advertise_data,
+			"Set/Get advertise data" },
 	{ "tx-power", "[on/off]", cmd_advertise_tx_power,
 			"Show/Enable/Disable TX power to be advertised",
 							NULL },
-- 
2.14.3

[PATCH v2 8/9] client: Make info command print Advertising{Flags,Data}

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

---
 client/main.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/client/main.c b/client/main.c
index ebd7c195c..54bd5371a 100644
--- a/client/main.c
+++ b/client/main.c
@@ -1490,6 +1490,8 @@ static void cmd_info(int argc, char *argv[])
 	print_property(proxy, "ServiceData");
 	print_property(proxy, "RSSI");
 	print_property(proxy, "TxPower");
+	print_property(proxy, "AdvertisingFlags");
+	print_property(proxy, "AdvertisingData");
 
 	return bt_shell_noninteractive_quit(EXIT_SUCCESS);
 }
-- 
2.14.3

[PATCH v2 9/9] test/example-advertisement: Add Data property

[Luiz Augusto von Dentz]

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

This adds Data property using TDS AD type:

@ MGMT Command: Add Advertising (0x003e) plen 59
        Instance: 1
        Flags: 0x00000001
          Switch into Connectable mode
        Duration: 0
        Timeout: 0
        Advertising data length: 29
        16-bit Service UUIDs (complete): 2 entries
          Heart Rate (0x180d)
          Battery Service (0x180f)
        Company: internal use (65535)
          Data: 0001020304
        Service Data (UUID 0x9999): 0001020304
        Transport Discovery Data
          Organization: Bluetooth SIG (0x01)
          Flags: 0x01
            Role: 0x01
              Seeker Only
            Transport Data Incomplete: False (0x00)
            Transport State: 0x00
              Off
          Length: 0
          Data:
        Scan response length: 19
        Name (complete): TestAdvertisement
---
 test/example-advertisement | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/test/example-advertisement b/test/example-advertisement
index 26c3578df..d9b5b42d8 100755
--- a/test/example-advertisement
+++ b/test/example-advertisement
@@ -53,6 +53,7 @@ class Advertisement(dbus.service.Object):
         self.service_data = None
         self.local_name = None
         self.include_tx_power = None
+        self.data = None
         dbus.service.Object.__init__(self, bus, self.path)
 
     def get_properties(self):
@@ -74,6 +75,10 @@ class Advertisement(dbus.service.Object):
             properties['LocalName'] = dbus.String(self.local_name)
         if self.include_tx_power is not None:
             properties['IncludeTxPower'] = dbus.Boolean(self.include_tx_power)
+
+        if self.data is not None:
+            properties['Data'] = dbus.Dictionary(
+                self.data, signature='yv')
         return {LE_ADVERTISEMENT_IFACE: properties}
 
     def get_path(self):
@@ -104,6 +109,11 @@ class Advertisement(dbus.service.Object):
             self.local_name = ""
         self.local_name = dbus.String(name)
 
+    def add_data(self, ad_type, data):
+        if not self.data:
+            self.data = dbus.Dictionary({}, signature='yv')
+        self.data[ad_type] = dbus.Array(data, signature='y')
+
     @dbus.service.method(DBUS_PROP_IFACE,
                          in_signature='s',
                          out_signature='a{sv}')
@@ -130,6 +140,7 @@ class TestAdvertisement(Advertisement):
         self.add_service_data('9999', [0x00, 0x01, 0x02, 0x03, 0x04])
         self.add_local_name('TestAdvertisement')
         self.include_tx_power = True
+        self.add_data(0x26, [0x01, 0x01, 0x00])
 
 
 def register_ad_cb():
-- 
2.14.3

Re: [PATCH v2 1/9] shared/ad: Add support for arbritary type

[Luiz Augusto von Dentz]

Hi,
On Wed, May 2, 2018 at 12:24 PM Luiz Augusto von Dentz
<luiz.dentz@gmail.com>
wrote:

> From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

> This adds APIs to include data of arbritary type as long as it is not
> of a type which blacklisted due to being handled already or it is
> considered to safe to be set by an application.
> ---
>   src/shared/ad.c | 231
++++++++++++++++++++++++++++++++++++++++++++++++++++----
>   src/shared/ad.h |  60 +++++++++++++++
>   2 files changed, 278 insertions(+), 13 deletions(-)

> diff --git a/src/shared/ad.c b/src/shared/ad.c
> index 255794dc4..f0e62cef1 100644
> --- a/src/shared/ad.c
> +++ b/src/shared/ad.c
> @@ -37,6 +37,7 @@ struct bt_ad {
>          struct queue *manufacturer_data;
>          struct queue *solicit_uuids;
>          struct queue *service_data;
> +       struct queue *data;
>   };

>   struct bt_ad *bt_ad_new(void)
> @@ -48,6 +49,7 @@ struct bt_ad *bt_ad_new(void)
>          ad->manufacturer_data = queue_new();
>          ad->solicit_uuids = queue_new();
>          ad->service_data = queue_new();
> +       ad->data = queue_new();
>          ad->appearance = UINT16_MAX;

>          return bt_ad_ref(ad);
> @@ -94,6 +96,14 @@ static bool manuf_match(const void *data, const void
*elem)
>          return manuf->manufacturer_id == manuf_id;
>   }

> +static void data_destroy(void *data)
> +{
> +       struct bt_ad_data *ad = data;
> +
> +       free(ad->data);
> +       free(ad);
> +}
> +
>   void bt_ad_unref(struct bt_ad *ad)
>   {
>          if (!ad)
> @@ -110,6 +120,8 @@ void bt_ad_unref(struct bt_ad *ad)

>          queue_destroy(ad->service_data, uuid_destroy);

> +       queue_destroy(ad->data, data_destroy);
> +
>          free(ad->name);

>          free(ad);
> @@ -203,6 +215,24 @@ static size_t name_length(const char *name, size_t
*pos)
>          return len;
>   }

> +static size_t data_length(struct queue *queue)
> +{
> +       size_t length = 0;
> +       const struct queue_entry *entry;
> +
> +       entry = queue_get_entries(queue);
> +
> +       while (entry) {
> +               struct bt_ad_data *data = entry->data;
> +
> +               length += 1 + 1 + data->len;
> +
> +               entry = entry->next;
> +       }
> +
> +       return length;
> +}
> +
>   static size_t calculate_length(struct bt_ad *ad)
>   {
>          size_t length = 0;
> @@ -219,6 +249,8 @@ static size_t calculate_length(struct bt_ad *ad)

>          length += ad->appearance != UINT16_MAX ? 4 : 0;

> +       length += data_length(ad->data);
> +
>          return length;
>   }

> @@ -258,21 +290,21 @@ static void serialize_uuids(struct queue *uuids,
uint8_t uuid_type,
>   static void serialize_service_uuids(struct queue *uuids, uint8_t *buf,
>                                                                  uint8_t
*pos)
>   {
> -       serialize_uuids(uuids, BT_UUID16, EIR_UUID16_ALL, buf, pos);
> +       serialize_uuids(uuids, BT_UUID16, BT_AD_UUID16_ALL, buf, pos);

> -       serialize_uuids(uuids, BT_UUID32, EIR_UUID32_ALL, buf, pos);
> +       serialize_uuids(uuids, BT_UUID32, BT_AD_UUID32_ALL, buf, pos);

> -       serialize_uuids(uuids, BT_UUID128, EIR_UUID128_ALL, buf, pos);
> +       serialize_uuids(uuids, BT_UUID128, BT_AD_UUID128_ALL, buf, pos);
>   }

>   static void serialize_solicit_uuids(struct queue *uuids, uint8_t *buf,
>                                                                  uint8_t
*pos)
>   {
> -       serialize_uuids(uuids, BT_UUID16, EIR_SOLICIT16, buf, pos);
> +       serialize_uuids(uuids, BT_UUID16, BT_AD_SOLICIT16, buf, pos);

> -       serialize_uuids(uuids, BT_UUID32, EIR_SOLICIT32, buf, pos);
> +       serialize_uuids(uuids, BT_UUID32, BT_AD_SOLICIT32, buf, pos);

> -       serialize_uuids(uuids, BT_UUID128, EIR_SOLICIT128, buf, pos);
> +       serialize_uuids(uuids, BT_UUID128, BT_AD_SOLICIT128, buf, pos);
>   }

>   static void serialize_manuf_data(struct queue *manuf_data, uint8_t *buf,
> @@ -285,7 +317,7 @@ static void serialize_manuf_data(struct queue
*manuf_data, uint8_t *buf,

>                  buf[(*pos)++] = data->len + 2 + 1;

> -               buf[(*pos)++] = EIR_MANUFACTURER_DATA;
> +               buf[(*pos)++] = BT_AD_MANUFACTURER_DATA;

>                  bt_put_le16(data->manufacturer_id, buf + (*pos));

> @@ -312,13 +344,13 @@ static void serialize_service_data(struct queue
*service_data, uint8_t *buf,

>                  switch (uuid_len) {
>                  case 2:
> -                       buf[(*pos)++] = EIR_SVC_DATA16;
> +                       buf[(*pos)++] = BT_AD_SERVICE_DATA16;
>                          break;
>                  case 4:
> -                       buf[(*pos)++] = EIR_SVC_DATA32;
> +                       buf[(*pos)++] = BT_AD_SERVICE_DATA32;
>                          break;
>                  case 16:
> -                       buf[(*pos)++] = EIR_SVC_DATA128;
> +                       buf[(*pos)++] = BT_AD_SERVICE_DATA128;
>                          break;
>                  }

> @@ -340,14 +372,14 @@ static void serialize_service_data(struct queue
*service_data, uint8_t *buf,
>   static void serialize_name(const char *name, uint8_t *buf, uint8_t *pos)
>   {
>          int len;
> -       uint8_t type = EIR_NAME_COMPLETE;
> +       uint8_t type = BT_AD_NAME_COMPLETE;

>          if (!name)
>                  return;

>          len = strlen(name);
>          if (len > MAX_ADV_DATA_LEN - (*pos + 2)) {
> -               type = EIR_NAME_SHORT;
> +               type = BT_AD_NAME_SHORT;
>                  len = MAX_ADV_DATA_LEN - (*pos + 2);
>          }

> @@ -364,12 +396,30 @@ static void serialize_appearance(uint16_t value,
uint8_t *buf, uint8_t *pos)
>                  return;

>          buf[(*pos)++] = sizeof(value) + 1;
> -       buf[(*pos)++] = EIR_GAP_APPEARANCE;
> +       buf[(*pos)++] = BT_AD_GAP_APPEARANCE;

>          bt_put_le16(value, buf + (*pos));
>          *pos += 2;
>   }

> +static void serialize_data(struct queue *queue, uint8_t *buf, uint8_t
*pos)
> +{
> +       const struct queue_entry *entry = queue_get_entries(queue);
> +
> +       while (entry) {
> +               struct bt_ad_data *data = entry->data;
> +
> +               buf[(*pos)++] = data->len + 1;
> +               buf[(*pos)++] = data->type;
> +
> +               memcpy(buf + *pos, data->data, data->len);
> +
> +               *pos += data->len;
> +
> +               entry = entry->next;
> +       }
> +}
> +
>   uint8_t *bt_ad_generate(struct bt_ad *ad, size_t *length)
>   {
>          uint8_t *adv_data;
> @@ -399,6 +449,8 @@ uint8_t *bt_ad_generate(struct bt_ad *ad, size_t
*length)

>          serialize_appearance(ad->appearance, adv_data, &pos);

> +       serialize_data(ad->data, adv_data, &pos);
> +
>          return adv_data;
>   }

> @@ -756,3 +808,156 @@ void bt_ad_clear_appearance(struct bt_ad *ad)

>          ad->appearance = UINT16_MAX;
>   }
> +
> +static bool data_type_match(const void *data, const void *user_data)
> +{
> +       const struct bt_ad_data *a = data;
> +       const uint8_t type = PTR_TO_UINT(user_data);
> +
> +       return a->type == type;
> +}
> +
> +static uint8_t type_blacklist[] = {
> +       BT_AD_FLAGS,
> +       BT_AD_UUID16_SOME,
> +       BT_AD_UUID16_ALL,
> +       BT_AD_UUID32_SOME,
> +       BT_AD_UUID32_ALL,
> +       BT_AD_UUID128_SOME,
> +       BT_AD_UUID128_ALL,
> +       BT_AD_NAME_SHORT,
> +       BT_AD_NAME_COMPLETE,
> +       BT_AD_TX_POWER,
> +       BT_AD_CLASS_OF_DEV,
> +       BT_AD_SSP_HASH,
> +       BT_AD_SSP_RANDOMIZER,
> +       BT_AD_DEVICE_ID,
> +       BT_AD_SMP_TK,
> +       BT_AD_SMP_OOB_FLAGS,
> +       BT_AD_SLAVE_CONN_INTERVAL,
> +       BT_AD_SOLICIT16,
> +       BT_AD_SOLICIT128,
> +       BT_AD_SERVICE_DATA16,
> +       BT_AD_PUBLIC_ADDRESS,
> +       BT_AD_RANDOM_ADDRESS,
> +       BT_AD_GAP_APPEARANCE,
> +       BT_AD_ADVERTISING_INTERVAL,
> +       BT_AD_LE_DEVICE_ADDRESS,
> +       BT_AD_LE_ROLE,
> +       BT_AD_SSP_HASH_P256,
> +       BT_AD_SSP_RANDOMIZER_P256,
> +       BT_AD_SOLICIT32,
> +       BT_AD_SERVICE_DATA32,
> +       BT_AD_SERVICE_DATA128,
> +       BT_AD_LE_SC_CONFIRM_VALUE,
> +       BT_AD_LE_SC_RANDOM_VALUE,
> +       BT_AD_LE_SUPPORTED_FEATURES,
> +       BT_AD_CHANNEL_MAP_UPDATE_IND,
> +       BT_AD_MESH_PROV,
> +       BT_AD_MESH_DATA,
> +       BT_AD_MESH_BEACON,
> +       BT_AD_3D_INFO_DATA,
> +       BT_AD_MANUFACTURER_DATA,
> +};
> +
> +bool bt_ad_add_data(struct bt_ad *ad, uint8_t type, void *data, size_t
len)
> +{
> +       struct bt_ad_data *new_data;
> +       size_t i;
> +
> +       if (!ad)
> +               return false;
> +
> +       if (len > (MAX_ADV_DATA_LEN - 2))
> +               return false;
> +
> +       new_data = queue_find(ad->data, data_type_match,
UINT_TO_PTR(type));
> +       if (new_data) {
> +               if (new_data->len == len && !memcmp(new_data->data, data,
len))
> +                       return false;
> +               new_data->data = realloc(new_data->data, len);
> +               memcpy(new_data->data, data, len);
> +               new_data->len = len;
> +               return true;
> +       }
> +
> +       for (i = 0; i < sizeof(type_blacklist); i++) {
> +               if (type == type_blacklist[i])
> +                       return false;
> +       }
> +
> +       new_data = new0(struct bt_ad_data, 1);
> +       new_data->type = type;
> +       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->data, new_data))
> +               return true;
> +
> +       data_destroy(new_data);
> +
> +       return false;
> +}
> +
> +static bool data_match(const void *data, const void *user_data)
> +{
> +       const struct bt_ad_data *d1 = data;
> +       const struct bt_ad_data *d2 = user_data;
> +
> +       if (d1->type != d2->type)
> +               return false;
> +
> +       if (d1->len != d2->len)
> +               return false;
> +
> +       return !memcmp(d1->data, d2->data, d1->len);
> +}
> +
> +bool bt_ad_has_data(struct bt_ad *ad, const struct bt_ad_data *data)
> +{
> +       if (!ad)
> +               return false;
> +
> +       if (!data)
> +               return !queue_isempty(ad->data);
> +
> +       return queue_find(ad->data, data_match, data);
> +}
> +
> +void bt_ad_foreach_data(struct bt_ad *ad, bt_ad_func_t func, void
*user_data)
> +{
> +       if (!ad)
> +               return;
> +
> +       queue_foreach(ad->data, func, user_data);
> +}
> +
> +bool bt_ad_remove_data(struct bt_ad *ad, uint8_t type)
> +{
> +       struct bt_ad_data *data;
> +
> +       if (!ad)
> +               return false;
> +
> +       data = queue_remove_if(ad->data, data_type_match,
UINT_TO_PTR(type));
> +       if (!data)
> +               return false;
> +
> +       data_destroy(data);
> +
> +       return true;
> +}
> +
> +void bt_ad_clear_data(struct bt_ad *ad)
> +{
> +       if (!ad)
> +               return;
> +
> +       queue_remove_all(ad->data, NULL, NULL, data_destroy);
> +}
> diff --git a/src/shared/ad.h b/src/shared/ad.h
> index f0e3b8127..8d705323b 100644
> --- a/src/shared/ad.h
> +++ b/src/shared/ad.h
> @@ -27,6 +27,50 @@
>   #include "lib/bluetooth.h"
>   #include "lib/uuid.h"

> +#define BT_AD_FLAGS                    0x01
> +#define BT_AD_UUID16_SOME              0x02
> +#define BT_AD_UUID16_ALL               0x03
> +#define BT_AD_UUID32_SOME              0x04
> +#define BT_AD_UUID32_ALL               0x05
> +#define BT_AD_UUID128_SOME             0x06
> +#define BT_AD_UUID128_ALL              0x07
> +#define BT_AD_NAME_SHORT               0x08
> +#define BT_AD_NAME_COMPLETE            0x09
> +#define BT_AD_TX_POWER                 0x0a
> +#define BT_AD_CLASS_OF_DEV             0x0d
> +#define BT_AD_SSP_HASH                 0x0e
> +#define BT_AD_SSP_RANDOMIZER           0x0f
> +#define BT_AD_DEVICE_ID                        0x10
> +#define BT_AD_SMP_TK                   0x10
> +#define BT_AD_SMP_OOB_FLAGS            0x11
> +#define BT_AD_SLAVE_CONN_INTERVAL      0x12
> +#define BT_AD_SOLICIT16                        0x14
> +#define BT_AD_SOLICIT128               0x15
> +#define BT_AD_SERVICE_DATA16           0x16
> +#define BT_AD_PUBLIC_ADDRESS           0x17
> +#define BT_AD_RANDOM_ADDRESS           0x18
> +#define BT_AD_GAP_APPEARANCE           0x19
> +#define BT_AD_ADVERTISING_INTERVAL     0x1a
> +#define BT_AD_LE_DEVICE_ADDRESS                0x1b
> +#define BT_AD_LE_ROLE                  0x1c
> +#define BT_AD_SSP_HASH_P256            0x1d
> +#define BT_AD_SSP_RANDOMIZER_P256      0x1e
> +#define BT_AD_SOLICIT32                        0x1f
> +#define BT_AD_SERVICE_DATA32           0x20
> +#define BT_AD_SERVICE_DATA128          0x21
> +#define BT_AD_LE_SC_CONFIRM_VALUE      0x22
> +#define BT_AD_LE_SC_RANDOM_VALUE       0x23
> +#define BT_AD_URI                      0x24
> +#define BT_AD_INDOOR_POSITIONING       0x25
> +#define BT_AD_TRANSPORT_DISCOVERY      0x26
> +#define BT_AD_LE_SUPPORTED_FEATURES    0x27
> +#define BT_AD_CHANNEL_MAP_UPDATE_IND   0x28
> +#define BT_AD_MESH_PROV                        0x29
> +#define BT_AD_MESH_DATA                        0x2a
> +#define BT_AD_MESH_BEACON              0x2b
> +#define BT_AD_3D_INFO_DATA             0x3d
> +#define BT_AD_MANUFACTURER_DATA                0xff
> +
>   typedef void (*bt_ad_func_t)(void *data, void *user_data);

>   struct bt_ad;
> @@ -39,6 +83,12 @@ struct bt_ad_manufacturer_data {

>   struct bt_ad_service_data {
>          bt_uuid_t uuid;
> +       size_t len;
> +       void *data;
> +};
> +
> +struct bt_ad_data {
> +       uint8_t type;
>          uint8_t *data;
>          size_t len;
>   };
> @@ -96,3 +146,13 @@ void bt_ad_clear_name(struct bt_ad *ad);
>   bool bt_ad_add_appearance(struct bt_ad *ad, uint16_t appearance);

>   void bt_ad_clear_appearance(struct bt_ad *ad);
> +
> +bool bt_ad_add_data(struct bt_ad *ad, uint8_t type, void *data, size_t
len);
> +
> +bool bt_ad_has_data(struct bt_ad *ad, const struct bt_ad_data *data);
> +
> +void bt_ad_foreach_data(struct bt_ad *ad, bt_ad_func_t func, void
*user_data);
> +
> +bool bt_ad_remove_data(struct bt_ad *ad, uint8_t type);
> +
> +void bt_ad_clear_data(struct bt_ad *ad);
> --
> 2.14.3

Applied.


-- 
Luiz Augusto von Dentz