diff --git a/subsys/bluetooth/mesh/friend.c b/subsys/bluetooth/mesh/friend.c index 3a7da4bb568..330287efcc0 100644 --- a/subsys/bluetooth/mesh/friend.c +++ b/subsys/bluetooth/mesh/friend.c @@ -68,48 +68,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, - BT_MESH_ADV_DATA, - FRIEND_XMIT, K_NO_WAIT); - if (!buf) { - discard_buffer(); - } - } while (!buf); - - BT_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 == BT_MESH_ADDR_UNASSIGNED) { @@ -149,6 +107,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 @@ -183,16 +155,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; @@ -334,7 +303,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, + BT_MESH_ADV_DATA, + FRIEND_XMIT, K_NO_WAIT); + if (!buf) { + return NULL; + } + + BT_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) { @@ -896,7 +873,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; @@ -915,12 +893,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; @@ -937,7 +919,7 @@ static void enqueue_friend_pdu(struct bt_mesh_friend *frnd, } adv = FRIEND_ADV(buf); - seg = get_seg(frnd, BT_MESH_ADV(buf)->addr, &adv->seq_auth); + seg = get_seg(frnd, BT_MESH_ADV(buf)->addr, &adv->seq_auth, seg_count); if (!seg) { BT_ERR("No free friend segment RX contexts for 0x%04x", BT_MESH_ADV(buf)->addr); @@ -962,6 +944,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; } } @@ -1027,13 +1013,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("Friendship not established with 0x%04x", 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--; @@ -1096,7 +1086,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; @@ -1134,7 +1125,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); @@ -1143,7 +1134,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; @@ -1178,7 +1170,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); } @@ -1228,9 +1220,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 = 0; + int i; + + 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 && BT_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_BT_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; + + if (seg_count > CONFIG_BT_MESH_FRIEND_QUEUE_SIZE) { + return false; + } + + 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_BT_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; @@ -1247,16 +1348,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; @@ -1272,10 +1382,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; @@ -1315,9 +1434,8 @@ void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src, BT_WARN("Clearing incomplete segments for 0x%04x", 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/subsys/bluetooth/mesh/friend.h b/subsys/bluetooth/mesh/friend.h index 1e6147f3d3e..7832dfa1035 100644 --- a/subsys/bluetooth/mesh/friend.h +++ b/subsys/bluetooth/mesh/friend.h @@ -17,12 +17,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/subsys/bluetooth/mesh/net.h b/subsys/bluetooth/mesh/net.h index da953db0dc6..dc3f939b1b8 100644 --- a/subsys/bluetooth/mesh/net.h +++ b/subsys/bluetooth/mesh/net.h @@ -107,6 +107,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/subsys/bluetooth/mesh/transport.c b/subsys/bluetooth/mesh/transport.c index 5cf722a8b63..a859608d1f7 100644 --- a/subsys/bluetooth/mesh/transport.c +++ b/subsys/bluetooth/mesh/transport.c @@ -140,8 +140,21 @@ static int send_unseg(struct bt_mesh_net_tx *tx, struct net_buf_simple *sdu, net_buf_add_mem(buf, sdu->data, sdu->len); if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) { + if (!bt_mesh_friend_queue_has_space(tx->sub->net_idx, + tx->src, tx->ctx->addr, + NULL, 1)) { + if (BT_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, BT_MESH_FRIEND_PDU_SINGLE, - NULL, &buf->b) && + NULL, 1, &buf->b) && BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { /* PDUs for a specific Friend should only go * out through the Friend Queue. @@ -152,6 +165,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); } @@ -357,6 +371,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_BT_MESH_FRIEND) && + !bt_mesh_friend_queue_has_space(tx->sub->net_idx, net_tx->src, + tx->dst, &tx->seq_auth, + tx->seg_n + 1) && + BT_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; @@ -395,6 +420,7 @@ 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) && BT_MESH_ADDR_IS_UNICAST(net_tx->ctx->addr)) { /* PDUs for a specific Friend should only go @@ -959,7 +985,7 @@ int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data, if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) { if (bt_mesh_friend_enqueue_tx(tx, BT_MESH_FRIEND_PDU_SINGLE, - seq_auth, &buf->b) && + seq_auth, 1, &buf->b) && BT_MESH_ADDR_IS_UNICAST(tx->ctx->addr)) { /* PDUs for a specific Friend should only go * out through the Friend Queue. @@ -1169,7 +1195,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; @@ -1226,6 +1253,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) { @@ -1275,6 +1304,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_BT_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) { @@ -1369,6 +1414,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 = BT_MESH_FRIEND_PDU_SINGLE; struct net_buf_simple_state state; + u8_t seg_count = 0; int err; if (IS_ENABLED(CONFIG_BT_MESH_FRIEND)) { @@ -1415,8 +1461,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 = 1; err = trans_unseg(buf, rx, &seq_auth); } @@ -1441,9 +1488,11 @@ int bt_mesh_trans_recv(struct net_buf_simple *buf, struct bt_mesh_net_rx *rx) if (IS_ENABLED(CONFIG_BT_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); } }