[PATCH BlueZ v2] Pattern match on service- and manufacturer data

[Sebastian Forslund <sebastian.david.forslund@gmail.com>]

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