---
include/net/bluetooth/hci_core.h | 16 ++
include/net/bluetooth/mgmt.h | 26 ++
net/bluetooth/hci_core.c | 42 ++++
net/bluetooth/hci_event.c | 3 +-
net/bluetooth/mgmt.c | 499 ++++++++++++++++++++++++++++++++++++++-
5 files changed, 582 insertions(+), 4 deletions(-)
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b8685a7..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,10 @@ 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;
struct list_head blacklist;
struct list_head whitelist;
@@ -334,6 +346,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];
@@ -1303,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);
@@ -1369,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..8d652b3 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_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_START_SERVICE_DISCOVERY_SIZE 1
+
+#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 cb05d7f..af6bf39 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2580,6 +2580,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);
@@ -3846,6 +3847,7 @@ static void le_scan_disable_work(struct work_struct *work)
BT_DBG("%s", hdev->name);
+ clean_up_service_discovery(hdev);
hci_req_init(&req, hdev);
hci_req_add_le_scan_disable(&req);
@@ -3855,6 +3857,41 @@ static void le_scan_disable_work(struct work_struct *work)
BT_ERR("Disable LE scanning request failed: err %d", err);
}
+static void le_scan_restart_work_complete(struct hci_dev *hdev, u8 status)
+{
+ hdev->scan_restart = false;
+
+ if (status) {
+ BT_ERR("Failed to restart LE scanning: status %d", status);
+ return;
+ }
+}
+
+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);
+
+ hdev->scan_restart = true;
+
+ 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;
@@ -4010,6 +4047,10 @@ struct hci_dev *hci_alloc_dev(void)
mutex_init(&hdev->lock);
mutex_init(&hdev->req_lock);
+ INIT_LIST_HEAD(&hdev->discov_uuid_filter);
+ hdev->service_filter_enabled = false;
+ hdev->scan_restart = false;
+
INIT_LIST_HEAD(&hdev->mgmt_pending);
INIT_LIST_HEAD(&hdev->blacklist);
INIT_LIST_HEAD(&hdev->whitelist);
@@ -4032,6 +4073,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 9629153..b372b02 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1155,7 +1155,8 @@ static void hci_cc_le_set_scan_enable(struct hci_dev *hdev,
/* Cancel this timer so that we don't try to disable scanning
* when it's already disabled.
*/
- cancel_delayed_work(&hdev->le_scan_disable);
+ if(!hdev->scan_restart)
+ cancel_delayed_work(&hdev->le_scan_disable);
clear_bit(HCI_LE_SCAN, &hdev->dev_flags);
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 9c4daf7..08d48a7 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)
+{
+ if (hdev->service_filter_enabled) {
+ struct uuid_filter *filterptr = NULL;
+ struct uuid_filter *tmp = NULL;
+
+ hdev->service_filter_enabled = false;
+ cancel_delayed_work_sync(&hdev->le_scan_restart);
+
+ if(!list_empty(&hdev->discov_uuid_filter)) {
+ list_for_each_entry_safe(filterptr, tmp,
+ &hdev->discov_uuid_filter, list)
+ {
+ __list_del_entry(&(filterptr->list));
+ kfree(filterptr);
+ }
+ }
+ }
+}
+
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);
}
@@ -5641,6 +5664,345 @@ unlock:
return err;
}
+static int mgmt_start_service_discovery_failed(struct hci_dev *hdev, u8 status)
+{
+ struct pending_cmd *cmd;
+ u8 type;
+ int err;
+
+ hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
+
+ cmd = mgmt_pending_find(MGMT_OP_START_SERVICE_DISCOVERY, hdev);
+ if (!cmd)
+ return -ENOENT;
+
+ type = hdev->discovery.type;
+
+ err = cmd_complete(cmd->sk, hdev->id, cmd->opcode, mgmt_status(status),
+ &type, sizeof(type));
+ mgmt_pending_remove(cmd);
+
+ return err;
+}
+
+static void start_service_discovery_complete(struct hci_dev *hdev, u8 status)
+{
+ unsigned long timeout = 0;
+
+ BT_DBG("status %d", status);
+
+ if (status) {
+ hci_dev_lock(hdev);
+ mgmt_start_service_discovery_failed(hdev, status);
+ hci_dev_unlock(hdev);
+ return;
+ }
+
+ hci_dev_lock(hdev);
+ hci_discovery_set_state(hdev, DISCOVERY_FINDING);
+ hci_dev_unlock(hdev);
+
+ switch (hdev->discovery.type) {
+ case DISCOV_TYPE_LE:
+ timeout = msecs_to_jiffies(DISCOV_LE_TIMEOUT);
+ break;
+
+ case DISCOV_TYPE_INTERLEAVED:
+ timeout = msecs_to_jiffies(hdev->discov_interleaved_timeout);
+ break;
+
+ case DISCOV_TYPE_BREDR:
+ break;
+
+ default:
+ BT_ERR("Invalid discovery type %d", hdev->discovery.type);
+ }
+
+ if (!timeout)
+ return;
+
+ queue_delayed_work(hdev->workqueue, &hdev->le_scan_disable, timeout);
+}
+
+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_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) */
+ u8 lap[3] = { 0x33, 0x8b, 0x9e };
+ u8 status, own_addr_type;
+ int i, err;
+ u16 filter_count, expected_len;
+
+ 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 bytes, got %u bytes",
+ expected_len, len);
+
+ return cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_INVALID_PARAMS);
+ }
+
+ hci_dev_lock(hdev);
+
+ if (!hdev_is_powered(hdev)) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_NOT_POWERED);
+ goto failed;
+ }
+
+ if (test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_BUSY);
+ goto failed;
+ }
+
+ if (hdev->discovery.state != DISCOVERY_STOPPED) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_BUSY);
+ 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;
+
+ hci_req_init(&req, hdev);
+
+ switch (hdev->discovery.type) {
+ case DISCOV_TYPE_BREDR:
+ status = mgmt_bredr_support(hdev);
+ if (status) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ status);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ if (test_bit(HCI_INQUIRY, &hdev->flags)) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_BUSY);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ hci_inquiry_cache_flush(hdev);
+
+ memset(&inq_cp, 0, sizeof(inq_cp));
+ memcpy(&inq_cp.lap, lap, sizeof(inq_cp.lap));
+ inq_cp.length = DISCOV_BREDR_INQUIRY_LEN;
+ hci_req_add(&req, HCI_OP_INQUIRY, sizeof(inq_cp), &inq_cp);
+ break;
+
+ case DISCOV_TYPE_LE:
+ case DISCOV_TYPE_INTERLEAVED:
+ status = mgmt_le_support(hdev);
+ if (status) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ status);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ if (hdev->discovery.type == DISCOV_TYPE_INTERLEAVED &&
+ !test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_NOT_SUPPORTED);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ if (test_bit(HCI_LE_ADV, &hdev->dev_flags)) {
+ /* Don't let discovery abort an outgoing
+ * connection attempt that's using directed
+ * advertising.
+ */
+ if (hci_conn_hash_lookup_state(hdev, LE_LINK,
+ BT_CONNECT)) {
+ err = cmd_status(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_REJECTED);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ disable_advertising(&req);
+ }
+
+ /* If controller is scanning, it means the background scanning
+ * is running. Thus, we should temporarily stop it in order to
+ * set the discovery scanning parameters.
+ */
+ if (test_bit(HCI_LE_SCAN, &hdev->dev_flags))
+ hci_req_add_le_scan_disable(&req);
+
+ memset(¶m_cp, 0, sizeof(param_cp));
+
+ /* All active scans will be done with either a resolvable
+ * private address (when privacy feature has been enabled)
+ * or unresolvable private address.
+ */
+ err = hci_update_random_address(&req, true, &own_addr_type);
+ if (err < 0) {
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_FAILED);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ param_cp.type = LE_SCAN_ACTIVE;
+ param_cp.interval = cpu_to_le16(DISCOV_LE_SCAN_INT);
+ param_cp.window = cpu_to_le16(DISCOV_LE_SCAN_WIN);
+ param_cp.own_address_type = own_addr_type;
+ hci_req_add(&req, HCI_OP_LE_SET_SCAN_PARAM, sizeof(param_cp),
+ ¶m_cp);
+
+ memset(&enable_cp, 0, sizeof(enable_cp));
+ enable_cp.enable = LE_SCAN_ENABLE;
+ enable_cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE;
+ hci_req_add(&req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(enable_cp),
+ &enable_cp);
+ break;
+
+ default:
+ err = cmd_status(sk, hdev->id, MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_INVALID_PARAMS);
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ for (i = 0; i < filter_count; i++) {
+ struct mgmt_uuid_filter *key = &cp->filter[i];
+ struct uuid_filter *tmp_uuid;
+
+ tmp_uuid = kmalloc(sizeof(struct uuid_filter), GFP_KERNEL);
+ if (!tmp_uuid)
+ return -ENOMEM;
+
+ memcpy(tmp_uuid->uuid, key->uuid, 16);
+ tmp_uuid->range_method = key->range_method;
+ tmp_uuid->pathloss = key->pathloss;
+ tmp_uuid->rssi = key->rssi;
+ INIT_LIST_HEAD( &tmp_uuid->list ) ;
+
+ list_add(&tmp_uuid->list, &hdev->discov_uuid_filter);
+ }
+ hdev->service_filter_enabled = true;
+
+ err = hci_req_run(&req, start_service_discovery_complete);
+ if (err < 0)
+ mgmt_pending_remove(cmd);
+ else
+ hci_discovery_set_state(hdev, DISCOVERY_STARTING);
+
+failed:
+ hci_dev_unlock(hdev);
+ return err;
+}
+
+static int mgmt_stop_service_discovery_failed(struct hci_dev *hdev, u8 status)
+{
+ struct pending_cmd *cmd;
+ int err;
+
+ cmd = mgmt_pending_find(MGMT_OP_STOP_SERVICE_DISCOVERY, hdev);
+ if (!cmd)
+ return -ENOENT;
+
+ hdev->service_filter_enabled = false;
+ err = cmd_complete(cmd->sk, hdev->id, cmd->opcode, mgmt_status(status),
+ &hdev->discovery.type, sizeof(hdev->discovery.type));
+ mgmt_pending_remove(cmd);
+
+ return err;
+}
+
+static void stop_service_discovery_complete(struct hci_dev *hdev, u8 status)
+{
+ BT_DBG("status %d", status);
+
+ hci_dev_lock(hdev);
+
+ if (status) {
+ mgmt_stop_service_discovery_failed(hdev, status);
+ goto unlock;
+ }
+
+ hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
+
+unlock:
+ hci_dev_unlock(hdev);
+}
+
+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;
+ struct pending_cmd *cmd;
+ struct hci_request req;
+ int err;
+
+ BT_DBG("%s", hdev->name);
+
+ hci_dev_lock(hdev);
+
+ if (!hci_discovery_active(hdev)) {
+ err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_SERVICE_DISCOVERY,
+ 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_SERVICE_DISCOVERY,
+ MGMT_STATUS_INVALID_PARAMS, &mgmt_cp->type,
+ sizeof(mgmt_cp->type));
+ goto unlock;
+ }
+
+ cmd = mgmt_pending_add(sk, MGMT_OP_STOP_SERVICE_DISCOVERY, hdev, NULL, 0);
+ if (!cmd) {
+ err = -ENOMEM;
+ goto unlock;
+ }
+
+ hci_req_init(&req, hdev);
+
+ hci_stop_discovery(&req);
+
+ err = hci_req_run(&req, stop_service_discovery_complete);
+ if (!err) {
+ hci_discovery_set_state(hdev, DISCOVERY_STOPPING);
+ goto unlock;
+ }
+
+ mgmt_pending_remove(cmd);
+
+ /* If no HCI commands were sent we're done */
+ if (err == -ENODATA) {
+ err = cmd_complete(sk, hdev->id, MGMT_OP_STOP_SERVICE_DISCOVERY, 0,
+ &mgmt_cp->type, sizeof(mgmt_cp->type));
+ hci_discovery_set_state(hdev, DISCOVERY_STOPPED);
+ }
+
+unlock:
+ hci_dev_unlock(hdev);
+ return err;
+}
+
static const struct mgmt_handler {
int (*func) (struct sock *sk, struct hci_dev *hdev, void *data,
u16 data_len);
@@ -5705,6 +6067,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)
@@ -6774,6 +7138,85 @@ 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];
+};
+
+//static u8 baseUUID[16] = {0x00,0x00,0x00,0x00,/*-*/0x00,0x00,/*-*/0x10,0x00,/*-*/ 0x80,0x00,/*-*/0x00,0x80,0x5f,0x9b,0x34,0xfb};
+static 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(struct parsed_uuid), GFP_KERNEL);
+ if (tmp_uuid == NULL)
+ return; //TODO: 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 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)
@@ -6819,7 +7262,52 @@ 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;
+ u8 txpwr_present = 0;
+ struct uuid_filter *filterptr = NULL;
+ struct parsed_uuid *uuidptr = NULL;
+ struct parsed_uuid *tmp = NULL;
+ bool service_match = false, full_match = false;
+
+ eir_parse(eir, eir_len, &uuids, &txpwr_present, &txpwr);
+ if(!list_empty(&uuids)) {
+ list_for_each_entry_safe(uuidptr, tmp, &uuids, list)
+ {
+ list_for_each_entry(filterptr, &hdev->discov_uuid_filter, list)
+ {
+ if(memcmp(filterptr->uuid, uuidptr->uuid, 16) == 0) {
+ service_match = true;
+
+ if( filterptr->range_method == MGMT_RANGE_NONE) {
+ full_match = true;
+ } else if ( (filterptr->range_method & MGMT_RANGE_PATHLOSS)
+ && txpwr_present
+ && (txpwr - rssi) <= filterptr->pathloss ) {
+ full_match = true;
+ } else if( (filterptr->range_method & MGMT_RANGE_RSSI)
+ && rssi >= filterptr->rssi ) {
+ full_match = true;
+ }
+
+ }
+ }
+ __list_del_entry(&(uuidptr->list));
+ kfree(uuidptr);
+ }
+ }
+
+ if(full_match) {
+ mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL);
+ }
+ if(service_match) {
+ queue_delayed_work(hdev->workqueue, &hdev->le_scan_restart,
+ DISCOV_LE_RESTART_DELAY);
+ }
+ } 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,
@@ -6852,10 +7340,15 @@ 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
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
