From b747a83690c8f53bc7a3f75899415c699b2c51aa Mon Sep 17 00:00:00 2001 From: Manish Mandlik Date: Wed, 20 Jul 2022 16:21:13 -0700 Subject: Bluetooth: hci_sync: Refactor add Adv Monitor Make use of hci_cmd_sync_queue for adding an advertisement monitor. Signed-off-by: Manish Mandlik Reviewed-by: Miao-chen Chou Signed-off-by: Luiz Augusto von Dentz --- net/bluetooth/msft.c | 171 ++++++++++++--------------------------------------- 1 file changed, 40 insertions(+), 131 deletions(-) (limited to 'net/bluetooth/msft.c') diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c index f43994523b1f..54fbc718e893 100644 --- a/net/bluetooth/msft.c +++ b/net/bluetooth/msft.c @@ -99,15 +99,12 @@ struct msft_data { __u8 evt_prefix_len; __u8 *evt_prefix; struct list_head handle_map; - __u16 pending_add_handle; __u16 pending_remove_handle; __u8 resuming; __u8 suspending; __u8 filter_enabled; }; -static int __msft_add_monitor_pattern(struct hci_dev *hdev, - struct adv_monitor *monitor); static int __msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, u16 handle); @@ -164,34 +161,6 @@ failed: return false; } -static void reregister_monitor(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 resumed */ - msft->resuming = false; - hci_update_passive_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++; - } -} - /* is_mgmt = true matches the handle exposed to userspace via mgmt. * is_mgmt = false matches the handle used by the msft controller. * This function requires the caller holds hdev->lock @@ -243,34 +212,27 @@ static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle, return count; } -static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev, - u8 status, u16 opcode, - struct sk_buff *skb) +static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode, + struct adv_monitor *monitor, + struct sk_buff *skb) { struct msft_rp_le_monitor_advertisement *rp; - struct adv_monitor *monitor; struct msft_monitor_advertisement_handle_data *handle_data; struct msft_data *msft = hdev->msft_data; + int status = 0; hci_dev_lock(hdev); - monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle); - if (!monitor) { - bt_dev_err(hdev, "msft add advmon: monitor %u is not found!", - msft->pending_add_handle); + rp = (struct msft_rp_le_monitor_advertisement *)skb->data; + if (skb->len < sizeof(*rp)) { status = HCI_ERROR_UNSPECIFIED; goto unlock; } + status = rp->status; if (status) goto unlock; - rp = (struct msft_rp_le_monitor_advertisement *)skb->data; - if (skb->len < sizeof(*rp)) { - status = HCI_ERROR_UNSPECIFIED; - goto unlock; - } - handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL); if (!handle_data) { status = HCI_ERROR_UNSPECIFIED; @@ -285,13 +247,12 @@ static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev, monitor->state = ADV_MONITOR_STATE_OFFLOADED; unlock: - if (status && monitor) + if (status) hci_free_adv_monitor(hdev, monitor); hci_dev_unlock(hdev); - if (!msft->resuming) - hci_add_adv_patterns_monitor_complete(hdev, status); + return status; } static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, @@ -463,7 +424,6 @@ static int msft_add_monitor_sync(struct hci_dev *hdev, ptrdiff_t offset = 0; u8 pattern_count = 0; struct sk_buff *skb; - u8 status; if (!msft_monitor_pattern_valid(monitor)) return -EINVAL; @@ -505,20 +465,40 @@ static int msft_add_monitor_sync(struct hci_dev *hdev, if (IS_ERR(skb)) return PTR_ERR(skb); - status = skb->data[0]; - skb_pull(skb, 1); + return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode, + monitor, skb); +} - msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb); +/* This function requires the caller holds hci_req_sync_lock */ +static void reregister_monitor(struct hci_dev *hdev) +{ + struct adv_monitor *monitor; + struct msft_data *msft = hdev->msft_data; + int handle = 0; - return status; + if (!msft) + return; + + msft->resuming = true; + + while (1) { + monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); + if (!monitor) + break; + + msft_add_monitor_sync(hdev, monitor); + + handle++; + } + + /* All monitors have been reregistered */ + msft->resuming = false; } /* This function requires the caller holds hci_req_sync_lock */ int msft_resume_sync(struct hci_dev *hdev) { struct msft_data *msft = hdev->msft_data; - struct adv_monitor *monitor; - int handle = 0; if (!msft || !msft_monitor_supported(hdev)) return 0; @@ -533,24 +513,12 @@ int msft_resume_sync(struct hci_dev *hdev) hci_dev_unlock(hdev); - msft->resuming = true; - - while (1) { - monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); - if (!monitor) - break; - - msft_add_monitor_sync(hdev, monitor); - - handle++; - } - - /* All monitors have been resumed */ - msft->resuming = false; + reregister_monitor(hdev); return 0; } +/* This function requires the caller holds hci_req_sync_lock */ void msft_do_open(struct hci_dev *hdev) { struct msft_data *msft = hdev->msft_data; @@ -583,7 +551,7 @@ void msft_do_open(struct hci_dev *hdev) /* Monitors get removed on power off, so we need to explicitly * tell the controller to re-monitor. */ - reregister_monitor(hdev, 0); + reregister_monitor(hdev); } } @@ -829,66 +797,7 @@ static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, hci_dev_unlock(hdev); } -/* This function requires the caller holds hdev->lock */ -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; - struct msft_le_monitor_advertisement_pattern *pattern; - struct adv_pattern *entry; - struct hci_request req; - struct msft_data *msft = hdev->msft_data; - size_t total_size = sizeof(*cp) + sizeof(*pattern_data); - ptrdiff_t offset = 0; - u8 pattern_count = 0; - int err = 0; - - if (!msft_monitor_pattern_valid(monitor)) - return -EINVAL; - - list_for_each_entry(entry, &monitor->patterns, list) { - pattern_count++; - total_size += sizeof(*pattern) + entry->length; - } - - cp = kmalloc(total_size, GFP_KERNEL); - if (!cp) - return -ENOMEM; - - cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; - cp->rssi_high = monitor->rssi.high_threshold; - cp->rssi_low = monitor->rssi.low_threshold; - cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; - cp->rssi_sampling_period = monitor->rssi.sampling_period; - - cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; - - pattern_data = (void *)cp->data; - pattern_data->count = pattern_count; - - list_for_each_entry(entry, &monitor->patterns, list) { - pattern = (void *)(pattern_data->data + offset); - /* the length also includes data_type and offset */ - pattern->length = entry->length + 2; - pattern->data_type = entry->ad_type; - pattern->start_byte = entry->offset; - memcpy(pattern->pattern, entry->value, entry->length); - offset += sizeof(*pattern) + entry->length; - } - - hci_req_init(&req, hdev); - hci_req_add(&req, hdev->msft_opcode, total_size, cp); - err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb); - kfree(cp); - - if (!err) - msft->pending_add_handle = monitor->handle; - - return err; -} - -/* This function requires the caller holds hdev->lock */ +/* This function requires the caller holds hci_req_sync_lock */ int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) { struct msft_data *msft = hdev->msft_data; @@ -899,7 +808,7 @@ int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) if (msft->resuming || msft->suspending) return -EBUSY; - return __msft_add_monitor_pattern(hdev, monitor); + return msft_add_monitor_sync(hdev, monitor); } /* This function requires the caller holds hdev->lock */ -- cgit v1.2.3