Received-SPF: pass (google.com: domain of linux-bluetooth+bounces-424-linux.lists.archive=gmail.com@vger.kernel.org designates 2604:1380:4601:e00::3 as permitted sender) client-ip=2604:1380:4601:e00::3;
Received-SPF: Fail (protection.outlook.com: domain of axis.com does not
 designate 195.60.68.100 as permitted sender) receiver=protection.outlook.com;
 client-ip=195.60.68.100; helo=mail.axis.com;
From: Sebastian Forslund <sebastif@axis.com>
To: <linux-bluetooth@vger.kernel.org>
CC: Sebastian Forslund <sebastif@axis.com>
Subject: [PATCH BlueZ v2] Pattern match on service- and manufacturer data
Date: Thu, 7 Dec 2023 10:09:02 +0100
Message-ID: <20231207090902.884270-1-sebastif@axis.com>
In-Reply-To: <20231207061900.194-1-sebastif@axis.com>
References: <20231207061900.194-1-sebastif@axis.com>
Precedence: bulk
MIME-Version: 1.0
Content-Transfer-Encoding: 8bit
Content-Type: text/plain
X-MS-Exchange-CrossTenant-OriginalArrivalTime: 07 Dec 2023 09:09:31.3607
 (UTC)
X-MS-Exchange-CrossTenant-Network-Message-Id: 68770163-0d80-4537-2651-08dbf7043928
X-MS-Exchange-CrossTenant-Id: 78703d3c-b907-432f-b066-88f7af9ca3af
X-MS-Exchange-CrossTenant-OriginalAttributedTenantConnectingIp: TenantId=78703d3c-b907-432f-b066-88f7af9ca3af;Ip=[195.60.68.100];Helo=[mail.axis.com]
X-MS-Exchange-CrossTenant-AuthSource:
	AM4PEPF00025F97.EURPRD83.prod.outlook.com
X-MS-Exchange-CrossTenant-AuthAs: Anonymous
X-MS-Exchange-CrossTenant-FromEntityHeader: HybridOnPrem
X-MS-Exchange-Transport-CrossTenantHeadersStamped: AS8PR02MB9625

When advertisement monitoring, manufacturer data and service data was
not being matched against. This made it so that advertisement monitoring
with or_patterns did not work that type of data.

We must start matching against the data in the manufacturer_data and
service_data queues. Run a different match-function depending on the
type of monitor that is being matched against.

Closes: https://github.com/bluez/bluez/issues/652
---

v1->v2: Pattern match using queue_find() instead of queue_foreach() so
        that iterations stop when a match is found. Shorten the name
        of the match functions

 src/shared/ad.c | 115 +++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 105 insertions(+), 10 deletions(-)

diff --git a/src/shared/ad.c b/src/shared/ad.c
index 951c56c60..2d6fc628e 100644
--- a/src/shared/ad.c
+++ b/src/shared/ad.c
@@ -1324,36 +1324,110 @@ struct bt_ad_pattern *bt_ad_pattern_new(uint8_t type, size_t offset, size_t len,
 	return pattern;
 }
 
-static void pattern_ad_data_match(void *data, void *user_data)
+static bool match_manufacturer(const void *data, const void *user_data)
 {
-	struct bt_ad_data *ad_data = data;
-	struct pattern_match_info *info = user_data;
-	struct bt_ad_pattern *pattern;
+	const struct bt_ad_manufacturer_data *manufacturer_data = data;
+	const struct pattern_match_info *info = user_data;
+	const struct bt_ad_pattern *pattern;
+	uint8_t all_data[BT_AD_MAX_DATA_LEN];
+
+	if (!manufacturer_data || !info)
+		return false;
+
+	if (info->matched_pattern)
+		return false;
+
+	pattern = info->current_pattern;
+
+	if (!pattern || pattern->type != BT_AD_MANUFACTURER_DATA)
+		return false;
+
+	/* Take the manufacturer ID into account */
+	if (manufacturer_data->len + 2 < pattern->offset + pattern->len)
+		return false;
+
+	memcpy(&all_data[0], &manufacturer_data->manufacturer_id, 2);
+	memcpy(&all_data[2], manufacturer_data->data, manufacturer_data->len);
+
+	if (!memcmp(all_data + pattern->offset, pattern->data,
+							pattern->len)) {
+		return true;
+	}
+
+	return false;
+}
+
+static bool match_service(const void *data, const void *user_data)
+{
+	const struct bt_ad_service_data *service_data = data;
+	const struct pattern_match_info *info = user_data;
+	const struct bt_ad_pattern *pattern;
+
+	if (!service_data || !info)
+		return false;
+
+	if (info->matched_pattern)
+		return false;
+
+	pattern = info->current_pattern;
+
+	if (!pattern)
+		return false;
+
+	switch (pattern->type) {
+	case BT_AD_SERVICE_DATA16:
+	case BT_AD_SERVICE_DATA32:
+	case BT_AD_SERVICE_DATA128:
+		break;
+	default:
+		return false;
+	}
+
+	if (service_data->len < pattern->offset + pattern->len)
+		return false;
+
+	if (!memcmp(service_data->data + pattern->offset, pattern->data,
+							pattern->len)) {
+		return true;
+	}
+
+	return false;
+}
+
+static bool match_ad_data(const void *data, const void *user_data)
+{
+	const struct bt_ad_data *ad_data = data;
+	const struct pattern_match_info *info = user_data;
+	const struct bt_ad_pattern *pattern;
 
 	if (!ad_data || !info)
-		return;
+		return false;
 
 	if (info->matched_pattern)
-		return;
+		return false;
 
 	pattern = info->current_pattern;
 
 	if (!pattern || ad_data->type != pattern->type)
-		return;
+		return false;
 
 	if (ad_data->len < pattern->offset + pattern->len)
-		return;
+		return false;
 
 	if (!memcmp(ad_data->data + pattern->offset, pattern->data,
 								pattern->len)) {
-		info->matched_pattern = pattern;
+		return true;
 	}
+
+	return false;
 }
 
 static void pattern_match(void *data, void *user_data)
 {
 	struct bt_ad_pattern *pattern = data;
 	struct pattern_match_info *info = user_data;
+	struct bt_ad *ad;
+	void *matched = NULL;
 
 	if (!pattern || !info)
 		return;
@@ -1362,8 +1436,29 @@ static void pattern_match(void *data, void *user_data)
 		return;
 
 	info->current_pattern = pattern;
+	ad = info->ad;
+
+	if (!ad)
+		return;
+
+	switch (pattern->type) {
+	case BT_AD_MANUFACTURER_DATA:
+		matched = queue_find(ad->manufacturer_data, match_manufacturer,
+				user_data);
+		break;
+	case BT_AD_SERVICE_DATA16:
+	case BT_AD_SERVICE_DATA32:
+	case BT_AD_SERVICE_DATA128:
+		matched = queue_find(ad->service_data, match_service,
+				user_data);
+		break;
+	default:
+		matched = queue_find(ad->data, match_ad_data, user_data);
+		break;
+	}
 
-	bt_ad_foreach_data(info->ad, pattern_ad_data_match, info);
+	if (matched)
+		info->matched_pattern = info->current_pattern;
 }
 
 struct bt_ad_pattern *bt_ad_pattern_match(struct bt_ad *ad,
-- 
2.39.2