Bluetooth: Add signal handlers for channel moves
[linux-2.6.git] / net / bluetooth / l2cap_core.c
index 7b06375..335dc6f 100644 (file)
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/smp.h>
 
 int disable_ertm;
+int enable_hs;
 
 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
-static u8 l2cap_fixed_chan[8] = { 0x02, };
+static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
 
-static struct workqueue_struct *_busy_wq;
-
-struct bt_sock_list l2cap_sk_list = {
-       .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
-};
-
-static void l2cap_busy_work(struct work_struct *work);
+static LIST_HEAD(chan_list);
+static DEFINE_RWLOCK(chan_list_lock);
 
 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
                                u8 code, u8 ident, u16 dlen, void *data);
+static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
+                                                               void *data);
 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
+static void l2cap_send_disconn_req(struct l2cap_conn *conn,
+                               struct l2cap_chan *chan, int err);
 
 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
 
 /* ---- L2CAP channels ---- */
+
+static inline void chan_hold(struct l2cap_chan *c)
+{
+       atomic_inc(&c->refcnt);
+}
+
+static inline void chan_put(struct l2cap_chan *c)
+{
+       if (atomic_dec_and_test(&c->refcnt))
+               kfree(c);
+}
+
 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
 {
        struct l2cap_chan *c;
 
        list_for_each_entry(c, &conn->chan_l, list) {
-               struct sock *s = c->sk;
-               if (l2cap_pi(s)->dcid == cid)
+               if (c->dcid == cid)
                        return c;
        }
        return NULL;
@@ -93,8 +105,7 @@ static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16
        struct l2cap_chan *c;
 
        list_for_each_entry(c, &conn->chan_l, list) {
-               struct sock *s = c->sk;
-               if (l2cap_pi(s)->scid == cid)
+               if (c->scid == cid)
                        return c;
        }
        return NULL;
@@ -137,6 +148,64 @@ static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn
        return c;
 }
 
+static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
+{
+       struct l2cap_chan *c;
+
+       list_for_each_entry(c, &chan_list, global_l) {
+               if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
+                       goto found;
+       }
+
+       c = NULL;
+found:
+       return c;
+}
+
+int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm)
+{
+       int err;
+
+       write_lock_bh(&chan_list_lock);
+
+       if (psm && __l2cap_global_chan_by_addr(psm, src)) {
+               err = -EADDRINUSE;
+               goto done;
+       }
+
+       if (psm) {
+               chan->psm = psm;
+               chan->sport = psm;
+               err = 0;
+       } else {
+               u16 p;
+
+               err = -EINVAL;
+               for (p = 0x1001; p < 0x1100; p += 2)
+                       if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) {
+                               chan->psm   = cpu_to_le16(p);
+                               chan->sport = cpu_to_le16(p);
+                               err = 0;
+                               break;
+                       }
+       }
+
+done:
+       write_unlock_bh(&chan_list_lock);
+       return err;
+}
+
+int l2cap_add_scid(struct l2cap_chan *chan,  __u16 scid)
+{
+       write_lock_bh(&chan_list_lock);
+
+       chan->scid = scid;
+
+       write_unlock_bh(&chan_list_lock);
+
+       return 0;
+}
+
 static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 {
        u16 cid = L2CAP_CID_DYN_START;
@@ -149,7 +218,63 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
        return 0;
 }
 
-struct l2cap_chan *l2cap_chan_alloc(struct sock *sk)
+static void l2cap_set_timer(struct l2cap_chan *chan, struct timer_list *timer, long timeout)
+{
+       BT_DBG("chan %p state %d timeout %ld", chan, chan->state, timeout);
+
+       if (!mod_timer(timer, jiffies + msecs_to_jiffies(timeout)))
+               chan_hold(chan);
+}
+
+static void l2cap_clear_timer(struct l2cap_chan *chan, struct timer_list *timer)
+{
+       BT_DBG("chan %p state %d", chan, chan->state);
+
+       if (timer_pending(timer) && del_timer(timer))
+               chan_put(chan);
+}
+
+static void l2cap_state_change(struct l2cap_chan *chan, int state)
+{
+       chan->state = state;
+       chan->ops->state_change(chan->data, state);
+}
+
+static void l2cap_chan_timeout(unsigned long arg)
+{
+       struct l2cap_chan *chan = (struct l2cap_chan *) arg;
+       struct sock *sk = chan->sk;
+       int reason;
+
+       BT_DBG("chan %p state %d", chan, chan->state);
+
+       bh_lock_sock(sk);
+
+       if (sock_owned_by_user(sk)) {
+               /* sk is owned by user. Try again later */
+               __set_chan_timer(chan, HZ / 5);
+               bh_unlock_sock(sk);
+               chan_put(chan);
+               return;
+       }
+
+       if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
+               reason = ECONNREFUSED;
+       else if (chan->state == BT_CONNECT &&
+                                       chan->sec_level != BT_SECURITY_SDP)
+               reason = ECONNREFUSED;
+       else
+               reason = ETIMEDOUT;
+
+       l2cap_chan_close(chan, reason);
+
+       bh_unlock_sock(sk);
+
+       chan->ops->close(chan->data);
+       chan_put(chan);
+}
+
+struct l2cap_chan *l2cap_chan_create(struct sock *sk)
 {
        struct l2cap_chan *chan;
 
@@ -159,57 +284,81 @@ struct l2cap_chan *l2cap_chan_alloc(struct sock *sk)
 
        chan->sk = sk;
 
+       write_lock_bh(&chan_list_lock);
+       list_add(&chan->global_l, &chan_list);
+       write_unlock_bh(&chan_list_lock);
+
+       setup_timer(&chan->chan_timer, l2cap_chan_timeout, (unsigned long) chan);
+
+       chan->state = BT_OPEN;
+
+       atomic_set(&chan->refcnt, 1);
+
        return chan;
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_destroy(struct l2cap_chan *chan)
 {
-       struct sock *sk = chan->sk;
+       write_lock_bh(&chan_list_lock);
+       list_del(&chan->global_l);
+       write_unlock_bh(&chan_list_lock);
+
+       chan_put(chan);
+}
 
+static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+{
        BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
-                       l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid);
+                       chan->psm, chan->dcid);
 
        conn->disc_reason = 0x13;
 
-       l2cap_pi(sk)->conn = conn;
+       chan->conn = conn;
 
-       if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
+       if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
                if (conn->hcon->type == LE_LINK) {
                        /* LE connection */
                        chan->omtu = L2CAP_LE_DEFAULT_MTU;
-                       l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA;
-                       l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA;
+                       chan->scid = L2CAP_CID_LE_DATA;
+                       chan->dcid = L2CAP_CID_LE_DATA;
                } else {
                        /* Alloc CID for connection-oriented socket */
-                       l2cap_pi(sk)->scid = l2cap_alloc_cid(conn);
+                       chan->scid = l2cap_alloc_cid(conn);
                        chan->omtu = L2CAP_DEFAULT_MTU;
                }
-       } else if (sk->sk_type == SOCK_DGRAM) {
+       } else if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
                /* Connectionless socket */
-               l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
-               l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS;
+               chan->scid = L2CAP_CID_CONN_LESS;
+               chan->dcid = L2CAP_CID_CONN_LESS;
                chan->omtu = L2CAP_DEFAULT_MTU;
        } else {
                /* Raw socket can send/recv signalling messages only */
-               l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING;
-               l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING;
+               chan->scid = L2CAP_CID_SIGNALING;
+               chan->dcid = L2CAP_CID_SIGNALING;
                chan->omtu = L2CAP_DEFAULT_MTU;
        }
 
-       sock_hold(sk);
+       chan->local_id          = L2CAP_BESTEFFORT_ID;
+       chan->local_stype       = L2CAP_SERV_BESTEFFORT;
+       chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
+       chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
+       chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
+       chan->local_flush_to    = L2CAP_DEFAULT_FLUSH_TO;
+
+       chan_hold(chan);
 
        list_add(&chan->list, &conn->chan_l);
 }
 
 /* Delete channel.
  * Must be called on the locked socket. */
-void l2cap_chan_del(struct l2cap_chan *chan, int err)
+static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 {
        struct sock *sk = chan->sk;
-       struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+       struct l2cap_conn *conn = chan->conn;
        struct sock *parent = bt_sk(sk)->parent;
 
-       l2cap_sock_clear_timer(sk);
+       __clear_chan_timer(chan);
 
        BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
 
@@ -218,13 +367,13 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
                write_lock_bh(&conn->chan_lock);
                list_del(&chan->list);
                write_unlock_bh(&conn->chan_lock);
-               __sock_put(sk);
+               chan_put(chan);
 
-               l2cap_pi(sk)->conn = NULL;
+               chan->conn = NULL;
                hci_conn_put(conn->hcon);
        }
 
-       sk->sk_state = BT_CLOSED;
+       l2cap_state_change(chan, BT_CLOSED);
        sock_set_flag(sk, SOCK_ZAPPED);
 
        if (err)
@@ -236,37 +385,108 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
        } else
                sk->sk_state_change(sk);
 
-       if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE &&
-                       chan->conf_state & L2CAP_CONF_INPUT_DONE))
-               goto free;
+       if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
+                       test_bit(CONF_INPUT_DONE, &chan->conf_state)))
+               return;
 
        skb_queue_purge(&chan->tx_q);
 
        if (chan->mode == L2CAP_MODE_ERTM) {
                struct srej_list *l, *tmp;
 
-               del_timer(&chan->retrans_timer);
-               del_timer(&chan->monitor_timer);
-               del_timer(&chan->ack_timer);
+               __clear_retrans_timer(chan);
+               __clear_monitor_timer(chan);
+               __clear_ack_timer(chan);
 
                skb_queue_purge(&chan->srej_q);
-               skb_queue_purge(&chan->busy_q);
 
                list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
                        list_del(&l->list);
                        kfree(l);
                }
        }
+}
+
+static void l2cap_chan_cleanup_listen(struct sock *parent)
+{
+       struct sock *sk;
+
+       BT_DBG("parent %p", parent);
 
-free:
-       kfree(chan);
+       /* Close not yet accepted channels */
+       while ((sk = bt_accept_dequeue(parent, NULL))) {
+               struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+               __clear_chan_timer(chan);
+               lock_sock(sk);
+               l2cap_chan_close(chan, ECONNRESET);
+               release_sock(sk);
+               chan->ops->close(chan->data);
+       }
 }
 
-static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
+void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 {
+       struct l2cap_conn *conn = chan->conn;
        struct sock *sk = chan->sk;
 
-       if (sk->sk_type == SOCK_RAW) {
+       BT_DBG("chan %p state %d socket %p", chan, chan->state, sk->sk_socket);
+
+       switch (chan->state) {
+       case BT_LISTEN:
+               l2cap_chan_cleanup_listen(sk);
+
+               l2cap_state_change(chan, BT_CLOSED);
+               sock_set_flag(sk, SOCK_ZAPPED);
+               break;
+
+       case BT_CONNECTED:
+       case BT_CONFIG:
+               if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
+                                       conn->hcon->type == ACL_LINK) {
+                       __clear_chan_timer(chan);
+                       __set_chan_timer(chan, sk->sk_sndtimeo);
+                       l2cap_send_disconn_req(conn, chan, reason);
+               } else
+                       l2cap_chan_del(chan, reason);
+               break;
+
+       case BT_CONNECT2:
+               if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
+                                       conn->hcon->type == ACL_LINK) {
+                       struct l2cap_conn_rsp rsp;
+                       __u16 result;
+
+                       if (bt_sk(sk)->defer_setup)
+                               result = L2CAP_CR_SEC_BLOCK;
+                       else
+                               result = L2CAP_CR_BAD_PSM;
+                       l2cap_state_change(chan, BT_DISCONN);
+
+                       rsp.scid   = cpu_to_le16(chan->dcid);
+                       rsp.dcid   = cpu_to_le16(chan->scid);
+                       rsp.result = cpu_to_le16(result);
+                       rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
+                       l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
+                                                       sizeof(rsp), &rsp);
+               }
+
+               l2cap_chan_del(chan, reason);
+               break;
+
+       case BT_CONNECT:
+       case BT_DISCONN:
+               l2cap_chan_del(chan, reason);
+               break;
+
+       default:
+               sock_set_flag(sk, SOCK_ZAPPED);
+               break;
+       }
+}
+
+static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
+{
+       if (chan->chan_type == L2CAP_CHAN_RAW) {
                switch (chan->sec_level) {
                case BT_SECURITY_HIGH:
                        return HCI_AT_DEDICATED_BONDING_MITM;
@@ -275,7 +495,7 @@ static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
                default:
                        return HCI_AT_NO_BONDING;
                }
-       } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) {
+       } else if (chan->psm == cpu_to_le16(0x0001)) {
                if (chan->sec_level == BT_SECURITY_LOW)
                        chan->sec_level = BT_SECURITY_SDP;
 
@@ -298,7 +518,7 @@ static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
 /* Service level security */
 static inline int l2cap_check_security(struct l2cap_chan *chan)
 {
-       struct l2cap_conn *conn = l2cap_pi(chan->sk)->conn;
+       struct l2cap_conn *conn = chan->conn;
        __u8 auth_type;
 
        auth_type = l2cap_get_auth_type(chan);
@@ -306,7 +526,7 @@ static inline int l2cap_check_security(struct l2cap_chan *chan)
        return hci_conn_security(conn->hcon, chan->sec_level, auth_type);
 }
 
-u8 l2cap_get_ident(struct l2cap_conn *conn)
+static u8 l2cap_get_ident(struct l2cap_conn *conn)
 {
        u8 id;
 
@@ -328,7 +548,7 @@ u8 l2cap_get_ident(struct l2cap_conn *conn)
        return id;
 }
 
-void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
+static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
 {
        struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
        u8 flags;
@@ -343,39 +563,59 @@ void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *d
        else
                flags = ACL_START;
 
-       hci_send_acl(conn->hcon, skb, flags);
+       bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
+       skb->priority = HCI_PRIO_MAX;
+
+       hci_send_acl(conn->hchan, skb, flags);
+}
+
+static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
+{
+       struct hci_conn *hcon = chan->conn->hcon;
+       u16 flags;
+
+       BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
+                                                       skb->priority);
+
+       if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
+                                       lmp_no_flush_capable(hcon->hdev))
+               flags = ACL_START_NO_FLUSH;
+       else
+               flags = ACL_START;
+
+       bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+       hci_send_acl(chan->conn->hchan, skb, flags);
 }
 
-static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control)
+static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control)
 {
        struct sk_buff *skb;
        struct l2cap_hdr *lh;
-       struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
-       struct l2cap_conn *conn = pi->conn;
-       struct sock *sk = (struct sock *)pi;
-       int count, hlen = L2CAP_HDR_SIZE + 2;
-       u8 flags;
+       struct l2cap_conn *conn = chan->conn;
+       int count, hlen;
 
-       if (sk->sk_state != BT_CONNECTED)
+       if (chan->state != BT_CONNECTED)
                return;
 
+       if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+               hlen = L2CAP_EXT_HDR_SIZE;
+       else
+               hlen = L2CAP_ENH_HDR_SIZE;
+
        if (chan->fcs == L2CAP_FCS_CRC16)
-               hlen += 2;
+               hlen += L2CAP_FCS_SIZE;
 
-       BT_DBG("chan %p, control 0x%2.2x", chan, control);
+       BT_DBG("chan %p, control 0x%8.8x", chan, control);
 
        count = min_t(unsigned int, conn->mtu, hlen);
-       control |= L2CAP_CTRL_FRAME_TYPE;
 
-       if (chan->conn_state & L2CAP_CONN_SEND_FBIT) {
-               control |= L2CAP_CTRL_FINAL;
-               chan->conn_state &= ~L2CAP_CONN_SEND_FBIT;
-       }
+       control |= __set_sframe(chan);
 
-       if (chan->conn_state & L2CAP_CONN_SEND_PBIT) {
-               control |= L2CAP_CTRL_POLL;
-               chan->conn_state &= ~L2CAP_CONN_SEND_PBIT;
-       }
+       if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
+               control |= __set_ctrl_final(chan);
+
+       if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
+               control |= __set_ctrl_poll(chan);
 
        skb = bt_skb_alloc(count, GFP_ATOMIC);
        if (!skb)
@@ -383,44 +623,40 @@ static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control)
 
        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
        lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
-       lh->cid = cpu_to_le16(pi->dcid);
-       put_unaligned_le16(control, skb_put(skb, 2));
+       lh->cid = cpu_to_le16(chan->dcid);
+
+       __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
 
        if (chan->fcs == L2CAP_FCS_CRC16) {
-               u16 fcs = crc16(0, (u8 *)lh, count - 2);
-               put_unaligned_le16(fcs, skb_put(skb, 2));
+               u16 fcs = crc16(0, (u8 *)lh, count - L2CAP_FCS_SIZE);
+               put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE));
        }
 
-       if (lmp_no_flush_capable(conn->hcon->hdev))
-               flags = ACL_START_NO_FLUSH;
-       else
-               flags = ACL_START;
-
-       hci_send_acl(pi->conn->hcon, skb, flags);
+       skb->priority = HCI_PRIO_MAX;
+       l2cap_do_send(chan, skb);
 }
 
-static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control)
+static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control)
 {
-       if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
-               control |= L2CAP_SUPER_RCV_NOT_READY;
-               chan->conn_state |= L2CAP_CONN_RNR_SENT;
+       if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
+               set_bit(CONN_RNR_SENT, &chan->conn_state);
        } else
-               control |= L2CAP_SUPER_RCV_READY;
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
 
-       control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+       control |= __set_reqseq(chan, chan->buffer_seq);
 
        l2cap_send_sframe(chan, control);
 }
 
 static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 {
-       return !(chan->conf_state & L2CAP_CONF_CONNECT_PEND);
+       return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
-       struct sock *sk = chan->sk;
-       struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+       struct l2cap_conn *conn = chan->conn;
 
        if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) {
                if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE))
@@ -429,11 +665,11 @@ static void l2cap_do_start(struct l2cap_chan *chan)
                if (l2cap_check_security(chan) &&
                                __l2cap_no_conn_pending(chan)) {
                        struct l2cap_conn_req req;
-                       req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                       req.psm  = l2cap_pi(sk)->psm;
+                       req.scid = cpu_to_le16(chan->scid);
+                       req.psm  = chan->psm;
 
                        chan->ident = l2cap_get_ident(conn);
-                       chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
+                       set_bit(CONF_CONNECT_PEND, &chan->conf_state);
 
                        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
                                                        sizeof(req), &req);
@@ -469,7 +705,7 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
        }
 }
 
-void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
+static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
 {
        struct sock *sk;
        struct l2cap_disconn_req req;
@@ -480,17 +716,17 @@ void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, in
        sk = chan->sk;
 
        if (chan->mode == L2CAP_MODE_ERTM) {
-               del_timer(&chan->retrans_timer);
-               del_timer(&chan->monitor_timer);
-               del_timer(&chan->ack_timer);
+               __clear_retrans_timer(chan);
+               __clear_monitor_timer(chan);
+               __clear_ack_timer(chan);
        }
 
-       req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
-       req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+       req.dcid = cpu_to_le16(chan->dcid);
+       req.scid = cpu_to_le16(chan->scid);
        l2cap_send_cmd(conn, l2cap_get_ident(conn),
                        L2CAP_DISCONN_REQ, sizeof(req), &req);
 
-       sk->sk_state = BT_DISCONN;
+       l2cap_state_change(chan, BT_DISCONN);
        sk->sk_err = err;
 }
 
@@ -508,13 +744,12 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 
                bh_lock_sock(sk);
 
-               if (sk->sk_type != SOCK_SEQPACKET &&
-                               sk->sk_type != SOCK_STREAM) {
+               if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
                        bh_unlock_sock(sk);
                        continue;
                }
 
-               if (sk->sk_state == BT_CONNECT) {
+               if (chan->state == BT_CONNECT) {
                        struct l2cap_conn_req req;
 
                        if (!l2cap_check_security(chan) ||
@@ -523,43 +758,43 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                                continue;
                        }
 
-                       if (!l2cap_mode_supported(chan->mode,
-                                       conn->feat_mask)
-                                       && chan->conf_state &
-                                       L2CAP_CONF_STATE2_DEVICE) {
-                               /* __l2cap_sock_close() calls list_del(chan)
+                       if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
+                                       && test_bit(CONF_STATE2_DEVICE,
+                                       &chan->conf_state)) {
+                               /* l2cap_chan_close() calls list_del(chan)
                                 * so release the lock */
-                               read_unlock_bh(&conn->chan_lock);
-                                __l2cap_sock_close(sk, ECONNRESET);
-                               read_lock_bh(&conn->chan_lock);
+                               read_unlock(&conn->chan_lock);
+                               l2cap_chan_close(chan, ECONNRESET);
+                               read_lock(&conn->chan_lock);
                                bh_unlock_sock(sk);
                                continue;
                        }
 
-                       req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                       req.psm  = l2cap_pi(sk)->psm;
+                       req.scid = cpu_to_le16(chan->scid);
+                       req.psm  = chan->psm;
 
                        chan->ident = l2cap_get_ident(conn);
-                       chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
+                       set_bit(CONF_CONNECT_PEND, &chan->conf_state);
 
                        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ,
                                                        sizeof(req), &req);
 
-               } else if (sk->sk_state == BT_CONNECT2) {
+               } else if (chan->state == BT_CONNECT2) {
                        struct l2cap_conn_rsp rsp;
                        char buf[128];
-                       rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
-                       rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
+                       rsp.scid = cpu_to_le16(chan->dcid);
+                       rsp.dcid = cpu_to_le16(chan->scid);
 
                        if (l2cap_check_security(chan)) {
                                if (bt_sk(sk)->defer_setup) {
                                        struct sock *parent = bt_sk(sk)->parent;
                                        rsp.result = cpu_to_le16(L2CAP_CR_PEND);
                                        rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
-                                       parent->sk_data_ready(parent, 0);
+                                       if (parent)
+                                               parent->sk_data_ready(parent, 0);
 
                                } else {
-                                       sk->sk_state = BT_CONFIG;
+                                       l2cap_state_change(chan, BT_CONFIG);
                                        rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
                                        rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
                                }
@@ -571,13 +806,13 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
                                                        sizeof(rsp), &rsp);
 
-                       if (chan->conf_state & L2CAP_CONF_REQ_SENT ||
+                       if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
                                        rsp.result != L2CAP_CR_SUCCESS) {
                                bh_unlock_sock(sk);
                                continue;
                        }
 
-                       chan->conf_state |= L2CAP_CONF_REQ_SENT;
+                       set_bit(CONF_REQ_SENT, &chan->conf_state);
                        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                                l2cap_build_conf_req(chan, buf), buf);
                        chan->num_conf_req++;
@@ -592,46 +827,51 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 /* Find socket with cid and source bdaddr.
  * Returns closest match, locked.
  */
-static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src)
+static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src)
 {
-       struct sock *sk = NULL, *sk1 = NULL;
-       struct hlist_node *node;
+       struct l2cap_chan *c, *c1 = NULL;
 
-       read_lock(&l2cap_sk_list.lock);
+       read_lock(&chan_list_lock);
 
-       sk_for_each(sk, node, &l2cap_sk_list.head) {
-               if (state && sk->sk_state != state)
+       list_for_each_entry(c, &chan_list, global_l) {
+               struct sock *sk = c->sk;
+
+               if (state && c->state != state)
                        continue;
 
-               if (l2cap_pi(sk)->scid == cid) {
+               if (c->scid == cid) {
                        /* Exact match. */
-                       if (!bacmp(&bt_sk(sk)->src, src))
-                               break;
+                       if (!bacmp(&bt_sk(sk)->src, src)) {
+                               read_unlock(&chan_list_lock);
+                               return c;
+                       }
 
                        /* Closest match */
                        if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
-                               sk1 = sk;
+                               c1 = c;
                }
        }
 
-       read_unlock(&l2cap_sk_list.lock);
+       read_unlock(&chan_list_lock);
 
-       return node ? sk : sk1;
+       return c1;
 }
 
 static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 {
        struct sock *parent, *sk;
-       struct l2cap_chan *chan;
+       struct l2cap_chan *chan, *pchan;
 
        BT_DBG("");
 
        /* Check if we have socket listening on cid */
-       parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
+       pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
                                                        conn->src);
-       if (!parent)
+       if (!pchan)
                return;
 
+       parent = pchan->sk;
+
        bh_lock_sock(parent);
 
        /* Check for backlog size */
@@ -640,24 +880,16 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
                goto clean;
        }
 
-       sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
-       if (!sk)
-               goto clean;
-
-       chan = l2cap_chan_alloc(sk);
-       if (!chan) {
-               l2cap_sock_kill(sk);
+       chan = pchan->ops->new_connection(pchan->data);
+       if (!chan)
                goto clean;
-       }
 
-       l2cap_pi(sk)->chan = chan;
+       sk = chan->sk;
 
        write_lock_bh(&conn->chan_lock);
 
        hci_conn_hold(conn->hcon);
 
-       l2cap_sock_init(sk, parent);
-
        bacpy(&bt_sk(sk)->src, conn->src);
        bacpy(&bt_sk(sk)->dst, conn->dst);
 
@@ -665,9 +897,9 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
        __l2cap_chan_add(conn, chan);
 
-       l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
+       __set_chan_timer(chan, sk->sk_sndtimeo);
 
-       sk->sk_state = BT_CONNECTED;
+       l2cap_state_change(chan, BT_CONNECTED);
        parent->sk_data_ready(parent, 0);
 
        write_unlock_bh(&conn->chan_lock);
@@ -676,6 +908,23 @@ clean:
        bh_unlock_sock(parent);
 }
 
+static void l2cap_chan_ready(struct sock *sk)
+{
+       struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+       struct sock *parent = bt_sk(sk)->parent;
+
+       BT_DBG("sk %p, parent %p", sk, parent);
+
+       chan->conf_state = 0;
+       __clear_chan_timer(chan);
+
+       l2cap_state_change(chan, BT_CONNECTED);
+       sk->sk_state_change(sk);
+
+       if (parent)
+               parent->sk_data_ready(parent, 0);
+}
+
 static void l2cap_conn_ready(struct l2cap_conn *conn)
 {
        struct l2cap_chan *chan;
@@ -685,6 +934,9 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
        if (!conn->hcon->out && conn->hcon->type == LE_LINK)
                l2cap_le_conn_ready(conn);
 
+       if (conn->hcon->out && conn->hcon->type == LE_LINK)
+               smp_conn_security(conn, conn->hcon->pending_sec_level);
+
        read_lock(&conn->chan_lock);
 
        list_for_each_entry(chan, &conn->chan_l, list) {
@@ -693,17 +945,15 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
                bh_lock_sock(sk);
 
                if (conn->hcon->type == LE_LINK) {
-                       l2cap_sock_clear_timer(sk);
-                       sk->sk_state = BT_CONNECTED;
-                       sk->sk_state_change(sk);
-               }
+                       if (smp_conn_security(conn, chan->sec_level))
+                               l2cap_chan_ready(sk);
 
-               if (sk->sk_type != SOCK_SEQPACKET &&
-                               sk->sk_type != SOCK_STREAM) {
-                       l2cap_sock_clear_timer(sk);
-                       sk->sk_state = BT_CONNECTED;
+               } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
+                       __clear_chan_timer(chan);
+                       l2cap_state_change(chan, BT_CONNECTED);
                        sk->sk_state_change(sk);
-               } else if (sk->sk_state == BT_CONNECT)
+
+               } else if (chan->state == BT_CONNECT)
                        l2cap_do_start(chan);
 
                bh_unlock_sock(sk);
@@ -724,7 +974,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
        list_for_each_entry(chan, &conn->chan_l, list) {
                struct sock *sk = chan->sk;
 
-               if (chan->force_reliable)
+               if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
                        sk->sk_err = err;
        }
 
@@ -741,21 +991,72 @@ static void l2cap_info_timeout(unsigned long arg)
        l2cap_conn_start(conn);
 }
 
+static void l2cap_conn_del(struct hci_conn *hcon, int err)
+{
+       struct l2cap_conn *conn = hcon->l2cap_data;
+       struct l2cap_chan *chan, *l;
+       struct sock *sk;
+
+       if (!conn)
+               return;
+
+       BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
+
+       kfree_skb(conn->rx_skb);
+
+       /* Kill channels */
+       list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
+               sk = chan->sk;
+               bh_lock_sock(sk);
+               l2cap_chan_del(chan, err);
+               bh_unlock_sock(sk);
+               chan->ops->close(chan->data);
+       }
+
+       hci_chan_del(conn->hchan);
+
+       if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
+               del_timer_sync(&conn->info_timer);
+
+       if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->pend)) {
+               del_timer(&conn->security_timer);
+               smp_chan_destroy(conn);
+       }
+
+       hcon->l2cap_data = NULL;
+       kfree(conn);
+}
+
+static void security_timeout(unsigned long arg)
+{
+       struct l2cap_conn *conn = (void *) arg;
+
+       l2cap_conn_del(conn->hcon, ETIMEDOUT);
+}
+
 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 {
        struct l2cap_conn *conn = hcon->l2cap_data;
+       struct hci_chan *hchan;
 
        if (conn || status)
                return conn;
 
+       hchan = hci_chan_create(hcon);
+       if (!hchan)
+               return NULL;
+
        conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
-       if (!conn)
+       if (!conn) {
+               hci_chan_del(hchan);
                return NULL;
+       }
 
        hcon->l2cap_data = conn;
        conn->hcon = hcon;
+       conn->hchan = hchan;
 
-       BT_DBG("hcon %p conn %p", hcon, conn);
+       BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
 
        if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
                conn->mtu = hcon->hdev->le_mtu;
@@ -772,7 +1073,10 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 
        INIT_LIST_HEAD(&conn->chan_l);
 
-       if (hcon->type != LE_LINK)
+       if (hcon->type == LE_LINK)
+               setup_timer(&conn->security_timer, security_timeout,
+                                               (unsigned long) conn);
+       else
                setup_timer(&conn->info_timer, l2cap_info_timeout,
                                                (unsigned long) conn);
 
@@ -781,35 +1085,6 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
        return conn;
 }
 
-static void l2cap_conn_del(struct hci_conn *hcon, int err)
-{
-       struct l2cap_conn *conn = hcon->l2cap_data;
-       struct l2cap_chan *chan, *l;
-       struct sock *sk;
-
-       if (!conn)
-               return;
-
-       BT_DBG("hcon %p conn %p, err %d", hcon, conn, err);
-
-       kfree_skb(conn->rx_skb);
-
-       /* Kill channels */
-       list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
-               sk = chan->sk;
-               bh_lock_sock(sk);
-               l2cap_chan_del(chan, err);
-               bh_unlock_sock(sk);
-               l2cap_sock_kill(sk);
-       }
-
-       if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)
-               del_timer_sync(&conn->info_timer);
-
-       hcon->l2cap_data = NULL;
-       kfree(conn);
-}
-
 static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
        write_lock_bh(&conn->chan_lock);
@@ -822,31 +1097,34 @@ static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *ch
 /* Find socket with psm and source bdaddr.
  * Returns closest match.
  */
-static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src)
+static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src)
 {
-       struct sock *sk = NULL, *sk1 = NULL;
-       struct hlist_node *node;
+       struct l2cap_chan *c, *c1 = NULL;
+
+       read_lock(&chan_list_lock);
 
-       read_lock(&l2cap_sk_list.lock);
+       list_for_each_entry(c, &chan_list, global_l) {
+               struct sock *sk = c->sk;
 
-       sk_for_each(sk, node, &l2cap_sk_list.head) {
-               if (state && sk->sk_state != state)
+               if (state && c->state != state)
                        continue;
 
-               if (l2cap_pi(sk)->psm == psm) {
+               if (c->psm == psm) {
                        /* Exact match. */
-                       if (!bacmp(&bt_sk(sk)->src, src))
-                               break;
+                       if (!bacmp(&bt_sk(sk)->src, src)) {
+                               read_unlock(&chan_list_lock);
+                               return c;
+                       }
 
                        /* Closest match */
                        if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
-                               sk1 = sk;
+                               c1 = c;
                }
        }
 
-       read_unlock(&l2cap_sk_list.lock);
+       read_unlock(&chan_list_lock);
 
-       return node ? sk : sk1;
+       return c1;
 }
 
 int l2cap_chan_connect(struct l2cap_chan *chan)
@@ -861,7 +1139,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
        int err;
 
        BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
-                                                       l2cap_pi(sk)->psm);
+                                                       chan->psm);
 
        hdev = hci_get_route(dst, src);
        if (!hdev)
@@ -871,7 +1149,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
 
        auth_type = l2cap_get_auth_type(chan);
 
-       if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA)
+       if (chan->dcid == L2CAP_CID_LE_DATA)
                hcon = hci_connect(hdev, LE_LINK, dst,
                                        chan->sec_level, auth_type);
        else
@@ -895,15 +1173,14 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
 
        l2cap_chan_add(conn, chan);
 
-       sk->sk_state = BT_CONNECT;
-       l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
+       l2cap_state_change(chan, BT_CONNECT);
+       __set_chan_timer(chan, sk->sk_sndtimeo);
 
        if (hcon->state == BT_CONNECTED) {
-               if (sk->sk_type != SOCK_SEQPACKET &&
-                               sk->sk_type != SOCK_STREAM) {
-                       l2cap_sock_clear_timer(sk);
+               if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
+                       __clear_chan_timer(chan);
                        if (l2cap_check_security(chan))
-                               sk->sk_state = BT_CONNECTED;
+                               l2cap_state_change(chan, BT_CONNECTED);
                } else
                        l2cap_do_start(chan);
        }
@@ -918,14 +1195,14 @@ done:
 
 int __l2cap_wait_ack(struct sock *sk)
 {
+       struct l2cap_chan *chan = l2cap_pi(sk)->chan;
        DECLARE_WAITQUEUE(wait, current);
        int err = 0;
        int timeo = HZ/5;
 
        add_wait_queue(sk_sleep(sk), &wait);
-       while ((l2cap_pi(sk)->chan->unacked_frames > 0 && l2cap_pi(sk)->conn)) {
-               set_current_state(TASK_INTERRUPTIBLE);
-
+       set_current_state(TASK_INTERRUPTIBLE);
+       while (chan->unacked_frames > 0 && chan->conn) {
                if (!timeo)
                        timeo = HZ/5;
 
@@ -937,6 +1214,7 @@ int __l2cap_wait_ack(struct sock *sk)
                release_sock(sk);
                timeo = schedule_timeout(timeo);
                lock_sock(sk);
+               set_current_state(TASK_INTERRUPTIBLE);
 
                err = sock_error(sk);
                if (err)
@@ -956,13 +1234,13 @@ static void l2cap_monitor_timeout(unsigned long arg)
 
        bh_lock_sock(sk);
        if (chan->retry_count >= chan->remote_max_tx) {
-               l2cap_send_disconn_req(l2cap_pi(sk)->conn, chan, ECONNABORTED);
+               l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
                bh_unlock_sock(sk);
                return;
        }
 
        chan->retry_count++;
-       __mod_monitor_timer();
+       __set_monitor_timer(chan);
 
        l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
        bh_unlock_sock(sk);
@@ -977,9 +1255,9 @@ static void l2cap_retrans_timeout(unsigned long arg)
 
        bh_lock_sock(sk);
        chan->retry_count = 1;
-       __mod_monitor_timer();
+       __set_monitor_timer(chan);
 
-       chan->conn_state |= L2CAP_CONN_WAIT_F;
+       set_bit(CONN_WAIT_F, &chan->conn_state);
 
        l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL);
        bh_unlock_sock(sk);
@@ -1001,52 +1279,38 @@ static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
        }
 
        if (!chan->unacked_frames)
-               del_timer(&chan->retrans_timer);
+               __clear_retrans_timer(chan);
 }
 
-void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
-{
-       struct sock *sk = chan->sk;
-       struct hci_conn *hcon = l2cap_pi(sk)->conn->hcon;
-       u16 flags;
-
-       BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len);
-
-       if (!chan->flushable && lmp_no_flush_capable(hcon->hdev))
-               flags = ACL_START_NO_FLUSH;
-       else
-               flags = ACL_START;
-
-       hci_send_acl(hcon, skb, flags);
-}
-
-void l2cap_streaming_send(struct l2cap_chan *chan)
+static void l2cap_streaming_send(struct l2cap_chan *chan)
 {
        struct sk_buff *skb;
-       u16 control, fcs;
+       u32 control;
+       u16 fcs;
 
        while ((skb = skb_dequeue(&chan->tx_q))) {
-               control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE);
-               control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
-               put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE);
+               control = __get_control(chan, skb->data + L2CAP_HDR_SIZE);
+               control |= __set_txseq(chan, chan->next_tx_seq);
+               __put_control(chan, control, skb->data + L2CAP_HDR_SIZE);
 
                if (chan->fcs == L2CAP_FCS_CRC16) {
-                       fcs = crc16(0, (u8 *)skb->data, skb->len - 2);
-                       put_unaligned_le16(fcs, skb->data + skb->len - 2);
+                       fcs = crc16(0, (u8 *)skb->data,
+                                               skb->len - L2CAP_FCS_SIZE);
+                       put_unaligned_le16(fcs,
+                                       skb->data + skb->len - L2CAP_FCS_SIZE);
                }
 
                l2cap_do_send(chan, skb);
 
-               chan->next_tx_seq = (chan->next_tx_seq + 1) % 64;
+               chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
        }
 }
 
-static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
+static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq)
 {
-       struct sock *sk = chan->sk;
-       struct l2cap_pinfo *pi = l2cap_pi(sk);
        struct sk_buff *skb, *tx_skb;
-       u16 control, fcs;
+       u16 fcs;
+       u32 control;
 
        skb = skb_peek(&chan->tx_q);
        if (!skb)
@@ -1063,49 +1327,49 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
 
        if (chan->remote_max_tx &&
                        bt_cb(skb)->retries == chan->remote_max_tx) {
-               l2cap_send_disconn_req(pi->conn, chan, ECONNABORTED);
+               l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
                return;
        }
 
        tx_skb = skb_clone(skb, GFP_ATOMIC);
        bt_cb(skb)->retries++;
-       control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
-       control &= L2CAP_CTRL_SAR;
 
-       if (chan->conn_state & L2CAP_CONN_SEND_FBIT) {
-               control |= L2CAP_CTRL_FINAL;
-               chan->conn_state &= ~L2CAP_CONN_SEND_FBIT;
-       }
+       control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
+       control &= __get_sar_mask(chan);
+
+       if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
+               control |= __set_ctrl_final(chan);
 
-       control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
-                       | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
+       control |= __set_reqseq(chan, chan->buffer_seq);
+       control |= __set_txseq(chan, tx_seq);
 
-       put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+       __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
 
        if (chan->fcs == L2CAP_FCS_CRC16) {
-               fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
-               put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
+               fcs = crc16(0, (u8 *)tx_skb->data,
+                                               tx_skb->len - L2CAP_FCS_SIZE);
+               put_unaligned_le16(fcs,
+                               tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE);
        }
 
        l2cap_do_send(chan, tx_skb);
 }
 
-int l2cap_ertm_send(struct l2cap_chan *chan)
+static int l2cap_ertm_send(struct l2cap_chan *chan)
 {
        struct sk_buff *skb, *tx_skb;
-       struct sock *sk = chan->sk;
-       struct l2cap_pinfo *pi = l2cap_pi(sk);
-       u16 control, fcs;
+       u16 fcs;
+       u32 control;
        int nsent = 0;
 
-       if (sk->sk_state != BT_CONNECTED)
+       if (chan->state != BT_CONNECTED)
                return -ENOTCONN;
 
        while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) {
 
                if (chan->remote_max_tx &&
                                bt_cb(skb)->retries == chan->remote_max_tx) {
-                       l2cap_send_disconn_req(pi->conn, chan, ECONNABORTED);
+                       l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
                        break;
                }
 
@@ -1113,29 +1377,31 @@ int l2cap_ertm_send(struct l2cap_chan *chan)
 
                bt_cb(skb)->retries++;
 
-               control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
-               control &= L2CAP_CTRL_SAR;
+               control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE);
+               control &= __get_sar_mask(chan);
 
-               if (chan->conn_state & L2CAP_CONN_SEND_FBIT) {
-                       control |= L2CAP_CTRL_FINAL;
-                       chan->conn_state &= ~L2CAP_CONN_SEND_FBIT;
-               }
-               control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
-                               | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
-               put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+               if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
+                       control |= __set_ctrl_final(chan);
+
+               control |= __set_reqseq(chan, chan->buffer_seq);
+               control |= __set_txseq(chan, chan->next_tx_seq);
 
+               __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE);
 
                if (chan->fcs == L2CAP_FCS_CRC16) {
-                       fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
-                       put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
+                       fcs = crc16(0, (u8 *)skb->data,
+                                               tx_skb->len - L2CAP_FCS_SIZE);
+                       put_unaligned_le16(fcs, skb->data +
+                                               tx_skb->len - L2CAP_FCS_SIZE);
                }
 
                l2cap_do_send(chan, tx_skb);
 
-               __mod_retrans_timer();
+               __set_retrans_timer(chan);
 
                bt_cb(skb)->tx_seq = chan->next_tx_seq;
-               chan->next_tx_seq = (chan->next_tx_seq + 1) % 64;
+
+               chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq);
 
                if (bt_cb(skb)->retries == 1)
                        chan->unacked_frames++;
@@ -1167,13 +1433,13 @@ static int l2cap_retransmit_frames(struct l2cap_chan *chan)
 
 static void l2cap_send_ack(struct l2cap_chan *chan)
 {
-       u16 control = 0;
+       u32 control = 0;
 
-       control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+       control |= __set_reqseq(chan, chan->buffer_seq);
 
-       if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
-               control |= L2CAP_SUPER_RCV_NOT_READY;
-               chan->conn_state |= L2CAP_CONN_RNR_SENT;
+       if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
+               set_bit(CONN_RNR_SENT, &chan->conn_state);
                l2cap_send_sframe(chan, control);
                return;
        }
@@ -1181,27 +1447,27 @@ static void l2cap_send_ack(struct l2cap_chan *chan)
        if (l2cap_ertm_send(chan) > 0)
                return;
 
-       control |= L2CAP_SUPER_RCV_READY;
+       control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
        l2cap_send_sframe(chan, control);
 }
 
 static void l2cap_send_srejtail(struct l2cap_chan *chan)
 {
        struct srej_list *tail;
-       u16 control;
+       u32 control;
 
-       control = L2CAP_SUPER_SELECT_REJECT;
-       control |= L2CAP_CTRL_FINAL;
+       control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
+       control |= __set_ctrl_final(chan);
 
        tail = list_entry((&chan->srej_l)->prev, struct srej_list, list);
-       control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+       control |= __set_reqseq(chan, tail->tx_seq);
 
        l2cap_send_sframe(chan, control);
 }
 
 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 l2cap_conn *conn = l2cap_pi(sk)->chan->conn;
        struct sk_buff **frag;
        int err, sent = 0;
 
@@ -1222,6 +1488,8 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
                if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
                        return -EFAULT;
 
+               (*frag)->priority = skb->priority;
+
                sent += count;
                len  -= count;
 
@@ -1231,14 +1499,17 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
        return sent;
 }
 
-struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
+                                               struct msghdr *msg, size_t len,
+                                               u32 priority)
 {
-       struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+       struct sock *sk = chan->sk;
+       struct l2cap_conn *conn = chan->conn;
        struct sk_buff *skb;
-       int err, count, hlen = L2CAP_HDR_SIZE + 2;
+       int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
        struct l2cap_hdr *lh;
 
-       BT_DBG("sk %p len %d", sk, (int)len);
+       BT_DBG("sk %p len %d priority %u", sk, (int)len, priority);
 
        count = min_t(unsigned int, (conn->mtu - hlen), len);
        skb = bt_skb_send_alloc(sk, count + hlen,
@@ -1246,11 +1517,13 @@ struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, s
        if (!skb)
                return ERR_PTR(err);
 
+       skb->priority = priority;
+
        /* Create L2CAP header */
        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
-       lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+       lh->cid = cpu_to_le16(chan->dcid);
        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
-       put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
+       put_unaligned_le16(chan->psm, skb_put(skb, 2));
 
        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
        if (unlikely(err < 0)) {
@@ -1260,9 +1533,12 @@ struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, s
        return skb;
 }
 
-struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan,
+                                               struct msghdr *msg, size_t len,
+                                               u32 priority)
 {
-       struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+       struct sock *sk = chan->sk;
+       struct l2cap_conn *conn = chan->conn;
        struct sk_buff *skb;
        int err, count, hlen = L2CAP_HDR_SIZE;
        struct l2cap_hdr *lh;
@@ -1275,9 +1551,11 @@ struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size
        if (!skb)
                return ERR_PTR(err);
 
+       skb->priority = priority;
+
        /* Create L2CAP header */
        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
-       lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+       lh->cid = cpu_to_le16(chan->dcid);
        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
 
        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
@@ -1288,12 +1566,14 @@ struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size
        return skb;
 }
 
-struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
+static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
+                                               struct msghdr *msg, size_t len,
+                                               u32 control, u16 sdulen)
 {
        struct sock *sk = chan->sk;
-       struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+       struct l2cap_conn *conn = chan->conn;
        struct sk_buff *skb;
-       int err, count, hlen = L2CAP_HDR_SIZE + 2;
+       int err, count, hlen;
        struct l2cap_hdr *lh;
 
        BT_DBG("sk %p len %d", sk, (int)len);
@@ -1301,11 +1581,16 @@ struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *
        if (!conn)
                return ERR_PTR(-ENOTCONN);
 
+       if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+               hlen = L2CAP_EXT_HDR_SIZE;
+       else
+               hlen = L2CAP_ENH_HDR_SIZE;
+
        if (sdulen)
-               hlen += 2;
+               hlen += L2CAP_SDULEN_SIZE;
 
        if (chan->fcs == L2CAP_FCS_CRC16)
-               hlen += 2;
+               hlen += L2CAP_FCS_SIZE;
 
        count = min_t(unsigned int, (conn->mtu - hlen), len);
        skb = bt_skb_send_alloc(sk, count + hlen,
@@ -1315,11 +1600,13 @@ struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *
 
        /* Create L2CAP header */
        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
-       lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+       lh->cid = cpu_to_le16(chan->dcid);
        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
-       put_unaligned_le16(control, skb_put(skb, 2));
+
+       __put_control(chan, control, skb_put(skb, __ctrl_size(chan)));
+
        if (sdulen)
-               put_unaligned_le16(sdulen, skb_put(skb, 2));
+               put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE));
 
        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
        if (unlikely(err < 0)) {
@@ -1328,21 +1615,21 @@ struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *
        }
 
        if (chan->fcs == L2CAP_FCS_CRC16)
-               put_unaligned_le16(0, skb_put(skb, 2));
+               put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE));
 
        bt_cb(skb)->retries = 0;
        return skb;
 }
 
-int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
 {
        struct sk_buff *skb;
        struct sk_buff_head sar_queue;
-       u16 control;
+       u32 control;
        size_t size = 0;
 
        skb_queue_head_init(&sar_queue);
-       control = L2CAP_SDU_START;
+       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);
@@ -1355,10 +1642,10 @@ int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t le
                size_t buflen;
 
                if (len > chan->remote_mps) {
-                       control = L2CAP_SDU_CONTINUE;
+                       control = __set_ctrl_sar(chan, L2CAP_SAR_CONTINUE);
                        buflen = chan->remote_mps;
                } else {
-                       control = L2CAP_SDU_END;
+                       control = __set_ctrl_sar(chan, L2CAP_SAR_END);
                        buflen = len;
                }
 
@@ -1379,28 +1666,84 @@ int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t le
        return size;
 }
 
-static void l2cap_chan_ready(struct sock *sk)
+int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
+                                                               u32 priority)
 {
-       struct sock *parent = bt_sk(sk)->parent;
-       struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+       struct sk_buff *skb;
+       u32 control;
+       int err;
 
-       BT_DBG("sk %p, parent %p", sk, parent);
+       /* Connectionless channel */
+       if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
+               skb = l2cap_create_connless_pdu(chan, msg, len, priority);
+               if (IS_ERR(skb))
+                       return PTR_ERR(skb);
 
-       chan->conf_state = 0;
-       l2cap_sock_clear_timer(sk);
+               l2cap_do_send(chan, skb);
+               return len;
+       }
 
-       if (!parent) {
-               /* Outgoing channel.
-                * Wake up socket sleeping on connect.
-                */
-               sk->sk_state = BT_CONNECTED;
-               sk->sk_state_change(sk);
-       } else {
-               /* Incoming channel.
-                * Wake up socket sleeping on accept.
-                */
-               parent->sk_data_ready(parent, 0);
+       switch (chan->mode) {
+       case L2CAP_MODE_BASIC:
+               /* Check outgoing MTU */
+               if (len > chan->omtu)
+                       return -EMSGSIZE;
+
+               /* Create a basic PDU */
+               skb = l2cap_create_basic_pdu(chan, msg, len, priority);
+               if (IS_ERR(skb))
+                       return PTR_ERR(skb);
+
+               l2cap_do_send(chan, skb);
+               err = len;
+               break;
+
+       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);
+
+                       __skb_queue_tail(&chan->tx_q, skb);
+
+                       if (chan->tx_send_head == NULL)
+                               chan->tx_send_head = skb;
+
+               } else {
+                       /* Segment SDU into multiples PDUs */
+                       err = l2cap_sar_segment_sdu(chan, msg, len);
+                       if (err < 0)
+                               return err;
+               }
+
+               if (chan->mode == L2CAP_MODE_STREAMING) {
+                       l2cap_streaming_send(chan);
+                       err = len;
+                       break;
+               }
+
+               if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
+                               test_bit(CONN_WAIT_F, &chan->conn_state)) {
+                       err = len;
+                       break;
+               }
+
+               err = l2cap_ertm_send(chan);
+               if (err >= 0)
+                       err = len;
+
+               break;
+
+       default:
+               BT_DBG("bad state %1.1x", chan->mode);
+               err = -EBADFD;
        }
+
+       return err;
 }
 
 /* Copy frame to all raw sockets on that connection */
@@ -1414,7 +1757,7 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
        read_lock(&conn->chan_lock);
        list_for_each_entry(chan, &conn->chan_l, list) {
                struct sock *sk = chan->sk;
-               if (sk->sk_type != SOCK_RAW)
+               if (chan->chan_type != L2CAP_CHAN_RAW)
                        continue;
 
                /* Don't send frame to the socket it came from */
@@ -1424,7 +1767,7 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
                if (!nskb)
                        continue;
 
-               if (sock_queue_rcv_skb(sk, nskb))
+               if (chan->ops->recv(chan->data, nskb))
                        kfree_skb(nskb);
        }
        read_unlock(&conn->chan_lock);
@@ -1557,6 +1900,37 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
        *ptr += L2CAP_CONF_OPT_SIZE + len;
 }
 
+static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
+{
+       struct l2cap_conf_efs efs;
+
+       switch(chan->mode) {
+       case L2CAP_MODE_ERTM:
+               efs.id          = chan->local_id;
+               efs.stype       = chan->local_stype;
+               efs.msdu        = cpu_to_le16(chan->local_msdu);
+               efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
+               efs.acc_lat     = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
+               efs.flush_to    = cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
+               break;
+
+       case L2CAP_MODE_STREAMING:
+               efs.id          = 1;
+               efs.stype       = L2CAP_SERV_BESTEFFORT;
+               efs.msdu        = cpu_to_le16(chan->local_msdu);
+               efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
+               efs.acc_lat     = 0;
+               efs.flush_to    = 0;
+               break;
+
+       default:
+               return;
+       }
+
+       l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
+                                                       (unsigned long) &efs);
+}
+
 static void l2cap_ack_timeout(unsigned long arg)
 {
        struct l2cap_chan *chan = (void *) arg;
@@ -1583,11 +1957,9 @@ static inline void l2cap_ertm_init(struct l2cap_chan *chan)
        setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan);
 
        skb_queue_head_init(&chan->srej_q);
-       skb_queue_head_init(&chan->busy_q);
 
        INIT_LIST_HEAD(&chan->srej_l);
 
-       INIT_WORK(&chan->busy_work, l2cap_busy_work);
 
        sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
 }
@@ -1605,12 +1977,36 @@ static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
        }
 }
 
+static inline bool __l2cap_ews_supported(struct l2cap_chan *chan)
+{
+       return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW;
+}
+
+static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
+{
+       return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW;
+}
+
+static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
+{
+       if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
+                                               __l2cap_ews_supported(chan)) {
+               /* use extended control field */
+               set_bit(FLAG_EXT_CTRL, &chan->flags);
+               chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
+       } else {
+               chan->tx_win = min_t(u16, chan->tx_win,
+                                               L2CAP_DEFAULT_TX_WINDOW);
+               chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
+       }
+}
+
 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
 {
-       struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
        struct l2cap_conf_req *req = data;
        struct l2cap_conf_rfc rfc = { .mode = chan->mode };
        void *ptr = req->data;
+       u16 size;
 
        BT_DBG("chan %p", chan);
 
@@ -1620,12 +2016,15 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
        switch (chan->mode) {
        case L2CAP_MODE_STREAMING:
        case L2CAP_MODE_ERTM:
-               if (chan->conf_state & L2CAP_CONF_STATE2_DEVICE)
+               if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state))
                        break;
 
+               if (__l2cap_efs_supported(chan))
+                       set_bit(FLAG_EFS_ENABLE, &chan->flags);
+
                /* fall through */
        default:
-               chan->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
+               chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask);
                break;
        }
 
@@ -1635,8 +2034,8 @@ done:
 
        switch (chan->mode) {
        case L2CAP_MODE_BASIC:
-               if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) &&
-                               !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING))
+               if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
+                               !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
                        break;
 
                rfc.mode            = L2CAP_MODE_BASIC;
@@ -1652,25 +2051,39 @@ done:
 
        case L2CAP_MODE_ERTM:
                rfc.mode            = L2CAP_MODE_ERTM;
-               rfc.txwin_size      = chan->tx_win;
                rfc.max_transmit    = chan->max_tx;
                rfc.retrans_timeout = 0;
                rfc.monitor_timeout = 0;
-               rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
-               if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
-                       rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
+
+               size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
+                                               L2CAP_EXT_HDR_SIZE -
+                                               L2CAP_SDULEN_SIZE -
+                                               L2CAP_FCS_SIZE);
+               rfc.max_pdu_size = cpu_to_le16(size);
+
+               l2cap_txwin_setup(chan);
+
+               rfc.txwin_size = min_t(u16, chan->tx_win,
+                                               L2CAP_DEFAULT_TX_WINDOW);
 
                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
                                                        (unsigned long) &rfc);
 
-               if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
+               if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
+                       l2cap_add_opt_efs(&ptr, chan);
+
+               if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
                        break;
 
                if (chan->fcs == L2CAP_FCS_NONE ||
-                               chan->conf_state & L2CAP_CONF_NO_FCS_RECV) {
+                               test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
                        chan->fcs = L2CAP_FCS_NONE;
                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
                }
+
+               if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+                       l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
+                                                               chan->tx_win);
                break;
 
        case L2CAP_MODE_STREAMING:
@@ -1679,25 +2092,31 @@ done:
                rfc.max_transmit    = 0;
                rfc.retrans_timeout = 0;
                rfc.monitor_timeout = 0;
-               rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
-               if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
-                       rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
+
+               size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
+                                               L2CAP_EXT_HDR_SIZE -
+                                               L2CAP_SDULEN_SIZE -
+                                               L2CAP_FCS_SIZE);
+               rfc.max_pdu_size = cpu_to_le16(size);
 
                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
                                                        (unsigned long) &rfc);
 
-               if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
+               if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
+                       l2cap_add_opt_efs(&ptr, chan);
+
+               if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
                        break;
 
                if (chan->fcs == L2CAP_FCS_NONE ||
-                               chan->conf_state & L2CAP_CONF_NO_FCS_RECV) {
+                               test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
                        chan->fcs = L2CAP_FCS_NONE;
                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
                }
                break;
        }
 
-       req->dcid  = cpu_to_le16(pi->dcid);
+       req->dcid  = cpu_to_le16(chan->dcid);
        req->flags = cpu_to_le16(0);
 
        return ptr - data;
@@ -1705,7 +2124,6 @@ done:
 
 static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
 {
-       struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
        struct l2cap_conf_rsp *rsp = data;
        void *ptr = rsp->data;
        void *req = chan->conf_req;
@@ -1713,8 +2131,11 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
        int type, hint, olen;
        unsigned long val;
        struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
+       struct l2cap_conf_efs efs;
+       u8 remote_efs = 0;
        u16 mtu = L2CAP_DEFAULT_MTU;
        u16 result = L2CAP_CONF_SUCCESS;
+       u16 size;
 
        BT_DBG("chan %p", chan);
 
@@ -1743,8 +2164,23 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
 
                case L2CAP_CONF_FCS:
                        if (val == L2CAP_FCS_NONE)
-                               chan->conf_state |= L2CAP_CONF_NO_FCS_RECV;
+                               set_bit(CONF_NO_FCS_RECV, &chan->conf_state);
+                       break;
+
+               case L2CAP_CONF_EFS:
+                       remote_efs = 1;
+                       if (olen == sizeof(efs))
+                               memcpy(&efs, (void *) val, olen);
+                       break;
+
+               case L2CAP_CONF_EWS:
+                       if (!enable_hs)
+                               return -ECONNREFUSED;
 
+                       set_bit(FLAG_EXT_CTRL, &chan->flags);
+                       set_bit(CONF_EWS_RECV, &chan->conf_state);
+                       chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
+                       chan->remote_tx_win = val;
                        break;
 
                default:
@@ -1763,12 +2199,19 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
        switch (chan->mode) {
        case L2CAP_MODE_STREAMING:
        case L2CAP_MODE_ERTM:
-               if (!(chan->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
+               if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
                        chan->mode = l2cap_select_mode(rfc.mode,
-                                       pi->conn->feat_mask);
+                                       chan->conn->feat_mask);
                        break;
                }
 
+               if (remote_efs) {
+                       if (__l2cap_efs_supported(chan))
+                               set_bit(FLAG_EFS_ENABLE, &chan->flags);
+                       else
+                               return -ECONNREFUSED;
+               }
+
                if (chan->mode != rfc.mode)
                        return -ECONNREFUSED;
 
@@ -1787,7 +2230,6 @@ done:
                                        sizeof(rfc), (unsigned long) &rfc);
        }
 
-
        if (result == L2CAP_CONF_SUCCESS) {
                /* Configure output options and let the other side know
                 * which ones we don't like. */
@@ -1796,44 +2238,87 @@ done:
                        result = L2CAP_CONF_UNACCEPT;
                else {
                        chan->omtu = mtu;
-                       chan->conf_state |= L2CAP_CONF_MTU_DONE;
+                       set_bit(CONF_MTU_DONE, &chan->conf_state);
                }
                l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu);
 
+               if (remote_efs) {
+                       if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
+                                       efs.stype != L2CAP_SERV_NOTRAFIC &&
+                                       efs.stype != chan->local_stype) {
+
+                               result = L2CAP_CONF_UNACCEPT;
+
+                               if (chan->num_conf_req >= 1)
+                                       return -ECONNREFUSED;
+
+                               l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
+                                                       sizeof(efs),
+                                                       (unsigned long) &efs);
+                       } else {
+                               /* Send PENDING Conf Rsp */
+                               result = L2CAP_CONF_PENDING;
+                               set_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
+                       }
+               }
+
                switch (rfc.mode) {
                case L2CAP_MODE_BASIC:
                        chan->fcs = L2CAP_FCS_NONE;
-                       chan->conf_state |= L2CAP_CONF_MODE_DONE;
+                       set_bit(CONF_MODE_DONE, &chan->conf_state);
                        break;
 
                case L2CAP_MODE_ERTM:
-                       chan->remote_tx_win = rfc.txwin_size;
-                       chan->remote_max_tx = rfc.max_transmit;
+                       if (!test_bit(CONF_EWS_RECV, &chan->conf_state))
+                               chan->remote_tx_win = rfc.txwin_size;
+                       else
+                               rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
 
-                       if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
-                               rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
+                       chan->remote_max_tx = rfc.max_transmit;
 
-                       chan->remote_mps = le16_to_cpu(rfc.max_pdu_size);
+                       size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
+                                               chan->conn->mtu -
+                                               L2CAP_EXT_HDR_SIZE -
+                                               L2CAP_SDULEN_SIZE -
+                                               L2CAP_FCS_SIZE);
+                       rfc.max_pdu_size = cpu_to_le16(size);
+                       chan->remote_mps = size;
 
                        rfc.retrans_timeout =
                                le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO);
                        rfc.monitor_timeout =
                                le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO);
 
-                       chan->conf_state |= L2CAP_CONF_MODE_DONE;
+                       set_bit(CONF_MODE_DONE, &chan->conf_state);
 
                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
                                        sizeof(rfc), (unsigned long) &rfc);
 
+                       if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
+                               chan->remote_id = efs.id;
+                               chan->remote_stype = efs.stype;
+                               chan->remote_msdu = le16_to_cpu(efs.msdu);
+                               chan->remote_flush_to =
+                                               le32_to_cpu(efs.flush_to);
+                               chan->remote_acc_lat =
+                                               le32_to_cpu(efs.acc_lat);
+                               chan->remote_sdu_itime =
+                                       le32_to_cpu(efs.sdu_itime);
+                               l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
+                                       sizeof(efs), (unsigned long) &efs);
+                       }
                        break;
 
                case L2CAP_MODE_STREAMING:
-                       if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10)
-                               rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
+                       size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
+                                               chan->conn->mtu -
+                                               L2CAP_EXT_HDR_SIZE -
+                                               L2CAP_SDULEN_SIZE -
+                                               L2CAP_FCS_SIZE);
+                       rfc.max_pdu_size = cpu_to_le16(size);
+                       chan->remote_mps = size;
 
-                       chan->remote_mps = le16_to_cpu(rfc.max_pdu_size);
-
-                       chan->conf_state |= L2CAP_CONF_MODE_DONE;
+                       set_bit(CONF_MODE_DONE, &chan->conf_state);
 
                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
                                        sizeof(rfc), (unsigned long) &rfc);
@@ -1848,9 +2333,9 @@ done:
                }
 
                if (result == L2CAP_CONF_SUCCESS)
-                       chan->conf_state |= L2CAP_CONF_OUTPUT_DONE;
+                       set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
        }
-       rsp->scid   = cpu_to_le16(pi->dcid);
+       rsp->scid   = cpu_to_le16(chan->dcid);
        rsp->result = cpu_to_le16(result);
        rsp->flags  = cpu_to_le16(0x0000);
 
@@ -1859,15 +2344,13 @@ done:
 
 static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
 {
-       struct sock *sk = chan->sk;
-       struct l2cap_pinfo *pi = l2cap_pi(sk);
        struct l2cap_conf_req *req = data;
        void *ptr = req->data;
        int type, olen;
        unsigned long val;
        struct l2cap_conf_rfc rfc;
 
-       BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
+       BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data);
 
        while (len >= L2CAP_CONF_OPT_SIZE) {
                len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
@@ -1892,7 +2375,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                        if (olen == sizeof(rfc))
                                memcpy(&rfc, (void *)val, olen);
 
-                       if ((chan->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
+                       if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
                                                        rfc.mode != chan->mode)
                                return -ECONNREFUSED;
 
@@ -1901,6 +2384,13 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
                                        sizeof(rfc), (unsigned long) &rfc);
                        break;
+
+               case L2CAP_CONF_EWS:
+                       chan->tx_win = min_t(u16, val,
+                                               L2CAP_DEFAULT_EXT_WINDOW);
+                       l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
+                                                       chan->tx_win);
+                       break;
                }
        }
 
@@ -1909,7 +2399,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
 
        chan->mode = rfc.mode;
 
-       if (*result == L2CAP_CONF_SUCCESS) {
+       if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) {
                switch (rfc.mode) {
                case L2CAP_MODE_ERTM:
                        chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout);
@@ -1921,46 +2411,42 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                }
        }
 
-       req->dcid   = cpu_to_le16(pi->dcid);
+       req->dcid   = cpu_to_le16(chan->dcid);
        req->flags  = cpu_to_le16(0x0000);
 
        return ptr - data;
 }
 
-static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
+static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
 {
        struct l2cap_conf_rsp *rsp = data;
        void *ptr = rsp->data;
 
-       BT_DBG("sk %p", sk);
+       BT_DBG("chan %p", chan);
 
-       rsp->scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
+       rsp->scid   = cpu_to_le16(chan->dcid);
        rsp->result = cpu_to_le16(result);
        rsp->flags  = cpu_to_le16(flags);
 
        return ptr - data;
 }
 
-void __l2cap_connect_rsp_defer(struct sock *sk)
+void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
 {
        struct l2cap_conn_rsp rsp;
-       struct l2cap_conn *conn = l2cap_pi(sk)->conn;
-       struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+       struct l2cap_conn *conn = chan->conn;
        u8 buf[128];
 
-       sk->sk_state = BT_CONFIG;
-
-       rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
-       rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
+       rsp.scid   = cpu_to_le16(chan->dcid);
+       rsp.dcid   = cpu_to_le16(chan->scid);
        rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
        rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
        l2cap_send_cmd(conn, chan->ident,
                                L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
-       if (chan->conf_state & L2CAP_CONF_REQ_SENT)
+       if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
                return;
 
-       chan->conf_state |= L2CAP_CONF_REQ_SENT;
        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                        l2cap_build_conf_req(chan, buf), buf);
        chan->num_conf_req++;
@@ -2002,9 +2488,9 @@ done:
 
 static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
 {
-       struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
+       struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
 
-       if (rej->reason != 0x0000)
+       if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD)
                return 0;
 
        if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
@@ -2024,7 +2510,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 {
        struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
        struct l2cap_conn_rsp rsp;
-       struct l2cap_chan *chan = NULL;
+       struct l2cap_chan *chan = NULL, *pchan;
        struct sock *parent, *sk = NULL;
        int result, status = L2CAP_CS_NO_INFO;
 
@@ -2034,12 +2520,14 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
        BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid);
 
        /* Check if we have socket listening on psm */
-       parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src);
-       if (!parent) {
+       pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src);
+       if (!pchan) {
                result = L2CAP_CR_BAD_PSM;
                goto sendresp;
        }
 
+       parent = pchan->sk;
+
        bh_lock_sock(parent);
 
        /* Check if the ACL is secure enough (if not SDP) */
@@ -2058,17 +2546,11 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
                goto response;
        }
 
-       sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
-       if (!sk)
-               goto response;
-
-       chan = l2cap_chan_alloc(sk);
-       if (!chan) {
-               l2cap_sock_kill(sk);
+       chan = pchan->ops->new_connection(pchan->data);
+       if (!chan)
                goto response;
-       }
 
-       l2cap_pi(sk)->chan = chan;
+       sk = chan->sk;
 
        write_lock_bh(&conn->chan_lock);
 
@@ -2076,47 +2558,46 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
        if (__l2cap_get_chan_by_dcid(conn, scid)) {
                write_unlock_bh(&conn->chan_lock);
                sock_set_flag(sk, SOCK_ZAPPED);
-               l2cap_sock_kill(sk);
+               chan->ops->close(chan->data);
                goto response;
        }
 
        hci_conn_hold(conn->hcon);
 
-       l2cap_sock_init(sk, parent);
        bacpy(&bt_sk(sk)->src, conn->src);
        bacpy(&bt_sk(sk)->dst, conn->dst);
-       l2cap_pi(sk)->psm  = psm;
-       l2cap_pi(sk)->dcid = scid;
+       chan->psm  = psm;
+       chan->dcid = scid;
 
        bt_accept_enqueue(parent, sk);
 
        __l2cap_chan_add(conn, chan);
 
-       dcid = l2cap_pi(sk)->scid;
+       dcid = chan->scid;
 
-       l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
+       __set_chan_timer(chan, sk->sk_sndtimeo);
 
        chan->ident = cmd->ident;
 
        if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
                if (l2cap_check_security(chan)) {
                        if (bt_sk(sk)->defer_setup) {
-                               sk->sk_state = BT_CONNECT2;
+                               l2cap_state_change(chan, BT_CONNECT2);
                                result = L2CAP_CR_PEND;
                                status = L2CAP_CS_AUTHOR_PEND;
                                parent->sk_data_ready(parent, 0);
                        } else {
-                               sk->sk_state = BT_CONFIG;
+                               l2cap_state_change(chan, BT_CONFIG);
                                result = L2CAP_CR_SUCCESS;
                                status = L2CAP_CS_NO_INFO;
                        }
                } else {
-                       sk->sk_state = BT_CONNECT2;
+                       l2cap_state_change(chan, BT_CONNECT2);
                        result = L2CAP_CR_PEND;
                        status = L2CAP_CS_AUTHEN_PEND;
                }
        } else {
-               sk->sk_state = BT_CONNECT2;
+               l2cap_state_change(chan, BT_CONNECT2);
                result = L2CAP_CR_PEND;
                status = L2CAP_CS_NO_INFO;
        }
@@ -2147,10 +2628,10 @@ sendresp:
                                        L2CAP_INFO_REQ, sizeof(info), &info);
        }
 
-       if (chan && !(chan->conf_state & L2CAP_CONF_REQ_SENT) &&
+       if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
                                result == L2CAP_CR_SUCCESS) {
                u8 buf[128];
-               chan->conf_state |= L2CAP_CONF_REQ_SENT;
+               set_bit(CONF_REQ_SENT, &chan->conf_state);
                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                        l2cap_build_conf_req(chan, buf), buf);
                chan->num_conf_req++;
@@ -2188,31 +2669,29 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
        switch (result) {
        case L2CAP_CR_SUCCESS:
-               sk->sk_state = BT_CONFIG;
+               l2cap_state_change(chan, BT_CONFIG);
                chan->ident = 0;
-               l2cap_pi(sk)->dcid = dcid;
-               chan->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
+               chan->dcid = dcid;
+               clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
 
-               if (chan->conf_state & L2CAP_CONF_REQ_SENT)
+               if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
                        break;
 
-               chan->conf_state |= L2CAP_CONF_REQ_SENT;
-
                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                        l2cap_build_conf_req(chan, req), req);
                chan->num_conf_req++;
                break;
 
        case L2CAP_CR_PEND:
-               chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
+               set_bit(CONF_CONNECT_PEND, &chan->conf_state);
                break;
 
        default:
                /* don't delete l2cap channel if sk is owned by user */
                if (sock_owned_by_user(sk)) {
-                       sk->sk_state = BT_DISCONN;
-                       l2cap_sock_clear_timer(sk);
-                       l2cap_sock_set_timer(sk, HZ / 5);
+                       l2cap_state_change(chan, BT_DISCONN);
+                       __clear_chan_timer(chan);
+                       __set_chan_timer(chan, HZ / 5);
                        break;
                }
 
@@ -2226,14 +2705,12 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
 static inline void set_default_fcs(struct l2cap_chan *chan)
 {
-       struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
-
        /* FCS is enabled only in ERTM or streaming mode, if one or both
         * sides request it.
         */
        if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING)
                chan->fcs = L2CAP_FCS_NONE;
-       else if (!(pi->chan->conf_state & L2CAP_CONF_NO_FCS_RECV))
+       else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state))
                chan->fcs = L2CAP_FCS_CRC16;
 }
 
@@ -2257,10 +2734,13 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
        sk = chan->sk;
 
-       if (sk->sk_state != BT_CONFIG) {
-               struct l2cap_cmd_rej rej;
+       if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) {
+               struct l2cap_cmd_rej_cid rej;
+
+               rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID);
+               rej.scid = cpu_to_le16(chan->scid);
+               rej.dcid = cpu_to_le16(chan->dcid);
 
-               rej.reason = cpu_to_le16(0x0002);
                l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
                                sizeof(rej), &rej);
                goto unlock;
@@ -2268,9 +2748,9 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
        /* Reject if config buffer is too small. */
        len = cmd_len - sizeof(*req);
-       if (chan->conf_len + len > sizeof(chan->conf_req)) {
+       if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                               l2cap_build_conf_rsp(sk, rsp,
+                               l2cap_build_conf_rsp(chan, rsp,
                                        L2CAP_CONF_REJECT, flags), rsp);
                goto unlock;
        }
@@ -2282,7 +2762,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
        if (flags & 0x0001) {
                /* Incomplete config. Send empty response. */
                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                               l2cap_build_conf_rsp(sk, rsp,
+                               l2cap_build_conf_rsp(chan, rsp,
                                        L2CAP_CONF_SUCCESS, 0x0001), rsp);
                goto unlock;
        }
@@ -2300,13 +2780,13 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
        /* Reset config buffer. */
        chan->conf_len = 0;
 
-       if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE))
+       if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state))
                goto unlock;
 
-       if (chan->conf_state & L2CAP_CONF_INPUT_DONE) {
+       if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
                set_default_fcs(chan);
 
-               sk->sk_state = BT_CONNECTED;
+               l2cap_state_change(chan, BT_CONNECTED);
 
                chan->next_tx_seq = 0;
                chan->expected_tx_seq = 0;
@@ -2318,14 +2798,28 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                goto unlock;
        }
 
-       if (!(chan->conf_state & L2CAP_CONF_REQ_SENT)) {
+       if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
                u8 buf[64];
-               chan->conf_state |= L2CAP_CONF_REQ_SENT;
                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                        l2cap_build_conf_req(chan, buf), buf);
                chan->num_conf_req++;
        }
 
+       /* Got Conf Rsp PENDING from remote side and asume we sent
+          Conf Rsp PENDING in the code above */
+       if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
+                       test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
+
+               /* check compatibility */
+
+               clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
+               set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
+
+               l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
+                                       l2cap_build_conf_rsp(chan, rsp,
+                                       L2CAP_CONF_SUCCESS, 0x0000), rsp);
+       }
+
 unlock:
        bh_unlock_sock(sk);
        return 0;
@@ -2355,8 +2849,33 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
        switch (result) {
        case L2CAP_CONF_SUCCESS:
                l2cap_conf_rfc_get(chan, rsp->data, len);
+               clear_bit(CONF_REM_CONF_PEND, &chan->conf_state);
                break;
 
+       case L2CAP_CONF_PENDING:
+               set_bit(CONF_REM_CONF_PEND, &chan->conf_state);
+
+               if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
+                       char buf[64];
+
+                       len = l2cap_parse_conf_rsp(chan, rsp->data, len,
+                                                               buf, &result);
+                       if (len < 0) {
+                               l2cap_send_disconn_req(conn, chan, ECONNRESET);
+                               goto done;
+                       }
+
+                       /* check compatibility */
+
+                       clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
+                       set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
+
+                       l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
+                                               l2cap_build_conf_rsp(chan, buf,
+                                               L2CAP_CONF_SUCCESS, 0x0000), buf);
+               }
+               goto done;
+
        case L2CAP_CONF_UNACCEPT:
                if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
                        char req[64];
@@ -2385,7 +2904,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
        default:
                sk->sk_err = ECONNRESET;
-               l2cap_sock_set_timer(sk, HZ * 5);
+               __set_chan_timer(chan, HZ * 5);
                l2cap_send_disconn_req(conn, chan, ECONNRESET);
                goto done;
        }
@@ -2393,12 +2912,12 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
        if (flags & 0x01)
                goto done;
 
-       chan->conf_state |= L2CAP_CONF_INPUT_DONE;
+       set_bit(CONF_INPUT_DONE, &chan->conf_state);
 
-       if (chan->conf_state & L2CAP_CONF_OUTPUT_DONE) {
+       if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
                set_default_fcs(chan);
 
-               sk->sk_state = BT_CONNECTED;
+               l2cap_state_change(chan, BT_CONNECTED);
                chan->next_tx_seq = 0;
                chan->expected_tx_seq = 0;
                skb_queue_head_init(&chan->tx_q);
@@ -2432,17 +2951,17 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 
        sk = chan->sk;
 
-       rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
-       rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
+       rsp.dcid = cpu_to_le16(chan->scid);
+       rsp.scid = cpu_to_le16(chan->dcid);
        l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp);
 
        sk->sk_shutdown = SHUTDOWN_MASK;
 
        /* don't delete l2cap channel if sk is owned by user */
        if (sock_owned_by_user(sk)) {
-               sk->sk_state = BT_DISCONN;
-               l2cap_sock_clear_timer(sk);
-               l2cap_sock_set_timer(sk, HZ / 5);
+               l2cap_state_change(chan, BT_DISCONN);
+               __clear_chan_timer(chan);
+               __set_chan_timer(chan, HZ / 5);
                bh_unlock_sock(sk);
                return 0;
        }
@@ -2450,7 +2969,7 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
        l2cap_chan_del(chan, ECONNRESET);
        bh_unlock_sock(sk);
 
-       l2cap_sock_kill(sk);
+       chan->ops->close(chan->data);
        return 0;
 }
 
@@ -2474,9 +2993,9 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
 
        /* don't delete l2cap channel if sk is owned by user */
        if (sock_owned_by_user(sk)) {
-               sk->sk_state = BT_DISCONN;
-               l2cap_sock_clear_timer(sk);
-               l2cap_sock_set_timer(sk, HZ / 5);
+               l2cap_state_change(chan,BT_DISCONN);
+               __clear_chan_timer(chan);
+               __set_chan_timer(chan, HZ / 5);
                bh_unlock_sock(sk);
                return 0;
        }
@@ -2484,7 +3003,7 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
        l2cap_chan_del(chan, 0);
        bh_unlock_sock(sk);
 
-       l2cap_sock_kill(sk);
+       chan->ops->close(chan->data);
        return 0;
 }
 
@@ -2506,15 +3025,25 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                if (!disable_ertm)
                        feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
                                                         | L2CAP_FEAT_FCS;
+               if (enable_hs)
+                       feat_mask |= L2CAP_FEAT_EXT_FLOW
+                                               | L2CAP_FEAT_EXT_WINDOW;
+
                put_unaligned_le32(feat_mask, rsp->data);
                l2cap_send_cmd(conn, cmd->ident,
                                        L2CAP_INFO_RSP, sizeof(buf), buf);
        } else if (type == L2CAP_IT_FIXED_CHAN) {
                u8 buf[12];
                struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
+
+               if (enable_hs)
+                       l2cap_fixed_chan[0] |= L2CAP_FC_A2MP;
+               else
+                       l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP;
+
                rsp->type   = cpu_to_le16(L2CAP_IT_FIXED_CHAN);
                rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
-               memcpy(buf + 4, l2cap_fixed_chan, 8);
+               memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
                l2cap_send_cmd(conn, cmd->ident,
                                        L2CAP_INFO_RSP, sizeof(buf), buf);
        } else {
@@ -2581,6 +3110,165 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
        return 0;
 }
 
+static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
+                                       struct l2cap_cmd_hdr *cmd, u16 cmd_len,
+                                       void *data)
+{
+       struct l2cap_create_chan_req *req = data;
+       struct l2cap_create_chan_rsp rsp;
+       u16 psm, scid;
+
+       if (cmd_len != sizeof(*req))
+               return -EPROTO;
+
+       if (!enable_hs)
+               return -EINVAL;
+
+       psm = le16_to_cpu(req->psm);
+       scid = le16_to_cpu(req->scid);
+
+       BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id);
+
+       /* Placeholder: Always reject */
+       rsp.dcid = 0;
+       rsp.scid = cpu_to_le16(scid);
+       rsp.result = L2CAP_CR_NO_MEM;
+       rsp.status = L2CAP_CS_NO_INFO;
+
+       l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP,
+                      sizeof(rsp), &rsp);
+
+       return 0;
+}
+
+static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
+                                       struct l2cap_cmd_hdr *cmd, void *data)
+{
+       BT_DBG("conn %p", conn);
+
+       return l2cap_connect_rsp(conn, cmd, data);
+}
+
+static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident,
+                                                       u16 icid, u16 result)
+{
+       struct l2cap_move_chan_rsp rsp;
+
+       BT_DBG("icid %d, result %d", icid, result);
+
+       rsp.icid = cpu_to_le16(icid);
+       rsp.result = cpu_to_le16(result);
+
+       l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp);
+}
+
+static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn,
+                               struct l2cap_chan *chan, u16 icid, u16 result)
+{
+       struct l2cap_move_chan_cfm cfm;
+       u8 ident;
+
+       BT_DBG("icid %d, result %d", icid, result);
+
+       ident = l2cap_get_ident(conn);
+       if (chan)
+               chan->ident = ident;
+
+       cfm.icid = cpu_to_le16(icid);
+       cfm.result = cpu_to_le16(result);
+
+       l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm);
+}
+
+static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident,
+                                                               u16 icid)
+{
+       struct l2cap_move_chan_cfm_rsp rsp;
+
+       BT_DBG("icid %d", icid);
+
+       rsp.icid = cpu_to_le16(icid);
+       l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp);
+}
+
+static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
+                       struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
+{
+       struct l2cap_move_chan_req *req = data;
+       u16 icid = 0;
+       u16 result = L2CAP_MR_NOT_ALLOWED;
+
+       if (cmd_len != sizeof(*req))
+               return -EPROTO;
+
+       icid = le16_to_cpu(req->icid);
+
+       BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id);
+
+       if (!enable_hs)
+               return -EINVAL;
+
+       /* Placeholder: Always refuse */
+       l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result);
+
+       return 0;
+}
+
+static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn,
+                       struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
+{
+       struct l2cap_move_chan_rsp *rsp = data;
+       u16 icid, result;
+
+       if (cmd_len != sizeof(*rsp))
+               return -EPROTO;
+
+       icid = le16_to_cpu(rsp->icid);
+       result = le16_to_cpu(rsp->result);
+
+       BT_DBG("icid %d, result %d", icid, result);
+
+       /* Placeholder: Always unconfirmed */
+       l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED);
+
+       return 0;
+}
+
+static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn,
+                       struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
+{
+       struct l2cap_move_chan_cfm *cfm = data;
+       u16 icid, result;
+
+       if (cmd_len != sizeof(*cfm))
+               return -EPROTO;
+
+       icid = le16_to_cpu(cfm->icid);
+       result = le16_to_cpu(cfm->result);
+
+       BT_DBG("icid %d, result %d", icid, result);
+
+       l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid);
+
+       return 0;
+}
+
+static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
+                       struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data)
+{
+       struct l2cap_move_chan_cfm_rsp *rsp = data;
+       u16 icid;
+
+       if (cmd_len != sizeof(*rsp))
+               return -EPROTO;
+
+       icid = le16_to_cpu(rsp->icid);
+
+       BT_DBG("icid %d", icid);
+
+       return 0;
+}
+
 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
                                                        u16 to_multiplier)
 {
@@ -2693,6 +3381,30 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
                err = l2cap_information_rsp(conn, cmd, data);
                break;
 
+       case L2CAP_CREATE_CHAN_REQ:
+               err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
+               break;
+
+       case L2CAP_CREATE_CHAN_RSP:
+               err = l2cap_create_channel_rsp(conn, cmd, data);
+               break;
+
+       case L2CAP_MOVE_CHAN_REQ:
+               err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
+               break;
+
+       case L2CAP_MOVE_CHAN_RSP:
+               err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data);
+               break;
+
+       case L2CAP_MOVE_CHAN_CFM:
+               err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data);
+               break;
+
+       case L2CAP_MOVE_CHAN_CFM_RSP:
+               err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data);
+               break;
+
        default:
                BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code);
                err = -EINVAL;
@@ -2752,12 +3464,12 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
                        err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data);
 
                if (err) {
-                       struct l2cap_cmd_rej rej;
+                       struct l2cap_cmd_rej_unk rej;
 
                        BT_ERR("Wrong link type (%d)", err);
 
                        /* FIXME: Map err to a valid reason */
-                       rej.reason = cpu_to_le16(0);
+                       rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
                        l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
                }
 
@@ -2771,10 +3483,15 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
 static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
 {
        u16 our_fcs, rcv_fcs;
-       int hdr_size = L2CAP_HDR_SIZE + 2;
+       int hdr_size;
+
+       if (test_bit(FLAG_EXT_CTRL, &chan->flags))
+               hdr_size = L2CAP_EXT_HDR_SIZE;
+       else
+               hdr_size = L2CAP_ENH_HDR_SIZE;
 
        if (chan->fcs == L2CAP_FCS_CRC16) {
-               skb_trim(skb, skb->len - 2);
+               skb_trim(skb, skb->len - L2CAP_FCS_SIZE);
                rcv_fcs = get_unaligned_le16(skb->data + skb->len);
                our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
 
@@ -2786,31 +3503,31 @@ static int l2cap_check_fcs(struct l2cap_chan *chan,  struct sk_buff *skb)
 
 static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
 {
-       u16 control = 0;
+       u32 control = 0;
 
        chan->frames_sent = 0;
 
-       control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+       control |= __set_reqseq(chan, chan->buffer_seq);
 
-       if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
-               control |= L2CAP_SUPER_RCV_NOT_READY;
+       if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
                l2cap_send_sframe(chan, control);
-               chan->conn_state |= L2CAP_CONN_RNR_SENT;
+               set_bit(CONN_RNR_SENT, &chan->conn_state);
        }
 
-       if (chan->conn_state & L2CAP_CONN_REMOTE_BUSY)
+       if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state))
                l2cap_retransmit_frames(chan);
 
        l2cap_ertm_send(chan);
 
-       if (!(chan->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
+       if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) &&
                        chan->frames_sent == 0) {
-               control |= L2CAP_SUPER_RCV_READY;
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
                l2cap_send_sframe(chan, control);
        }
 }
 
-static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar)
+static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar)
 {
        struct sk_buff *next_skb;
        int tx_seq_offset, next_tx_seq_offset;
@@ -2824,18 +3541,14 @@ static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb,
                return 0;
        }
 
-       tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
-       if (tx_seq_offset < 0)
-               tx_seq_offset += 64;
+       tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
 
        do {
                if (bt_cb(next_skb)->tx_seq == tx_seq)
                        return -EINVAL;
 
-               next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
-                                               chan->buffer_seq) % 64;
-               if (next_tx_seq_offset < 0)
-                       next_tx_seq_offset += 64;
+               next_tx_seq_offset = __seq_offset(chan,
+                               bt_cb(next_skb)->tx_seq, chan->buffer_seq);
 
                if (next_tx_seq_offset > tx_seq_offset) {
                        __skb_queue_before(&chan->srej_q, next_skb, skb);
@@ -2852,353 +3565,188 @@ static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb,
        return 0;
 }
 
-static int l2cap_ertm_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
+static void append_skb_frag(struct sk_buff *skb,
+                       struct sk_buff *new_frag, struct sk_buff **last_frag)
 {
-       struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
-       struct sk_buff *_skb;
-       int err;
+       /* skb->len reflects data in skb as well as all fragments
+        * skb->data_len reflects only data in fragments
+        */
+       if (!skb_has_frag_list(skb))
+               skb_shinfo(skb)->frag_list = new_frag;
 
-       switch (control & L2CAP_CTRL_SAR) {
-       case L2CAP_SDU_UNSEGMENTED:
-               if (chan->conn_state & L2CAP_CONN_SAR_SDU)
-                       goto drop;
+       new_frag->next = NULL;
 
-               err = sock_queue_rcv_skb(chan->sk, skb);
-               if (!err)
-                       return err;
+       (*last_frag)->next = new_frag;
+       *last_frag = new_frag;
+
+       skb->len += new_frag->len;
+       skb->data_len += new_frag->len;
+       skb->truesize += new_frag->truesize;
+}
+
+static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
+{
+       int err = -EINVAL;
+
+       switch (__get_ctrl_sar(chan, control)) {
+       case L2CAP_SAR_UNSEGMENTED:
+               if (chan->sdu)
+                       break;
 
+               err = chan->ops->recv(chan->data, skb);
                break;
 
-       case L2CAP_SDU_START:
-               if (chan->conn_state & L2CAP_CONN_SAR_SDU)
-                       goto drop;
+       case L2CAP_SAR_START:
+               if (chan->sdu)
+                       break;
 
                chan->sdu_len = get_unaligned_le16(skb->data);
+               skb_pull(skb, L2CAP_SDULEN_SIZE);
 
-               if (chan->sdu_len > chan->imtu)
-                       goto disconnect;
-
-               chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC);
-               if (!chan->sdu)
-                       return -ENOMEM;
+               if (chan->sdu_len > chan->imtu) {
+                       err = -EMSGSIZE;
+                       break;
+               }
 
-               /* pull sdu_len bytes only after alloc, because of Local Busy
-                * condition we have to be sure that this will be executed
-                * only once, i.e., when alloc does not fail */
-               skb_pull(skb, 2);
+               if (skb->len >= chan->sdu_len)
+                       break;
 
-               memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
+               chan->sdu = skb;
+               chan->sdu_last_frag = skb;
 
-               chan->conn_state |= L2CAP_CONN_SAR_SDU;
-               chan->partial_sdu_len = skb->len;
+               skb = NULL;
+               err = 0;
                break;
 
-       case L2CAP_SDU_CONTINUE:
-               if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
-                       goto disconnect;
-
+       case L2CAP_SAR_CONTINUE:
                if (!chan->sdu)
-                       goto disconnect;
+                       break;
 
-               chan->partial_sdu_len += skb->len;
-               if (chan->partial_sdu_len > chan->sdu_len)
-                       goto drop;
+               append_skb_frag(chan->sdu, skb,
+                               &chan->sdu_last_frag);
+               skb = NULL;
 
-               memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
+               if (chan->sdu->len >= chan->sdu_len)
+                       break;
 
+               err = 0;
                break;
 
-       case L2CAP_SDU_END:
-               if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
-                       goto disconnect;
-
+       case L2CAP_SAR_END:
                if (!chan->sdu)
-                       goto disconnect;
-
-               if (!(chan->conn_state & L2CAP_CONN_SAR_RETRY)) {
-                       chan->partial_sdu_len += skb->len;
+                       break;
 
-                       if (chan->partial_sdu_len > chan->imtu)
-                               goto drop;
+               append_skb_frag(chan->sdu, skb,
+                               &chan->sdu_last_frag);
+               skb = NULL;
 
-                       if (chan->partial_sdu_len != chan->sdu_len)
-                               goto drop;
+               if (chan->sdu->len != chan->sdu_len)
+                       break;
 
-                       memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
-               }
+               err = chan->ops->recv(chan->data, chan->sdu);
 
-               _skb = skb_clone(chan->sdu, GFP_ATOMIC);
-               if (!_skb) {
-                       chan->conn_state |= L2CAP_CONN_SAR_RETRY;
-                       return -ENOMEM;
+               if (!err) {
+                       /* Reassembly complete */
+                       chan->sdu = NULL;
+                       chan->sdu_last_frag = NULL;
+                       chan->sdu_len = 0;
                }
-
-               err = sock_queue_rcv_skb(chan->sk, _skb);
-               if (err < 0) {
-                       kfree_skb(_skb);
-                       chan->conn_state |= L2CAP_CONN_SAR_RETRY;
-                       return err;
-               }
-
-               chan->conn_state &= ~L2CAP_CONN_SAR_RETRY;
-               chan->conn_state &= ~L2CAP_CONN_SAR_SDU;
-
-               kfree_skb(chan->sdu);
                break;
        }
 
-       kfree_skb(skb);
-       return 0;
-
-drop:
-       kfree_skb(chan->sdu);
-       chan->sdu = NULL;
+       if (err) {
+               kfree_skb(skb);
+               kfree_skb(chan->sdu);
+               chan->sdu = NULL;
+               chan->sdu_last_frag = NULL;
+               chan->sdu_len = 0;
+       }
 
-disconnect:
-       l2cap_send_disconn_req(pi->conn, chan, ECONNRESET);
-       kfree_skb(skb);
-       return 0;
+       return err;
 }
 
-static int l2cap_try_push_rx_skb(struct l2cap_chan *chan)
+static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
 {
-       struct sk_buff *skb;
-       u16 control;
-       int err;
+       u32 control;
 
-       while ((skb = skb_dequeue(&chan->busy_q))) {
-               control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
-               err = l2cap_ertm_reassembly_sdu(chan, skb, control);
-               if (err < 0) {
-                       skb_queue_head(&chan->busy_q, skb);
-                       return -EBUSY;
-               }
+       BT_DBG("chan %p, Enter local busy", chan);
 
-               chan->buffer_seq = (chan->buffer_seq + 1) % 64;
-       }
+       set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
 
-       if (!(chan->conn_state & L2CAP_CONN_RNR_SENT))
-               goto done;
-
-       control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
-       control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
+       control = __set_reqseq(chan, chan->buffer_seq);
+       control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
        l2cap_send_sframe(chan, control);
-       chan->retry_count = 1;
 
-       del_timer(&chan->retrans_timer);
-       __mod_monitor_timer();
+       set_bit(CONN_RNR_SENT, &chan->conn_state);
 
-       chan->conn_state |= L2CAP_CONN_WAIT_F;
-
-done:
-       chan->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
-       chan->conn_state &= ~L2CAP_CONN_RNR_SENT;
-
-       BT_DBG("chan %p, Exit local busy", chan);
-
-       return 0;
+       __clear_ack_timer(chan);
 }
 
-static void l2cap_busy_work(struct work_struct *work)
+static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
 {
-       DECLARE_WAITQUEUE(wait, current);
-       struct l2cap_chan *chan =
-               container_of(work, struct l2cap_chan, busy_work);
-       struct sock *sk = chan->sk;
-       int n_tries = 0, timeo = HZ/5, err;
-       struct sk_buff *skb;
-
-       lock_sock(sk);
-
-       add_wait_queue(sk_sleep(sk), &wait);
-       while ((skb = skb_peek(&chan->busy_q))) {
-               set_current_state(TASK_INTERRUPTIBLE);
-
-               if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
-                       err = -EBUSY;
-                       l2cap_send_disconn_req(l2cap_pi(sk)->conn, chan, EBUSY);
-                       break;
-               }
-
-               if (!timeo)
-                       timeo = HZ/5;
+       u32 control;
 
-               if (signal_pending(current)) {
-                       err = sock_intr_errno(timeo);
-                       break;
-               }
+       if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
+               goto done;
 
-               release_sock(sk);
-               timeo = schedule_timeout(timeo);
-               lock_sock(sk);
+       control = __set_reqseq(chan, chan->buffer_seq);
+       control |= __set_ctrl_poll(chan);
+       control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
+       l2cap_send_sframe(chan, control);
+       chan->retry_count = 1;
 
-               err = sock_error(sk);
-               if (err)
-                       break;
+       __clear_retrans_timer(chan);
+       __set_monitor_timer(chan);
 
-               if (l2cap_try_push_rx_skb(chan) == 0)
-                       break;
-       }
+       set_bit(CONN_WAIT_F, &chan->conn_state);
 
-       set_current_state(TASK_RUNNING);
-       remove_wait_queue(sk_sleep(sk), &wait);
+done:
+       clear_bit(CONN_LOCAL_BUSY, &chan->conn_state);
+       clear_bit(CONN_RNR_SENT, &chan->conn_state);
 
-       release_sock(sk);
+       BT_DBG("chan %p, Exit local busy", chan);
 }
 
-static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
+void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
 {
-       int sctrl, err;
-
-       if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) {
-               bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
-               __skb_queue_tail(&chan->busy_q, skb);
-               return l2cap_try_push_rx_skb(chan);
-
-
-       }
-
-       err = l2cap_ertm_reassembly_sdu(chan, skb, control);
-       if (err >= 0) {
-               chan->buffer_seq = (chan->buffer_seq + 1) % 64;
-               return err;
+       if (chan->mode == L2CAP_MODE_ERTM) {
+               if (busy)
+                       l2cap_ertm_enter_local_busy(chan);
+               else
+                       l2cap_ertm_exit_local_busy(chan);
        }
-
-       /* Busy Condition */
-       BT_DBG("chan %p, Enter local busy", chan);
-
-       chan->conn_state |= L2CAP_CONN_LOCAL_BUSY;
-       bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
-       __skb_queue_tail(&chan->busy_q, skb);
-
-       sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
-       sctrl |= L2CAP_SUPER_RCV_NOT_READY;
-       l2cap_send_sframe(chan, sctrl);
-
-       chan->conn_state |= L2CAP_CONN_RNR_SENT;
-
-       del_timer(&chan->ack_timer);
-
-       queue_work(_busy_wq, &chan->busy_work);
-
-       return err;
 }
 
-static int l2cap_streaming_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
+static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
 {
-       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 (chan->conn_state & L2CAP_CONN_SAR_SDU) {
-                       kfree_skb(chan->sdu);
-                       break;
-               }
-
-               err = sock_queue_rcv_skb(chan->sk, skb);
-               if (!err)
-                       return 0;
-
-               break;
-
-       case L2CAP_SDU_START:
-               if (chan->conn_state & L2CAP_CONN_SAR_SDU) {
-                       kfree_skb(chan->sdu);
-                       break;
-               }
-
-               chan->sdu_len = get_unaligned_le16(skb->data);
-               skb_pull(skb, 2);
-
-               if (chan->sdu_len > chan->imtu) {
-                       err = -EMSGSIZE;
-                       break;
-               }
-
-               chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC);
-               if (!chan->sdu) {
-                       err = -ENOMEM;
-                       break;
-               }
+       struct sk_buff *skb;
+       u32 control;
 
-               memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
+       while ((skb = skb_peek(&chan->srej_q)) &&
+                       !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
+               int err;
 
-               chan->conn_state |= L2CAP_CONN_SAR_SDU;
-               chan->partial_sdu_len = skb->len;
-               err = 0;
-               break;
-
-       case L2CAP_SDU_CONTINUE:
-               if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
+               if (bt_cb(skb)->tx_seq != tx_seq)
                        break;
 
-               memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
-
-               chan->partial_sdu_len += skb->len;
-               if (chan->partial_sdu_len > chan->sdu_len)
-                       kfree_skb(chan->sdu);
-               else
-                       err = 0;
-
-               break;
+               skb = skb_dequeue(&chan->srej_q);
+               control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
+               err = l2cap_reassemble_sdu(chan, skb, control);
 
-       case L2CAP_SDU_END:
-               if (!(chan->conn_state & L2CAP_CONN_SAR_SDU))
+               if (err < 0) {
+                       l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                        break;
-
-               memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len);
-
-               chan->conn_state &= ~L2CAP_CONN_SAR_SDU;
-               chan->partial_sdu_len += skb->len;
-
-               if (chan->partial_sdu_len > chan->imtu)
-                       goto drop;
-
-               if (chan->partial_sdu_len == chan->sdu_len) {
-                       _skb = skb_clone(chan->sdu, GFP_ATOMIC);
-                       err = sock_queue_rcv_skb(chan->sk, _skb);
-                       if (err < 0)
-                               kfree_skb(_skb);
                }
-               err = 0;
-
-drop:
-               kfree_skb(chan->sdu);
-               break;
-       }
-
-       kfree_skb(skb);
-       return err;
-}
-
-static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
-{
-       struct sk_buff *skb;
-       u16 control;
 
-       while ((skb = skb_peek(&chan->srej_q))) {
-               if (bt_cb(skb)->tx_seq != tx_seq)
-                       break;
-
-               skb = skb_dequeue(&chan->srej_q);
-               control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
-               l2cap_ertm_reassembly_sdu(chan, skb, control);
-               chan->buffer_seq_srej =
-                       (chan->buffer_seq_srej + 1) % 64;
-               tx_seq = (tx_seq + 1) % 64;
+               chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej);
+               tx_seq = __next_seq(chan, tx_seq);
        }
 }
 
-static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
+static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq)
 {
        struct srej_list *l, *tmp;
-       u16 control;
+       u32 control;
 
        list_for_each_entry_safe(l, tmp, &chan->srej_l, list) {
                if (l->tx_seq == tx_seq) {
@@ -3206,73 +3754,73 @@ static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq)
                        kfree(l);
                        return;
                }
-               control = L2CAP_SUPER_SELECT_REJECT;
-               control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+               control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
+               control |= __set_reqseq(chan, l->tx_seq);
                l2cap_send_sframe(chan, control);
                list_del(&l->list);
                list_add_tail(&l->list, &chan->srej_l);
        }
 }
 
-static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq)
+static void l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq)
 {
        struct srej_list *new;
-       u16 control;
+       u32 control;
 
        while (tx_seq != chan->expected_tx_seq) {
-               control = L2CAP_SUPER_SELECT_REJECT;
-               control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+               control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ);
+               control |= __set_reqseq(chan, chan->expected_tx_seq);
                l2cap_send_sframe(chan, control);
 
                new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
                new->tx_seq = chan->expected_tx_seq;
-               chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
+
+               chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
+
                list_add_tail(&new->list, &chan->srej_l);
        }
-       chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
+
+       chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
 }
 
-static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
+static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
 {
-       struct l2cap_pinfo *pi = l2cap_pi(chan->sk);
-       u8 tx_seq = __get_txseq(rx_control);
-       u8 req_seq = __get_reqseq(rx_control);
-       u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
+       u16 tx_seq = __get_txseq(chan, rx_control);
+       u16 req_seq = __get_reqseq(chan, rx_control);
+       u8 sar = __get_ctrl_sar(chan, rx_control);
        int tx_seq_offset, expected_tx_seq_offset;
        int num_to_ack = (chan->tx_win/6) + 1;
        int err = 0;
 
-       BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len,
+       BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len,
                                                        tx_seq, rx_control);
 
-       if (L2CAP_CTRL_FINAL & rx_control &&
-                       chan->conn_state & L2CAP_CONN_WAIT_F) {
-               del_timer(&chan->monitor_timer);
+       if (__is_ctrl_final(chan, rx_control) &&
+                       test_bit(CONN_WAIT_F, &chan->conn_state)) {
+               __clear_monitor_timer(chan);
                if (chan->unacked_frames > 0)
-                       __mod_retrans_timer();
-               chan->conn_state &= ~L2CAP_CONN_WAIT_F;
+                       __set_retrans_timer(chan);
+               clear_bit(CONN_WAIT_F, &chan->conn_state);
        }
 
        chan->expected_ack_seq = req_seq;
        l2cap_drop_acked_frames(chan);
 
-       if (tx_seq == chan->expected_tx_seq)
-               goto expected;
-
-       tx_seq_offset = (tx_seq - chan->buffer_seq) % 64;
-       if (tx_seq_offset < 0)
-               tx_seq_offset += 64;
+       tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq);
 
        /* invalid tx_seq */
        if (tx_seq_offset >= chan->tx_win) {
-               l2cap_send_disconn_req(pi->conn, chan, ECONNRESET);
+               l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                goto drop;
        }
 
-       if (chan->conn_state == L2CAP_CONN_LOCAL_BUSY)
+       if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state))
                goto drop;
 
-       if (chan->conn_state & L2CAP_CONN_SREJ_SENT) {
+       if (tx_seq == chan->expected_tx_seq)
+               goto expected;
+
+       if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
                struct srej_list *first;
 
                first = list_first_entry(&chan->srej_l,
@@ -3286,7 +3834,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
 
                        if (list_empty(&chan->srej_l)) {
                                chan->buffer_seq = chan->buffer_seq_srej;
-                               chan->conn_state &= ~L2CAP_CONN_SREJ_SENT;
+                               clear_bit(CONN_SREJ_SENT, &chan->conn_state);
                                l2cap_send_ack(chan);
                                BT_DBG("chan %p, Exit SREJ_SENT", chan);
                        }
@@ -3306,16 +3854,14 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
                        l2cap_send_srejframe(chan, tx_seq);
                }
        } else {
-               expected_tx_seq_offset =
-                       (chan->expected_tx_seq - chan->buffer_seq) % 64;
-               if (expected_tx_seq_offset < 0)
-                       expected_tx_seq_offset += 64;
+               expected_tx_seq_offset = __seq_offset(chan,
+                               chan->expected_tx_seq, chan->buffer_seq);
 
                /* duplicated tx_seq */
                if (tx_seq_offset < expected_tx_seq_offset)
                        goto drop;
 
-               chan->conn_state |= L2CAP_CONN_SREJ_SENT;
+               set_bit(CONN_SREJ_SENT, &chan->conn_state);
 
                BT_DBG("chan %p, Enter SREJ", chan);
 
@@ -3323,39 +3869,40 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
                chan->buffer_seq_srej = chan->buffer_seq;
 
                __skb_queue_head_init(&chan->srej_q);
-               __skb_queue_head_init(&chan->busy_q);
                l2cap_add_to_srej_queue(chan, skb, tx_seq, sar);
 
-               chan->conn_state |= L2CAP_CONN_SEND_PBIT;
+               set_bit(CONN_SEND_PBIT, &chan->conn_state);
 
                l2cap_send_srejframe(chan, tx_seq);
 
-               del_timer(&chan->ack_timer);
+               __clear_ack_timer(chan);
        }
        return 0;
 
 expected:
-       chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
+       chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq);
 
-       if (chan->conn_state & L2CAP_CONN_SREJ_SENT) {
+       if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
                bt_cb(skb)->tx_seq = tx_seq;
                bt_cb(skb)->sar = sar;
                __skb_queue_tail(&chan->srej_q, skb);
                return 0;
        }
 
-       err = l2cap_push_rx_skb(chan, skb, rx_control);
-       if (err < 0)
-               return 0;
+       err = l2cap_reassemble_sdu(chan, skb, rx_control);
+       chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
 
-       if (rx_control & L2CAP_CTRL_FINAL) {
-               if (chan->conn_state & L2CAP_CONN_REJ_ACT)
-                       chan->conn_state &= ~L2CAP_CONN_REJ_ACT;
-               else
+       if (err < 0) {
+               l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
+               return err;
+       }
+
+       if (__is_ctrl_final(chan, rx_control)) {
+               if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
                        l2cap_retransmit_frames(chan);
        }
 
-       __mod_ack_timer();
+       __set_ack_timer(chan);
 
        chan->num_acked = (chan->num_acked + 1) % num_to_ack;
        if (chan->num_acked == num_to_ack - 1)
@@ -3368,159 +3915,157 @@ drop:
        return 0;
 }
 
-static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control)
+static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control)
 {
-       BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control),
-                                               rx_control);
+       BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan,
+                               __get_reqseq(chan, rx_control), rx_control);
 
-       chan->expected_ack_seq = __get_reqseq(rx_control);
+       chan->expected_ack_seq = __get_reqseq(chan, rx_control);
        l2cap_drop_acked_frames(chan);
 
-       if (rx_control & L2CAP_CTRL_POLL) {
-               chan->conn_state |= L2CAP_CONN_SEND_FBIT;
-               if (chan->conn_state & L2CAP_CONN_SREJ_SENT) {
-                       if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
+       if (__is_ctrl_poll(chan, rx_control)) {
+               set_bit(CONN_SEND_FBIT, &chan->conn_state);
+               if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
+                       if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
                                        (chan->unacked_frames > 0))
-                               __mod_retrans_timer();
+                               __set_retrans_timer(chan);
 
-                       chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+                       clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
                        l2cap_send_srejtail(chan);
                } else {
                        l2cap_send_i_or_rr_or_rnr(chan);
                }
 
-       } else if (rx_control & L2CAP_CTRL_FINAL) {
-               chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+       } else if (__is_ctrl_final(chan, rx_control)) {
+               clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-               if (chan->conn_state & L2CAP_CONN_REJ_ACT)
-                       chan->conn_state &= ~L2CAP_CONN_REJ_ACT;
-               else
+               if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
                        l2cap_retransmit_frames(chan);
 
        } else {
-               if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
+               if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) &&
                                (chan->unacked_frames > 0))
-                       __mod_retrans_timer();
+                       __set_retrans_timer(chan);
 
-               chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
-               if (chan->conn_state & L2CAP_CONN_SREJ_SENT)
+               clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
+               if (test_bit(CONN_SREJ_SENT, &chan->conn_state))
                        l2cap_send_ack(chan);
                else
                        l2cap_ertm_send(chan);
        }
 }
 
-static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control)
+static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control)
 {
-       u8 tx_seq = __get_reqseq(rx_control);
+       u16 tx_seq = __get_reqseq(chan, rx_control);
 
-       BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
+       BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
 
-       chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+       clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
        chan->expected_ack_seq = tx_seq;
        l2cap_drop_acked_frames(chan);
 
-       if (rx_control & L2CAP_CTRL_FINAL) {
-               if (chan->conn_state & L2CAP_CONN_REJ_ACT)
-                       chan->conn_state &= ~L2CAP_CONN_REJ_ACT;
-               else
+       if (__is_ctrl_final(chan, rx_control)) {
+               if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
                        l2cap_retransmit_frames(chan);
        } else {
                l2cap_retransmit_frames(chan);
 
-               if (chan->conn_state & L2CAP_CONN_WAIT_F)
-                       chan->conn_state |= L2CAP_CONN_REJ_ACT;
+               if (test_bit(CONN_WAIT_F, &chan->conn_state))
+                       set_bit(CONN_REJ_ACT, &chan->conn_state);
        }
 }
-static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control)
+static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control)
 {
-       u8 tx_seq = __get_reqseq(rx_control);
+       u16 tx_seq = __get_reqseq(chan, rx_control);
 
-       BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
+       BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
 
-       chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+       clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
-       if (rx_control & L2CAP_CTRL_POLL) {
+       if (__is_ctrl_poll(chan, rx_control)) {
                chan->expected_ack_seq = tx_seq;
                l2cap_drop_acked_frames(chan);
 
-               chan->conn_state |= L2CAP_CONN_SEND_FBIT;
+               set_bit(CONN_SEND_FBIT, &chan->conn_state);
                l2cap_retransmit_one_frame(chan, tx_seq);
 
                l2cap_ertm_send(chan);
 
-               if (chan->conn_state & L2CAP_CONN_WAIT_F) {
+               if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
                        chan->srej_save_reqseq = tx_seq;
-                       chan->conn_state |= L2CAP_CONN_SREJ_ACT;
+                       set_bit(CONN_SREJ_ACT, &chan->conn_state);
                }
-       } else if (rx_control & L2CAP_CTRL_FINAL) {
-               if ((chan->conn_state & L2CAP_CONN_SREJ_ACT) &&
+       } else if (__is_ctrl_final(chan, rx_control)) {
+               if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
                                chan->srej_save_reqseq == tx_seq)
-                       chan->conn_state &= ~L2CAP_CONN_SREJ_ACT;
+                       clear_bit(CONN_SREJ_ACT, &chan->conn_state);
                else
                        l2cap_retransmit_one_frame(chan, tx_seq);
        } else {
                l2cap_retransmit_one_frame(chan, tx_seq);
-               if (chan->conn_state & L2CAP_CONN_WAIT_F) {
+               if (test_bit(CONN_WAIT_F, &chan->conn_state)) {
                        chan->srej_save_reqseq = tx_seq;
-                       chan->conn_state |= L2CAP_CONN_SREJ_ACT;
+                       set_bit(CONN_SREJ_ACT, &chan->conn_state);
                }
        }
 }
 
-static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control)
+static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control)
 {
-       u8 tx_seq = __get_reqseq(rx_control);
+       u16 tx_seq = __get_reqseq(chan, rx_control);
 
-       BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control);
+       BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control);
 
-       chan->conn_state |= L2CAP_CONN_REMOTE_BUSY;
+       set_bit(CONN_REMOTE_BUSY, &chan->conn_state);
        chan->expected_ack_seq = tx_seq;
        l2cap_drop_acked_frames(chan);
 
-       if (rx_control & L2CAP_CTRL_POLL)
-               chan->conn_state |= L2CAP_CONN_SEND_FBIT;
+       if (__is_ctrl_poll(chan, rx_control))
+               set_bit(CONN_SEND_FBIT, &chan->conn_state);
 
-       if (!(chan->conn_state & L2CAP_CONN_SREJ_SENT)) {
-               del_timer(&chan->retrans_timer);
-               if (rx_control & L2CAP_CTRL_POLL)
+       if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
+               __clear_retrans_timer(chan);
+               if (__is_ctrl_poll(chan, rx_control))
                        l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL);
                return;
        }
 
-       if (rx_control & L2CAP_CTRL_POLL)
+       if (__is_ctrl_poll(chan, rx_control)) {
                l2cap_send_srejtail(chan);
-       else
-               l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY);
+       } else {
+               rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR);
+               l2cap_send_sframe(chan, rx_control);
+       }
 }
 
-static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb)
+static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb)
 {
-       BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len);
+       BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len);
 
-       if (L2CAP_CTRL_FINAL & rx_control &&
-                       chan->conn_state & L2CAP_CONN_WAIT_F) {
-               del_timer(&chan->monitor_timer);
+       if (__is_ctrl_final(chan, rx_control) &&
+                       test_bit(CONN_WAIT_F, &chan->conn_state)) {
+               __clear_monitor_timer(chan);
                if (chan->unacked_frames > 0)
-                       __mod_retrans_timer();
-               chan->conn_state &= ~L2CAP_CONN_WAIT_F;
+                       __set_retrans_timer(chan);
+               clear_bit(CONN_WAIT_F, &chan->conn_state);
        }
 
-       switch (rx_control & L2CAP_CTRL_SUPERVISE) {
-       case L2CAP_SUPER_RCV_READY:
+       switch (__get_ctrl_super(chan, rx_control)) {
+       case L2CAP_SUPER_RR:
                l2cap_data_channel_rrframe(chan, rx_control);
                break;
 
-       case L2CAP_SUPER_REJECT:
+       case L2CAP_SUPER_REJ:
                l2cap_data_channel_rejframe(chan, rx_control);
                break;
 
-       case L2CAP_SUPER_SELECT_REJECT:
+       case L2CAP_SUPER_SREJ:
                l2cap_data_channel_srejframe(chan, rx_control);
                break;
 
-       case L2CAP_SUPER_RCV_NOT_READY:
+       case L2CAP_SUPER_RNR:
                l2cap_data_channel_rnrframe(chan, rx_control);
                break;
        }
@@ -3532,13 +4077,12 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_cont
 static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
 {
        struct l2cap_chan *chan = l2cap_pi(sk)->chan;
-       struct l2cap_pinfo *pi = l2cap_pi(sk);
-       u16 control;
-       u8 req_seq;
+       u32 control;
+       u16 req_seq;
        int len, next_tx_seq_offset, req_seq_offset;
 
-       control = get_unaligned_le16(skb->data);
-       skb_pull(skb, 2);
+       control = __get_control(chan, skb->data);
+       skb_pull(skb, __ctrl_size(chan));
        len = skb->len;
 
        /*
@@ -3549,36 +4093,33 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
        if (l2cap_check_fcs(chan, skb))
                goto drop;
 
-       if (__is_sar_start(control) && __is_iframe(control))
-               len -= 2;
+       if (__is_sar_start(chan, control) && !__is_sframe(chan, control))
+               len -= L2CAP_SDULEN_SIZE;
 
        if (chan->fcs == L2CAP_FCS_CRC16)
-               len -= 2;
+               len -= L2CAP_FCS_SIZE;
 
        if (len > chan->mps) {
-               l2cap_send_disconn_req(pi->conn, chan, ECONNRESET);
+               l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                goto drop;
        }
 
-       req_seq = __get_reqseq(control);
-       req_seq_offset = (req_seq - chan->expected_ack_seq) % 64;
-       if (req_seq_offset < 0)
-               req_seq_offset += 64;
+       req_seq = __get_reqseq(chan, control);
+
+       req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq);
 
-       next_tx_seq_offset =
-               (chan->next_tx_seq - chan->expected_ack_seq) % 64;
-       if (next_tx_seq_offset < 0)
-               next_tx_seq_offset += 64;
+       next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq,
+                                               chan->expected_ack_seq);
 
        /* check for invalid req-seq */
        if (req_seq_offset > next_tx_seq_offset) {
-               l2cap_send_disconn_req(pi->conn, chan, ECONNRESET);
+               l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                goto drop;
        }
 
-       if (__is_iframe(control)) {
+       if (!__is_sframe(chan, control)) {
                if (len < 0) {
-                       l2cap_send_disconn_req(pi->conn, chan, ECONNRESET);
+                       l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                        goto drop;
                }
 
@@ -3586,7 +4127,7 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
        } else {
                if (len != 0) {
                        BT_ERR("%d", len);
-                       l2cap_send_disconn_req(pi->conn, chan, ECONNRESET);
+                       l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                        goto drop;
                }
 
@@ -3603,10 +4144,9 @@ drop:
 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
 {
        struct l2cap_chan *chan;
-       struct sock *sk;
-       struct l2cap_pinfo *pi;
-       u16 control;
-       u8 tx_seq;
+       struct sock *sk = NULL;
+       u32 control;
+       u16 tx_seq;
        int len;
 
        chan = l2cap_get_chan_by_scid(conn, cid);
@@ -3616,11 +4156,10 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
        }
 
        sk = chan->sk;
-       pi = l2cap_pi(sk);
 
        BT_DBG("chan %p, len %d", chan, skb->len);
 
-       if (sk->sk_state != BT_CONNECTED)
+       if (chan->state != BT_CONNECTED)
                goto drop;
 
        switch (chan->mode) {
@@ -3633,7 +4172,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                if (chan->imtu < skb->len)
                        goto drop;
 
-               if (!sock_queue_rcv_skb(sk, skb))
+               if (!chan->ops->recv(chan->data, skb))
                        goto done;
                break;
 
@@ -3648,30 +4187,38 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                goto done;
 
        case L2CAP_MODE_STREAMING:
-               control = get_unaligned_le16(skb->data);
-               skb_pull(skb, 2);
+               control = __get_control(chan, skb->data);
+               skb_pull(skb, __ctrl_size(chan));
                len = skb->len;
 
                if (l2cap_check_fcs(chan, skb))
                        goto drop;
 
-               if (__is_sar_start(control))
-                       len -= 2;
+               if (__is_sar_start(chan, control))
+                       len -= L2CAP_SDULEN_SIZE;
 
                if (chan->fcs == L2CAP_FCS_CRC16)
-                       len -= 2;
+                       len -= L2CAP_FCS_SIZE;
 
-               if (len > chan->mps || len < 0 || __is_sframe(control))
+               if (len > chan->mps || len < 0 || __is_sframe(chan, control))
                        goto drop;
 
-               tx_seq = __get_txseq(control);
+               tx_seq = __get_txseq(chan, control);
 
-               if (chan->expected_tx_seq == tx_seq)
-                       chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64;
-               else
-                       chan->expected_tx_seq = (tx_seq + 1) % 64;
+               if (chan->expected_tx_seq != tx_seq) {
+                       /* Frame(s) missing - must discard partial SDU */
+                       kfree_skb(chan->sdu);
+                       chan->sdu = NULL;
+                       chan->sdu_last_frag = NULL;
+                       chan->sdu_len = 0;
 
-               l2cap_streaming_reassembly_sdu(chan, skb, control);
+                       /* TODO: Notify userland of missing data */
+               }
+
+               chan->expected_tx_seq = __next_seq(chan, tx_seq);
+
+               if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE)
+                       l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
 
                goto done;
 
@@ -3692,23 +4239,26 @@ done:
 
 static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb)
 {
-       struct sock *sk;
+       struct sock *sk = NULL;
+       struct l2cap_chan *chan;
 
-       sk = l2cap_get_sock_by_psm(0, psm, conn->src);
-       if (!sk)
+       chan = l2cap_global_chan_by_psm(0, psm, conn->src);
+       if (!chan)
                goto drop;
 
+       sk = chan->sk;
+
        bh_lock_sock(sk);
 
        BT_DBG("sk %p, len %d", sk, skb->len);
 
-       if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
+       if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
                goto drop;
 
-       if (l2cap_pi(sk)->chan->imtu < skb->len)
+       if (chan->imtu < skb->len)
                goto drop;
 
-       if (!sock_queue_rcv_skb(sk, skb))
+       if (!chan->ops->recv(chan->data, skb))
                goto done;
 
 drop:
@@ -3722,23 +4272,26 @@ done:
 
 static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb)
 {
-       struct sock *sk;
+       struct sock *sk = NULL;
+       struct l2cap_chan *chan;
 
-       sk = l2cap_get_sock_by_scid(0, cid, conn->src);
-       if (!sk)
+       chan = l2cap_global_chan_by_scid(0, cid, conn->src);
+       if (!chan)
                goto drop;
 
+       sk = chan->sk;
+
        bh_lock_sock(sk);
 
        BT_DBG("sk %p, len %d", sk, skb->len);
 
-       if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED)
+       if (chan->state != BT_BOUND && chan->state != BT_CONNECTED)
                goto drop;
 
-       if (l2cap_pi(sk)->chan->imtu < skb->len)
+       if (chan->imtu < skb->len)
                goto drop;
 
-       if (!sock_queue_rcv_skb(sk, skb))
+       if (!chan->ops->recv(chan->data, skb))
                goto done;
 
 drop:
@@ -3783,6 +4336,11 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
                l2cap_att_channel(conn, cid, skb);
                break;
 
+       case L2CAP_CID_SMP:
+               if (smp_sig_channel(conn, skb))
+                       l2cap_conn_del(conn->hcon, EACCES);
+               break;
+
        default:
                l2cap_data_channel(conn, cid, skb);
                break;
@@ -3794,8 +4352,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
 static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
 {
        int exact = 0, lm1 = 0, lm2 = 0;
-       register struct sock *sk;
-       struct hlist_node *node;
+       struct l2cap_chan *c;
 
        if (type != ACL_LINK)
                return -EINVAL;
@@ -3803,25 +4360,25 @@ static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
        BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
 
        /* Find listening sockets and check their link_mode */
-       read_lock(&l2cap_sk_list.lock);
-       sk_for_each(sk, node, &l2cap_sk_list.head) {
-               struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+       read_lock(&chan_list_lock);
+       list_for_each_entry(c, &chan_list, global_l) {
+               struct sock *sk = c->sk;
 
-               if (sk->sk_state != BT_LISTEN)
+               if (c->state != BT_LISTEN)
                        continue;
 
                if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
                        lm1 |= HCI_LM_ACCEPT;
-                       if (chan->role_switch)
+                       if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
                                lm1 |= HCI_LM_MASTER;
                        exact++;
                } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
                        lm2 |= HCI_LM_ACCEPT;
-                       if (chan->role_switch)
+                       if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
                                lm2 |= HCI_LM_MASTER;
                }
        }
-       read_unlock(&l2cap_sk_list.lock);
+       read_unlock(&chan_list_lock);
 
        return exact ? lm1 : lm2;
 }
@@ -3840,7 +4397,7 @@ static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
                if (conn)
                        l2cap_conn_ready(conn);
        } else
-               l2cap_conn_del(hcon, bt_err(status));
+               l2cap_conn_del(hcon, bt_to_errno(status));
 
        return 0;
 }
@@ -3851,7 +4408,7 @@ static int l2cap_disconn_ind(struct hci_conn *hcon)
 
        BT_DBG("hcon %p", hcon);
 
-       if (hcon->type != ACL_LINK || !conn)
+       if ((hcon->type != ACL_LINK && hcon->type != LE_LINK) || !conn)
                return 0x13;
 
        return conn->disc_reason;
@@ -3864,27 +4421,25 @@ static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
        if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK))
                return -EINVAL;
 
-       l2cap_conn_del(hcon, bt_err(reason));
+       l2cap_conn_del(hcon, bt_to_errno(reason));
 
        return 0;
 }
 
 static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt)
 {
-       struct sock *sk = chan->sk;
-
-       if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
+       if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED)
                return;
 
        if (encrypt == 0x00) {
                if (chan->sec_level == BT_SECURITY_MEDIUM) {
-                       l2cap_sock_clear_timer(sk);
-                       l2cap_sock_set_timer(sk, HZ * 5);
+                       __clear_chan_timer(chan);
+                       __set_chan_timer(chan, HZ * 5);
                } else if (chan->sec_level == BT_SECURITY_HIGH)
-                       __l2cap_sock_close(sk, ECONNREFUSED);
+                       l2cap_chan_close(chan, ECONNREFUSED);
        } else {
                if (chan->sec_level == BT_SECURITY_MEDIUM)
-                       l2cap_sock_clear_timer(sk);
+                       __clear_chan_timer(chan);
        }
 }
 
@@ -3898,6 +4453,11 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
        BT_DBG("conn %p", conn);
 
+       if (hcon->type == LE_LINK) {
+               smp_distribute_keys(conn, 0);
+               del_timer(&conn->security_timer);
+       }
+
        read_lock(&conn->chan_lock);
 
        list_for_each_entry(chan, &conn->chan_l, list) {
@@ -3905,50 +4465,72 @@ static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
                bh_lock_sock(sk);
 
-               if (chan->conf_state & L2CAP_CONF_CONNECT_PEND) {
+               BT_DBG("chan->scid %d", chan->scid);
+
+               if (chan->scid == L2CAP_CID_LE_DATA) {
+                       if (!status && encrypt) {
+                               chan->sec_level = hcon->sec_level;
+                               l2cap_chan_ready(sk);
+                       }
+
                        bh_unlock_sock(sk);
                        continue;
                }
 
-               if (!status && (sk->sk_state == BT_CONNECTED ||
-                                               sk->sk_state == BT_CONFIG)) {
+               if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) {
+                       bh_unlock_sock(sk);
+                       continue;
+               }
+
+               if (!status && (chan->state == BT_CONNECTED ||
+                                               chan->state == BT_CONFIG)) {
                        l2cap_check_encryption(chan, encrypt);
                        bh_unlock_sock(sk);
                        continue;
                }
 
-               if (sk->sk_state == BT_CONNECT) {
+               if (chan->state == BT_CONNECT) {
                        if (!status) {
                                struct l2cap_conn_req req;
-                               req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                               req.psm  = l2cap_pi(sk)->psm;
+                               req.scid = cpu_to_le16(chan->scid);
+                               req.psm  = chan->psm;
 
                                chan->ident = l2cap_get_ident(conn);
-                               chan->conf_state |= L2CAP_CONF_CONNECT_PEND;
+                               set_bit(CONF_CONNECT_PEND, &chan->conf_state);
 
                                l2cap_send_cmd(conn, chan->ident,
                                        L2CAP_CONN_REQ, sizeof(req), &req);
                        } else {
-                               l2cap_sock_clear_timer(sk);
-                               l2cap_sock_set_timer(sk, HZ / 10);
+                               __clear_chan_timer(chan);
+                               __set_chan_timer(chan, HZ / 10);
                        }
-               } else if (sk->sk_state == BT_CONNECT2) {
+               } else if (chan->state == BT_CONNECT2) {
                        struct l2cap_conn_rsp rsp;
-                       __u16 result;
+                       __u16 res, stat;
 
                        if (!status) {
-                               sk->sk_state = BT_CONFIG;
-                               result = L2CAP_CR_SUCCESS;
+                               if (bt_sk(sk)->defer_setup) {
+                                       struct sock *parent = bt_sk(sk)->parent;
+                                       res = L2CAP_CR_PEND;
+                                       stat = L2CAP_CS_AUTHOR_PEND;
+                                       if (parent)
+                                               parent->sk_data_ready(parent, 0);
+                               } else {
+                                       l2cap_state_change(chan, BT_CONFIG);
+                                       res = L2CAP_CR_SUCCESS;
+                                       stat = L2CAP_CS_NO_INFO;
+                               }
                        } else {
-                               sk->sk_state = BT_DISCONN;
-                               l2cap_sock_set_timer(sk, HZ / 10);
-                               result = L2CAP_CR_SEC_BLOCK;
+                               l2cap_state_change(chan, BT_DISCONN);
+                               __set_chan_timer(chan, HZ / 10);
+                               res = L2CAP_CR_SEC_BLOCK;
+                               stat = L2CAP_CS_NO_INFO;
                        }
 
-                       rsp.scid   = cpu_to_le16(l2cap_pi(sk)->dcid);
-                       rsp.dcid   = cpu_to_le16(l2cap_pi(sk)->scid);
-                       rsp.result = cpu_to_le16(result);
-                       rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
+                       rsp.scid   = cpu_to_le16(chan->dcid);
+                       rsp.dcid   = cpu_to_le16(chan->scid);
+                       rsp.result = cpu_to_le16(res);
+                       rsp.status = cpu_to_le16(stat);
                        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
                                                        sizeof(rsp), &rsp);
                }
@@ -4074,25 +4656,22 @@ drop:
 
 static int l2cap_debugfs_show(struct seq_file *f, void *p)
 {
-       struct sock *sk;
-       struct hlist_node *node;
+       struct l2cap_chan *c;
 
-       read_lock_bh(&l2cap_sk_list.lock);
+       read_lock_bh(&chan_list_lock);
 
-       sk_for_each(sk, node, &l2cap_sk_list.head) {
-               struct l2cap_pinfo *pi = l2cap_pi(sk);
-               struct l2cap_chan *chan = pi->chan;
+       list_for_each_entry(c, &chan_list, global_l) {
+               struct sock *sk = c->sk;
 
                seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
                                        batostr(&bt_sk(sk)->src),
                                        batostr(&bt_sk(sk)->dst),
-                                       sk->sk_state, __le16_to_cpu(pi->psm),
-                                       pi->scid, pi->dcid,
-                                       chan->imtu, chan->omtu, chan->sec_level,
-                                       chan->mode);
-       }
+                                       c->state, __le16_to_cpu(c->psm),
+                                       c->scid, c->dcid, c->imtu, c->omtu,
+                                       c->sec_level, c->mode);
+}
 
-       read_unlock_bh(&l2cap_sk_list.lock);
+       read_unlock_bh(&chan_list_lock);
 
        return 0;
 }
@@ -4130,12 +4709,6 @@ int __init l2cap_init(void)
        if (err < 0)
                return err;
 
-       _busy_wq = create_singlethread_workqueue("l2cap");
-       if (!_busy_wq) {
-               err = -ENOMEM;
-               goto error;
-       }
-
        err = hci_register_proto(&l2cap_hci_proto);
        if (err < 0) {
                BT_ERR("L2CAP protocol registration failed");
@@ -4153,7 +4726,6 @@ int __init l2cap_init(void)
        return 0;
 
 error:
-       destroy_workqueue(_busy_wq);
        l2cap_cleanup_sockets();
        return err;
 }
@@ -4162,9 +4734,6 @@ void l2cap_exit(void)
 {
        debugfs_remove(l2cap_debugfs);
 
-       flush_workqueue(_busy_wq);
-       destroy_workqueue(_busy_wq);
-
        if (hci_unregister_proto(&l2cap_hci_proto) < 0)
                BT_ERR("L2CAP protocol unregistration failed");
 
@@ -4173,3 +4742,6 @@ void l2cap_exit(void)
 
 module_param(disable_ertm, bool, 0644);
 MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
+
+module_param(enable_hs, bool, 0644);
+MODULE_PARM_DESC(enable_hs, "Enable High Speed");