@@ -261,13 +261,20 @@ struct adv_rssi_thresholds {
struct adv_monitor {
struct list_head patterns;
struct adv_rssi_thresholds rssi;
- bool active;
__u16 handle;
+
+ enum {
+ ADV_MONITOR_STATE_NOT_REGISTERED,
+ ADV_MONITOR_STATE_REGISTERED,
+ ADV_MONITOR_STATE_OFFLOADED
+ } state;
};
#define HCI_MIN_ADV_MONITOR_HANDLE 1
-#define HCI_MAX_ADV_MONITOR_NUM_HANDLES 32
+#define HCI_MAX_ADV_MONITOR_NUM_HANDLES 32
#define HCI_MAX_ADV_MONITOR_NUM_PATTERNS 16
+#define HCI_ADV_MONITOR_EXT_NONE 1
+#define HCI_ADV_MONITOR_EXT_MSFT 2
#define HCI_MAX_SHORT_NAME_LENGTH 10
@@ -1326,9 +1333,12 @@ void hci_adv_instances_set_rpa_expired(struct hci_dev *hdev, bool rpa_expired);
void hci_adv_monitors_clear(struct hci_dev *hdev);
void hci_free_adv_monitor(struct adv_monitor *monitor);
-int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor);
+int hci_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status);
+bool hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
+ int *err);
int hci_remove_adv_monitor(struct hci_dev *hdev, u16 handle);
bool hci_is_adv_monitoring(struct hci_dev *hdev);
+int hci_get_adv_monitor_offload_ext(struct hci_dev *hdev);
void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb);
@@ -1804,6 +1814,7 @@ void mgmt_advertising_added(struct sock *sk, struct hci_dev *hdev,
void mgmt_advertising_removed(struct sock *sk, struct hci_dev *hdev,
u8 instance);
int mgmt_phy_configuration_changed(struct hci_dev *hdev, struct sock *skip);
+int mgmt_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status);
u8 hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, u16 latency,
u16 to_multiplier);
@@ -3070,27 +3070,55 @@ void hci_free_adv_monitor(struct adv_monitor *monitor)
kfree(monitor);
}
-/* This function requires the caller holds hdev->lock */
-int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
+int hci_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status)
+{
+ return mgmt_add_adv_patterns_monitor_complete(hdev, status);
+}
+
+/* Assigns handle to a monitor, and if offloading is supported and power is on,
+ * also attempts to forward the request to the controller.
+ * Returns true if request is forwarded (result is pending), false otherwise.
+ * This function requires the caller holds hdev->lock.
+ */
+bool hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
+ int *err)
{
int min, max, handle;
- if (!monitor)
- return -EINVAL;
+ *err = 0;
+
+ if (!monitor) {
+ *err = -EINVAL;
+ return false;
+ }
min = HCI_MIN_ADV_MONITOR_HANDLE;
max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES;
handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max,
GFP_KERNEL);
- if (handle < 0)
- return handle;
+ if (handle < 0) {
+ *err = handle;
+ return false;
+ }
- hdev->adv_monitors_cnt++;
monitor->handle = handle;
- hci_update_background_scan(hdev);
+ if (!hdev_is_powered(hdev))
+ return false;
- return 0;
+ switch (hci_get_adv_monitor_offload_ext(hdev)) {
+ case HCI_ADV_MONITOR_EXT_NONE:
+ hci_update_background_scan(hdev);
+ BT_DBG("%s add monitor status %d", hdev->name, *err);
+ /* Message was not forwarded to controller - not an error */
+ return false;
+ case HCI_ADV_MONITOR_EXT_MSFT:
+ *err = msft_add_monitor_pattern(hdev, monitor);
+ BT_DBG("%s add monitor msft status %d", hdev->name, *err);
+ break;
+ }
+
+ return (*err == 0);
}
static int free_adv_monitor(int id, void *ptr, void *data)
@@ -3134,6 +3162,14 @@ bool hci_is_adv_monitoring(struct hci_dev *hdev)
return !idr_is_empty(&hdev->adv_monitors_idr);
}
+int hci_get_adv_monitor_offload_ext(struct hci_dev *hdev)
+{
+ if (msft_monitor_supported(hdev))
+ return HCI_ADV_MONITOR_EXT_MSFT;
+
+ return HCI_ADV_MONITOR_EXT_NONE;
+}
+
struct bdaddr_list *hci_bdaddr_list_lookup(struct list_head *bdaddr_list,
bdaddr_t *bdaddr, u8 type)
{
@@ -4185,6 +4185,7 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
int handle, err;
size_t rp_size = 0;
__u32 supported = 0;
+ __u32 enabled = 0;
__u16 num_handles = 0;
__u16 handles[HCI_MAX_ADV_MONITOR_NUM_HANDLES];
@@ -4192,12 +4193,11 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
hci_dev_lock(hdev);
- if (msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR)
+ if (msft_monitor_supported(hdev))
supported |= MGMT_ADV_MONITOR_FEATURE_MASK_OR_PATTERNS;
- idr_for_each_entry(&hdev->adv_monitors_idr, monitor, handle) {
+ idr_for_each_entry(&hdev->adv_monitors_idr, monitor, handle)
handles[num_handles++] = monitor->handle;
- }
hci_dev_unlock(hdev);
@@ -4206,11 +4206,11 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
if (!rp)
return -ENOMEM;
- /* Once controller-based monitoring is in place, the enabled_features
- * should reflect the use.
- */
+ /* All supported features are currently enabled */
+ enabled = supported;
+
rp->supported_features = cpu_to_le32(supported);
- rp->enabled_features = 0;
+ rp->enabled_features = cpu_to_le32(enabled);
rp->max_num_handles = cpu_to_le16(HCI_MAX_ADV_MONITOR_NUM_HANDLES);
rp->max_num_patterns = HCI_MAX_ADV_MONITOR_NUM_PATTERNS;
rp->num_handles = cpu_to_le16(num_handles);
@@ -4226,6 +4226,45 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
return err;
}
+int mgmt_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status)
+{
+ struct mgmt_rp_add_adv_patterns_monitor rp;
+ struct mgmt_pending_cmd *cmd;
+ struct adv_monitor *monitor;
+ int err = 0;
+
+ hci_dev_lock(hdev);
+
+ cmd = pending_find(MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI, hdev);
+ if (!cmd) {
+ cmd = pending_find(MGMT_OP_ADD_ADV_PATTERNS_MONITOR, hdev);
+ if (!cmd)
+ goto done;
+ }
+
+ monitor = cmd->user_data;
+ rp.monitor_handle = cpu_to_le16(monitor->handle);
+
+ if (!status) {
+ mgmt_adv_monitor_added(cmd->sk, hdev, monitor->handle);
+ hdev->adv_monitors_cnt++;
+ if (monitor->state == ADV_MONITOR_STATE_NOT_REGISTERED)
+ monitor->state = ADV_MONITOR_STATE_REGISTERED;
+ hci_update_background_scan(hdev);
+ }
+
+ err = mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode,
+ mgmt_status(status), &rp, sizeof(rp));
+ mgmt_pending_remove(cmd);
+
+done:
+ hci_dev_unlock(hdev);
+ bt_dev_dbg(hdev, "add monitor %d complete, status %d",
+ rp.monitor_handle, status);
+
+ return err;
+}
+
static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
void *data, u16 len, u16 op)
{
@@ -4236,14 +4275,25 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
struct mgmt_adv_pattern *patterns = NULL;
struct adv_monitor *m = NULL;
struct adv_pattern *p = NULL;
- unsigned int mp_cnt = 0, prev_adv_monitors_cnt;
+ struct mgmt_pending_cmd *cmd;
__u8 cp_ofst = 0, cp_len = 0;
- int err, i;
+ int err, status, i;
+ bool pending;
u8 pattern_count;
u16 expected_len;
BT_DBG("request for %s", hdev->name);
+ hci_dev_lock(hdev);
+
+ if (pending_find(MGMT_OP_SET_LE, hdev) ||
+ pending_find(MGMT_OP_ADD_ADV_PATTERNS_MONITOR, hdev) ||
+ pending_find(MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI, hdev) ||
+ pending_find(MGMT_OP_REMOVE_ADV_MONITOR, hdev)) {
+ status = MGMT_STATUS_BUSY;
+ goto unlock;
+ }
+
if (op == MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI) {
cp_rssi = data;
pattern_count = cp_rssi->pattern_count;
@@ -4258,20 +4308,20 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
expected_len = sizeof(*cp) + pattern_count * sizeof(*patterns);
}
- if (len != expected_len || pattern_count == 0) {
- err = mgmt_cmd_status(sk, hdev->id, op,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
+ if (len != expected_len || pattern_count == 0 ||
+ pattern_count > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
+ status = MGMT_STATUS_INVALID_PARAMS;
+ goto unlock;
}
m = kmalloc(sizeof(*m), GFP_KERNEL);
if (!m) {
- err = -ENOMEM;
- goto failed;
+ status = MGMT_STATUS_NO_RESOURCES;
+ goto unlock;
}
INIT_LIST_HEAD(&m->patterns);
- m->active = false;
+ m->state = ADV_MONITOR_STATE_NOT_REGISTERED;
if (rssi) {
m->rssi.low_threshold = rssi->low_threshold;
@@ -4295,26 +4345,19 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
}
for (i = 0; i < pattern_count; i++) {
- if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
- err = mgmt_cmd_status(sk, hdev->id, op,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
- }
-
cp_ofst = patterns[i].offset;
cp_len = patterns[i].length;
if (cp_ofst >= HCI_MAX_AD_LENGTH ||
cp_len > HCI_MAX_AD_LENGTH ||
(cp_ofst + cp_len) > HCI_MAX_AD_LENGTH) {
- err = mgmt_cmd_status(sk, hdev->id, op,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
+ status = MGMT_STATUS_INVALID_PARAMS;
+ goto unlock;
}
p = kmalloc(sizeof(*p), GFP_KERNEL);
if (!p) {
- err = -ENOMEM;
- goto failed;
+ status = MGMT_STATUS_NO_RESOURCES;
+ goto unlock;
}
p->ad_type = patterns[i].ad_type;
@@ -4326,41 +4369,46 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
list_add(&p->list, &m->patterns);
}
- if (mp_cnt != pattern_count) {
- err = mgmt_cmd_status(sk, hdev->id, op,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
+ cmd = mgmt_pending_add(sk, op, hdev, data, len);
+ if (!cmd) {
+ status = MGMT_STATUS_NO_RESOURCES;
+ goto unlock;
}
- hci_dev_lock(hdev);
-
- prev_adv_monitors_cnt = hdev->adv_monitors_cnt;
-
- err = hci_add_adv_monitor(hdev, m);
+ pending = hci_add_adv_monitor(hdev, m, &err);
if (err) {
- if (err == -ENOSPC) {
- mgmt_cmd_status(sk, hdev->id, op,
- MGMT_STATUS_NO_RESOURCES);
- }
+ if (err == -ENOSPC || err == -ENOMEM)
+ status = MGMT_STATUS_NO_RESOURCES;
+ else if (err == -EINVAL)
+ status = MGMT_STATUS_INVALID_PARAMS;
+ else
+ status = MGMT_STATUS_FAILED;
+
+ mgmt_pending_remove(cmd);
goto unlock;
}
- if (hdev->adv_monitors_cnt > prev_adv_monitors_cnt)
+ if (!pending) {
+ mgmt_pending_remove(cmd);
+ rp.monitor_handle = cpu_to_le16(m->handle);
mgmt_adv_monitor_added(sk, hdev, m->handle);
+ m->state = ADV_MONITOR_STATE_REGISTERED;
+ hdev->adv_monitors_cnt++;
- hci_dev_unlock(hdev);
+ hci_dev_unlock(hdev);
+ return mgmt_cmd_complete(sk, hdev->id, op, MGMT_STATUS_SUCCESS,
+ &rp, sizeof(rp));
+ }
- rp.monitor_handle = cpu_to_le16(m->handle);
+ hci_dev_unlock(hdev);
- return mgmt_cmd_complete(sk, hdev->id, op,
- MGMT_STATUS_SUCCESS, &rp, sizeof(rp));
+ cmd->user_data = m;
+ return 0;
unlock:
hci_dev_unlock(hdev);
-
-failed:
hci_free_adv_monitor(m);
- return err;
+ return mgmt_cmd_status(sk, hdev->id, op, status);
}
static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
@@ -5,9 +5,16 @@
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/mgmt.h>
+#include "hci_request.h"
+#include "mgmt_util.h"
#include "msft.h"
+#define MSFT_RSSI_THRESHOLD_VALUE_MIN -127
+#define MSFT_RSSI_THRESHOLD_VALUE_MAX 20
+#define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C
+
#define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
struct msft_cp_read_supported_features {
__u8 sub_opcode;
@@ -21,12 +28,55 @@ struct msft_rp_read_supported_features {
__u8 evt_prefix[];
} __packed;
+#define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03
+#define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01
+struct msft_le_monitor_advertisement_pattern {
+ __u8 length;
+ __u8 data_type;
+ __u8 start_byte;
+ __u8 pattern[0];
+};
+
+struct msft_le_monitor_advertisement_pattern_data {
+ __u8 count;
+ __u8 data[0];
+};
+
+struct msft_cp_le_monitor_advertisement {
+ __u8 sub_opcode;
+ __s8 rssi_high;
+ __s8 rssi_low;
+ __u8 rssi_low_interval;
+ __u8 rssi_sampling_period;
+ __u8 cond_type;
+ __u8 data[0];
+} __packed;
+
+struct msft_rp_le_monitor_advertisement {
+ __u8 status;
+ __u8 sub_opcode;
+ __u8 handle;
+} __packed;
+
+struct msft_monitor_advertisement_handle_data {
+ __u8 msft_handle;
+ __u16 mgmt_handle;
+ struct list_head list;
+};
+
struct msft_data {
__u64 features;
__u8 evt_prefix_len;
__u8 *evt_prefix;
+ struct list_head handle_map;
+ __u16 pending_add_handle;
};
+bool msft_monitor_supported(struct hci_dev *hdev)
+{
+ return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
+}
+
static bool read_supported_features(struct hci_dev *hdev,
struct msft_data *msft)
{
@@ -90,12 +140,14 @@ void msft_do_open(struct hci_dev *hdev)
return;
}
+ INIT_LIST_HEAD(&msft->handle_map);
hdev->msft_data = msft;
}
void msft_do_close(struct hci_dev *hdev)
{
struct msft_data *msft = hdev->msft_data;
+ struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
if (!msft)
return;
@@ -104,6 +156,11 @@ void msft_do_close(struct hci_dev *hdev)
hdev->msft_data = NULL;
+ list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
+ list_del(&handle_data->list);
+ kfree(handle_data);
+ }
+
kfree(msft->evt_prefix);
kfree(msft);
}
@@ -145,5 +202,147 @@ __u64 msft_get_features(struct hci_dev *hdev)
{
struct msft_data *msft = hdev->msft_data;
- return msft ? msft->features : 0;
+ return msft ? msft->features : 0;
+}
+
+static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
+ u8 status, u16 opcode,
+ struct sk_buff *skb)
+{
+ struct msft_rp_le_monitor_advertisement *rp;
+ struct adv_monitor *monitor;
+ struct msft_monitor_advertisement_handle_data *handle_data;
+ struct msft_data *msft = hdev->msft_data;
+
+ hci_dev_lock(hdev);
+
+ monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
+ if (!monitor) {
+ bt_dev_err(hdev, "msft add advmon: monitor %d is not found!",
+ msft->pending_add_handle);
+ status = HCI_ERROR_UNSPECIFIED;
+ goto unlock;
+ }
+
+ if (status)
+ goto unlock;
+
+ rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
+ if (skb->len < sizeof(*rp)) {
+ status = HCI_ERROR_UNSPECIFIED;
+ goto unlock;
+ }
+
+ handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
+ if (!handle_data) {
+ status = HCI_ERROR_UNSPECIFIED;
+ goto unlock;
+ }
+
+ handle_data->mgmt_handle = monitor->handle;
+ handle_data->msft_handle = rp->handle;
+ INIT_LIST_HEAD(&handle_data->list);
+ list_add(&handle_data->list, &msft->handle_map);
+
+ monitor->state = ADV_MONITOR_STATE_OFFLOADED;
+
+unlock:
+ if (status && monitor) {
+ idr_remove(&hdev->adv_monitors_idr, monitor->handle);
+ hci_free_adv_monitor(monitor);
+ }
+
+ hci_dev_unlock(hdev);
+
+ hci_add_adv_patterns_monitor_complete(hdev, status);
+}
+
+static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
+{
+ struct adv_rssi_thresholds *r = &monitor->rssi;
+
+ if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
+ r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
+ r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
+ r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
+ return false;
+
+ /* High_threshold_timeout is not supported,
+ * once high_threshold is reached, events are immediately reported.
+ */
+ if (r->high_threshold_timeout != 0)
+ return false;
+
+ if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
+ return false;
+
+ /* Sampling period from 0x00 to 0xFF are all allowed */
+ return true;
+}
+
+static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
+{
+ return msft_monitor_rssi_valid(monitor);
+ /* No additional check needed for pattern-based monitor */
+}
+
+/* This function requires the caller holds hdev->lock */
+int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
+{
+ struct msft_cp_le_monitor_advertisement *cp;
+ struct msft_le_monitor_advertisement_pattern_data *pattern_data;
+ struct msft_le_monitor_advertisement_pattern *pattern;
+ struct adv_pattern *entry;
+ struct hci_request req;
+ struct msft_data *msft = hdev->msft_data;
+ size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
+ ptrdiff_t offset = 0;
+ u8 pattern_count = 0;
+ int err = 0;
+
+ if (!msft)
+ return -EOPNOTSUPP;
+
+ if (!msft_monitor_pattern_valid(monitor))
+ return -EINVAL;
+
+ list_for_each_entry(entry, &monitor->patterns, list) {
+ pattern_count++;
+ total_size += sizeof(*pattern) + entry->length;
+ }
+
+ cp = kmalloc(total_size, GFP_KERNEL);
+ if (!cp)
+ return -ENOMEM;
+
+ cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
+ cp->rssi_high = monitor->rssi.high_threshold;
+ cp->rssi_low = monitor->rssi.low_threshold;
+ cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
+ cp->rssi_sampling_period = monitor->rssi.sampling_period;
+
+ cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
+
+ pattern_data = (void *)cp->data;
+ pattern_data->count = pattern_count;
+
+ list_for_each_entry(entry, &monitor->patterns, list) {
+ pattern = (void *)(pattern_data->data + offset);
+ /* the length also includes data_type and offset */
+ pattern->length = entry->length + 2;
+ pattern->data_type = entry->ad_type;
+ pattern->start_byte = entry->offset;
+ memcpy(pattern->pattern, entry->value, entry->length);
+ offset += sizeof(*pattern) + entry->length;
+ }
+
+ hci_req_init(&req, hdev);
+ hci_req_add(&req, hdev->msft_opcode, total_size, cp);
+ err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
+ kfree(cp);
+
+ if (!err)
+ msft->pending_add_handle = monitor->handle;
+
+ return err;
}
@@ -12,16 +12,28 @@
#if IS_ENABLED(CONFIG_BT_MSFTEXT)
+bool msft_monitor_supported(struct hci_dev *hdev);
void msft_do_open(struct hci_dev *hdev);
void msft_do_close(struct hci_dev *hdev);
void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb);
__u64 msft_get_features(struct hci_dev *hdev);
+int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor);
#else
+static inline bool msft_monitor_supported(struct hci_dev *hdev)
+{
+ return false;
+}
+
static inline void msft_do_open(struct hci_dev *hdev) {}
static inline void msft_do_close(struct hci_dev *hdev) {}
static inline void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb) {}
static inline __u64 msft_get_features(struct hci_dev *hdev) { return 0; }
+static inline int msft_add_monitor_pattern(struct hci_dev *hdev,
+ struct adv_monitor *monitor)
+{
+ return -EOPNOTSUPP;
+}
#endif