4 files changed, 63 insertions, 15 deletions

diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c
index 07659cfd6d7b..7d99410e6c1a 100644
--- a/net/nfc/hci/command.c
+++ b/net/nfc/hci/command.c
@@ -344,7 +344,7 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
                 return -EADDRINUSE;
 
         if (pipe != NFC_HCI_INVALID_PIPE)
-                goto pipe_is_open;
+                goto open_pipe;
 
         switch (dest_gate) {
         case NFC_HCI_LINK_MGMT_GATE:
@@ -361,6 +361,7 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
                 break;
         }
 
+open_pipe:
         r = nfc_hci_open_pipe(hdev, pipe);
         if (r < 0) {
                 if (pipe_created)
@@ -371,7 +372,6 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
                 return r;
         }
 
-pipe_is_open:
         hdev->gate2pipe[dest_gate] = pipe;
 
         return 0;
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index bc571b0efb92..7bea574d5934 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -33,17 +33,20 @@
 /* Largest headroom needed for outgoing HCI commands */
 #define HCI_CMDS_HEADROOM 1
 
-static int nfc_hci_result_to_errno(u8 result)
+int nfc_hci_result_to_errno(u8 result)
 {
         switch (result) {
         case NFC_HCI_ANY_OK:
                 return 0;
+        case NFC_HCI_ANY_E_REG_PAR_UNKNOWN:
+                return -EOPNOTSUPP;
         case NFC_HCI_ANY_E_TIMEOUT:
                 return -ETIME;
         default:
                 return -1;
         }
 }
+EXPORT_SYMBOL(nfc_hci_result_to_errno);
 
 static void nfc_hci_msg_tx_work(struct work_struct *work)
 {
@@ -167,7 +170,7 @@ void nfc_hci_cmd_received(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
         kfree_skb(skb);
 }
 
-static u32 nfc_hci_sak_to_protocol(u8 sak)
+u32 nfc_hci_sak_to_protocol(u8 sak)
 {
         switch (NFC_HCI_TYPE_A_SEL_PROT(sak)) {
         case NFC_HCI_TYPE_A_SEL_PROT_MIFARE:
@@ -182,6 +185,7 @@ static u32 nfc_hci_sak_to_protocol(u8 sak)
                 return 0xffffffff;
         }
 }
+EXPORT_SYMBOL(nfc_hci_sak_to_protocol);
 
 int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 {
@@ -284,6 +288,12 @@ void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
                             struct sk_buff *skb)
 {
         int r = 0;
+        u8 gate = nfc_hci_pipe2gate(hdev, pipe);
+
+        if (gate == 0xff) {
+                pr_err("Discarded event %x to unopened pipe %x\n", event, pipe);
+                goto exit;
+        }
 
         switch (event) {
         case NFC_HCI_EVT_TARGET_DISCOVERED:
@@ -307,14 +317,11 @@ void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
                         goto exit;
                 }
 
-                r = nfc_hci_target_discovered(hdev,
-                                              nfc_hci_pipe2gate(hdev, pipe));
+                r = nfc_hci_target_discovered(hdev, gate);
                 break;
         default:
                 if (hdev->ops->event_received) {
-                        hdev->ops->event_received(hdev,
-                                                nfc_hci_pipe2gate(hdev, pipe),
-                                                event, skb);
+                        hdev->ops->event_received(hdev, gate, event, skb);
                         return;
                 }
 
@@ -419,6 +426,10 @@ static int hci_dev_version(struct nfc_hci_dev *hdev)
 
         r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
                               NFC_HCI_ID_MGMT_VERSION_SW, &skb);
+        if (r == -EOPNOTSUPP) {
+                pr_info("Software/Hardware info not available\n");
+                return 0;
+        }
         if (r < 0)
                 return r;
 
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index ed2d17312d61..df24be48d4da 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -528,6 +528,23 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
         if (local == NULL)
                 return -ENODEV;
 
+        /* Remote is ready but has not acknowledged our frames */
+        if((sock->remote_ready &&
+            skb_queue_len(&sock->tx_pending_queue) >= sock->rw &&
+            skb_queue_len(&sock->tx_queue) >= 2 * sock->rw)) {
+                pr_err("Pending queue is full %d frames\n",
+                       skb_queue_len(&sock->tx_pending_queue));
+                return -ENOBUFS;
+        }
+
+        /* Remote is not ready and we've been queueing enough frames */
+        if ((!sock->remote_ready &&
+             skb_queue_len(&sock->tx_queue) >= 2 * sock->rw)) {
+                pr_err("Tx queue is full %d frames\n",
+                       skb_queue_len(&sock->tx_queue));
+                return -ENOBUFS;
+        }
+
         msg_data = kzalloc(len, GFP_KERNEL);
         if (msg_data == NULL)
                 return -ENOMEM;
@@ -579,7 +596,7 @@ int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
         struct sk_buff *pdu;
         struct nfc_llcp_local *local;
         size_t frag_len = 0, remaining_len;
-        u8 *msg_ptr;
+        u8 *msg_ptr, *msg_data;
         int err;
 
         pr_debug("Send UI frame len %zd\n", len);
@@ -588,8 +605,17 @@ int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
         if (local == NULL)
                 return -ENODEV;
 
+        msg_data = kzalloc(len, GFP_KERNEL);
+        if (msg_data == NULL)
+                return -ENOMEM;
+
+        if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+                kfree(msg_data);
+                return -EFAULT;
+        }
+
         remaining_len = len;
-        msg_ptr = (u8 *) msg->msg_iov;
+        msg_ptr = msg_data;
 
         while (remaining_len > 0) {
 
@@ -616,6 +642,8 @@ int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
                 msg_ptr += frag_len;
         }
 
+        kfree(msg_data);
+
         return len;
 }
 
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 2e9ddf34c099..2df87056c6df 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -656,6 +656,8 @@ static void nfc_llcp_tx_work(struct work_struct *work)
                 if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
                         nfc_llcp_send_symm(local->dev);
                 } else {
+                        struct sk_buff *copy_skb = NULL;
+                        u8 ptype = nfc_llcp_ptype(skb);
                         int ret;
 
                         pr_debug("Sending pending skb\n");
@@ -663,22 +665,29 @@ static void nfc_llcp_tx_work(struct work_struct *work)
                                        DUMP_PREFIX_OFFSET, 16, 1,
                                        skb->data, skb->len, true);
 
+                        if (ptype == LLCP_PDU_I)
+                                copy_skb = skb_copy(skb, GFP_ATOMIC);
+
                         nfc_llcp_send_to_raw_sock(local, skb,
                                                   NFC_LLCP_DIRECTION_TX);
 
                         ret = nfc_data_exchange(local->dev, local->target_idx,
                                                 skb, nfc_llcp_recv, local);
 
-                        if (!ret && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
-                                skb = skb_get(skb);
-                                skb_queue_tail(&llcp_sock->tx_pending_queue,
-                                               skb);
+                        if (ret) {
+                                kfree_skb(copy_skb);
+                                goto out;
                         }
+
+                        if (ptype == LLCP_PDU_I && copy_skb)
+                                skb_queue_tail(&llcp_sock->tx_pending_queue,
+                                               copy_skb);
                 }
         } else {
                 nfc_llcp_send_symm(local->dev);
         }
 
+out:
         mod_timer(&local->link_timer,
                   jiffies + msecs_to_jiffies(2 * local->remote_lto));
 }