Bluetooth: L2CAP: Move functions in preparation for queuing

This moves necessary functions that will be needed for queuing packets
while waiting for more credits.

Jira: ZEP-1776

Change-Id: I030c696d432ec5be1b8e6b649e953da145929777
Signed-off-by: Luiz Augusto von Dentz <[email protected]>

Author: Vudentz

subsys/bluetooth/host/l2cap.c

@@ -998,6 +998,134 @@ static void le_disconn_rsp(struct bt_l2cap *l2cap, uint8_t ident,
 	bt_l2cap_chan_del(&chan->chan);
 }
 
+static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch,
+					     struct net_buf *buf,
+					     size_t sdu_hdr_len)
+{
+	struct net_buf *seg;
+	uint16_t headroom;
+	uint16_t len;
+
+	/* Segment if data (+ data headroom) is bigger than MPS */
+	if (buf->len + sdu_hdr_len > ch->tx.mps) {
+		goto segment;
+	}
+
+	/* Segment if there is no space in the user_data */
+	if (buf->pool->user_data_size < BT_BUF_USER_DATA_MIN) {
+		BT_WARN("Too small buffer user_data_size %u",
+			buf->pool->user_data_size);
+		goto segment;
+	}
+
+	headroom = sizeof(struct bt_hci_acl_hdr) +
+		   sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
+
+	/* Check if original buffer has enough headroom and don't have any
+	 * fragments.
+	 */
+	if (net_buf_headroom(buf) >= headroom && !buf->frags) {
+		if (sdu_hdr_len) {
+			/* Push SDU length if set */
+			net_buf_push_le16(buf, net_buf_frags_len(buf));
+		}
+		return net_buf_ref(buf);
+	}
+
+segment:
+	seg = bt_l2cap_create_pdu(&le_data_pool, 0);
+
+	if (sdu_hdr_len) {
+		net_buf_add_le16(seg, net_buf_frags_len(buf));
+	}
+
+	/* Don't send more that TX MPS including SDU length */
+	len = min(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len);
+	/* Limit if original buffer is smaller than the segment */
+	len = min(buf->len, len);
+	net_buf_add_mem(seg, buf->data, len);
+	net_buf_pull(buf, len);
+
+	BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
+
+	return seg;
+}
+
+static int l2cap_chan_le_send(struct bt_l2cap_le_chan *ch, struct net_buf *buf,
+			      uint16_t sdu_hdr_len)
+{
+	int len;
+
+	/* Wait for credits */
+	if (k_sem_take(&ch->tx.credits, K_NO_WAIT)) {
+		BT_DBG("No credits to transmit packet");
+		return -EAGAIN;
+	}
+
+	buf = l2cap_chan_create_seg(ch, buf, sdu_hdr_len);
+	if (!buf) {
+		return -ENOMEM;
+	}
+
+	/* Channel may have been disconnected while waiting for credits */
+	if (!ch->chan.conn) {
+		net_buf_unref(buf);
+		return -ECONNRESET;
+	}
+
+	BT_DBG("ch %p cid 0x%04x len %u credits %u", ch, ch->tx.cid,
+	       buf->len, k_sem_count_get(&ch->tx.credits));
+
+	len = buf->len;
+
+	bt_l2cap_send(ch->chan.conn, ch->tx.cid, buf);
+
+	return len;
+}
+
+static int l2cap_chan_le_send_sdu(struct bt_l2cap_le_chan *ch,
+				  struct net_buf *buf)
+{
+	int ret, sent, total_len;
+	struct net_buf *frag;
+
+	total_len = net_buf_frags_len(buf);
+
+	if (total_len > ch->tx.mtu) {
+		return -EMSGSIZE;
+	}
+
+	frag = buf;
+	if (!frag->len && frag->frags) {
+		frag = frag->frags;
+	}
+
+	/* Add SDU length for the first segment */
+	ret = l2cap_chan_le_send(ch, frag, BT_L2CAP_SDU_HDR_LEN);
+	if (ret < 0) {
+		return ret;
+	}
+
+	/* Send remaining segments */
+	for (sent = ret; sent < total_len; sent += ret) {
+		/* Proceed to next fragment */
+		if (!frag->len) {
+			frag = net_buf_frag_del(buf, frag);
+		}
+
+		ret = l2cap_chan_le_send(ch, frag, 0);
+		if (ret < 0) {
+			return ret;
+		}
+	}
+
+	BT_DBG("ch %p cid 0x%04x sent %u", ch, ch->tx.cid, sent);
+
+	net_buf_unref(buf);
+
+	return ret;
+}
+
 static void le_credits(struct bt_l2cap *l2cap, uint8_t ident,
 		       struct net_buf *buf)
 {
@@ -1499,131 +1627,6 @@ int bt_l2cap_chan_disconnect(struct bt_l2cap_chan *chan)
 	return 0;
 }
 
-static struct net_buf *l2cap_chan_create_seg(struct bt_l2cap_le_chan *ch,
-					     struct net_buf *buf,
-					     size_t sdu_hdr_len)
-{
-	struct net_buf *seg;
-	uint16_t headroom;
-	uint16_t len;
-
-	/* Segment if data (+ data headroom) is bigger than MPS */
-	if (buf->len + sdu_hdr_len > ch->tx.mps) {
-		goto segment;
-	}
-
-	/* Segment if there is no space in the user_data */
-	if (buf->pool->user_data_size < BT_BUF_USER_DATA_MIN) {
-		BT_WARN("Too small buffer user_data_size %u",
-			buf->pool->user_data_size);
-		goto segment;
-	}
-
-	headroom = sizeof(struct bt_hci_acl_hdr) +
-		   sizeof(struct bt_l2cap_hdr) + sdu_hdr_len;
-
-	/* Check if original buffer has enough headroom and don't have any
-	 * fragments.
-	 */
-	if (net_buf_headroom(buf) >= headroom && !buf->frags) {
-		if (sdu_hdr_len) {
-			/* Push SDU length if set */
-			net_buf_push_le16(buf, net_buf_frags_len(buf));
-		}
-		return net_buf_ref(buf);
-	}
-
-segment:
-	seg = bt_l2cap_create_pdu(&le_data_pool, 0);
-
-	if (sdu_hdr_len) {
-		net_buf_add_le16(seg, net_buf_frags_len(buf));
-	}
-
-	/* Don't send more that TX MPS including SDU length */
-	len = min(net_buf_tailroom(seg), ch->tx.mps - sdu_hdr_len);
-	/* Limit if original buffer is smaller than the segment */
-	len = min(buf->len, len);
-	net_buf_add_mem(seg, buf->data, len);
-	net_buf_pull(buf, len);
-
-	BT_DBG("ch %p seg %p len %u", ch, seg, seg->len);
-
-	return seg;
-}
-
-static int l2cap_chan_le_send(struct bt_l2cap_le_chan *ch, struct net_buf *buf,
-			      uint16_t sdu_hdr_len)
-{
-	int len;
-
-	/* Wait for credits */
-	k_sem_take(&ch->tx.credits, K_FOREVER);
-
-	buf = l2cap_chan_create_seg(ch, buf, sdu_hdr_len);
-	if (!buf) {
-		return -ENOMEM;
-	}
-
-	/* Channel may have been disconnected while waiting for credits */
-	if (!ch->chan.conn) {
-		net_buf_unref(buf);
-		return -ECONNRESET;
-	}
-
-	BT_DBG("ch %p cid 0x%04x len %u credits %u", ch, ch->tx.cid,
-	       buf->len, k_sem_count_get(&ch->tx.credits));
-
-	len = buf->len;
-
-	bt_l2cap_send(ch->chan.conn, ch->tx.cid, buf);
-
-	return len;
-}
-
-static int l2cap_chan_le_send_sdu(struct bt_l2cap_le_chan *ch,
-				  struct net_buf *buf)
-{
-	int ret, sent, total_len;
-	struct net_buf *frag;
-
-	total_len = net_buf_frags_len(buf);
-
-	if (total_len > ch->tx.mtu) {
-		return -EMSGSIZE;
-	}
-
-	frag = buf;
-	if (!frag->len && frag->frags) {
-		frag = frag->frags;
-	}
-
-	/* Add SDU length for the first segment */
-	ret = l2cap_chan_le_send(ch, frag, BT_L2CAP_SDU_HDR_LEN);
-	if (ret < 0) {
-		return ret;
-	}
-
-	/* Send remaining segments */
-	for (sent = ret; sent < total_len; sent += ret) {
-		/* Proceed to next fragment */
-		if (!frag->len) {
-			frag = net_buf_frag_del(buf, frag);
-		}
-
-		ret = l2cap_chan_le_send(ch, frag, 0);
-		if (ret < 0) {
-			return ret;
-		}
-	}
-
-	BT_DBG("ch %p cid 0x%04x sent %u", ch, ch->tx.cid, sent);
-
-	net_buf_unref(buf);
-
-	return sent;
-}
-
 int bt_l2cap_chan_send(struct bt_l2cap_chan *chan, struct net_buf *buf)
 {
 	int err;