Bluetooth: Mesh: Fix adhering to the configured Friend Queue size

Qualification test case MESH/NODE/FRND/FN/BV-08-C requires that we do
not store more messages than the reported Friend Queue size. The
implementation was so far opportunistic and stored more if it could
(it would later discard if necessary to make sure all queues can store
the required amount). The spec also requires the queues to have new
messages overwrite old ones (in the style of a circular buffer), so we
have to keep track of which buffers are part of the same segmented
message (so we discard all buffers belonging to the same message).

To pass the test case, add APIs to check for space in the Friend
queue, and track the number of buffers for each incoming segmented
message.

Fixes #18090

Signed-off-by: Johan Hedberg <[email protected]>

Author: Johan Hedberg

subsys/bluetooth/host/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) {
@@ -895,7 +872,8 @@ int bt_mesh_friend_req(struct bt_mesh_net_rx *rx, struct net_buf_simple *buf)
 }
 
 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;
@@ -914,12 +892,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;
@@ -936,7 +918,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);
@@ -961,6 +943,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;
 	}
 }
 
@@ -1026,13 +1012,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--;
@@ -1095,7 +1085,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;
@@ -1133,7 +1124,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);
@@ -1142,7 +1133,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;
@@ -1177,7 +1169,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);
 }
@@ -1227,9 +1219,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;
 
@@ -1246,16 +1347,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;
@@ -1271,10 +1381,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;
@@ -1314,9 +1433,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;
 		}
 	}
 }