Bluetooth: hci_sync: Refactor add Adv Monitor

+1 -4

include/net/bluetooth/hci_core.h

··· 1420 1420 1421 1421 void hci_adv_monitors_clear(struct hci_dev *hdev); 1422 1422 void hci_free_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor); 1423 - int hci_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status); 1424 1423 int hci_remove_adv_monitor_complete(struct hci_dev *hdev, u8 status); 1425 - bool hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, 1426 - int *err); 1424 + int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor); 1427 1425 bool hci_remove_single_adv_monitor(struct hci_dev *hdev, u16 handle, int *err); 1428 1426 bool hci_remove_all_adv_monitor(struct hci_dev *hdev, int *err); 1429 1427 bool hci_is_adv_monitoring(struct hci_dev *hdev); ··· 1883 1885 u8 instance); 1884 1886 void mgmt_adv_monitor_removed(struct hci_dev *hdev, u16 handle); 1885 1887 int mgmt_phy_configuration_changed(struct hci_dev *hdev, struct sock *skip); 1886 - int mgmt_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status); 1887 1888 int mgmt_remove_adv_monitor_complete(struct hci_dev *hdev, u8 status); 1888 1889 void mgmt_adv_monitor_device_lost(struct hci_dev *hdev, u16 handle, 1889 1890 bdaddr_t *bdaddr, u8 addr_type);

+20 -26

net/bluetooth/hci_core.c

··· 1880 1880 kfree(monitor); 1881 1881 } 1882 1882 1883 - int hci_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status) 1884 - { 1885 - return mgmt_add_adv_patterns_monitor_complete(hdev, status); 1886 - } 1887 - 1888 1883 int hci_remove_adv_monitor_complete(struct hci_dev *hdev, u8 status) 1889 1884 { 1890 1885 return mgmt_remove_adv_monitor_complete(hdev, status); ··· 1887 1892 1888 1893 /* Assigns handle to a monitor, and if offloading is supported and power is on, 1889 1894 * also attempts to forward the request to the controller. 1890 - * Returns true if request is forwarded (result is pending), false otherwise. 1891 - * This function requires the caller holds hdev->lock. 1895 + * This function requires the caller holds hci_req_sync_lock. 1892 1896 */ 1893 - bool hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, 1894 - int *err) 1897 + int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor) 1895 1898 { 1896 1899 int min, max, handle; 1900 + int status = 0; 1897 1901 1898 - *err = 0; 1902 + if (!monitor) 1903 + return -EINVAL; 1899 1904 1900 - if (!monitor) { 1901 - *err = -EINVAL; 1902 - return false; 1903 - } 1905 + hci_dev_lock(hdev); 1904 1906 1905 1907 min = HCI_MIN_ADV_MONITOR_HANDLE; 1906 1908 max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES; 1907 1909 handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max, 1908 1910 GFP_KERNEL); 1909 - if (handle < 0) { 1910 - *err = handle; 1911 - return false; 1912 - } 1911 + 1912 + hci_dev_unlock(hdev); 1913 + 1914 + if (handle < 0) 1915 + return handle; 1913 1916 1914 1917 monitor->handle = handle; 1915 1918 1916 1919 if (!hdev_is_powered(hdev)) 1917 - return false; 1920 + return status; 1918 1921 1919 1922 switch (hci_get_adv_monitor_offload_ext(hdev)) { 1920 1923 case HCI_ADV_MONITOR_EXT_NONE: 1921 - hci_update_passive_scan(hdev); 1922 - bt_dev_dbg(hdev, "%s add monitor status %d", hdev->name, *err); 1924 + bt_dev_dbg(hdev, "%s add monitor %d status %d", hdev->name, 1925 + monitor->handle, status); 1923 1926 /* Message was not forwarded to controller - not an error */ 1924 - return false; 1927 + break; 1928 + 1925 1929 case HCI_ADV_MONITOR_EXT_MSFT: 1926 - *err = msft_add_monitor_pattern(hdev, monitor); 1927 - bt_dev_dbg(hdev, "%s add monitor msft status %d", hdev->name, 1928 - *err); 1930 + status = msft_add_monitor_pattern(hdev, monitor); 1931 + bt_dev_dbg(hdev, "%s add monitor %d msft status %d", hdev->name, 1932 + monitor->handle, status); 1929 1933 break; 1930 1934 } 1931 1935 1932 - return (*err == 0); 1936 + return status; 1933 1937 } 1934 1938 1935 1939 /* Attempts to tell the controller and free the monitor. If somehow the

+17 -35

net/bluetooth/mgmt.c

··· 4646 4646 return err; 4647 4647 } 4648 4648 4649 - int mgmt_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status) 4649 + static void mgmt_add_adv_patterns_monitor_complete(struct hci_dev *hdev, 4650 + void *data, int status) 4650 4651 { 4651 4652 struct mgmt_rp_add_adv_patterns_monitor rp; 4652 - struct mgmt_pending_cmd *cmd; 4653 - struct adv_monitor *monitor; 4654 - int err = 0; 4653 + struct mgmt_pending_cmd *cmd = data; 4654 + struct adv_monitor *monitor = cmd->user_data; 4655 4655 4656 4656 hci_dev_lock(hdev); 4657 4657 4658 - cmd = pending_find(MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI, hdev); 4659 - if (!cmd) { 4660 - cmd = pending_find(MGMT_OP_ADD_ADV_PATTERNS_MONITOR, hdev); 4661 - if (!cmd) 4662 - goto done; 4663 - } 4664 - 4665 - monitor = cmd->user_data; 4666 4658 rp.monitor_handle = cpu_to_le16(monitor->handle); 4667 4659 4668 4660 if (!status) { ··· 4665 4673 hci_update_passive_scan(hdev); 4666 4674 } 4667 4675 4668 - err = mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, 4669 - mgmt_status(status), &rp, sizeof(rp)); 4676 + mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, 4677 + mgmt_status(status), &rp, sizeof(rp)); 4670 4678 mgmt_pending_remove(cmd); 4671 4679 4672 - done: 4673 4680 hci_dev_unlock(hdev); 4674 - bt_dev_dbg(hdev, "add monitor %d complete, status %u", 4681 + bt_dev_dbg(hdev, "add monitor %d complete, status %d", 4675 4682 rp.monitor_handle, status); 4683 + } 4676 4684 4677 - return err; 4685 + static int mgmt_add_adv_patterns_monitor_sync(struct hci_dev *hdev, void *data) 4686 + { 4687 + struct mgmt_pending_cmd *cmd = data; 4688 + struct adv_monitor *monitor = cmd->user_data; 4689 + 4690 + return hci_add_adv_monitor(hdev, monitor); 4678 4691 } 4679 4692 4680 4693 static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev, 4681 4694 struct adv_monitor *m, u8 status, 4682 4695 void *data, u16 len, u16 op) 4683 4696 { 4684 - struct mgmt_rp_add_adv_patterns_monitor rp; 4685 4697 struct mgmt_pending_cmd *cmd; 4686 4698 int err; 4687 - bool pending; 4688 4699 4689 4700 hci_dev_lock(hdev); 4690 4701 ··· 4709 4714 } 4710 4715 4711 4716 cmd->user_data = m; 4712 - pending = hci_add_adv_monitor(hdev, m, &err); 4717 + err = hci_cmd_sync_queue(hdev, mgmt_add_adv_patterns_monitor_sync, cmd, 4718 + mgmt_add_adv_patterns_monitor_complete); 4713 4719 if (err) { 4714 - if (err == -ENOSPC || err == -ENOMEM) 4720 + if (err == -ENOMEM) 4715 4721 status = MGMT_STATUS_NO_RESOURCES; 4716 - else if (err == -EINVAL) 4717 - status = MGMT_STATUS_INVALID_PARAMS; 4718 4722 else 4719 4723 status = MGMT_STATUS_FAILED; 4720 4724 4721 4725 mgmt_pending_remove(cmd); 4722 4726 goto unlock; 4723 - } 4724 - 4725 - if (!pending) { 4726 - mgmt_pending_remove(cmd); 4727 - rp.monitor_handle = cpu_to_le16(m->handle); 4728 - mgmt_adv_monitor_added(sk, hdev, m->handle); 4729 - m->state = ADV_MONITOR_STATE_REGISTERED; 4730 - hdev->adv_monitors_cnt++; 4731 - 4732 - hci_dev_unlock(hdev); 4733 - return mgmt_cmd_complete(sk, hdev->id, op, MGMT_STATUS_SUCCESS, 4734 - &rp, sizeof(rp)); 4735 4727 } 4736 4728 4737 4729 hci_dev_unlock(hdev);

+41 -132

net/bluetooth/msft.c

··· 99 99 __u8 evt_prefix_len; 100 100 __u8 *evt_prefix; 101 101 struct list_head handle_map; 102 - __u16 pending_add_handle; 103 102 __u16 pending_remove_handle; 104 103 __u8 resuming; 105 104 __u8 suspending; 106 105 __u8 filter_enabled; 107 106 }; 108 107 109 - static int __msft_add_monitor_pattern(struct hci_dev *hdev, 110 - struct adv_monitor *monitor); 111 108 static int __msft_remove_monitor(struct hci_dev *hdev, 112 109 struct adv_monitor *monitor, u16 handle); 113 110 ··· 161 164 return false; 162 165 } 163 166 164 - static void reregister_monitor(struct hci_dev *hdev, int handle) 165 - { 166 - struct adv_monitor *monitor; 167 - struct msft_data *msft = hdev->msft_data; 168 - int err; 169 - 170 - while (1) { 171 - monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 172 - if (!monitor) { 173 - /* All monitors have been resumed */ 174 - msft->resuming = false; 175 - hci_update_passive_scan(hdev); 176 - return; 177 - } 178 - 179 - msft->pending_add_handle = (u16)handle; 180 - err = __msft_add_monitor_pattern(hdev, monitor); 181 - 182 - /* If success, we return and wait for monitor added callback */ 183 - if (!err) 184 - return; 185 - 186 - /* Otherwise remove the monitor and keep registering */ 187 - hci_free_adv_monitor(hdev, monitor); 188 - handle++; 189 - } 190 - } 191 - 192 167 /* is_mgmt = true matches the handle exposed to userspace via mgmt. 193 168 * is_mgmt = false matches the handle used by the msft controller. 194 169 * This function requires the caller holds hdev->lock ··· 212 243 return count; 213 244 } 214 245 215 - static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev, 216 - u8 status, u16 opcode, 217 - struct sk_buff *skb) 246 + static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode, 247 + struct adv_monitor *monitor, 248 + struct sk_buff *skb) 218 249 { 219 250 struct msft_rp_le_monitor_advertisement *rp; 220 - struct adv_monitor *monitor; 221 251 struct msft_monitor_advertisement_handle_data *handle_data; 222 252 struct msft_data *msft = hdev->msft_data; 253 + int status = 0; 223 254 224 255 hci_dev_lock(hdev); 225 - 226 - monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle); 227 - if (!monitor) { 228 - bt_dev_err(hdev, "msft add advmon: monitor %u is not found!", 229 - msft->pending_add_handle); 230 - status = HCI_ERROR_UNSPECIFIED; 231 - goto unlock; 232 - } 233 - 234 - if (status) 235 - goto unlock; 236 256 237 257 rp = (struct msft_rp_le_monitor_advertisement *)skb->data; 238 258 if (skb->len < sizeof(*rp)) { 239 259 status = HCI_ERROR_UNSPECIFIED; 240 260 goto unlock; 241 261 } 262 + 263 + status = rp->status; 264 + if (status) 265 + goto unlock; 242 266 243 267 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL); 244 268 if (!handle_data) { ··· 247 285 monitor->state = ADV_MONITOR_STATE_OFFLOADED; 248 286 249 287 unlock: 250 - if (status && monitor) 288 + if (status) 251 289 hci_free_adv_monitor(hdev, monitor); 252 290 253 291 hci_dev_unlock(hdev); 254 292 255 - if (!msft->resuming) 256 - hci_add_adv_patterns_monitor_complete(hdev, status); 293 + return status; 257 294 } 258 295 259 296 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, ··· 424 463 ptrdiff_t offset = 0; 425 464 u8 pattern_count = 0; 426 465 struct sk_buff *skb; 427 - u8 status; 428 466 429 467 if (!msft_monitor_pattern_valid(monitor)) 430 468 return -EINVAL; ··· 465 505 if (IS_ERR(skb)) 466 506 return PTR_ERR(skb); 467 507 468 - status = skb->data[0]; 469 - skb_pull(skb, 1); 508 + return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode, 509 + monitor, skb); 510 + } 470 511 471 - msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb); 512 + /* This function requires the caller holds hci_req_sync_lock */ 513 + static void reregister_monitor(struct hci_dev *hdev) 514 + { 515 + struct adv_monitor *monitor; 516 + struct msft_data *msft = hdev->msft_data; 517 + int handle = 0; 472 518 473 - return status; 519 + if (!msft) 520 + return; 521 + 522 + msft->resuming = true; 523 + 524 + while (1) { 525 + monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 526 + if (!monitor) 527 + break; 528 + 529 + msft_add_monitor_sync(hdev, monitor); 530 + 531 + handle++; 532 + } 533 + 534 + /* All monitors have been reregistered */ 535 + msft->resuming = false; 474 536 } 475 537 476 538 /* This function requires the caller holds hci_req_sync_lock */ 477 539 int msft_resume_sync(struct hci_dev *hdev) 478 540 { 479 541 struct msft_data *msft = hdev->msft_data; 480 - struct adv_monitor *monitor; 481 - int handle = 0; 482 542 483 543 if (!msft || !msft_monitor_supported(hdev)) 484 544 return 0; ··· 513 533 514 534 hci_dev_unlock(hdev); 515 535 516 - msft->resuming = true; 517 - 518 - while (1) { 519 - monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 520 - if (!monitor) 521 - break; 522 - 523 - msft_add_monitor_sync(hdev, monitor); 524 - 525 - handle++; 526 - } 527 - 528 - /* All monitors have been resumed */ 529 - msft->resuming = false; 536 + reregister_monitor(hdev); 530 537 531 538 return 0; 532 539 } 533 540 541 + /* This function requires the caller holds hci_req_sync_lock */ 534 542 void msft_do_open(struct hci_dev *hdev) 535 543 { 536 544 struct msft_data *msft = hdev->msft_data; ··· 551 583 /* Monitors get removed on power off, so we need to explicitly 552 584 * tell the controller to re-monitor. 553 585 */ 554 - reregister_monitor(hdev, 0); 586 + reregister_monitor(hdev); 555 587 } 556 588 } 557 589 ··· 797 829 hci_dev_unlock(hdev); 798 830 } 799 831 800 - /* This function requires the caller holds hdev->lock */ 801 - static int __msft_add_monitor_pattern(struct hci_dev *hdev, 802 - struct adv_monitor *monitor) 803 - { 804 - struct msft_cp_le_monitor_advertisement *cp; 805 - struct msft_le_monitor_advertisement_pattern_data *pattern_data; 806 - struct msft_le_monitor_advertisement_pattern *pattern; 807 - struct adv_pattern *entry; 808 - struct hci_request req; 809 - struct msft_data *msft = hdev->msft_data; 810 - size_t total_size = sizeof(*cp) + sizeof(*pattern_data); 811 - ptrdiff_t offset = 0; 812 - u8 pattern_count = 0; 813 - int err = 0; 814 - 815 - if (!msft_monitor_pattern_valid(monitor)) 816 - return -EINVAL; 817 - 818 - list_for_each_entry(entry, &monitor->patterns, list) { 819 - pattern_count++; 820 - total_size += sizeof(*pattern) + entry->length; 821 - } 822 - 823 - cp = kmalloc(total_size, GFP_KERNEL); 824 - if (!cp) 825 - return -ENOMEM; 826 - 827 - cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 828 - cp->rssi_high = monitor->rssi.high_threshold; 829 - cp->rssi_low = monitor->rssi.low_threshold; 830 - cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; 831 - cp->rssi_sampling_period = monitor->rssi.sampling_period; 832 - 833 - cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 834 - 835 - pattern_data = (void *)cp->data; 836 - pattern_data->count = pattern_count; 837 - 838 - list_for_each_entry(entry, &monitor->patterns, list) { 839 - pattern = (void *)(pattern_data->data + offset); 840 - /* the length also includes data_type and offset */ 841 - pattern->length = entry->length + 2; 842 - pattern->data_type = entry->ad_type; 843 - pattern->start_byte = entry->offset; 844 - memcpy(pattern->pattern, entry->value, entry->length); 845 - offset += sizeof(*pattern) + entry->length; 846 - } 847 - 848 - hci_req_init(&req, hdev); 849 - hci_req_add(&req, hdev->msft_opcode, total_size, cp); 850 - err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb); 851 - kfree(cp); 852 - 853 - if (!err) 854 - msft->pending_add_handle = monitor->handle; 855 - 856 - return err; 857 - } 858 - 859 - /* This function requires the caller holds hdev->lock */ 832 + /* This function requires the caller holds hci_req_sync_lock */ 860 833 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) 861 834 { 862 835 struct msft_data *msft = hdev->msft_data; ··· 808 899 if (msft->resuming || msft->suspending) 809 900 return -EBUSY; 810 901 811 - return __msft_add_monitor_pattern(hdev, monitor); 902 + return msft_add_monitor_sync(hdev, monitor); 812 903 } 813 904 814 905 /* This function requires the caller holds hdev->lock */

Configure Feed

Configure Feed