@@ -257,6 +257,15 @@ struct adv_info {
#define HCI_ADV_TX_POWER_NO_PREFERENCE 0x7F
+struct monitored_device {
+ struct list_head list;
+
+ bdaddr_t bdaddr;
+ __u8 addr_type;
+ __u16 handle;
+ bool notified;
+};
+
struct adv_pattern {
struct list_head list;
__u8 ad_type;
@@ -588,6 +597,8 @@ struct hci_dev {
struct delayed_work interleave_scan;
+ struct list_head monitored_devices;
+
#if IS_ENABLED(CONFIG_BT_LEDS)
struct led_trigger *power_led;
#endif
@@ -2503,6 +2503,7 @@ struct hci_dev *hci_alloc_dev_priv(int sizeof_priv)
INIT_LIST_HEAD(&hdev->conn_hash.list);
INIT_LIST_HEAD(&hdev->adv_instances);
INIT_LIST_HEAD(&hdev->blocked_keys);
+ INIT_LIST_HEAD(&hdev->monitored_devices);
INIT_LIST_HEAD(&hdev->local_codecs);
INIT_WORK(&hdev->rx_work, hci_rx_work);
@@ -80,6 +80,14 @@ struct msft_rp_le_set_advertisement_filter_enable {
__u8 sub_opcode;
} __packed;
+#define MSFT_EV_LE_MONITOR_DEVICE 0x02
+struct msft_ev_le_monitor_device {
+ __u8 addr_type;
+ bdaddr_t bdaddr;
+ __u8 monitor_handle;
+ __u8 monitor_state;
+} __packed;
+
struct msft_monitor_advertisement_handle_data {
__u8 msft_handle;
__u16 mgmt_handle;
@@ -266,6 +274,7 @@ static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
struct msft_data *msft = hdev->msft_data;
int err;
bool pending;
+ struct monitored_device *dev, *tmp;
if (status)
goto done;
@@ -296,6 +305,15 @@ static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
list_del(&handle_data->list);
kfree(handle_data);
+
+ /* Clear any monitored devices by this Adv Monitor */
+ list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices,
+ list) {
+ if (dev->handle == handle_data->mgmt_handle) {
+ list_del(&dev->list);
+ kfree(dev);
+ }
+ }
}
/* If remove all monitors is required, we need to continue the process
@@ -538,6 +556,7 @@ void msft_do_close(struct hci_dev *hdev)
struct msft_data *msft = hdev->msft_data;
struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
struct adv_monitor *monitor;
+ struct monitored_device *dev, *tmp_dev;
if (!msft)
return;
@@ -557,6 +576,16 @@ void msft_do_close(struct hci_dev *hdev)
list_del(&handle_data->list);
kfree(handle_data);
}
+
+ hci_dev_lock(hdev);
+
+ /* Clear any devices that are being monitored */
+ list_for_each_entry_safe(dev, tmp_dev, &hdev->monitored_devices, list) {
+ list_del(&dev->list);
+ kfree(dev);
+ }
+
+ hci_dev_unlock(hdev);
}
void msft_register(struct hci_dev *hdev)
@@ -590,6 +619,90 @@ void msft_unregister(struct hci_dev *hdev)
kfree(msft);
}
+/* This function requires the caller holds hdev->lock */
+static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
+ __u8 addr_type, __u16 mgmt_handle)
+{
+ struct monitored_device *dev;
+
+ dev = kmalloc(sizeof(*dev), GFP_KERNEL);
+ if (!dev) {
+ bt_dev_err(hdev, "MSFT vendor event %u: no memory",
+ MSFT_EV_LE_MONITOR_DEVICE);
+ return;
+ }
+
+ bacpy(&dev->bdaddr, bdaddr);
+ dev->addr_type = addr_type;
+ dev->handle = mgmt_handle;
+ dev->notified = false;
+
+ INIT_LIST_HEAD(&dev->list);
+ list_add(&dev->list, &hdev->monitored_devices);
+}
+
+/* This function requires the caller holds hdev->lock */
+static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
+ __u8 addr_type, __u16 mgmt_handle)
+{
+ struct monitored_device *dev, *tmp;
+
+ list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
+ if (dev->handle == mgmt_handle) {
+ list_del(&dev->list);
+ kfree(dev);
+
+ break;
+ }
+ }
+}
+
+/* This function requires the caller holds hdev->lock */
+static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+ struct msft_ev_le_monitor_device *ev = (void *)skb->data;
+ struct msft_monitor_advertisement_handle_data *handle_data;
+ u8 addr_type;
+
+ if (skb->len < sizeof(*ev)) {
+ bt_dev_err(hdev,
+ "MSFT vendor event %u: insufficient data (len: %u)",
+ MSFT_EV_LE_MONITOR_DEVICE, skb->len);
+ return;
+ }
+ skb_pull(skb, sizeof(*ev));
+
+ bt_dev_dbg(hdev,
+ "MSFT vendor event %u: handle 0x%04x state %d addr %pMR",
+ MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
+ ev->monitor_state, &ev->bdaddr);
+
+ handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
+
+ switch (ev->addr_type) {
+ case ADDR_LE_DEV_PUBLIC:
+ addr_type = BDADDR_LE_PUBLIC;
+ break;
+
+ case ADDR_LE_DEV_RANDOM:
+ addr_type = BDADDR_LE_RANDOM;
+ break;
+
+ default:
+ bt_dev_err(hdev,
+ "MSFT vendor event %u: unknown addr type 0x%02x",
+ ev->addr_type);
+ return;
+ }
+
+ if (ev->monitor_state)
+ msft_device_found(hdev, &ev->bdaddr, addr_type,
+ handle_data->mgmt_handle);
+ else
+ msft_device_lost(hdev, &ev->bdaddr, addr_type,
+ handle_data->mgmt_handle);
+}
+
void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
struct msft_data *msft = hdev->msft_data;
@@ -617,10 +730,22 @@ void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
if (skb->len < 1)
return;
+ hci_dev_lock(hdev);
+
event = *skb->data;
skb_pull(skb, 1);
- bt_dev_dbg(hdev, "MSFT vendor event %u", event);
+ switch (event) {
+ case MSFT_EV_LE_MONITOR_DEVICE:
+ msft_monitor_device_evt(hdev, skb);
+ break;
+
+ default:
+ bt_dev_dbg(hdev, "MSFT vendor event %u", event);
+ break;
+ }
+
+ hci_dev_unlock(hdev);
}
__u64 msft_get_features(struct hci_dev *hdev)