mirror of
https://github.com/Fishwaldo/linux-bl808.git
synced 2025-03-18 04:54:52 +00:00
Bluetooth: advmon offload MSFT add rssi support
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>
This commit is contained in:
Archie Pusaka
Marcel Holtmann
parent
7f9f2c3f7d
commit
b4a221ea8a
3 changed files with 178 additions and 72 deletions
|
|
@ -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;
|
||||
unsigned int prev_adv_monitors_cnt;
|
||||
int err;
|
||||
|
||||
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);
|
||||
if (status)
|
||||
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);
|
||||
}
|
||||
|
||||
if (mp_cnt != cp->pattern_count) {
|
||||
err = mgmt_cmd_status(sk, hdev->id,
|
||||
MGMT_OP_ADD_ADV_PATTERNS_MONITOR,
|
||||
MGMT_STATUS_INVALID_PARAMS);
|
||||
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 @@ unlock:
|
|||
|
||||
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)
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue