Return-Path: From: Jakub Pawlowski To: linux-bluetooth@vger.kernel.org Cc: Jakub Pawlowski Subject: [PATCH] Add new method - service discovery to mgmt Date: Thu, 23 Oct 2014 10:15:35 -0700 Message-Id: <1414084535-23864-2-git-send-email-jpawlowski@google.com> In-Reply-To: <1414084535-23864-1-git-send-email-jpawlowski@google.com> References: <1414084535-23864-1-git-send-email-jpawlowski@google.com> Sender: linux-bluetooth-owner@vger.kernel.org List-ID: --- 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