3 files changed, 64 insertions, 4 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 11a3b7d98dc5..bf8ae4f0b90c 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -488,7 +488,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);
 
@@ -502,7 +502,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
 
         kfree(msg_data);
 
-        return 0;
+        return len;
 }
 
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 92988aa620dc..42994fac26d6 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -448,6 +448,8 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 {
         struct nfc_llcp_sock *sock, *llcp_sock, *n;
 
+        pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
         if (ssap == 0 && dsap == 0)
                 return NULL;
 
@@ -783,6 +785,7 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
 {
         struct nfc_llcp_sock *llcp_sock;
+        struct sock *sk;
         u8 dsap, ssap;
 
         dsap = nfc_llcp_dsap(skb);
@@ -801,10 +804,14 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
         }
 
         llcp_sock->dsap = ssap;
+        sk = &llcp_sock->sk;
 
         nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
                            skb->len - LLCP_HEADER_SIZE);
 
+        sk->sk_state = LLCP_CONNECTED;
+        sk->sk_state_change(sk);
+
         nfc_llcp_sock_put(llcp_sock);
 }
 
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index c13e02ebdef9..3f339b19d140 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -27,6 +27,42 @@
 #include "../nfc.h"
 #include "llcp.h"
 
+static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
+{
+        DECLARE_WAITQUEUE(wait, current);
+        int err = 0;
+
+        pr_debug("sk %p", sk);
+
+        add_wait_queue(sk_sleep(sk), &wait);
+        set_current_state(TASK_INTERRUPTIBLE);
+
+        while (sk->sk_state != state) {
+                if (!timeo) {
+                        err = -EINPROGRESS;
+                        break;
+                }
+
+                if (signal_pending(current)) {
+                        err = sock_intr_errno(timeo);
+                        break;
+                }
+
+                release_sock(sk);
+                timeo = schedule_timeout(timeo);
+                lock_sock(sk);
+                set_current_state(TASK_INTERRUPTIBLE);
+
+                err = sock_error(sk);
+                if (err)
+                        break;
+        }
+
+        __set_current_state(TASK_RUNNING);
+        remove_wait_queue(sk_sleep(sk), &wait);
+        return err;
+}
+
 static struct proto llcp_sock_proto = {
         .name     = "NFC_LLCP",
         .owner    = THIS_MODULE,
@@ -304,11 +340,24 @@ static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
                 mask |= POLLERR;
 
         if (!skb_queue_empty(&sk->sk_receive_queue))
-                mask |= POLLIN;
+                mask |= POLLIN | POLLRDNORM;
 
         if (sk->sk_state == LLCP_CLOSED)
                 mask |= POLLHUP;
 
+        if (sk->sk_shutdown & RCV_SHUTDOWN)
+                mask |= POLLRDHUP | POLLIN | POLLRDNORM;
+
+        if (sk->sk_shutdown == SHUTDOWN_MASK)
+                mask |= POLLHUP;
+
+        if (sock_writeable(sk))
+                mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
+        else
+                set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
+
+        pr_debug("mask 0x%x\n", mask);
+
         return mask;
 }
 
@@ -462,9 +511,13 @@ static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
         if (ret)
                 goto put_dev;
 
-        sk->sk_state = LLCP_CONNECTED;
+        ret = sock_wait_state(sk, LLCP_CONNECTED,
+                              sock_sndtimeo(sk, flags & O_NONBLOCK));
+        if (ret)
+                goto put_dev;
 
         release_sock(sk);
+
         return 0;
 
 put_dev: