1 files changed, 122 insertions, 79 deletions

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 1d32680..17a578f 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -37,7 +37,6 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
 	struct sock *sk, *parent_sk;
 	int i;
 
-
 	mutex_lock(&local->socket_lock);
 
 	for (i = 0; i < LLCP_MAX_SAP; i++) {
@@ -47,7 +46,7 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
 
 		/* Release all child sockets */
 		list_for_each_entry_safe(s, n, &parent->list, list) {
-			list_del(&s->list);
+			list_del_init(&s->list);
 			sk = &s->sk;
 
 			lock_sock(sk);
@@ -56,9 +55,12 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
 				nfc_put_device(s->dev);
 
 			sk->sk_state = LLCP_CLOSED;
-			sock_set_flag(sk, SOCK_DEAD);
 
 			release_sock(sk);
+
+			sock_orphan(sk);
+
+			s->local = NULL;
 		}
 
 		parent_sk = &parent->sk;
@@ -70,18 +72,19 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
 			struct sock *accept_sk;
 
 			list_for_each_entry_safe(lsk, n, &parent->accept_queue,
-								accept_queue) {
+						 accept_queue) {
 				accept_sk = &lsk->sk;
 				lock_sock(accept_sk);
 
 				nfc_llcp_accept_unlink(accept_sk);
 
 				accept_sk->sk_state = LLCP_CLOSED;
-				sock_set_flag(accept_sk, SOCK_DEAD);
 
 				release_sock(accept_sk);
 
 				sock_orphan(accept_sk);
+
+				lsk->local = NULL;
 			}
 		}
 
@@ -89,18 +92,32 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
 			nfc_put_device(parent->dev);
 
 		parent_sk->sk_state = LLCP_CLOSED;
-		sock_set_flag(parent_sk, SOCK_DEAD);
 
 		release_sock(parent_sk);
+
+		sock_orphan(parent_sk);
+
+		parent->local = NULL;
 	}
 
 	mutex_unlock(&local->socket_lock);
 }
 
+static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
+{
+	mutex_lock(&local->sdp_lock);
+
+	local->local_wks = 0;
+	local->local_sdp = 0;
+	local->local_sap = 0;
+
+	mutex_unlock(&local->sdp_lock);
+}
+
 static void nfc_llcp_timeout_work(struct work_struct *work)
 {
 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-							timeout_work);
+						    timeout_work);
 
 	nfc_dep_link_down(local->dev);
 }
@@ -146,7 +163,7 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
 
 	num_wks = ARRAY_SIZE(wks);
 
-	for (sap = 0 ; sap < num_wks; sap++) {
+	for (sap = 0; sap < num_wks; sap++) {
 		if (wks[sap] == NULL)
 			continue;
 
@@ -158,13 +175,13 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
 }
 
 u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
-				struct nfc_llcp_sock *sock)
+			 struct nfc_llcp_sock *sock)
 {
 	mutex_lock(&local->sdp_lock);
 
 	if (sock->service_name != NULL && sock->service_name_len > 0) {
 		int ssap = nfc_llcp_wks_sap(sock->service_name,
-						sock->service_name_len);
+					    sock->service_name_len);
 
 		if (ssap > 0) {
 			pr_debug("WKS %d\n", ssap);
@@ -176,7 +193,7 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
 				return LLCP_SAP_MAX;
 			}
 
-			set_bit(BIT(ssap), &local->local_wks);
+			set_bit(ssap, &local->local_wks);
 			mutex_unlock(&local->sdp_lock);
 
 			return ssap;
@@ -195,25 +212,25 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
 
 		pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
 
-		set_bit(BIT(ssap), &local->local_sdp);
+		set_bit(ssap, &local->local_sdp);
 		mutex_unlock(&local->sdp_lock);
 
 		return LLCP_WKS_NUM_SAP + ssap;
 
 	} else if (sock->ssap != 0) {
 		if (sock->ssap < LLCP_WKS_NUM_SAP) {
-			if (!(local->local_wks & BIT(sock->ssap))) {
-				set_bit(BIT(sock->ssap), &local->local_wks);
+			if (!test_bit(sock->ssap, &local->local_wks)) {
+				set_bit(sock->ssap, &local->local_wks);
 				mutex_unlock(&local->sdp_lock);
 
 				return sock->ssap;
 			}
 
 		} else if (sock->ssap < LLCP_SDP_NUM_SAP) {
-			if (!(local->local_sdp &
-				BIT(sock->ssap - LLCP_WKS_NUM_SAP))) {
-				set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP),
-							&local->local_sdp);
+			if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP,
+				      &local->local_sdp)) {
+				set_bit(sock->ssap - LLCP_WKS_NUM_SAP,
+					&local->local_sdp);
 				mutex_unlock(&local->sdp_lock);
 
 				return sock->ssap;
@@ -238,7 +255,7 @@ u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
 		return LLCP_SAP_MAX;
 	}
 
-	set_bit(BIT(local_ssap), &local->local_sap);
+	set_bit(local_ssap, &local->local_sap);
 
 	mutex_unlock(&local->sdp_lock);
 
@@ -265,12 +282,12 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
 
 	mutex_lock(&local->sdp_lock);
 
-	clear_bit(1 << local_ssap, sdp);
+	clear_bit(local_ssap, sdp);
 
 	mutex_unlock(&local->sdp_lock);
 }
 
-u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len)
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
 {
 	struct nfc_llcp_local *local;
 
@@ -294,7 +311,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 
 	version = LLCP_VERSION_11;
 	version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
-							1, &version_length);
+					 1, &version_length);
 	gb_len += version_length;
 
 	/* 1500 ms */
@@ -304,7 +321,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 
 	pr_debug("Local wks 0x%lx\n", local->local_wks);
 	wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
-								&wks_length);
+				     &wks_length);
 	gb_len += wks_length;
 
 	gb_len += ARRAY_SIZE(llcp_magic);
@@ -349,8 +366,7 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
 	memcpy(local->remote_gb, gb, gb_len);
 	local->remote_gb_len = gb_len;
 
-	if (local->remote_gb == NULL ||
-			local->remote_gb_len == 0)
+	if (local->remote_gb == NULL || local->remote_gb_len == 0)
 		return -ENODEV;
 
 	if (memcmp(local->remote_gb, llcp_magic, 3)) {
@@ -359,26 +375,27 @@ int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
 	}
 
 	return nfc_llcp_parse_tlv(local,
-			&local->remote_gb[3], local->remote_gb_len - 3);
+				  &local->remote_gb[3],
+				  local->remote_gb_len - 3);
 }
 
 static void nfc_llcp_tx_work(struct work_struct *work)
 {
 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-							tx_work);
+						    tx_work);
 	struct sk_buff *skb;
 
 	skb = skb_dequeue(&local->tx_queue);
 	if (skb != NULL) {
 		pr_debug("Sending pending skb\n");
 		nfc_data_exchange(local->dev, local->target_idx,
-					skb, nfc_llcp_recv, local);
+				  skb, nfc_llcp_recv, local);
 	} else {
 		nfc_llcp_send_symm(local->dev);
 	}
 
 	mod_timer(&local->link_timer,
-			jiffies + msecs_to_jiffies(local->remote_lto));
+		  jiffies + msecs_to_jiffies(local->remote_lto));
 }
 
 static u8 nfc_llcp_dsap(struct sk_buff *pdu)
@@ -408,13 +425,13 @@ static u8 nfc_llcp_nr(struct sk_buff *pdu)
 
 static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
 {
-	pdu->data[2] = (sock->send_n << 4) | ((sock->recv_n - 1) % 16);
+	pdu->data[2] = (sock->send_n << 4) | (sock->recv_n % 16);
 	sock->send_n = (sock->send_n + 1) % 16;
 	sock->recv_ack_n = (sock->recv_n - 1) % 16;
 }
 
 static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
-						u8 ssap, u8 dsap)
+					       u8 ssap, u8 dsap)
 {
 	struct nfc_llcp_sock *sock, *llcp_sock, *n;
 
@@ -438,7 +455,7 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 
 	list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {
 		pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
-				&llcp_sock->sk, llcp_sock->dsap);
+			 &llcp_sock->sk, llcp_sock->dsap);
 		if (llcp_sock->dsap == dsap) {
 			sock_hold(&llcp_sock->sk);
 			mutex_unlock(&local->socket_lock);
@@ -482,7 +499,7 @@ static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
 }
 
 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
-				struct sk_buff *skb)
+				  struct sk_buff *skb)
 {
 	struct sock *new_sk, *parent;
 	struct nfc_llcp_sock *sock, *new_sock;
@@ -494,7 +511,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 	pr_debug("%d %d\n", dsap, ssap);
 
 	nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
-				skb->len - LLCP_HEADER_SIZE);
+			   skb->len - LLCP_HEADER_SIZE);
 
 	if (dsap != LLCP_SAP_SDP) {
 		bound_sap = dsap;
@@ -513,7 +530,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 		lock_sock(&sock->sk);
 
 		if (sock->dsap == LLCP_SAP_SDP &&
-				sock->sk.sk_state == LLCP_LISTEN)
+		    sock->sk.sk_state == LLCP_LISTEN)
 			goto enqueue;
 	} else {
 		u8 *sn;
@@ -529,23 +546,23 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 
 		mutex_lock(&local->socket_lock);
 		for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
-								bound_sap++) {
+		     bound_sap++) {
 			sock = local->sockets[bound_sap];
 			if (sock == NULL)
 				continue;
 
 			if (sock->service_name == NULL ||
-				sock->service_name_len == 0)
+			    sock->service_name_len == 0)
 					continue;
 
 			if (sock->service_name_len != sn_len)
 				continue;
 
 			if (sock->dsap == LLCP_SAP_SDP &&
-					sock->sk.sk_state == LLCP_LISTEN &&
-					!memcmp(sn, sock->service_name, sn_len)) {
+			    sock->sk.sk_state == LLCP_LISTEN &&
+			    !memcmp(sn, sock->service_name, sn_len)) {
 				pr_debug("Found service name at SAP %d\n",
-								bound_sap);
+					 bound_sap);
 				sock_hold(&sock->sk);
 				mutex_unlock(&local->socket_lock);
 
@@ -570,8 +587,7 @@ enqueue:
 		goto fail;
 	}
 
-	new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type,
-				     GFP_ATOMIC);
+	new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
 	if (new_sk == NULL) {
 		reason = LLCP_DM_REJ;
 		release_sock(&sock->sk);
@@ -616,8 +632,39 @@ fail:
 
 }
 
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
+{
+	int nr_frames = 0;
+	struct nfc_llcp_local *local = sock->local;
+
+	pr_debug("Remote ready %d tx queue len %d remote rw %d",
+		 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
+		 local->remote_rw);
+
+	/* Try to queue some I frames for transmission */
+	while (sock->remote_ready &&
+	       skb_queue_len(&sock->tx_pending_queue) < local->remote_rw) {
+		struct sk_buff *pdu, *pending_pdu;
+
+		pdu = skb_dequeue(&sock->tx_queue);
+		if (pdu == NULL)
+			break;
+
+		/* Update N(S)/N(R) */
+		nfc_llcp_set_nrns(sock, pdu);
+
+		pending_pdu = skb_clone(pdu, GFP_KERNEL);
+
+		skb_queue_tail(&local->tx_queue, pdu);
+		skb_queue_tail(&sock->tx_pending_queue, pending_pdu);
+		nr_frames++;
+	}
+
+	return nr_frames;
+}
+
 static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
-				struct sk_buff *skb)
+			       struct sk_buff *skb)
 {
 	struct nfc_llcp_sock *llcp_sock;
 	struct sock *sk;
@@ -644,15 +691,15 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 		nfc_llcp_sock_put(llcp_sock);
 	}
 
-	if (ns == llcp_sock->recv_n)
-		llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
-	else
-		pr_err("Received out of sequence I PDU\n");
-
 	/* Pass the payload upstream */
 	if (ptype == LLCP_PDU_I) {
 		pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
 
+		if (ns == llcp_sock->recv_n)
+			llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
+		else
+			pr_err("Received out of sequence I PDU\n");
+
 		skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
 		if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
 			pr_err("receive queue is full\n");
@@ -673,30 +720,20 @@ static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
 			}
 	}
 
-	/* Queue some I frames for transmission */
-	while (llcp_sock->remote_ready &&
-		skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) {
-		struct sk_buff *pdu, *pending_pdu;
-
-		pdu = skb_dequeue(&llcp_sock->tx_queue);
-		if (pdu == NULL)
-			break;
-
-		/* Update N(S)/N(R) */
-		nfc_llcp_set_nrns(llcp_sock, pdu);
+	if (ptype == LLCP_PDU_RR)
+		llcp_sock->remote_ready = true;
+	else if (ptype == LLCP_PDU_RNR)
+		llcp_sock->remote_ready = false;
 
-		pending_pdu = skb_clone(pdu, GFP_KERNEL);
-
-		skb_queue_tail(&local->tx_queue, pdu);
-		skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu);
-	}
+	if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
+		nfc_llcp_send_rr(llcp_sock);
 
 	release_sock(sk);
 	nfc_llcp_sock_put(llcp_sock);
 }
 
 static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
-				struct sk_buff *skb)
+			       struct sk_buff *skb)
 {
 	struct nfc_llcp_sock *llcp_sock;
 	struct sock *sk;
@@ -718,7 +755,6 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 		nfc_llcp_sock_put(llcp_sock);
 	}
 
-
 	if (sk->sk_state == LLCP_CONNECTED) {
 		nfc_put_device(local->dev);
 		sk->sk_state = LLCP_CLOSED;
@@ -731,13 +767,11 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 	nfc_llcp_sock_put(llcp_sock);
 }
 
-static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
-				struct sk_buff *skb)
+static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
 {
 	struct nfc_llcp_sock *llcp_sock;
 	u8 dsap, ssap;
 
-
 	dsap = nfc_llcp_dsap(skb);
 	ssap = nfc_llcp_ssap(skb);
 
@@ -756,7 +790,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
 	llcp_sock->dsap = ssap;
 
 	nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
-				skb->len - LLCP_HEADER_SIZE);
+			   skb->len - LLCP_HEADER_SIZE);
 
 	nfc_llcp_sock_put(llcp_sock);
 }
@@ -764,7 +798,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local,
 static void nfc_llcp_rx_work(struct work_struct *work)
 {
 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-								rx_work);
+						    rx_work);
 	u8 dsap, ssap, ptype;
 	struct sk_buff *skb;
 
@@ -802,6 +836,7 @@ static void nfc_llcp_rx_work(struct work_struct *work)
 
 	case LLCP_PDU_I:
 	case LLCP_PDU_RR:
+	case LLCP_PDU_RNR:
 		pr_debug("I frame\n");
 		nfc_llcp_recv_hdlc(local, skb);
 		break;
@@ -821,7 +856,7 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
 
 	pr_debug("Received an LLCP PDU\n");
 	if (err < 0) {
-		pr_err("err %d", err);
+		pr_err("err %d\n", err);
 		return;
 	}
 
@@ -840,6 +875,8 @@ void nfc_llcp_mac_is_down(struct nfc_dev *dev)
 	if (local == NULL)
 		return;
 
+	nfc_llcp_clear_sdp(local);
+
 	/* Close and purge all existing sockets */
 	nfc_llcp_socket_release(local);
 }
@@ -865,7 +902,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
 		queue_work(local->tx_wq, &local->tx_work);
 	} else {
 		mod_timer(&local->link_timer,
-			jiffies + msecs_to_jiffies(local->remote_lto));
+			  jiffies + msecs_to_jiffies(local->remote_lto));
 	}
 }
 
@@ -891,8 +928,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 	skb_queue_head_init(&local->tx_queue);
 	INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
 	snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
-	local->tx_wq = alloc_workqueue(name,
-			WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	local->tx_wq =
+		alloc_workqueue(name,
+				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				1);
 	if (local->tx_wq == NULL) {
 		err = -ENOMEM;
 		goto err_local;
@@ -901,8 +940,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 	local->rx_pending = NULL;
 	INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
 	snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
-	local->rx_wq = alloc_workqueue(name,
-			WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	local->rx_wq =
+		alloc_workqueue(name,
+				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				1);
 	if (local->rx_wq == NULL) {
 		err = -ENOMEM;
 		goto err_tx_wq;
@@ -910,8 +951,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 
 	INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
 	snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
-	local->timeout_wq = alloc_workqueue(name,
-			WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	local->timeout_wq =
+		alloc_workqueue(name,
+				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				1);
 	if (local->timeout_wq == NULL) {
 		err = -ENOMEM;
 		goto err_rx_wq;