Bluetooth: Move EIR update to hci_request.c

We'll soon need to update the EIR both from hci_request.c and mgmt.c
so move update_eir() as a more generic request helper to
hci_request.c.

Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 189 insertions, 0 deletions

diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c
index 3150461c52a4..030a1bb66ef5 100644
--- a/net/bluetooth/hci_request.c
+++ b/net/bluetooth/hci_request.c
@@ -21,6 +21,8 @@
    SOFTWARE IS DISCLAIMED.
 */
 
+#include <asm/unaligned.h>
+
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
@@ -430,6 +432,193 @@ void __hci_req_update_name(struct hci_request *req)
         hci_req_add(req, HCI_OP_WRITE_LOCAL_NAME, sizeof(cp), &cp);
 }
 
+#define PNP_INFO_SVCLASS_ID             0x1200
+
+static u8 *create_uuid16_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len)
+{
+        u8 *ptr = data, *uuids_start = NULL;
+        struct bt_uuid *uuid;
+
+        if (len < 4)
+                return ptr;
+
+        list_for_each_entry(uuid, &hdev->uuids, list) {
+                u16 uuid16;
+
+                if (uuid->size != 16)
+                        continue;
+
+                uuid16 = get_unaligned_le16(&uuid->uuid[12]);
+                if (uuid16 < 0x1100)
+                        continue;
+
+                if (uuid16 == PNP_INFO_SVCLASS_ID)
+                        continue;
+
+                if (!uuids_start) {
+                        uuids_start = ptr;
+                        uuids_start[0] = 1;
+                        uuids_start[1] = EIR_UUID16_ALL;
+                        ptr += 2;
+                }
+
+                /* Stop if not enough space to put next UUID */
+                if ((ptr - data) + sizeof(u16) > len) {
+                        uuids_start[1] = EIR_UUID16_SOME;
+                        break;
+                }
+
+                *ptr++ = (uuid16 & 0x00ff);
+                *ptr++ = (uuid16 & 0xff00) >> 8;
+                uuids_start[0] += sizeof(uuid16);
+        }
+
+        return ptr;
+}
+
+static u8 *create_uuid32_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len)
+{
+        u8 *ptr = data, *uuids_start = NULL;
+        struct bt_uuid *uuid;
+
+        if (len < 6)
+                return ptr;
+
+        list_for_each_entry(uuid, &hdev->uuids, list) {
+                if (uuid->size != 32)
+                        continue;
+
+                if (!uuids_start) {
+                        uuids_start = ptr;
+                        uuids_start[0] = 1;
+                        uuids_start[1] = EIR_UUID32_ALL;
+                        ptr += 2;
+                }
+
+                /* Stop if not enough space to put next UUID */
+                if ((ptr - data) + sizeof(u32) > len) {
+                        uuids_start[1] = EIR_UUID32_SOME;
+                        break;
+                }
+
+                memcpy(ptr, &uuid->uuid[12], sizeof(u32));
+                ptr += sizeof(u32);
+                uuids_start[0] += sizeof(u32);
+        }
+
+        return ptr;
+}
+
+static u8 *create_uuid128_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len)
+{
+        u8 *ptr = data, *uuids_start = NULL;
+        struct bt_uuid *uuid;
+
+        if (len < 18)
+                return ptr;
+
+        list_for_each_entry(uuid, &hdev->uuids, list) {
+                if (uuid->size != 128)
+                        continue;
+
+                if (!uuids_start) {
+                        uuids_start = ptr;
+                        uuids_start[0] = 1;
+                        uuids_start[1] = EIR_UUID128_ALL;
+                        ptr += 2;
+                }
+
+                /* Stop if not enough space to put next UUID */
+                if ((ptr - data) + 16 > len) {
+                        uuids_start[1] = EIR_UUID128_SOME;
+                        break;
+                }
+
+                memcpy(ptr, uuid->uuid, 16);
+                ptr += 16;
+                uuids_start[0] += 16;
+        }
+
+        return ptr;
+}
+
+static void create_eir(struct hci_dev *hdev, u8 *data)
+{
+        u8 *ptr = data;
+        size_t name_len;
+
+        name_len = strlen(hdev->dev_name);
+
+        if (name_len > 0) {
+                /* EIR Data type */
+                if (name_len > 48) {
+                        name_len = 48;
+                        ptr[1] = EIR_NAME_SHORT;
+                } else
+                        ptr[1] = EIR_NAME_COMPLETE;
+
+                /* EIR Data length */
+                ptr[0] = name_len + 1;
+
+                memcpy(ptr + 2, hdev->dev_name, name_len);
+
+                ptr += (name_len + 2);
+        }
+
+        if (hdev->inq_tx_power != HCI_TX_POWER_INVALID) {
+                ptr[0] = 2;
+                ptr[1] = EIR_TX_POWER;
+                ptr[2] = (u8) hdev->inq_tx_power;
+
+                ptr += 3;
+        }
+
+        if (hdev->devid_source > 0) {
+                ptr[0] = 9;
+                ptr[1] = EIR_DEVICE_ID;
+
+                put_unaligned_le16(hdev->devid_source, ptr + 2);
+                put_unaligned_le16(hdev->devid_vendor, ptr + 4);
+                put_unaligned_le16(hdev->devid_product, ptr + 6);
+                put_unaligned_le16(hdev->devid_version, ptr + 8);
+
+                ptr += 10;
+        }
+
+        ptr = create_uuid16_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data));
+        ptr = create_uuid32_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data));
+        ptr = create_uuid128_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data));
+}
+
+void __hci_req_update_eir(struct hci_request *req)
+{
+        struct hci_dev *hdev = req->hdev;
+        struct hci_cp_write_eir cp;
+
+        if (!hdev_is_powered(hdev))
+                return;
+
+        if (!lmp_ext_inq_capable(hdev))
+                return;
+
+        if (!hci_dev_test_flag(hdev, HCI_SSP_ENABLED))
+                return;
+
+        if (hci_dev_test_flag(hdev, HCI_SERVICE_CACHE))
+                return;
+
+        memset(&cp, 0, sizeof(cp));
+
+        create_eir(hdev, cp.data);
+
+        if (memcmp(cp.data, hdev->eir, sizeof(cp.data)) == 0)
+                return;
+
+        memcpy(hdev->eir, cp.data, sizeof(cp.data));
+
+        hci_req_add(req, HCI_OP_WRITE_EIR, sizeof(cp), &cp);
+}
+
 void hci_req_add_le_scan_disable(struct hci_request *req)
 {
         struct hci_cp_le_set_scan_enable cp;