3 files changed, 390 insertions, 51 deletions
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 968166a45f86..65a5cf868fd6 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -138,8 +138,9 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock);
 struct bt_skb_cb {
        __u8 pkt_type;
        __u8 incoming;
+        __u8 tx_seq;
 };
-#define bt_cb(skb) ((struct bt_skb_cb *)(skb->cb)) 
+#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
 static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)
 {
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 6fc76986d70a..9bbfbe74d400 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -29,7 +29,8 @@
 #define L2CAP_DEFAULT_MTU               672
 #define L2CAP_DEFAULT_MIN_MTU           48
 #define L2CAP_DEFAULT_FLUSH_TO          0xffff
-#define L2CAP_DEFAULT_TX_WINDOW         1
+#define L2CAP_DEFAULT_TX_WINDOW         63
+#define L2CAP_DEFAULT_NUM_TO_ACK        (L2CAP_DEFAULT_TX_WINDOW/5)
 #define L2CAP_DEFAULT_MAX_RECEIVE       1
 #define L2CAP_DEFAULT_RETRANS_TO        300    /* 300 milliseconds */
 #define L2CAP_DEFAULT_MONITOR_TO        1000   /* 1 second */
@@ -94,6 +95,31 @@ struct l2cap_conninfo {
 #define L2CAP_FCS_NONE          0x00
 #define L2CAP_FCS_CRC16         0x01
+/* L2CAP Control Field bit masks */
+#define L2CAP_CTRL_SAR               0xC000
+#define L2CAP_CTRL_REQSEQ            0x3F00
+#define L2CAP_CTRL_TXSEQ             0x007E
+#define L2CAP_CTRL_RETRANS           0x0080
+#define L2CAP_CTRL_FINAL             0x0080
+#define L2CAP_CTRL_POLL              0x0010
+#define L2CAP_CTRL_SUPERVISE         0x000C
+#define L2CAP_CTRL_FRAME_TYPE        0x0001 /* I- or S-Frame */
+#define L2CAP_CTRL_TXSEQ_SHIFT      1
+#define L2CAP_CTRL_REQSEQ_SHIFT     8
+/* L2CAP Supervisory Function */
+#define L2CAP_SUPER_RCV_READY           0x0000
+#define L2CAP_SUPER_REJECT              0x0004
+#define L2CAP_SUPER_RCV_NOT_READY       0x0008
+#define L2CAP_SUPER_SELECT_REJECT       0x000C
+/* L2CAP Segmentation and Reassembly */
+#define L2CAP_SDU_UNSEGMENTED       0x0000
+#define L2CAP_SDU_START             0x4000
+#define L2CAP_SDU_END               0x8000
+#define L2CAP_SDU_CONTINUE          0xC000
 /* L2CAP structures */
 struct l2cap_hdr {
        __le16     len;
@@ -262,6 +288,7 @@ struct l2cap_conn {
 /* ----- L2CAP channel and socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
+#define TX_QUEUE(sk) (&l2cap_pi(sk)->tx_queue)
 struct l2cap_pinfo {
        struct bt_sock  bt;
@@ -285,6 +312,13 @@ struct l2cap_pinfo {
        __u8            conf_len;
        __u8            conf_state;
+        __u8            next_tx_seq;
+        __u8            expected_ack_seq;
+        __u8            req_seq;
+        __u8            expected_tx_seq;
+        __u8            unacked_frames;
+        __u8            num_to_ack;
        __u8            ident;
        __u8            remote_tx_win;
@@ -295,6 +329,7 @@ struct l2cap_pinfo {
        __le16          sport;
+        struct sk_buff_head     tx_queue;
        struct l2cap_conn       *conn;
        struct sock             *next_c;
        struct sock             *prev_c;
@@ -311,6 +346,23 @@ struct l2cap_pinfo {
 #define L2CAP_CONF_MAX_CONF_REQ 2
 #define L2CAP_CONF_MAX_CONF_RSP 2
+static inline int l2cap_tx_window_full(struct sock *sk)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        int sub;
+        sub = (pi->next_tx_seq - pi->expected_ack_seq) % 64;
+        if (sub < 0)
+                sub += 64;
+        return (sub == pi->remote_tx_win);
+}
+#define __get_txseq(ctrl) ((ctrl) & L2CAP_CTRL_TXSEQ) >> 1
+#define __get_reqseq(ctrl) ((ctrl) & L2CAP_CTRL_REQSEQ) >> 8
+#define __is_iframe(ctrl) !((ctrl) & L2CAP_CTRL_FRAME_TYPE)
+#define __is_sframe(ctrl) (ctrl) & L2CAP_CTRL_FRAME_TYPE
 void l2cap_load(void);
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index c1b562085cb4..45b8697a7398 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -333,6 +333,30 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16
        return hci_send_acl(conn->hcon, skb, 0);
 }
+static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
+{
+        struct sk_buff *skb;
+        struct l2cap_hdr *lh;
+        struct l2cap_conn *conn = pi->conn;
+        int count;
+        BT_DBG("pi %p, control 0x%2.2x", pi, control);
+        count = min_t(unsigned int, conn->mtu, L2CAP_HDR_SIZE + 2);
+        control |= L2CAP_CTRL_FRAME_TYPE;
+        skb = bt_skb_alloc(count, GFP_ATOMIC);
+        if (!skb)
+                return -ENOMEM;
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->len = cpu_to_le16(2);
+        lh->cid = cpu_to_le16(pi->dcid);
+        put_unaligned_le16(control, skb_put(skb, 2));
+        return hci_send_acl(pi->conn->hcon, skb, 0);
+}
 static void l2cap_do_start(struct sock *sk)
 {
        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
@@ -1154,39 +1178,80 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
        return 0;
 }
-static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
+static void l2cap_drop_acked_frames(struct sock *sk)
 {
-        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
-        struct sk_buff *skb, **frag;
-        int err, hlen, count, sent = 0;
-        struct l2cap_hdr *lh;
-        BT_DBG("sk %p len %d", sk, len);
+        while ((skb = skb_peek(TX_QUEUE(sk)))) {
+                if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
+                        break;
-        /* First fragment (with L2CAP header) */
+                skb = skb_dequeue(TX_QUEUE(sk));
-        if (sk->sk_type == SOCK_DGRAM)
+                kfree_skb(skb);
-                hlen = L2CAP_HDR_SIZE + 2;
-        else
-                hlen = L2CAP_HDR_SIZE;
-        count = min_t(unsigned int, (conn->mtu - hlen), len);
+                l2cap_pi(sk)->unacked_frames--;
+        }
-        skb = bt_skb_send_alloc(sk, hlen + count,
+        return;
-                        msg->msg_flags & MSG_DONTWAIT, &err);
+}
-        if (!skb)
-                return err;
-        /* Create L2CAP header */
+static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
-        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+{
-        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
-        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        int err;
+        BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
+        err = hci_send_acl(pi->conn->hcon, skb, 0);
+        if (err < 0)
+                kfree_skb(skb);
+        return err;
+}
+static int l2cap_ertm_send(struct sock *sk)
+{
+        struct sk_buff *skb, *tx_skb;
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u16 control;
+        int err;
+        while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
+                tx_skb = skb_clone(skb, GFP_ATOMIC);
-        if (sk->sk_type == SOCK_DGRAM)
+                control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
-                put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
+                control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
+                                | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
+                put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+                err = l2cap_do_send(sk, tx_skb);
+                if (err < 0) {
+                        l2cap_send_disconn_req(pi->conn, sk);
+                        return err;
+                }
+                bt_cb(skb)->tx_seq = pi->next_tx_seq;
+                pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
+                pi->unacked_frames++;
+                if (skb_queue_is_last(TX_QUEUE(sk), skb))
+                        sk->sk_send_head = NULL;
+                else
+                        sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
+        }
+        return 0;
+}
+static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff **frag;
+        int err, sent = 0;
        if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
-                err = -EFAULT;
+                return -EFAULT;
-                goto fail;
        }
        sent += count;
@@ -1199,33 +1264,112 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
                *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
                if (!*frag)
-                        goto fail;
+                        return -EFAULT;
+                if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
-                if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
+                        return -EFAULT;
-                        err = -EFAULT;
-                        goto fail;
-                }
                sent += count;
                len  -= count;
                frag = &(*frag)->next;
        }
-        err = hci_send_acl(conn->hcon, skb, 0);
-        if (err < 0)
-                goto fail;
        return sent;
+}
-fail:
+static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
-        kfree_skb(skb);
+{
-        return err;
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
+        int err, count, hlen = L2CAP_HDR_SIZE + 2;
+        struct l2cap_hdr *lh;
+        BT_DBG("sk %p len %d", sk, (int)len);
+        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        skb = bt_skb_send_alloc(sk, count + hlen,
+                        msg->msg_flags & MSG_DONTWAIT, &err);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+        /* Create L2CAP header */
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
+        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+        if (unlikely(err < 0)) {
+                kfree_skb(skb);
+                return ERR_PTR(err);
+        }
+        return skb;
+}
+static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
+        int err, count, hlen = L2CAP_HDR_SIZE;
+        struct l2cap_hdr *lh;
+        BT_DBG("sk %p len %d", sk, (int)len);
+        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        skb = bt_skb_send_alloc(sk, count + hlen,
+                        msg->msg_flags & MSG_DONTWAIT, &err);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+        /* Create L2CAP header */
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+        if (unlikely(err < 0)) {
+                kfree_skb(skb);
+                return ERR_PTR(err);
+        }
+        return skb;
+}
+static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
+        int err, count, hlen = L2CAP_HDR_SIZE + 2;
+        struct l2cap_hdr *lh;
+        BT_DBG("sk %p len %d", sk, (int)len);
+        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        skb = bt_skb_send_alloc(sk, count + hlen,
+                        msg->msg_flags & MSG_DONTWAIT, &err);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+        /* Create L2CAP header */
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        put_unaligned_le16(control, skb_put(skb, 2));
+        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+        if (unlikely(err < 0)) {
+                kfree_skb(skb);
+                return ERR_PTR(err);
+        }
+        return skb;
 }
 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
 {
        struct sock *sk = sock->sk;
-        int err = 0;
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *skb;
+        u16 control;
+        int err;
        BT_DBG("sock %p, sk %p", sock, sk);
@@ -1237,16 +1381,67 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
                return -EOPNOTSUPP;
        /* Check outgoing MTU */
-        if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
+        if (sk->sk_type == SOCK_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
+                        && len > pi->omtu)
                return -EINVAL;
        lock_sock(sk);
-        if (sk->sk_state == BT_CONNECTED)
+        if (sk->sk_state != BT_CONNECTED) {
-                err = l2cap_do_send(sk, msg, len);
-        else
                err = -ENOTCONN;
+                goto done;
+        }
+        /* Connectionless channel */
+        if (sk->sk_type == SOCK_DGRAM) {
+                skb = l2cap_create_connless_pdu(sk, msg, len);
+                err = l2cap_do_send(sk, skb);
+                goto done;
+        }
+        switch (pi->mode) {
+        case L2CAP_MODE_BASIC:
+                /* Create a basic PDU */
+                skb = l2cap_create_basic_pdu(sk, msg, len);
+                if (IS_ERR(skb)) {
+                        err = PTR_ERR(skb);
+                        goto done;
+                }
+                err = l2cap_do_send(sk, skb);
+                if (!err)
+                        err = len;
+                break;
+        case L2CAP_MODE_ERTM:
+                /* Entire SDU fits into one PDU */
+                if (len <= pi->omtu) {
+                        control = L2CAP_SDU_UNSEGMENTED;
+                        skb = l2cap_create_ertm_pdu(sk, msg, len, control);
+                        if (IS_ERR(skb)) {
+                                err = PTR_ERR(skb);
+                                goto done;
+                        }
+                } else {
+                        /* FIXME: Segmentation will be added later */
+                        err = -EINVAL;
+                        goto done;
+                }
+                __skb_queue_tail(TX_QUEUE(sk), skb);
+                if (sk->sk_send_head == NULL)
+                        sk->sk_send_head = skb;
+                err = l2cap_ertm_send(sk);
+                if (!err)
+                        err = len;
+                break;
+        default:
+                BT_DBG("bad state %1.1x", pi->mode);
+                err = -EINVAL;
+        }
+done:
        release_sock(sk);
        return err;
 }
@@ -2301,6 +2496,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
        if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
                sk->sk_state = BT_CONNECTED;
+                l2cap_pi(sk)->next_tx_seq = 0;
+                l2cap_pi(sk)->expected_ack_seq = 0;
+                l2cap_pi(sk)->unacked_frames = 0;
+                __skb_queue_head_init(TX_QUEUE(sk));
                l2cap_chan_ready(sk);
                goto unlock;
        }
@@ -2375,6 +2574,9 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
        if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
                sk->sk_state = BT_CONNECTED;
+                l2cap_pi(sk)->expected_tx_seq = 0;
+                l2cap_pi(sk)->num_to_ack = 0;
+                __skb_queue_head_init(TX_QUEUE(sk));
                l2cap_chan_ready(sk);
        }
@@ -2405,6 +2607,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
        sk->sk_shutdown = SHUTDOWN_MASK;
+        skb_queue_purge(TX_QUEUE(sk));
        l2cap_chan_del(sk, ECONNRESET);
        bh_unlock_sock(sk);
@@ -2427,6 +2631,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
        if (!sk)
                return 0;
+        skb_queue_purge(TX_QUEUE(sk));
        l2cap_chan_del(sk, 0);
        bh_unlock_sock(sk);
@@ -2602,9 +2808,60 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
        kfree_skb(skb);
 }
+static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u8 tx_seq = __get_txseq(rx_control);
+        u16 tx_control = 0;
+        int err = 0;
+        BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+        if (tx_seq != pi->expected_tx_seq)
+                return -EINVAL;
+        pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
+        err = sock_queue_rcv_skb(sk, skb);
+        if (err)
+                return err;
+        pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
+        if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
+                tx_control |= L2CAP_CTRL_FRAME_TYPE;
+                tx_control |= L2CAP_SUPER_RCV_READY;
+                tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+                err = l2cap_send_sframe(pi, tx_control);
+        }
+        return err;
+}
+static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+        switch (rx_control & L2CAP_CTRL_SUPERVISE) {
+        case L2CAP_SUPER_RCV_READY:
+                pi->expected_ack_seq = __get_reqseq(rx_control);
+                l2cap_drop_acked_frames(sk);
+                l2cap_ertm_send(sk);
+                break;
+        case L2CAP_SUPER_RCV_NOT_READY:
+        case L2CAP_SUPER_REJECT:
+        case L2CAP_SUPER_SELECT_REJECT:
+                break;
+        }
+        return 0;
+}
 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
 {
        struct sock *sk;
+        u16 control;
+        int err;
        sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
        if (!sk) {
@@ -2617,16 +2874,40 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
        if (sk->sk_state != BT_CONNECTED)
                goto drop;
-        if (l2cap_pi(sk)->imtu < skb->len)
+        switch (l2cap_pi(sk)->mode) {
-                goto drop;
+        case L2CAP_MODE_BASIC:
+                /* If socket recv buffers overflows we drop data here
+                 * which is *bad* because L2CAP has to be reliable.
+                 * But we don't have any other choice. L2CAP doesn't
+                 * provide flow control mechanism. */
-        /* If socket recv buffers overflows we drop data here
+                if (l2cap_pi(sk)->imtu < skb->len)
-         * which is *bad* because L2CAP has to be reliable.
+                        goto drop;
-         * But we don't have any other choice. L2CAP doesn't
-         * provide flow control mechanism. */
-        if (!sock_queue_rcv_skb(sk, skb))
+                if (!sock_queue_rcv_skb(sk, skb))
-                goto done;
+                        goto done;
+                break;
+        case L2CAP_MODE_ERTM:
+                control = get_unaligned_le16(skb->data);
+                skb_pull(skb, 2);
+                if (l2cap_pi(sk)->imtu < skb->len)
+                        goto drop;
+                if (__is_iframe(control))
+                        err = l2cap_data_channel_iframe(sk, control, skb);
+                else
+                        err = l2cap_data_channel_sframe(sk, control, skb);
+                if (!err)
+                        goto done;
+                break;
+        default:
+                BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
+                break;
+        }
 drop:
        kfree_skb(skb);
@@ -2676,6 +2957,11 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
        cid = __le16_to_cpu(lh->cid);
        len = __le16_to_cpu(lh->len);
+        if (len != skb->len) {
+                kfree_skb(skb);
+                return;
+        }
        BT_DBG("len %d, cid 0x%4.4x", len, cid);
        switch (cid) {

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h index 968166a45f86..65a5cf868fd6 100644 --- a/include/net/bluetooth/bluetooth.h +++ b/include/net/bluetooth/bluetooth.h
@@ -138,8 +138,9 @@ struct sock bt_accept_dequeue(struct sock parent, struct socket *newsock);
138	struct bt_skb_cb {	138	struct bt_skb_cb {
139	__u8 pkt_type;	139	__u8 pkt_type;
140	__u8 incoming;	140	__u8 incoming;
		141	__u8 tx_seq;
141	};	142	};
142	#define bt_cb(skb) ((struct bt_skb_cb *)(skb->cb))	143	#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
143		144
144	static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)	145	static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)
145	{	146	{


diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h index 6fc76986d70a..9bbfbe74d400 100644 --- a/include/net/bluetooth/l2cap.h +++ b/include/net/bluetooth/l2cap.h
@@ -29,7 +29,8 @@
29	#define L2CAP_DEFAULT_MTU 672	29	#define L2CAP_DEFAULT_MTU 672
30	#define L2CAP_DEFAULT_MIN_MTU 48	30	#define L2CAP_DEFAULT_MIN_MTU 48
31	#define L2CAP_DEFAULT_FLUSH_TO 0xffff	31	#define L2CAP_DEFAULT_FLUSH_TO 0xffff
32	#define L2CAP_DEFAULT_TX_WINDOW 1	32	#define L2CAP_DEFAULT_TX_WINDOW 63
		33	#define L2CAP_DEFAULT_NUM_TO_ACK (L2CAP_DEFAULT_TX_WINDOW/5)
33	#define L2CAP_DEFAULT_MAX_RECEIVE 1	34	#define L2CAP_DEFAULT_MAX_RECEIVE 1
34	#define L2CAP_DEFAULT_RETRANS_TO 300 /* 300 milliseconds */	35	#define L2CAP_DEFAULT_RETRANS_TO 300 /* 300 milliseconds */
35	#define L2CAP_DEFAULT_MONITOR_TO 1000 /* 1 second */	36	#define L2CAP_DEFAULT_MONITOR_TO 1000 /* 1 second */
@@ -94,6 +95,31 @@ struct l2cap_conninfo {
94	#define L2CAP_FCS_NONE 0x00	95	#define L2CAP_FCS_NONE 0x00
95	#define L2CAP_FCS_CRC16 0x01	96	#define L2CAP_FCS_CRC16 0x01
96		97
		98	/* L2CAP Control Field bit masks */
		99	#define L2CAP_CTRL_SAR 0xC000
		100	#define L2CAP_CTRL_REQSEQ 0x3F00
		101	#define L2CAP_CTRL_TXSEQ 0x007E
		102	#define L2CAP_CTRL_RETRANS 0x0080
		103	#define L2CAP_CTRL_FINAL 0x0080
		104	#define L2CAP_CTRL_POLL 0x0010
		105	#define L2CAP_CTRL_SUPERVISE 0x000C
		106	#define L2CAP_CTRL_FRAME_TYPE 0x0001 /* I- or S-Frame */
		107
		108	#define L2CAP_CTRL_TXSEQ_SHIFT 1
		109	#define L2CAP_CTRL_REQSEQ_SHIFT 8
		110
		111	/* L2CAP Supervisory Function */
		112	#define L2CAP_SUPER_RCV_READY 0x0000
		113	#define L2CAP_SUPER_REJECT 0x0004
		114	#define L2CAP_SUPER_RCV_NOT_READY 0x0008
		115	#define L2CAP_SUPER_SELECT_REJECT 0x000C
		116
		117	/* L2CAP Segmentation and Reassembly */
		118	#define L2CAP_SDU_UNSEGMENTED 0x0000
		119	#define L2CAP_SDU_START 0x4000
		120	#define L2CAP_SDU_END 0x8000
		121	#define L2CAP_SDU_CONTINUE 0xC000
		122
97	/* L2CAP structures */	123	/* L2CAP structures */
98	struct l2cap_hdr {	124	struct l2cap_hdr {
99	__le16 len;	125	__le16 len;
@@ -262,6 +288,7 @@ struct l2cap_conn {
262		288
263	/* ----- L2CAP channel and socket info ----- */	289	/* ----- L2CAP channel and socket info ----- */
264	#define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)	290	#define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
		291	#define TX_QUEUE(sk) (&l2cap_pi(sk)->tx_queue)
265		292
266	struct l2cap_pinfo {	293	struct l2cap_pinfo {
267	struct bt_sock bt;	294	struct bt_sock bt;
@@ -285,6 +312,13 @@ struct l2cap_pinfo {
285	__u8 conf_len;	312	__u8 conf_len;
286	__u8 conf_state;	313	__u8 conf_state;
287		314
		315	__u8 next_tx_seq;
		316	__u8 expected_ack_seq;
		317	__u8 req_seq;
		318	__u8 expected_tx_seq;
		319	__u8 unacked_frames;
		320	__u8 num_to_ack;
		321
288	__u8 ident;	322	__u8 ident;
289		323
290	__u8 remote_tx_win;	324	__u8 remote_tx_win;
@@ -295,6 +329,7 @@ struct l2cap_pinfo {
295		329
296	__le16 sport;	330	__le16 sport;
297		331
		332	struct sk_buff_head tx_queue;
298	struct l2cap_conn *conn;	333	struct l2cap_conn *conn;
299	struct sock *next_c;	334	struct sock *next_c;
300	struct sock *prev_c;	335	struct sock *prev_c;
@@ -311,6 +346,23 @@ struct l2cap_pinfo {
311	#define L2CAP_CONF_MAX_CONF_REQ 2	346	#define L2CAP_CONF_MAX_CONF_REQ 2
312	#define L2CAP_CONF_MAX_CONF_RSP 2	347	#define L2CAP_CONF_MAX_CONF_RSP 2
313		348
		349	static inline int l2cap_tx_window_full(struct sock *sk)
		350	{
		351	struct l2cap_pinfo *pi = l2cap_pi(sk);
		352	int sub;
		353
		354	sub = (pi->next_tx_seq - pi->expected_ack_seq) % 64;
		355
		356	if (sub < 0)
		357	sub += 64;
		358
		359	return (sub == pi->remote_tx_win);
		360	}
		361
		362	#define __get_txseq(ctrl) ((ctrl) & L2CAP_CTRL_TXSEQ) >> 1
		363	#define __get_reqseq(ctrl) ((ctrl) & L2CAP_CTRL_REQSEQ) >> 8
		364	#define __is_iframe(ctrl) !((ctrl) & L2CAP_CTRL_FRAME_TYPE)
		365	#define __is_sframe(ctrl) (ctrl) & L2CAP_CTRL_FRAME_TYPE
314		366
315	void l2cap_load(void);	367	void l2cap_load(void);
316		368


diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c index c1b562085cb4..45b8697a7398 100644 --- a/net/bluetooth/l2cap.c +++ b/net/bluetooth/l2cap.c
@@ -333,6 +333,30 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16
333	return hci_send_acl(conn->hcon, skb, 0);	333	return hci_send_acl(conn->hcon, skb, 0);
334	}	334	}
335		335
		336	static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
		337	{
		338	struct sk_buff *skb;
		339	struct l2cap_hdr *lh;
		340	struct l2cap_conn *conn = pi->conn;
		341	int count;
		342
		343	BT_DBG("pi %p, control 0x%2.2x", pi, control);
		344
		345	count = min_t(unsigned int, conn->mtu, L2CAP_HDR_SIZE + 2);
		346	control \|= L2CAP_CTRL_FRAME_TYPE;
		347
		348	skb = bt_skb_alloc(count, GFP_ATOMIC);
		349	if (!skb)
		350	return -ENOMEM;
		351
		352	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
		353	lh->len = cpu_to_le16(2);
		354	lh->cid = cpu_to_le16(pi->dcid);
		355	put_unaligned_le16(control, skb_put(skb, 2));
		356
		357	return hci_send_acl(pi->conn->hcon, skb, 0);
		358	}
		359
336	static void l2cap_do_start(struct sock *sk)	360	static void l2cap_do_start(struct sock *sk)
337	{	361	{
338	struct l2cap_conn *conn = l2cap_pi(sk)->conn;	362	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
@@ -1154,39 +1178,80 @@ static int l2cap_sock_getname(struct socket sock, struct sockaddr addr, int *l
1154	return 0;	1178	return 0;
1155	}	1179	}
1156		1180
1157	static inline int l2cap_do_send(struct sock sk, struct msghdr msg, int len)	1181	static void l2cap_drop_acked_frames(struct sock *sk)
1158	{	1182	{
1159	struct l2cap_conn *conn = l2cap_pi(sk)->conn;	1183	struct sk_buff *skb;
1160	struct sk_buff skb, *frag;
1161	int err, hlen, count, sent = 0;
1162	struct l2cap_hdr *lh;
1163		1184
1164	BT_DBG("sk %p len %d", sk, len);	1185	while ((skb = skb_peek(TX_QUEUE(sk)))) {
		1186	if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
		1187	break;
1165		1188
1166	/* First fragment (with L2CAP header) */	1189	skb = skb_dequeue(TX_QUEUE(sk));
1167	if (sk->sk_type == SOCK_DGRAM)	1190	kfree_skb(skb);
1168	hlen = L2CAP_HDR_SIZE + 2;
1169	else
1170	hlen = L2CAP_HDR_SIZE;
1171		1191
1172	count = min_t(unsigned int, (conn->mtu - hlen), len);	1192	l2cap_pi(sk)->unacked_frames--;
		1193	}
1173		1194
1174	skb = bt_skb_send_alloc(sk, hlen + count,	1195	return;
1175	msg->msg_flags & MSG_DONTWAIT, &err);	1196	}
1176	if (!skb)
1177	return err;
1178		1197
1179	/* Create L2CAP header */	1198	static inline int l2cap_do_send(struct sock sk, struct sk_buff skb)
1180	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);	1199	{
1181	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);	1200	struct l2cap_pinfo *pi = l2cap_pi(sk);
1182	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));	1201	int err;
		1202
		1203	BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
		1204
		1205	err = hci_send_acl(pi->conn->hcon, skb, 0);
		1206	if (err < 0)
		1207	kfree_skb(skb);
		1208
		1209	return err;
		1210	}
		1211
		1212	static int l2cap_ertm_send(struct sock *sk)
		1213	{
		1214	struct sk_buff skb, tx_skb;
		1215	struct l2cap_pinfo *pi = l2cap_pi(sk);
		1216	u16 control;
		1217	int err;
		1218
		1219	while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
		1220	tx_skb = skb_clone(skb, GFP_ATOMIC);
1183		1221
1184	if (sk->sk_type == SOCK_DGRAM)	1222	control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
1185	put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));	1223	control \|= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
		1224	\| (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
		1225	put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
		1226
		1227	err = l2cap_do_send(sk, tx_skb);
		1228	if (err < 0) {
		1229	l2cap_send_disconn_req(pi->conn, sk);
		1230	return err;
		1231	}
		1232
		1233	bt_cb(skb)->tx_seq = pi->next_tx_seq;
		1234	pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
		1235
		1236	pi->unacked_frames++;
		1237
		1238	if (skb_queue_is_last(TX_QUEUE(sk), skb))
		1239	sk->sk_send_head = NULL;
		1240	else
		1241	sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
		1242	}
		1243
		1244	return 0;
		1245	}
		1246
		1247	static inline int l2cap_skbuff_fromiovec(struct sock sk, struct msghdr msg, int len, int count, struct sk_buff *skb)
		1248	{
		1249	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
		1250	struct sk_buff **frag;
		1251	int err, sent = 0;
1186		1252
1187	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {	1253	if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
1188	err = -EFAULT;	1254	return -EFAULT;
1189	goto fail;
1190	}	1255	}
1191		1256
1192	sent += count;	1257	sent += count;
@@ -1199,33 +1264,112 @@ static inline int l2cap_do_send(struct sock sk, struct msghdr msg, int len)
1199		1264
1200	*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);	1265	*frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
1201	if (!*frag)	1266	if (!*frag)
1202	goto fail;	1267	return -EFAULT;
1203		1268	if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
1204	if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {	1269	return -EFAULT;
1205	err = -EFAULT;
1206	goto fail;
1207	}
1208		1270
1209	sent += count;	1271	sent += count;
1210	len -= count;	1272	len -= count;
1211		1273
1212	frag = &(*frag)->next;	1274	frag = &(*frag)->next;
1213	}	1275	}
1214	err = hci_send_acl(conn->hcon, skb, 0);
1215	if (err < 0)
1216	goto fail;
1217		1276
1218	return sent;	1277	return sent;
		1278	}
1219		1279
1220	fail:	1280	static struct sk_buff l2cap_create_connless_pdu(struct sock sk, struct msghdr *msg, size_t len)
1221	kfree_skb(skb);	1281	{
1222	return err;	1282	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
		1283	struct sk_buff *skb;
		1284	int err, count, hlen = L2CAP_HDR_SIZE + 2;
		1285	struct l2cap_hdr *lh;
		1286
		1287	BT_DBG("sk %p len %d", sk, (int)len);
		1288
		1289	count = min_t(unsigned int, (conn->mtu - hlen), len);
		1290	skb = bt_skb_send_alloc(sk, count + hlen,
		1291	msg->msg_flags & MSG_DONTWAIT, &err);
		1292	if (!skb)
		1293	return ERR_PTR(-ENOMEM);
		1294
		1295	/* Create L2CAP header */
		1296	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
		1297	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
		1298	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
		1299	put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
		1300
		1301	err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
		1302	if (unlikely(err < 0)) {
		1303	kfree_skb(skb);
		1304	return ERR_PTR(err);
		1305	}
		1306	return skb;
		1307	}
		1308
		1309	static struct sk_buff l2cap_create_basic_pdu(struct sock sk, struct msghdr *msg, size_t len)
		1310	{
		1311	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
		1312	struct sk_buff *skb;
		1313	int err, count, hlen = L2CAP_HDR_SIZE;
		1314	struct l2cap_hdr *lh;
		1315
		1316	BT_DBG("sk %p len %d", sk, (int)len);
		1317
		1318	count = min_t(unsigned int, (conn->mtu - hlen), len);
		1319	skb = bt_skb_send_alloc(sk, count + hlen,
		1320	msg->msg_flags & MSG_DONTWAIT, &err);
		1321	if (!skb)
		1322	return ERR_PTR(-ENOMEM);
		1323
		1324	/* Create L2CAP header */
		1325	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
		1326	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
		1327	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
		1328
		1329	err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
		1330	if (unlikely(err < 0)) {
		1331	kfree_skb(skb);
		1332	return ERR_PTR(err);
		1333	}
		1334	return skb;
		1335	}
		1336
		1337	static struct sk_buff l2cap_create_ertm_pdu(struct sock sk, struct msghdr *msg, size_t len, u16 control)
		1338	{
		1339	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
		1340	struct sk_buff *skb;
		1341	int err, count, hlen = L2CAP_HDR_SIZE + 2;
		1342	struct l2cap_hdr *lh;
		1343
		1344	BT_DBG("sk %p len %d", sk, (int)len);
		1345
		1346	count = min_t(unsigned int, (conn->mtu - hlen), len);
		1347	skb = bt_skb_send_alloc(sk, count + hlen,
		1348	msg->msg_flags & MSG_DONTWAIT, &err);
		1349	if (!skb)
		1350	return ERR_PTR(-ENOMEM);
		1351
		1352	/* Create L2CAP header */
		1353	lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
		1354	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
		1355	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
		1356	put_unaligned_le16(control, skb_put(skb, 2));
		1357
		1358	err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
		1359	if (unlikely(err < 0)) {
		1360	kfree_skb(skb);
		1361	return ERR_PTR(err);
		1362	}
		1363	return skb;
1223	}	1364	}
1224		1365
1225	static int l2cap_sock_sendmsg(struct kiocb iocb, struct socket sock, struct msghdr *msg, size_t len)	1366	static int l2cap_sock_sendmsg(struct kiocb iocb, struct socket sock, struct msghdr *msg, size_t len)
1226	{	1367	{
1227	struct sock *sk = sock->sk;	1368	struct sock *sk = sock->sk;
1228	int err = 0;	1369	struct l2cap_pinfo *pi = l2cap_pi(sk);
		1370	struct sk_buff *skb;
		1371	u16 control;
		1372	int err;
1229		1373
1230	BT_DBG("sock %p, sk %p", sock, sk);	1374	BT_DBG("sock %p, sk %p", sock, sk);
1231		1375
@@ -1237,16 +1381,67 @@ static int l2cap_sock_sendmsg(struct kiocb iocb, struct socket sock, struct ms
1237	return -EOPNOTSUPP;	1381	return -EOPNOTSUPP;
1238		1382
1239	/* Check outgoing MTU */	1383	/* Check outgoing MTU */
1240	if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)	1384	if (sk->sk_type == SOCK_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
		1385	&& len > pi->omtu)
1241	return -EINVAL;	1386	return -EINVAL;
1242		1387
1243	lock_sock(sk);	1388	lock_sock(sk);
1244		1389
1245	if (sk->sk_state == BT_CONNECTED)	1390	if (sk->sk_state != BT_CONNECTED) {
1246	err = l2cap_do_send(sk, msg, len);
1247	else
1248	err = -ENOTCONN;	1391	err = -ENOTCONN;
		1392	goto done;
		1393	}
		1394
		1395	/* Connectionless channel */
		1396	if (sk->sk_type == SOCK_DGRAM) {
		1397	skb = l2cap_create_connless_pdu(sk, msg, len);
		1398	err = l2cap_do_send(sk, skb);
		1399	goto done;
		1400	}
1249		1401
		1402	switch (pi->mode) {
		1403	case L2CAP_MODE_BASIC:
		1404	/* Create a basic PDU */
		1405	skb = l2cap_create_basic_pdu(sk, msg, len);
		1406	if (IS_ERR(skb)) {
		1407	err = PTR_ERR(skb);
		1408	goto done;
		1409	}
		1410
		1411	err = l2cap_do_send(sk, skb);
		1412	if (!err)
		1413	err = len;
		1414	break;
		1415
		1416	case L2CAP_MODE_ERTM:
		1417	/* Entire SDU fits into one PDU */
		1418	if (len <= pi->omtu) {
		1419	control = L2CAP_SDU_UNSEGMENTED;
		1420	skb = l2cap_create_ertm_pdu(sk, msg, len, control);
		1421	if (IS_ERR(skb)) {
		1422	err = PTR_ERR(skb);
		1423	goto done;
		1424	}
		1425	} else {
		1426	/* FIXME: Segmentation will be added later */
		1427	err = -EINVAL;
		1428	goto done;
		1429	}
		1430	__skb_queue_tail(TX_QUEUE(sk), skb);
		1431	if (sk->sk_send_head == NULL)
		1432	sk->sk_send_head = skb;
		1433
		1434	err = l2cap_ertm_send(sk);
		1435	if (!err)
		1436	err = len;
		1437	break;
		1438
		1439	default:
		1440	BT_DBG("bad state %1.1x", pi->mode);
		1441	err = -EINVAL;
		1442	}
		1443
		1444	done:
1250	release_sock(sk);	1445	release_sock(sk);
1251	return err;	1446	return err;
1252	}	1447	}
@@ -2301,6 +2496,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
2301		2496
2302	if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {	2497	if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
2303	sk->sk_state = BT_CONNECTED;	2498	sk->sk_state = BT_CONNECTED;
		2499	l2cap_pi(sk)->next_tx_seq = 0;
		2500	l2cap_pi(sk)->expected_ack_seq = 0;
		2501	l2cap_pi(sk)->unacked_frames = 0;
		2502	__skb_queue_head_init(TX_QUEUE(sk));
2304	l2cap_chan_ready(sk);	2503	l2cap_chan_ready(sk);
2305	goto unlock;	2504	goto unlock;
2306	}	2505	}
@@ -2375,6 +2574,9 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
2375		2574
2376	if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {	2575	if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
2377	sk->sk_state = BT_CONNECTED;	2576	sk->sk_state = BT_CONNECTED;
		2577	l2cap_pi(sk)->expected_tx_seq = 0;
		2578	l2cap_pi(sk)->num_to_ack = 0;
		2579	__skb_queue_head_init(TX_QUEUE(sk));
2378	l2cap_chan_ready(sk);	2580	l2cap_chan_ready(sk);
2379	}	2581	}
2380		2582
@@ -2405,6 +2607,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
2405		2607
2406	sk->sk_shutdown = SHUTDOWN_MASK;	2608	sk->sk_shutdown = SHUTDOWN_MASK;
2407		2609
		2610	skb_queue_purge(TX_QUEUE(sk));
		2611
2408	l2cap_chan_del(sk, ECONNRESET);	2612	l2cap_chan_del(sk, ECONNRESET);
2409	bh_unlock_sock(sk);	2613	bh_unlock_sock(sk);
2410		2614
@@ -2427,6 +2631,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
2427	if (!sk)	2631	if (!sk)
2428	return 0;	2632	return 0;
2429		2633
		2634	skb_queue_purge(TX_QUEUE(sk));
		2635
2430	l2cap_chan_del(sk, 0);	2636	l2cap_chan_del(sk, 0);
2431	bh_unlock_sock(sk);	2637	bh_unlock_sock(sk);
2432		2638
@@ -2602,9 +2808,60 @@ static inline void l2cap_sig_channel(struct l2cap_conn conn, struct sk_buff sk
2602	kfree_skb(skb);	2808	kfree_skb(skb);
2603	}	2809	}
2604		2810
		2811	static inline int l2cap_data_channel_iframe(struct sock sk, u16 rx_control, struct sk_buff skb)
		2812	{
		2813	struct l2cap_pinfo *pi = l2cap_pi(sk);
		2814	u8 tx_seq = __get_txseq(rx_control);
		2815	u16 tx_control = 0;
		2816	int err = 0;
		2817
		2818	BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
		2819
		2820	if (tx_seq != pi->expected_tx_seq)
		2821	return -EINVAL;
		2822
		2823	pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
		2824	err = sock_queue_rcv_skb(sk, skb);
		2825	if (err)
		2826	return err;
		2827
		2828	pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
		2829	if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
		2830	tx_control \|= L2CAP_CTRL_FRAME_TYPE;
		2831	tx_control \|= L2CAP_SUPER_RCV_READY;
		2832	tx_control \|= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
		2833	err = l2cap_send_sframe(pi, tx_control);
		2834	}
		2835	return err;
		2836	}
		2837
		2838	static inline int l2cap_data_channel_sframe(struct sock sk, u16 rx_control, struct sk_buff skb)
		2839	{
		2840	struct l2cap_pinfo *pi = l2cap_pi(sk);
		2841
		2842	BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
		2843
		2844	switch (rx_control & L2CAP_CTRL_SUPERVISE) {
		2845	case L2CAP_SUPER_RCV_READY:
		2846	pi->expected_ack_seq = __get_reqseq(rx_control);
		2847	l2cap_drop_acked_frames(sk);
		2848	l2cap_ertm_send(sk);
		2849	break;
		2850
		2851	case L2CAP_SUPER_RCV_NOT_READY:
		2852	case L2CAP_SUPER_REJECT:
		2853	case L2CAP_SUPER_SELECT_REJECT:
		2854	break;
		2855	}
		2856
		2857	return 0;
		2858	}
		2859
2605	static inline int l2cap_data_channel(struct l2cap_conn conn, u16 cid, struct sk_buff skb)	2860	static inline int l2cap_data_channel(struct l2cap_conn conn, u16 cid, struct sk_buff skb)
2606	{	2861	{
2607	struct sock *sk;	2862	struct sock *sk;
		2863	u16 control;
		2864	int err;
2608		2865
2609	sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);	2866	sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
2610	if (!sk) {	2867	if (!sk) {
@@ -2617,16 +2874,40 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
2617	if (sk->sk_state != BT_CONNECTED)	2874	if (sk->sk_state != BT_CONNECTED)
2618	goto drop;	2875	goto drop;
2619		2876
2620	if (l2cap_pi(sk)->imtu < skb->len)	2877	switch (l2cap_pi(sk)->mode) {
2621	goto drop;	2878	case L2CAP_MODE_BASIC:
		2879	/* If socket recv buffers overflows we drop data here
		2880	* which is bad because L2CAP has to be reliable.
		2881	* But we don't have any other choice. L2CAP doesn't
		2882	* provide flow control mechanism. */
2622		2883
2623	/* If socket recv buffers overflows we drop data here	2884	if (l2cap_pi(sk)->imtu < skb->len)
2624	* which is bad because L2CAP has to be reliable.	2885	goto drop;
2625	* But we don't have any other choice. L2CAP doesn't
2626	* provide flow control mechanism. */
2627		2886
2628	if (!sock_queue_rcv_skb(sk, skb))	2887	if (!sock_queue_rcv_skb(sk, skb))
2629	goto done;	2888	goto done;
		2889	break;
		2890
		2891	case L2CAP_MODE_ERTM:
		2892	control = get_unaligned_le16(skb->data);
		2893	skb_pull(skb, 2);
		2894
		2895	if (l2cap_pi(sk)->imtu < skb->len)
		2896	goto drop;
		2897
		2898	if (__is_iframe(control))
		2899	err = l2cap_data_channel_iframe(sk, control, skb);
		2900	else
		2901	err = l2cap_data_channel_sframe(sk, control, skb);
		2902
		2903	if (!err)
		2904	goto done;
		2905	break;
		2906
		2907	default:
		2908	BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
		2909	break;
		2910	}
2630		2911
2631	drop:	2912	drop:
2632	kfree_skb(skb);	2913	kfree_skb(skb);
@@ -2676,6 +2957,11 @@ static void l2cap_recv_frame(struct l2cap_conn conn, struct sk_buff skb)
2676	cid = __le16_to_cpu(lh->cid);	2957	cid = __le16_to_cpu(lh->cid);
2677	len = __le16_to_cpu(lh->len);	2958	len = __le16_to_cpu(lh->len);
2678		2959
		2960	if (len != skb->len) {
		2961	kfree_skb(skb);
		2962	return;
		2963	}
		2964
2679	BT_DBG("len %d, cid 0x%4.4x", len, cid);	2965	BT_DBG("len %d, cid 0x%4.4x", len, cid);
2680		2966
2681	switch (cid) {	2967	switch (cid) {