Received-SPF: pass (google.com: domain of linux-bluetooth-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) client-ip=23.128.96.18;
From:   Miao-chen Chou <mcchou@chromium.org>
To:     Bluetooth Kernel Mailing List <linux-bluetooth@vger.kernel.org>
Cc:     Alain Michaud <alainm@chromium.org>,
        Manish Mandlik <mmandlik@chromium.org>,
        Marcel Holtmann <marcel@holtmann.org>,
        chromeos-bluetooth-upstreaming@chromium.org,
        Howard Chung <howardchung@google.com>,
        Luiz Augusto von Dentz <luiz.von.dentz@intel.com>,
        Miao-chen Chou <mcchou@chromium.org>,
        Archie Pusaka <apusaka@chromium.org>,
        Sonny Sasaka <sonnysasaka@chromium.org>
Subject: [BlueZ PATCH v8 1/6] shared/ad: Add support of bt_ad_pattern
Date:   Fri, 30 Oct 2020 17:52:46 -0700
Message-Id: <20201030175219.BlueZ.v8.1.Ie670925d87068d24a6e5c2e7dd9dd0e88c89a7da@changeid>
MIME-Version: 1.0
Content-Transfer-Encoding: 8bit
Precedence: bulk

This adds struct bt_ad_pattern and helpers functions to facilitate
pattern matching.

Reviewed-by: Archie Pusaka <apusaka@chromium.org>
Reviewed-by: Sonny Sasaka <sonnysasaka@chromium.org>
Reviewed-by: Howard Chung <howardchung@google.com>
---

Changes in v8:
- Modify signature of ad_replace_data() to avoid memory copy

 src/shared/ad.c | 151 +++++++++++++++++++++++++++++++++++++++++++++++-
 src/shared/ad.h |  16 +++++
 2 files changed, 166 insertions(+), 1 deletion(-)

diff --git a/src/shared/ad.c b/src/shared/ad.c
index a34d7a147..23c8c34f4 100644
--- a/src/shared/ad.c
+++ b/src/shared/ad.c
@@ -31,6 +31,12 @@ struct bt_ad {
 	struct queue *data;
 };
 
+struct pattern_match_info {
+	struct bt_ad *ad;
+	struct bt_ad_pattern *current_pattern;
+	struct bt_ad_pattern *matched_pattern;
+};
+
 struct bt_ad *bt_ad_new(void)
 {
 	struct bt_ad *ad;
@@ -46,6 +52,65 @@ struct bt_ad *bt_ad_new(void)
 	return bt_ad_ref(ad);
 }
 
+static bool ad_replace_data(struct bt_ad *ad, uint8_t type, const void *data,
+							size_t len);
+
+static bool ad_is_type_valid(uint8_t type)
+{
+	if (type > BT_AD_3D_INFO_DATA && type != BT_AD_MANUFACTURER_DATA)
+		return false;
+	if (type < BT_AD_FLAGS)
+		return false;
+
+	return true;
+}
+
+struct bt_ad *bt_ad_new_with_data(size_t len, const uint8_t *data)
+{
+	struct bt_ad *ad;
+	uint16_t parsed_len = 0;
+
+	if (data == NULL || !len)
+		return NULL;
+
+	ad = bt_ad_new();
+	if (!ad)
+		return NULL;
+
+	while (parsed_len < len - 1) {
+		uint8_t d_len;
+		uint8_t d_type;
+		const uint8_t *d;
+		uint8_t field_len = data[0];
+
+		if (field_len == 0)
+			break;
+
+		parsed_len += field_len + 1;
+
+		if (parsed_len > len)
+			break;
+
+		d = &data[2];
+		d_type = data[1];
+		d_len = field_len - 1;
+
+		if (!ad_is_type_valid(d_type))
+			goto failed;
+
+		if (!ad_replace_data(ad, d_type, d, d_len))
+			goto failed;
+
+		data += field_len + 1;
+	}
+
+	return ad;
+
+failed:
+	bt_ad_unref(ad);
+	return NULL;
+}
+
 struct bt_ad *bt_ad_ref(struct bt_ad *ad)
 {
 	if (!ad)
@@ -126,7 +191,7 @@ static bool data_type_match(const void *data, const void *user_data)
 	return a->type == type;
 }
 
-static bool ad_replace_data(struct bt_ad *ad, uint8_t type, void *data,
+static bool ad_replace_data(struct bt_ad *ad, uint8_t type, const void *data,
 							size_t len)
 {
 	struct bt_ad_data *new_data;
@@ -994,3 +1059,87 @@ void bt_ad_clear_data(struct bt_ad *ad)
 
 	queue_remove_all(ad->data, NULL, NULL, data_destroy);
 }
+
+struct bt_ad_pattern *bt_ad_pattern_new(uint8_t type, size_t offset, size_t len,
+							const uint8_t *data)
+{
+	struct bt_ad_pattern *pattern;
+
+	if (!data || !len || offset >= BT_AD_MAX_DATA_LEN ||
+		len > BT_AD_MAX_DATA_LEN || offset + len > BT_AD_MAX_DATA_LEN) {
+		return NULL;
+	}
+
+	if (!ad_is_type_valid(type))
+		return NULL;
+
+	pattern = new0(struct bt_ad_pattern, 1);
+	if (!pattern)
+		return NULL;
+
+	pattern->len = len;
+	pattern->type = type;
+	pattern->offset = offset;
+	memcpy(pattern->data, data, len);
+
+	return pattern;
+}
+
+static void pattern_ad_data_match(void *data, void *user_data)
+{
+	struct bt_ad_data *ad_data = data;
+	struct pattern_match_info *info = user_data;
+	struct bt_ad_pattern *pattern;
+
+	if (!ad_data || !info)
+		return;
+
+	if (info->matched_pattern)
+		return;
+
+	pattern = info->current_pattern;
+
+	if (!pattern || ad_data->type != pattern->type)
+		return;
+
+	if (ad_data->len < pattern->offset + pattern->len)
+		return;
+
+	if (!memcmp(ad_data->data + pattern->offset, pattern->data,
+								pattern->len)) {
+		info->matched_pattern = pattern;
+	}
+}
+
+static void pattern_match(void *data, void *user_data)
+{
+	struct bt_ad_pattern *pattern = data;
+	struct pattern_match_info *info = user_data;
+
+	if (!pattern || !info)
+		return;
+
+	if (info->matched_pattern)
+		return;
+
+	info->current_pattern = pattern;
+
+	bt_ad_foreach_data(info->ad, pattern_ad_data_match, info);
+}
+
+struct bt_ad_pattern *bt_ad_pattern_match(struct bt_ad *ad,
+							struct queue *patterns)
+{
+	struct pattern_match_info info;
+
+	if (!ad || queue_isempty(patterns))
+		return NULL;
+
+	info.ad = ad;
+	info.matched_pattern = NULL;
+	info.current_pattern = NULL;
+
+	queue_foreach(patterns, pattern_match, &info);
+
+	return info.matched_pattern;
+}
diff --git a/src/shared/ad.h b/src/shared/ad.h
index 83eacab66..13adcb406 100644
--- a/src/shared/ad.h
+++ b/src/shared/ad.h
@@ -68,6 +68,7 @@
 typedef void (*bt_ad_func_t)(void *data, void *user_data);
 
 struct bt_ad;
+struct queue;
 
 struct bt_ad_manufacturer_data {
 	uint16_t manufacturer_id;
@@ -87,8 +88,17 @@ struct bt_ad_data {
 	size_t len;
 };
 
+struct bt_ad_pattern {
+	uint8_t type;
+	uint8_t offset;
+	uint8_t len;
+	uint8_t data[BT_AD_MAX_DATA_LEN];
+};
+
 struct bt_ad *bt_ad_new(void);
 
+struct bt_ad *bt_ad_new_with_data(size_t len, const uint8_t *data);
+
 struct bt_ad *bt_ad_ref(struct bt_ad *ad);
 
 void bt_ad_unref(struct bt_ad *ad);
@@ -156,3 +166,9 @@ void bt_ad_foreach_data(struct bt_ad *ad, bt_ad_func_t func, void *user_data);
 bool bt_ad_remove_data(struct bt_ad *ad, uint8_t type);
 
 void bt_ad_clear_data(struct bt_ad *ad);
+
+struct bt_ad_pattern *bt_ad_pattern_new(uint8_t type, size_t offset,
+					size_t len, const uint8_t *data);
+
+struct bt_ad_pattern *bt_ad_pattern_match(struct bt_ad *ad,
+							struct queue *patterns);
-- 
2.26.2