Currently there is no way to restart le scan. It's needed in
preparation for new service scan method. The way it work: it disable,
and then enable le scan on controller. During this restart special flag
is set to make sure we won't remove disable scan work from workqueue.
Signed-off-by: Jakub Pawlowski <[email protected]>
---
include/net/bluetooth/hci.h | 1 +
include/net/bluetooth/hci_core.h | 3 +++
net/bluetooth/hci_core.c | 43 ++++++++++++++++++++++++++++++++++++++++
net/bluetooth/hci_event.c | 9 ++++++---
4 files changed, 53 insertions(+), 3 deletions(-)
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index e56f909..2f0bce2 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -203,6 +203,7 @@ enum {
HCI_FAST_CONNECTABLE,
HCI_BREDR_ENABLED,
HCI_LE_SCAN_INTERRUPTED,
+ HCI_LE_SCAN_RESTARTING,
};
/* A mask for the flags that are supposed to remain when a reset happens
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 396c098..6a022a4 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -337,6 +337,7 @@ struct hci_dev {
unsigned long dev_flags;
struct delayed_work le_scan_disable;
+ struct delayed_work le_scan_restart;
__s8 adv_tx_power;
__u8 adv_data[HCI_MAX_AD_LENGTH];
@@ -1326,6 +1327,7 @@ void hci_sock_dev_event(struct hci_dev *hdev, int event);
#define DISCOV_INTERLEAVED_TIMEOUT 5120 /* msec */
#define DISCOV_INTERLEAVED_INQUIRY_LEN 0x04
#define DISCOV_BREDR_INQUIRY_LEN 0x08
+#define DISCOV_LE_RESTART_DELAY 300 /* msec */
int mgmt_control(struct sock *sk, struct msghdr *msg, size_t len);
int mgmt_new_settings(struct hci_dev *hdev);
@@ -1403,6 +1405,7 @@ int hci_update_random_address(struct hci_request *req, bool require_privacy,
u8 *own_addr_type);
void hci_copy_identity_address(struct hci_dev *hdev, bdaddr_t *bdaddr,
u8 *bdaddr_type);
+void hci_restart_le_scan(struct hci_dev *hdev);
#define SCO_AIRMODE_MASK 0x0003
#define SCO_AIRMODE_CVSD 0x0000
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 5c319a4..1c3d6e6 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2557,6 +2557,7 @@ static int hci_dev_do_close(struct hci_dev *hdev)
cancel_delayed_work(&hdev->service_cache);
cancel_delayed_work_sync(&hdev->le_scan_disable);
+ cancel_delayed_work_sync(&hdev->le_scan_restart);
if (test_bit(HCI_MGMT, &hdev->dev_flags))
cancel_delayed_work_sync(&hdev->rpa_expired);
@@ -3851,6 +3852,8 @@ static void le_scan_disable_work(struct work_struct *work)
BT_DBG("%s", hdev->name);
+ cancel_delayed_work_sync(&hdev->le_scan_restart);
+
hci_req_init(&req, hdev);
hci_req_add_le_scan_disable(&req);
@@ -3860,6 +3863,45 @@ static void le_scan_disable_work(struct work_struct *work)
BT_ERR("Disable LE scanning request failed: err %d", err);
}
+void hci_restart_le_scan(struct hci_dev *hdev)
+{
+ queue_delayed_work(hdev->workqueue, &hdev->le_scan_restart,
+ msecs_to_jiffies(DISCOV_LE_RESTART_DELAY));
+}
+
+static void le_scan_restart_work_complete(struct hci_dev *hdev, u8 status)
+{
+ clear_bit(HCI_LE_SCAN_RESTARTING, &hdev->dev_flags);
+
+ if (status)
+ BT_ERR("Failed to restart LE scanning: status %d", status);
+}
+
+static void le_scan_restart_work(struct work_struct *work)
+{
+ struct hci_dev *hdev = container_of(work, struct hci_dev,
+ le_scan_restart.work);
+ struct hci_request req;
+ struct hci_cp_le_set_scan_enable cp;
+ int err;
+
+ BT_DBG("%s", hdev->name);
+
+ hci_req_init(&req, hdev);
+
+ hci_req_add_le_scan_disable(&req);
+
+ memset(&cp, 0, sizeof(cp));
+ cp.enable = LE_SCAN_ENABLE;
+ hci_req_add(&req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp);
+
+ set_bit(HCI_LE_SCAN_RESTARTING, &hdev->dev_flags);
+
+ err = hci_req_run(&req, le_scan_restart_work_complete);
+ if (err)
+ BT_ERR("Disable LE scanning request failed: err %d", err);
+}
+
static void set_random_addr(struct hci_request *req, bdaddr_t *rpa)
{
struct hci_dev *hdev = req->hdev;
@@ -4037,6 +4079,7 @@ struct hci_dev *hci_alloc_dev(void)
INIT_DELAYED_WORK(&hdev->power_off, hci_power_off);
INIT_DELAYED_WORK(&hdev->discov_off, hci_discov_off);
INIT_DELAYED_WORK(&hdev->le_scan_disable, le_scan_disable_work);
+ INIT_DELAYED_WORK(&hdev->le_scan_restart, le_scan_restart_work);
skb_queue_head_init(&hdev->rx_q);
skb_queue_head_init(&hdev->cmd_q);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index bd0a801..ed4d5e1 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1157,10 +1157,13 @@ static void hci_cc_le_set_scan_enable(struct hci_dev *hdev,
d->last_adv_data_len, NULL, 0);
}
- /* Cancel this timer so that we don't try to disable scanning
- * when it's already disabled.
+ /* If HCI_LE_SCAN_RESTARTING is set, don't cancel this timer,
+ * because we're just restarting scan. Otherwise cancel it so
+ * that we don't try to disable scanning when it's already
+ * disabled.
*/
- cancel_delayed_work(&hdev->le_scan_disable);
+ if (!test_bit(HCI_LE_SCAN_RESTARTING, &hdev->dev_flags))
+ cancel_delayed_work(&hdev->le_scan_disable);
clear_bit(HCI_LE_SCAN, &hdev->dev_flags);
--
2.1.0
On Tue, Nov 25, 2014 at 1:32 AM, Johan Hedberg <[email protected]> wrote:
> Hi Jakub,
>
> On Sat, Nov 22, 2014, Jakub Pawlowski wrote:
>> This commit extract generic_start_discovery and generic_stop_discovery
>> in preparation for start and stop service discovery. The reason behind
>> that is that both functions will share big part of code, and it would
>> be much easier to maintain just one generic method.
>>
>> Signed-off-by: Jakub Pawlowski <[email protected]>
>> ---
>> net/bluetooth/mgmt.c | 89 +++++++++++++++++++++++++++++++++-------------------
>> 1 file changed, 56 insertions(+), 33 deletions(-)
>
> People's dislike of these generic* functions was previously pointed out
> but no good options were proposed, so let me give a try:
>
> Considering that we don't allow multiple concurrent stop or start
> discovery operations (the mgmt commands will return "busy" errors),
> couldn't we simply look up whatever pending start/discovery command
> there is and then use that to derive our response? I.e. instead of a
> single mgmt_pending_find (which we anyway need to do) there'd
> potentially be two such calls.
>
> Regarding Stop Service Discovery, I'm starting to doubt whether the
> whole thing is needed at all. It's exactly the same as Stop Discovery
> and you don't even attempt to check that the Stop Discovery/Stop Service
> Discovery matches the actual ongoing discovery type (i.e. your code
> would allow stopping a normal discovery with Stop Service Discovery and
> vice versa). So I'd propose to just drop the new method and reuse the
> existing Stop Discovery to stop either one of these two types.
Removing stop discovery is ok, I'll do that.
When it comes to starting discovery, the flow now is like that:
1. The start_discovery or start_service_discovery is called
2. generic_start_discovery is called
- insde it there is some processing, then mgmt_pending_add is called
3. call to start_discovery_complete or
start_serivce_discovery_complete is made when all requests to hci
success
4. generic_start_discovery_complete is called, somewhere internally it
use mgmt_pending_find
So you mean change that would remove generic_start_discovery_complete,
and have only start_discovery_complete that gets proper opcode from
call to mgmt_pending_find ? I'll do that.
When it comes to generic_start_discovery, I don't think I can get rid
of that, I need two different methods to know opcode that was passed,
otherwise I would need to somehow have opcode passed from
mgmt_control, I don't want to touch that.
The concern that Arman have is:
"I'm not that big on calling these generic functions "generic_*".
Let's try to come up with something better."
but I couldn't come up with better name.
To sum up:
I'll get rid of all generic functions except generic_start_discovery,
but I'm willing to also get rid of it if there's easy way to somehow
get opcode. Is that ok ?
>
> Johan
Hi Jakub,
On Sat, Nov 22, 2014, Jakub Pawlowski wrote:
> Currently there is no way to restart le scan. It's needed in
> preparation for new service scan method. The way it work: it disable,
> and then enable le scan on controller. During this restart special flag
> is set to make sure we won't remove disable scan work from workqueue.
>
> Signed-off-by: Jakub Pawlowski <[email protected]>
> ---
> include/net/bluetooth/hci.h | 1 +
> include/net/bluetooth/hci_core.h | 3 +++
> net/bluetooth/hci_core.c | 43 ++++++++++++++++++++++++++++++++++++++++
> net/bluetooth/hci_event.c | 9 ++++++---
> 4 files changed, 53 insertions(+), 3 deletions(-)
>
> diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
> index e56f909..2f0bce2 100644
> --- a/include/net/bluetooth/hci.h
> +++ b/include/net/bluetooth/hci.h
> @@ -203,6 +203,7 @@ enum {
> HCI_FAST_CONNECTABLE,
> HCI_BREDR_ENABLED,
> HCI_LE_SCAN_INTERRUPTED,
> + HCI_LE_SCAN_RESTARTING,
> };
This should break 32-bit systems since we already have 32 flags for
hdev->dev flags. The same goes for the HCI_SERVICE_FILTER you add in a
later patch.
One solution that comes to mind is to extend the discovery struct with
its own unsigned long flags field - at least the second flag you're
adding doesn't really belong in dev_flags anyway.
> +static void le_scan_restart_work(struct work_struct *work)
> +{
> + struct hci_dev *hdev = container_of(work, struct hci_dev,
> + le_scan_restart.work);
> + struct hci_request req;
> + struct hci_cp_le_set_scan_enable cp;
> + int err;
> +
> + BT_DBG("%s", hdev->name);
> +
> + hci_req_init(&req, hdev);
> +
> + hci_req_add_le_scan_disable(&req);
> +
> + memset(&cp, 0, sizeof(cp));
> + cp.enable = LE_SCAN_ENABLE;
> + hci_req_add(&req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp);
> +
> + set_bit(HCI_LE_SCAN_RESTARTING, &hdev->dev_flags);
> +
> + err = hci_req_run(&req, le_scan_restart_work_complete);
> + if (err)
> + BT_ERR("Disable LE scanning request failed: err %d", err);
> +}
Shouldn't you be checking the value of HCI_LE_SCAN before adding any HCI
commands to the request (after all this function gets called with a delay
and the state may have changed).
Shouldn't the error log talk about restarting instead of disabling?
Johan
Hi Jakub,
On Sat, Nov 22, 2014, Jakub Pawlowski wrote:
> This patch introduces start service discovery and stop service
> discovery methods. The reason behind that is to enable users to find
> specific services in range by UUID. Whole filtering is done in
> mgmt_device_found.
>
> Signed-off-by: Jakub Pawlowski <[email protected]>
> ---
> include/net/bluetooth/hci.h | 1 +
> include/net/bluetooth/hci_core.h | 6 +
> include/net/bluetooth/mgmt.h | 16 ++
> net/bluetooth/hci_core.c | 1 +
> net/bluetooth/mgmt.c | 333 ++++++++++++++++++++++++++++++++++-----
> 5 files changed, 321 insertions(+), 36 deletions(-)
>
> diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
> index 2f0bce2..97b0893 100644
> --- a/include/net/bluetooth/hci.h
> +++ b/include/net/bluetooth/hci.h
> @@ -204,6 +204,7 @@ enum {
> HCI_BREDR_ENABLED,
> HCI_LE_SCAN_INTERRUPTED,
> HCI_LE_SCAN_RESTARTING,
> + HCI_SERVICE_FILTER,
> };
As I mentioned earlier these will not fit in the hdev->dev_flags
variable on 32-bit systems. The flag you're adding here would already be
the 34th flag.
> +/* cleans up the state set up by the start_service_discovery function. */
> +void mgmt_clean_up_service_discovery(struct hci_dev *hdev)
> +{
> + if (!test_and_clear_bit(HCI_SERVICE_FILTER, &hdev->dev_flags))
> + return;
> +
> + cancel_delayed_work_sync(&hdev->le_scan_restart);
> + if (hdev->discovery.sd_num_uuid > 0)
> + kfree(hdev->discovery.sd_uuid);
"if (hdev->discovery.sd_uuid)" would be a more correct check here,
however since it's valid to pass NULL to kfree I think this all can be
made unconditional.
> +static int init_service_discovery(struct hci_dev *hdev, s8 rssi, u16 num_uuid,
> + u8 (*uuid)[16])
> +{
> + hdev->discovery.sd_rssi = rssi;
> + hdev->discovery.sd_num_uuid = num_uuid;
> +
> + if (num_uuid > 0) {
> + hdev->discovery.sd_uuid = kmalloc(16 * num_uuid, GFP_KERNEL);
> + if (!hdev->discovery.sd_uuid)
> + return -ENOMEM;
> + memcpy(hdev->discovery.sd_uuid, uuid, 16 * num_uuid);
> + }
What I mentioned earlier is particularly valid since you're failing to
set sd_num_uuid back to 0 if kmalloc fails here.
> + if (expected_len != len) {
> + return cmd_complete(sk, hdev->id, opcode,
> + MGMT_STATUS_INVALID_PARAMS, &type,
> + sizeof(type));
> + }
The { } can be dropped here since it's a single-statement branch.
Johan
Hi Jakub,
On Sat, Nov 22, 2014, Jakub Pawlowski wrote:
> This commit extract generic_start_discovery and generic_stop_discovery
> in preparation for start and stop service discovery. The reason behind
> that is that both functions will share big part of code, and it would
> be much easier to maintain just one generic method.
>
> Signed-off-by: Jakub Pawlowski <[email protected]>
> ---
> net/bluetooth/mgmt.c | 89 +++++++++++++++++++++++++++++++++-------------------
> 1 file changed, 56 insertions(+), 33 deletions(-)
People's dislike of these generic* functions was previously pointed out
but no good options were proposed, so let me give a try:
Considering that we don't allow multiple concurrent stop or start
discovery operations (the mgmt commands will return "busy" errors),
couldn't we simply look up whatever pending start/discovery command
there is and then use that to derive our response? I.e. instead of a
single mgmt_pending_find (which we anyway need to do) there'd
potentially be two such calls.
Regarding Stop Service Discovery, I'm starting to doubt whether the
whole thing is needed at all. It's exactly the same as Stop Discovery
and you don't even attempt to check that the Stop Discovery/Stop Service
Discovery matches the actual ongoing discovery type (i.e. your code
would allow stopping a normal discovery with Stop Service Discovery and
vice versa). So I'd propose to just drop the new method and reuse the
existing Stop Discovery to stop either one of these two types.
Johan
This patch introduces start service discovery and stop service
discovery methods. The reason behind that is to enable users to find
specific services in range by UUID. Whole filtering is done in
mgmt_device_found.
Signed-off-by: Jakub Pawlowski <[email protected]>
---
include/net/bluetooth/hci.h | 1 +
include/net/bluetooth/hci_core.h | 6 +
include/net/bluetooth/mgmt.h | 16 ++
net/bluetooth/hci_core.c | 1 +
net/bluetooth/mgmt.c | 333 ++++++++++++++++++++++++++++++++++-----
5 files changed, 321 insertions(+), 36 deletions(-)
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 2f0bce2..97b0893 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -204,6 +204,7 @@ enum {
HCI_BREDR_ENABLED,
HCI_LE_SCAN_INTERRUPTED,
HCI_LE_SCAN_RESTARTING,
+ HCI_SERVICE_FILTER,
};
/* A mask for the flags that are supposed to remain when a reset happens
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a022a4..df251fe 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -75,6 +75,11 @@ struct discovery_state {
u32 last_adv_flags;
u8 last_adv_data[HCI_MAX_AD_LENGTH];
u8 last_adv_data_len;
+
+ /* sd prefix stands for service discovery related proeprties */
+ s8 sd_rssi;
+ u16 sd_num_uuid;
+ u8 (*sd_uuid)[16];
};
struct hci_conn_hash {
@@ -1395,6 +1400,7 @@ void mgmt_new_conn_param(struct hci_dev *hdev, bdaddr_t *bdaddr,
u16 max_interval, u16 latency, u16 timeout);
void mgmt_reenable_advertising(struct hci_dev *hdev);
void mgmt_smp_complete(struct hci_conn *conn, bool complete);
+void mgmt_clean_up_service_discovery(struct hci_dev *hdev);
u8 hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, u16 latency,
u16 to_multiplier);
diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index b391fd6..2568476 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -495,6 +495,22 @@ struct mgmt_cp_set_public_address {
} __packed;
#define MGMT_SET_PUBLIC_ADDRESS_SIZE 6
+#define MGMT_OP_START_SERVICE_DISCOVERY 0x003A
+#define MGMT_START_SERVICE_DISCOVERY_SIZE 1
+
+struct mgmt_cp_start_service_discovery {
+ __u8 type;
+ __s8 rssi_threshold;
+ __le16 num_uuid;
+ __u8 uuid[0][16];
+} __packed;
+
+#define MGMT_OP_STOP_SERVICE_DISCOVERY 0x003B
+struct mgmt_cp_stop_service_discovery {
+ __u8 type;
+} __packed;
+#define MGMT_STOP_SERVICE_DISCOVERY_SIZE 1
+
#define MGMT_EV_CMD_COMPLETE 0x0001
struct mgmt_ev_cmd_complete {
__le16 opcode;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 1c3d6e6..e3a9773 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -3853,6 +3853,7 @@ static void le_scan_disable_work(struct work_struct *work)
BT_DBG("%s", hdev->name);
cancel_delayed_work_sync(&hdev->le_scan_restart);
+ mgmt_clean_up_service_discovery(hdev);
hci_req_init(&req, hdev);
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 2687fc2..7d18093 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -93,6 +93,8 @@ 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,
+ MGMT_OP_STOP_SERVICE_DISCOVERY,
};
static const u16 mgmt_events[] = {
@@ -1258,6 +1260,18 @@ static void clean_up_hci_complete(struct hci_dev *hdev, u8 status)
}
}
+/* cleans up the state set up by the start_service_discovery function. */
+void mgmt_clean_up_service_discovery(struct hci_dev *hdev)
+{
+ if (!test_and_clear_bit(HCI_SERVICE_FILTER, &hdev->dev_flags))
+ return;
+
+ cancel_delayed_work_sync(&hdev->le_scan_restart);
+ if (hdev->discovery.sd_num_uuid > 0)
+ kfree(hdev->discovery.sd_uuid);
+ hdev->discovery.sd_num_uuid = 0;
+}
+
static bool hci_stop_discovery(struct hci_request *req)
{
struct hci_dev *hdev = req->hdev;
@@ -1270,6 +1284,7 @@ static bool hci_stop_discovery(struct hci_request *req)
hci_req_add(req, HCI_OP_INQUIRY_CANCEL, 0, NULL);
} else {
cancel_delayed_work(&hdev->le_scan_disable);
+ mgmt_clean_up_service_discovery(hdev);
hci_req_add_le_scan_disable(req);
}
@@ -3742,42 +3757,89 @@ static void start_discovery_complete(struct hci_dev *hdev, u8 status)
generic_start_discovery_complete(hdev, status, MGMT_OP_START_DISCOVERY);
}
+static void start_service_discovery_complete(struct hci_dev *hdev, u8 status)
+{
+ generic_start_discovery_complete(hdev, status,
+ MGMT_OP_START_SERVICE_DISCOVERY);
+}
+
+static int init_service_discovery(struct hci_dev *hdev, s8 rssi, u16 num_uuid,
+ u8 (*uuid)[16])
+{
+ hdev->discovery.sd_rssi = rssi;
+ hdev->discovery.sd_num_uuid = num_uuid;
+
+ if (num_uuid > 0) {
+ hdev->discovery.sd_uuid = kmalloc(16 * num_uuid, GFP_KERNEL);
+ if (!hdev->discovery.sd_uuid)
+ return -ENOMEM;
+ memcpy(hdev->discovery.sd_uuid, uuid, 16 * num_uuid);
+ }
+
+ set_bit(HCI_SERVICE_FILTER, &hdev->dev_flags);
+ return 0;
+}
+
static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
void *data, u16 len, u16 opcode)
{
- struct mgmt_cp_start_discovery *cp = data;
struct pending_cmd *cmd;
struct hci_cp_le_set_scan_param param_cp;
struct hci_cp_le_set_scan_enable enable_cp;
struct hci_cp_inquiry inq_cp;
struct hci_request req;
/* General inquiry access code (GIAC) */
+ s8 sd_rssi = 0;
u8 lap[3] = { 0x33, 0x8b, 0x9e };
- u8 status, own_addr_type;
+ u8 status, own_addr_type, type;
+ u8 (*sd_uuid)[16] = NULL;
+ u16 sd_num_uuid = 0;
int err;
BT_DBG("%s", hdev->name);
+ if (opcode == MGMT_OP_START_SERVICE_DISCOVERY) {
+ struct mgmt_cp_start_service_discovery *cp = data;
+ u16 expected_len, num_uuid_tmp;
+
+ type = cp->type;
+ num_uuid_tmp = __le16_to_cpu(cp->num_uuid);
+ expected_len = sizeof(*cp) + num_uuid_tmp * 16;
+
+ if (expected_len != len) {
+ return cmd_complete(sk, hdev->id, opcode,
+ MGMT_STATUS_INVALID_PARAMS, &type,
+ sizeof(type));
+ }
+
+ sd_rssi = cp->rssi_threshold;
+ sd_num_uuid = num_uuid_tmp;
+ if (sd_num_uuid > 0)
+ sd_uuid = cp->uuid;
+ } else {
+ struct mgmt_cp_start_discovery *cp = data;
+
+ type = cp->type;
+ }
+
hci_dev_lock(hdev);
if (!hdev_is_powered(hdev)) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_NOT_POWERED,
- &cp->type, sizeof(cp->type));
+ MGMT_STATUS_NOT_POWERED, &type,
+ sizeof(type));
goto failed;
}
if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_BUSY, &cp->type,
- sizeof(cp->type));
+ MGMT_STATUS_BUSY, &type, sizeof(type));
goto failed;
}
if (hdev->discovery.state != DISCOVERY_STOPPED) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_BUSY, &cp->type,
- sizeof(cp->type));
+ MGMT_STATUS_BUSY, &type, sizeof(type));
goto failed;
}
@@ -3787,7 +3849,7 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
goto failed;
}
- hdev->discovery.type = cp->type;
+ hdev->discovery.type = type;
hci_req_init(&req, hdev);
@@ -3795,16 +3857,16 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
case DISCOV_TYPE_BREDR:
status = mgmt_bredr_support(hdev);
if (status) {
- err = cmd_complete(sk, hdev->id, opcode, status,
- &cp->type, sizeof(cp->type));
+ err = cmd_complete(sk, hdev->id, opcode, status, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
if (test_bit(HCI_INQUIRY, &hdev->flags)) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_BUSY, &cp->type,
- sizeof(cp->type));
+ MGMT_STATUS_BUSY, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
@@ -3821,8 +3883,8 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
case DISCOV_TYPE_INTERLEAVED:
status = mgmt_le_support(hdev);
if (status) {
- err = cmd_complete(sk, hdev->id, opcode, status,
- &cp->type, sizeof(cp->type));
+ err = cmd_complete(sk, hdev->id, opcode, status, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
@@ -3830,8 +3892,8 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED &&
!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_NOT_SUPPORTED,
- &cp->type, sizeof(cp->type));
+ MGMT_STATUS_NOT_SUPPORTED, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
@@ -3844,9 +3906,8 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
if (hci_conn_hash_lookup_state(hdev, LE_LINK,
BT_CONNECT)) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_REJECTED,
- &cp->type,
- sizeof(cp->type));
+ MGMT_STATUS_REJECTED, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
@@ -3870,8 +3931,8 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
err = hci_update_random_address(&req, true, &own_addr_type);
if (err < 0) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_FAILED,
- &cp->type, sizeof(cp->type));
+ MGMT_STATUS_FAILED, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
@@ -3892,13 +3953,23 @@ static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
default:
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_INVALID_PARAMS,
- &cp->type, sizeof(cp->type));
+ MGMT_STATUS_INVALID_PARAMS, &type,
+ sizeof(type));
mgmt_pending_remove(cmd);
goto failed;
}
- err = hci_req_run(&req, start_discovery_complete);
+ if (opcode == MGMT_OP_START_SERVICE_DISCOVERY) {
+ err = init_service_discovery(hdev, sd_rssi, sd_num_uuid,
+ sd_uuid);
+ if (err)
+ goto failed;
+
+ err = hci_req_run(&req, start_service_discovery_complete);
+ } else {
+ err = hci_req_run(&req, start_discovery_complete);
+ }
+
if (err < 0)
mgmt_pending_remove(cmd);
else
@@ -3956,29 +4027,44 @@ static void stop_discovery_complete(struct hci_dev *hdev, u8 status)
generic_stop_discovery_complete(hdev, status, MGMT_OP_STOP_DISCOVERY);
}
+static void stop_service_discovery_complete(struct hci_dev *hdev, u8 status)
+{
+ generic_stop_discovery_complete(hdev, status,
+ MGMT_OP_STOP_SERVICE_DISCOVERY);
+}
+
static int generic_stop_discovery(struct sock *sk, struct hci_dev *hdev,
void *data, u16 len, u16 opcode)
{
- struct mgmt_cp_stop_discovery *mgmt_cp = data;
struct pending_cmd *cmd;
struct hci_request req;
int err;
+ u8 type;
BT_DBG("%s", hdev->name);
+ if (opcode == MGMT_OP_STOP_SERVICE_DISCOVERY) {
+ struct mgmt_cp_stop_service_discovery *mgmt_cp = data;
+
+ type = mgmt_cp->type;
+ } else {
+ struct mgmt_cp_stop_discovery *mgmt_cp = data;
+
+ type = mgmt_cp->type;
+ }
+
hci_dev_lock(hdev);
if (!hci_discovery_active(hdev)) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_REJECTED, &mgmt_cp->type,
- sizeof(mgmt_cp->type));
+ MGMT_STATUS_REJECTED, &type, sizeof(type));
goto unlock;
}
- if (hdev->discovery.type != mgmt_cp->type) {
+ if (hdev->discovery.type != type) {
err = cmd_complete(sk, hdev->id, opcode,
- MGMT_STATUS_INVALID_PARAMS, &mgmt_cp->type,
- sizeof(mgmt_cp->type));
+ MGMT_STATUS_INVALID_PARAMS, &type,
+ sizeof(type));
goto unlock;
}
@@ -3992,7 +4078,10 @@ static int generic_stop_discovery(struct sock *sk, struct hci_dev *hdev,
hci_stop_discovery(&req);
- err = hci_req_run(&req, stop_discovery_complete);
+ if (opcode == MGMT_OP_STOP_SERVICE_DISCOVERY)
+ err = hci_req_run(&req, stop_service_discovery_complete);
+ else
+ err = hci_req_run(&req, stop_discovery_complete);
if (!err) {
hci_discovery_set_state(hdev, DISCOVERY_STOPPING);
@@ -4004,7 +4093,7 @@ static int generic_stop_discovery(struct sock *sk, struct hci_dev *hdev,
/* If no HCI commands were sent we're done */
if (err == -ENODATA) {
err = cmd_complete(sk, hdev->id, opcode, 0,
- &mgmt_cp->type, sizeof(mgmt_cp->type));
+ &type, sizeof(type));
hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
}
@@ -5705,6 +5794,20 @@ unlock:
return err;
}
+static int start_service_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ return generic_start_discovery(sk, hdev, data, len,
+ MGMT_OP_START_SERVICE_DISCOVERY);
+}
+
+static int stop_service_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ return generic_stop_discovery(sk, hdev, data, len,
+ MGMT_OP_STOP_SERVICE_DISCOVERY);
+}
+
static const struct mgmt_handler {
int (*func) (struct sock *sk, struct hci_dev *hdev, void *data,
u16 data_len);
@@ -5769,6 +5872,8 @@ 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 },
+ { stop_service_discovery, false, MGMT_STOP_SERVICE_DISCOVERY_SIZE },
};
int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
@@ -6835,6 +6940,127 @@ void mgmt_read_local_oob_data_complete(struct hci_dev *hdev, u8 *hash192,
mgmt_pending_remove(cmd);
}
+struct parsed_uuid {
+ struct list_head list;
+ u8 uuid[16];
+};
+
+/* this is reversed hex representation of bluetooth base uuid. We need it for
+ * service uuid parsing in eir.
+ */
+static const u8 reverse_base_uuid[] = {
+ 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
+ 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+};
+
+static int add_uuid_to_list(struct list_head *uuids, u8 *uuid)
+{
+ struct parsed_uuid *tmp_uuid;
+
+ tmp_uuid = kmalloc(sizeof(*tmp_uuid), GFP_KERNEL);
+ if (tmp_uuid == NULL)
+ return -ENOMEM;
+
+ memcpy(tmp_uuid->uuid, uuid, 16);
+ INIT_LIST_HEAD(&tmp_uuid->list);
+ list_add(&tmp_uuid->list, uuids);
+ return 0;
+}
+
+static void free_uuids_list(struct list_head *uuids)
+{
+ struct parsed_uuid *uuid, *tmp;
+
+ list_for_each_entry_safe(uuid, tmp, uuids, list) {
+ __list_del_entry(&uuid->list);
+ kfree(uuid);
+ }
+}
+
+static int eir_parse(u8 *eir, u8 eir_len, struct list_head *uuids)
+{
+ size_t offset;
+ u8 uuid[16];
+ int i, ret;
+
+ offset = 0;
+ while (offset < eir_len) {
+ uint8_t field_len = eir[0];
+
+ /* Check for the end of EIR */
+ if (field_len == 0)
+ break;
+
+ if (offset + field_len > eir_len)
+ return -EINVAL;
+
+ switch (eir[1]) {
+ case EIR_UUID16_ALL:
+ case EIR_UUID16_SOME:
+ for (i = 0; i + 3 <= field_len; i += 2) {
+ memcpy(uuid, reverse_base_uuid, 16);
+ uuid[13] = eir[i + 3];
+ uuid[12] = eir[i + 2];
+ ret = add_uuid_to_list(uuids, uuid);
+ if (ret)
+ return ret;
+ }
+ break;
+ case EIR_UUID32_ALL:
+ case EIR_UUID32_SOME:
+ for (i = 0; i + 5 <= field_len; i += 4) {
+ memcpy(uuid, reverse_base_uuid, 16);
+ uuid[15] = eir[i + 5];
+ uuid[14] = eir[i + 4];
+ uuid[13] = eir[i + 3];
+ uuid[12] = eir[i + 2];
+ ret = add_uuid_to_list(uuids, uuid);
+ if (ret)
+ return ret;
+ }
+ break;
+ case EIR_UUID128_ALL:
+ case EIR_UUID128_SOME:
+ for (i = 0; i + 17 <= field_len; i += 16) {
+ memcpy(uuid, eir + i + 2, 16);
+ ret = add_uuid_to_list(uuids, uuid);
+ if (ret)
+ return ret;
+ }
+ break;
+ }
+
+ offset += field_len + 1;
+ eir += field_len + 1;
+ }
+ return 0;
+}
+
+enum {
+ NO_MATCH,
+ SERVICE_MATCH,
+ FULL_MATCH
+};
+
+static u8 find_matches(struct hci_dev *hdev, s8 rssi, struct list_head *uuids)
+{
+ struct parsed_uuid *uuidptr, *tmp_uuid;
+ int i, match_type = NO_MATCH, min_rssi = hdev->discovery.sd_rssi;
+
+ list_for_each_entry_safe(uuidptr, tmp_uuid, uuids, list) {
+ for (i = 0; i < hdev->discovery.sd_num_uuid; i++) {
+ u8 *filter_uuid = hdev->discovery.sd_uuid[i];
+
+ if (memcmp(filter_uuid, uuidptr->uuid, 16) != 0)
+ continue;
+ if (rssi >= min_rssi)
+ return FULL_MATCH;
+ match_type = SERVICE_MATCH;
+ }
+ }
+ return match_type;
+}
+
void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
u8 addr_type, u8 *dev_class, s8 rssi, u32 flags,
u8 *eir, u16 eir_len, u8 *scan_rsp, u8 scan_rsp_len)
@@ -6842,6 +7068,9 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
char buf[512];
struct mgmt_ev_device_found *ev = (void *) buf;
size_t ev_size;
+ LIST_HEAD(uuids);
+ int err = 0;
+ u8 match_type;
/* Don't send events for a non-kernel initiated discovery. With
* LE one exception is if we have pend_le_reports > 0 in which
@@ -6880,7 +7109,31 @@ void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
ev->eir_len = cpu_to_le16(eir_len + scan_rsp_len);
ev_size = sizeof(*ev) + eir_len + scan_rsp_len;
- mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL);
+ if (!test_bit(HCI_SERVICE_FILTER, &hdev->dev_flags) ||
+ (hdev->discovery.sd_rssi == 127 &&
+ hdev->discovery.sd_num_uuid == 0)) {
+ mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL);
+ return;
+ }
+
+ err = eir_parse(eir, eir_len, &uuids);
+ if (err) {
+ free_uuids_list(&uuids);
+ return;
+ }
+
+ match_type = find_matches(hdev, rssi, &uuids);
+ free_uuids_list(&uuids);
+
+ if (match_type == NO_MATCH)
+ return;
+
+ if (test_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks))
+ hci_restart_le_scan(hdev);
+
+ if (match_type == FULL_MATCH)
+ mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev,
+ ev_size, NULL);
}
void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
@@ -6913,10 +7166,18 @@ void mgmt_discovering(struct hci_dev *hdev, u8 discovering)
BT_DBG("%s discovering %u", hdev->name, discovering);
- if (discovering)
+ if (discovering) {
cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev);
- else
+ if (cmd == NULL)
+ cmd = mgmt_pending_find(MGMT_OP_START_SERVICE_DISCOVERY,
+ hdev);
+
+ } else {
cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev);
+ if (cmd == NULL)
+ cmd = mgmt_pending_find(MGMT_OP_STOP_SERVICE_DISCOVERY,
+ hdev);
+ }
if (cmd != NULL) {
u8 type = hdev->discovery.type;
--
2.1.0
This commit extract generic_start_discovery and generic_stop_discovery
in preparation for start and stop service discovery. The reason behind
that is that both functions will share big part of code, and it would
be much easier to maintain just one generic method.
Signed-off-by: Jakub Pawlowski <[email protected]>
---
net/bluetooth/mgmt.c | 89 +++++++++++++++++++++++++++++++++-------------------
1 file changed, 56 insertions(+), 33 deletions(-)
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index cbeef5f..2687fc2 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -3675,7 +3675,8 @@ done:
return err;
}
-static int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status)
+static int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status,
+ u16 opcode)
{
struct pending_cmd *cmd;
u8 type;
@@ -3683,7 +3684,7 @@ static int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status)
hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
- cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev);
+ cmd = mgmt_pending_find(opcode, hdev);
if (!cmd)
return -ENOENT;
@@ -3696,7 +3697,8 @@ static int mgmt_start_discovery_failed(struct hci_dev *hdev, u8 status)
return err;
}
-static void start_discovery_complete(struct hci_dev *hdev, u8 status)
+static void generic_start_discovery_complete(struct hci_dev *hdev, u8 status,
+ u16 opcode)
{
unsigned long timeout = 0;
@@ -3704,7 +3706,7 @@ static void start_discovery_complete(struct hci_dev *hdev, u8 status)
if (status) {
hci_dev_lock(hdev);
- mgmt_start_discovery_failed(hdev, status);
+ mgmt_start_discovery_failed(hdev, status, opcode);
hci_dev_unlock(hdev);
return;
}
@@ -3735,8 +3737,13 @@ static void start_discovery_complete(struct hci_dev *hdev, u8 status)
queue_delayed_work(hdev->workqueue, &hdev->le_scan_disable, timeout);
}
-static int start_discovery(struct sock *sk, struct hci_dev *hdev,
- void *data, u16 len)
+static void start_discovery_complete(struct hci_dev *hdev, u8 status)
+{
+ generic_start_discovery_complete(hdev, status, MGMT_OP_START_DISCOVERY);
+}
+
+static int generic_start_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len, u16 opcode)
{
struct mgmt_cp_start_discovery *cp = data;
struct pending_cmd *cmd;
@@ -3754,27 +3761,27 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
hci_dev_lock(hdev);
if (!hdev_is_powered(hdev)) {
- err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_NOT_POWERED,
&cp->type, sizeof(cp->type));
goto failed;
}
if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {
- err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_BUSY, &cp->type,
sizeof(cp->type));
goto failed;
}
if (hdev->discovery.state != DISCOVERY_STOPPED) {
- err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_BUSY, &cp->type,
sizeof(cp->type));
goto failed;
}
- cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, NULL, 0);
+ cmd = mgmt_pending_add(sk, opcode, hdev, NULL, 0);
if (!cmd) {
err = -ENOMEM;
goto failed;
@@ -3788,16 +3795,14 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
case DISCOV_TYPE_BREDR:
status = mgmt_bredr_support(hdev);
if (status) {
- err = cmd_complete(sk, hdev->id,
- MGMT_OP_START_DISCOVERY, status,
+ err = cmd_complete(sk, hdev->id, opcode, status,
&cp->type, sizeof(cp->type));
mgmt_pending_remove(cmd);
goto failed;
}
if (test_bit(HCI_INQUIRY, &hdev->flags)) {
- err = cmd_complete(sk, hdev->id,
- MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_BUSY, &cp->type,
sizeof(cp->type));
mgmt_pending_remove(cmd);
@@ -3816,8 +3821,7 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
case DISCOV_TYPE_INTERLEAVED:
status = mgmt_le_support(hdev);
if (status) {
- err = cmd_complete(sk, hdev->id,
- MGMT_OP_START_DISCOVERY, status,
+ err = cmd_complete(sk, hdev->id, opcode, status,
&cp->type, sizeof(cp->type));
mgmt_pending_remove(cmd);
goto failed;
@@ -3825,8 +3829,7 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED &&
!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
- err = cmd_complete(sk, hdev->id,
- MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_NOT_SUPPORTED,
&cp->type, sizeof(cp->type));
mgmt_pending_remove(cmd);
@@ -3840,8 +3843,7 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
*/
if (hci_conn_hash_lookup_state(hdev, LE_LINK,
BT_CONNECT)) {
- err = cmd_complete(sk, hdev->id,
- MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_REJECTED,
&cp->type,
sizeof(cp->type));
@@ -3867,8 +3869,7 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
*/
err = hci_update_random_address(&req, true, &own_addr_type);
if (err < 0) {
- err = cmd_complete(sk, hdev->id,
- MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_FAILED,
&cp->type, sizeof(cp->type));
mgmt_pending_remove(cmd);
@@ -3890,7 +3891,7 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
break;
default:
- err = cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_INVALID_PARAMS,
&cp->type, sizeof(cp->type));
mgmt_pending_remove(cmd);
@@ -3908,12 +3909,20 @@ failed:
return err;
}
-static int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status)
+static int start_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ return generic_start_discovery(sk, hdev, data, len,
+ MGMT_OP_START_DISCOVERY);
+}
+
+static int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status,
+ u16 opcode)
{
struct pending_cmd *cmd;
int err;
- cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev);
+ cmd = mgmt_pending_find(opcode, hdev);
if (!cmd)
return -ENOENT;
@@ -3924,14 +3933,15 @@ static int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status)
return err;
}
-static void stop_discovery_complete(struct hci_dev *hdev, u8 status)
+static void generic_stop_discovery_complete(struct hci_dev *hdev, u8 status,
+ u16 opcode)
{
BT_DBG("status %d", status);
hci_dev_lock(hdev);
if (status) {
- mgmt_stop_discovery_failed(hdev, status);
+ mgmt_stop_discovery_failed(hdev, status, opcode);
goto unlock;
}
@@ -3941,8 +3951,13 @@ unlock:
hci_dev_unlock(hdev);
}
-static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
- u16 len)
+static void stop_discovery_complete(struct hci_dev *hdev, u8 status)
+{
+ generic_stop_discovery_complete(hdev, status, MGMT_OP_STOP_DISCOVERY);
+}
+
+static int generic_stop_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len, u16 opcode)
{
struct mgmt_cp_stop_discovery *mgmt_cp = data;
struct pending_cmd *cmd;
@@ -3954,20 +3969,20 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
hci_dev_lock(hdev);
if (!hci_discovery_active(hdev)) {
- err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_REJECTED, &mgmt_cp->type,
sizeof(mgmt_cp->type));
goto unlock;
}
if (hdev->discovery.type != mgmt_cp->type) {
- err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY,
+ err = cmd_complete(sk, hdev->id, opcode,
MGMT_STATUS_INVALID_PARAMS, &mgmt_cp->type,
sizeof(mgmt_cp->type));
goto unlock;
}
- cmd = mgmt_pending_add(sk, MGMT_OP_STOP_DISCOVERY, hdev, NULL, 0);
+ cmd = mgmt_pending_add(sk, opcode, hdev, NULL, 0);
if (!cmd) {
err = -ENOMEM;
goto unlock;
@@ -3978,6 +3993,7 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
hci_stop_discovery(&req);
err = hci_req_run(&req, stop_discovery_complete);
+
if (!err) {
hci_discovery_set_state(hdev, DISCOVERY_STOPPING);
goto unlock;
@@ -3987,7 +4003,7 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
/* If no HCI commands were sent we're done */
if (err == -ENODATA) {
- err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, 0,
+ err = cmd_complete(sk, hdev->id, opcode, 0,
&mgmt_cp->type, sizeof(mgmt_cp->type));
hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
}
@@ -3997,6 +4013,13 @@ unlock:
return err;
}
+static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
+ u16 len)
+{
+ return generic_stop_discovery(sk, hdev, data, len,
+ MGMT_OP_STOP_DISCOVERY);
+}
+
static int confirm_name(struct sock *sk, struct hci_dev *hdev, void *data,
u16 len)
{
--
2.1.0