[Backport v1.14-branch] Bluetooth: Mesh: Fix adhering to the configured Friend Queue size

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;
+
+	if (seg_count > CONFIG_BT_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 && 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;
+
+	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;
 		}
 	}
 }