Received-SPF: pass (google.com: domain of linux-bluetooth+bounces-251-linux.lists.archive=gmail.com@vger.kernel.org designates 2604:1380:40f1:3f00::1 as permitted sender) client-ip=2604:1380:40f1:3f00::1;
From: "sebastif@axis.com" <sebastian.david.forslund@gmail.com>
To: linux-bluetooth@vger.kernel.org
Cc: Sebastian Forslund <sebastif@axis.com>
Subject: [PATCH BlueZ 1/1] Pattern match on service- and manufacturer data
Date: Mon, 27 Nov 2023 20:56:32 +0100
Message-Id: <20231127195632.445-1-sebastif@axis.com>
Precedence: bulk
MIME-Version: 1.0
Content-Transfer-Encoding: quoted-printable

From: Sebastian Forslund <sebastif@axis.com>=0D

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
Fixes: https://github.com/bluez/bluez/issues/652=0D
---=0D
 src/shared/ad.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++-=0D
 1 file changed, 77 insertions(+), 1 deletion(-)=0D
=0D
diff --git a/src/shared/ad.c b/src/shared/ad.c=0D
index 951c56c60..7cfe47291 100644=0D
--- a/src/shared/ad.c=0D
+++ b/src/shared/ad.c=0D
@@ -1324,6 +1324,70 @@ struct bt_ad_pattern *bt_ad_pattern_new(uint8_t type=
, size_t offset, size_t len,=0D
 	return pattern;=0D
 }=0D
 =0D
+static void pattern_manufacturer_data_match(void *data, void *user_data) {=
=0D
+	struct bt_ad_manufacturer_data *manufacturer_data =3D data;=0D
+	struct pattern_match_info *info =3D user_data;=0D
+	struct bt_ad_pattern *pattern;=0D
+	uint8_t all_data[BT_AD_MAX_DATA_LEN];=0D
+=0D
+	if (!manufacturer_data || !info)=0D
+		return;=0D
+=0D
+	if (info->matched_pattern)=0D
+		return;=0D
+=0D
+	pattern =3D info->current_pattern;=0D
+=0D
+	if (!pattern || pattern->type !=3D BT_AD_MANUFACTURER_DATA)=0D
+		return;=0D
+=0D
+	/* Take the manufacturer ID into account */=0D
+	if (manufacturer_data->len + 2 < pattern->offset + pattern->len)=0D
+		return;=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
+		info->matched_pattern =3D pattern;=0D
+	}=0D
+}=0D
+=0D
+static void pattern_service_data_match(void *data, void *user_data) {=0D
+  struct bt_ad_service_data *service_data =3D data;=0D
+  struct pattern_match_info *info =3D user_data;=0D
+  struct bt_ad_pattern *pattern;=0D
+=0D
+  if (!service_data || !info)=0D
+    return;=0D
+=0D
+  if (info->matched_pattern)=0D
+    return;=0D
+=0D
+  pattern =3D info->current_pattern;=0D
+=0D
+  if (!pattern)=0D
+    return;=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;=0D
+  }=0D
+=0D
+  if (service_data->len < pattern->offset + pattern->len)=0D
+    return;=0D
+=0D
+  if (!memcmp(service_data->data + pattern->offset, pattern->data,=0D
+              pattern->len)) {=0D
+    info->matched_pattern =3D pattern;=0D
+  }=0D
+}=0D
+=0D
 static void pattern_ad_data_match(void *data, void *user_data)=0D
 {=0D
 	struct bt_ad_data *ad_data =3D data;=0D
@@ -1363,7 +1427,19 @@ static void pattern_match(void *data, void *user_dat=
a)=0D
 =0D
 	info->current_pattern =3D pattern;=0D
 =0D
-	bt_ad_foreach_data(info->ad, pattern_ad_data_match, info);=0D
+	switch (pattern->type) {=0D
+	case BT_AD_MANUFACTURER_DATA:=0D
+		queue_foreach(info->ad->manufacturer_data, pattern_manufacturer_data_mat=
ch, info);=0D
+		break;=0D
+	case BT_AD_SERVICE_DATA16:=0D
+	case BT_AD_SERVICE_DATA32:=0D
+	case BT_AD_SERVICE_DATA128:=0D
+		queue_foreach(info->ad->service_data, pattern_service_data_match, info);=
=0D
+		break;=0D
+	default:=0D
+		bt_ad_foreach_data(info->ad, pattern_ad_data_match, info);=0D
+		break;=0D
+	}=0D
 }=0D
 =0D
 struct bt_ad_pattern *bt_ad_pattern_match(struct bt_ad *ad,=0D
-- =0D
2.39.2=0D
=0D