diff options
-rw-r--r-- | net/bluetooth/msft.c | 76 |
1 files changed, 71 insertions, 5 deletions
diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c index f5aa0e3b1b9b..d25c6936daa4 100644 --- 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 */ |