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