| Message ID | 20201030175219.BlueZ.v8.1.Ie670925d87068d24a6e5c2e7dd9dd0e88c89a7da@changeid (mailing list archive) |
|---|---|
| State | Accepted |
| Headers | show |
| Series | [BlueZ,v8,1/6] shared/ad: Add support of bt_ad_pattern | expand |
Hi Miao, On Fri, Oct 30, 2020 at 5:56 PM Miao-chen Chou <mcchou@chromium.org> wrote: > > 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 Applied, thanks.
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);