Received-SPF: pass (google.com: domain of linux-bluetooth+bounces-415-linux.lists.archive=gmail.com@vger.kernel.org designates 139.178.88.99 as permitted sender) client-ip=139.178.88.99;
From: Sebastian Forslund <sebastian.david.forslund@gmail.com>
To: sebastian.david.forslund@gmail.com,
	linux-bluetooth@vger.kernel.org
Cc: Sebastian Forslund <sebastif@axis.com>
Subject: [PATCH BlueZ v2] Pattern match on service- and manufacturer data
Date: Wed,  6 Dec 2023 22:24:57 +0100
Message-Id: <20231206212457.275-1-sebastif@axis.com>
Precedence: bulk
MIME-Version: 1.0
Content-Transfer-Encoding: quoted-printable

When advertisement monitoring, manufacturer data and service data was=0D
not being matched against. This made it so that advertisement monitoring=0D
with or_patterns did not work that type of data.=0D
=0D
We must start matching against the data in the manufacturer_data and=0D
service_data queues. Run a different match-function depending on the=0D
type of monitor that is being matched against.=0D
=0D
Closes: https://github.com/bluez/bluez/issues/652=0D
---=0D
=0D
v1->v2: Pattern match using queue_find() instead of queue_foreach() so=0D
        that iterations stop when a match is found. Shorten the name=0D
        of the match functions=0D
=0D
 src/shared/ad.c | 115 +++++++++++++++++++++++++++++++++++++++++++-----=0D
 1 file changed, 105 insertions(+), 10 deletions(-)=0D
=0D
diff --git a/src/shared/ad.c b/src/shared/ad.c=0D
index 951c56c60..2d6fc628e 100644=0D
--- a/src/shared/ad.c=0D
+++ b/src/shared/ad.c=0D
@@ -1324,36 +1324,110 @@ struct bt_ad_pattern *bt_ad_pattern_new(uint8_t ty=
pe, size_t offset, size_t len,=0D
 	return pattern;=0D
 }=0D
 =0D
-static void pattern_ad_data_match(void *data, void *user_data)=0D
+static bool match_manufacturer(const void *data, const void *user_data)=0D
 {=0D
-	struct bt_ad_data *ad_data =3D data;=0D
-	struct pattern_match_info *info =3D user_data;=0D
-	struct bt_ad_pattern *pattern;=0D
+	const struct bt_ad_manufacturer_data *manufacturer_data =3D data;=0D
+	const struct pattern_match_info *info =3D user_data;=0D
+	const struct bt_ad_pattern *pattern;=0D
+	uint8_t all_data[BT_AD_MAX_DATA_LEN];=0D
+=0D
+	if (!manufacturer_data || !info)=0D
+		return false;=0D
+=0D
+	if (info->matched_pattern)=0D
+		return false;=0D
+=0D
+	pattern =3D info->current_pattern;=0D
+=0D
+	if (!pattern || pattern->type !=3D BT_AD_MANUFACTURER_DATA)=0D
+		return false;=0D
+=0D
+	/* Take the manufacturer ID into account */=0D
+	if (manufacturer_data->len + 2 < pattern->offset + pattern->len)=0D
+		return false;=0D
+=0D
+	memcpy(&all_data[0], &manufacturer_data->manufacturer_id, 2);=0D
+	memcpy(&all_data[2], manufacturer_data->data, manufacturer_data->len);=0D
+=0D
+	if (!memcmp(all_data + pattern->offset, pattern->data,=0D
+							pattern->len)) {=0D
+		return true;=0D
+	}=0D
+=0D
+	return false;=0D
+}=0D
+=0D
+static bool match_service(const void *data, const void *user_data)=0D
+{=0D
+	const struct bt_ad_service_data *service_data =3D data;=0D
+	const struct pattern_match_info *info =3D user_data;=0D
+	const struct bt_ad_pattern *pattern;=0D
+=0D
+	if (!service_data || !info)=0D
+		return false;=0D
+=0D
+	if (info->matched_pattern)=0D
+		return false;=0D
+=0D
+	pattern =3D info->current_pattern;=0D
+=0D
+	if (!pattern)=0D
+		return false;=0D
+=0D
+	switch (pattern->type) {=0D
+	case BT_AD_SERVICE_DATA16:=0D
+	case BT_AD_SERVICE_DATA32:=0D
+	case BT_AD_SERVICE_DATA128:=0D
+		break;=0D
+	default:=0D
+		return false;=0D
+	}=0D
+=0D
+	if (service_data->len < pattern->offset + pattern->len)=0D
+		return false;=0D
+=0D
+	if (!memcmp(service_data->data + pattern->offset, pattern->data,=0D
+							pattern->len)) {=0D
+		return true;=0D
+	}=0D
+=0D
+	return false;=0D
+}=0D
+=0D
+static bool match_ad_data(const void *data, const void *user_data)=0D
+{=0D
+	const struct bt_ad_data *ad_data =3D data;=0D
+	const struct pattern_match_info *info =3D user_data;=0D
+	const struct bt_ad_pattern *pattern;=0D
 =0D
 	if (!ad_data || !info)=0D
-		return;=0D
+		return false;=0D
 =0D
 	if (info->matched_pattern)=0D
-		return;=0D
+		return false;=0D
 =0D
 	pattern =3D info->current_pattern;=0D
 =0D
 	if (!pattern || ad_data->type !=3D pattern->type)=0D
-		return;=0D
+		return false;=0D
 =0D
 	if (ad_data->len < pattern->offset + pattern->len)=0D
-		return;=0D
+		return false;=0D
 =0D
 	if (!memcmp(ad_data->data + pattern->offset, pattern->data,=0D
 								pattern->len)) {=0D
-		info->matched_pattern =3D pattern;=0D
+		return true;=0D
 	}=0D
+=0D
+	return false;=0D
 }=0D
 =0D
 static void pattern_match(void *data, void *user_data)=0D
 {=0D
 	struct bt_ad_pattern *pattern =3D data;=0D
 	struct pattern_match_info *info =3D user_data;=0D
+	struct bt_ad *ad;=0D
+	void *matched =3D NULL;=0D
 =0D
 	if (!pattern || !info)=0D
 		return;=0D
@@ -1362,8 +1436,29 @@ static void pattern_match(void *data, void *user_dat=
a)=0D
 		return;=0D
 =0D
 	info->current_pattern =3D pattern;=0D
+	ad =3D info->ad;=0D
+=0D
+	if (!ad)=0D
+		return;=0D
+=0D
+	switch (pattern->type) {=0D
+	case BT_AD_MANUFACTURER_DATA:=0D
+		matched =3D queue_find(ad->manufacturer_data, match_manufacturer,=0D
+				user_data);=0D
+		break;=0D
+	case BT_AD_SERVICE_DATA16:=0D
+	case BT_AD_SERVICE_DATA32:=0D
+	case BT_AD_SERVICE_DATA128:=0D
+		matched =3D queue_find(ad->service_data, match_service,=0D
+				user_data);=0D
+		break;=0D
+	default:=0D
+		matched =3D queue_find(ad->data, match_ad_data, user_data);=0D
+		break;=0D
+	}=0D
 =0D
-	bt_ad_foreach_data(info->ad, pattern_ad_data_match, info);=0D
+	if (matched)=0D
+		info->matched_pattern =3D info->current_pattern;=0D
 }=0D
 =0D
 struct bt_ad_pattern *bt_ad_pattern_match(struct bt_ad *ad,=0D
-- =0D
2.39.2=0D
=0D