Bluetooth: EWS: recalculate L2CAP header size

Recalculate length of L2CAP header based on Control field length.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 97aa545d66d9fc0bd57c224a5b7be29d018e9d03..439e7150f1506163639744b470df3bcc114a4ab4 100644 (file)
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -566,12 +566,17 @@ static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control)
        struct sk_buff *skb;
        struct l2cap_hdr *lh;
        struct l2cap_conn *conn = chan->conn;
-       int count, hlen = L2CAP_HDR_SIZE + 2;
+       int count, hlen;
        u8 flags;
 
        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;
 
@@ -1534,7 +1539,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan,
        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);
@@ -1542,6 +1547,11 @@ 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;
 
@@ -3098,7 +3108,12 @@ 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);