Bluetooth: Add signal handlers for channel moves
[linux-2.6.git] / net / bluetooth / l2cap_core.c
index 6e34312..335dc6f 100644 (file)
@@ -60,7 +60,7 @@ 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 LIST_HEAD(chan_list);
 static DEFINE_RWLOCK(chan_list_lock);
@@ -220,7 +220,7 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 
 static void l2cap_set_timer(struct l2cap_chan *chan, struct timer_list *timer, long timeout)
 {
-       BT_DBG("chan %p state %d timeout %ld", chan->sk, chan->state, 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);
@@ -338,6 +338,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
                chan->omtu = L2CAP_DEFAULT_MTU;
        }
 
+       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);
@@ -557,34 +564,58 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
                flags = ACL_START;
 
        bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON;
+       skb->priority = HCI_PRIO_MAX;
 
-       hci_send_acl(conn->hcon, skb, flags);
+       hci_send_acl(conn->hchan, skb, flags);
 }
 
-static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control)
+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, u32 control)
 {
        struct sk_buff *skb;
        struct l2cap_hdr *lh;
        struct l2cap_conn *conn = chan->conn;
-       int count, hlen = L2CAP_HDR_SIZE + 2;
-       u8 flags;
+       int count, hlen;
 
        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;
+
+       control |= __set_sframe(chan);
 
        if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
-               control |= L2CAP_CTRL_FINAL;
+               control |= __set_ctrl_final(chan);
 
        if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state))
-               control |= L2CAP_CTRL_POLL;
+               control |= __set_ctrl_poll(chan);
 
        skb = bt_skb_alloc(count, GFP_ATOMIC);
        if (!skb)
@@ -593,32 +624,27 @@ 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(chan->dcid);
-       put_unaligned_le16(control, skb_put(skb, 2));
+
+       __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;
-
-       bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags);
-
-       hci_send_acl(chan->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 (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-               control |= L2CAP_SUPER_RCV_NOT_READY;
+               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);
 }
@@ -987,6 +1013,8 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
                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);
 
@@ -1009,18 +1037,26 @@ static void security_timeout(unsigned long arg)
 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;
@@ -1246,48 +1282,35 @@ static void l2cap_drop_acked_frames(struct l2cap_chan *chan)
                __clear_retrans_timer(chan);
 }
 
-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", chan, skb, skb->len);
-
-       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(hcon, skb, flags);
-}
-
 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 sk_buff *skb, *tx_skb;
-       u16 control, fcs;
+       u16 fcs;
+       u32 control;
 
        skb = skb_peek(&chan->tx_q);
        if (!skb)
@@ -1310,20 +1333,23 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
 
        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;
+
+       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 |= L2CAP_CTRL_FINAL;
+               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);
@@ -1332,7 +1358,8 @@ static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq)
 static int l2cap_ertm_send(struct l2cap_chan *chan)
 {
        struct sk_buff *skb, *tx_skb;
-       u16 control, fcs;
+       u16 fcs;
+       u32 control;
        int nsent = 0;
 
        if (chan->state != BT_CONNECTED)
@@ -1350,20 +1377,22 @@ static 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 (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state))
-                       control |= L2CAP_CTRL_FINAL;
+                       control |= __set_ctrl_final(chan);
 
-               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);
+               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);
@@ -1371,7 +1400,8 @@ static int l2cap_ertm_send(struct l2cap_chan *chan)
                __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++;
@@ -1403,12 +1433,12 @@ 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 (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-               control |= L2CAP_SUPER_RCV_NOT_READY;
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
                set_bit(CONN_RNR_SENT, &chan->conn_state);
                l2cap_send_sframe(chan, control);
                return;
@@ -1417,20 +1447,20 @@ 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);
 }
@@ -1458,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;
 
@@ -1467,15 +1499,17 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
        return sent;
 }
 
-static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, 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 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,
@@ -1483,6 +1517,8 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
        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(chan->dcid);
@@ -1497,7 +1533,9 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct
        return skb;
 }
 
-static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, 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 sock *sk = chan->sk;
        struct l2cap_conn *conn = chan->conn;
@@ -1513,6 +1551,8 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms
        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(chan->dcid);
@@ -1528,12 +1568,12 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct ms
 
 static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
                                                struct msghdr *msg, size_t len,
-                                               u16 control, u16 sdulen)
+                                               u32 control, u16 sdulen)
 {
        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;
        struct l2cap_hdr *lh;
 
        BT_DBG("sk %p len %d", sk, (int)len);
@@ -1541,11 +1581,16 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
        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,
@@ -1557,9 +1602,11 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
        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)) {
@@ -1568,7 +1615,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
        }
 
        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;
@@ -1578,11 +1625,11 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
 {
        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);
@@ -1595,10 +1642,10 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
                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;
                }
 
@@ -1619,15 +1666,16 @@ static int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, si
        return size;
 }
 
-int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
+int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
+                                                               u32 priority)
 {
        struct sk_buff *skb;
-       u16 control;
+       u32 control;
        int err;
 
        /* Connectionless channel */
        if (chan->chan_type == L2CAP_CHAN_CONN_LESS) {
-               skb = l2cap_create_connless_pdu(chan, msg, len);
+               skb = l2cap_create_connless_pdu(chan, msg, len, priority);
                if (IS_ERR(skb))
                        return PTR_ERR(skb);
 
@@ -1642,7 +1690,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
                        return -EMSGSIZE;
 
                /* Create a basic PDU */
-               skb = l2cap_create_basic_pdu(chan, msg, len);
+               skb = l2cap_create_basic_pdu(chan, msg, len, priority);
                if (IS_ERR(skb))
                        return PTR_ERR(skb);
 
@@ -1654,7 +1702,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len)
        case L2CAP_MODE_STREAMING:
                /* Entire SDU fits into one PDU */
                if (len <= chan->remote_mps) {
-                       control = L2CAP_SDU_UNSEGMENTED;
+                       control = __set_ctrl_sar(chan, L2CAP_SAR_UNSEGMENTED);
                        skb = l2cap_create_iframe_pdu(chan, msg, len, control,
                                                                        0);
                        if (IS_ERR(skb))
@@ -1852,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;
@@ -1903,15 +1982,23 @@ 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))
+                                               __l2cap_ews_supported(chan)) {
                /* use extended control field */
                set_bit(FLAG_EXT_CTRL, &chan->flags);
-       else
+               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)
@@ -1919,6 +2006,7 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
        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);
 
@@ -1931,6 +2019,9 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data)
                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, chan->conn->feat_mask);
@@ -1963,9 +2054,12 @@ done:
                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 > chan->conn->mtu - 10)
-                       rfc.max_pdu_size = cpu_to_le16(chan->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);
 
@@ -1975,6 +2069,9 @@ done:
                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
                                                        (unsigned long) &rfc);
 
+               if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
+                       l2cap_add_opt_efs(&ptr, chan);
+
                if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
                        break;
 
@@ -1995,13 +2092,19 @@ 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 > chan->conn->mtu - 10)
-                       rfc.max_pdu_size = cpu_to_le16(chan->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 (test_bit(FLAG_EFS_ENABLE, &chan->flags))
+                       l2cap_add_opt_efs(&ptr, chan);
+
                if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS))
                        break;
 
@@ -2028,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);
 
@@ -2059,7 +2165,12 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
                case L2CAP_CONF_FCS:
                        if (val == L2CAP_FCS_NONE)
                                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:
@@ -2068,6 +2179,7 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
 
                        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;
 
@@ -2093,6 +2205,13 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
                        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;
 
@@ -2111,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. */
@@ -2124,6 +2242,26 @@ done:
                }
                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;
@@ -2138,10 +2276,13 @@ done:
 
                        chan->remote_max_tx = rfc.max_transmit;
 
-                       if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10)
-                               rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
-
-                       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);
@@ -2153,13 +2294,29 @@ done:
                        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) > chan->conn->mtu - 10)
-                               rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10);
-
-                       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;
 
                        set_bit(CONF_MODE_DONE, &chan->conf_state);
 
@@ -2231,8 +2388,8 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                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);
+                       l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
+                                                       chan->tx_win);
                        break;
                }
        }
@@ -2242,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);
@@ -2648,6 +2805,21 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                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;
@@ -2677,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];
@@ -2838,9 +3035,15 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
        } 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 {
@@ -2907,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)
 {
@@ -3019,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;
@@ -3097,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);
 
@@ -3112,14 +3503,14 @@ 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 (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
-               control |= L2CAP_SUPER_RCV_NOT_READY;
+               control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
                l2cap_send_sframe(chan, control);
                set_bit(CONN_RNR_SENT, &chan->conn_state);
        }
@@ -3131,12 +3522,12 @@ static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
 
        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;
@@ -3150,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);
@@ -3197,24 +3584,24 @@ static void append_skb_frag(struct sk_buff *skb,
        skb->truesize += new_frag->truesize;
 }
 
-static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control)
+static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control)
 {
        int err = -EINVAL;
 
-       switch (control & L2CAP_CTRL_SAR) {
-       case L2CAP_SDU_UNSEGMENTED:
+       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:
+       case L2CAP_SAR_START:
                if (chan->sdu)
                        break;
 
                chan->sdu_len = get_unaligned_le16(skb->data);
-               skb_pull(skb, 2);
+               skb_pull(skb, L2CAP_SDULEN_SIZE);
 
                if (chan->sdu_len > chan->imtu) {
                        err = -EMSGSIZE;
@@ -3231,7 +3618,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1
                err = 0;
                break;
 
-       case L2CAP_SDU_CONTINUE:
+       case L2CAP_SAR_CONTINUE:
                if (!chan->sdu)
                        break;
 
@@ -3245,7 +3632,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1
                err = 0;
                break;
 
-       case L2CAP_SDU_END:
+       case L2CAP_SAR_END:
                if (!chan->sdu)
                        break;
 
@@ -3280,14 +3667,14 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u1
 
 static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
 {
-       u16 control;
+       u32 control;
 
        BT_DBG("chan %p, Enter local busy", chan);
 
        set_bit(CONN_LOCAL_BUSY, &chan->conn_state);
 
-       control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
-       control |= L2CAP_SUPER_RCV_NOT_READY;
+       control = __set_reqseq(chan, chan->buffer_seq);
+       control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR);
        l2cap_send_sframe(chan, control);
 
        set_bit(CONN_RNR_SENT, &chan->conn_state);
@@ -3297,13 +3684,14 @@ static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan)
 
 static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan)
 {
-       u16 control;
+       u32 control;
 
        if (!test_bit(CONN_RNR_SENT, &chan->conn_state))
                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_poll(chan);
+       control |= __set_ctrl_super(chan, L2CAP_SUPER_RR);
        l2cap_send_sframe(chan, control);
        chan->retry_count = 1;
 
@@ -3329,10 +3717,10 @@ void l2cap_chan_busy(struct l2cap_chan *chan, int busy)
        }
 }
 
-static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
+static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq)
 {
        struct sk_buff *skb;
-       u16 control;
+       u32 control;
 
        while ((skb = skb_peek(&chan->srej_q)) &&
                        !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
@@ -3342,7 +3730,7 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
                        break;
 
                skb = skb_dequeue(&chan->srej_q);
-               control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+               control = __set_ctrl_sar(chan, bt_cb(skb)->sar);
                err = l2cap_reassemble_sdu(chan, skb, control);
 
                if (err < 0) {
@@ -3350,16 +3738,15 @@ static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq)
                        break;
                }
 
-               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) {
@@ -3367,45 +3754,48 @@ 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)
 {
-       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 &&
+       if (__is_ctrl_final(chan, rx_control) &&
                        test_bit(CONN_WAIT_F, &chan->conn_state)) {
                __clear_monitor_timer(chan);
                if (chan->unacked_frames > 0)
@@ -3416,9 +3806,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
        chan->expected_ack_seq = req_seq;
        l2cap_drop_acked_frames(chan);
 
-       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) {
@@ -3466,10 +3854,8 @@ 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)
@@ -3494,7 +3880,7 @@ static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_cont
        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 (test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
                bt_cb(skb)->tx_seq = tx_seq;
@@ -3504,13 +3890,14 @@ expected:
        }
 
        err = l2cap_reassemble_sdu(chan, skb, rx_control);
-       chan->buffer_seq = (chan->buffer_seq + 1) % 64;
+       chan->buffer_seq = __next_seq(chan, chan->buffer_seq);
+
        if (err < 0) {
                l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                return err;
        }
 
-       if (rx_control & L2CAP_CTRL_FINAL) {
+       if (__is_ctrl_final(chan, rx_control)) {
                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
                        l2cap_retransmit_frames(chan);
        }
@@ -3528,15 +3915,15 @@ 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) {
+       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) &&
@@ -3549,7 +3936,7 @@ static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_co
                        l2cap_send_i_or_rr_or_rnr(chan);
                }
 
-       } else if (rx_control & L2CAP_CTRL_FINAL) {
+       } else if (__is_ctrl_final(chan, rx_control)) {
                clear_bit(CONN_REMOTE_BUSY, &chan->conn_state);
 
                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
@@ -3568,18 +3955,18 @@ static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_co
        }
 }
 
-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);
 
        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 (__is_ctrl_final(chan, rx_control)) {
                if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state))
                        l2cap_retransmit_frames(chan);
        } else {
@@ -3589,15 +3976,15 @@ static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_c
                        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);
 
        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);
 
@@ -3610,7 +3997,7 @@ static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_
                        chan->srej_save_reqseq = tx_seq;
                        set_bit(CONN_SREJ_ACT, &chan->conn_state);
                }
-       } else if (rx_control & L2CAP_CTRL_FINAL) {
+       } else if (__is_ctrl_final(chan, rx_control)) {
                if (test_bit(CONN_SREJ_ACT, &chan->conn_state) &&
                                chan->srej_save_reqseq == tx_seq)
                        clear_bit(CONN_SREJ_ACT, &chan->conn_state);
@@ -3625,37 +4012,39 @@ static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_
        }
 }
 
-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);
 
        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)
+       if (__is_ctrl_poll(chan, rx_control))
                set_bit(CONN_SEND_FBIT, &chan->conn_state);
 
        if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) {
                __clear_retrans_timer(chan);
-               if (rx_control & L2CAP_CTRL_POLL)
+               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 &&
+       if (__is_ctrl_final(chan, rx_control) &&
                        test_bit(CONN_WAIT_F, &chan->conn_state)) {
                __clear_monitor_timer(chan);
                if (chan->unacked_frames > 0)
@@ -3663,20 +4052,20 @@ static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_cont
                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;
        }
@@ -3688,12 +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;
-       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;
 
        /*
@@ -3704,26 +4093,23 @@ 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(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) {
@@ -3731,7 +4117,7 @@ static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
                goto drop;
        }
 
-       if (__is_iframe(control)) {
+       if (!__is_sframe(chan, control)) {
                if (len < 0) {
                        l2cap_send_disconn_req(chan->conn, chan, ECONNRESET);
                        goto drop;
@@ -3759,8 +4145,8 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
 {
        struct l2cap_chan *chan;
        struct sock *sk = NULL;
-       u16 control;
-       u8 tx_seq;
+       u32 control;
+       u16 tx_seq;
        int len;
 
        chan = l2cap_get_chan_by_scid(conn, cid);
@@ -3801,23 +4187,23 @@ 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) {
                        /* Frame(s) missing - must discard partial SDU */
@@ -3829,7 +4215,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                        /* TODO: Notify userland of missing data */
                }
 
-               chan->expected_tx_seq = (tx_seq + 1) % 64;
+               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);