Bluetooth: advmon offload MSFT handle controller reset

When the controller is powered off, the registered advertising monitor
is removed from the controller. This patch handles the re-registration
of those monitors when the power is on.

Signed-off-by: Archie Pusaka <apusaka@chromium.org>
Reviewed-by: Miao-chen Chou <mcchou@chromium.org>
Reviewed-by: Yun-Hao Chung <howardchung@google.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c
index f5aa0e3..d25c693 100644 (file)
--- a/net/bluetooth/msft.c
+++ b/net/bluetooth/msft.c
@@ -82,8 +82,12 @@ struct msft_data {
        struct list_head handle_map;
        __u16 pending_add_handle;
        __u16 pending_remove_handle;
+       __u8 reregistering;
 };
 
+static int __msft_add_monitor_pattern(struct hci_dev *hdev,
+                                     struct adv_monitor *monitor);
+
 bool msft_monitor_supported(struct hci_dev *hdev)
 {
        return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
@@ -134,6 +138,35 @@ failed:
        return false;
 }
 
+/* This function requires the caller holds hdev->lock */
+static void reregister_monitor_on_restart(struct hci_dev *hdev, int handle)
+{
+       struct adv_monitor *monitor;
+       struct msft_data *msft = hdev->msft_data;
+       int err;
+
+       while (1) {
+               monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
+               if (!monitor) {
+                       /* All monitors have been reregistered */
+                       msft->reregistering = false;
+                       hci_update_background_scan(hdev);
+                       return;
+               }
+
+               msft->pending_add_handle = (u16)handle;
+               err = __msft_add_monitor_pattern(hdev, monitor);
+
+               /* If success, we return and wait for monitor added callback */
+               if (!err)
+                       return;
+
+               /* Otherwise remove the monitor and keep registering */
+               hci_free_adv_monitor(hdev, monitor);
+               handle++;
+       }
+}
+
 void msft_do_open(struct hci_dev *hdev)
 {
        struct msft_data *msft;
@@ -154,12 +187,18 @@ void msft_do_open(struct hci_dev *hdev)
 
        INIT_LIST_HEAD(&msft->handle_map);
        hdev->msft_data = msft;
+
+       if (msft_monitor_supported(hdev)) {
+               msft->reregistering = true;
+               reregister_monitor_on_restart(hdev, 0);
+       }
 }
 
 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;
 
        if (!msft)
                return;
@@ -169,6 +208,12 @@ void msft_do_close(struct hci_dev *hdev)
        hdev->msft_data = NULL;
 
        list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
+               monitor = idr_find(&hdev->adv_monitors_idr,
+                                  handle_data->mgmt_handle);
+
+               if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
+                       monitor->state = ADV_MONITOR_STATE_REGISTERED;
+
                list_del(&handle_data->list);
                kfree(handle_data);
        }
@@ -282,9 +327,15 @@ unlock:
        if (status && monitor)
                hci_free_adv_monitor(hdev, monitor);
 
+       /* If in restart/reregister sequence, keep registering. */
+       if (msft->reregistering)
+               reregister_monitor_on_restart(hdev,
+                                             msft->pending_add_handle + 1);
+
        hci_dev_unlock(hdev);
 
-       hci_add_adv_patterns_monitor_complete(hdev, status);
+       if (!msft->reregistering)
+               hci_add_adv_patterns_monitor_complete(hdev, status);
 }
 
 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
@@ -374,7 +425,8 @@ static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
 }
 
 /* This function requires the caller holds hdev->lock */
-int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
+static int __msft_add_monitor_pattern(struct hci_dev *hdev,
+                                     struct adv_monitor *monitor)
 {
        struct msft_cp_le_monitor_advertisement *cp;
        struct msft_le_monitor_advertisement_pattern_data *pattern_data;
@@ -387,9 +439,6 @@ int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
        u8 pattern_count = 0;
        int err = 0;
 
-       if (!msft)
-               return -EOPNOTSUPP;
-
        if (!msft_monitor_pattern_valid(monitor))
                return -EINVAL;
 
@@ -435,6 +484,20 @@ int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
 }
 
 /* This function requires the caller holds hdev->lock */
+int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
+{
+       struct msft_data *msft = hdev->msft_data;
+
+       if (!msft)
+               return -EOPNOTSUPP;
+
+       if (msft->reregistering)
+               return -EBUSY;
+
+       return __msft_add_monitor_pattern(hdev, monitor);
+}
+
+/* This function requires the caller holds hdev->lock */
 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
                        u16 handle)
 {
@@ -447,6 +510,9 @@ int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
        if (!msft)
                return -EOPNOTSUPP;
 
+       if (msft->reregistering)
+               return -EBUSY;
+
        handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 
        /* If no matched handle, just remove without telling controller */