Bluetooth: eir: Add helpers for managing service data

This adds helpers for accessing and appending service data (0x16) ad
type.

Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 62a9bb0..fe7935b 100644 (file)
--- 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 7e930f7..7d77fb0 100644 (file)
--- 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 43f1945..62f2374 100644 (file)
--- 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);