1 files changed, 164 insertions, 77 deletions

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 9ab17ec50ce7..6094a2099bd6 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);