diff mbox series

[v3,2/5] Bluetooth: advmon offload MSFT add monitor

Message ID 20201216123317.v3.2.I4969a334028027df34dab9740cd16bbce278633c@changeid (mailing list archive)
State Superseded
Delegated to: Netdev Maintainers
Headers show
Series MSFT offloading support for advertisement monitor | expand

Checks

Context Check Description
netdev/cover_letter success Link
netdev/fixes_present success Link
netdev/patch_count success Link
netdev/tree_selection success Guessed tree name to be net-next
netdev/subject_prefix warning Target tree name not specified in the subject
netdev/source_inline success Was 0 now: 0
netdev/verify_signedoff success Link
netdev/module_param success Was 0 now: 0
netdev/build_32bit success Errors and warnings before: 32 this patch: 32
netdev/kdoc success Errors and warnings before: 0 this patch: 0
netdev/verify_fixes success Link
netdev/checkpatch warning CHECK: Alignment should match open parenthesis
netdev/build_allmodconfig_warn success Errors and warnings before: 32 this patch: 32
netdev/header_inline success Link
netdev/stable success Stable not CCed

Commit Message

Archie Pusaka Dec. 16, 2020, 4:33 a.m. UTC 
From: Archie Pusaka <apusaka@chromium.org>

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>

---

(no changes since v2)

Changes in v2:
* Also implement the new MGMT opcode and merge the functionality with
  the old one.

 include/net/bluetooth/hci_core.h |  17 ++-
 net/bluetooth/hci_core.c         |  54 +++++++--
 net/bluetooth/mgmt.c             | 144 ++++++++++++++--------
 net/bluetooth/msft.c             | 201 ++++++++++++++++++++++++++++++-
 net/bluetooth/msft.h             |  12 ++
 5 files changed, 367 insertions(+), 61 deletions(-)

Comments

Marcel Holtmann Dec. 21, 2020, 9:09 a.m. UTC | #1 
Hi Archie,

> 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>
> 
> ---
> 
> (no changes since v2)
> 
> Changes in v2:
> * Also implement the new MGMT opcode and merge the functionality with
>  the old one.
> 
> include/net/bluetooth/hci_core.h |  17 ++-
> net/bluetooth/hci_core.c         |  54 +++++++--
> net/bluetooth/mgmt.c             | 144 ++++++++++++++--------
> net/bluetooth/msft.c             | 201 ++++++++++++++++++++++++++++++-
> net/bluetooth/msft.h             |  12 ++
> 5 files changed, 367 insertions(+), 61 deletions(-)
> 
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index 8b7cf3620938..879d1e38ce96 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -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_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);
> diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
> index 9d2c9a1c552f..fa13e35f775d 100644
> --- a/net/bluetooth/hci_core.c
> +++ b/net/bluetooth/hci_core.c
> @@ -3070,27 +3070,55 @@ 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;
> 
> -	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_DBG("%s add monitor status %d", hdev->name, *err);

lets use bt_dev_dbg.

> +		/* 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_DBG("%s add monitor msft status %d", hdev->name, *err);
> +		break;
> +	}
> +
> +	return (*err == 0);
> }
> 
> static int free_adv_monitor(int id, void *ptr, void *data)
> @@ -3134,6 +3162,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)
> {
> diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
> index cd574054aa39..9bd9f6540664 100644
> --- a/net/bluetooth/mgmt.c
> +++ b/net/bluetooth/mgmt.c
> @@ -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,6 +4226,45 @@ 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,
> 				      void *data, u16 len, u16 op)
> {
> @@ -4236,14 +4275,25 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
> 	struct mgmt_adv_pattern *patterns = NULL;
> 	struct adv_monitor *m = NULL;
> 	struct adv_pattern *p = NULL;
> -	unsigned int mp_cnt = 0, prev_adv_monitors_cnt;
> +	struct mgmt_pending_cmd *cmd;
> 	__u8 cp_ofst = 0, cp_len = 0;
> -	int err, i;
> +	int err, status, i;
> +	bool pending;
> 	u8 pattern_count;
> 	u16 expected_len;
> 
> 	BT_DBG("request for %s", hdev->name);
> 
> +	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;
> +	}
> +
> 	if (op == MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI) {
> 		cp_rssi = data;
> 		pattern_count = cp_rssi->pattern_count;
> @@ -4258,20 +4308,20 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
> 		expected_len = sizeof(*cp) + pattern_count * sizeof(*patterns);
> 	}
> 
> -	if (len != expected_len || pattern_count == 0) {
> -		err = mgmt_cmd_status(sk, hdev->id, op,
> -				      MGMT_STATUS_INVALID_PARAMS);
> -		goto failed;
> +	if (len != expected_len || pattern_count == 0 ||
> +	    pattern_count > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
> +		status = MGMT_STATUS_INVALID_PARAMS;
> +		goto unlock;
> 	}
> 
> 	m = kmalloc(sizeof(*m), GFP_KERNEL);
> 	if (!m) {
> -		err = -ENOMEM;
> -		goto failed;
> +		status = MGMT_STATUS_NO_RESOURCES;
> +		goto unlock;
> 	}
> 
> 	INIT_LIST_HEAD(&m->patterns);
> -	m->active = false;
> +	m->state = ADV_MONITOR_STATE_NOT_REGISTERED;
> 
> 	if (rssi) {
> 		m->rssi.low_threshold = rssi->low_threshold;
> @@ -4295,26 +4345,19 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
> 	}
> 
> 	for (i = 0; i < pattern_count; i++) {
> -		if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
> -			err = mgmt_cmd_status(sk, hdev->id, op,
> -					      MGMT_STATUS_INVALID_PARAMS);
> -			goto failed;
> -		}
> -
> 		cp_ofst = patterns[i].offset;
> 		cp_len = 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, op,
> -					      MGMT_STATUS_INVALID_PARAMS);
> -			goto failed;
> +			status = MGMT_STATUS_INVALID_PARAMS;
> +			goto unlock;
> 		}
> 
> 		p = kmalloc(sizeof(*p), GFP_KERNEL);
> 		if (!p) {
> -			err = -ENOMEM;
> -			goto failed;
> +			status = MGMT_STATUS_NO_RESOURCES;
> +			goto unlock;
> 		}
> 
> 		p->ad_type = patterns[i].ad_type;
> @@ -4326,41 +4369,46 @@ static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
> 		list_add(&p->list, &m->patterns);
> 	}
> 
> -	if (mp_cnt != pattern_count) {
> -		err = mgmt_cmd_status(sk, hdev->id, op,
> -				      MGMT_STATUS_INVALID_PARAMS);
> -		goto failed;
> +	cmd = mgmt_pending_add(sk, op, hdev, data, len);
> +	if (!cmd) {
> +		status = MGMT_STATUS_NO_RESOURCES;
> +		goto unlock;
> 	}
> 
> -	hci_dev_lock(hdev);
> -
> -	prev_adv_monitors_cnt = hdev->adv_monitors_cnt;
> -
> -	err = hci_add_adv_monitor(hdev, m);
> +	pending = hci_add_adv_monitor(hdev, m, &err);
> 	if (err) {
> -		if (err == -ENOSPC) {
> -			mgmt_cmd_status(sk, hdev->id, op,
> -					MGMT_STATUS_NO_RESOURCES);
> -		}
> +		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);
> +		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 err;
> +	return mgmt_cmd_status(sk, hdev->id, op, status);
> }
> 
> static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
> diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c
> index 4b39534a14a1..e4b8fe71b9c3 100644
> --- a/net/bluetooth/msft.c
> +++ b/net/bluetooth/msft.c
> @@ -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);
> }
> @@ -145,5 +202,147 @@ __u64 msft_get_features(struct hci_dev *hdev)
> {
> 	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;
> }
> diff --git a/net/bluetooth/msft.h b/net/bluetooth/msft.h
> index e9c478e890b8..0ac9b15322b1 100644
> --- a/net/bluetooth/msft.h
> +++ b/net/bluetooth/msft.h
> @@ -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;
> +}

Regards

Marcel
diff mbox series

Patch

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 8b7cf3620938..879d1e38ce96 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -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_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);
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 9d2c9a1c552f..fa13e35f775d 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -3070,27 +3070,55 @@  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;
 
-	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_DBG("%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_DBG("%s add monitor msft status %d", hdev->name, *err);
+		break;
+	}
+
+	return (*err == 0);
 }
 
 static int free_adv_monitor(int id, void *ptr, void *data)
@@ -3134,6 +3162,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)
 {
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index cd574054aa39..9bd9f6540664 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -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,6 +4226,45 @@  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,
 				      void *data, u16 len, u16 op)
 {
@@ -4236,14 +4275,25 @@  static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
 	struct mgmt_adv_pattern *patterns = NULL;
 	struct adv_monitor *m = NULL;
 	struct adv_pattern *p = NULL;
-	unsigned int mp_cnt = 0, prev_adv_monitors_cnt;
+	struct mgmt_pending_cmd *cmd;
 	__u8 cp_ofst = 0, cp_len = 0;
-	int err, i;
+	int err, status, i;
+	bool pending;
 	u8 pattern_count;
 	u16 expected_len;
 
 	BT_DBG("request for %s", hdev->name);
 
+	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;
+	}
+
 	if (op == MGMT_OP_ADD_ADV_PATTERNS_MONITOR_RSSI) {
 		cp_rssi = data;
 		pattern_count = cp_rssi->pattern_count;
@@ -4258,20 +4308,20 @@  static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
 		expected_len = sizeof(*cp) + pattern_count * sizeof(*patterns);
 	}
 
-	if (len != expected_len || pattern_count == 0) {
-		err = mgmt_cmd_status(sk, hdev->id, op,
-				      MGMT_STATUS_INVALID_PARAMS);
-		goto failed;
+	if (len != expected_len || pattern_count == 0 ||
+	    pattern_count > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
+		status = MGMT_STATUS_INVALID_PARAMS;
+		goto unlock;
 	}
 
 	m = kmalloc(sizeof(*m), GFP_KERNEL);
 	if (!m) {
-		err = -ENOMEM;
-		goto failed;
+		status = MGMT_STATUS_NO_RESOURCES;
+		goto unlock;
 	}
 
 	INIT_LIST_HEAD(&m->patterns);
-	m->active = false;
+	m->state = ADV_MONITOR_STATE_NOT_REGISTERED;
 
 	if (rssi) {
 		m->rssi.low_threshold = rssi->low_threshold;
@@ -4295,26 +4345,19 @@  static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
 	}
 
 	for (i = 0; i < pattern_count; i++) {
-		if (++mp_cnt > HCI_MAX_ADV_MONITOR_NUM_PATTERNS) {
-			err = mgmt_cmd_status(sk, hdev->id, op,
-					      MGMT_STATUS_INVALID_PARAMS);
-			goto failed;
-		}
-
 		cp_ofst = patterns[i].offset;
 		cp_len = 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, op,
-					      MGMT_STATUS_INVALID_PARAMS);
-			goto failed;
+			status = MGMT_STATUS_INVALID_PARAMS;
+			goto unlock;
 		}
 
 		p = kmalloc(sizeof(*p), GFP_KERNEL);
 		if (!p) {
-			err = -ENOMEM;
-			goto failed;
+			status = MGMT_STATUS_NO_RESOURCES;
+			goto unlock;
 		}
 
 		p->ad_type = patterns[i].ad_type;
@@ -4326,41 +4369,46 @@  static int __add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
 		list_add(&p->list, &m->patterns);
 	}
 
-	if (mp_cnt != pattern_count) {
-		err = mgmt_cmd_status(sk, hdev->id, op,
-				      MGMT_STATUS_INVALID_PARAMS);
-		goto failed;
+	cmd = mgmt_pending_add(sk, op, hdev, data, len);
+	if (!cmd) {
+		status = MGMT_STATUS_NO_RESOURCES;
+		goto unlock;
 	}
 
-	hci_dev_lock(hdev);
-
-	prev_adv_monitors_cnt = hdev->adv_monitors_cnt;
-
-	err = hci_add_adv_monitor(hdev, m);
+	pending = hci_add_adv_monitor(hdev, m, &err);
 	if (err) {
-		if (err == -ENOSPC) {
-			mgmt_cmd_status(sk, hdev->id, op,
-					MGMT_STATUS_NO_RESOURCES);
-		}
+		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);
+		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 err;
+	return mgmt_cmd_status(sk, hdev->id, op, status);
 }
 
 static int add_adv_patterns_monitor(struct sock *sk, struct hci_dev *hdev,
diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c
index 4b39534a14a1..e4b8fe71b9c3 100644
--- a/net/bluetooth/msft.c
+++ b/net/bluetooth/msft.c
@@ -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);
 }
@@ -145,5 +202,147 @@  __u64 msft_get_features(struct hci_dev *hdev)
 {
 	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;
 }
diff --git a/net/bluetooth/msft.h b/net/bluetooth/msft.h
index e9c478e890b8..0ac9b15322b1 100644
--- a/net/bluetooth/msft.h
+++ b/net/bluetooth/msft.h
@@ -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