system/linux
system/linux
1
0
Fork 0
mirror of https://github.com/torvalds/linux synced 2024-09-16 09:05:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Bluetooth: msft: Handle MSFT Monitor Device Event

Browse source 
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: Manish Mandlik <mmandlik@google.com>
Reviewed-by: Miao-chen Chou <mcchou@google.com>
Signed-off-by: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>
This commit is contained in:
Manish Mandlik 2022-01-11 08:14:25 -08:00 committed by Luiz Augusto von Dentz
parent 3afee21181
commit 3368aa357f
3 changed files with 162 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
include/net/bluetooth/hci_core.h
View file

@ -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

1
net/bluetooth/hci_core.c
View file

@ -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);

158
net/bluetooth/msft.c
View file

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