mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
synced 2026-07-22 03:27:30 -04:00
Bluetooth: advmon offload MSFT add monitor
Enables advertising monitor offloading to the controller, if MSFT extension is supported. The kernel won't adjust the monitor parameters to match what the controller supports - that is the user space's responsibility. This patch only manages the addition of monitors. Monitor removal is going to be handled by another patch. 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:
committed by
Marcel Holtmann
parent
b4a221ea8a
commit
a2a4dedf88
@@ -261,13 +261,20 @@ struct adv_rssi_thresholds {
|
|||||||
struct adv_monitor {
|
struct adv_monitor {
|
||||||
struct list_head patterns;
|
struct list_head patterns;
|
||||||
struct adv_rssi_thresholds rssi;
|
struct adv_rssi_thresholds rssi;
|
||||||
bool active;
|
|
||||||
__u16 handle;
|
__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_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_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
|
#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_adv_monitors_clear(struct hci_dev *hdev);
|
||||||
void hci_free_adv_monitor(struct adv_monitor *monitor);
|
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);
|
int hci_remove_adv_monitor(struct hci_dev *hdev, u16 handle);
|
||||||
bool hci_is_adv_monitoring(struct hci_dev *hdev);
|
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);
|
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,
|
void mgmt_advertising_removed(struct sock *sk, struct hci_dev *hdev,
|
||||||
u8 instance);
|
u8 instance);
|
||||||
int mgmt_phy_configuration_changed(struct hci_dev *hdev, struct sock *skip);
|
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,
|
u8 hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, u16 latency,
|
||||||
u16 to_multiplier);
|
u16 to_multiplier);
|
||||||
|
|||||||
@@ -3070,27 +3070,56 @@ void hci_free_adv_monitor(struct adv_monitor *monitor)
|
|||||||
kfree(monitor);
|
kfree(monitor);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* This function requires the caller holds hdev->lock */
|
int hci_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status)
|
||||||
int hci_add_adv_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
|
{
|
||||||
|
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;
|
int min, max, handle;
|
||||||
|
|
||||||
if (!monitor)
|
*err = 0;
|
||||||
return -EINVAL;
|
|
||||||
|
if (!monitor) {
|
||||||
|
*err = -EINVAL;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
min = HCI_MIN_ADV_MONITOR_HANDLE;
|
min = HCI_MIN_ADV_MONITOR_HANDLE;
|
||||||
max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES;
|
max = HCI_MIN_ADV_MONITOR_HANDLE + HCI_MAX_ADV_MONITOR_NUM_HANDLES;
|
||||||
handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max,
|
handle = idr_alloc(&hdev->adv_monitors_idr, monitor, min, max,
|
||||||
GFP_KERNEL);
|
GFP_KERNEL);
|
||||||
if (handle < 0)
|
if (handle < 0) {
|
||||||
return handle;
|
*err = handle;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
hdev->adv_monitors_cnt++;
|
|
||||||
monitor->handle = handle;
|
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_dev_dbg(hdev, "%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_dev_dbg(hdev, "%s add monitor msft status %d", hdev->name,
|
||||||
|
*err);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return (*err == 0);
|
||||||
}
|
}
|
||||||
|
|
||||||
static int free_adv_monitor(int id, void *ptr, void *data)
|
static int free_adv_monitor(int id, void *ptr, void *data)
|
||||||
@@ -3134,6 +3163,14 @@ bool hci_is_adv_monitoring(struct hci_dev *hdev)
|
|||||||
return !idr_is_empty(&hdev->adv_monitors_idr);
|
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,
|
struct bdaddr_list *hci_bdaddr_list_lookup(struct list_head *bdaddr_list,
|
||||||
bdaddr_t *bdaddr, u8 type)
|
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;
|
int handle, err;
|
||||||
size_t rp_size = 0;
|
size_t rp_size = 0;
|
||||||
__u32 supported = 0;
|
__u32 supported = 0;
|
||||||
|
__u32 enabled = 0;
|
||||||
__u16 num_handles = 0;
|
__u16 num_handles = 0;
|
||||||
__u16 handles[HCI_MAX_ADV_MONITOR_NUM_HANDLES];
|
__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);
|
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;
|
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;
|
handles[num_handles++] = monitor->handle;
|
||||||
}
|
|
||||||
|
|
||||||
hci_dev_unlock(hdev);
|
hci_dev_unlock(hdev);
|
||||||
|
|
||||||
@@ -4206,11 +4206,11 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
|
|||||||
if (!rp)
|
if (!rp)
|
||||||
return -ENOMEM;
|
return -ENOMEM;
|
||||||
|
|
||||||
/* Once controller-based monitoring is in place, the enabled_features
|
/* All supported features are currently enabled */
|
||||||
* should reflect the use.
|
enabled = supported;
|
||||||
*/
|
|
||||||
rp->supported_features = cpu_to_le32(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_handles = cpu_to_le16(HCI_MAX_ADV_MONITOR_NUM_HANDLES);
|
||||||
rp->max_num_patterns = HCI_MAX_ADV_MONITOR_NUM_PATTERNS;
|
rp->max_num_patterns = HCI_MAX_ADV_MONITOR_NUM_PATTERNS;
|
||||||
rp->num_handles = cpu_to_le16(num_handles);
|
rp->num_handles = cpu_to_le16(num_handles);
|
||||||
@@ -4226,44 +4226,105 @@ static int read_adv_mon_features(struct sock *sk, struct hci_dev *hdev,
|
|||||||
return err;
|
return err;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
|
int mgmt_add_adv_patterns_monitor_complete(struct hci_dev *hdev, u8 status)
|
||||||
struct adv_monitor *m, u8 status, u16 op)
|
|
||||||
{
|
{
|
||||||
struct mgmt_rp_add_adv_patterns_monitor rp;
|
struct mgmt_rp_add_adv_patterns_monitor rp;
|
||||||
unsigned int prev_adv_monitors_cnt;
|
struct mgmt_pending_cmd *cmd;
|
||||||
int err;
|
struct adv_monitor *monitor;
|
||||||
|
int err = 0;
|
||||||
if (status)
|
|
||||||
goto failed;
|
|
||||||
|
|
||||||
hci_dev_lock(hdev);
|
hci_dev_lock(hdev);
|
||||||
|
|
||||||
prev_adv_monitors_cnt = hdev->adv_monitors_cnt;
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
err = hci_add_adv_monitor(hdev, m);
|
monitor = cmd->user_data;
|
||||||
if (err) {
|
rp.monitor_handle = cpu_to_le16(monitor->handle);
|
||||||
if (err == -ENOSPC)
|
|
||||||
status = MGMT_STATUS_NO_RESOURCES;
|
|
||||||
else
|
|
||||||
status = MGMT_STATUS_FAILED;
|
|
||||||
|
|
||||||
|
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,
|
||||||
|
struct adv_monitor *m, u8 status,
|
||||||
|
void *data, u16 len, u16 op)
|
||||||
|
{
|
||||||
|
struct mgmt_rp_add_adv_patterns_monitor rp;
|
||||||
|
struct mgmt_pending_cmd *cmd;
|
||||||
|
int err;
|
||||||
|
bool pending;
|
||||||
|
|
||||||
|
hci_dev_lock(hdev);
|
||||||
|
|
||||||
|
if (status)
|
||||||
|
goto unlock;
|
||||||
|
|
||||||
|
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;
|
goto unlock;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (hdev->adv_monitors_cnt > prev_adv_monitors_cnt)
|
cmd = mgmt_pending_add(sk, op, hdev, data, len);
|
||||||
|
if (!cmd) {
|
||||||
|
status = MGMT_STATUS_NO_RESOURCES;
|
||||||
|
goto unlock;
|
||||||
|
}
|
||||||
|
|
||||||
|
pending = hci_add_adv_monitor(hdev, m, &err);
|
||||||
|
if (err) {
|
||||||
|
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 (!pending) {
|
||||||
|
mgmt_pending_remove(cmd);
|
||||||
|
rp.monitor_handle = cpu_to_le16(m->handle);
|
||||||
mgmt_adv_monitor_added(sk, hdev, m->handle);
|
mgmt_adv_monitor_added(sk, hdev, m->handle);
|
||||||
|
m->state = ADV_MONITOR_STATE_REGISTERED;
|
||||||
|
hdev->adv_monitors_cnt++;
|
||||||
|
|
||||||
|
hci_dev_unlock(hdev);
|
||||||
|
return mgmt_cmd_complete(sk, hdev->id, op, MGMT_STATUS_SUCCESS,
|
||||||
|
&rp, sizeof(rp));
|
||||||
|
}
|
||||||
|
|
||||||
hci_dev_unlock(hdev);
|
hci_dev_unlock(hdev);
|
||||||
|
|
||||||
rp.monitor_handle = cpu_to_le16(m->handle);
|
cmd->user_data = m;
|
||||||
|
return 0;
|
||||||
return mgmt_cmd_complete(sk, hdev->id, op,
|
|
||||||
MGMT_STATUS_SUCCESS, &rp, sizeof(rp));
|
|
||||||
|
|
||||||
unlock:
|
unlock:
|
||||||
hci_dev_unlock(hdev);
|
hci_dev_unlock(hdev);
|
||||||
|
|
||||||
failed:
|
|
||||||
hci_free_adv_monitor(m);
|
hci_free_adv_monitor(m);
|
||||||
return mgmt_cmd_status(sk, hdev->id, op, status);
|
return mgmt_cmd_status(sk, hdev->id, op, status);
|
||||||
}
|
}
|
||||||
@@ -4298,13 +4359,9 @@ static u8 parse_adv_monitor_pattern(struct adv_monitor *m, u8 pattern_count,
|
|||||||
{
|
{
|
||||||
u8 offset = 0, length = 0;
|
u8 offset = 0, length = 0;
|
||||||
struct adv_pattern *p = NULL;
|
struct adv_pattern *p = NULL;
|
||||||
unsigned int mp_cnt = 0;
|
|
||||||
int i;
|
int i;
|
||||||
|
|
||||||
for (i = 0; i < pattern_count; 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;
|
offset = patterns[i].offset;
|
||||||
length = patterns[i].length;
|
length = patterns[i].length;
|
||||||
if (offset >= HCI_MAX_AD_LENGTH ||
|
if (offset >= HCI_MAX_AD_LENGTH ||
|
||||||
@@ -4325,9 +4382,6 @@ static u8 parse_adv_monitor_pattern(struct adv_monitor *m, u8 pattern_count,
|
|||||||
list_add(&p->list, &m->patterns);
|
list_add(&p->list, &m->patterns);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mp_cnt != pattern_count)
|
|
||||||
return MGMT_STATUS_INVALID_PARAMS;
|
|
||||||
|
|
||||||
return MGMT_STATUS_SUCCESS;
|
return MGMT_STATUS_SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -4364,7 +4418,7 @@ static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
|
|||||||
status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);
|
status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);
|
||||||
|
|
||||||
done:
|
done:
|
||||||
return __add_adv_patterns_monitor(sk, hdev, m, status,
|
return __add_adv_patterns_monitor(sk, hdev, m, status, data, len,
|
||||||
MGMT_OP_ADD_ADV_PATTERNS_MONITOR);
|
MGMT_OP_ADD_ADV_PATTERNS_MONITOR);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -4401,7 +4455,7 @@ static int add_adv_patterns_monitor_rssi(struct sock *sk, struct hci_dev *hdev,
|
|||||||
status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);
|
status = parse_adv_monitor_pattern(m, cp->pattern_count, cp->patterns);
|
||||||
|
|
||||||
done:
|
done:
|
||||||
return __add_adv_patterns_monitor(sk, hdev, m, status,
|
return __add_adv_patterns_monitor(sk, hdev, m, status, data, len,
|
||||||
MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI);
|
MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -5,9 +5,16 @@
|
|||||||
|
|
||||||
#include <net/bluetooth/bluetooth.h>
|
#include <net/bluetooth/bluetooth.h>
|
||||||
#include <net/bluetooth/hci_core.h>
|
#include <net/bluetooth/hci_core.h>
|
||||||
|
#include <net/bluetooth/mgmt.h>
|
||||||
|
|
||||||
|
#include "hci_request.h"
|
||||||
|
#include "mgmt_util.h"
|
||||||
#include "msft.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
|
#define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
|
||||||
struct msft_cp_read_supported_features {
|
struct msft_cp_read_supported_features {
|
||||||
__u8 sub_opcode;
|
__u8 sub_opcode;
|
||||||
@@ -21,12 +28,55 @@ struct msft_rp_read_supported_features {
|
|||||||
__u8 evt_prefix[];
|
__u8 evt_prefix[];
|
||||||
} __packed;
|
} __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 {
|
struct msft_data {
|
||||||
__u64 features;
|
__u64 features;
|
||||||
__u8 evt_prefix_len;
|
__u8 evt_prefix_len;
|
||||||
__u8 *evt_prefix;
|
__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,
|
static bool read_supported_features(struct hci_dev *hdev,
|
||||||
struct msft_data *msft)
|
struct msft_data *msft)
|
||||||
{
|
{
|
||||||
@@ -90,12 +140,14 @@ void msft_do_open(struct hci_dev *hdev)
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
INIT_LIST_HEAD(&msft->handle_map);
|
||||||
hdev->msft_data = msft;
|
hdev->msft_data = msft;
|
||||||
}
|
}
|
||||||
|
|
||||||
void msft_do_close(struct hci_dev *hdev)
|
void msft_do_close(struct hci_dev *hdev)
|
||||||
{
|
{
|
||||||
struct msft_data *msft = hdev->msft_data;
|
struct msft_data *msft = hdev->msft_data;
|
||||||
|
struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
|
||||||
|
|
||||||
if (!msft)
|
if (!msft)
|
||||||
return;
|
return;
|
||||||
@@ -104,6 +156,11 @@ void msft_do_close(struct hci_dev *hdev)
|
|||||||
|
|
||||||
hdev->msft_data = NULL;
|
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->evt_prefix);
|
||||||
kfree(msft);
|
kfree(msft);
|
||||||
}
|
}
|
||||||
@@ -145,5 +202,147 @@ __u64 msft_get_features(struct hci_dev *hdev)
|
|||||||
{
|
{
|
||||||
struct msft_data *msft = hdev->msft_data;
|
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)
|
#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_open(struct hci_dev *hdev);
|
||||||
void msft_do_close(struct hci_dev *hdev);
|
void msft_do_close(struct hci_dev *hdev);
|
||||||
void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb);
|
void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb);
|
||||||
__u64 msft_get_features(struct hci_dev *hdev);
|
__u64 msft_get_features(struct hci_dev *hdev);
|
||||||
|
int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor);
|
||||||
|
|
||||||
#else
|
#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_open(struct hci_dev *hdev) {}
|
||||||
static inline void msft_do_close(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 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 __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
|
#endif
|
||||||
|
|||||||
Reference in New Issue
Block a user