Commit 01ce70b0 authored by Luiz Augusto von Dentz's avatar Luiz Augusto von Dentz Committed by Marcel Holtmann
Browse files

Bluetooth: eir: Move EIR/Adv Data functions to its own file 



This moves functions manipulating EIR/Adv Data to its own file so it
can be reused by other files.

Signed-off-by: default avatarLuiz Augusto von Dentz <luiz.von.dentz@intel.com>
Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
parent 81be03e0

include/net/bluetooth/hci_core.h

+2 −37
Original line number Diff line number Diff line
@@ -1380,6 +1380,8 @@ int hci_set_adv_instance_data(struct hci_dev *hdev, u8 instance, 
			 u16 scan_rsp_len, u8 *scan_rsp_data);

int hci_remove_adv_instance(struct hci_dev *hdev, u8 instance);

void hci_adv_instances_set_rpa_expired(struct hci_dev *hdev, bool rpa_expired);

u32 hci_adv_instance_flags(struct hci_dev *hdev, u8 instance);

bool hci_adv_instance_is_scannable(struct hci_dev *hdev, u8 instance);



void hci_adv_monitors_clear(struct hci_dev *hdev);

void hci_free_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor);
@@ -1632,43 +1634,6 @@ static inline void hci_role_switch_cfm(struct hci_conn *conn, __u8 status, 
	mutex_unlock(&hci_cb_list_lock);

}



static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type,

				 size_t *data_len)

{

	size_t parsed = 0;



	if (eir_len < 2)

		return NULL;



	while (parsed < eir_len - 1) {

		u8 field_len = eir[0];



		if (field_len == 0)

			break;



		parsed += field_len + 1;



		if (parsed > eir_len)

			break;



		if (eir[1] != type) {

			eir += field_len + 1;

			continue;

		}



		/* Zero length data */

		if (field_len == 1)

			return NULL;



		if (data_len)

			*data_len = field_len - 1;



		return &eir[2];

	}



	return NULL;

}



static inline bool hci_bdaddr_is_rpa(bdaddr_t *bdaddr, u8 addr_type)

{

	if (addr_type != ADDR_LE_DEV_RANDOM)

net/bluetooth/Makefile

+2 −1
Original line number Diff line number Diff line
@@ -14,7 +14,8 @@ bluetooth_6lowpan-y := 6lowpan.o 


bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \

	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o lib.o \

	ecdh_helper.o hci_request.o mgmt_util.o mgmt_config.o hci_codec.o

	ecdh_helper.o hci_request.o mgmt_util.o mgmt_config.o hci_codec.o \

	eir.o



bluetooth-$(CONFIG_BT_BREDR) += sco.o

bluetooth-$(CONFIG_BT_HS) += a2mp.o amp.o

net/bluetooth/eir.c

0 → 100644
+335 −0
Original line number Diff line number Diff line 
// 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);

}



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;

}

net/bluetooth/eir.h

0 → 100644
+72 −0
Original line number Diff line number Diff line 
/* SPDX-License-Identifier: GPL-2.0 */

/*

 * BlueZ - Bluetooth protocol stack for Linux

 *

 * Copyright (C) 2021 Intel Corporation

 */



void eir_create(struct hci_dev *hdev, u8 *data);



u8 eir_create_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr);

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);



static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type,

				  u8 *data, u8 data_len)

{

	eir[eir_len++] = sizeof(type) + data_len;

	eir[eir_len++] = type;

	memcpy(&eir[eir_len], data, data_len);

	eir_len += data_len;



	return eir_len;

}



static inline u16 eir_append_le16(u8 *eir, u16 eir_len, u8 type, u16 data)

{

	eir[eir_len++] = sizeof(type) + sizeof(data);

	eir[eir_len++] = type;

	put_unaligned_le16(data, &eir[eir_len]);

	eir_len += sizeof(data);



	return eir_len;

}



static inline void *eir_get_data(u8 *eir, size_t eir_len, u8 type,

				 size_t *data_len)

{

	size_t parsed = 0;



	if (eir_len < 2)

		return NULL;



	while (parsed < eir_len - 1) {

		u8 field_len = eir[0];



		if (field_len == 0)

			break;



		parsed += field_len + 1;



		if (parsed > eir_len)

			break;



		if (eir[1] != type) {

			eir += field_len + 1;

			continue;

		}



		/* Zero length data */

		if (field_len == 1)

			return NULL;



		if (data_len)

			*data_len = field_len - 1;



		return &eir[2];

	}



	return NULL;

}

net/bluetooth/hci_core.c

+54 −0
Original line number Diff line number Diff line
@@ -3084,6 +3084,60 @@ int hci_set_adv_instance_data(struct hci_dev *hdev, u8 instance, 
	return 0;

}



/* This function requires the caller holds hdev->lock */

u32 hci_adv_instance_flags(struct hci_dev *hdev, u8 instance)

{

	u32 flags;

	struct adv_info *adv;



	if (instance == 0x00) {

		/* Instance 0 always manages the "Tx Power" and "Flags"

		 * fields

		 */

		flags = MGMT_ADV_FLAG_TX_POWER | MGMT_ADV_FLAG_MANAGED_FLAGS;



		/* For instance 0, the HCI_ADVERTISING_CONNECTABLE setting

		 * corresponds to the "connectable" instance flag.

		 */

		if (hci_dev_test_flag(hdev, HCI_ADVERTISING_CONNECTABLE))

			flags |= MGMT_ADV_FLAG_CONNECTABLE;



		if (hci_dev_test_flag(hdev, HCI_LIMITED_DISCOVERABLE))

			flags |= MGMT_ADV_FLAG_LIMITED_DISCOV;

		else if (hci_dev_test_flag(hdev, HCI_DISCOVERABLE))

			flags |= MGMT_ADV_FLAG_DISCOV;



		return flags;

	}



	adv = hci_find_adv_instance(hdev, instance);



	/* Return 0 when we got an invalid instance identifier. */

	if (!adv)

		return 0;



	return adv->flags;

}



bool hci_adv_instance_is_scannable(struct hci_dev *hdev, u8 instance)

{

	struct adv_info *adv;



	/* Instance 0x00 always set local name */

	if (instance == 0x00)

		return true;



	adv = hci_find_adv_instance(hdev, instance);

	if (!adv)

		return false;



	if (adv->flags & MGMT_ADV_FLAG_APPEARANCE ||

	    adv->flags & MGMT_ADV_FLAG_LOCAL_NAME)

		return true;



	return adv->scan_rsp_len ? true : false;

}



/* This function requires the caller holds hdev->lock */

void hci_adv_monitors_clear(struct hci_dev *hdev)

{