Bluetooth: msft: Handle MSFT Monitor Device Event

Whenever the controller starts/stops monitoring a bt device, it sends
MSFT Monitor Device event. Add handler to read this vendor event.

Test performed:
- Verified by logs that the MSFT Monitor Device event is received from
  the controller whenever it starts/stops monitoring a device.

Signed-off-by: Manish Mandlik <mmandlik@google.com>
Reviewed-by: Miao-chen Chou <mcchou@google.com>
Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 586f69d084a2a0d1ec4868cf035ff449633671bf..639fb9f57ae750053b357f24a89a888fe805881d 100644 (file)
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -258,6 +258,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;
@@ -591,6 +600,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

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 2b7bd3655b076634773fa08065071101c2273819..14c2da9d33ff47ce3b75c5e899304815c62270d1 100644 (file)
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -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);

diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c
index 6a943634b31a26ab0ddf54d2b89472b155c86415..213eab2f085a34dc5a19c131de1f9c9f24ea9afb 100644 (file)
--- a/net/bluetooth/msft.c
+++ b/net/bluetooth/msft.c
@@ -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;
@@ -204,6 +212,30 @@ static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
        return NULL;
 }
 
+/* This function requires the caller holds hdev->lock */
+static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
+                                  bdaddr_t *bdaddr, __u8 addr_type)
+{
+       struct monitored_device *dev, *tmp;
+       int count = 0;
+
+       list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
+               /* mgmt_handle == 0 indicates remove all devices, whereas,
+                * bdaddr == NULL indicates remove all devices matching the
+                * mgmt_handle.
+                */
+               if ((!mgmt_handle || dev->handle == mgmt_handle) &&
+                   (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
+                                addr_type == dev->addr_type))) {
+                       list_del(&dev->list);
+                       kfree(dev);
+                       count++;
+               }
+       }
+
+       return count;
+}
+
 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
                                             u8 status, u16 opcode,
                                             struct sk_buff *skb)
@@ -294,6 +326,10 @@ static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
                if (monitor && !msft->suspending)
                        hci_free_adv_monitor(hdev, monitor);
 
+               /* Clear any monitored devices by this Adv Monitor */
+               msft_monitor_device_del(hdev, handle_data->mgmt_handle, NULL,
+                                       0);
+
                list_del(&handle_data->list);
                kfree(handle_data);
        }
@@ -557,6 +593,13 @@ 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 */
+       msft_monitor_device_del(hdev, 0, NULL, 0);
+
+       hci_dev_unlock(hdev);
 }
 
 void msft_register(struct hci_dev *hdev)
@@ -590,10 +633,97 @@ 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)
+{
+       if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type)) {
+               bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
+                          MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
+       }
+}
+
+static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
+                          u8 ev, size_t len)
+{
+       void *data;
+
+       data = skb_pull_data(skb, len);
+       if (!data)
+               bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
+
+       return data;
+}
+
+/* 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;
+       struct msft_monitor_advertisement_handle_data *handle_data;
+       u8 addr_type;
+
+       ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
+       if (!ev)
+               return;
+
+       bt_dev_dbg(hdev,
+                  "MSFT vendor event 0x%02x: 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 0x%02x: unknown addr type 0x%02x",
+                          MSFT_EV_LE_MONITOR_DEVICE, 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, void *data, struct sk_buff *skb)
 {
        struct msft_data *msft = hdev->msft_data;
-       u8 event;
+       u8 *evt_prefix;
+       u8 *evt;
 
        if (!msft)
                return;
@@ -602,13 +732,12 @@ void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
         * matches, and otherwise just return.
         */
        if (msft->evt_prefix_len > 0) {
-               if (skb->len < msft->evt_prefix_len)
+               evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
+               if (!evt_prefix)
                        return;
 
-               if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
+               if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
                        return;
-
-               skb_pull(skb, msft->evt_prefix_len);
        }
 
        /* Every event starts at least with an event code and the rest of
@@ -617,10 +746,23 @@ void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
        if (skb->len < 1)
                return;
 
-       event = *skb->data;
-       skb_pull(skb, 1);
+       evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
+       if (!evt)
+               return;
+
+       hci_dev_lock(hdev);
+
+       switch (*evt) {
+       case MSFT_EV_LE_MONITOR_DEVICE:
+               msft_monitor_device_evt(hdev, skb);
+               break;
 
-       bt_dev_dbg(hdev, "MSFT vendor event %u", event);
+       default:
+               bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
+               break;
+       }
+
+       hci_dev_unlock(hdev);
 }
 
 __u64 msft_get_features(struct hci_dev *hdev)