// SPDX-License-Identifier: GPL-2.0-only/* * Copyright (C) 2020 Google Corporation */ #include <net/bluetooth/bluetooth.h>#include <net/bluetooth/hci_core.h>#include <net/bluetooth/mgmt.h> #include "mgmt_util.h"#include "msft.h" #define MSFT_RSSI_THRESHOLD_VALUE_MIN		-127#define MSFT_RSSI_THRESHOLD_VALUE_MAX		20#define MSFT_RSSI_LOW_TIMEOUT_MAX		0x3C #define MSFT_OP_READ_SUPPORTED_FEATURES		0x00struct msft_cp_read_supported_features {	__u8   sub_opcode;} __packed; struct msft_rp_read_supported_features {	__u8   status;	__u8   sub_opcode;	__le64 features;	__u8   evt_prefix_len;	__u8   evt_prefix[];} __packed; #define MSFT_OP_LE_MONITOR_ADVERTISEMENT	0x03#define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN	0x01struct msft_le_monitor_advertisement_pattern {	__u8 length;	__u8 data_type;	__u8 start_byte;	__u8 pattern[];}; struct msft_le_monitor_advertisement_pattern_data {	__u8 count;	__u8 data[];}; struct msft_cp_le_monitor_advertisement {	__u8 sub_opcode;	__s8 rssi_high;	__s8 rssi_low;	__u8 rssi_low_interval;	__u8 rssi_sampling_period;	__u8 cond_type;	__u8 data[];} __packed; struct msft_rp_le_monitor_advertisement {	__u8 status;	__u8 sub_opcode;	__u8 handle;} __packed; #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT	0x04struct msft_cp_le_cancel_monitor_advertisement {	__u8 sub_opcode;	__u8 handle;} __packed; struct msft_rp_le_cancel_monitor_advertisement {	__u8 status;	__u8 sub_opcode;} __packed; #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE	0x05struct msft_cp_le_set_advertisement_filter_enable {	__u8 sub_opcode;	__u8 enable;} __packed; struct msft_rp_le_set_advertisement_filter_enable {	__u8 status;	__u8 sub_opcode;} __packed; #define MSFT_EV_LE_MONITOR_DEVICE	0x02struct msft_ev_le_monitor_device {	__u8     addr_type;	bdaddr_t bdaddr;	__u8     monitor_handle;	__u8     monitor_state;} __packed; struct msft_monitor_advertisement_handle_data {	__u8  msft_handle;	__u16 mgmt_handle;	__s8 rssi_high;	__s8 rssi_low;	__u8 rssi_low_interval;	__u8 rssi_sampling_period;	__u8 cond_type;	struct list_head list;}; enum monitor_addr_filter_state {	AF_STATE_IDLE,	AF_STATE_ADDING,	AF_STATE_ADDED,	AF_STATE_REMOVING,}; #define MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR	0x04struct msft_monitor_addr_filter_data {	__u8     msft_handle;	__u8     pattern_handle; /* address filters pertain to */	__u16    mgmt_handle;	int      state;	__s8     rssi_high;	__s8     rssi_low;	__u8     rssi_low_interval;	__u8     rssi_sampling_period;	__u8     addr_type;	bdaddr_t bdaddr;	struct list_head list;}; struct msft_data {	__u64 features;	__u8  evt_prefix_len;	__u8  *evt_prefix;	struct list_head handle_map;	struct list_head address_filters;	__u8 resuming;	__u8 suspending;	__u8 filter_enabled;	/* To synchronize add/remove address filter and monitor device event.*/	struct mutex filter_lock;}; bool msft_monitor_supported(struct hci_dev *hdev){	return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);} static bool read_supported_features(struct hci_dev *hdev,				    struct msft_data *msft){	struct msft_cp_read_supported_features cp;	struct msft_rp_read_supported_features *rp;	struct sk_buff *skb; 	cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES; 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,			     HCI_CMD_TIMEOUT);	if (IS_ERR(skb)) {		bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",			   PTR_ERR(skb));		return false;	} 	if (skb->len < sizeof(*rp)) {		bt_dev_err(hdev, "MSFT supported features length mismatch");		goto failed;	} 	rp = (struct msft_rp_read_supported_features *)skb->data; 	if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)		goto failed; 	if (skb->len < sizeof(*rp) + rp->evt_prefix_len) {		bt_dev_err(hdev, "MSFT event prefix length mismatch");		goto failed;	} 	if (rp->evt_prefix_len > 0) {		msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,					   GFP_KERNEL);		if (!msft->evt_prefix)			goto failed;	} 	msft->evt_prefix_len = rp->evt_prefix_len;	msft->features = __le64_to_cpu(rp->features); 	if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)		hdev->msft_curve_validity = true; 	kfree_skb(skb);	return true; failed:	kfree_skb(skb);	return false;} /* 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 */static struct msft_monitor_advertisement_handle_data *msft_find_handle_data				(struct hci_dev *hdev, u16 handle, bool is_mgmt){	struct msft_monitor_advertisement_handle_data *entry;	struct msft_data *msft = hdev->msft_data; 	list_for_each_entry(entry, &msft->handle_map, list) {		if (is_mgmt && entry->mgmt_handle == handle)			return entry;		if (!is_mgmt && entry->msft_handle == handle)			return entry;	} 	return NULL;} /* This function requires the caller holds msft->filter_lock */static struct msft_monitor_addr_filter_data *msft_find_address_data			(struct hci_dev *hdev, u8 addr_type, bdaddr_t *addr,			 u8 pattern_handle){	struct msft_monitor_addr_filter_data *entry;	struct msft_data *msft = hdev->msft_data; 	list_for_each_entry(entry, &msft->address_filters, list) {		if (entry->pattern_handle == pattern_handle &&		    addr_type == entry->addr_type &&		    !bacmp(addr, &entry->bdaddr))			return entry;	} 	return NULL;} /* This function requires the caller holds hdev->lock */static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,				   bdaddr_t *bdaddr, __u8 addr_type,				   bool notify){	struct monitored_device *dev, *tmp;	int count = 0; 	list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {		/* mgmt_handle == 0 indicates remove all devices, whereas,		 * bdaddr == NULL indicates remove all devices matching the		 * mgmt_handle.		 */		if ((!mgmt_handle || dev->handle == mgmt_handle) &&		    (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&				 addr_type == dev->addr_type))) {			if (notify && dev->notified) {				mgmt_adv_monitor_device_lost(hdev, dev->handle,							     &dev->bdaddr,							     dev->addr_type);			} 			list_del(&dev->list);			kfree(dev);			count++;		}	} 	return count;} 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 msft_monitor_advertisement_handle_data *handle_data;	struct msft_data *msft = hdev->msft_data;	int status = 0; 	hci_dev_lock(hdev); 	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; 	handle_data = kmalloc_obj(*handle_data);	if (!handle_data) {		status = HCI_ERROR_UNSPECIFIED;		goto unlock;	} 	handle_data->mgmt_handle = monitor->handle;	handle_data->msft_handle = rp->handle;	handle_data->cond_type   = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;	INIT_LIST_HEAD(&handle_data->list);	list_add(&handle_data->list, &msft->handle_map); 	monitor->state = ADV_MONITOR_STATE_OFFLOADED; unlock:	if (status && msft->resuming)		hci_free_adv_monitor(hdev, monitor); 	hci_dev_unlock(hdev); 	return status;} /* This function requires the caller holds hci_req_sync_lock */static void msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle){	struct msft_monitor_addr_filter_data *address_filter, *n;	struct msft_cp_le_cancel_monitor_advertisement cp;	struct msft_data *msft = hdev->msft_data;	struct list_head head;	struct sk_buff *skb; 	INIT_LIST_HEAD(&head); 	/* Cancel all corresponding address monitors */	mutex_lock(&msft->filter_lock); 	list_for_each_entry_safe(address_filter, n, &msft->address_filters,				 list) {		if (address_filter->pattern_handle != handle)			continue; 		list_del(&address_filter->list); 		/* Keep the address filter and let		 * msft_add_address_filter_sync() remove and free the address		 * filter.		 */		if (address_filter->state == AF_STATE_ADDING) {			address_filter->state = AF_STATE_REMOVING;			continue;		} 		/* Keep the address filter and let		 * msft_cancel_address_filter_sync() remove and free the address		 * filter		 */		if (address_filter->state == AF_STATE_REMOVING)			continue; 		list_add_tail(&address_filter->list, &head);	} 	mutex_unlock(&msft->filter_lock); 	list_for_each_entry_safe(address_filter, n, &head, list) {		list_del(&address_filter->list); 		cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;		cp.handle = address_filter->msft_handle; 		skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,				     HCI_CMD_TIMEOUT);		if (IS_ERR(skb)) {			kfree(address_filter);			continue;		} 		kfree_skb(skb); 		bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",			   &address_filter->bdaddr); 		kfree(address_filter);	}} static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,						   u16 opcode,						   struct adv_monitor *monitor,						   struct sk_buff *skb){	struct msft_rp_le_cancel_monitor_advertisement *rp;	struct msft_monitor_advertisement_handle_data *handle_data;	struct msft_data *msft = hdev->msft_data;	int status = 0;	u8 msft_handle; 	rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;	if (skb->len < sizeof(*rp)) {		status = HCI_ERROR_UNSPECIFIED;		goto done;	} 	status = rp->status;	if (status)		goto done; 	hci_dev_lock(hdev); 	handle_data = msft_find_handle_data(hdev, monitor->handle, true); 	if (handle_data) {		if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)			monitor->state = ADV_MONITOR_STATE_REGISTERED; 		/* Do not free the monitor if it is being removed due to		 * suspend. It will be re-monitored on resume.		 */		if (!msft->suspending) {			hci_free_adv_monitor(hdev, monitor); 			/* Clear any monitored devices by this Adv Monitor */			msft_monitor_device_del(hdev, handle_data->mgmt_handle,						NULL, 0, false);		} 		msft_handle = handle_data->msft_handle; 		list_del(&handle_data->list);		kfree(handle_data); 		hci_dev_unlock(hdev); 		msft_remove_addr_filters_sync(hdev, msft_handle);	} else {		hci_dev_unlock(hdev);	} done:	return status;} /* This function requires the caller holds hci_req_sync_lock */static int msft_remove_monitor_sync(struct hci_dev *hdev,				    struct adv_monitor *monitor){	struct msft_cp_le_cancel_monitor_advertisement cp;	struct msft_monitor_advertisement_handle_data *handle_data;	struct sk_buff *skb; 	handle_data = msft_find_handle_data(hdev, monitor->handle, true); 	/* If no matched handle, just remove without telling controller */	if (!handle_data)		return -ENOENT; 	cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;	cp.handle = handle_data->msft_handle; 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,			     HCI_CMD_TIMEOUT);	if (IS_ERR(skb))		return PTR_ERR(skb); 	return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,						       monitor, skb);} /* This function requires the caller holds hci_req_sync_lock */int msft_suspend_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; 	msft->suspending = true; 	while (1) {		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);		if (!monitor)			break; 		msft_remove_monitor_sync(hdev, monitor); 		handle++;	} 	/* All monitors have been removed */	msft->suspending = false; 	return 0;} static bool msft_monitor_rssi_valid(struct adv_monitor *monitor){	struct adv_rssi_thresholds *r = &monitor->rssi; 	if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||	    r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||	    r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||	    r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)		return false; 	/* High_threshold_timeout is not supported,	 * once high_threshold is reached, events are immediately reported.	 */	if (r->high_threshold_timeout != 0)		return false; 	if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)		return false; 	/* Sampling period from 0x00 to 0xFF are all allowed */

Lines 1–500 of 1206. Load more