From 8a7ab6cb3d5f4d5c2ff022a1f1e464e370198747 Mon Sep 17 00:00:00 2001 From: lly Date: Mon, 2 Sep 2019 15:30:40 +0800 Subject: [PATCH] ble_mesh: fix adhering to the configured Friend Queue size --- components/bt/esp_ble_mesh/mesh_core/friend.c | 254 +++++++++++++----- components/bt/esp_ble_mesh/mesh_core/friend.h | 9 +- components/bt/esp_ble_mesh/mesh_core/net.h | 6 + .../bt/esp_ble_mesh/mesh_core/transport.c | 65 ++++- 4 files changed, 256 insertions(+), 78 deletions(-) diff --git a/components/bt/esp_ble_mesh/mesh_core/friend.c b/components/bt/esp_ble_mesh/mesh_core/friend.c index de8ca4cc2c..3d637d9bd2 100644 --- a/components/bt/esp_ble_mesh/mesh_core/friend.c +++ b/components/bt/esp_ble_mesh/mesh_core/friend.c @@ -70,48 +70,6 @@ static struct bt_mesh_adv *adv_alloc(int id) return &adv_pool[id].adv; } -static void discard_buffer(void) -{ - struct bt_mesh_friend *frnd = &bt_mesh.frnd[0]; - struct net_buf *buf; - int i; - - /* Find the Friend context with the most queued buffers */ - for (i = 1; i < ARRAY_SIZE(bt_mesh.frnd); i++) { - if (bt_mesh.frnd[i].queue_size > frnd->queue_size) { - frnd = &bt_mesh.frnd[i]; - } - } - - buf = net_buf_slist_get(&frnd->queue); - __ASSERT_NO_MSG(buf != NULL); - BT_WARN("Discarding buffer %p for LPN 0x%04x", buf, frnd->lpn); - net_buf_unref(buf); -} - -static struct net_buf *friend_buf_alloc(u16_t src) -{ - struct net_buf *buf; - - BT_DBG("src 0x%04x", src); - - do { - buf = bt_mesh_adv_create_from_pool(&friend_buf_pool, adv_alloc, - BLE_MESH_ADV_DATA, - FRIEND_XMIT, K_NO_WAIT); - if (!buf) { - discard_buffer(); - } - } while (!buf); - - BLE_MESH_ADV(buf)->addr = src; - FRIEND_ADV(buf)->seq_auth = TRANS_SEQ_AUTH_NVAL; - - BT_DBG("allocated buf %p", buf); - - return buf; -} - static bool is_lpn_unicast(struct bt_mesh_friend *frnd, u16_t addr) { if (frnd->lpn == BLE_MESH_ADDR_UNASSIGNED) { @@ -151,6 +109,20 @@ struct bt_mesh_friend *bt_mesh_friend_find(u16_t net_idx, u16_t lpn_addr, return NULL; } +static void purge_buffers(sys_slist_t *list) +{ + while (!sys_slist_is_empty(list)) { + struct net_buf *buf; + + buf = (void *)sys_slist_get_not_empty(list); + + buf->frags = NULL; + buf->flags &= ~NET_BUF_FRAGS; + + net_buf_unref(buf); + } +} + /* Intentionally start a little bit late into the ReceiveWindow when * it's large enough. This may improve reliability with some platforms, * like the PTS, where the receiver might not have sufficiently compensated @@ -185,16 +157,13 @@ static void friend_clear(struct bt_mesh_friend *frnd) frnd->last = NULL; } - while (!sys_slist_is_empty(&frnd->queue)) { - net_buf_unref(net_buf_slist_get(&frnd->queue)); - } + purge_buffers(&frnd->queue); for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) { struct bt_mesh_friend_seg *seg = &frnd->seg[i]; - while (!sys_slist_is_empty(&seg->queue)) { - net_buf_unref(net_buf_slist_get(&seg->queue)); - } + purge_buffers(&seg->queue); + seg->seg_count = 0U; } frnd->valid = 0U; @@ -336,7 +305,15 @@ static struct net_buf *create_friend_pdu(struct bt_mesh_friend *frnd, sub = bt_mesh_subnet_get(frnd->net_idx); __ASSERT_NO_MSG(sub != NULL); - buf = friend_buf_alloc(info->src); + buf = bt_mesh_adv_create_from_pool(&friend_buf_pool, adv_alloc, + BLE_MESH_ADV_DATA, + FRIEND_XMIT, K_NO_WAIT); + if (!buf) { + return NULL; + } + + BLE_MESH_ADV(buf)->addr = info->src; + FRIEND_ADV(buf)->seq_auth = TRANS_SEQ_AUTH_NVAL; /* Friend Offer needs master security credentials */ if (info->ctl && TRANS_CTL_OP(sdu->data) == TRANS_CTL_OP_FRIEND_OFFER) { @@ -897,7 +874,8 @@ init_friend: } static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd, - u16_t src, u64_t *seq_auth) + u16_t src, u64_t *seq_auth, + u8_t seg_count) { struct bt_mesh_friend_seg *unassigned = NULL; int i; @@ -916,12 +894,16 @@ static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd, } } + if (unassigned) { + unassigned->seg_count = seg_count; + } + return unassigned; } static void enqueue_friend_pdu(struct bt_mesh_friend *frnd, enum bt_mesh_friend_pdu_type type, - struct net_buf *buf) + u8_t seg_count, struct net_buf *buf) { struct bt_mesh_friend_seg *seg; struct friend_adv *adv; @@ -938,7 +920,7 @@ static void enqueue_friend_pdu(struct bt_mesh_friend *frnd, } adv = FRIEND_ADV(buf); - seg = get_seg(frnd, BLE_MESH_ADV(buf)->addr, &adv->seq_auth); + seg = get_seg(frnd, BLE_MESH_ADV(buf)->addr, &adv->seq_auth, seg_count); if (!seg) { BT_ERR("%s, No free friend segment RX contexts for 0x%04x", __func__, BLE_MESH_ADV(buf)->addr); @@ -963,6 +945,10 @@ static void enqueue_friend_pdu(struct bt_mesh_friend *frnd, } sys_slist_merge_slist(&frnd->queue, &seg->queue); + seg->seg_count = 0U; + } else { + /* Mark the buffer as having more to come after it */ + buf->flags |= NET_BUF_FRAGS; } } @@ -1028,13 +1014,17 @@ static void friend_timeout(struct k_work *work) return; } - frnd->last = net_buf_slist_get(&frnd->queue); + frnd->last = (void *)sys_slist_get(&frnd->queue); if (!frnd->last) { BT_WARN("%s, Friendship not established with 0x%04x", __func__, frnd->lpn); friend_clear(frnd); return; } + /* Clear the flag we use for segment tracking */ + frnd->last->flags &= ~NET_BUF_FRAGS; + frnd->last->frags = NULL; + BT_DBG("Sending buf %p from Friend Queue of LPN 0x%04x", frnd->last, frnd->lpn); frnd->queue_size--; @@ -1097,7 +1087,8 @@ static void friend_purge_old_ack(struct bt_mesh_friend *frnd, u64_t *seq_auth, static void friend_lpn_enqueue_rx(struct bt_mesh_friend *frnd, struct bt_mesh_net_rx *rx, enum bt_mesh_friend_pdu_type type, - u64_t *seq_auth, struct net_buf_simple *sbuf) + u64_t *seq_auth, u8_t seg_count, + struct net_buf_simple *sbuf) { struct friend_pdu_info info; struct net_buf *buf; @@ -1135,7 +1126,7 @@ static void friend_lpn_enqueue_rx(struct bt_mesh_friend *frnd, FRIEND_ADV(buf)->seq_auth = *seq_auth; } - enqueue_friend_pdu(frnd, type, buf); + enqueue_friend_pdu(frnd, type, seg_count, buf); BT_DBG("Queued message for LPN 0x%04x, queue_size %u", frnd->lpn, frnd->queue_size); @@ -1144,7 +1135,8 @@ static void friend_lpn_enqueue_rx(struct bt_mesh_friend *frnd, static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd, struct bt_mesh_net_tx *tx, enum bt_mesh_friend_pdu_type type, - u64_t *seq_auth, struct net_buf_simple *sbuf) + u64_t *seq_auth, u8_t seg_count, + struct net_buf_simple *sbuf) { struct friend_pdu_info info; struct net_buf *buf; @@ -1179,7 +1171,7 @@ static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd, FRIEND_ADV(buf)->seq_auth = *seq_auth; } - enqueue_friend_pdu(frnd, type, buf); + enqueue_friend_pdu(frnd, type, seg_count, buf); BT_DBG("Queued message for LPN 0x%04x", frnd->lpn); } @@ -1229,9 +1221,118 @@ bool bt_mesh_friend_match(u16_t net_idx, u16_t addr) return false; } +static bool friend_queue_has_space(struct bt_mesh_friend *frnd, u16_t addr, + u64_t *seq_auth, u8_t seg_count) +{ + u32_t total = 0U; + int i; + + if (seg_count > CONFIG_BLE_MESH_FRIEND_QUEUE_SIZE) { + return false; + } + + for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) { + struct bt_mesh_friend_seg *seg = &frnd->seg[i]; + + if (seq_auth) { + struct net_buf *buf; + + /* If there's a segment queue for this message then the + * space verification has already happened. + */ + buf = (void *)sys_slist_peek_head(&seg->queue); + if (buf && BLE_MESH_ADV(buf)->addr == addr && + FRIEND_ADV(buf)->seq_auth == *seq_auth) { + return true; + } + } + + total += seg->seg_count; + } + + /* If currently pending segments combined with this segmented message + * are more than the Friend Queue Size, then there's no space. This + * is because we don't have a mechanism of aborting already pending + * segmented messages to free up buffers. + */ + return (CONFIG_BLE_MESH_FRIEND_QUEUE_SIZE - total) > seg_count; +} + +bool bt_mesh_friend_queue_has_space(u16_t net_idx, u16_t src, u16_t dst, + u64_t *seq_auth, u8_t seg_count) +{ + bool someone_has_space = false, friend_match = false; + int i; + + for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) { + struct bt_mesh_friend *frnd = &bt_mesh.frnd[i]; + + if (!friend_lpn_matches(frnd, net_idx, dst)) { + continue; + } + + friend_match = true; + + if (friend_queue_has_space(frnd, src, seq_auth, seg_count)) { + someone_has_space = true; + } + } + + /* If there were no matched LPNs treat this as success, so the + * transport layer can continue its work. + */ + if (!friend_match) { + return true; + } + + /* From the transport layers perspective it's good enough that at + * least one Friend Queue has space. If there were multiple Friend + * matches then the destination must be a group address, in which + * case e.g. segment acks are not sent. + */ + return someone_has_space; +} + +static bool friend_queue_prepare_space(struct bt_mesh_friend *frnd, u16_t addr, + u64_t *seq_auth, u8_t seg_count) +{ + bool pending_segments; + u8_t avail_space; + + if (!friend_queue_has_space(frnd, addr, seq_auth, seg_count)) { + return false; + } + + avail_space = CONFIG_BLE_MESH_FRIEND_QUEUE_SIZE - frnd->queue_size; + pending_segments = false; + + while (pending_segments || avail_space < seg_count) { + struct net_buf *buf = (void *)sys_slist_get(&frnd->queue); + + if (!buf) { + BT_ERR("Unable to free up enough buffers"); + return false; + } + + frnd->queue_size--; + avail_space++; + + pending_segments = (buf->flags & NET_BUF_FRAGS); + + /* Make sure old slist entry state doesn't remain */ + buf->frags = NULL; + buf->flags &= ~NET_BUF_FRAGS; + + net_buf_unref(buf); + } + + return true; +} + void bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx *rx, enum bt_mesh_friend_pdu_type type, - u64_t *seq_auth, struct net_buf_simple *sbuf) + u64_t *seq_auth, u8_t seg_count, + struct net_buf_simple *sbuf) { int i; @@ -1248,16 +1349,25 @@ void bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx *rx, for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) { struct bt_mesh_friend *frnd = &bt_mesh.frnd[i]; - if (friend_lpn_matches(frnd, rx->sub->net_idx, - rx->ctx.recv_dst)) { - friend_lpn_enqueue_rx(frnd, rx, type, seq_auth, sbuf); + if (!friend_lpn_matches(frnd, rx->sub->net_idx, + rx->ctx.recv_dst)) { + continue; } + + if (!friend_queue_prepare_space(frnd, rx->ctx.addr, seq_auth, + seg_count)) { + continue; + } + + friend_lpn_enqueue_rx(frnd, rx, type, seq_auth, seg_count, + sbuf); } } bool bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx *tx, enum bt_mesh_friend_pdu_type type, - u64_t *seq_auth, struct net_buf_simple *sbuf) + u64_t *seq_auth, u8_t seg_count, + struct net_buf_simple *sbuf) { bool matched = false; int i; @@ -1273,10 +1383,19 @@ bool bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx *tx, for (i = 0; i < ARRAY_SIZE(bt_mesh.frnd); i++) { struct bt_mesh_friend *frnd = &bt_mesh.frnd[i]; - if (friend_lpn_matches(frnd, tx->sub->net_idx, tx->ctx->addr)) { - friend_lpn_enqueue_tx(frnd, tx, type, seq_auth, sbuf); - matched = true; + if (!friend_lpn_matches(frnd, tx->sub->net_idx, + tx->ctx->addr)) { + continue; + } + + if (!friend_queue_prepare_space(frnd, tx->src, seq_auth, + seg_count)) { + continue; } + + friend_lpn_enqueue_tx(frnd, tx, type, seq_auth, seg_count, + sbuf); + matched = true; } return matched; @@ -1316,9 +1435,8 @@ void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src, BT_WARN("%s, Clearing incomplete segments for 0x%04x", __func__, src); - while (!sys_slist_is_empty(&seg->queue)) { - net_buf_unref(net_buf_slist_get(&seg->queue)); - } + purge_buffers(&seg->queue); + seg->seg_count = 0U; } } } diff --git a/components/bt/esp_ble_mesh/mesh_core/friend.h b/components/bt/esp_ble_mesh/mesh_core/friend.h index 008a342c9b..c6ecf7e6c3 100644 --- a/components/bt/esp_ble_mesh/mesh_core/friend.h +++ b/components/bt/esp_ble_mesh/mesh_core/friend.h @@ -20,12 +20,17 @@ bool bt_mesh_friend_match(u16_t net_idx, u16_t addr); struct bt_mesh_friend *bt_mesh_friend_find(u16_t net_idx, u16_t lpn_addr, bool valid, bool established); +bool bt_mesh_friend_queue_has_space(u16_t net_idx, u16_t src, u16_t dst, + u64_t *seq_auth, u8_t seg_count); + void bt_mesh_friend_enqueue_rx(struct bt_mesh_net_rx *rx, enum bt_mesh_friend_pdu_type type, - u64_t *seq_auth, struct net_buf_simple *sbuf); + u64_t *seq_auth, u8_t seg_count, + struct net_buf_simple *sbuf); bool bt_mesh_friend_enqueue_tx(struct bt_mesh_net_tx *tx, enum bt_mesh_friend_pdu_type type, - u64_t *seq_auth, struct net_buf_simple *sbuf); + u64_t *seq_auth, u8_t seg_count, + struct net_buf_simple *sbuf); void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src, u16_t dst, u64_t *seq_auth); diff --git a/components/bt/esp_ble_mesh/mesh_core/net.h b/components/bt/esp_ble_mesh/mesh_core/net.h index 75e60c8486..150e2ff361 100644 --- a/components/bt/esp_ble_mesh/mesh_core/net.h +++ b/components/bt/esp_ble_mesh/mesh_core/net.h @@ -115,6 +115,12 @@ struct bt_mesh_friend { struct bt_mesh_friend_seg { sys_slist_t queue; + + /* The target number of segments, i.e. not necessarily + * the current number of segments, in the queue. This is + * used for Friend Queue free space calculations. + */ + u8_t seg_count; } seg[FRIEND_SEG_RX]; struct net_buf *last; diff --git a/components/bt/esp_ble_mesh/mesh_core/transport.c b/components/bt/esp_ble_mesh/mesh_core/transport.c index 8246d3316a..19a3b0c668 100644 --- a/components/bt/esp_ble_mesh/mesh_core/transport.c +++ b/components/bt/esp_ble_mesh/mesh_core/transport.c @@ -141,8 +141,21 @@ static int send_unseg(struct bt_mesh_net_tx *tx, struct net_buf_simple *sdu, if (IS_ENABLED(CONFIG_BLE_MESH_NODE) && bt_mesh_is_provisioned()) { if (IS_ENABLED(CONFIG_BLE_MESH_FRIEND)) { + if (!bt_mesh_friend_queue_has_space(tx->sub->net_idx, + tx->src, tx->ctx->addr, + NULL, 1)) { + if (BLE_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { + BT_ERR("Not enough space in Friend Queue"); + net_buf_unref(buf); + return -ENOBUFS; + } else { + BT_WARN("No space in Friend Queue"); + goto send; + } + } + if (bt_mesh_friend_enqueue_tx(tx, BLE_MESH_FRIEND_PDU_SINGLE, - NULL, &buf->b) && + NULL, 1, &buf->b) && BLE_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { /* PDUs for a specific Friend should only go * out through the Friend Queue. @@ -154,6 +167,7 @@ static int send_unseg(struct bt_mesh_net_tx *tx, struct net_buf_simple *sdu, } } +send: return bt_mesh_net_send(tx, buf, cb, cb_data); } @@ -365,6 +379,17 @@ static int send_seg(struct bt_mesh_net_tx *net_tx, struct net_buf_simple *sdu, BT_DBG("SeqZero 0x%04x", seq_zero); + if (IS_ENABLED(CONFIG_BLE_MESH_FRIEND) && + !bt_mesh_friend_queue_has_space(tx->sub->net_idx, net_tx->src, + tx->dst, &tx->seq_auth, + tx->seg_n + 1) && + BLE_MESH_ADDR_IS_UNICAST(tx->dst)) { + BT_ERR("Not enough space in Friend Queue for %u segments", + tx->seg_n + 1); + seg_tx_reset(tx); + return -ENOBUFS; + } + for (seg_o = 0U; sdu->len; seg_o++) { struct net_buf *seg; u16_t len; @@ -404,8 +429,9 @@ static int send_seg(struct bt_mesh_net_tx *net_tx, struct net_buf_simple *sdu, if (bt_mesh_friend_enqueue_tx(net_tx, type, &tx->seq_auth, + tx->seg_n + 1, &seg->b) && - BLE_MESH_ADDR_IS_UNICAST(net_tx->ctx->addr)) { + BLE_MESH_ADDR_IS_UNICAST(net_tx->ctx->addr)) { /* PDUs for a specific Friend should only go * out through the Friend Queue. */ @@ -1029,8 +1055,8 @@ int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data, if (IS_ENABLED(CONFIG_BLE_MESH_FRIEND)) { if (bt_mesh_friend_enqueue_tx(tx, BLE_MESH_FRIEND_PDU_SINGLE, - seq_auth, &buf->b) && - BLE_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { + seq_auth, 1, &buf->b) && + BLE_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { /* PDUs for a specific Friend should only go * out through the Friend Queue. */ @@ -1237,7 +1263,8 @@ static struct seg_rx *seg_rx_alloc(struct bt_mesh_net_rx *net_rx, } static int trans_seg(struct net_buf_simple *buf, struct bt_mesh_net_rx *net_rx, - enum bt_mesh_friend_pdu_type *pdu_type, u64_t *seq_auth) + enum bt_mesh_friend_pdu_type *pdu_type, u64_t *seq_auth, + u8_t *seg_count) { struct bt_mesh_rpl *rpl = NULL; struct seg_rx *rx; @@ -1294,6 +1321,8 @@ static int trans_seg(struct net_buf_simple *buf, struct bt_mesh_net_rx *net_rx, ((((net_rx->seq & BIT_MASK(14)) - seq_zero)) & BIT_MASK(13)))); + *seg_count = seg_n + 1; + /* Look for old RX sessions */ rx = seg_rx_find(net_rx, seq_auth); if (rx) { @@ -1342,6 +1371,22 @@ static int trans_seg(struct net_buf_simple *buf, struct bt_mesh_net_rx *net_rx, return -EMSGSIZE; } + /* Verify early that there will be space in the Friend Queue(s) in + * case this message is destined to an LPN of ours. + */ + if (IS_ENABLED(CONFIG_BLE_MESH_FRIEND) && + net_rx->friend_match && !net_rx->local_match && + !bt_mesh_friend_queue_has_space(net_rx->sub->net_idx, + net_rx->ctx.addr, + net_rx->ctx.recv_dst, seq_auth, + *seg_count)) { + BT_ERR("No space in Friend Queue for %u segments", *seg_count); + send_ack(net_rx->sub, net_rx->ctx.recv_dst, net_rx->ctx.addr, + net_rx->ctx.send_ttl, seq_auth, 0, + net_rx->friend_match); + return -ENOBUFS; + } + /* Look for free slot for a new RX session */ rx = seg_rx_alloc(net_rx, hdr, seq_auth, seg_n); if (!rx) { @@ -1436,6 +1481,7 @@ int bt_mesh_trans_recv(struct net_buf_simple *buf, struct bt_mesh_net_rx *rx) u64_t seq_auth = TRANS_SEQ_AUTH_NVAL; enum bt_mesh_friend_pdu_type pdu_type = BLE_MESH_FRIEND_PDU_SINGLE; struct net_buf_simple_state state; + u8_t seg_count = 0U; int err; if (IS_ENABLED(CONFIG_BLE_MESH_FRIEND)) { @@ -1479,8 +1525,9 @@ int bt_mesh_trans_recv(struct net_buf_simple *buf, struct bt_mesh_net_rx *rx) return 0; } - err = trans_seg(buf, rx, &pdu_type, &seq_auth); + err = trans_seg(buf, rx, &pdu_type, &seq_auth, &seg_count); } else { + seg_count = 1U; err = trans_unseg(buf, rx, &seq_auth); } @@ -1508,9 +1555,11 @@ int bt_mesh_trans_recv(struct net_buf_simple *buf, struct bt_mesh_net_rx *rx) if (IS_ENABLED(CONFIG_BLE_MESH_NODE) && bt_mesh_is_provisioned()) { if (IS_ENABLED(CONFIG_BLE_MESH_FRIEND) && rx->friend_match && !err) { if (seq_auth == TRANS_SEQ_AUTH_NVAL) { - bt_mesh_friend_enqueue_rx(rx, pdu_type, NULL, buf); + bt_mesh_friend_enqueue_rx(rx, pdu_type, NULL, + seg_count, buf); } else { - bt_mesh_friend_enqueue_rx(rx, pdu_type, &seq_auth, buf); + bt_mesh_friend_enqueue_rx(rx, pdu_type, &seq_auth, + seg_count, buf); } } } -- 2.40.0