From: Luiz Augusto von Dentz <luiz.von.dentz@xxxxxxxxx>
This makes use of util_iov_pull_* helpers to parse data.
---
src/shared/ad.c | 266 ++++++++++++++++++++++++++++++++++++++++++++----
src/shared/ad.h | 10 +-
2 files changed, 253 insertions(+), 23 deletions(-)
diff --git a/src/shared/ad.c b/src/shared/ad.c
index e8bebe70708e..951c56c604e6 100644
--- a/src/shared/ad.c
+++ b/src/shared/ad.c
@@ -14,6 +14,11 @@
#define _GNU_SOURCE
+#include <ctype.h>
+
+#include "lib/bluetooth.h"
+#include "lib/hci.h"
+
#include "src/shared/ad.h"
#include "src/eir.h"
@@ -80,7 +85,11 @@ static bool ad_is_type_valid(uint8_t type)
struct bt_ad *bt_ad_new_with_data(size_t len, const uint8_t *data)
{
struct bt_ad *ad;
- uint16_t parsed_len = 0;
+ struct iovec iov = {
+ .iov_base = (void *)data,
+ .iov_len = len,
+ };
+ uint8_t elen;
if (data == NULL || !len)
return NULL;
@@ -89,31 +98,29 @@ struct bt_ad *bt_ad_new_with_data(size_t len, const uint8_t *data)
if (!ad)
return NULL;
- while (parsed_len < len - 1) {
- uint8_t d_len;
- uint8_t d_type;
- const uint8_t *d;
- uint8_t field_len = data[0];
+ bt_ad_set_max_len(ad, len);
- if (field_len == 0)
+ while (util_iov_pull_u8(&iov, &elen)) {
+ uint8_t type;
+ void *data;
+
+ if (elen == 0 || elen > iov.iov_len)
break;
- parsed_len += field_len + 1;
-
- if (parsed_len > len)
- break;
-
- d = &data[2];
- d_type = data[1];
- d_len = field_len - 1;
-
- if (!ad_is_type_valid(d_type))
+ if (!util_iov_pull_u8(&iov, &type))
goto failed;
- if (!ad_replace_data(ad, d_type, d, d_len))
+ elen--;
+
+ if (!ad_is_type_valid(type))
goto failed;
- data += field_len + 1;
+ data = util_iov_pull_mem(&iov, elen);
+ if (!data)
+ goto failed;
+
+ if (!ad_replace_data(ad, type, data, elen))
+ goto failed;
}
return ad;
@@ -203,10 +210,181 @@ static bool data_type_match(const void *data, const void *user_data)
return a->type == type;
}
+static bool ad_replace_uuid16(struct bt_ad *ad, struct iovec *iov)
+{
+ uint16_t value;
+
+ while ((util_iov_pull_le16(iov, &value))) {
+ bt_uuid_t uuid;
+
+ if (bt_uuid16_create(&uuid, value))
+ return false;
+
+ if (bt_ad_has_service_uuid(ad, &uuid))
+ continue;
+
+ if (!bt_ad_add_service_uuid(ad, &uuid))
+ return false;
+ }
+
+ return true;
+}
+
+static bool ad_replace_uuid32(struct bt_ad *ad, struct iovec *iov)
+{
+ uint32_t value;
+
+ while ((util_iov_pull_le32(iov, &value))) {
+ bt_uuid_t uuid;
+
+ if (bt_uuid32_create(&uuid, value))
+ return false;
+
+ if (bt_ad_has_service_uuid(ad, &uuid))
+ continue;
+
+ if (!bt_ad_add_service_uuid(ad, &uuid))
+ return false;
+ }
+
+ return true;
+}
+
+static bool ad_replace_uuid128(struct bt_ad *ad, struct iovec *iov)
+{
+ void *data;
+
+ while ((data = util_iov_pull_mem(iov, 16))) {
+ uint128_t value;
+ bt_uuid_t uuid;
+
+ bswap_128(data, &value);
+
+ if (bt_uuid128_create(&uuid, value))
+ return false;
+
+ if (bt_ad_has_service_uuid(ad, &uuid))
+ continue;
+
+ if (!bt_ad_add_service_uuid(ad, &uuid))
+ return false;
+ }
+
+ return true;
+}
+
+static bool ad_replace_name(struct bt_ad *ad, struct iovec *iov)
+{
+ char utf8_name[HCI_MAX_NAME_LENGTH + 2];
+ int i;
+
+ memset(utf8_name, 0, sizeof(utf8_name));
+ strncpy(utf8_name, (const char *)iov->iov_base, iov->iov_len);
+
+ if (strisutf8(utf8_name, iov->iov_len))
+ goto done;
+
+ /* Assume ASCII, and replace all non-ASCII with spaces */
+ for (i = 0; utf8_name[i] != '\0'; i++) {
+ if (!isascii(utf8_name[i]))
+ utf8_name[i] = ' ';
+ }
+
+ /* Remove leading and trailing whitespace characters */
+ strstrip(utf8_name);
+
+done:
+ return bt_ad_add_name(ad, utf8_name);
+}
+
+static bool ad_replace_uuid16_data(struct bt_ad *ad, struct iovec *iov)
+{
+ uint16_t value;
+ bt_uuid_t uuid;
+
+ if (!util_iov_pull_le16(iov, &value))
+ return false;
+
+ if (bt_uuid16_create(&uuid, value))
+ return false;
+
+ return bt_ad_add_service_data(ad, &uuid, iov->iov_base, iov->iov_len);
+}
+
+static bool ad_replace_uuid32_data(struct bt_ad *ad, struct iovec *iov)
+{
+ uint32_t value;
+ bt_uuid_t uuid;
+
+ if (!util_iov_pull_le32(iov, &value))
+ return false;
+
+ if (bt_uuid32_create(&uuid, value))
+ return false;
+
+ return bt_ad_add_service_data(ad, &uuid, iov->iov_base, iov->iov_len);
+}
+
+static bool ad_replace_uuid128_data(struct bt_ad *ad, struct iovec *iov)
+{
+ void *data;
+ uint128_t value;
+ bt_uuid_t uuid;
+
+ data = util_iov_pull_mem(iov, 16);
+ if (!data)
+ return false;
+
+ bswap_128(data, &value);
+
+ if (bt_uuid128_create(&uuid, value))
+ return false;
+
+ return bt_ad_add_service_data(ad, &uuid, iov->iov_base, iov->iov_len);
+}
+
+static bool ad_replace_manufacturer_data(struct bt_ad *ad, struct iovec *iov)
+{
+ uint16_t value;
+
+ if (!util_iov_pull_le16(iov, &value))
+ return false;
+
+ return bt_ad_add_manufacturer_data(ad, value, iov->iov_base,
+ iov->iov_len);
+}
+
static bool ad_replace_data(struct bt_ad *ad, uint8_t type, const void *data,
size_t len)
{
struct bt_ad_data *new_data;
+ struct iovec iov = {
+ .iov_base = (void *)data,
+ .iov_len = len,
+ };
+
+ switch (type) {
+ case BT_AD_UUID16_SOME:
+ case BT_AD_UUID16_ALL:
+ return ad_replace_uuid16(ad, &iov);
+ case BT_AD_UUID32_SOME:
+ case BT_AD_UUID32_ALL:
+ return ad_replace_uuid32(ad, &iov);
+ case BT_AD_UUID128_SOME:
+ case BT_AD_UUID128_ALL:
+ return ad_replace_uuid128(ad, &iov);
+ case BT_AD_NAME_SHORT:
+ case BT_AD_NAME_COMPLETE:
+ return ad_replace_name(ad, &iov);
+ case BT_AD_SERVICE_DATA16:
+ return ad_replace_uuid16_data(ad, &iov);
+ case BT_AD_SERVICE_DATA32:
+ return ad_replace_uuid32_data(ad, &iov);
+ case BT_AD_SERVICE_DATA128:
+ return ad_replace_uuid128_data(ad, &iov);
+ case BT_AD_MANUFACTURER_DATA:
+ return ad_replace_manufacturer_data(ad, &iov);
+ }
new_data = queue_find(ad->data, data_type_match, UINT_TO_PTR(type));
if (new_data) {
@@ -220,13 +398,12 @@ static bool ad_replace_data(struct bt_ad *ad, uint8_t type, const void *data,
new_data = new0(struct bt_ad_data, 1);
new_data->type = type;
- new_data->data = malloc(len);
+ new_data->data = util_memdup(data, 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))
@@ -590,7 +767,7 @@ 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);
+ return !bt_uuid_cmp(match_uuid, uuid);
}
static bool queue_remove_uuid(struct queue *queue, bt_uuid_t *uuid)
@@ -618,6 +795,14 @@ bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
return queue_add_uuid(ad->service_uuids, uuid);
}
+bool bt_ad_has_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid)
+{
+ if (!ad)
+ return false;
+
+ return queue_find(ad->service_uuids, uuid_match, uuid);
+}
+
bool bt_ad_remove_service_uuid(struct bt_ad *ad, bt_uuid_t *uuid)
{
if (!ad)
@@ -894,6 +1079,14 @@ bool bt_ad_add_name(struct bt_ad *ad, const char *name)
return true;
}
+const char *bt_ad_get_name(struct bt_ad *ad)
+{
+ if (!ad)
+ return false;
+
+ return ad->name;
+}
+
void bt_ad_clear_name(struct bt_ad *ad)
{
if (!ad)
@@ -933,6 +1126,20 @@ bool bt_ad_add_flags(struct bt_ad *ad, uint8_t *flags, size_t len)
return ad_replace_data(ad, BT_AD_FLAGS, flags, len);
}
+uint8_t bt_ad_get_flags(struct bt_ad *ad)
+{
+ struct bt_ad_data *data;
+
+ if (!ad)
+ return 0;
+
+ data = queue_find(ad->data, data_type_match, UINT_TO_PTR(BT_AD_FLAGS));
+ if (!data || data->len != 1)
+ return 0;
+
+ return data->data[0];
+}
+
bool bt_ad_has_flags(struct bt_ad *ad)
{
struct bt_ad_data *data;
@@ -1077,6 +1284,21 @@ void bt_ad_clear_data(struct bt_ad *ad)
queue_remove_all(ad->data, NULL, NULL, data_destroy);
}
+int8_t bt_ad_get_tx_power(struct bt_ad *ad)
+{
+ struct bt_ad_data *data;
+
+ if (!ad)
+ return 0;
+
+ data = queue_find(ad->data, data_type_match,
+ UINT_TO_PTR(BT_AD_TX_POWER));
+ if (!data || data->len != 1)
+ return 127;
+
+ return data->data[0];
+}
+
struct bt_ad_pattern *bt_ad_pattern_new(uint8_t type, size_t offset, size_t len,
const uint8_t *data)
{
diff --git a/src/shared/ad.h b/src/shared/ad.h
index 93ba1b6cfa0b..87b3401a389d 100644
--- a/src/shared/ad.h
+++ b/src/shared/ad.h
@@ -112,11 +112,13 @@ bool bt_ad_is_empty(struct bt_ad *ad);
bool bt_ad_add_service_uuid(struct bt_ad *ad, const bt_uuid_t *uuid);
+bool bt_ad_has_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,
+bool bt_ad_add_manufacturer_data(struct bt_ad *ad, uint16_t id,
void *data, size_t len);
bool bt_ad_has_manufacturer_data(struct bt_ad *ad,
@@ -150,6 +152,8 @@ void bt_ad_clear_service_data(struct bt_ad *ad);
bool bt_ad_add_name(struct bt_ad *ad, const char *name);
+const char *bt_ad_get_name(struct bt_ad *ad);
+
void bt_ad_clear_name(struct bt_ad *ad);
bool bt_ad_add_appearance(struct bt_ad *ad, uint16_t appearance);
@@ -160,6 +164,8 @@ bool bt_ad_add_flags(struct bt_ad *ad, uint8_t *flags, size_t len);
bool bt_ad_has_flags(struct bt_ad *ad);
+uint8_t bt_ad_get_flags(struct bt_ad *ad);
+
void bt_ad_clear_flags(struct bt_ad *ad);
bool bt_ad_add_data(struct bt_ad *ad, uint8_t type, void *data, size_t len);
@@ -172,6 +178,8 @@ bool bt_ad_remove_data(struct bt_ad *ad, uint8_t type);
void bt_ad_clear_data(struct bt_ad *ad);
+int8_t bt_ad_get_tx_power(struct bt_ad *ad);
+
struct bt_ad_pattern *bt_ad_pattern_new(uint8_t type, size_t offset,
size_t len, const uint8_t *data);
--
2.40.1
