diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h index f24c4e053..fa84d3b1d 100644 --- a/nimble/host/include/host/ble_gap.h +++ b/nimble/host/include/host/ble_gap.h @@ -22,6 +22,7 @@ #include #include "host/ble_hs.h" +#include "host/ble_hs_adv.h" #ifdef __cplusplus extern "C" { #endif @@ -222,10 +223,7 @@ struct ble_gap_disc_desc { uint8_t length_data; int8_t rssi; uint8_t addr[6]; - - /*** LE advertising report fields; both null if no data present. */ uint8_t *data; - struct ble_hs_adv_fields *fields; /*** * LE direct advertising report fields; direct_addr_type is diff --git a/nimble/host/include/host/ble_hs_adv.h b/nimble/host/include/host/ble_hs_adv.h index d15c8ee79..538fc7dac 100644 --- a/nimble/host/include/host/ble_hs_adv.h +++ b/nimble/host/include/host/ble_hs_adv.h @@ -31,6 +31,15 @@ extern "C" { /** Max field payload size (account for 2-byte header). */ #define BLE_HS_ADV_MAX_FIELD_SZ (BLE_HS_ADV_MAX_SZ - 2) +struct ble_hs_adv_field { + uint8_t length; + uint8_t type; + uint8_t value[]; +}; + +typedef int (* ble_hs_adv_parse_func_t) (const struct ble_hs_adv_field *, + void *); + struct ble_hs_adv_fields { /*** 0x01 - Flags. */ uint8_t flags; @@ -148,6 +157,12 @@ struct ble_hs_adv_fields { int ble_hs_adv_set_fields(const struct ble_hs_adv_fields *adv_fields, uint8_t *dst, uint8_t *dst_len, uint8_t max_len); +int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src, + uint8_t src_len); + +int ble_hs_adv_parse(const uint8_t *data, uint8_t length, + ble_hs_adv_parse_func_t func, void *user_data); + #ifdef __cplusplus } #endif diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 8d46aa433..3b9892c1c 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -1056,7 +1056,7 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) return; #endif - struct ble_hs_adv_fields fields; + const struct ble_hs_adv_field *flags; int rc; STATS_INC(ble_gap_stats, rx_adv_report); @@ -1065,22 +1065,18 @@ ble_gap_rx_adv_report(struct ble_gap_disc_desc *desc) return; } - rc = ble_hs_adv_parse_fields(&fields, desc->data, desc->length_data); - if (rc != 0) { - /* XXX: Increment stat. */ - return; - } - /* If a limited discovery procedure is active, discard non-limited * advertisements. */ - if (ble_gap_master.disc.limited && - !(fields.flags & BLE_HS_ADV_F_DISC_LTD)) { - - return; + if (ble_gap_master.disc.limited) { + rc = ble_hs_adv_find_field(BLE_HS_ADV_TYPE_FLAGS, desc->data, + desc->length_data, &flags); + if ((rc == 0) && (flags->length == 2) && + !(flags->value[0] & BLE_HS_ADV_F_DISC_LTD)) { + return; + } } - desc->fields = &fields; ble_gap_disc_report(desc); } diff --git a/nimble/host/src/ble_hs_adv.c b/nimble/host/src/ble_hs_adv.c index 1d658174c..37236c8cd 100644 --- a/nimble/host/src/ble_hs_adv.c +++ b/nimble/host/src/ble_hs_adv.c @@ -23,6 +23,11 @@ #include "host/ble_hs_adv.h" #include "ble_hs_priv.h" +struct find_field_data { + uint8_t type; + const struct ble_hs_adv_field *field; +}; + static uint16_t ble_hs_adv_uuids16[BLE_HS_ADV_MAX_FIELD_SZ / 2]; static uint32_t ble_hs_adv_uuids32[BLE_HS_ADV_MAX_FIELD_SZ / 4]; @@ -561,3 +566,65 @@ ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src, return 0; } + +int +ble_hs_adv_parse(const uint8_t *data, uint8_t length, + ble_hs_adv_parse_func_t func, void *user_data) +{ + const struct ble_hs_adv_field *field; + + while (length > 1) { + field = (const void *) data; + + if (field->length >= length) { + return BLE_HS_EBADDATA; + } + + if (func(field, user_data) == 0) { + return 0; + } + + length -= 1 + field->length; + length += 1 + field->length; + } + + return 0; +} + +static int +find_field_func(const struct ble_hs_adv_field *field, void *user_data) +{ + struct find_field_data *ffd = user_data; + + if (field->type != ffd->type) { + return BLE_HS_EAGAIN; + } + + ffd->field = field; + + return 0; +} + +int +ble_hs_adv_find_field(uint8_t type, const uint8_t *data, uint8_t length, + const struct ble_hs_adv_field **out) +{ + int rc; + struct find_field_data ffd = { + .type = type, + .field = NULL, + }; + + rc = ble_hs_adv_parse(data, length, find_field_func, &ffd); + if (rc != 0) { + return rc; + } + + if (!ffd.field) { + return BLE_HS_ENOENT; + } + + *out = ffd.field; + + return 0; +} diff --git a/nimble/host/src/ble_hs_adv_priv.h b/nimble/host/src/ble_hs_adv_priv.h index f913b0903..5c8a6ecc9 100644 --- a/nimble/host/src/ble_hs_adv_priv.h +++ b/nimble/host/src/ble_hs_adv_priv.h @@ -26,8 +26,8 @@ extern "C" { int ble_hs_adv_set_flat(uint8_t type, int data_len, const void *data, uint8_t *dst, uint8_t *dst_len, uint8_t max_len); -int ble_hs_adv_parse_fields(struct ble_hs_adv_fields *adv_fields, uint8_t *src, - uint8_t src_len); +int ble_hs_adv_find_field(uint8_t type, const uint8_t *data, uint8_t length, + const struct ble_hs_adv_field **out); #ifdef __cplusplus } diff --git a/nimble/host/src/ble_hs_hci_evt.c b/nimble/host/src/ble_hs_hci_evt.c index 546e38bd3..4305124be 100644 --- a/nimble/host/src/ble_hs_hci_evt.c +++ b/nimble/host/src/ble_hs_hci_evt.c @@ -434,7 +434,6 @@ ble_hs_hci_evt_le_dir_adv_rpt(uint8_t subevent, uint8_t *data, int len) /* Data fields not present in a direct advertising report. */ desc.data = NULL; - desc.fields = NULL; for (i = 0; i < num_reports; i++) { suboff = 0; diff --git a/nimble/host/test/src/ble_gap_test.c b/nimble/host/test/src/ble_gap_test.c index 7aaebeeed..a13640e6b 100644 --- a/nimble/host/test/src/ble_gap_test.c +++ b/nimble/host/test/src/ble_gap_test.c @@ -630,7 +630,7 @@ TEST_CASE(ble_gap_test_case_disc_ltd_mismatch) struct ble_gap_disc_desc desc = { .event_type = BLE_HCI_ADV_TYPE_ADV_IND, .addr_type = BLE_ADDR_TYPE_PUBLIC, - .length_data = 0, + .length_data = 3, .rssi = 0, .addr = { 1, 2, 3, 4, 5, 6 }, .data = (uint8_t[BLE_HS_ADV_MAX_SZ]){