1 files changed, 164 insertions, 77 deletions

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 9ab17ec..6094a20 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -131,6 +131,44 @@ int nfc_llcp_local_put(struct nfc_llcp_local *local)
 	return kref_put(&local->ref, local_release);
 }
 
+static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
+					       u8 ssap, u8 dsap)
+{
+	struct sock *sk;
+	struct hlist_node *node;
+	struct nfc_llcp_sock *llcp_sock;
+
+	pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
+	if (ssap == 0 && dsap == 0)
+		return NULL;
+
+	read_lock(&local->sockets.lock);
+
+	llcp_sock = NULL;
+
+	sk_for_each(sk, node, &local->sockets.head) {
+		llcp_sock = nfc_llcp_sock(sk);
+
+		if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap)
+			break;
+	}
+
+	read_unlock(&local->sockets.lock);
+
+	if (llcp_sock == NULL)
+		return NULL;
+
+	sock_hold(&llcp_sock->sk);
+
+	return llcp_sock;
+}
+
+static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+{
+	sock_put(&sock->sk);
+}
+
 static void nfc_llcp_timeout_work(struct work_struct *work)
 {
 	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
@@ -191,6 +229,51 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
 	return -EINVAL;
 }
 
+static
+struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
+					    u8 *sn, size_t sn_len)
+{
+	struct sock *sk;
+	struct hlist_node *node;
+	struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+	pr_debug("sn %zd %p\n", sn_len, sn);
+
+	if (sn == NULL || sn_len == 0)
+		return NULL;
+
+	read_lock(&local->sockets.lock);
+
+	llcp_sock = NULL;
+
+	sk_for_each(sk, node, &local->sockets.head) {
+		tmp_sock = nfc_llcp_sock(sk);
+
+		pr_debug("llcp sock %p\n", tmp_sock);
+
+		if (tmp_sock->sk.sk_state != LLCP_LISTEN)
+			continue;
+
+		if (tmp_sock->service_name == NULL ||
+		    tmp_sock->service_name_len == 0)
+			continue;
+
+		if (tmp_sock->service_name_len != sn_len)
+			continue;
+
+		if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
+			llcp_sock = tmp_sock;
+			break;
+		}
+	}
+
+	read_unlock(&local->sockets.lock);
+
+	pr_debug("Found llcp sock %p\n", llcp_sock);
+
+	return llcp_sock;
+}
+
 u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
 			 struct nfc_llcp_sock *sock)
 {
@@ -217,22 +300,19 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
 		}
 
 		/*
-		 * This is not a well known service,
-		 * we should try to find a local SDP free spot
+		 * Check if there already is a non WKS socket bound
+		 * to this service name.
 		 */
-		ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
-		if (ssap == LLCP_SDP_NUM_SAP) {
+		if (nfc_llcp_sock_from_sn(local, sock->service_name,
+					  sock->service_name_len) != NULL) {
 			mutex_unlock(&local->sdp_lock);
 
 			return LLCP_SAP_MAX;
 		}
 
-		pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
-
-		set_bit(ssap, &local->local_sdp);
 		mutex_unlock(&local->sdp_lock);
 
-		return LLCP_WKS_NUM_SAP + ssap;
+		return LLCP_SDP_UNBOUND;
 
 	} else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
 		if (!test_bit(sock->ssap, &local->local_wks)) {
@@ -276,8 +356,34 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
 		local_ssap = ssap;
 		sdp = &local->local_wks;
 	} else if (ssap < LLCP_LOCAL_NUM_SAP) {
+		atomic_t *client_cnt;
+
 		local_ssap = ssap - LLCP_WKS_NUM_SAP;
 		sdp = &local->local_sdp;
+		client_cnt = &local->local_sdp_cnt[local_ssap];
+
+		pr_debug("%d clients\n", atomic_read(client_cnt));
+
+		mutex_lock(&local->sdp_lock);
+
+		if (atomic_dec_and_test(client_cnt)) {
+			struct nfc_llcp_sock *l_sock;
+
+			pr_debug("No more clients for SAP %d\n", ssap);
+
+			clear_bit(local_ssap, sdp);
+
+			/* Find the listening sock and set it back to UNBOUND */
+			l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
+			if (l_sock) {
+				l_sock->ssap = LLCP_SDP_UNBOUND;
+				nfc_llcp_sock_put(l_sock);
+			}
+		}
+
+		mutex_unlock(&local->sdp_lock);
+
+		return;
 	} else if (ssap < LLCP_MAX_SAP) {
 		local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
 		sdp = &local->local_sap;
@@ -292,6 +398,28 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
 	mutex_unlock(&local->sdp_lock);
 }
 
+static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
+{
+	u8 ssap;
+
+	mutex_lock(&local->sdp_lock);
+
+	ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
+	if (ssap == LLCP_SDP_NUM_SAP) {
+		mutex_unlock(&local->sdp_lock);
+
+		return LLCP_SAP_MAX;
+	}
+
+	pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
+
+	set_bit(ssap, &local->local_sdp);
+
+	mutex_unlock(&local->sdp_lock);
+
+	return LLCP_WKS_NUM_SAP + ssap;
+}
+
 static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 {
 	u8 *gb_cur, *version_tlv, version, version_length;
@@ -493,74 +621,12 @@ out:
 	return llcp_sock;
 }
 
-static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
-					       u8 ssap, u8 dsap)
-{
-	struct sock *sk;
-	struct hlist_node *node;
-	struct nfc_llcp_sock *llcp_sock;
-
-	pr_debug("ssap dsap %d %d\n", ssap, dsap);
-
-	if (ssap == 0 && dsap == 0)
-		return NULL;
-
-	read_lock(&local->sockets.lock);
-
-	llcp_sock = NULL;
-
-	sk_for_each(sk, node, &local->sockets.head) {
-		llcp_sock = nfc_llcp_sock(sk);
-
-		if (llcp_sock->ssap == ssap &&
-		    llcp_sock->dsap == dsap)
-			break;
-	}
-
-	read_unlock(&local->sockets.lock);
-
-	if (llcp_sock == NULL)
-		return NULL;
-
-	sock_hold(&llcp_sock->sk);
-
-	return llcp_sock;
-}
-
 static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
 						  u8 *sn, size_t sn_len)
 {
-	struct sock *sk;
-	struct hlist_node *node;
 	struct nfc_llcp_sock *llcp_sock;
 
-	pr_debug("sn %zd\n", sn_len);
-
-	if (sn == NULL || sn_len == 0)
-		return NULL;
-
-	read_lock(&local->sockets.lock);
-
-	llcp_sock = NULL;
-
-	sk_for_each(sk, node, &local->sockets.head) {
-		llcp_sock = nfc_llcp_sock(sk);
-
-		if (llcp_sock->sk.sk_state != LLCP_LISTEN)
-			continue;
-
-		if (llcp_sock->service_name == NULL ||
-		    llcp_sock->service_name_len == 0)
-			continue;
-
-		if (llcp_sock->service_name_len != sn_len)
-			continue;
-
-		if (memcmp(sn, llcp_sock->service_name, sn_len) == 0)
-			break;
-	}
-
-	read_unlock(&local->sockets.lock);
+	llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
 
 	if (llcp_sock == NULL)
 		return NULL;
@@ -570,11 +636,6 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
 	return llcp_sock;
 }
 
-static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
-{
-	sock_put(&sock->sk);
-}
-
 static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
 {
 	u8 *tlv = &skb->data[2], type, length;
@@ -646,6 +707,21 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 		goto fail;
 	}
 
+	if (sock->ssap == LLCP_SDP_UNBOUND) {
+		u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
+
+		pr_debug("First client, reserving %d\n", ssap);
+
+		if (ssap == LLCP_SAP_MAX) {
+			reason = LLCP_DM_REJ;
+			release_sock(&sock->sk);
+			sock_put(&sock->sk);
+			goto fail;
+		}
+
+		sock->ssap = ssap;
+	}
+
 	new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
 	if (new_sk == NULL) {
 		reason = LLCP_DM_REJ;
@@ -659,10 +735,21 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 	new_sock->local = nfc_llcp_local_get(local);
 	new_sock->miu = local->remote_miu;
 	new_sock->nfc_protocol = sock->nfc_protocol;
-	new_sock->ssap = sock->ssap;
 	new_sock->dsap = ssap;
 	new_sock->target_idx = local->target_idx;
 	new_sock->parent = parent;
+	new_sock->ssap = sock->ssap;
+	if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
+		atomic_t *client_count;
+
+		pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
+
+		client_count =
+			&local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
+
+		atomic_inc(client_count);
+		new_sock->reserved_ssap = sock->ssap;
+	}
 
 	nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
 				      skb->len - LLCP_HEADER_SIZE);