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 <luiz.von.dentz@intel.com>
This commit is contained in:
Luiz Augusto von Dentz 2017-02-16 16:37:41 +02:00 committed by Johan Hedberg
commit 03ff07ea7f

View file

@ -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;