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 bf8ae4f0b90c933d9dc3dbda70416498df2e97fa..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)
@@ -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;
 }
@@ -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);