2 files changed, 48 insertions, 0 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 6db280ddc4c8..79415353cc28 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -579,6 +579,52 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
         return len;
 }
 
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                           struct msghdr *msg, size_t len)
+{
+        struct sk_buff *pdu;
+        struct nfc_llcp_local *local;
+        size_t frag_len = 0, remaining_len;
+        u8 *msg_ptr;
+        int err;
+
+        pr_debug("Send UI frame len %zd\n", len);
+
+        local = sock->local;
+        if (local == NULL)
+                return -ENODEV;
+
+        remaining_len = len;
+        msg_ptr = (u8 *) msg->msg_iov;
+
+        while (remaining_len > 0) {
+
+                frag_len = min_t(size_t, sock->miu, remaining_len);
+
+                pr_debug("Fragment %zd bytes remaining %zd",
+                         frag_len, remaining_len);
+
+                pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+                                         frag_len + LLCP_HEADER_SIZE, &err);
+                if (pdu == NULL) {
+                        pr_err("Could not allocate PDU\n");
+                        continue;
+                }
+
+                pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+                memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+                /* No need to check for the peer RW for UI frames */
+                skb_queue_tail(&local->tx_queue, pdu);
+
+                remaining_len -= frag_len;
+                msg_ptr += frag_len;
+        }
+
+        return len;
+}
+
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
 {
         struct sk_buff *skb;
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index e06d03571644..276da3a6a589 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -221,6 +221,8 @@ int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
 int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
                           struct msghdr *msg, size_t len);
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                           struct msghdr *msg, size_t len);
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
 
 /* Socket API */