[PATCH v2 1/8] Bluetooth: eir: Add helpers for managing service data
From: Luiz Augusto von Dentz <luiz.dentz@gmail.com>
Date: 2022-05-06 21:57:50
Subsystem:
bluetooth subsystem, the rest · Maintainers:
Marcel Holtmann, Luiz Augusto von Dentz, Linus Torvalds
From: Luiz Augusto von Dentz <redacted> This adds helpers for accessing and appending service data (0x16) ad type. Signed-off-by: Luiz Augusto von Dentz <redacted> --- v2: Fixes CI/kernel test robot findings include/net/bluetooth/hci.h | 1 + net/bluetooth/eir.c | 31 +++++++++++++++++++++++++++++++ net/bluetooth/eir.h | 4 ++++ 3 files changed, 36 insertions(+)
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 62a9bb022aed..fe7935be7dc4 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h@@ -625,6 +625,7 @@ enum { #define EIR_SSP_RAND_R192 0x0F /* Simple Pairing Randomizer R-192 */ #define EIR_DEVICE_ID 0x10 /* device ID */ #define EIR_APPEARANCE 0x19 /* Device appearance */ +#define EIR_SERVICE_DATA 0x16 /* Service Data */ #define EIR_LE_BDADDR 0x1B /* LE Bluetooth device address */ #define EIR_LE_ROLE 0x1C /* LE role */ #define EIR_SSP_HASH_C256 0x1D /* Simple Pairing Hash C-256 */
diff --git a/net/bluetooth/eir.c b/net/bluetooth/eir.c
index 7e930f77ecab..7d77fb00c2bf 100644
--- a/net/bluetooth/eir.c
+++ b/net/bluetooth/eir.c@@ -55,6 +55,19 @@ u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len) return eir_append_le16(ptr, ad_len, EIR_APPEARANCE, hdev->appearance); } +u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data, + u8 data_len) +{ + eir[eir_len++] = sizeof(u8) + sizeof(uuid) + data_len; + eir[eir_len++] = EIR_SERVICE_DATA; + put_unaligned_le16(uuid, &eir[eir_len]); + eir_len += sizeof(uuid); + memcpy(&eir[eir_len], data, data_len); + eir_len += data_len; + + return eir_len; +} + static u8 *create_uuid16_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) { u8 *ptr = data, *uuids_start = NULL;
@@ -333,3 +346,21 @@ u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr) return scan_rsp_len; } + +void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len) +{ + while ((eir = eir_get_data(eir, eir_len, EIR_SERVICE_DATA, len))) { + u16 value = get_unaligned_le16(eir); + + if (uuid == value) { + if (len) + *len -= 2; + return &eir[2]; + } + + eir += *len; + eir_len -= *len; + } + + return NULL; +}
diff --git a/net/bluetooth/eir.h b/net/bluetooth/eir.h
index 43f1945bffc5..62f2374078f2 100644
--- a/net/bluetooth/eir.h
+++ b/net/bluetooth/eir.h@@ -14,6 +14,8 @@ u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr); u8 eir_append_local_name(struct hci_dev *hdev, u8 *eir, u8 ad_len); u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len); +u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data, + u8 data_len); static inline u16 eir_precalc_len(u8 data_len) {
@@ -92,3 +94,5 @@ static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type, return NULL; } + +void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len);
--
2.35.1