diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 2560cfe80db855..6734b394c6e7a0 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -257,6 +257,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; @@ -588,6 +597,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 diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index fdc0dcf8ee364b..d4bcd511530a01 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -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); diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c index 1122097e1e49a3..06b3b006deb873 100644 --- a/net/bluetooth/msft.c +++ b/net/bluetooth/msft.c @@ -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; @@ -266,6 +274,7 @@ static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, struct msft_data *msft = hdev->msft_data; int err; bool pending; + struct monitored_device *dev, *tmp; if (status) goto done; @@ -296,6 +305,15 @@ static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, list_del(&handle_data->list); kfree(handle_data); + + /* Clear any monitored devices by this Adv Monitor */ + list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, + list) { + if (dev->handle == handle_data->mgmt_handle) { + list_del(&dev->list); + kfree(dev); + } + } } /* If remove all monitors is required, we need to continue the process @@ -538,6 +556,7 @@ void msft_do_close(struct hci_dev *hdev) struct msft_data *msft = hdev->msft_data; struct msft_monitor_advertisement_handle_data *handle_data, *tmp; struct adv_monitor *monitor; + struct monitored_device *dev, *tmp_dev; if (!msft) return; @@ -557,6 +576,16 @@ 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 */ + list_for_each_entry_safe(dev, tmp_dev, &hdev->monitored_devices, list) { + list_del(&dev->list); + kfree(dev); + } + + hci_dev_unlock(hdev); } void msft_register(struct hci_dev *hdev) @@ -590,6 +619,90 @@ 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) +{ + struct monitored_device *dev, *tmp; + + list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) { + if (dev->handle == mgmt_handle) { + list_del(&dev->list); + kfree(dev); + + break; + } + } +} + +/* 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 = (void *)skb->data; + struct msft_monitor_advertisement_handle_data *handle_data; + u8 addr_type; + + if (skb->len < sizeof(*ev)) { + bt_dev_err(hdev, + "MSFT vendor event %u: insufficient data (len: %u)", + MSFT_EV_LE_MONITOR_DEVICE, skb->len); + return; + } + skb_pull(skb, sizeof(*ev)); + + bt_dev_dbg(hdev, + "MSFT vendor event %u: 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 %u: unknown addr type 0x%02x", + 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, struct sk_buff *skb) { struct msft_data *msft = hdev->msft_data; @@ -617,10 +730,22 @@ void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb) if (skb->len < 1) return; + hci_dev_lock(hdev); + event = *skb->data; skb_pull(skb, 1); - bt_dev_dbg(hdev, "MSFT vendor event %u", event); + switch (event) { + case MSFT_EV_LE_MONITOR_DEVICE: + msft_monitor_device_evt(hdev, skb); + break; + + default: + bt_dev_dbg(hdev, "MSFT vendor event %u", event); + break; + } + + hci_dev_unlock(hdev); } __u64 msft_get_features(struct hci_dev *hdev)