Commit 3368aa35 authored by Manish Mandlik's avatar Manish Mandlik Committed by Luiz Augusto von Dentz
Browse files

Bluetooth: msft: Handle MSFT Monitor Device Event 



Whenever the controller starts/stops monitoring a bt device, it sends
MSFT Monitor Device event. Add handler to read this vendor event.

Test performed:
- Verified by logs that the MSFT Monitor Device event is received from
  the controller whenever it starts/stops monitoring a device.

Signed-off-by: default avatarManish Mandlik <mmandlik@google.com>
Reviewed-by: default avatarMiao-chen Chou <mcchou@google.com>
Signed-off-by: default avatarLuiz Augusto von Dentz <luiz.von.dentz@intel.com>
parent 3afee211

include/net/bluetooth/hci_core.h

+11 −0
Original line number Diff line number Diff line
@@ -258,6 +258,15 @@ struct adv_info { 


#define HCI_ADV_TX_POWER_NO_PREFERENCE 0x7F



struct monitored_device {

	struct list_head list;



	bdaddr_t bdaddr;

	__u8     addr_type;

	__u16    handle;

	bool     notified;

};



struct adv_pattern {

	struct list_head list;

	__u8 ad_type;
@@ -591,6 +600,8 @@ struct hci_dev { 


	struct delayed_work	interleave_scan;



	struct list_head	monitored_devices;



#if IS_ENABLED(CONFIG_BT_LEDS)

	struct led_trigger	*power_led;

#endif

net/bluetooth/hci_core.c

+1 −0
Original line number Diff line number Diff line
@@ -2503,6 +2503,7 @@ struct hci_dev *hci_alloc_dev_priv(int sizeof_priv) 
	INIT_LIST_HEAD(&hdev->conn_hash.list);

	INIT_LIST_HEAD(&hdev->adv_instances);

	INIT_LIST_HEAD(&hdev->blocked_keys);

	INIT_LIST_HEAD(&hdev->monitored_devices);



	INIT_LIST_HEAD(&hdev->local_codecs);

	INIT_WORK(&hdev->rx_work, hci_rx_work);

net/bluetooth/msft.c

+150 −8
Original line number Diff line number Diff line
@@ -80,6 +80,14 @@ struct msft_rp_le_set_advertisement_filter_enable { 
	__u8 sub_opcode;

} __packed;



#define MSFT_EV_LE_MONITOR_DEVICE	0x02

struct 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;
@@ -204,6 +212,30 @@ static struct msft_monitor_advertisement_handle_data *msft_find_handle_data 
	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)

{

	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))) {

			list_del(&dev->list);

			kfree(dev);

			count++;

		}

	}



	return count;

}



static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,

					     u8 status, u16 opcode,

					     struct sk_buff *skb)
@@ -294,6 +326,10 @@ static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, 
		if (monitor && !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);



		list_del(&handle_data->list);

		kfree(handle_data);

	}
@@ -557,6 +593,13 @@ void msft_do_close(struct hci_dev *hdev) 
		list_del(&handle_data->list);

		kfree(handle_data);

	}



	hci_dev_lock(hdev);



	/* Clear any devices that are being monitored */

	msft_monitor_device_del(hdev, 0, NULL, 0);



	hci_dev_unlock(hdev);

}



void msft_register(struct hci_dev *hdev)
@@ -590,10 +633,97 @@ void msft_unregister(struct hci_dev *hdev) 
	kfree(msft);

}



/* This function requires the caller holds hdev->lock */

static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,

			      __u8 addr_type, __u16 mgmt_handle)

{

	struct monitored_device *dev;



	dev = kmalloc(sizeof(*dev), GFP_KERNEL);

	if (!dev) {

		bt_dev_err(hdev, "MSFT vendor event %u: no memory",

			   MSFT_EV_LE_MONITOR_DEVICE);

		return;

	}



	bacpy(&dev->bdaddr, bdaddr);

	dev->addr_type = addr_type;

	dev->handle = mgmt_handle;

	dev->notified = false;



	INIT_LIST_HEAD(&dev->list);

	list_add(&dev->list, &hdev->monitored_devices);

}



/* This function requires the caller holds hdev->lock */

static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,

			     __u8 addr_type, __u16 mgmt_handle)

{

	if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type)) {

		bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",

			   MSFT_EV_LE_MONITOR_DEVICE, bdaddr);

	}

}



static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,

			   u8 ev, size_t len)

{

	void *data;



	data = skb_pull_data(skb, len);

	if (!data)

		bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);



	return data;

}



/* This function requires the caller holds hdev->lock */

static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)

{

	struct msft_ev_le_monitor_device *ev;

	struct msft_monitor_advertisement_handle_data *handle_data;

	u8 addr_type;



	ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));

	if (!ev)

		return;



	bt_dev_dbg(hdev,

		   "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",

		   MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,

		   ev->monitor_state, &ev->bdaddr);



	handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);



	switch (ev->addr_type) {

	case ADDR_LE_DEV_PUBLIC:

		addr_type = BDADDR_LE_PUBLIC;

		break;



	case ADDR_LE_DEV_RANDOM:

		addr_type = BDADDR_LE_RANDOM;

		break;



	default:

		bt_dev_err(hdev,

			   "MSFT vendor event 0x%02x: unknown addr type 0x%02x",

			   MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);

		return;

	}



	if (ev->monitor_state)

		msft_device_found(hdev, &ev->bdaddr, addr_type,

				  handle_data->mgmt_handle);

	else

		msft_device_lost(hdev, &ev->bdaddr, addr_type,

				 handle_data->mgmt_handle);

}



void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)

{

	struct msft_data *msft = hdev->msft_data;

	u8 event;

	u8 *evt_prefix;

	u8 *evt;



	if (!msft)

		return;
@@ -602,13 +732,12 @@ void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) 
	 * matches, and otherwise just return.

	 */

	if (msft->evt_prefix_len > 0) {

		if (skb->len < msft->evt_prefix_len)

		evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);

		if (!evt_prefix)

			return;



		if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))

		if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))

			return;



		skb_pull(skb, msft->evt_prefix_len);

	}



	/* Every event starts at least with an event code and the rest of
@@ -617,10 +746,23 @@ void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) 
	if (skb->len < 1)

		return;



	event = *skb->data;

	skb_pull(skb, 1);

	evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));

	if (!evt)

		return;



	bt_dev_dbg(hdev, "MSFT vendor event %u", event);

	hci_dev_lock(hdev);



	switch (*evt) {

	case MSFT_EV_LE_MONITOR_DEVICE:

		msft_monitor_device_evt(hdev, skb);

		break;



	default:

		bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);

		break;

	}



	hci_dev_unlock(hdev);

}



__u64 msft_get_features(struct hci_dev *hdev)