cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

eir.c (8072B)


      1// SPDX-License-Identifier: GPL-2.0
      2/*
      3 * BlueZ - Bluetooth protocol stack for Linux
      4 *
      5 * Copyright (C) 2021 Intel Corporation
      6 */
      7
      8#include <net/bluetooth/bluetooth.h>
      9#include <net/bluetooth/hci_core.h>
     10#include <net/bluetooth/mgmt.h>
     11
     12#include "eir.h"
     13
     14#define PNP_INFO_SVCLASS_ID		0x1200
     15
     16u8 eir_append_local_name(struct hci_dev *hdev, u8 *ptr, u8 ad_len)
     17{
     18	size_t short_len;
     19	size_t complete_len;
     20
     21	/* no space left for name (+ NULL + type + len) */
     22	if ((HCI_MAX_AD_LENGTH - ad_len) < HCI_MAX_SHORT_NAME_LENGTH + 3)
     23		return ad_len;
     24
     25	/* use complete name if present and fits */
     26	complete_len = strlen(hdev->dev_name);
     27	if (complete_len && complete_len <= HCI_MAX_SHORT_NAME_LENGTH)
     28		return eir_append_data(ptr, ad_len, EIR_NAME_COMPLETE,
     29				       hdev->dev_name, complete_len + 1);
     30
     31	/* use short name if present */
     32	short_len = strlen(hdev->short_name);
     33	if (short_len)
     34		return eir_append_data(ptr, ad_len, EIR_NAME_SHORT,
     35				       hdev->short_name, short_len + 1);
     36
     37	/* use shortened full name if present, we already know that name
     38	 * is longer then HCI_MAX_SHORT_NAME_LENGTH
     39	 */
     40	if (complete_len) {
     41		u8 name[HCI_MAX_SHORT_NAME_LENGTH + 1];
     42
     43		memcpy(name, hdev->dev_name, HCI_MAX_SHORT_NAME_LENGTH);
     44		name[HCI_MAX_SHORT_NAME_LENGTH] = '\0';
     45
     46		return eir_append_data(ptr, ad_len, EIR_NAME_SHORT, name,
     47				       sizeof(name));
     48	}
     49
     50	return ad_len;
     51}
     52
     53u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len)
     54{
     55	return eir_append_le16(ptr, ad_len, EIR_APPEARANCE, hdev->appearance);
     56}
     57
     58u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data,
     59			   u8 data_len)
     60{
     61	eir[eir_len++] = sizeof(u8) + sizeof(uuid) + data_len;
     62	eir[eir_len++] = EIR_SERVICE_DATA;
     63	put_unaligned_le16(uuid, &eir[eir_len]);
     64	eir_len += sizeof(uuid);
     65	memcpy(&eir[eir_len], data, data_len);
     66	eir_len += data_len;
     67
     68	return eir_len;
     69}
     70
     71static u8 *create_uuid16_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len)
     72{
     73	u8 *ptr = data, *uuids_start = NULL;
     74	struct bt_uuid *uuid;
     75
     76	if (len < 4)
     77		return ptr;
     78
     79	list_for_each_entry(uuid, &hdev->uuids, list) {
     80		u16 uuid16;
     81
     82		if (uuid->size != 16)
     83			continue;
     84
     85		uuid16 = get_unaligned_le16(&uuid->uuid[12]);
     86		if (uuid16 < 0x1100)
     87			continue;
     88
     89		if (uuid16 == PNP_INFO_SVCLASS_ID)
     90			continue;
     91
     92		if (!uuids_start) {
     93			uuids_start = ptr;
     94			uuids_start[0] = 1;
     95			uuids_start[1] = EIR_UUID16_ALL;
     96			ptr += 2;
     97		}
     98
     99		/* Stop if not enough space to put next UUID */
    100		if ((ptr - data) + sizeof(u16) > len) {
    101			uuids_start[1] = EIR_UUID16_SOME;
    102			break;
    103		}
    104
    105		*ptr++ = (uuid16 & 0x00ff);
    106		*ptr++ = (uuid16 & 0xff00) >> 8;
    107		uuids_start[0] += sizeof(uuid16);
    108	}
    109
    110	return ptr;
    111}
    112
    113static u8 *create_uuid32_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len)
    114{
    115	u8 *ptr = data, *uuids_start = NULL;
    116	struct bt_uuid *uuid;
    117
    118	if (len < 6)
    119		return ptr;
    120
    121	list_for_each_entry(uuid, &hdev->uuids, list) {
    122		if (uuid->size != 32)
    123			continue;
    124
    125		if (!uuids_start) {
    126			uuids_start = ptr;
    127			uuids_start[0] = 1;
    128			uuids_start[1] = EIR_UUID32_ALL;
    129			ptr += 2;
    130		}
    131
    132		/* Stop if not enough space to put next UUID */
    133		if ((ptr - data) + sizeof(u32) > len) {
    134			uuids_start[1] = EIR_UUID32_SOME;
    135			break;
    136		}
    137
    138		memcpy(ptr, &uuid->uuid[12], sizeof(u32));
    139		ptr += sizeof(u32);
    140		uuids_start[0] += sizeof(u32);
    141	}
    142
    143	return ptr;
    144}
    145
    146static u8 *create_uuid128_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len)
    147{
    148	u8 *ptr = data, *uuids_start = NULL;
    149	struct bt_uuid *uuid;
    150
    151	if (len < 18)
    152		return ptr;
    153
    154	list_for_each_entry(uuid, &hdev->uuids, list) {
    155		if (uuid->size != 128)
    156			continue;
    157
    158		if (!uuids_start) {
    159			uuids_start = ptr;
    160			uuids_start[0] = 1;
    161			uuids_start[1] = EIR_UUID128_ALL;
    162			ptr += 2;
    163		}
    164
    165		/* Stop if not enough space to put next UUID */
    166		if ((ptr - data) + 16 > len) {
    167			uuids_start[1] = EIR_UUID128_SOME;
    168			break;
    169		}
    170
    171		memcpy(ptr, uuid->uuid, 16);
    172		ptr += 16;
    173		uuids_start[0] += 16;
    174	}
    175
    176	return ptr;
    177}
    178
    179void eir_create(struct hci_dev *hdev, u8 *data)
    180{
    181	u8 *ptr = data;
    182	size_t name_len;
    183
    184	name_len = strlen(hdev->dev_name);
    185
    186	if (name_len > 0) {
    187		/* EIR Data type */
    188		if (name_len > 48) {
    189			name_len = 48;
    190			ptr[1] = EIR_NAME_SHORT;
    191		} else {
    192			ptr[1] = EIR_NAME_COMPLETE;
    193		}
    194
    195		/* EIR Data length */
    196		ptr[0] = name_len + 1;
    197
    198		memcpy(ptr + 2, hdev->dev_name, name_len);
    199
    200		ptr += (name_len + 2);
    201	}
    202
    203	if (hdev->inq_tx_power != HCI_TX_POWER_INVALID) {
    204		ptr[0] = 2;
    205		ptr[1] = EIR_TX_POWER;
    206		ptr[2] = (u8)hdev->inq_tx_power;
    207
    208		ptr += 3;
    209	}
    210
    211	if (hdev->devid_source > 0) {
    212		ptr[0] = 9;
    213		ptr[1] = EIR_DEVICE_ID;
    214
    215		put_unaligned_le16(hdev->devid_source, ptr + 2);
    216		put_unaligned_le16(hdev->devid_vendor, ptr + 4);
    217		put_unaligned_le16(hdev->devid_product, ptr + 6);
    218		put_unaligned_le16(hdev->devid_version, ptr + 8);
    219
    220		ptr += 10;
    221	}
    222
    223	ptr = create_uuid16_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data));
    224	ptr = create_uuid32_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data));
    225	ptr = create_uuid128_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data));
    226}
    227
    228u8 eir_create_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr)
    229{
    230	struct adv_info *adv = NULL;
    231	u8 ad_len = 0, flags = 0;
    232	u32 instance_flags;
    233
    234	/* Return 0 when the current instance identifier is invalid. */
    235	if (instance) {
    236		adv = hci_find_adv_instance(hdev, instance);
    237		if (!adv)
    238			return 0;
    239	}
    240
    241	instance_flags = hci_adv_instance_flags(hdev, instance);
    242
    243	/* If instance already has the flags set skip adding it once
    244	 * again.
    245	 */
    246	if (adv && eir_get_data(adv->adv_data, adv->adv_data_len, EIR_FLAGS,
    247				NULL))
    248		goto skip_flags;
    249
    250	/* The Add Advertising command allows userspace to set both the general
    251	 * and limited discoverable flags.
    252	 */
    253	if (instance_flags & MGMT_ADV_FLAG_DISCOV)
    254		flags |= LE_AD_GENERAL;
    255
    256	if (instance_flags & MGMT_ADV_FLAG_LIMITED_DISCOV)
    257		flags |= LE_AD_LIMITED;
    258
    259	if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED))
    260		flags |= LE_AD_NO_BREDR;
    261
    262	if (flags || (instance_flags & MGMT_ADV_FLAG_MANAGED_FLAGS)) {
    263		/* If a discovery flag wasn't provided, simply use the global
    264		 * settings.
    265		 */
    266		if (!flags)
    267			flags |= mgmt_get_adv_discov_flags(hdev);
    268
    269		/* If flags would still be empty, then there is no need to
    270		 * include the "Flags" AD field".
    271		 */
    272		if (flags) {
    273			ptr[0] = 0x02;
    274			ptr[1] = EIR_FLAGS;
    275			ptr[2] = flags;
    276
    277			ad_len += 3;
    278			ptr += 3;
    279		}
    280	}
    281
    282skip_flags:
    283	if (adv) {
    284		memcpy(ptr, adv->adv_data, adv->adv_data_len);
    285		ad_len += adv->adv_data_len;
    286		ptr += adv->adv_data_len;
    287	}
    288
    289	if (instance_flags & MGMT_ADV_FLAG_TX_POWER) {
    290		s8 adv_tx_power;
    291
    292		if (ext_adv_capable(hdev)) {
    293			if (adv)
    294				adv_tx_power = adv->tx_power;
    295			else
    296				adv_tx_power = hdev->adv_tx_power;
    297		} else {
    298			adv_tx_power = hdev->adv_tx_power;
    299		}
    300
    301		/* Provide Tx Power only if we can provide a valid value for it */
    302		if (adv_tx_power != HCI_TX_POWER_INVALID) {
    303			ptr[0] = 0x02;
    304			ptr[1] = EIR_TX_POWER;
    305			ptr[2] = (u8)adv_tx_power;
    306
    307			ad_len += 3;
    308			ptr += 3;
    309		}
    310	}
    311
    312	return ad_len;
    313}
    314
    315static u8 create_default_scan_rsp(struct hci_dev *hdev, u8 *ptr)
    316{
    317	u8 scan_rsp_len = 0;
    318
    319	if (hdev->appearance)
    320		scan_rsp_len = eir_append_appearance(hdev, ptr, scan_rsp_len);
    321
    322	return eir_append_local_name(hdev, ptr, scan_rsp_len);
    323}
    324
    325u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr)
    326{
    327	struct adv_info *adv;
    328	u8 scan_rsp_len = 0;
    329
    330	if (!instance)
    331		return create_default_scan_rsp(hdev, ptr);
    332
    333	adv = hci_find_adv_instance(hdev, instance);
    334	if (!adv)
    335		return 0;
    336
    337	if ((adv->flags & MGMT_ADV_FLAG_APPEARANCE) && hdev->appearance)
    338		scan_rsp_len = eir_append_appearance(hdev, ptr, scan_rsp_len);
    339
    340	memcpy(&ptr[scan_rsp_len], adv->scan_rsp_data, adv->scan_rsp_len);
    341
    342	scan_rsp_len += adv->scan_rsp_len;
    343
    344	if (adv->flags & MGMT_ADV_FLAG_LOCAL_NAME)
    345		scan_rsp_len = eir_append_local_name(hdev, ptr, scan_rsp_len);
    346
    347	return scan_rsp_len;
    348}
    349
    350void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len)
    351{
    352	while ((eir = eir_get_data(eir, eir_len, EIR_SERVICE_DATA, len))) {
    353		u16 value = get_unaligned_le16(eir);
    354
    355		if (uuid == value) {
    356			if (len)
    357				*len -= 2;
    358			return &eir[2];
    359		}
    360
    361		eir += *len;
    362		eir_len -= *len;
    363	}
    364
    365	return NULL;
    366}