NFC: Fragment LLCP I frames

Based on the receiver MIU, we have to fragment the frame to be
transmitted.

Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 43 insertions, 14 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index a02292ba25c7..bfe35b8f225e 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -444,29 +444,58 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 				struct msghdr *msg, size_t len)
 {
 	struct sk_buff *pdu;
-	struct sock *sk;
+	struct sock *sk = &sock->sk;
+	struct nfc_llcp_local *local;
+	size_t frag_len = 0, remaining_len;
+	u8 *msg_data, *msg_ptr;
 
-	pr_debug("Send I frame\n");
+	pr_debug("Send I frame len %zd\n", len);
 
-	pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, len + LLCP_SEQUENCE_SIZE);
-	if (pdu == NULL)
-		return -ENOMEM;
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
 
-	skb_put(pdu, LLCP_SEQUENCE_SIZE);
+	msg_data = kzalloc(len, GFP_KERNEL);
+	if (msg_data == NULL)
+		return -ENOMEM;
 
-	if (memcpy_fromiovec(skb_put(pdu, len), msg->msg_iov, len)) {
-		kfree_skb(pdu);
-		return -EFAULT;
+	if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+			kfree(msg_data);
+			return -EFAULT;
 	}
 
-	skb_queue_head(&sock->tx_queue, pdu);
+	remaining_len = len;
+	msg_ptr = msg_data;
+
+	while (remaining_len > 0) {
+
+		frag_len = min_t(u16, local->remote_miu, remaining_len);
 
-	sk = &sock->sk;
-	lock_sock(sk);
+		pr_debug("Fragment %zd bytes remaining %zd",
+			 frag_len, remaining_len);
 
-	nfc_llcp_queue_i_frames(sock);
+		pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
+					frag_len + LLCP_SEQUENCE_SIZE);
+		if (pdu == NULL)
+			return -ENOMEM;
+
+		skb_put(pdu, LLCP_SEQUENCE_SIZE);
+
+		memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		skb_queue_head(&sock->tx_queue, pdu);
+
+		lock_sock(sk);
+
+		nfc_llcp_queue_i_frames(sock);
+
+		release_sock(sk);
+
+		remaining_len -= frag_len;
+		msg_ptr += len;
+	}
 
-	release_sock(sk);
+	kfree(msg_data);
 
 	return 0;
 }