* [PATCH v1 2/4] Extract generic_start_discovery and generic_stop_discovery in preparation for start_service_discovery and stop_service_discovery.
2014-11-26 0:45 [PATCH v1 0/4] Start service discovery Jakub Pawlowski
2014-11-26 0:45 ` [PATCH v1 1/4] Add le_scan_restart that can be used to restart le scan. In order to use it please i.e. do: Jakub Pawlowski
@ 2014-11-26 0:45 ` Jakub Pawlowski
2014-11-26 0:45 ` [PATCH v1 3/4] Adding HCI_QUIRK_ADV_RSSI_DEDUP. When this quirk is set, the device, when in scan with deduplication is not sending advertising reports for device when RSSI value changes Jakub Pawlowski
2014-11-26 0:45 ` [PATCH v1 4/4] Introducing start service discovery and stop service discovery methods Jakub Pawlowski
3 siblings, 0 replies; 6+ messages in thread
From: Jakub Pawlowski @ 2014-11-26 0:45 UTC (permalink / raw)
To: linux-bluetooth; +Cc: Jakub Pawlowski
---
net/bluetooth/mgmt.c | 102 ++++++++++++++++++++++++++++++++++-----------------
1 file changed, 68 insertions(+), 34 deletions(-)
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 9c4daf7..f2b80b7 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -3648,7 +3648,8 @@ static int remove_remote_oob_data(struct sock *sk, struct hci_dev *hdev,
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,
+ int discovery_op)
{
struct pending_cmd *cmd;
u8 type;
@@ -3656,7 +3657,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(discovery_op, hdev);
if (!cmd)
return -ENOENT;
@@ -3669,7 +3670,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,
+ int discovery_op)
{
unsigned long timeout = 0;
@@ -3677,7 +3679,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, discovery_op);
hci_dev_unlock(hdev);
return;
}
@@ -3708,10 +3710,18 @@ 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,
+ int discovery_op,
+ void (*complete_cb)(struct hci_dev *hdev, u8 status),
+ u8 type,
+ struct mgmt_uuid_filter *serv_filters,
+ __le16 serv_filter_cnt)
{
- 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;
@@ -3727,30 +3737,30 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
hci_dev_lock(hdev);
if (!hdev_is_powered(hdev)) {
- err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_NOT_POWERED);
goto failed;
}
if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {
- err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_BUSY);
goto failed;
}
if (hdev->discovery.state != DISCOVERY_STOPPED) {
- err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_BUSY);
goto failed;
}
- cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, NULL, 0);
+ cmd = mgmt_pending_add(sk, discovery_op, hdev, NULL, 0);
if (!cmd) {
err = -ENOMEM;
goto failed;
}
- hdev->discovery.type = cp->type;
+ hdev->discovery.type = type;
hci_req_init(&req, hdev);
@@ -3758,14 +3768,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_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
status);
mgmt_pending_remove(cmd);
goto failed;
}
if (test_bit(HCI_INQUIRY, &hdev->flags)) {
- err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_BUSY);
mgmt_pending_remove(cmd);
goto failed;
@@ -3783,7 +3793,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_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
status);
mgmt_pending_remove(cmd);
goto failed;
@@ -3791,7 +3801,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_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_NOT_SUPPORTED);
mgmt_pending_remove(cmd);
goto failed;
@@ -3805,7 +3815,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_status(sk, hdev->id,
- MGMT_OP_START_DISCOVERY,
+ discovery_op,
MGMT_STATUS_REJECTED);
mgmt_pending_remove(cmd);
goto failed;
@@ -3829,7 +3839,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_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_FAILED);
mgmt_pending_remove(cmd);
goto failed;
@@ -3850,13 +3860,13 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
break;
default:
- err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
+ err = cmd_status(sk, hdev->id, discovery_op,
MGMT_STATUS_INVALID_PARAMS);
mgmt_pending_remove(cmd);
goto failed;
}
- err = hci_req_run(&req, start_discovery_complete);
+ err = hci_req_run(&req, complete_cb);
if (err < 0)
mgmt_pending_remove(cmd);
else
@@ -3867,12 +3877,22 @@ 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)
+{
+ struct mgmt_cp_start_discovery *cp = data;
+
+ return generic_start_discovery(sk, hdev, MGMT_OP_START_DISCOVERY,
+ start_discovery_complete, cp->type, NULL, 0);
+}
+
+static int mgmt_stop_discovery_failed(struct hci_dev *hdev, u8 status,
+ int discovery_op)
{
struct pending_cmd *cmd;
int err;
- cmd = mgmt_pending_find(MGMT_OP_STOP_DISCOVERY, hdev);
+ cmd = mgmt_pending_find(discovery_op, hdev);
if (!cmd)
return -ENOENT;
@@ -3883,14 +3903,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,
+ int discovery_op)
{
BT_DBG("status %d", status);
hci_dev_lock(hdev);
if (status) {
- mgmt_stop_discovery_failed(hdev, status);
+ mgmt_stop_discovery_failed(hdev, status, discovery_op);
goto unlock;
}
@@ -3900,10 +3921,15 @@ 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,
+ __u8 type,
+ void (*stop_discovery_cb)(struct hci_dev *, u8))
{
- struct mgmt_cp_stop_discovery *mgmt_cp = data;
struct pending_cmd *cmd;
struct hci_request req;
int err;
@@ -3914,15 +3940,14 @@ static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
if (!hci_discovery_active(hdev)) {
err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY,
- 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, MGMT_OP_STOP_DISCOVERY,
- MGMT_STATUS_INVALID_PARAMS, &mgmt_cp->type,
- sizeof(mgmt_cp->type));
+ MGMT_STATUS_INVALID_PARAMS, &type,
+ sizeof(type));
goto unlock;
}
@@ -3936,7 +3961,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);
+ err = hci_req_run(&req, stop_discovery_cb);
if (!err) {
hci_discovery_set_state(hdev, DISCOVERY_STOPPING);
goto unlock;
@@ -3947,7 +3972,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,
- &mgmt_cp->type, sizeof(mgmt_cp->type));
+ &type, sizeof(type));
hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
}
@@ -3956,6 +3981,15 @@ unlock:
return err;
}
+static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data,
+ u16 len)
+{
+ struct mgmt_cp_stop_discovery *mgmt_cp = data;
+
+ return generic_stop_discovery(sk, hdev, mgmt_cp->type,
+ stop_discovery_complete);
+}
+
static int confirm_name(struct sock *sk, struct hci_dev *hdev, void *data,
u16 len)
{
--
2.1.0.rc2.206.gedb03e5
^ permalink raw reply related [flat|nested] 6+ messages in thread* [PATCH v1 4/4] Introducing start service discovery and stop service discovery methods.
2014-11-26 0:45 [PATCH v1 0/4] Start service discovery Jakub Pawlowski
` (2 preceding siblings ...)
2014-11-26 0:45 ` [PATCH v1 3/4] Adding HCI_QUIRK_ADV_RSSI_DEDUP. When this quirk is set, the device, when in scan with deduplication is not sending advertising reports for device when RSSI value changes Jakub Pawlowski
@ 2014-11-26 0:45 ` Jakub Pawlowski
3 siblings, 0 replies; 6+ messages in thread
From: Jakub Pawlowski @ 2014-11-26 0:45 UTC (permalink / raw)
To: linux-bluetooth; +Cc: Jakub Pawlowski
---
include/net/bluetooth/hci_core.h | 13 ++
include/net/bluetooth/mgmt.h | 26 +++
net/bluetooth/hci_core.c | 3 +
net/bluetooth/mgmt.c | 341 ++++++++++++++++++++++++++++++++-------
4 files changed, 323 insertions(+), 60 deletions(-)
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 0865bc1..d2b6661 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -143,6 +143,14 @@ struct oob_data {
u8 randomizer256[16];
};
+struct uuid_filter {
+ struct list_head list;
+ u8 range_method;
+ s8 pathloss;
+ s8 rssi;
+ u8 uuid[16];
+};
+
#define HCI_MAX_SHORT_NAME_LENGTH 10
/* Default LE RPA expiry time, 15 minutes */
@@ -307,6 +315,8 @@ struct hci_dev {
struct discovery_state discovery;
struct hci_conn_hash conn_hash;
+ struct list_head discov_uuid_filter;
+ bool service_filter_enabled;
bool scan_restart;
struct list_head mgmt_pending;
@@ -1306,6 +1316,8 @@ void hci_sock_dev_event(struct hci_dev *hdev, int event);
#define DISCOV_INTERLEAVED_INQUIRY_LEN 0x04
#define DISCOV_BREDR_INQUIRY_LEN 0x08
+#define DISCOV_LE_RESTART_DELAY msecs_to_jiffies(300)
+
int mgmt_control(struct sock *sk, struct msghdr *msg, size_t len);
int mgmt_new_settings(struct hci_dev *hdev);
void mgmt_index_added(struct hci_dev *hdev);
@@ -1372,6 +1384,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 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 414cd2f..46588d7 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -495,6 +495,32 @@ 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
+
+#define MGMT_RANGE_NONE 0X00
+#define MGMT_RANGE_RSSI 0X01
+#define MGMT_RANGE_PATHLOSS 0X02
+
+struct mgmt_uuid_filter {
+ __u8 range_method;
+ __s8 pathloss;
+ __s8 rssi;
+ __u8 uuid[16];
+} __packed;
+
+struct mgmt_cp_start_service_discovery {
+ __u8 type;
+ __le16 filter_count;
+ struct mgmt_uuid_filter filter[0];
+} __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 343279c..2bbcadb 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -3848,6 +3848,7 @@ static void le_scan_disable_work(struct work_struct *work)
BT_DBG("%s", hdev->name);
cancel_delayed_work_sync(&hdev->le_scan_restart);
+ clean_up_service_discovery(hdev);
hci_req_init(&req, hdev);
@@ -4043,6 +4044,8 @@ struct hci_dev *hci_alloc_dev(void)
hdev->conn_info_min_age = DEFAULT_CONN_INFO_MIN_AGE;
hdev->conn_info_max_age = DEFAULT_CONN_INFO_MAX_AGE;
+ INIT_LIST_HEAD(&hdev->discov_uuid_filter);
+ hdev->service_filter_enabled = false;
hdev->scan_restart = false;
mutex_init(&hdev->lock);
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index f2b80b7..9d08f83 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,26 @@ static void clean_up_hci_complete(struct hci_dev *hdev, u8 status)
}
}
+void clean_up_service_discovery(struct hci_dev *hdev)
+{
+ struct uuid_filter *filter = NULL;
+ struct uuid_filter *tmp = NULL;
+
+ if (!hdev->service_filter_enabled)
+ return;
+
+ hdev->service_filter_enabled = false;
+ cancel_delayed_work_sync(&hdev->le_scan_restart);
+
+ if (list_empty(&hdev->discov_uuid_filter))
+ return;
+
+ list_for_each_entry_safe(filter, tmp, &hdev->discov_uuid_filter, list) {
+ __list_del_entry(&filter->list);
+ kfree(filter);
+ }
+}
+
static bool hci_stop_discovery(struct hci_request *req)
{
struct hci_dev *hdev = req->hdev;
@@ -1270,6 +1292,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);
+ clean_up_service_discovery(hdev);
hci_req_add_le_scan_disable(req);
}
@@ -5675,6 +5698,53 @@ unlock:
return err;
}
+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 start_service_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ struct mgmt_cp_start_service_discovery *cp = data;
+ u16 expected_len, filter_count;
+
+ BT_DBG("%s", hdev->name);
+
+ filter_count = __le16_to_cpu(cp->filter_count);
+
+ expected_len = sizeof(*cp) +
+ filter_count * sizeof(struct mgmt_uuid_filter);
+ if (expected_len != len) {
+ BT_ERR("start_service_discovery: expected %u, got %u bytes",
+ expected_len, len);
+
+ return cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_INVALID_PARAMS);
+ }
+
+ return generic_start_discovery(sk, hdev,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ start_service_discovery_complete,
+ cp->type, cp->filter, cp->filter_count);
+}
+
+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 stop_service_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ struct mgmt_cp_stop_service_discovery *mgmt_cp = data;
+
+ return generic_stop_discovery(sk, hdev, mgmt_cp->type,
+ stop_service_discovery_complete);
+}
+
static const struct mgmt_handler {
int (*func) (struct sock *sk, struct hci_dev *hdev, void *data,
u16 data_len);
@@ -5682,63 +5752,65 @@ static const struct mgmt_handler {
size_t data_len;
} mgmt_handlers[] = {
{ NULL }, /* 0x0000 (no command) */
- { read_version, false, MGMT_READ_VERSION_SIZE },
- { read_commands, false, MGMT_READ_COMMANDS_SIZE },
- { read_index_list, false, MGMT_READ_INDEX_LIST_SIZE },
- { read_controller_info, false, MGMT_READ_INFO_SIZE },
- { set_powered, false, MGMT_SETTING_SIZE },
- { set_discoverable, false, MGMT_SET_DISCOVERABLE_SIZE },
- { set_connectable, false, MGMT_SETTING_SIZE },
- { set_fast_connectable, false, MGMT_SETTING_SIZE },
- { set_bondable, false, MGMT_SETTING_SIZE },
- { set_link_security, false, MGMT_SETTING_SIZE },
- { set_ssp, false, MGMT_SETTING_SIZE },
- { set_hs, false, MGMT_SETTING_SIZE },
- { set_le, false, MGMT_SETTING_SIZE },
- { set_dev_class, false, MGMT_SET_DEV_CLASS_SIZE },
- { set_local_name, false, MGMT_SET_LOCAL_NAME_SIZE },
- { add_uuid, false, MGMT_ADD_UUID_SIZE },
- { remove_uuid, false, MGMT_REMOVE_UUID_SIZE },
- { load_link_keys, true, MGMT_LOAD_LINK_KEYS_SIZE },
- { load_long_term_keys, true, MGMT_LOAD_LONG_TERM_KEYS_SIZE },
- { disconnect, false, MGMT_DISCONNECT_SIZE },
- { get_connections, false, MGMT_GET_CONNECTIONS_SIZE },
- { pin_code_reply, false, MGMT_PIN_CODE_REPLY_SIZE },
- { pin_code_neg_reply, false, MGMT_PIN_CODE_NEG_REPLY_SIZE },
- { set_io_capability, false, MGMT_SET_IO_CAPABILITY_SIZE },
- { pair_device, false, MGMT_PAIR_DEVICE_SIZE },
- { cancel_pair_device, false, MGMT_CANCEL_PAIR_DEVICE_SIZE },
- { unpair_device, false, MGMT_UNPAIR_DEVICE_SIZE },
- { user_confirm_reply, false, MGMT_USER_CONFIRM_REPLY_SIZE },
- { user_confirm_neg_reply, false, MGMT_USER_CONFIRM_NEG_REPLY_SIZE },
- { user_passkey_reply, false, MGMT_USER_PASSKEY_REPLY_SIZE },
- { user_passkey_neg_reply, false, MGMT_USER_PASSKEY_NEG_REPLY_SIZE },
- { read_local_oob_data, false, MGMT_READ_LOCAL_OOB_DATA_SIZE },
- { add_remote_oob_data, true, MGMT_ADD_REMOTE_OOB_DATA_SIZE },
- { remove_remote_oob_data, false, MGMT_REMOVE_REMOTE_OOB_DATA_SIZE },
- { start_discovery, false, MGMT_START_DISCOVERY_SIZE },
- { stop_discovery, false, MGMT_STOP_DISCOVERY_SIZE },
- { confirm_name, false, MGMT_CONFIRM_NAME_SIZE },
- { block_device, false, MGMT_BLOCK_DEVICE_SIZE },
- { unblock_device, false, MGMT_UNBLOCK_DEVICE_SIZE },
- { set_device_id, false, MGMT_SET_DEVICE_ID_SIZE },
- { set_advertising, false, MGMT_SETTING_SIZE },
- { set_bredr, false, MGMT_SETTING_SIZE },
- { set_static_address, false, MGMT_SET_STATIC_ADDRESS_SIZE },
- { set_scan_params, false, MGMT_SET_SCAN_PARAMS_SIZE },
- { set_secure_conn, false, MGMT_SETTING_SIZE },
- { set_debug_keys, false, MGMT_SETTING_SIZE },
- { set_privacy, false, MGMT_SET_PRIVACY_SIZE },
- { load_irks, true, MGMT_LOAD_IRKS_SIZE },
- { get_conn_info, false, MGMT_GET_CONN_INFO_SIZE },
- { get_clock_info, false, MGMT_GET_CLOCK_INFO_SIZE },
- { add_device, false, MGMT_ADD_DEVICE_SIZE },
- { remove_device, false, MGMT_REMOVE_DEVICE_SIZE },
- { load_conn_param, true, MGMT_LOAD_CONN_PARAM_SIZE },
- { read_unconf_index_list, false, MGMT_READ_UNCONF_INDEX_LIST_SIZE },
- { 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 },
+ { read_version, false, MGMT_READ_VERSION_SIZE },
+ { read_commands, false, MGMT_READ_COMMANDS_SIZE },
+ { read_index_list, false, MGMT_READ_INDEX_LIST_SIZE },
+ { read_controller_info, false, MGMT_READ_INFO_SIZE },
+ { set_powered, false, MGMT_SETTING_SIZE },
+ { set_discoverable, false, MGMT_SET_DISCOVERABLE_SIZE },
+ { set_connectable, false, MGMT_SETTING_SIZE },
+ { set_fast_connectable, false, MGMT_SETTING_SIZE },
+ { set_bondable, false, MGMT_SETTING_SIZE },
+ { set_link_security, false, MGMT_SETTING_SIZE },
+ { set_ssp, false, MGMT_SETTING_SIZE },
+ { set_hs, false, MGMT_SETTING_SIZE },
+ { set_le, false, MGMT_SETTING_SIZE },
+ { set_dev_class, false, MGMT_SET_DEV_CLASS_SIZE },
+ { set_local_name, false, MGMT_SET_LOCAL_NAME_SIZE },
+ { add_uuid, false, MGMT_ADD_UUID_SIZE },
+ { remove_uuid, false, MGMT_REMOVE_UUID_SIZE },
+ { load_link_keys, true, MGMT_LOAD_LINK_KEYS_SIZE },
+ { load_long_term_keys, true, MGMT_LOAD_LONG_TERM_KEYS_SIZE },
+ { disconnect, false, MGMT_DISCONNECT_SIZE },
+ { get_connections, false, MGMT_GET_CONNECTIONS_SIZE },
+ { pin_code_reply, false, MGMT_PIN_CODE_REPLY_SIZE },
+ { pin_code_neg_reply, false, MGMT_PIN_CODE_NEG_REPLY_SIZE },
+ { set_io_capability, false, MGMT_SET_IO_CAPABILITY_SIZE },
+ { pair_device, false, MGMT_PAIR_DEVICE_SIZE },
+ { cancel_pair_device, false, MGMT_CANCEL_PAIR_DEVICE_SIZE },
+ { unpair_device, false, MGMT_UNPAIR_DEVICE_SIZE },
+ { user_confirm_reply, false, MGMT_USER_CONFIRM_REPLY_SIZE },
+ { user_confirm_neg_reply, false, MGMT_USER_CONFIRM_NEG_REPLY_SIZE },
+ { user_passkey_reply, false, MGMT_USER_PASSKEY_REPLY_SIZE },
+ { user_passkey_neg_reply, false, MGMT_USER_PASSKEY_NEG_REPLY_SIZE },
+ { read_local_oob_data, false, MGMT_READ_LOCAL_OOB_DATA_SIZE },
+ { add_remote_oob_data, true, MGMT_ADD_REMOTE_OOB_DATA_SIZE },
+ { remove_remote_oob_data, false, MGMT_REMOVE_REMOTE_OOB_DATA_SIZE },
+ { start_discovery, false, MGMT_START_DISCOVERY_SIZE },
+ { stop_discovery, false, MGMT_STOP_DISCOVERY_SIZE },
+ { confirm_name, false, MGMT_CONFIRM_NAME_SIZE },
+ { block_device, false, MGMT_BLOCK_DEVICE_SIZE },
+ { unblock_device, false, MGMT_UNBLOCK_DEVICE_SIZE },
+ { set_device_id, false, MGMT_SET_DEVICE_ID_SIZE },
+ { set_advertising, false, MGMT_SETTING_SIZE },
+ { set_bredr, false, MGMT_SETTING_SIZE },
+ { set_static_address, false, MGMT_SET_STATIC_ADDRESS_SIZE },
+ { set_scan_params, false, MGMT_SET_SCAN_PARAMS_SIZE },
+ { set_secure_conn, false, MGMT_SETTING_SIZE },
+ { set_debug_keys, false, MGMT_SETTING_SIZE },
+ { set_privacy, false, MGMT_SET_PRIVACY_SIZE },
+ { load_irks, true, MGMT_LOAD_IRKS_SIZE },
+ { get_conn_info, false, MGMT_GET_CONN_INFO_SIZE },
+ { get_clock_info, false, MGMT_GET_CLOCK_INFO_SIZE },
+ { add_device, false, MGMT_ADD_DEVICE_SIZE },
+ { remove_device, false, MGMT_REMOVE_DEVICE_SIZE },
+ { load_conn_param, true, MGMT_LOAD_CONN_PARAM_SIZE },
+ { read_unconf_index_list, false, MGMT_READ_UNCONF_INDEX_LIST_SIZE },
+ { 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)
@@ -6808,6 +6880,121 @@ 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
+ * 00000000-0000-1000-8000-00805F9B34FB. We need it for service uuid parsing in
+ * eir
+ */
+static const u8 reverse_base_uuid[16] = {0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00,
+ 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+static void 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; /* how to handle if there's no memory?*/
+
+ memcpy(tmp_uuid->uuid, uuid, 16);
+ INIT_LIST_HEAD(&tmp_uuid->list);
+ list_add(&tmp_uuid->list, uuids);
+}
+
+static void eir_parse(u8 *eir, u8 eir_len, struct list_head *uuids,
+ u8 *txpwr_present, s8 *txpwr)
+{
+ size_t offset;
+ u8 uuid[16];
+ int loop;
+
+ 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)
+ goto failed;
+
+ switch (eir[1]) {
+ case EIR_TX_POWER:
+ *txpwr_present = 1;
+ *txpwr = eir[2];
+ break;
+ case EIR_UUID16_ALL:
+ case EIR_UUID16_SOME:
+ for (loop = 0; loop + 3 <= field_len; loop += 2) {
+ memcpy(uuid, reverse_base_uuid, 16);
+ uuid[13] = eir[loop + 3];
+ uuid[12] = eir[loop + 2];
+ add_uuid_to_list(uuids, uuid);
+ }
+ break;
+ case EIR_UUID32_ALL:
+ case EIR_UUID32_SOME:
+ for (loop = 0; loop + 5 <= field_len; loop += 4) {
+ memcpy(uuid, reverse_base_uuid, 16);
+ uuid[15] = eir[loop + 5];
+ uuid[14] = eir[loop + 4];
+ uuid[13] = eir[loop + 3];
+ uuid[12] = eir[loop + 2];
+ add_uuid_to_list(uuids, uuid);
+ }
+ break;
+ case EIR_UUID128_ALL:
+ case EIR_UUID128_SOME:
+ for (loop = 0; loop + 17 <= field_len; loop += 4) {
+ memcpy(uuid, eir + loop + 2, 16);
+ add_uuid_to_list(uuids, uuid);
+ }
+ break;
+ }
+
+ offset += field_len + 1;
+ eir += field_len + 1;
+ }
+
+failed:
+ offset = 8;
+}
+
+void find_matches(struct hci_dev *hdev, s8 rssi, struct list_head *uuids,
+ u8 txpwr_present, s8 txpwr, bool *full_match,
+ bool *service_match)
+{
+ struct uuid_filter *filter = NULL;
+ struct parsed_uuid *uuidptr = NULL;
+ struct parsed_uuid *tmp = NULL;
+
+ list_for_each_entry_safe(uuidptr, tmp, uuids, list) {
+ list_for_each_entry(filter, &hdev->discov_uuid_filter, list) {
+ if (memcmp(filter->uuid, uuidptr->uuid, 16) == 0) {
+ *service_match = true;
+ if (filter->range_method == MGMT_RANGE_NONE) {
+ *full_match = true;
+ } else if ((filter->range_method &
+ MGMT_RANGE_PATHLOSS) && txpwr_present &&
+ (txpwr - rssi) <= filter->pathloss) {
+ *full_match = true;
+ } else if ((filter->range_method
+ & MGMT_RANGE_RSSI) &&
+ rssi >= filter->rssi) {
+ *full_match = true;
+ }
+ }
+ }
+ __list_del_entry(&uuidptr->list);
+ kfree(uuidptr);
+ }
+}
+
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)
@@ -6853,7 +7040,33 @@ 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 (hdev->service_filter_enabled) {
+ LIST_HEAD(uuids);
+ s8 txpwr = 0;
+ u8 txpwr_present = 0;
+ bool service_match = false, full_match = false;
+
+ eir_parse(eir, eir_len, &uuids, &txpwr_present, &txpwr);
+ if (!list_empty(&uuids)) {
+ find_matches(hdev, rssi, &uuids, txpwr_present, txpwr,
+ &full_match, &service_match);
+
+ if (!service_match)
+ return;
+
+ if (test_bit(HCI_QUIRK_ADV_RSSI_DEDUP, &hdev->quirks)) {
+ queue_delayed_work(hdev->workqueue,
+ &hdev->le_scan_restart,
+ DISCOV_LE_RESTART_DELAY);
+ }
+
+ if (full_match)
+ mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev,
+ ev_size, NULL);
+ }
+ } else {
+ 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,
@@ -6886,10 +7099,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.rc2.206.gedb03e5
^ permalink raw reply related [flat|nested] 6+ messages in thread