1 files changed, 86 insertions, 58 deletions

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index fcd09fb4b94c..2b30bd767779 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1634,6 +1634,7 @@ static void l2cap_streaming_send(struct l2cap_chan *chan)
         while ((skb = skb_dequeue(&chan->tx_q))) {
                 control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
                 control |= __set_txseq(chan, chan->next_tx_seq);
+                control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
                 __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
 
                 if (chan->fcs == L2CAP_FCS_CRC16) {
@@ -1706,6 +1707,9 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
         if (chan->state != BT_CONNECTED)
                 return -ENOTCONN;
 
+        if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
+                return 0;
+
         while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
 
                 if (bt_cb(skb)->control.retries == chan->remote_max_tx &&
@@ -1726,6 +1730,7 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
 
                 control |= __set_reqseq(chan, chan->buffer_seq);
                 control |= __set_txseq(chan, chan->next_tx_seq);
+                control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar);
 
                 __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
 
@@ -1921,7 +1926,7 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
 
 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
                                                 struct msghdr *msg, size_t len,
-                                                u32 control, u16 sdulen)
+                                                u16 sdulen)
 {
         struct l2cap_conn *conn = chan->conn;
         struct sk_buff *skb;
@@ -1956,7 +1961,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
         lh->cid = cpu_to_le16(chan->dcid);
         lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
 
-        __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
+        __put_control(chan, 0, skb_put(skb, __ctrl_size(chan)));
 
         if (sdulen)
                 put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
@@ -1974,57 +1979,78 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
         return skb;
 }
 
-static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+static int l2cap_segment_sdu(struct l2cap_chan *chan,
+                             struct sk_buff_head *seg_queue,
+                             struct msghdr *msg, size_t len)
 {
         struct sk_buff *skb;
-        struct sk_buff_head sar_queue;
-        u32 control;
-        size_t size = 0;
+        u16 sdu_len;
+        size_t pdu_len;
+        int err = 0;
+        u8 sar;
 
-        skb_queue_head_init(&sar_queue);
-        control = __set_ctrl_sar(chan, L2CAP_SAR_START);
-        skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len);
-        if (IS_ERR(skb))
-                return PTR_ERR(skb);
+        BT_DBG("chan %p, msg %p, len %d", chan, msg, (int)len);
 
-        __skb_queue_tail(&sar_queue, skb);
-        len -= chan->remote_mps;
-        size += chan->remote_mps;
+        /* It is critical that ERTM PDUs fit in a single HCI fragment,
+         * so fragmented skbs are not used.  The HCI layer's handling
+         * of fragmented skbs is not compatible with ERTM's queueing.
+         */
 
-        while (len > 0) {
-                size_t buflen;
+        /* PDU size is derived from the HCI MTU */
+        pdu_len = chan->conn->mtu;
 
-                if (len > chan->remote_mps) {
-                        control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
-                        buflen = chan->remote_mps;
-                } else {
-                        control = __set_ctrl_sar(chan, L2CAP_SAR_END);
-                        buflen = len;
-                }
+        pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD);
+
+        /* Adjust for largest possible L2CAP overhead. */
+        pdu_len -= L2CAP_EXT_HDR_SIZE + L2CAP_FCS_SIZE;
+
+        /* Remote device may have requested smaller PDUs */
+        pdu_len = min_t(size_t, pdu_len, chan->remote_mps);
+
+        if (len <= pdu_len) {
+                sar = L2CAP_SAR_UNSEGMENTED;
+                sdu_len = 0;
+                pdu_len = len;
+        } else {
+                sar = L2CAP_SAR_START;
+                sdu_len = len;
+                pdu_len -= L2CAP_SDULEN_SIZE;
+        }
+
+        while (len > 0) {
+                skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len);
 
-                skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0);
                 if (IS_ERR(skb)) {
-                        skb_queue_purge(&sar_queue);
+                        __skb_queue_purge(seg_queue);
                         return PTR_ERR(skb);
                 }
 
-                __skb_queue_tail(&sar_queue, skb);
-                len -= buflen;
-                size += buflen;
+                bt_cb(skb)->control.sar = sar;
+                __skb_queue_tail(seg_queue, skb);
+
+                len -= pdu_len;
+                if (sdu_len) {
+                        sdu_len = 0;
+                        pdu_len += L2CAP_SDULEN_SIZE;
+                }
+
+                if (len <= pdu_len) {
+                        sar = L2CAP_SAR_END;
+                        pdu_len = len;
+                } else {
+                        sar = L2CAP_SAR_CONTINUE;
+                }
         }
-        skb_queue_splice_tail(&sar_queue, &chan->tx_q);
-        if (chan->tx_send_head == NULL)
-                chan->tx_send_head = sar_queue.next;
 
-        return size;
+        return err;
 }
 
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
                                                                 u32 priority)
 {
         struct sk_buff *skb;
-        u32 control;
         int err;
+        struct sk_buff_head seg_queue;
 
         /* Connectionless channel */
         if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
@@ -2053,42 +2079,44 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 
         case L2CAP_MODE_ERTM:
         case L2CAP_MODE_STREAMING:
-                /* Entire SDU fits into one PDU */
-                if (len <= chan->remote_mps) {
-                        control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
-                        skb = l2cap_create_iframe_pdu(chan, msg, len, control,
-                                                                        0);
-                        if (IS_ERR(skb))
-                                return PTR_ERR(skb);
+                /* Check outgoing MTU */
+                if (len > chan->omtu) {
+                        err = -EMSGSIZE;
+                        break;
+                }
 
-                        __skb_queue_tail(&chan->tx_q, skb);
+                __skb_queue_head_init(&seg_queue);
 
-                        if (chan->tx_send_head == NULL)
-                                chan->tx_send_head = skb;
+                /* Do segmentation before calling in to the state machine,
+                 * since it's possible to block while waiting for memory
+                 * allocation.
+                 */
+                err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
 
-                } else {
-                        /* Segment SDU into multiples PDUs */
-                        err = l2cap_sar_segment_sdu(chan, msg, len);
-                        if (err < 0)
-                                return err;
+                /* The channel could have been closed while segmenting,
+                 * check that it is still connected.
+                 */
+                if (chan->state != BT_CONNECTED) {
+                        __skb_queue_purge(&seg_queue);
+                        err = -ENOTCONN;
                 }
 
-                if (chan->mode == L2CAP_MODE_STREAMING) {
-                        l2cap_streaming_send(chan);
-                        err = len;
+                if (err)
                         break;
-                }
 
-                if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
-                                test_bit(CONN_WAIT_F, &chan->conn_state)) {
-                        err = len;
-                        break;
-                }
+                skb_queue_splice_tail_init(&seg_queue, &chan->tx_q);
+                if (chan->mode == L2CAP_MODE_ERTM)
+                        err = l2cap_ertm_send(chan);
+                else
+                        l2cap_streaming_send(chan);
 
-                err = l2cap_ertm_send(chan);
                 if (err >= 0)
                         err = len;
 
+                /* If the skbs were not queued for sending, they'll still be in
+                 * seg_queue and need to be purged.
+                 */
+                __skb_queue_purge(&seg_queue);
                 break;
 
         default: