1 files changed, 51 insertions, 17 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 7b76eb7192f3..b982b5b890d7 100644
--- 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(u16, 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);
 
@@ -497,12 +531,12 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 		release_sock(sk);
 
 		remaining_len -= frag_len;
-		msg_ptr += len;
+		msg_ptr += frag_len;
 	}
 
 	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);