diff mbox series

Bluetooth: msft: Extended monitor tracking by address filter

Message ID 20230316090729.14572-1-hildawu@realtek.com (mailing list archive)
State Superseded
Headers show
Series Bluetooth: msft: Extended monitor tracking by address filter | expand

Checks

Context Check Description
tedd_an/pre-ci_am success Success
tedd_an/CheckPatch success CheckPatch PASS
tedd_an/GitLint success Gitlint PASS
tedd_an/SubjectPrefix success Gitlint PASS
tedd_an/BuildKernel success BuildKernel PASS
tedd_an/CheckAllWarning warning CheckSparse WARNING net/bluetooth/msft.c: In function ‘msft_add_monitor_sync’:net/bluetooth/msft.c:521:43: warning: variable ‘rp’ set but not used [-Wunused-but-set-variable] 521 | struct msft_rp_le_monitor_advertisement *rp; | ^~
tedd_an/CheckSparse success CheckSparse PASS
tedd_an/CheckSmatch success CheckSparse PASS
tedd_an/BuildKernel32 success BuildKernel32 PASS
tedd_an/TestRunnerSetup success TestRunnerSetup PASS
tedd_an/TestRunner_l2cap-tester success TestRunner PASS
tedd_an/TestRunner_iso-tester success TestRunner PASS
tedd_an/TestRunner_bnep-tester success TestRunner PASS
tedd_an/TestRunner_mgmt-tester success TestRunner PASS
tedd_an/TestRunner_rfcomm-tester success TestRunner PASS
tedd_an/TestRunner_sco-tester success TestRunner PASS
tedd_an/TestRunner_ioctl-tester success TestRunner PASS
tedd_an/TestRunner_mesh-tester success TestRunner PASS
tedd_an/TestRunner_smp-tester success TestRunner PASS
tedd_an/TestRunner_userchan-tester success TestRunner PASS
tedd_an/IncrementalBuild success Incremental Build PASS

Commit Message

Hilda Wu March 16, 2023, 9:07 a.m. UTC
From: Hilda Wu <hildawu@realtek.com>

Since limited tracking device per condition, this feature is to support
tracking multiple devices concurrently.
When a pattern monitor detects the device, this feature issues an address
monitor for tracking that device. Let pattern monitor can keep monitor
new devices.
This feature adds an address filter when receiving a LE monitor device
event which monitor handle is for a pattern, and the controller started
monitoring the device. And this feature also has cancelled the monitor
advertisement from address filters when receiving a LE monitor device
event when the controller stopped monitoring the device specified by an
address and monitor handle.

Signed-off-by: Alex Lu <alex_lu@realsil.com.cn>
Signed-off-by: Hilda Wu <hildawu@realtek.com>
---
 net/bluetooth/msft.c | 538 +++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 524 insertions(+), 14 deletions(-)

Comments

bluez.test.bot@gmail.com March 16, 2023, 9:34 a.m. UTC | #1
This is automated email and please do not reply to this email!

Dear submitter,

Thank you for submitting the patches to the linux bluetooth mailing list.
This is a CI test results with your patch series:
PW Link:https://patchwork.kernel.org/project/bluetooth/list/?series=730681

---Test result---

Test Summary:
CheckPatch                    PASS      1.15 seconds
GitLint                       PASS      0.35 seconds
SubjectPrefix                 PASS      0.13 seconds
BuildKernel                   PASS      33.18 seconds
CheckAllWarning               WARNING   36.29 seconds
CheckSparse                   PASS      40.72 seconds
CheckSmatch                   PASS      109.56 seconds
BuildKernel32                 PASS      31.46 seconds
TestRunnerSetup               PASS      451.70 seconds
TestRunner_l2cap-tester       PASS      17.58 seconds
TestRunner_iso-tester         PASS      17.80 seconds
TestRunner_bnep-tester        PASS      5.75 seconds
TestRunner_mgmt-tester        PASS      113.51 seconds
TestRunner_rfcomm-tester      PASS      9.31 seconds
TestRunner_sco-tester         PASS      8.55 seconds
TestRunner_ioctl-tester       PASS      10.00 seconds
TestRunner_mesh-tester        PASS      7.28 seconds
TestRunner_smp-tester         PASS      8.35 seconds
TestRunner_userchan-tester    PASS      6.11 seconds
IncrementalBuild              PASS      30.34 seconds

Details
##############################
Test: CheckAllWarning - WARNING
Desc: Run linux kernel with all warning enabled
Output:
net/bluetooth/msft.c: In function ‘msft_add_monitor_sync’:net/bluetooth/msft.c:521:43: warning: variable ‘rp’ set but not used [-Wunused-but-set-variable]  521 |  struct msft_rp_le_monitor_advertisement *rp;      |                                           ^~


---
Regards,
Linux Bluetooth
kernel test robot March 16, 2023, 10:23 a.m. UTC | #2
Hi,

Thank you for the patch! Perhaps something to improve:

[auto build test WARNING on bluetooth/master]
[also build test WARNING on bluetooth-next/master]
[If your patch is applied to the wrong git tree, kindly drop us a note.
And when submitting patch, we suggest to use '--base' as documented in
https://git-scm.com/docs/git-format-patch#_base_tree_information]

url:    https://github.com/intel-lab-lkp/linux/commits/hildawu-realtek-com/Bluetooth-msft-Extended-monitor-tracking-by-address-filter/20230316-170950
base:   https://git.kernel.org/pub/scm/linux/kernel/git/bluetooth/bluetooth.git master
patch link:    https://lore.kernel.org/r/20230316090729.14572-1-hildawu%40realtek.com
patch subject: [PATCH] Bluetooth: msft: Extended monitor tracking by address filter
config: riscv-allmodconfig (https://download.01.org/0day-ci/archive/20230316/202303161807.AcfCGsAP-lkp@intel.com/config)
compiler: riscv64-linux-gcc (GCC) 12.1.0
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # https://github.com/intel-lab-lkp/linux/commit/cee47af4605a9e5cba61be1ab1d92e8748d92e1e
        git remote add linux-review https://github.com/intel-lab-lkp/linux
        git fetch --no-tags linux-review hildawu-realtek-com/Bluetooth-msft-Extended-monitor-tracking-by-address-filter/20230316-170950
        git checkout cee47af4605a9e5cba61be1ab1d92e8748d92e1e
        # save the config file
        mkdir build_dir && cp config build_dir/.config
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-12.1.0 make.cross W=1 O=build_dir ARCH=riscv olddefconfig
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=gcc-12.1.0 make.cross W=1 O=build_dir ARCH=riscv SHELL=/bin/bash net/

If you fix the issue, kindly add following tag where applicable
| Reported-by: kernel test robot <lkp@intel.com>
| Link: https://lore.kernel.org/oe-kbuild-all/202303161807.AcfCGsAP-lkp@intel.com/

All warnings (new ones prefixed by >>):

   net/bluetooth/msft.c: In function 'msft_add_monitor_sync':
>> net/bluetooth/msft.c:521:50: warning: variable 'rp' set but not used [-Wunused-but-set-variable]
     521 |         struct msft_rp_le_monitor_advertisement *rp;
         |                                                  ^~


vim +/rp +521 net/bluetooth/msft.c

   507	
   508	static int msft_add_monitor_sync(struct hci_dev *hdev,
   509					 struct adv_monitor *monitor)
   510	{
   511		struct msft_cp_le_monitor_advertisement *cp;
   512		struct msft_le_monitor_advertisement_pattern_data *pattern_data;
   513		struct msft_le_monitor_advertisement_pattern *pattern;
   514		struct adv_pattern *entry;
   515		size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
   516		ptrdiff_t offset = 0;
   517		u8 pattern_count = 0;
   518		struct sk_buff *skb;
   519		int err;
   520		struct msft_monitor_advertisement_handle_data *handle_data;
 > 521		struct msft_rp_le_monitor_advertisement *rp;
   522	
   523		if (!msft_monitor_pattern_valid(monitor))
   524			return -EINVAL;
   525	
   526		list_for_each_entry(entry, &monitor->patterns, list) {
   527			pattern_count++;
   528			total_size += sizeof(*pattern) + entry->length;
   529		}
   530	
   531		cp = kmalloc(total_size, GFP_KERNEL);
   532		if (!cp)
   533			return -ENOMEM;
   534	
   535		cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
   536		cp->rssi_high = monitor->rssi.high_threshold;
   537		cp->rssi_low = monitor->rssi.low_threshold;
   538		cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
   539		cp->rssi_sampling_period = monitor->rssi.sampling_period;
   540	
   541		cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
   542	
   543		pattern_data = (void *)cp->data;
   544		pattern_data->count = pattern_count;
   545	
   546		list_for_each_entry(entry, &monitor->patterns, list) {
   547			pattern = (void *)(pattern_data->data + offset);
   548			/* the length also includes data_type and offset */
   549			pattern->length = entry->length + 2;
   550			pattern->data_type = entry->ad_type;
   551			pattern->start_byte = entry->offset;
   552			memcpy(pattern->pattern, entry->value, entry->length);
   553			offset += sizeof(*pattern) + entry->length;
   554		}
   555	
   556		skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
   557				     HCI_CMD_TIMEOUT);
   558	
   559		if (IS_ERR_OR_NULL(skb)) {
   560			kfree(cp);
   561			return PTR_ERR(skb);
   562		}
   563	
   564		err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
   565						       monitor, skb);
   566		if (!err) {
   567			rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
   568			handle_data = msft_find_handle_data(hdev, monitor->handle,
   569							    true);
   570			if (handle_data) {
   571				handle_data->rssi_high   = cp->rssi_high;
   572				handle_data->rssi_low    = cp->rssi_low;
   573				handle_data->rssi_low_interval    =
   574							cp->rssi_low_interval;
   575				handle_data->rssi_sampling_period =
   576							cp->rssi_sampling_period;
   577			}
   578		}
   579		kfree(cp);
   580	
   581		return err;
   582	}
   583
kernel test robot March 16, 2023, 4:30 p.m. UTC | #3
Hi,

Thank you for the patch! Perhaps something to improve:

[auto build test WARNING on bluetooth/master]
[also build test WARNING on bluetooth-next/master net-next/main net/main linus/master v6.3-rc2 next-20230316]
[If your patch is applied to the wrong git tree, kindly drop us a note.
And when submitting patch, we suggest to use '--base' as documented in
https://git-scm.com/docs/git-format-patch#_base_tree_information]

url:    https://github.com/intel-lab-lkp/linux/commits/hildawu-realtek-com/Bluetooth-msft-Extended-monitor-tracking-by-address-filter/20230316-170950
base:   https://git.kernel.org/pub/scm/linux/kernel/git/bluetooth/bluetooth.git master
patch link:    https://lore.kernel.org/r/20230316090729.14572-1-hildawu%40realtek.com
patch subject: [PATCH] Bluetooth: msft: Extended monitor tracking by address filter
config: i386-randconfig-a016-20230313 (https://download.01.org/0day-ci/archive/20230317/202303170056.UsZ6RDV4-lkp@intel.com/config)
compiler: clang version 14.0.6 (https://github.com/llvm/llvm-project f28c006a5895fc0e329fe15fead81e37457cb1d1)
reproduce (this is a W=1 build):
        wget https://raw.githubusercontent.com/intel/lkp-tests/master/sbin/make.cross -O ~/bin/make.cross
        chmod +x ~/bin/make.cross
        # https://github.com/intel-lab-lkp/linux/commit/cee47af4605a9e5cba61be1ab1d92e8748d92e1e
        git remote add linux-review https://github.com/intel-lab-lkp/linux
        git fetch --no-tags linux-review hildawu-realtek-com/Bluetooth-msft-Extended-monitor-tracking-by-address-filter/20230316-170950
        git checkout cee47af4605a9e5cba61be1ab1d92e8748d92e1e
        # save the config file
        mkdir build_dir && cp config build_dir/.config
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=clang make.cross W=1 O=build_dir ARCH=i386 olddefconfig
        COMPILER_INSTALL_PATH=$HOME/0day COMPILER=clang make.cross W=1 O=build_dir ARCH=i386 SHELL=/bin/bash net/bluetooth/

If you fix the issue, kindly add following tag where applicable
| Reported-by: kernel test robot <lkp@intel.com>
| Link: https://lore.kernel.org/oe-kbuild-all/202303170056.UsZ6RDV4-lkp@intel.com/

All warnings (new ones prefixed by >>):

>> net/bluetooth/msft.c:521:43: warning: variable 'rp' set but not used [-Wunused-but-set-variable]
           struct msft_rp_le_monitor_advertisement *rp;
                                                    ^
   1 warning generated.


vim +/rp +521 net/bluetooth/msft.c

   507	
   508	static int msft_add_monitor_sync(struct hci_dev *hdev,
   509					 struct adv_monitor *monitor)
   510	{
   511		struct msft_cp_le_monitor_advertisement *cp;
   512		struct msft_le_monitor_advertisement_pattern_data *pattern_data;
   513		struct msft_le_monitor_advertisement_pattern *pattern;
   514		struct adv_pattern *entry;
   515		size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
   516		ptrdiff_t offset = 0;
   517		u8 pattern_count = 0;
   518		struct sk_buff *skb;
   519		int err;
   520		struct msft_monitor_advertisement_handle_data *handle_data;
 > 521		struct msft_rp_le_monitor_advertisement *rp;
   522	
   523		if (!msft_monitor_pattern_valid(monitor))
   524			return -EINVAL;
   525	
   526		list_for_each_entry(entry, &monitor->patterns, list) {
   527			pattern_count++;
   528			total_size += sizeof(*pattern) + entry->length;
   529		}
   530	
   531		cp = kmalloc(total_size, GFP_KERNEL);
   532		if (!cp)
   533			return -ENOMEM;
   534	
   535		cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
   536		cp->rssi_high = monitor->rssi.high_threshold;
   537		cp->rssi_low = monitor->rssi.low_threshold;
   538		cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
   539		cp->rssi_sampling_period = monitor->rssi.sampling_period;
   540	
   541		cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
   542	
   543		pattern_data = (void *)cp->data;
   544		pattern_data->count = pattern_count;
   545	
   546		list_for_each_entry(entry, &monitor->patterns, list) {
   547			pattern = (void *)(pattern_data->data + offset);
   548			/* the length also includes data_type and offset */
   549			pattern->length = entry->length + 2;
   550			pattern->data_type = entry->ad_type;
   551			pattern->start_byte = entry->offset;
   552			memcpy(pattern->pattern, entry->value, entry->length);
   553			offset += sizeof(*pattern) + entry->length;
   554		}
   555	
   556		skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
   557				     HCI_CMD_TIMEOUT);
   558	
   559		if (IS_ERR_OR_NULL(skb)) {
   560			kfree(cp);
   561			return PTR_ERR(skb);
   562		}
   563	
   564		err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
   565						       monitor, skb);
   566		if (!err) {
   567			rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
   568			handle_data = msft_find_handle_data(hdev, monitor->handle,
   569							    true);
   570			if (handle_data) {
   571				handle_data->rssi_high   = cp->rssi_high;
   572				handle_data->rssi_low    = cp->rssi_low;
   573				handle_data->rssi_low_interval    =
   574							cp->rssi_low_interval;
   575				handle_data->rssi_sampling_period =
   576							cp->rssi_sampling_period;
   577			}
   578		}
   579		kfree(cp);
   580	
   581		return err;
   582	}
   583
Simon Horman March 18, 2023, 2:22 p.m. UTC | #4
On Thu, Mar 16, 2023 at 05:07:29PM +0800, hildawu@realtek.com wrote:
> From: Hilda Wu <hildawu@realtek.com>
> 
> Since limited tracking device per condition, this feature is to support
> tracking multiple devices concurrently.
> When a pattern monitor detects the device, this feature issues an address
> monitor for tracking that device. Let pattern monitor can keep monitor
> new devices.
> This feature adds an address filter when receiving a LE monitor device
> event which monitor handle is for a pattern, and the controller started
> monitoring the device. And this feature also has cancelled the monitor
> advertisement from address filters when receiving a LE monitor device
> event when the controller stopped monitoring the device specified by an
> address and monitor handle.
> 
> Signed-off-by: Alex Lu <alex_lu@realsil.com.cn>
> Signed-off-by: Hilda Wu <hildawu@realtek.com>
> ---
>  net/bluetooth/msft.c | 538 +++++++++++++++++++++++++++++++++++++++++--
>  1 file changed, 524 insertions(+), 14 deletions(-)
> 
> diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c

...

> @@ -254,6 +305,64 @@ static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
>  	return status;
>  }
>  
> +/* This function requires the caller holds hci_req_sync_lock */
> +static int msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle)

This function always returns 0.
And the caller ignores the return value.
So I think it's return type can be changed to void.

> +{
> +	struct msft_monitor_addr_filter_data *address_filter, *n;
> +	struct msft_data *msft = hdev->msft_data;
> +	struct msft_cp_le_cancel_monitor_advertisement cp;
> +	struct sk_buff *skb;
> +	struct list_head head;

Suggestion:

I assume that it is not standard practice for bluetooth code.
But, FWIIW, I do find it significantly easier to read code that
uses reverse xmas tree - longest line to shortest - for local
variable declarations.

In this case, that would be:

	struct msft_monitor_addr_filter_data *address_filter, *n;
	struct msft_cp_le_cancel_monitor_advertisement cp;
	struct msft_data *msft = hdev->msft_data;
	struct list_head head;
	struct sk_buff *skb;

...

> @@ -400,6 +516,9 @@ static int msft_add_monitor_sync(struct hci_dev *hdev,
>  	ptrdiff_t offset = 0;
>  	u8 pattern_count = 0;
>  	struct sk_buff *skb;
> +	int err;
> +	struct msft_monitor_advertisement_handle_data *handle_data;
> +	struct msft_rp_le_monitor_advertisement *rp;

As per the build bot, rp is set but not the value is not used.
I guess rp can be removed entirely.

Link: https://lore.kernel.org/netdev/202303161807.AcfCGsAP-lkp@intel.com/
Link: https://lore.kernel.org/netdev/202303170056.UsZ6RDV4-lkp@intel.com/

>  
>  	if (!msft_monitor_pattern_valid(monitor))
>  		return -EINVAL;
> @@ -436,16 +555,30 @@ static int msft_add_monitor_sync(struct hci_dev *hdev,
>  
>  	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
>  			     HCI_CMD_TIMEOUT);
> -	kfree(cp);
>  
>  	if (IS_ERR_OR_NULL(skb)) {
> -		if (!skb)
> -			return -EIO;
> +		kfree(cp);
>  		return PTR_ERR(skb);
>  	}
>  
> -	return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
> -						monitor, skb);
> +	err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
> +					       monitor, skb);
> +	if (!err) {
> +		rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
> +		handle_data = msft_find_handle_data(hdev, monitor->handle,
> +						    true);
> +		if (handle_data) {
> +			handle_data->rssi_high   = cp->rssi_high;
> +			handle_data->rssi_low    = cp->rssi_low;
> +			handle_data->rssi_low_interval    =
> +						cp->rssi_low_interval;
> +			handle_data->rssi_sampling_period =
> +						cp->rssi_sampling_period;
> +		}
> +	}
> +	kfree(cp);
> +
> +	return err;

I think it would be more idiomatic to write the above something like this.
(* completely untested! *)

	err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
					       monitor, skb);
	if (err)
		goto out_free;

	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
	if (!handle_data)
		goto out_free;

	handle_data->rssi_high = cp->rssi_high;
	handle_data->rssi_low = cp->rssi_low;
	handle_data->rssi_low_interval = cp->rssi_low_interval;
	handle_data->rssi_sampling_period = cp->rssi_sampling_period;

out_free:
	kfree(cp);
	return err;

>  }
>  
>  /* This function requires the caller holds hci_req_sync_lock */
> @@ -497,6 +630,41 @@ int msft_resume_sync(struct hci_dev *hdev)
>  	return 0;
>  }
>  
> +/* This function requires the caller holds hci_req_sync_lock */
> +static bool msft_address_monitor_assist_realtek(struct hci_dev *hdev)
> +{
> +	struct sk_buff *skb;
> +	struct {
> +		__u8   status;
> +		__u8   chip_id;
> +	} *rp;
> +
> +	skb = __hci_cmd_sync(hdev, 0xfc6f, 0, NULL, HCI_CMD_TIMEOUT);
> +	if (IS_ERR_OR_NULL(skb)) {
> +		bt_dev_err(hdev, "MSFT: Failed to send the cmd 0xfc6f");
> +		return false;

This seems like an error case that should propagate.

> +	}
> +
> +	rp = (void *)skb->data;
> +	if (skb->len < sizeof(*rp) || rp->status) {
> +		kfree_skb(skb);
> +		return false;

Ditto.

Also, probably this warrant's a cleanup path.

	if (cond) {
		err = -EINVAL;
		goto out_free;
	}
	...

out_free:
	kfree(cp);
	return err;


> +	}
> +
> +	/* RTL8822C chip id: 13
> +	 * RTL8852A chip id: 18
> +	 * RTL8852C chip id: 25
> +	 */
> +	if (rp->chip_id == 13 || rp->chip_id == 18 || rp->chip_id == 25) {
> +		kfree_skb(skb);
> +		return true;

This could also leverage a label such as 'out_free'.

> +	}
> +
> +	kfree_skb(skb);
> +
> +	return false;
> +}
> +
>  /* This function requires the caller holds hci_req_sync_lock */
>  void msft_do_open(struct hci_dev *hdev)
>  {
> @@ -518,6 +686,10 @@ void msft_do_open(struct hci_dev *hdev)
>  	msft->evt_prefix_len = 0;
>  	msft->features = 0;
>  
> +	if (hdev->manufacturer == 0x005d)

Perhaps 0x005d could be a #define to make it clearer what it means.

> +		msft->addr_monitor_assist =
> +			msft_address_monitor_assist_realtek(hdev);
> +
>  	if (!read_supported_features(hdev, msft)) {
>  		hdev->msft_data = NULL;
>  		kfree(msft);

...

> @@ -645,12 +881,237 @@ static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
>  	return data;
>  }
>  
> +static int msft_add_address_filter_sync(struct hci_dev *hdev, void *data)
> +{
> +	struct sk_buff *skb = data;
> +	struct msft_monitor_addr_filter_data *address_filter = NULL;
> +	struct sk_buff *nskb;
> +	struct msft_rp_le_monitor_advertisement *rp;
> +	bool remove = false;
> +	struct msft_data *msft = hdev->msft_data;
> +	int err;
> +
> +	if (!msft) {
> +		bt_dev_err(hdev, "MSFT: msft data is freed");
> +		err = -EINVAL;
> +		goto error;
> +	}
> +
> +	mutex_lock(&msft->filter_lock);
> +
> +	address_filter = msft_find_address_data(hdev,
> +						addr_filter_cb(skb)->addr_type,
> +						&addr_filter_cb(skb)->bdaddr,
> +						addr_filter_cb(skb)->pattern_handle);

nit: mutex_unlock() could go here, to avoid duplicating it below.

> +	if (!address_filter) {
> +		bt_dev_warn(hdev, "MSFT: No address (%pMR) filter to enable",
> +			    &addr_filter_cb(skb)->bdaddr);
> +		mutex_unlock(&msft->filter_lock);
> +		err = -ENODEV;
> +		goto error;
> +	}
> +
> +	mutex_unlock(&msft->filter_lock);

...

> +/* This function requires the caller holds msft->filter_lock */
> +static struct msft_monitor_addr_filter_data *msft_add_address_filter
> +		(struct hci_dev *hdev, u8 addr_type, bdaddr_t *bdaddr,
> +		 struct msft_monitor_advertisement_handle_data *handle_data)
> +{
> +	struct sk_buff *skb;
> +	struct msft_cp_le_monitor_advertisement *cp;
> +	struct msft_monitor_addr_filter_data *address_filter = NULL;
> +	size_t size;
> +	struct msft_data *msft = hdev->msft_data;
> +	int err;
> +
> +	size = sizeof(*cp) + sizeof(addr_type) + sizeof(*bdaddr);
> +	skb = alloc_skb(size, GFP_KERNEL);
> +	if (!skb) {
> +		bt_dev_err(hdev, "MSFT: alloc skb err in device evt");
> +		return NULL;
> +	}
> +
> +	cp = skb_put(skb, sizeof(*cp));
> +	cp->sub_opcode	    = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
> +	cp->rssi_high	    = handle_data->rssi_high;
> +	cp->rssi_low	    = handle_data->rssi_low;
> +	cp->rssi_low_interval    = handle_data->rssi_low_interval;
> +	cp->rssi_sampling_period = handle_data->rssi_sampling_period;
> +	cp->cond_type	    = MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR;
> +	skb_put_u8(skb, addr_type);
> +	skb_put_data(skb, bdaddr, sizeof(*bdaddr));
> +
> +	address_filter = kzalloc(sizeof(*address_filter), GFP_KERNEL);
> +	if (!address_filter) {
> +		kfree_skb(skb);
> +		return NULL;
> +	}
> +
> +	address_filter->active		     = false;
> +	address_filter->msft_handle	     = 0xff;
> +	address_filter->pattern_handle	     = handle_data->msft_handle;
> +	address_filter->mgmt_handle	     = handle_data->mgmt_handle;
> +	address_filter->rssi_high	     = cp->rssi_high;
> +	address_filter->rssi_low	     = cp->rssi_low;
> +	address_filter->rssi_low_interval    = cp->rssi_low_interval;
> +	address_filter->rssi_sampling_period = cp->rssi_sampling_period;
> +	address_filter->addr_type	     = addr_type;
> +	bacpy(&address_filter->bdaddr, bdaddr);
> +	list_add_tail(&address_filter->list, &msft->address_filters);
> +
> +	addr_filter_cb(skb)->pattern_handle = address_filter->pattern_handle;
> +	addr_filter_cb(skb)->addr_type = addr_type;
> +	bacpy(&addr_filter_cb(skb)->bdaddr, bdaddr);
> +
> +	err = hci_cmd_sync_queue(hdev, msft_add_address_filter_sync, skb, NULL);
> +	if (err < 0) {
> +		bt_dev_err(hdev, "MSFT: Add address %pMR filter err", bdaddr);
> +		list_del(&address_filter->list);
> +		kfree(address_filter);
> +		kfree_skb(skb);
> +		return NULL;
> +	}
> +
> +	bt_dev_info(hdev, "MSFT: Add device %pMR address filter",
> +		    &address_filter->bdaddr);
> +
> +	return address_filter;

I think it would be more idiomatic to handle duplicated cleanup on error
using a label, something like this:

err_skb:
	kfree(skb);
	return NULL;

> +}

...
diff mbox series

Patch

diff --git a/net/bluetooth/msft.c b/net/bluetooth/msft.c
index bf5cee48916c..9e1f294b92a2 100644
--- a/net/bluetooth/msft.c
+++ b/net/bluetooth/msft.c
@@ -91,17 +91,49 @@  struct msft_ev_le_monitor_device {
 struct msft_monitor_advertisement_handle_data {
 	__u8  msft_handle;
 	__u16 mgmt_handle;
+	__s8 rssi_high;
+	__s8 rssi_low;
+	__u8 rssi_low_interval;
+	__u8 rssi_sampling_period;
+	__u8 cond_type;
 	struct list_head list;
 };
 
+#define MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR	0x04
+struct msft_monitor_addr_filter_data {
+	__u8     msft_handle;
+	__u8     pattern_handle; /* address filters pertain to */
+	__u16    mgmt_handle;
+	bool     active;
+	__s8     rssi_high;
+	__s8     rssi_low;
+	__u8     rssi_low_interval;
+	__u8     rssi_sampling_period;
+	__u8     addr_type;
+	bdaddr_t bdaddr;
+	struct list_head list;
+};
+
+struct addr_filter_skb_cb {
+	u8       pattern_handle;
+	u8       addr_type;
+	bdaddr_t bdaddr;
+};
+
+#define addr_filter_cb(skb) ((struct addr_filter_skb_cb *)((skb)->cb))
+
 struct msft_data {
 	__u64 features;
 	__u8  evt_prefix_len;
 	__u8  *evt_prefix;
 	struct list_head handle_map;
+	struct list_head address_filters;
 	__u8 resuming;
 	__u8 suspending;
 	__u8 filter_enabled;
+	bool addr_monitor_assist;
+	/* To synchronize add/remove address filter and monitor device event.*/
+	struct mutex filter_lock;
 };
 
 bool msft_monitor_supported(struct hci_dev *hdev)
@@ -180,6 +212,24 @@  static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
 	return NULL;
 }
 
+/* This function requires the caller holds msft->filter_lock */
+static struct msft_monitor_addr_filter_data *msft_find_address_data
+			(struct hci_dev *hdev, u8 addr_type, bdaddr_t *addr,
+			 u8 pattern_handle)
+{
+	struct msft_monitor_addr_filter_data *entry;
+	struct msft_data *msft = hdev->msft_data;
+
+	list_for_each_entry(entry, &msft->address_filters, list) {
+		if (entry->pattern_handle == pattern_handle &&
+		    addr_type == entry->addr_type &&
+		    !bacmp(addr, &entry->bdaddr))
+			return entry;
+	}
+
+	return NULL;
+}
+
 /* This function requires the caller holds hdev->lock */
 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
 				   bdaddr_t *bdaddr, __u8 addr_type,
@@ -240,6 +290,7 @@  static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
 
 	handle_data->mgmt_handle = monitor->handle;
 	handle_data->msft_handle = rp->handle;
+	handle_data->cond_type   = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
 	INIT_LIST_HEAD(&handle_data->list);
 	list_add(&handle_data->list, &msft->handle_map);
 
@@ -254,6 +305,64 @@  static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
 	return status;
 }
 
+/* This function requires the caller holds hci_req_sync_lock */
+static int msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle)
+{
+	struct msft_monitor_addr_filter_data *address_filter, *n;
+	struct msft_data *msft = hdev->msft_data;
+	struct msft_cp_le_cancel_monitor_advertisement cp;
+	struct sk_buff *skb;
+	struct list_head head;
+
+	INIT_LIST_HEAD(&head);
+
+	/* Cancel all corresponding address monitors */
+	mutex_lock(&msft->filter_lock);
+
+	list_for_each_entry_safe(address_filter, n, &msft->address_filters,
+				 list) {
+		if (address_filter->pattern_handle != handle)
+			continue;
+
+		list_del(&address_filter->list);
+
+		/* If the address_filter was added but haven't been enabled,
+		 * just free it.
+		 */
+		if (!address_filter->active) {
+			kfree(address_filter);
+			continue;
+		}
+
+		list_add_tail(&address_filter->list, &head);
+	}
+
+	mutex_unlock(&msft->filter_lock);
+
+	list_for_each_entry_safe(address_filter, n, &head, list) {
+		list_del(&address_filter->list);
+
+		cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
+		cp.handle = address_filter->msft_handle;
+
+		skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
+				     HCI_CMD_TIMEOUT);
+		if (IS_ERR_OR_NULL(skb)) {
+			kfree(address_filter);
+			continue;
+		}
+
+		kfree_skb(skb);
+
+		bt_dev_info(hdev, "MSFT: Canceled device %pMR address filter",
+			    &address_filter->bdaddr);
+
+		kfree(address_filter);
+	}
+
+	return 0;
+}
+
 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 						   u16 opcode,
 						   struct adv_monitor *monitor,
@@ -263,6 +372,7 @@  static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 	struct msft_monitor_advertisement_handle_data *handle_data;
 	struct msft_data *msft = hdev->msft_data;
 	int status = 0;
+	u8 msft_handle;
 
 	rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
 	if (skb->len < sizeof(*rp)) {
@@ -293,11 +403,17 @@  static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 						NULL, 0, false);
 		}
 
+		msft_handle = handle_data->msft_handle;
+
 		list_del(&handle_data->list);
 		kfree(handle_data);
-	}
 
-	hci_dev_unlock(hdev);
+		hci_dev_unlock(hdev);
+
+		msft_remove_addr_filters_sync(hdev, msft_handle);
+	} else {
+		hci_dev_unlock(hdev);
+	}
 
 done:
 	return status;
@@ -400,6 +516,9 @@  static int msft_add_monitor_sync(struct hci_dev *hdev,
 	ptrdiff_t offset = 0;
 	u8 pattern_count = 0;
 	struct sk_buff *skb;
+	int err;
+	struct msft_monitor_advertisement_handle_data *handle_data;
+	struct msft_rp_le_monitor_advertisement *rp;
 
 	if (!msft_monitor_pattern_valid(monitor))
 		return -EINVAL;
@@ -436,16 +555,30 @@  static int msft_add_monitor_sync(struct hci_dev *hdev,
 
 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
 			     HCI_CMD_TIMEOUT);
-	kfree(cp);
 
 	if (IS_ERR_OR_NULL(skb)) {
-		if (!skb)
-			return -EIO;
+		kfree(cp);
 		return PTR_ERR(skb);
 	}
 
-	return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
-						monitor, skb);
+	err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
+					       monitor, skb);
+	if (!err) {
+		rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
+		handle_data = msft_find_handle_data(hdev, monitor->handle,
+						    true);
+		if (handle_data) {
+			handle_data->rssi_high   = cp->rssi_high;
+			handle_data->rssi_low    = cp->rssi_low;
+			handle_data->rssi_low_interval    =
+						cp->rssi_low_interval;
+			handle_data->rssi_sampling_period =
+						cp->rssi_sampling_period;
+		}
+	}
+	kfree(cp);
+
+	return err;
 }
 
 /* This function requires the caller holds hci_req_sync_lock */
@@ -497,6 +630,41 @@  int msft_resume_sync(struct hci_dev *hdev)
 	return 0;
 }
 
+/* This function requires the caller holds hci_req_sync_lock */
+static bool msft_address_monitor_assist_realtek(struct hci_dev *hdev)
+{
+	struct sk_buff *skb;
+	struct {
+		__u8   status;
+		__u8   chip_id;
+	} *rp;
+
+	skb = __hci_cmd_sync(hdev, 0xfc6f, 0, NULL, HCI_CMD_TIMEOUT);
+	if (IS_ERR_OR_NULL(skb)) {
+		bt_dev_err(hdev, "MSFT: Failed to send the cmd 0xfc6f");
+		return false;
+	}
+
+	rp = (void *)skb->data;
+	if (skb->len < sizeof(*rp) || rp->status) {
+		kfree_skb(skb);
+		return false;
+	}
+
+	/* RTL8822C chip id: 13
+	 * RTL8852A chip id: 18
+	 * RTL8852C chip id: 25
+	 */
+	if (rp->chip_id == 13 || rp->chip_id == 18 || rp->chip_id == 25) {
+		kfree_skb(skb);
+		return true;
+	}
+
+	kfree_skb(skb);
+
+	return false;
+}
+
 /* This function requires the caller holds hci_req_sync_lock */
 void msft_do_open(struct hci_dev *hdev)
 {
@@ -518,6 +686,10 @@  void msft_do_open(struct hci_dev *hdev)
 	msft->evt_prefix_len = 0;
 	msft->features = 0;
 
+	if (hdev->manufacturer == 0x005d)
+		msft->addr_monitor_assist =
+			msft_address_monitor_assist_realtek(hdev);
+
 	if (!read_supported_features(hdev, msft)) {
 		hdev->msft_data = NULL;
 		kfree(msft);
@@ -538,6 +710,7 @@  void msft_do_close(struct hci_dev *hdev)
 {
 	struct msft_data *msft = hdev->msft_data;
 	struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
+	struct msft_monitor_addr_filter_data *address_filter, *n;
 	struct adv_monitor *monitor;
 
 	if (!msft)
@@ -559,6 +732,14 @@  void msft_do_close(struct hci_dev *hdev)
 		kfree(handle_data);
 	}
 
+	mutex_lock(&msft->filter_lock);
+	list_for_each_entry_safe(address_filter, n, &msft->address_filters,
+				 list) {
+		list_del(&address_filter->list);
+		kfree(address_filter);
+	}
+	mutex_unlock(&msft->filter_lock);
+
 	hci_dev_lock(hdev);
 
 	/* Clear any devices that are being monitored and notify device lost */
@@ -568,6 +749,58 @@  void msft_do_close(struct hci_dev *hdev)
 	hci_dev_unlock(hdev);
 }
 
+static int msft_cancel_address_filter_sync(struct hci_dev *hdev, void *data)
+{
+	struct msft_monitor_addr_filter_data *address_filter = NULL;
+	struct msft_cp_le_cancel_monitor_advertisement cp;
+	struct msft_data *msft = hdev->msft_data;
+	struct sk_buff *nskb;
+	u8 handle = PTR_ERR(data);
+
+	if (!msft) {
+		bt_dev_err(hdev, "MSFT: msft data is freed");
+		return -EINVAL;
+	}
+
+	mutex_lock(&msft->filter_lock);
+
+	list_for_each_entry(address_filter, &msft->address_filters, list) {
+		if (address_filter->active &&
+		    handle == address_filter->msft_handle) {
+			break;
+		}
+	}
+	if (!address_filter) {
+		bt_dev_warn(hdev, "MSFT: No active addr filter (%u) to cancel",
+			    handle);
+		mutex_unlock(&msft->filter_lock);
+		return -ENODEV;
+	}
+	list_del(&address_filter->list);
+
+	mutex_unlock(&msft->filter_lock);
+
+	cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
+	cp.handle = address_filter->msft_handle;
+
+	nskb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
+			      HCI_CMD_TIMEOUT);
+	if (IS_ERR_OR_NULL(nskb)) {
+		bt_dev_err(hdev, "MSFT: Failed to cancel address (%pMR) filter",
+			   &address_filter->bdaddr);
+		kfree(address_filter);
+		return -EIO;
+	}
+	kfree_skb(nskb);
+
+	bt_dev_info(hdev, "MSFT: Canceled device %pMR address filter",
+		    &address_filter->bdaddr);
+
+	kfree(address_filter);
+
+	return 0;
+}
+
 void msft_register(struct hci_dev *hdev)
 {
 	struct msft_data *msft = NULL;
@@ -581,7 +814,9 @@  void msft_register(struct hci_dev *hdev)
 	}
 
 	INIT_LIST_HEAD(&msft->handle_map);
+	INIT_LIST_HEAD(&msft->address_filters);
 	hdev->msft_data = msft;
+	mutex_init(&msft->filter_lock);
 }
 
 void msft_unregister(struct hci_dev *hdev)
@@ -596,6 +831,7 @@  void msft_unregister(struct hci_dev *hdev)
 	hdev->msft_data = NULL;
 
 	kfree(msft->evt_prefix);
+	mutex_destroy(&msft->filter_lock);
 	kfree(msft);
 }
 
@@ -645,12 +881,237 @@  static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
 	return data;
 }
 
+static int msft_add_address_filter_sync(struct hci_dev *hdev, void *data)
+{
+	struct sk_buff *skb = data;
+	struct msft_monitor_addr_filter_data *address_filter = NULL;
+	struct sk_buff *nskb;
+	struct msft_rp_le_monitor_advertisement *rp;
+	bool remove = false;
+	struct msft_data *msft = hdev->msft_data;
+	int err;
+
+	if (!msft) {
+		bt_dev_err(hdev, "MSFT: msft data is freed");
+		err = -EINVAL;
+		goto error;
+	}
+
+	mutex_lock(&msft->filter_lock);
+
+	address_filter = msft_find_address_data(hdev,
+						addr_filter_cb(skb)->addr_type,
+						&addr_filter_cb(skb)->bdaddr,
+						addr_filter_cb(skb)->pattern_handle);
+	if (!address_filter) {
+		bt_dev_warn(hdev, "MSFT: No address (%pMR) filter to enable",
+			    &addr_filter_cb(skb)->bdaddr);
+		mutex_unlock(&msft->filter_lock);
+		err = -ENODEV;
+		goto error;
+	}
+
+	mutex_unlock(&msft->filter_lock);
+
+send_cmd:
+	nskb = __hci_cmd_sync(hdev, hdev->msft_opcode, skb->len, skb->data,
+			      HCI_CMD_TIMEOUT);
+	if (IS_ERR_OR_NULL(nskb)) {
+		bt_dev_err(hdev, "Failed to enable address %pMR filter",
+			   &address_filter->bdaddr);
+		nskb = NULL;
+		remove = true;
+		goto done;
+	}
+
+	rp = (struct msft_rp_le_monitor_advertisement *)nskb->data;
+	if (nskb->len < sizeof(*rp) ||
+	    rp->sub_opcode != MSFT_OP_LE_MONITOR_ADVERTISEMENT) {
+		remove = true;
+		goto done;
+	}
+
+	/* If Controller's memory capacity exceeded, cancel the first address
+	 * filter in the msft->address_filters, then try to add the new address
+	 * filter.
+	 */
+	if (rp->status == HCI_ERROR_MEMORY_EXCEEDED) {
+		struct msft_cp_le_cancel_monitor_advertisement cp;
+		struct msft_monitor_addr_filter_data *n;
+		u8 addr_type = 0xff;
+
+		mutex_lock(&msft->filter_lock);
+
+		/* If the current address filter is the first one in
+		 * msft->address_filters, it means no active address filter in
+		 * Controller.
+		 */
+		if (list_is_first(&address_filter->list,
+				  &msft->address_filters)) {
+			mutex_unlock(&msft->filter_lock);
+			bt_dev_err(hdev, "Memory capacity exceeded");
+			remove = true;
+			goto done;
+		}
+
+		n = list_first_entry(&msft->address_filters,
+				     struct msft_monitor_addr_filter_data,
+				     list);
+		list_del(&n->list);
+
+		mutex_unlock(&msft->filter_lock);
+
+		cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
+		cp.handle = n->msft_handle;
+
+		nskb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
+				      HCI_CMD_TIMEOUT);
+		if (IS_ERR_OR_NULL(nskb)) {
+			bt_dev_err(hdev, "MSFT: Failed to cancel filter (%pMR)",
+				   &n->bdaddr);
+			kfree(n);
+			remove = true;
+			goto done;
+		}
+
+		/* Fake a device lost event after canceling the corresponding
+		 * address filter.
+		 */
+		hci_dev_lock(hdev);
+
+		switch (n->addr_type) {
+		case ADDR_LE_DEV_PUBLIC:
+			addr_type = BDADDR_LE_PUBLIC;
+			break;
+
+		case ADDR_LE_DEV_RANDOM:
+			addr_type = BDADDR_LE_RANDOM;
+			break;
+
+		default:
+			bt_dev_err(hdev, "MSFT unknown addr type 0x%02x",
+				   n->addr_type);
+			break;
+		}
+
+		msft_device_lost(hdev, &n->bdaddr, addr_type,
+				 n->mgmt_handle);
+		hci_dev_unlock(hdev);
+
+		kfree(n);
+		kfree_skb(nskb);
+		goto send_cmd;
+	} else if (rp->status) {
+		bt_dev_err(hdev, "Enable address filter err (status 0x%02x)",
+			   rp->status);
+		remove = true;
+	}
+
+done:
+	kfree_skb(skb);
+
+	mutex_lock(&msft->filter_lock);
+
+	/* Be careful about address_filter that is not protected by the
+	 * filter_lock while the above __hci_cmd_sync() is running.
+	 */
+	if (remove) {
+		bt_dev_warn(hdev, "MSFT: Remove address (%pMR) filter",
+			    &address_filter->bdaddr);
+		list_del(&address_filter->list);
+		kfree(address_filter);
+	} else {
+		address_filter->active = true;
+		address_filter->msft_handle = rp->handle;
+		bt_dev_info(hdev, "MSFT: Address %pMR filter enabled",
+			    &address_filter->bdaddr);
+	}
+
+	mutex_unlock(&msft->filter_lock);
+
+	kfree_skb(nskb);
+
+	return 0;
+error:
+	kfree_skb(skb);
+	return err;
+}
+
+/* This function requires the caller holds msft->filter_lock */
+static struct msft_monitor_addr_filter_data *msft_add_address_filter
+		(struct hci_dev *hdev, u8 addr_type, bdaddr_t *bdaddr,
+		 struct msft_monitor_advertisement_handle_data *handle_data)
+{
+	struct sk_buff *skb;
+	struct msft_cp_le_monitor_advertisement *cp;
+	struct msft_monitor_addr_filter_data *address_filter = NULL;
+	size_t size;
+	struct msft_data *msft = hdev->msft_data;
+	int err;
+
+	size = sizeof(*cp) + sizeof(addr_type) + sizeof(*bdaddr);
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (!skb) {
+		bt_dev_err(hdev, "MSFT: alloc skb err in device evt");
+		return NULL;
+	}
+
+	cp = skb_put(skb, sizeof(*cp));
+	cp->sub_opcode	    = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
+	cp->rssi_high	    = handle_data->rssi_high;
+	cp->rssi_low	    = handle_data->rssi_low;
+	cp->rssi_low_interval    = handle_data->rssi_low_interval;
+	cp->rssi_sampling_period = handle_data->rssi_sampling_period;
+	cp->cond_type	    = MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR;
+	skb_put_u8(skb, addr_type);
+	skb_put_data(skb, bdaddr, sizeof(*bdaddr));
+
+	address_filter = kzalloc(sizeof(*address_filter), GFP_KERNEL);
+	if (!address_filter) {
+		kfree_skb(skb);
+		return NULL;
+	}
+
+	address_filter->active		     = false;
+	address_filter->msft_handle	     = 0xff;
+	address_filter->pattern_handle	     = handle_data->msft_handle;
+	address_filter->mgmt_handle	     = handle_data->mgmt_handle;
+	address_filter->rssi_high	     = cp->rssi_high;
+	address_filter->rssi_low	     = cp->rssi_low;
+	address_filter->rssi_low_interval    = cp->rssi_low_interval;
+	address_filter->rssi_sampling_period = cp->rssi_sampling_period;
+	address_filter->addr_type	     = addr_type;
+	bacpy(&address_filter->bdaddr, bdaddr);
+	list_add_tail(&address_filter->list, &msft->address_filters);
+
+	addr_filter_cb(skb)->pattern_handle = address_filter->pattern_handle;
+	addr_filter_cb(skb)->addr_type = addr_type;
+	bacpy(&addr_filter_cb(skb)->bdaddr, bdaddr);
+
+	err = hci_cmd_sync_queue(hdev, msft_add_address_filter_sync, skb, NULL);
+	if (err < 0) {
+		bt_dev_err(hdev, "MSFT: Add address %pMR filter err", bdaddr);
+		list_del(&address_filter->list);
+		kfree(address_filter);
+		kfree_skb(skb);
+		return NULL;
+	}
+
+	bt_dev_info(hdev, "MSFT: Add device %pMR address filter",
+		    &address_filter->bdaddr);
+
+	return address_filter;
+}
+
 /* This function requires the caller holds hdev->lock */
 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct msft_ev_le_monitor_device *ev;
 	struct msft_monitor_advertisement_handle_data *handle_data;
+	struct msft_monitor_addr_filter_data *n, *address_filter = NULL;
 	u8 addr_type;
+	u16 mgmt_handle = 0xffff;
+	struct msft_data *msft = hdev->msft_data;
 
 	ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
 	if (!ev)
@@ -662,9 +1123,52 @@  static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		   ev->monitor_state, &ev->bdaddr);
 
 	handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
-	if (!handle_data)
+
+	if (!msft->addr_monitor_assist) {
+		if (!handle_data)
+			return;
+		mgmt_handle = handle_data->mgmt_handle;
+		goto report_state;
+	}
+
+	if (handle_data) {
+		/* Don't report any device found/lost event from pattern
+		 * monitors. Pattern monitor always has its address filters for
+		 * tracking devices.
+		 */
+
+		address_filter = msft_find_address_data(hdev, ev->addr_type,
+							&ev->bdaddr,
+							handle_data->msft_handle);
+		if (address_filter)
+			return;
+
+		if (ev->monitor_state && handle_data->cond_type ==
+				MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN)
+			msft_add_address_filter(hdev, ev->addr_type,
+						&ev->bdaddr, handle_data);
+
+		return;
+	}
+
+	/* This device event is not from pattern monitor.
+	 * Report it if there is a corresponding address_filter for it.
+	 */
+	list_for_each_entry(n, &msft->address_filters, list) {
+		if (n->active && n->msft_handle == ev->monitor_handle) {
+			mgmt_handle = n->mgmt_handle;
+			address_filter = n;
+			break;
+		}
+	}
+
+	if (!address_filter) {
+		bt_dev_warn(hdev, "MSFT: Unexpected device event %pMR, %u, %u",
+			    &ev->bdaddr, ev->monitor_handle, ev->monitor_state);
 		return;
+	}
 
+report_state:
 	switch (ev->addr_type) {
 	case ADDR_LE_DEV_PUBLIC:
 		addr_type = BDADDR_LE_PUBLIC;
@@ -681,12 +1185,16 @@  static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		return;
 	}
 
-	if (ev->monitor_state)
-		msft_device_found(hdev, &ev->bdaddr, addr_type,
-				  handle_data->mgmt_handle);
-	else
-		msft_device_lost(hdev, &ev->bdaddr, addr_type,
-				 handle_data->mgmt_handle);
+	if (ev->monitor_state) {
+		msft_device_found(hdev, &ev->bdaddr, addr_type, mgmt_handle);
+	} else {
+		if (address_filter && address_filter->active)
+			hci_cmd_sync_queue(hdev,
+					   msft_cancel_address_filter_sync,
+					   ERR_PTR(address_filter->msft_handle),
+					   NULL);
+		msft_device_lost(hdev, &ev->bdaddr, addr_type, mgmt_handle);
+	}
 }
 
 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
@@ -724,7 +1232,9 @@  void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
 
 	switch (*evt) {
 	case MSFT_EV_LE_MONITOR_DEVICE:
+		mutex_lock(&msft->filter_lock);
 		msft_monitor_device_evt(hdev, skb);
+		mutex_unlock(&msft->filter_lock);
 		break;
 
 	default: