LinuxLists.cc - [RFC v3 1/6] Bluetooth: Add HCI_RSSI

2014-12-04 20:22:47

Subject: [RFC v3 1/6] Bluetooth: Add HCI_RSSI_INVALID for unknown RSSI value

From: Marcel Holtmann <[email protected]>

The Bluetooth core specification defines the value 127 as invalid for
RSSI values. So instead of hard coding it, lets add a constant for it.

Signed-off-by: Marcel Holtmann <[email protected]>
Signed-off-by: Jakub Pawlowski <[email protected]>
---
include/net/bluetooth/hci.h | 1 +
1 file changed, 1 insertion(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 569c077..b6f7be1 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -412,6 +412,7 @@ enum {

/* The core spec defines 127 as the "not available" value */
#define HCI_TX_POWER_INVALID 127
+#define HCI_RSSI_INVALID 127

#define HCI_ROLE_MASTER 0x00
#define HCI_ROLE_SLAVE 0x01
--
2.2.0.rc0.207.ga3a616c

2014-12-04 20:22:51

by Jakub Pawlowski

[permalink] [raw]

Subject: [RFC v3 5/6] Bluetooth: add le_scan_restart

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_core.h | 9 +++++++++
net/bluetooth/hci_core.c | 42 ++++++++++++++++++++++++++++++++++++++++
net/bluetooth/hci_event.c | 9 ++++++---
3 files changed, 57 insertions(+), 3 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 83ca58b..ef57213 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -56,6 +56,13 @@ struct inquiry_entry {
struct inquiry_data data;
};

+/* BR/EDR and/or LE discovery state flags: the flags defined here should
+ * represent state of discovery
+ */
+enum {
+ HCI_LE_SCAN_RESTARTING,
+};
+
struct discovery_state {
int type;
enum {
@@ -75,6 +82,7 @@ struct discovery_state {
u32 last_adv_flags;
u8 last_adv_data[HCI_MAX_AD_LENGTH];
u8 last_adv_data_len;
+ unsigned long flags;
s8 rssi;
u16 uuid_count;
u8 (*uuids)[16];
@@ -342,6 +350,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];
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 42f86dc..35a4479 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2616,6 +2616,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);
@@ -3884,6 +3885,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);
@@ -3893,6 +3896,44 @@ 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)
+{
+ clear_bit(HCI_LE_SCAN_RESTARTING, &hdev->discovery.flags);
+
+ if (status)
+ BT_ERR("Failed to restart LE scan: 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);
+
+ /* If controller is not scanning we are done. */
+ if (!test_bit(HCI_LE_SCAN, &hdev->dev_flags))
+ return;
+
+ hci_req_init(&req, hdev);
+
+ hci_req_add_le_scan_disable(&req);
+
+ memset(&cp, 0, sizeof(cp));
+ cp.enable = LE_SCAN_ENABLE;
+ cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE;
+ hci_req_add(&req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp);
+
+ set_bit(HCI_LE_SCAN_RESTARTING, &hdev->discovery.flags);
+
+ err = hci_req_run(&req, le_scan_restart_work_complete);
+ if (err)
+ BT_ERR("Restart LE scan request failed: err %d", err);
+}
+
static void set_random_addr(struct hci_request *req, bdaddr_t *rpa)
{
struct hci_dev *hdev = req->hdev;
@@ -4070,6 +4111,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 f4e2a61..8c580d1 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->discovery.flags))
+ cancel_delayed_work(&hdev->le_scan_disable);

clear_bit(HCI_LE_SCAN, &hdev->dev_flags);

--
2.2.0.rc0.207.ga3a616c

2014-12-04 20:22:52

by Jakub Pawlowski

[permalink] [raw]

Subject: [RFC v3 5/6] Bluetooth: Add le_scan_restart

2014-12-04 20:22:49

by Jakub Pawlowski

[permalink] [raw]

Subject: [RFC v3 3/6] Bluetooth: Add extra discovery fields for storing filter information

With the upcoming addition of support for Start Service Discovery, the
discovery handling needs to filter on RSSI and UUID values. For that
they need to be stored in the discovery handling. This patch adds the
appropriate fields and also make sure they are reset when discovery
has been stopped.

Signed-off-by: Jakub Pawlowski <[email protected]>
Signed-off-by: Marcel Holtmann <[email protected]>
---
include/net/bluetooth/hci_core.h | 4 ++++
net/bluetooth/hci_core.c | 14 ++++++++++++++
net/bluetooth/mgmt.c | 2 ++
3 files changed, 20 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1dae700..83ca58b 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -75,6 +75,9 @@ struct discovery_state {
u32 last_adv_flags;
u8 last_adv_data[HCI_MAX_AD_LENGTH];
u8 last_adv_data_len;
+ s8 rssi;
+ u16 uuid_count;
+ u8 (*uuids)[16];
};

struct hci_conn_hash {
@@ -503,6 +506,7 @@ static inline void discovery_init(struct hci_dev *hdev)
INIT_LIST_HEAD(&hdev->discovery.all);
INIT_LIST_HEAD(&hdev->discovery.unknown);
INIT_LIST_HEAD(&hdev->discovery.resolve);
+ hdev->discovery.rssi = HCI_RSSI_INVALID;
}

bool hci_discovery_active(struct hci_dev *hdev);
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index f001856..42f86dc 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2052,6 +2052,20 @@ void hci_discovery_set_state(struct hci_dev *hdev, int state)
case DISCOVERY_STOPPED:
hci_update_background_scan(hdev);

+ /* Reset RSSI and UUID filters to ensure Start Discovery
+ * and Start Service Discovery operate properly no matter
+ * which one started the previous discovery.
+ *
+ * While the Start Discovery and Start Service Discovery
+ * operations will set proper values for RSSI and UUID
+ * count, it is important to actually free the allocated
+ * list of UUIDs here.
+ */
+ hdev->discovery.rssi = HCI_RSSI_INVALID;
+ hdev->discovery.uuid_count = 0;
+ kfree(hdev->discovery.uuids);
+ hdev->discovery.uuids = NULL;
+
if (old_state != DISCOVERY_STARTING)
mgmt_discovering(hdev, 0);
break;
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 415ba41..b6a0f3e 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -3867,6 +3867,8 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
}

hdev->discovery.type = cp->type;
+ hdev->discovery.rssi = HCI_RSSI_INVALID;
+ hdev->discovery.uuid_count = 0;

hci_req_init(&req, hdev);

--
2.2.0.rc0.207.ga3a616c

2014-12-04 20:22:53

by Jakub Pawlowski

[permalink] [raw]

Subject: [RFC v3 6/6] Bluetooth: Add service discovery filtering

This patch adds support for LE packet filtering when service discovery
is running.

Signed-off-by: Jakub Pawlowski <[email protected]>
---
include/net/bluetooth/hci_core.h | 1 +
net/bluetooth/mgmt.c | 92 +++++++++++++++++++++++++++++++++++++++-
2 files changed, 92 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index ef57213..1e9e88e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -508,6 +508,7 @@ int sco_recv_scodata(struct hci_conn *hcon, struct sk_buff *skb);
/* ----- Inquiry cache ----- */
#define INQUIRY_CACHE_AGE_MAX (HZ*30) /* 30 seconds */
#define INQUIRY_ENTRY_AGE_MAX (HZ*60) /* 60 seconds */
+#define DISCOV_LE_RESTART_DELAY 200 /* msec */

static inline void discovery_init(struct hci_dev *hdev)
{
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index b0363851..66bff27 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -6908,6 +6908,83 @@ void mgmt_read_local_oob_data_complete(struct hci_dev *hdev, u8 *hash192,
mgmt_pending_remove(cmd);
}

+/* 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 bool cmp_filter_uuids(struct hci_dev *hdev, u8 uuid[16])
+{
+ int i;
+
+ for (i = 0; i < hdev->discovery.uuid_count; i++)
+ if (memcmp(hdev->discovery.uuids[i], uuid, 16) != 0)
+ return true;
+
+ return false;
+}
+
+static bool find_service_discov_match(struct hci_dev *hdev, u8 *eir, u8 eir_len)
+{
+ size_t offset;
+ u8 uuid[16];
+ int i;
+
+ 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);
+ memcpy(uuid + 12, eir + i + 2, 2);
+ if (cmp_filter_uuids(hdev, uuid))
+ return true;
+ }
+ break;
+ case EIR_UUID32_ALL:
+ case EIR_UUID32_SOME:
+ for (i = 0; i + 5 <= field_len; i += 4) {
+ memcpy(uuid, reverse_base_uuid, 16);
+ memcpy(uuid + 12, eir + i + 2, 4);
+ if (cmp_filter_uuids(hdev, uuid))
+ return true;
+ }
+ break;
+ case EIR_UUID128_ALL:
+ case EIR_UUID128_SOME:
+ for (i = 0; i + 17 <= field_len; i += 16) {
+ memcpy(uuid, eir + i + 2, 16);
+ if (cmp_filter_uuids(hdev, uuid))
+ return true;
+ }
+ break;
+ }
+
+ offset += field_len + 1;
+ eir += field_len + 1;
+ }
+ return false;
+}
+
+static void restart_le_scan(struct hci_dev *hdev)
+{
+ queue_delayed_work(hdev->workqueue, &hdev->le_scan_restart,
+ msecs_to_jiffies(DISCOV_LE_RESTART_DELAY));
+}
+
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)
@@ -6953,7 +7030,20 @@ 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->discovery.rssi == 127 && hdev->discovery.uuid_count == 0) {
+ mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL);
+ return;
+ }
+
+ if (!find_service_discov_match(hdev, eir, eir_len))
+ return;
+
+ if (test_bit(HCI_LE_SCAN, &hdev->dev_flags))
+ restart_le_scan(hdev);
+
+ if (rssi >= hdev->discovery.rssi)
+ 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,
--
2.2.0.rc0.207.ga3a616c

2014-12-04 20:22:50

by Jakub Pawlowski

[permalink] [raw]

Subject: [RFC v3 4/6] Bluetooth: Add support for Start Service Discovery command

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

Signed-off-by: Jakub Pawlowski <[email protected]>
Signed-off-by: Marcel Holtmann <[email protected]>
---
net/bluetooth/mgmt.c | 106 +++++++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 106 insertions(+)

diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index b6a0f3e..b0363851 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -93,6 +93,7 @@ static const u16 mgmt_commands[] = {
MGMT_OP_READ_CONFIG_INFO,
MGMT_OP_SET_EXTERNAL_CONFIG,
MGMT_OP_SET_PUBLIC_ADDRESS,
+ MGMT_OP_START_SERVICE_DISCOVERY,
};

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

cmd = mgmt_pending_find(MGMT_OP_START_DISCOVERY, hdev);
+ if (!cmd)
+ cmd = mgmt_pending_find(MGMT_OP_START_SERVICE_DISCOVERY, hdev);
+
if (cmd) {
u8 type = hdev->discovery.type;

@@ -3892,6 +3896,107 @@ failed:
return err;
}

+static int start_service_discovery(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ struct mgmt_cp_start_service_discovery *cp = data;
+ struct pending_cmd *cmd;
+ struct hci_request req;
+ const u16 max_uuid_count = ((U16_MAX - sizeof(*cp)) / 16);
+ u16 uuid_count, expected_len;
+ u8 status;
+ int err;
+
+ BT_DBG("%s", hdev->name);
+
+ hci_dev_lock(hdev);
+
+ if (!hdev_is_powered(hdev)) {
+ err = cmd_complete(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_NOT_POWERED,
+ &cp->type, sizeof(cp->type));
+ goto failed;
+ }
+
+ if (hdev->discovery.state != DISCOVERY_STOPPED ||
+ test_bit(HCI_PERIODIC_INQ, &hdev->dev_flags)) {
+ err = cmd_complete(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_BUSY, &cp->type,
+ sizeof(cp->type));
+ goto failed;
+ }
+
+ uuid_count = __le16_to_cpu(cp->uuid_count);
+ if (uuid_count > max_uuid_count) {
+ BT_ERR("service_discovery: too big uuid_count value %u",
+ uuid_count);
+ err = cmd_complete(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_INVALID_PARAMS, &cp->type,
+ sizeof(cp->type));
+ goto failed;
+ }
+
+ expected_len = sizeof(*cp) + uuid_count * 16;
+ if (expected_len != len) {
+ BT_ERR("service_discovery: expected %u bytes, got %u bytes",
+ expected_len, len);
+ err = cmd_complete(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_INVALID_PARAMS, &cp->type,
+ sizeof(cp->type));
+ goto failed;
+ }
+
+ cmd = mgmt_pending_add(sk, MGMT_OP_START_SERVICE_DISCOVERY,
+ hdev, NULL, 0);
+ if (!cmd) {
+ err = -ENOMEM;
+ goto failed;
+ }
+
+ hdev->discovery.type = cp->type;
+ hdev->discovery.rssi = cp->rssi;
+ hdev->discovery.uuid_count = uuid_count;
+
+ if (uuid_count > 0) {
+ hdev->discovery.uuids = kmemdup(cp->uuids, uuid_count * 16,
+ GFP_KERNEL);
+ if (!hdev->discovery.uuids) {
+ err = cmd_complete(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ MGMT_STATUS_FAILED,
+ &cp->type, sizeof(cp->type));
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+ }
+
+ hci_req_init(&req, hdev);
+
+ if (!trigger_discovery(&req, &status)) {
+ err = cmd_complete(sk, hdev->id,
+ MGMT_OP_START_SERVICE_DISCOVERY,
+ status, &cp->type, sizeof(cp->type));
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ err = hci_req_run(&req, start_discovery_complete);
+ if (err < 0) {
+ mgmt_pending_remove(cmd);
+ goto failed;
+ }
+
+ hci_discovery_set_state(hdev, DISCOVERY_STARTING);
+
+failed:
+ hci_dev_unlock(hdev);
+ return err;
+}
+
static void stop_discovery_complete(struct hci_dev *hdev, u8 status)
{
struct pending_cmd *cmd;
@@ -5726,6 +5831,7 @@ static const struct mgmt_handler {
{ read_config_info, false, MGMT_READ_CONFIG_INFO_SIZE },
{ set_external_config, false, MGMT_SET_EXTERNAL_CONFIG_SIZE },
{ set_public_address, false, MGMT_SET_PUBLIC_ADDRESS_SIZE },
+ { start_service_discovery,true, MGMT_START_SERVICE_DISCOVERY_SIZE },
};

int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
--
2.2.0.rc0.207.ga3a616c

2014-12-04 20:22:48

by Jakub Pawlowski

[permalink] [raw]

Subject: [RFC v3 2/6] Bluetooth: Add definitions for MGMT_OP_START_SERVICE_DISCOVERY

This patch adds the opcode and structure for Start Service Discovery
operation.

Signed-off-by: Jakub Pawlowski <[email protected]>
Signed-off-by: Marcel Holtmann <[email protected]>
---
include/net/bluetooth/mgmt.h | 9 +++++++++
1 file changed, 9 insertions(+)

diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index 9b382ea..95c34d5 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -498,6 +498,15 @@ struct mgmt_cp_set_public_address {
} __packed;
#define MGMT_SET_PUBLIC_ADDRESS_SIZE 6

+#define MGMT_OP_START_SERVICE_DISCOVERY 0x003A
+struct mgmt_cp_start_service_discovery {
+ __u8 type;
+ __s8 rssi;
+ __le16 uuid_count;
+ __u8 uuids[0][16];
+} __packed;
+#define MGMT_START_SERVICE_DISCOVERY_SIZE 4
+
#define MGMT_EV_CMD_COMPLETE 0x0001
struct mgmt_ev_cmd_complete {
__le16 opcode;
--
2.2.0.rc0.207.ga3a616c