Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 66ea9427 authored by Jakub Pawlowski's avatar Jakub Pawlowski Committed by Johan Hedberg
Browse files

Bluetooth: Add support for Start Service Discovery command 



This patch adds support for the Start Service Discovery command. It
does all the checks for command parameters and configured the discovery
filter settings correctly. However the actual support for filtering
will be added with another patch.

Signed-off-by: default avatarJakub Pawlowski <jpawlowski@google.com>
Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
Signed-off-by: default avatarJohan Hedberg <johan.hedberg@intel.com>
parent 799ce93d

net/bluetooth/mgmt.c

+106 −0
Original line number Diff line number Diff line
@@ -93,6 +93,7 @@ static const u16 mgmt_commands[] = { 
	MGMT_OP_READ_CONFIG_INFO,

	MGMT_OP_SET_EXTERNAL_CONFIG,

	MGMT_OP_SET_PUBLIC_ADDRESS,

	MGMT_OP_START_SERVICE_DISCOVERY,

};



static const u16 mgmt_events[] = {
@@ -3793,6 +3794,9 @@ static void start_discovery_complete(struct hci_dev *hdev, u8 status) 
	hci_dev_lock(hdev);



	cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev);

	if (!cmd)

		cmd = mgmt_pending_find(MGMT_OP_START_SERVICE_DISCOVERY, hdev);



	if (cmd) {

		u8 type = hdev->discovery.type;
@@ -3892,6 +3896,107 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev, 
	return err;

}



static int start_service_discovery(struct sock *sk, struct hci_dev *hdev,

				   void *data, u16 len)

{

	struct mgmt_cp_start_service_discovery *cp = data;

	struct pending_cmd *cmd;

	struct hci_request req;

	const u16 max_uuid_count = ((U16_MAX - sizeof(*cp)) / 16);

	u16 uuid_count, expected_len;

	u8 status;

	int err;



	BT_DBG("%s", hdev->name);



	hci_dev_lock(hdev);



	if (!hdev_is_powered(hdev)) {

		err = cmd_complete(sk, hdev->id,

				   MGMT_OP_START_SERVICE_DISCOVERY,

				   MGMT_STATUS_NOT_POWERED,

				   &cp->type, sizeof(cp->type));

		goto failed;

	}



	if (hdev->discovery.state != DISCOVERY_STOPPED ||

	    test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {

		err = cmd_complete(sk, hdev->id,

				   MGMT_OP_START_SERVICE_DISCOVERY,

				   MGMT_STATUS_BUSY, &cp->type,

				   sizeof(cp->type));

		goto failed;

	}



	uuid_count = __le16_to_cpu(cp->uuid_count);

	if (uuid_count > max_uuid_count) {

		BT_ERR("service_discovery: too big uuid_count value %u",

		       uuid_count);

		err = cmd_complete(sk, hdev->id,

				   MGMT_OP_START_SERVICE_DISCOVERY,

				   MGMT_STATUS_INVALID_PARAMS, &cp->type,

				   sizeof(cp->type));

		goto failed;

	}



	expected_len = sizeof(*cp) + uuid_count * 16;

	if (expected_len != len) {

		BT_ERR("service_discovery: expected %u bytes, got %u bytes",

		       expected_len, len);

		err = cmd_complete(sk, hdev->id,

				   MGMT_OP_START_SERVICE_DISCOVERY,

				   MGMT_STATUS_INVALID_PARAMS, &cp->type,

				   sizeof(cp->type));

		goto failed;

	}



	cmd = mgmt_pending_add(sk, MGMT_OP_START_SERVICE_DISCOVERY,

			       hdev, NULL, 0);

	if (!cmd) {

		err = -ENOMEM;

		goto failed;

	}



	hdev->discovery.type = cp->type;

	hdev->discovery.rssi = cp->rssi;

	hdev->discovery.uuid_count = uuid_count;



	if (uuid_count > 0) {

		hdev->discovery.uuids = kmemdup(cp->uuids, uuid_count * 16,

						GFP_KERNEL);

		if (!hdev->discovery.uuids) {

			err = cmd_complete(sk, hdev->id,

					   MGMT_OP_START_SERVICE_DISCOVERY,

					   MGMT_STATUS_FAILED,

					   &cp->type, sizeof(cp->type));

			mgmt_pending_remove(cmd);

			goto failed;

		}

	}



	hci_req_init(&req, hdev);



	if (!trigger_discovery(&req, &status)) {

		err = cmd_complete(sk, hdev->id,

				   MGMT_OP_START_SERVICE_DISCOVERY,

				   status, &cp->type, sizeof(cp->type));

		mgmt_pending_remove(cmd);

		goto failed;

	}



	err = hci_req_run(&req, start_discovery_complete);

	if (err < 0) {

		mgmt_pending_remove(cmd);

		goto failed;

	}



	hci_discovery_set_state(hdev, DISCOVERY_STARTING);



failed:

	hci_dev_unlock(hdev);

	return err;

}



static void stop_discovery_complete(struct hci_dev *hdev, u8 status)

{

	struct pending_cmd *cmd;
@@ -5726,6 +5831,7 @@ static const struct mgmt_handler { 
	{ read_config_info,       false, MGMT_READ_CONFIG_INFO_SIZE },

	{ set_external_config,    false, MGMT_SET_EXTERNAL_CONFIG_SIZE },

	{ set_public_address,     false, MGMT_SET_PUBLIC_ADDRESS_SIZE },

	{ start_service_discovery,true,  MGMT_START_SERVICE_DISCOVERY_SIZE },

};



int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)