Skip to content

Commit

Permalink
Bluetooth: advmon offload MSFT add rssi support
Browse files Browse the repository at this point in the history 
MSFT needs rssi parameter for monitoring advertisement packet,
therefore we should supply them from mgmt. This adds a new opcode
to add advertisement monitor with rssi parameters.

Signed-off-by: Archie Pusaka <apusaka@chromium.org>
Reviewed-by: Manish Mandlik <mmandlik@chromium.org>
Reviewed-by: Miao-chen Chou <mcchou@chromium.org>
Reviewed-by: Yun-Hao Chung <howardchung@google.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
  • Loading branch information
Archie Pusaka authored and Marcel Holtmann committed Jan 25, 2021
1 parent 7f9f2c3 commit b4a221e
Show file tree
Hide file tree
Showing 3 changed files with 178 additions and 72 deletions.
9 changes: 9 additions & 0 deletions include/net/bluetooth/hci_core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};
Expand Down
16 changes: 16 additions & 0 deletions include/net/bluetooth/mgmt.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
225 changes: 153 additions & 72 deletions net/bluetooth/mgmt.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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[] = {
Expand Down Expand Up @@ -4225,87 +4226,27 @@ 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);

prev_adv_monitors_cnt = hdev->adv_monitors_cnt;

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;
}

Expand All @@ -4316,15 +4257,152 @@ 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:
hci_dev_unlock(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,
Expand Down Expand Up @@ -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)
Expand Down

0 comments on commit b4a221e

Please sign in to comment.