Bluetooth: advmon offload MSFT add monitor

Enables advertising monitor offloading to the controller, if MSFT
extension is supported. The kernel won't adjust the monitor parameters
to match what the controller supports - that is the user space's
responsibility.

This patch only manages the addition of monitors. Monitor removal is
going to be handled by another patch.

Signed-off-by: Archie Pusaka <apusaka@chromium.org>
Reviewed-by: Manish Mandlik <mmandlik@chromium.org>
Reviewed-by: Miao-chen Chou <mcchou@chromium.org>
Reviewed-by: Yun-Hao Chung <howardchung@google.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 84 insertions, 30 deletions

diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 72d37c80e071..fea5e9763b72 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,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);
+		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);
 }