Bluetooth: Add support for instance scan response (4117ed70) · Commits · 戴 / test

net/bluetooth/mgmt.c

+52 −14

Original line number	Diff line number	Diff line
		@@ -793,7 +793,7 @@ static struct mgmt_pending_cmd *pending_find_data(u16 opcode,
		return mgmt_pending_find_data(HCI_CHANNEL_CONTROL, opcode, hdev, data);
		}

		static u8 create_scan_rsp_data(struct hci_dev hdev, u8 ptr)
		static u8 create_default_scan_rsp_data(struct hci_dev hdev, u8 ptr)
		{
		u8 ad_len = 0;
		size_t name_len;
		@@ -819,7 +819,19 @@ static u8 create_scan_rsp_data(struct hci_dev hdev, u8 ptr)
		return ad_len;
		}

		static void update_scan_rsp_data(struct hci_request *req)
		static u8 create_instance_scan_rsp_data(struct hci_dev hdev, u8 ptr)
		{
		/* TODO: Set the appropriate entries based on advertising instance flags
		* here once flags other than 0 are supported.
		*/
		memcpy(ptr, hdev->adv_instance.scan_rsp_data,
		hdev->adv_instance.scan_rsp_len);

		return hdev->adv_instance.scan_rsp_len;
		}

		static void update_scan_rsp_data_for_instance(struct hci_request *req,
		u8 instance)
		{
		struct hci_dev *hdev = req->hdev;
		struct hci_cp_le_set_scan_rsp_data cp;
		@@ -830,10 +842,13 @@ static void update_scan_rsp_data(struct hci_request *req)

		memset(&cp, 0, sizeof(cp));

		len = create_scan_rsp_data(hdev, cp.data);
		if (instance)
		len = create_instance_scan_rsp_data(hdev, cp.data);
		else
		len = create_default_scan_rsp_data(hdev, cp.data);

		if (hdev->scan_rsp_data_len == len &&
		memcmp(cp.data, hdev->scan_rsp_data, len) == 0)
		!memcmp(cp.data, hdev->scan_rsp_data, len))
		return;

		memcpy(hdev->scan_rsp_data, cp.data, sizeof(cp.data));
		@@ -844,6 +859,25 @@ static void update_scan_rsp_data(struct hci_request *req)
		hci_req_add(req, HCI_OP_LE_SET_SCAN_RSP_DATA, sizeof(cp), &cp);
		}

		static void update_scan_rsp_data(struct hci_request *req)
		{
		struct hci_dev *hdev = req->hdev;
		u8 instance;

		/* The "Set Advertising" setting supersedes the "Add Advertising"
		* setting. Here we set the scan response data based on which
		* setting was set. When neither apply, default to the global settings,
		* represented by instance "0".
		*/
		if (hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE) &&
		!hci_dev_test_flag(hdev, HCI_ADVERTISING))
		instance = 0x01;
		else
		instance = 0x00;

		update_scan_rsp_data_for_instance(req, instance);
		}

		static u8 get_adv_discov_flags(struct hci_dev *hdev)
		{
		struct mgmt_pending_cmd *cmd;
		@@ -4547,6 +4581,7 @@ static int set_advertising(struct sock sk, struct hci_dev hdev, void *data,
		if (val) {
		/* Switch to instance "0" for the Set Advertising setting. */
		update_adv_data_for_instance(&req, 0);
		update_scan_rsp_data_for_instance(&req, 0);
		enable_advertising(&req);
		} else {
		disable_advertising(&req);
		@@ -6408,25 +6443,25 @@ static int read_adv_features(struct sock sk, struct hci_dev hdev,
		return err;
		}

		static bool adv_data_is_valid(struct hci_dev hdev, u32 adv_flags, u8 adv_data,
		u8 adv_data_len)
		static bool tlv_data_is_valid(struct hci_dev hdev, u32 adv_flags, u8 data,
		u8 len)
		{
		u8 max_adv_len = HCI_MAX_AD_LENGTH;
		u8 max_len = HCI_MAX_AD_LENGTH;
		int i, cur_len;

		/* TODO: Correctly reduce adv_len based on adv_flags. */
		/* TODO: Correctly reduce len based on adv_flags. */

		if (adv_data_len > max_adv_len)
		if (len > max_len)
		return false;

		/* Make sure that adv_data is correctly formatted. */
		for (i = 0, cur_len = 0; i < adv_data_len; i += (cur_len + 1)) {
		cur_len = adv_data[i];
		/* Make sure that the data is correctly formatted. */
		for (i = 0, cur_len = 0; i < len; i += (cur_len + 1)) {
		cur_len = data[i];

		/* If the current field length would exceed the total data
		* length, then it's invalid.
		*/
		if (i + cur_len >= adv_data_len)
		if (i + cur_len >= len)
		return false;
		}

		@@ -6526,7 +6561,9 @@ static int add_advertising(struct sock sk, struct hci_dev hdev,
		goto unlock;
		}

		if (!adv_data_is_valid(hdev, flags, cp->data, cp->adv_data_len)) {
		if (!tlv_data_is_valid(hdev, flags, cp->data, cp->adv_data_len) \|\|
		!tlv_data_is_valid(hdev, flags, cp->data + cp->adv_data_len,
		cp->scan_rsp_len)) {
		err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING,
		MGMT_STATUS_INVALID_PARAMS);
		goto unlock;
		@@ -6570,6 +6607,7 @@ static int add_advertising(struct sock sk, struct hci_dev hdev,
		hci_req_init(&req, hdev);

		update_adv_data(&req);
		update_scan_rsp_data(&req);
		enable_advertising(&req);

		err = hci_req_run(&req, add_advertising_complete);

Admin message