1 files changed, 112 insertions, 7 deletions

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 1c35c328181d..cfd672419315 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -3329,12 +3329,111 @@ static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_
         __skb_queue_tail(SREJ_QUEUE(sk), skb);
 }
 
-static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
+static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *_skb;
+        int err = 0;
+
+        switch (control & L2CAP_CTRL_SAR) {
+        case L2CAP_SDU_UNSEGMENTED:
+                if (pi->conn_state & L2CAP_CONN_SAR_SDU)
+                        goto drop;
+
+                err = sock_queue_rcv_skb(sk, skb);
+                if (!err)
+                        return err;
+
+                break;
+
+        case L2CAP_SDU_START:
+                if (pi->conn_state & L2CAP_CONN_SAR_SDU)
+                        goto drop;
+
+                pi->sdu_len = get_unaligned_le16(skb->data);
+                skb_pull(skb, 2);
+
+                if (pi->sdu_len > pi->imtu)
+                        goto disconnect;
+
+                pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
+                if (!pi->sdu) {
+                        err = -ENOMEM;
+                        break;
+                }
+
+                memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+                pi->conn_state |= L2CAP_CONN_SAR_SDU;
+                pi->partial_sdu_len = skb->len;
+                break;
+
+        case L2CAP_SDU_CONTINUE:
+                if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+                        goto disconnect;
+
+                if (!pi->sdu)
+                        goto disconnect;
+
+                memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+                pi->partial_sdu_len += skb->len;
+                if (pi->partial_sdu_len > pi->sdu_len)
+                        goto drop;
+
+                break;
+
+        case L2CAP_SDU_END:
+                if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+                        goto disconnect;
+
+                if (!pi->sdu)
+                        goto disconnect;
+
+                memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+                pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
+                pi->partial_sdu_len += skb->len;
+
+                if (pi->partial_sdu_len > pi->imtu)
+                        goto drop;
+
+                if (pi->partial_sdu_len != pi->sdu_len)
+                        goto drop;
+
+                _skb = skb_clone(pi->sdu, GFP_ATOMIC);
+                err = sock_queue_rcv_skb(sk, _skb);
+                if (err < 0)
+                        kfree_skb(_skb);
+
+                kfree_skb(pi->sdu);
+                break;
+        }
+
+        kfree_skb(skb);
+        return err;
+
+drop:
+        kfree_skb(pi->sdu);
+        pi->sdu = NULL;
+
+disconnect:
+        l2cap_send_disconn_req(pi->conn, sk);
+        kfree_skb(skb);
+        return 0;
+}
+
+static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
 {
         struct l2cap_pinfo *pi = l2cap_pi(sk);
         struct sk_buff *_skb;
         int err = -EINVAL;
 
+        /*
+         * TODO: We have to notify the userland if some data is lost with the
+         * Streaming Mode.
+         */
+
         switch (control & L2CAP_CTRL_SAR) {
         case L2CAP_SDU_UNSEGMENTED:
                 if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
@@ -3429,7 +3528,7 @@ static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
 
                 skb = skb_dequeue(SREJ_QUEUE(sk));
                 control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
-                l2cap_sar_reassembly_sdu(sk, skb, control);
+                l2cap_ertm_reassembly_sdu(sk, skb, control);
                 l2cap_pi(sk)->buffer_seq_srej =
                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
                 tx_seq++;
@@ -3566,7 +3665,7 @@ expected:
 
         pi->buffer_seq = (pi->buffer_seq + 1) % 64;
 
-        err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
+        err = l2cap_ertm_reassembly_sdu(sk, skb, rx_control);
         if (err < 0)
                 return err;
 
@@ -3790,8 +3889,10 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                  * Receiver will miss it and start proper recovery
                  * procedures and ask retransmission.
                  */
-                if (len > pi->mps)
+                if (len > pi->mps) {
+                        l2cap_send_disconn_req(pi->conn, sk);
                         goto drop;
+                }
 
                 if (l2cap_check_fcs(pi, skb))
                         goto drop;
@@ -3813,13 +3914,17 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                 }
 
                 if (__is_iframe(control)) {
-                        if (len < 4)
+                        if (len < 4) {
+                                l2cap_send_disconn_req(pi->conn, sk);
                                 goto drop;
+                        }
 
                         l2cap_data_channel_iframe(sk, control, skb);
                 } else {
-                        if (len != 0)
+                        if (len != 0) {
+                                l2cap_send_disconn_req(pi->conn, sk);
                                 goto drop;
+                        }
 
                         l2cap_data_channel_sframe(sk, control, skb);
                 }
@@ -3850,7 +3955,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                 else
                         pi->expected_tx_seq = (tx_seq + 1) % 64;
 
-                l2cap_sar_reassembly_sdu(sk, skb, control);
+                l2cap_streaming_reassembly_sdu(sk, skb, control);
 
                 goto done;