summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--net/nfc/llcp/llcp.c241
-rw-r--r--net/nfc/llcp/llcp.h2
2 files changed, 166 insertions, 77 deletions
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 9ab17ec50ce..6094a2099bd 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);
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index 374cc4779af..83b8bba5a28 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -37,6 +37,7 @@ enum llcp_state {
#define LLCP_LOCAL_NUM_SAP 32
#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP)
#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP)
+#define LLCP_SDP_UNBOUND (LLCP_MAX_SAP + 1)
struct nfc_llcp_sock;
@@ -69,6 +70,7 @@ struct nfc_llcp_local {
unsigned long local_wks; /* Well known services */
unsigned long local_sdp; /* Local services */
unsigned long local_sap; /* Local SAPs, not available for discovery */
+ atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP];
/* local */
u8 gb[NFC_MAX_GT_LEN];
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-23 14:18:25 +0000