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;
 }