Skip to content

Commit

Permalink
Bluetooth: msft: Handle MSFT Monitor Device Event
Browse files Browse the repository at this point in the history 
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>
  • Loading branch information
Manish Mandlik authored and Luiz Augusto von Dentz committed Jan 20, 2022
1 parent 3afee21 commit 3368aa3
Show file tree
Hide file tree
Showing 3 changed files with 162 additions and 8 deletions.
11 changes: 11 additions & 0 deletions include/net/bluetooth/hci_core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions net/bluetooth/hci_core.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
158 changes: 150 additions & 8 deletions net/bluetooth/msft.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand All @@ -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;

hci_dev_lock(hdev);

switch (*evt) {
case MSFT_EV_LE_MONITOR_DEVICE:
msft_monitor_device_evt(hdev, skb);
break;

bt_dev_dbg(hdev, "MSFT vendor event %u", event);
default:
bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
break;
}

hci_dev_unlock(hdev);
}

__u64 msft_get_features(struct hci_dev *hdev)
Expand Down

0 comments on commit 3368aa3

Please sign in to comment.