@@ -250,8 +250,17 @@ struct adv_pattern {
__u8 value[HCI_MAX_AD_LENGTH];
};
+struct adv_rssi_thresholds {
+ __s8 low_threshold;
+ __s8 high_threshold;
+ __u16 low_threshold_timeout;
+ __u16 high_threshold_timeout;
+ __u8 sampling_period;
+};
+
struct adv_monitor {
struct list_head patterns;
+ struct adv_rssi_thresholds rssi;
bool active;
__u16 handle;
};
@@ -821,6 +821,22 @@ struct mgmt_rp_add_ext_adv_data {
__u8 instance;
} __packed;
+struct mgmt_adv_rssi_thresholds {
+ __s8 high_threshold;
+ __le16 high_threshold_timeout;
+ __s8 low_threshold;
+ __le16 low_threshold_timeout;
+ __u8 sampling_period;
+} __packed;
+
+#define MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI 0x0056
+struct mgmt_cp_add_adv_patterns_monitor_rssi {
+ struct mgmt_adv_rssi_thresholds rssi;
+ __u8 pattern_count;
+ struct mgmt_adv_pattern patterns[];
+} __packed;
+#define MGMT_ADD_ADV_PATTERNS_MONITOR_RSSI_SIZE 8
+
#define MGMT_EV_CMD_COMPLETE 0x0001
struct mgmt_ev_cmd_complete {
__le16 opcode;
@@ -124,6 +124,7 @@ static const u16 mgmt_commands[] = {
MGMT_OP_REMOVE_ADV_MONITOR,
MGMT_OP_ADD_EXT_ADV_PARAMS,
MGMT_OP_ADD_EXT_ADV_DATA,
+ MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI,
};
static const u16 mgmt_events[] = {
@@ -4225,75 +4226,15 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
return err;
}
-static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
- void *data, u16 len)
+static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
+ struct adv_monitor *m, u8 status, u16 op)
{
- struct mgmt_cp_add_adv_patterns_monitor *cp = data;
struct mgmt_rp_add_adv_patterns_monitor rp;
- struct adv_monitor *m = NULL;
- struct adv_pattern *p = NULL;
- unsigned int mp_cnt = 0, prev_adv_monitors_cnt;
- __u8 cp_ofst = 0, cp_len = 0;
- int err, i;
-
- BT_DBG("request for %s", hdev->name);
-
- if (len <= sizeof(*cp) || cp->pattern_count == 0) {
- err = mgmt_cmd_status(sk, hdev->id,
- MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
- }
-
- m = kmalloc(sizeof(*m), GFP_KERNEL);
- if (!m) {
- err = -ENOMEM;
- goto failed;
- }
-
- INIT_LIST_HEAD(&m->patterns);
- m->active = false;
-
- for (i = 0; i < cp->pattern_count; i++) {
- if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
- err = mgmt_cmd_status(sk, hdev->id,
- MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
- }
-
- cp_ofst = cp->patterns[i].offset;
- cp_len = cp->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,
- MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
- MGMT_STATUS_INVALID_PARAMS);
- goto failed;
- }
-
- p = kmalloc(sizeof(*p), GFP_KERNEL);
- if (!p) {
- err = -ENOMEM;
- goto failed;
- }
-
- p->ad_type = cp->patterns[i].ad_type;
- p->offset = cp->patterns[i].offset;
- p->length = cp->patterns[i].length;
- memcpy(p->value, cp->patterns[i].value, p->length);
-
- INIT_LIST_HEAD(&p->list);
- list_add(&p->list, &m->patterns);
- }
+ unsigned int prev_adv_monitors_cnt;
+ int err;
- if (mp_cnt != cp->pattern_count) {
- err = mgmt_cmd_status(sk, hdev->id,
- MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
- MGMT_STATUS_INVALID_PARAMS);
+ if (status)
goto failed;
- }
hci_dev_lock(hdev);
@@ -4301,11 +4242,11 @@ static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
err = hci_add_adv_monitor(hdev, m);
if (err) {
- if (err == -ENOSPC) {
- mgmt_cmd_status(sk, hdev->id,
- MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
- MGMT_STATUS_NO_RESOURCES);
- }
+ if (err == -ENOSPC)
+ status = MGMT_STATUS_NO_RESOURCES;
+ else
+ status = MGMT_STATUS_FAILED;
+
goto unlock;
}
@@ -4316,7 +4257,7 @@ static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
rp.monitor_handle = cpu_to_le16(m->handle);
- return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
+ return mgmt_cmd_complete(sk, hdev->id, op,
MGMT_STATUS_SUCCESS, &rp, sizeof(rp));
unlock:
@@ -4324,7 +4265,144 @@ static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
failed:
hci_free_adv_monitor(m);
- return err;
+ return mgmt_cmd_status(sk, hdev->id, op, status);
+}
+
+static void parse_adv_monitor_rssi(struct adv_monitor *m,
+ struct mgmt_adv_rssi_thresholds *rssi)
+{
+ if (rssi) {
+ m->rssi.low_threshold = rssi->low_threshold;
+ m->rssi.low_threshold_timeout =
+ __le16_to_cpu(rssi->low_threshold_timeout);
+ m->rssi.high_threshold = rssi->high_threshold;
+ m->rssi.high_threshold_timeout =
+ __le16_to_cpu(rssi->high_threshold_timeout);
+ m->rssi.sampling_period = rssi->sampling_period;
+ } else {
+ /* Default values. These numbers are the least constricting
+ * parameters for MSFT API to work, so it behaves as if there
+ * are no rssi parameter to consider. May need to be changed
+ * if other API are to be supported.
+ */
+ m->rssi.low_threshold = -127;
+ m->rssi.low_threshold_timeout = 60;
+ m->rssi.high_threshold = -127;
+ m->rssi.high_threshold_timeout = 0;
+ m->rssi.sampling_period = 0;
+ }
+}
+
+static u8 parse_adv_monitor_pattern(struct adv_monitor *m, u8 pattern_count,
+ struct mgmt_adv_pattern *patterns)
+{
+ u8 offset = 0, length = 0;
+ struct adv_pattern *p = NULL;
+ unsigned int mp_cnt = 0;
+ int i;
+
+ for (i = 0; i < pattern_count; i++) {
+ if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS)
+ return MGMT_STATUS_INVALID_PARAMS;
+
+ offset = patterns[i].offset;
+ length = patterns[i].length;
+ if (offset >= HCI_MAX_AD_LENGTH ||
+ length > HCI_MAX_AD_LENGTH ||
+ (offset + length) > HCI_MAX_AD_LENGTH)
+ return MGMT_STATUS_INVALID_PARAMS;
+
+ p = kmalloc(sizeof(*p), GFP_KERNEL);
+ if (!p)
+ return MGMT_STATUS_NO_RESOURCES;
+
+ p->ad_type = patterns[i].ad_type;
+ p->offset = patterns[i].offset;
+ p->length = patterns[i].length;
+ memcpy(p->value, patterns[i].value, p->length);
+
+ INIT_LIST_HEAD(&p->list);
+ list_add(&p->list, &m->patterns);
+ }
+
+ if (mp_cnt != pattern_count)
+ return MGMT_STATUS_INVALID_PARAMS;
+
+ return MGMT_STATUS_SUCCESS;
+}
+
+static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ struct mgmt_cp_add_adv_patterns_monitor *cp = data;
+ struct adv_monitor *m = NULL;
+ u8 status = MGMT_STATUS_SUCCESS;
+ size_t expected_size = sizeof(*cp);
+
+ BT_DBG("request for %s", hdev->name);
+
+ if (len <= sizeof(*cp)) {
+ status = MGMT_STATUS_INVALID_PARAMS;
+ goto done;
+ }
+
+ expected_size += cp->pattern_count * sizeof(struct mgmt_adv_pattern);
+ if (len != expected_size) {
+ status = MGMT_STATUS_INVALID_PARAMS;
+ goto done;
+ }
+
+ m = kzalloc(sizeof(*m), GFP_KERNEL);
+ if (!m) {
+ status = MGMT_STATUS_NO_RESOURCES;
+ goto done;
+ }
+
+ INIT_LIST_HEAD(&m->patterns);
+
+ parse_adv_monitor_rssi(m, NULL);
+ status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);
+
+done:
+ return __add_adv_patterns_monitor(sk, hdev, m, status,
+ MGMT_OP_ADD_ADV_PATTERNS_MONITOR);
+}
+
+static int add_adv_patterns_monitor_rssi(struct sock *sk, struct hci_dev *hdev,
+ void *data, u16 len)
+{
+ struct mgmt_cp_add_adv_patterns_monitor_rssi *cp = data;
+ struct adv_monitor *m = NULL;
+ u8 status = MGMT_STATUS_SUCCESS;
+ size_t expected_size = sizeof(*cp);
+
+ BT_DBG("request for %s", hdev->name);
+
+ if (len <= sizeof(*cp)) {
+ status = MGMT_STATUS_INVALID_PARAMS;
+ goto done;
+ }
+
+ expected_size += cp->pattern_count * sizeof(struct mgmt_adv_pattern);
+ if (len != expected_size) {
+ status = MGMT_STATUS_INVALID_PARAMS;
+ goto done;
+ }
+
+ m = kzalloc(sizeof(*m), GFP_KERNEL);
+ if (!m) {
+ status = MGMT_STATUS_NO_RESOURCES;
+ goto done;
+ }
+
+ INIT_LIST_HEAD(&m->patterns);
+
+ parse_adv_monitor_rssi(m, &cp->rssi);
+ status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);
+
+done:
+ return __add_adv_patterns_monitor(sk, hdev, m, status,
+ MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI);
}
static int remove_adv_monitor(struct sock *sk, struct hci_dev *hdev,
@@ -8242,6 +8320,9 @@ static const struct hci_mgmt_handler mgmt_handlers[] = {
HCI_MGMT_VAR_LEN },
{ add_ext_adv_data, MGMT_ADD_EXT_ADV_DATA_SIZE,
HCI_MGMT_VAR_LEN },
+ { add_adv_patterns_monitor_rssi,
+ MGMT_ADD_ADV_PATTERNS_MONITOR_RSSI_SIZE,
+ HCI_MGMT_VAR_LEN },
};
void mgmt_index_added(struct hci_dev *hdev)