net/bluetooth/msft.c

137 bool msft_monitor_supported(struct hci_dev *hdev)
139 	return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
142 static bool read_supported_features(struct hci_dev *hdev,
151 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
154 		bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
160 		bt_dev_err(hdev, "MSFT supported features length mismatch");
180 		hdev->msft_curve_validity = true;
192  * This function requires the caller holds hdev->lock
195 				(struct hci_dev *hdev, u16 handle, bool is_mgmt)
198 	struct msft_data *msft = hdev->msft_data;
212 			(struct hci_dev *hdev, u8 addr_type, bdaddr_t *addr,
216 	struct msft_data *msft = hdev->msft_data;
228 /* This function requires the caller holds hdev->lock */
229 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
236 	list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
245 				mgmt_adv_monitor_device_lost(hdev, dev->handle,
259 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
265 	struct msft_data *msft = hdev->msft_data;
268 	hci_dev_lock(hdev);
296 		hci_free_adv_monitor(hdev, monitor);
298 	hci_dev_unlock(hdev);
304 static void msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle)
308 	struct msft_data *msft = hdev->msft_data;
351 		skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
360 		bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",
367 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
374 	struct msft_data *msft = hdev->msft_data;
388 	hci_dev_lock(hdev);
390 	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
400 			hci_free_adv_monitor(hdev, monitor);
403 			msft_monitor_device_del(hdev, handle_data->mgmt_handle,
412 		hci_dev_unlock(hdev);
414 		msft_remove_addr_filters_sync(hdev, msft_handle);
416 		hci_dev_unlock(hdev);
424 static int msft_remove_monitor_sync(struct hci_dev *hdev,
431 	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
440 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
445 	return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
450 int msft_suspend_sync(struct hci_dev *hdev)
452 	struct msft_data *msft = hdev->msft_data;
456 	if (!msft || !msft_monitor_supported(hdev))
462 		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
466 		msft_remove_monitor_sync(hdev, monitor);
506 static int msft_add_monitor_sync(struct hci_dev *hdev,
553 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
561 	err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
566 	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
583 static void reregister_monitor(struct hci_dev *hdev)
586 	struct msft_data *msft = hdev->msft_data;
595 		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
599 		msft_add_monitor_sync(hdev, monitor);
609 int msft_resume_sync(struct hci_dev *hdev)
611 	struct msft_data *msft = hdev->msft_data;
613 	if (!msft || !msft_monitor_supported(hdev))
616 	hci_dev_lock(hdev);
621 	hdev->advmon_pend_notify = false;
622 	msft_monitor_device_del(hdev, 0, NULL, 0, true);
624 	hci_dev_unlock(hdev);
626 	reregister_monitor(hdev);
632 void msft_do_open(struct hci_dev *hdev)
634 	struct msft_data *msft = hdev->msft_data;
636 	if (hdev->msft_opcode == HCI_OP_NOP)
640 		bt_dev_err(hdev, "MSFT extension not registered");
644 	bt_dev_dbg(hdev, "Initialize MSFT extension");
652 	if (!read_supported_features(hdev, msft)) {
653 		hdev->msft_data = NULL;
658 	if (msft_monitor_supported(hdev)) {
660 		msft_set_filter_enable(hdev, true);
664 		reregister_monitor(hdev);
668 void msft_do_close(struct hci_dev *hdev)
670 	struct msft_data *msft = hdev->msft_data;
678 	bt_dev_dbg(hdev, "Cleanup of MSFT extension");
684 		monitor = idr_find(&hdev->adv_monitors_idr,
702 	hci_dev_lock(hdev);
705 	hdev->advmon_pend_notify = false;
706 	msft_monitor_device_del(hdev, 0, NULL, 0, true);
708 	hci_dev_unlock(hdev);
711 static int msft_cancel_address_filter_sync(struct hci_dev *hdev, void *data)
715 	struct msft_data *msft = hdev->msft_data;
720 		bt_dev_err(hdev, "MSFT: msft data is freed");
725 	if (!test_bit(HCI_UP, &hdev->flags))
735 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
738 		bt_dev_err(hdev, "MSFT: Failed to cancel address (%pMR) filter",
745 	bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",
754 void msft_register(struct hci_dev *hdev)
758 	bt_dev_dbg(hdev, "Register MSFT extension");
762 		bt_dev_err(hdev, "Failed to register MSFT extension");
768 	hdev->msft_data = msft;
772 void msft_release(struct hci_dev *hdev)
774 	struct msft_data *msft = hdev->msft_data;
779 	bt_dev_dbg(hdev, "Unregister MSFT extension");
781 	hdev->msft_data = NULL;
788 /* This function requires the caller holds hdev->lock */
789 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
796 		bt_dev_err(hdev, "MSFT vendor event %u: no memory",
807 	list_add(&dev->list, &hdev->monitored_devices);
808 	hdev->advmon_pend_notify = true;
811 /* This function requires the caller holds hdev->lock */
812 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
815 	if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
817 		bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
822 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
829 		bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
834 static int msft_add_address_filter_sync(struct hci_dev *hdev, void *data)
839 	struct msft_data *msft = hdev->msft_data;
845 		bt_dev_err(hdev, "MSFT: msft data is freed");
850 	if (!test_bit(HCI_UP, &hdev->flags))
874 		bt_dev_err(hdev, "MSFT: Alloc cmd param err");
889 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, size, cp,
894 		bt_dev_err(hdev, "Failed to enable address %pMR filter",
910 		bt_dev_warn(hdev, "MSFT: Remove address (%pMR) filter",
917 		bt_dev_dbg(hdev, "MSFT: Address %pMR filter enabled",
929 		(struct hci_dev *hdev, u8 addr_type, bdaddr_t *bdaddr,
933 	struct msft_data *msft = hdev->msft_data;
957 	err = hci_cmd_sync_queue(hdev, msft_add_address_filter_sync,
960 		bt_dev_err(hdev, "MSFT: Add address %pMR filter err", bdaddr);
966 	bt_dev_dbg(hdev, "MSFT: Add device %pMR address filter",
972 /* This function requires the caller holds hdev->lock */
973 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
978 	struct msft_data *msft = hdev->msft_data;
982 	ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
986 	bt_dev_dbg(hdev,
991 	handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
993 	if (!test_bit(HCI_QUIRK_USE_MSFT_EXT_ADDRESS_FILTER, &hdev->quirks)) {
1006 		address_filter = msft_find_address_data(hdev, ev->addr_type,
1014 			msft_add_address_filter(hdev, ev->addr_type,
1033 		bt_dev_warn(hdev, "MSFT: Unexpected device event %pMR, %u, %u",
1049 		bt_dev_err(hdev,
1056 		msft_device_found(hdev, &ev->bdaddr, addr_type, mgmt_handle);
1060 			hci_cmd_sync_queue(hdev,
1065 		msft_device_lost(hdev, &ev->bdaddr, addr_type, mgmt_handle);
1069 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
1071 	struct msft_data *msft = hdev->msft_data;
1082 		evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
1096 	evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
1100 	hci_dev_lock(hdev);
1105 		msft_monitor_device_evt(hdev, skb);
1110 		bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
1114 	hci_dev_unlock(hdev);
1117 __u64 msft_get_features(struct hci_dev *hdev)
1119 	struct msft_data *msft = hdev->msft_data;
1124 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
1129 	struct msft_data *msft = hdev->msft_data;
1141 	hci_dev_lock(hdev);
1146 		bt_dev_warn(hdev, "MSFT filter_enable is already %s",
1149 	hci_dev_unlock(hdev);
1153 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
1155 	struct msft_data *msft = hdev->msft_data;
1163 	return msft_add_monitor_sync(hdev, monitor);
1167 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
1169 	struct msft_data *msft = hdev->msft_data;
1177 	return msft_remove_monitor_sync(hdev, monitor);
1180 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
1183 	struct msft_data *msft = hdev->msft_data;
1191 	err = __hci_cmd_sync_status(hdev, hdev->msft_opcode, sizeof(cp), &cp,
1194 	msft_le_set_advertisement_filter_enable_cb(hdev, &cp, err);
1199 bool msft_curve_validity(struct hci_dev *hdev)
1201 	return hdev->msft_curve_validity;