NFC: LLCP's MIUX is 10 bytes long, not 7

[deliverable/linux.git] / net / nfc / llcp / commands.c

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index ef10ffcb4b6ffb5ed79eecdc8005dccc0de34575..b982b5b890d73da30a315567851d5d91e7ec9285 100644 (file)
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -51,7 +51,7 @@ static u8 llcp_tlv8(u8 *tlv, u8 type)
        return tlv[2];
 }
 
-static u8 llcp_tlv16(u8 *tlv, u8 type)
+static u16 llcp_tlv16(u8 *tlv, u8 type)
 {
        if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
                return 0;
@@ -67,7 +67,7 @@ static u8 llcp_tlv_version(u8 *tlv)
 
 static u16 llcp_tlv_miux(u8 *tlv)
 {
-       return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7f;
+       return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
 }
 
 static u16 llcp_tlv_wks(u8 *tlv)
@@ -102,7 +102,7 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
        length = llcp_tlv_length[type];
        if (length == 0 && value_length == 0)
                return NULL;
-       else
+       else if (length == 0)
                length = value_length;
 
        *tlv_length = 2 + length;
@@ -117,8 +117,8 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
        return tlv;
 }
 
-int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
-                      u8 *tlv_array, u16 tlv_array_len)
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+                         u8 *tlv_array, u16 tlv_array_len)
 {
        u8 *tlv = tlv_array, type, length, offset = 0;
 
@@ -149,8 +149,45 @@ int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
                case LLCP_TLV_OPT:
                        local->remote_opt = llcp_tlv_opt(tlv);
                        break;
+               default:
+                       pr_err("Invalid gt tlv value 0x%x\n", type);
+                       break;
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+       pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
+                local->remote_version, local->remote_miu,
+                local->remote_lto, local->remote_opt,
+                local->remote_wks);
+
+       return 0;
+}
+
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+                                 u8 *tlv_array, u16 tlv_array_len)
+{
+       u8 *tlv = tlv_array, type, length, offset = 0;
+
+       pr_debug("TLV array length %d\n", tlv_array_len);
+
+       if (sock == NULL)
+               return -ENOTCONN;
+
+       while (offset < tlv_array_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               pr_debug("type 0x%x length %d\n", type, length);
+
+               switch (type) {
+               case LLCP_TLV_MIUX:
+                       sock->miu = llcp_tlv_miux(tlv) + 128;
+                       break;
                case LLCP_TLV_RW:
-                       local->remote_rw = llcp_tlv_rw(tlv);
+                       sock->rw = llcp_tlv_rw(tlv);
                        break;
                case LLCP_TLV_SN:
                        break;
@@ -163,10 +200,7 @@ int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
                tlv += length + 2;
        }
 
-       pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n",
-                local->remote_version, local->remote_miu,
-                local->remote_lto, local->remote_opt,
-                local->remote_wks, local->remote_rw);
+       pr_debug("sock %p rw %d miu %d\n", sock, sock->rw, sock->miu);
 
        return 0;
 }
@@ -248,7 +282,7 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
 
        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
 
-       skb = llcp_add_header(skb, sock->ssap, sock->dsap, LLCP_PDU_DISC);
+       skb = llcp_add_header(skb, sock->dsap, sock->ssap, LLCP_PDU_DISC);
 
        skb_queue_tail(&local->tx_queue, skb);
 
@@ -416,7 +450,7 @@ int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
 
        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
 
-       skb = llcp_add_header(skb, ssap, dsap, LLCP_PDU_DM);
+       skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM);
 
        memcpy(skb_put(skb, 1), &reason, 1);
 
@@ -474,7 +508,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
        while (remaining_len > 0) {
 
-               frag_len = min_t(size_t, local->remote_miu, remaining_len);
+               frag_len = min_t(size_t, sock->miu, remaining_len);
 
                pr_debug("Fragment %zd bytes remaining %zd",
                         frag_len, remaining_len);
@@ -488,7 +522,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
                memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
 
-               skb_queue_head(&sock->tx_queue, pdu);
+               skb_queue_tail(&sock->tx_queue, pdu);
 
                lock_sock(sk);
 
@@ -502,7 +536,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
        kfree(msg_data);
 
-       return 0;
+       return len;
 }
 
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
@@ -522,7 +556,7 @@ int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
 
        skb_put(skb, LLCP_SEQUENCE_SIZE);
 
-       skb->data[2] = sock->recv_n % 16;
+       skb->data[2] = sock->recv_n;
 
        skb_queue_head(&local->tx_queue, skb);