3 files changed, 64 insertions, 4 deletions

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index 11a3b7d..bf8ae4f 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 92988aa..42994fa 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 c13e02e..3f339b1 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: