Commit a2a4dedf authored by Archie Pusaka's avatar Archie Pusaka Committed by Marcel Holtmann
Browse files

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: default avatarArchie Pusaka <apusaka@chromium.org>
Reviewed-by: default avatarManish Mandlik <mmandlik@chromium.org>
Reviewed-by: default avatarMiao-chen Chou <mcchou@chromium.org>
Reviewed-by: default avatarYun-Hao Chung <howardchung@google.com>
Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
parent b4a221ea

include/net/bluetooth/hci_core.h

+14 −3
Original line number Diff line number Diff line
@@ -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_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);

net/bluetooth/hci_core.c

+46 −9
Original line number Diff line number Diff line
@@ -3070,27 +3070,56 @@ 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;



	if (!hdev_is_powered(hdev))

		return false;



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

	return (*err == 0);

}



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

}



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)

{

net/bluetooth/mgmt.c

+84 −30
Original line number Diff line number Diff line
@@ -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,44 +4226,105 @@ 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,

				      struct adv_monitor *m, u8 status, u16 op)

				      struct adv_monitor *m, u8 status,

				      void *data, u16 len, u16 op)

{

	struct mgmt_rp_add_adv_patterns_monitor rp;

	unsigned int prev_adv_monitors_cnt;

	struct mgmt_pending_cmd *cmd;

	int err;

	bool pending;



	hci_dev_lock(hdev);



	if (status)

		goto failed;

		goto unlock;



	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;

	}



	prev_adv_monitors_cnt = hdev->adv_monitors_cnt;

	cmd = mgmt_pending_add(sk, op, hdev, data, len);

	if (!cmd) {

		status = MGMT_STATUS_NO_RESOURCES;

		goto unlock;

	}



	err = hci_add_adv_monitor(hdev, m);

	pending = hci_add_adv_monitor(hdev, m, &err);

	if (err) {

		if (err == -ENOSPC)

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

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

	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 ||
@@ -4325,9 +4382,6 @@ static u8 parse_adv_monitor_pattern(struct adv_monitor *m, u8 pattern_count, 
		list_add(&p->list, &m->patterns);

	}



	if (mp_cnt != pattern_count)

		return MGMT_STATUS_INVALID_PARAMS;



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



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

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



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

}

net/bluetooth/msft.c

+200 −1
Original line number Diff line number Diff line
@@ -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);

}
@@ -147,3 +204,145 @@ __u64 msft_get_features(struct hci_dev *hdev) 


	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;

}

net/bluetooth/msft.h

+12 −0
Original line number Diff line number Diff line
@@ -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