From 5b501334672eca1274774aec14926898dea733eb Mon Sep 17 00:00:00 2001 From: Lingao Meng Date: Tue, 1 Mar 2022 21:56:50 +0800 Subject: [PATCH] Bluetooth: Host: Reconstruct stream interface Add `remaining_size` in struct ad_stream. By calculating the actual space required instead of using the maximum space. Signed-off-by: Lingao Meng --- subsys/bluetooth/host/adv.c | 58 ++++++++++++++++++++++++++++--------- 1 file changed, 45 insertions(+), 13 deletions(-) diff --git a/subsys/bluetooth/host/adv.c b/subsys/bluetooth/host/adv.c index 96d9bc8e28..ea2363f8ea 100644 --- a/subsys/bluetooth/host/adv.c +++ b/subsys/bluetooth/host/adv.c @@ -49,13 +49,30 @@ struct ad_stream { * The length and type are included in this offset. */ uint16_t current_ltv_offset; + + /* The remaining size of total ad[i].data[j].data_len + 2 for LTV header */ + size_t remaining_size; }; -static void ad_stream_new(struct ad_stream *stream, const struct bt_ad *ad, size_t ad_len) +static int ad_stream_new(struct ad_stream *stream, + const struct bt_ad *ad, size_t ad_len) { (void)memset(stream, 0, sizeof(*stream)); stream->ad = ad; stream->ad_len = ad_len; + + for (size_t i = 0; i < ad_len; i++) { + for (size_t j = 0; j < ad[i].len; j++) { + /* LTV length + type + value */ + stream->remaining_size += ad[i].data[j].data_len + 2; + + if (stream->remaining_size > BT_GAP_ADV_MAX_EXT_ADV_DATA_LEN) { + return -EINVAL; + } + } + } + + return 0; } /** @@ -67,8 +84,7 @@ static void ad_stream_new(struct ad_stream *stream, const struct bt_ad *ad, size */ static bool ad_stream_is_empty(const struct ad_stream *stream) { - /* If ad_index == ad_len, then we are past the last element in the ad array */ - return stream->ad_index == stream->ad_len; + return stream->remaining_size == 0; } /** @@ -96,7 +112,7 @@ static const struct bt_data *ad_stream_current_ltv_update(struct ad_stream *stre } } - if (ad_stream_is_empty(stream)) { + if (stream->ad_index == stream->ad_len) { return NULL; } else { return &stream->ad[stream->ad_index].data[stream->data_index]; @@ -146,6 +162,9 @@ static uint8_t ad_stream_read(struct ad_stream *stream, uint8_t *buf, uint8_t bu } } + __ASSERT_NO_MSG(stream->remaining_size >= read_len); + stream->remaining_size -= read_len; + return read_len; } @@ -565,23 +584,29 @@ static int hci_set_adv_ext_complete(struct bt_le_ext_adv *adv, uint16_t hci_op, static int hci_set_adv_ext_fragmented(struct bt_le_ext_adv *adv, uint16_t hci_op, const struct bt_ad *ad, size_t ad_len) { + int err; struct ad_stream stream; - - ad_stream_new(&stream, ad, ad_len); - bool is_first_iteration = true; + err = ad_stream_new(&stream, ad, ad_len); + if (err) { + return err; + } + while (!ad_stream_is_empty(&stream)) { struct bt_hci_cp_le_set_ext_adv_data *set_data; struct net_buf *buf; + size_t data_len = MIN(sizeof(set_data->data), stream.remaining_size); + const size_t cmd_size = data_len + + offsetof(struct bt_hci_cp_le_set_ext_adv_data, data); int err; - buf = bt_hci_cmd_create(hci_op, sizeof(*set_data)); + buf = bt_hci_cmd_create(hci_op, cmd_size); if (!buf) { return -ENOBUFS; } - set_data = net_buf_add(buf, sizeof(*set_data)); + set_data = net_buf_add(buf, cmd_size); set_data->handle = adv->handle; set_data->frag_pref = BT_HCI_LE_EXT_ADV_FRAG_ENABLED; @@ -667,24 +692,31 @@ static int set_sd(struct bt_le_ext_adv *adv, const struct bt_ad *sd, static int hci_set_per_adv_data(const struct bt_le_ext_adv *adv, const struct bt_data *ad, size_t ad_len) { + int err; struct ad_stream stream; struct bt_ad d = { .data = ad, .len = ad_len }; bool is_first_iteration = true; - ad_stream_new(&stream, &d, 1); + err = ad_stream_new(&stream, &d, 1); + if (err) { + return err; + } while (!ad_stream_is_empty(&stream)) { struct bt_hci_cp_le_set_per_adv_data *set_data; struct net_buf *buf; + size_t data_len = MIN(sizeof(set_data->data), stream.remaining_size); + const size_t cmd_size = data_len + + offsetof(struct bt_hci_cp_le_set_ext_adv_data, data); int err; - buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PER_ADV_DATA, sizeof(*set_data)); + buf = bt_hci_cmd_create(BT_HCI_OP_LE_SET_PER_ADV_DATA, cmd_size); if (!buf) { return -ENOBUFS; } - set_data = net_buf_add(buf, sizeof(*set_data)); - (void)memset(set_data, 0, sizeof(*set_data)); + set_data = net_buf_add(buf, cmd_size); + (void)memset(set_data, 0, cmd_size); set_data->handle = adv->handle; set_data->len = ad_stream_read(&stream, set_data->data, sizeof(set_data->data));