| // SPDX-License-Identifier: GPL-2.0 |
| /* |
| * BlueZ - Bluetooth protocol stack for Linux |
| * |
| * Copyright (C) 2021 Intel Corporation |
| */ |
| |
| #include <net/bluetooth/bluetooth.h> |
| #include <net/bluetooth/hci_core.h> |
| #include <net/bluetooth/mgmt.h> |
| |
| #include "eir.h" |
| |
| #define PNP_INFO_SVCLASS_ID 0x1200 |
| |
| u8 eir_append_local_name(struct hci_dev *hdev, u8 *ptr, u8 ad_len) |
| { |
| size_t short_len; |
| size_t complete_len; |
| |
| /* no space left for name (+ NULL + type + len) */ |
| if ((HCI_MAX_AD_LENGTH - ad_len) < HCI_MAX_SHORT_NAME_LENGTH + 3) |
| return ad_len; |
| |
| /* use complete name if present and fits */ |
| complete_len = strlen(hdev->dev_name); |
| if (complete_len && complete_len <= HCI_MAX_SHORT_NAME_LENGTH) |
| return eir_append_data(ptr, ad_len, EIR_NAME_COMPLETE, |
| hdev->dev_name, complete_len + 1); |
| |
| /* use short name if present */ |
| short_len = strlen(hdev->short_name); |
| if (short_len) |
| return eir_append_data(ptr, ad_len, EIR_NAME_SHORT, |
| hdev->short_name, short_len + 1); |
| |
| /* use shortened full name if present, we already know that name |
| * is longer then HCI_MAX_SHORT_NAME_LENGTH |
| */ |
| if (complete_len) { |
| u8 name[HCI_MAX_SHORT_NAME_LENGTH + 1]; |
| |
| memcpy(name, hdev->dev_name, HCI_MAX_SHORT_NAME_LENGTH); |
| name[HCI_MAX_SHORT_NAME_LENGTH] = '\0'; |
| |
| return eir_append_data(ptr, ad_len, EIR_NAME_SHORT, name, |
| sizeof(name)); |
| } |
| |
| return ad_len; |
| } |
| |
| 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; |
| 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; |
| } |
| |
| void eir_create(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)); |
| } |
| |
| u8 eir_create_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr) |
| { |
| struct adv_info *adv = NULL; |
| u8 ad_len = 0, flags = 0; |
| u32 instance_flags; |
| |
| /* Return 0 when the current instance identifier is invalid. */ |
| if (instance) { |
| adv = hci_find_adv_instance(hdev, instance); |
| if (!adv) |
| return 0; |
| } |
| |
| instance_flags = hci_adv_instance_flags(hdev, instance); |
| |
| /* If instance already has the flags set skip adding it once |
| * again. |
| */ |
| if (adv && eir_get_data(adv->adv_data, adv->adv_data_len, EIR_FLAGS, |
| NULL)) |
| goto skip_flags; |
| |
| /* The Add Advertising command allows userspace to set both the general |
| * and limited discoverable flags. |
| */ |
| if (instance_flags & MGMT_ADV_FLAG_DISCOV) |
| flags |= LE_AD_GENERAL; |
| |
| if (instance_flags & MGMT_ADV_FLAG_LIMITED_DISCOV) |
| flags |= LE_AD_LIMITED; |
| |
| if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) |
| flags |= LE_AD_NO_BREDR; |
| |
| if (flags || (instance_flags & MGMT_ADV_FLAG_MANAGED_FLAGS)) { |
| /* If a discovery flag wasn't provided, simply use the global |
| * settings. |
| */ |
| if (!flags) |
| flags |= mgmt_get_adv_discov_flags(hdev); |
| |
| /* If flags would still be empty, then there is no need to |
| * include the "Flags" AD field". |
| */ |
| if (flags) { |
| ptr[0] = 0x02; |
| ptr[1] = EIR_FLAGS; |
| ptr[2] = flags; |
| |
| ad_len += 3; |
| ptr += 3; |
| } |
| } |
| |
| skip_flags: |
| if (adv) { |
| memcpy(ptr, adv->adv_data, adv->adv_data_len); |
| ad_len += adv->adv_data_len; |
| ptr += adv->adv_data_len; |
| } |
| |
| if (instance_flags & MGMT_ADV_FLAG_TX_POWER) { |
| s8 adv_tx_power; |
| |
| if (ext_adv_capable(hdev)) { |
| if (adv) |
| adv_tx_power = adv->tx_power; |
| else |
| adv_tx_power = hdev->adv_tx_power; |
| } else { |
| adv_tx_power = hdev->adv_tx_power; |
| } |
| |
| /* Provide Tx Power only if we can provide a valid value for it */ |
| if (adv_tx_power != HCI_TX_POWER_INVALID) { |
| ptr[0] = 0x02; |
| ptr[1] = EIR_TX_POWER; |
| ptr[2] = (u8)adv_tx_power; |
| |
| ad_len += 3; |
| ptr += 3; |
| } |
| } |
| |
| return ad_len; |
| } |
| |
| static u8 create_default_scan_rsp(struct hci_dev *hdev, u8 *ptr) |
| { |
| u8 scan_rsp_len = 0; |
| |
| if (hdev->appearance) |
| scan_rsp_len = eir_append_appearance(hdev, ptr, scan_rsp_len); |
| |
| return eir_append_local_name(hdev, ptr, scan_rsp_len); |
| } |
| |
| u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr) |
| { |
| struct adv_info *adv; |
| u8 scan_rsp_len = 0; |
| |
| if (!instance) |
| return create_default_scan_rsp(hdev, ptr); |
| |
| adv = hci_find_adv_instance(hdev, instance); |
| if (!adv) |
| return 0; |
| |
| if ((adv->flags & MGMT_ADV_FLAG_APPEARANCE) && hdev->appearance) |
| scan_rsp_len = eir_append_appearance(hdev, ptr, scan_rsp_len); |
| |
| memcpy(&ptr[scan_rsp_len], adv->scan_rsp_data, adv->scan_rsp_len); |
| |
| scan_rsp_len += adv->scan_rsp_len; |
| |
| if (adv->flags & MGMT_ADV_FLAG_LOCAL_NAME) |
| scan_rsp_len = eir_append_local_name(hdev, ptr, scan_rsp_len); |
| |
| 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; |
| } |