diff options
| -rw-r--r-- | net/bluetooth/msft.c | 67 | ||||
| -rw-r--r-- | net/bluetooth/msft.h | 6 |
2 files changed, 73 insertions, 0 deletions
diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c index d25c6936daa4..b2ef654b1d3d 100644 --- a/net/bluetooth/msft.c +++ b/net/bluetooth/msft.c @@ -69,6 +69,17 @@ struct msft_rp_le_cancel_monitor_advertisement { __u8 sub_opcode; } __packed; +#define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05 +struct 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; + struct msft_monitor_advertisement_handle_data { __u8 msft_handle; __u16 mgmt_handle; @@ -83,6 +94,7 @@ struct msft_data { __u16 pending_add_handle; __u16 pending_remove_handle; __u8 reregistering; + __u8 filter_enabled; }; static int __msft_add_monitor_pattern(struct hci_dev *hdev, @@ -190,6 +202,7 @@ void msft_do_open(struct hci_dev *hdev) if (msft_monitor_supported(hdev)) { msft->reregistering = true; + msft_set_filter_enable(hdev, true); reregister_monitor_on_restart(hdev, 0); } } @@ -395,6 +408,40 @@ done: hci_remove_adv_monitor_complete(hdev, status); } +static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, + u8 status, u16 opcode, + struct sk_buff *skb) +{ + struct msft_cp_le_set_advertisement_filter_enable *cp; + struct msft_rp_le_set_advertisement_filter_enable *rp; + struct msft_data *msft = hdev->msft_data; + + rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data; + if (skb->len < sizeof(*rp)) + return; + + /* Error 0x0C would be returned if the filter enabled status is + * already set to whatever we were trying to set. + * Although the default state should be disabled, some controller set + * the initial value to enabled. Because there is no way to know the + * actual initial value before sending this command, here we also treat + * error 0x0C as success. + */ + if (status != 0x00 && status != 0x0C) + return; + + hci_dev_lock(hdev); + + cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); + msft->filter_enabled = cp->enable; + + if (status == 0x0C) + bt_dev_warn(hdev, "MSFT filter_enable is already %s", + cp->enable ? "on" : "off"); + + hci_dev_unlock(hdev); +} + static bool msft_monitor_rssi_valid(struct adv_monitor *monitor) { struct adv_rssi_thresholds *r = &monitor->rssi; @@ -531,3 +578,23 @@ int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, return err; } + +int msft_set_filter_enable(struct hci_dev *hdev, bool enable) +{ + struct msft_cp_le_set_advertisement_filter_enable cp; + struct hci_request req; + struct msft_data *msft = hdev->msft_data; + int err; + + if (!msft) + return -EOPNOTSUPP; + + cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE; + cp.enable = enable; + + hci_req_init(&req, hdev); + hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp); + err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb); + + return err; +} diff --git a/net/bluetooth/msft.h b/net/bluetooth/msft.h index 6f126a1f1688..f8e4d3a6d641 100644 --- a/net/bluetooth/msft.h +++ b/net/bluetooth/msft.h @@ -20,6 +20,7 @@ __u64 msft_get_features(struct hci_dev *hdev); int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor); int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, u16 handle); +int msft_set_filter_enable(struct hci_dev *hdev, bool enable); #else @@ -45,4 +46,9 @@ static inline int msft_remove_monitor(struct hci_dev *hdev, return -EOPNOTSUPP; } +static inline int msft_set_filter_enable(struct hci_dev *hdev, bool enable) +{ + return -EOPNOTSUPP; +} + #endif |