NFC: Socket linked list (a69f32af) · Commits · e / devices / android_kernel_fairphone_FP3

net/nfc/llcp/llcp.c

+137 −112

Original line number	Original line	Diff line number	Diff line
	@@ -31,45 +31,41 @@ static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};

	static struct list_head llcp_devices;		static struct list_head llcp_devices;

	static void nfc_llcp_socket_release(struct nfc_llcp_local *local)		void nfc_llcp_sock_link(struct llcp_sock_list l, struct sock sk)
	{		{
	struct nfc_llcp_sock parent, s, *n;		write_lock(&l->lock);
	struct sock sk, parent_sk;		sk_add_node(sk, &l->head);
	int i;		write_unlock(&l->lock);
			}

	mutex_lock(&local->socket_lock);		void nfc_llcp_sock_unlink(struct llcp_sock_list l, struct sock sk)
			{
			write_lock(&l->lock);
			sk_del_node_init(sk);
			write_unlock(&l->lock);
			}

	for (i = 0; i < LLCP_MAX_SAP; i++) {		static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
	parent = local->sockets[i];		{
	if (parent == NULL)		struct sock *sk;
	continue;		struct hlist_node node, tmp;
			struct nfc_llcp_sock *llcp_sock;

	/* Release all child sockets */		write_lock(&local->sockets.lock);
	list_for_each_entry_safe(s, n, &parent->list, list) {
	list_del_init(&s->list);		sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
	sk = &s->sk;		llcp_sock = nfc_llcp_sock(sk);

	lock_sock(sk);		lock_sock(sk);

	if (sk->sk_state == LLCP_CONNECTED)		if (sk->sk_state == LLCP_CONNECTED)
	nfc_put_device(s->dev);		nfc_put_device(llcp_sock->dev);

	sk->sk_state = LLCP_CLOSED;

	release_sock(sk);

	sock_orphan(sk);		if (sk->sk_state == LLCP_LISTEN) {
	}

	parent_sk = &parent->sk;

	lock_sock(parent_sk);

	if (parent_sk->sk_state == LLCP_LISTEN) {
	struct nfc_llcp_sock lsk, n;		struct nfc_llcp_sock lsk, n;
	struct sock *accept_sk;		struct sock *accept_sk;

	list_for_each_entry_safe(lsk, n, &parent->accept_queue,		list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
	accept_queue) {		accept_queue) {
	accept_sk = &lsk->sk;		accept_sk = &lsk->sk;
	lock_sock(accept_sk);		lock_sock(accept_sk);
	@@ -84,17 +80,16 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
	}		}
	}		}

	if (parent_sk->sk_state == LLCP_CONNECTED)		sk->sk_state = LLCP_CLOSED;
	nfc_put_device(parent->dev);

	parent_sk->sk_state = LLCP_CLOSED;		release_sock(sk);

	release_sock(parent_sk);		sock_orphan(sk);

	sock_orphan(parent_sk);		sk_del_node_init(sk);
	}		}

	mutex_unlock(&local->socket_lock);		write_unlock(&local->sockets.lock);
	}		}

	struct nfc_llcp_local nfc_llcp_local_get(struct nfc_llcp_local local)		struct nfc_llcp_local nfc_llcp_local_get(struct nfc_llcp_local local)
	@@ -122,6 +117,11 @@ static void local_release(struct kref *ref)

	int nfc_llcp_local_put(struct nfc_llcp_local *local)		int nfc_llcp_local_put(struct nfc_llcp_local *local)
	{		{
			WARN_ON(local == NULL);

			if (local == NULL)
			return 0;

	return kref_put(&local->ref, local_release);		return kref_put(&local->ref, local_release);
	}		}

	@@ -465,46 +465,107 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock sock, struct sk_buff pdu)
	sock->recv_ack_n = (sock->recv_n - 1) % 16;		sock->recv_ack_n = (sock->recv_n - 1) % 16;
	}		}

			static struct nfc_llcp_sock nfc_llcp_connecting_sock_get(struct nfc_llcp_local local,
			u8 ssap)
			{
			struct sock *sk;
			struct nfc_llcp_sock *llcp_sock;
			struct hlist_node *node;

			read_lock(&local->connecting_sockets.lock);

			sk_for_each(sk, node, &local->connecting_sockets.head) {
			llcp_sock = nfc_llcp_sock(sk);

			if (llcp_sock->ssap == ssap)
			goto out;
			}

			llcp_sock = NULL;

			out:
			read_unlock(&local->connecting_sockets.lock);

			sock_hold(&llcp_sock->sk);

			return llcp_sock;
			}

	static struct nfc_llcp_sock nfc_llcp_sock_get(struct nfc_llcp_local local,		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;		struct sock *sk;
			struct hlist_node *node;
			struct nfc_llcp_sock *llcp_sock;

	pr_debug("ssap dsap %d %d\n", ssap, dsap);		pr_debug("ssap dsap %d %d\n", ssap, dsap);

	if (ssap == 0 && dsap == 0)		if (ssap == 0 && dsap == 0)
	return NULL;		return NULL;

	mutex_lock(&local->socket_lock);		read_lock(&local->sockets.lock);
	sock = local->sockets[ssap];
	if (sock == NULL) {		llcp_sock = NULL;
	mutex_unlock(&local->socket_lock);
	return NULL;
	}

	pr_debug("root dsap %d (%d)\n", sock->dsap, dsap);		sk_for_each(sk, node, &local->sockets.head) {
			llcp_sock = nfc_llcp_sock(sk);

	if (sock->dsap == dsap) {		if (llcp_sock->ssap == ssap &&
	sock_hold(&sock->sk);		llcp_sock->dsap == dsap)
	mutex_unlock(&local->socket_lock);		break;
	return sock;
	}		}

	list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {		read_unlock(&local->sockets.lock);
	pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
	&llcp_sock->sk, llcp_sock->dsap);		if (llcp_sock == NULL)
	if (llcp_sock->dsap == dsap) {		return NULL;

	sock_hold(&llcp_sock->sk);		sock_hold(&llcp_sock->sk);
	mutex_unlock(&local->socket_lock);
	return llcp_sock;		return llcp_sock;
	}		}
	}

	pr_err("Could not find socket for %d %d\n", ssap, dsap);		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;

	mutex_unlock(&local->socket_lock);		pr_debug("sn %zd\n", sn_len);

			if (sn == NULL \|\| sn_len == 0)
	return NULL;		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);

			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)		static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
	@@ -540,7 +601,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
	{		{
	struct sock new_sk, parent;		struct sock new_sk, parent;
	struct nfc_llcp_sock sock, new_sock;		struct nfc_llcp_sock sock, new_sock;
	u8 dsap, ssap, bound_sap, reason;		u8 dsap, ssap, reason;

	dsap = nfc_llcp_dsap(skb);		dsap = nfc_llcp_dsap(skb);
	ssap = nfc_llcp_ssap(skb);		ssap = nfc_llcp_ssap(skb);
	@@ -551,24 +612,11 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
	skb->len - LLCP_HEADER_SIZE);		skb->len - LLCP_HEADER_SIZE);

	if (dsap != LLCP_SAP_SDP) {		if (dsap != LLCP_SAP_SDP) {
	bound_sap = dsap;		sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
			if (sock == NULL \|\| sock->sk.sk_state != LLCP_LISTEN) {
	mutex_lock(&local->socket_lock);
	sock = local->sockets[dsap];
	if (sock == NULL) {
	mutex_unlock(&local->socket_lock);
	reason = LLCP_DM_NOBOUND;		reason = LLCP_DM_NOBOUND;
	goto fail;		goto fail;
	}		}

	sock_hold(&sock->sk);
	mutex_unlock(&local->socket_lock);

	lock_sock(&sock->sk);

	if (sock->dsap == LLCP_SAP_SDP &&
	sock->sk.sk_state == LLCP_LISTEN)
	goto enqueue;
	} else {		} else {
	u8 *sn;		u8 *sn;
	size_t sn_len;		size_t sn_len;
	@@ -581,40 +629,15 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,

	pr_debug("Service name length %zu\n", sn_len);		pr_debug("Service name length %zu\n", sn_len);

	mutex_lock(&local->socket_lock);		sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
	for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;		if (sock == NULL) {
	bound_sap++) {		reason = LLCP_DM_NOBOUND;
	sock = local->sockets[bound_sap];		goto fail;
	if (sock == NULL)
	continue;

	if (sock->service_name == NULL \|\|
	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)) {
	pr_debug("Found service name at SAP %d\n",
	bound_sap);
	sock_hold(&sock->sk);
	mutex_unlock(&local->socket_lock);

	lock_sock(&sock->sk);

	goto enqueue;
	}
	}		}
	mutex_unlock(&local->socket_lock);
	}		}

	reason = LLCP_DM_NOBOUND;		lock_sock(&sock->sk);
	goto fail;

	enqueue:
	parent = &sock->sk;		parent = &sock->sk;

	if (sk_acceptq_is_full(parent)) {		if (sk_acceptq_is_full(parent)) {
	@@ -636,13 +659,13 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
	new_sock->dev = local->dev;		new_sock->dev = local->dev;
	new_sock->local = nfc_llcp_local_get(local);		new_sock->local = nfc_llcp_local_get(local);
	new_sock->nfc_protocol = sock->nfc_protocol;		new_sock->nfc_protocol = sock->nfc_protocol;
	new_sock->ssap = bound_sap;		new_sock->ssap = sock->ssap;
	new_sock->dsap = ssap;		new_sock->dsap = ssap;
	new_sock->parent = parent;		new_sock->parent = parent;

	pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);		pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);

	list_add_tail(&new_sock->list, &sock->list);		nfc_llcp_sock_link(&local->sockets, new_sk);

	nfc_llcp_accept_enqueue(&sock->sk, new_sk);		nfc_llcp_accept_enqueue(&sock->sk, new_sk);

	@@ -813,11 +836,7 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local local, struct sk_buff skb)
	dsap = nfc_llcp_dsap(skb);		dsap = nfc_llcp_dsap(skb);
	ssap = nfc_llcp_ssap(skb);		ssap = nfc_llcp_ssap(skb);

	llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);		llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);

	if (llcp_sock == NULL)
	llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);

	if (llcp_sock == NULL) {		if (llcp_sock == NULL) {
	pr_err("Invalid CC\n");		pr_err("Invalid CC\n");
	nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
	@@ -825,9 +844,13 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local local, struct sk_buff skb)
	return;		return;
	}		}

	llcp_sock->dsap = ssap;
	sk = &llcp_sock->sk;		sk = &llcp_sock->sk;

			/* Unlink from connecting and link to the client array */
			nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
			nfc_llcp_sock_link(&local->sockets, sk);
			llcp_sock->dsap = ssap;

	nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],		nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
	skb->len - LLCP_HEADER_SIZE);		skb->len - LLCP_HEADER_SIZE);

	@@ -967,7 +990,6 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
	INIT_LIST_HEAD(&local->list);		INIT_LIST_HEAD(&local->list);
	kref_init(&local->ref);		kref_init(&local->ref);
	mutex_init(&local->sdp_lock);		mutex_init(&local->sdp_lock);
	mutex_init(&local->socket_lock);
	init_timer(&local->link_timer);		init_timer(&local->link_timer);
	local->link_timer.data = (unsigned long) local;		local->link_timer.data = (unsigned long) local;
	local->link_timer.function = nfc_llcp_symm_timer;		local->link_timer.function = nfc_llcp_symm_timer;
	@@ -1007,6 +1029,9 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
	goto err_rx_wq;		goto err_rx_wq;
	}		}

			local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
			local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);

	nfc_llcp_build_gb(local);		nfc_llcp_build_gb(local);

	local->remote_miu = LLCP_DEFAULT_MIU;		local->remote_miu = LLCP_DEFAULT_MIU;

net/nfc/llcp/llcp.h

+9 −3

Original line number	Original line	Diff line number	Diff line
	@@ -40,6 +40,11 @@ enum llcp_state {

	struct nfc_llcp_sock;		struct nfc_llcp_sock;

			struct llcp_sock_list {
			struct hlist_head head;
			rwlock_t lock;
			};

	struct nfc_llcp_local {		struct nfc_llcp_local {
	struct list_head list;		struct list_head list;
	struct nfc_dev *dev;		struct nfc_dev *dev;
	@@ -47,7 +52,6 @@ struct nfc_llcp_local {
	struct kref ref;		struct kref ref;

	struct mutex sdp_lock;		struct mutex sdp_lock;
	struct mutex socket_lock;

	struct timer_list link_timer;		struct timer_list link_timer;
	struct sk_buff_head tx_queue;		struct sk_buff_head tx_queue;
	@@ -82,12 +86,12 @@ struct nfc_llcp_local {
	u8 remote_rw;		u8 remote_rw;

	/* sockets array */		/* sockets array */
	struct nfc_llcp_sock *sockets[LLCP_MAX_SAP];		struct llcp_sock_list sockets;
			struct llcp_sock_list connecting_sockets;
	};		};

	struct nfc_llcp_sock {		struct nfc_llcp_sock {
	struct sock sk;		struct sock sk;
	struct list_head list;
	struct nfc_dev *dev;		struct nfc_dev *dev;
	struct nfc_llcp_local *local;		struct nfc_llcp_local *local;
	u32 target_idx;		u32 target_idx;
	@@ -166,6 +170,8 @@ struct nfc_llcp_sock {
	#define LLCP_DM_REJ 0x03		#define LLCP_DM_REJ 0x03


			void nfc_llcp_sock_link(struct llcp_sock_list l, struct sock s);
			void nfc_llcp_sock_unlink(struct llcp_sock_list l, struct sock s);
	struct nfc_llcp_local nfc_llcp_find_local(struct nfc_dev dev);		struct nfc_llcp_local nfc_llcp_find_local(struct nfc_dev dev);
	struct nfc_llcp_local nfc_llcp_local_get(struct nfc_llcp_local local);		struct nfc_llcp_local nfc_llcp_local_get(struct nfc_llcp_local local);
	int nfc_llcp_local_put(struct nfc_llcp_local *local);		int nfc_llcp_local_put(struct nfc_llcp_local *local);

net/nfc/llcp/sock.c

+12 −20

Original line number	Original line	Diff line number	Diff line
	@@ -124,7 +124,7 @@ static int llcp_sock_bind(struct socket sock, struct sockaddr addr, int alen)
	if (llcp_sock->ssap == LLCP_MAX_SAP)		if (llcp_sock->ssap == LLCP_MAX_SAP)
	goto put_dev;		goto put_dev;

	local->sockets[llcp_sock->ssap] = llcp_sock;		nfc_llcp_sock_link(&local->sockets, sk);

	pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);		pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);

	@@ -379,15 +379,6 @@ static int llcp_sock_release(struct socket *sock)
	goto out;		goto out;
	}		}

	mutex_lock(&local->socket_lock);

	if (llcp_sock == local->sockets[llcp_sock->ssap])
	local->sockets[llcp_sock->ssap] = NULL;
	else
	list_del_init(&llcp_sock->list);

	mutex_unlock(&local->socket_lock);

	lock_sock(sk);		lock_sock(sk);

	/* Send a DISC */		/* Send a DISC */
	@@ -412,14 +403,12 @@ static int llcp_sock_release(struct socket *sock)
	}		}
	}		}

	/* Freeing the SAP */
	if ((sk->sk_state == LLCP_CONNECTED
	&& llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) \|\|
	sk->sk_state == LLCP_BOUND \|\| sk->sk_state == LLCP_LISTEN)
	nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);		nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);

	release_sock(sk);		release_sock(sk);

			nfc_llcp_sock_unlink(&local->sockets, sk);

	out:		out:
	sock_orphan(sk);		sock_orphan(sk);
	sock_put(sk);		sock_put(sk);
	@@ -505,21 +494,26 @@ static int llcp_sock_connect(struct socket sock, struct sockaddr _addr,
	llcp_sock->service_name_len,		llcp_sock->service_name_len,
	GFP_KERNEL);		GFP_KERNEL);

	local->sockets[llcp_sock->ssap] = llcp_sock;		nfc_llcp_sock_link(&local->connecting_sockets, sk);

	ret = nfc_llcp_send_connect(llcp_sock);		ret = nfc_llcp_send_connect(llcp_sock);
	if (ret)		if (ret)
	goto put_dev;		goto sock_unlink;

	ret = sock_wait_state(sk, LLCP_CONNECTED,		ret = sock_wait_state(sk, LLCP_CONNECTED,
	sock_sndtimeo(sk, flags & O_NONBLOCK));		sock_sndtimeo(sk, flags & O_NONBLOCK));
	if (ret)		if (ret)
	goto put_dev;		goto sock_unlink;

	release_sock(sk);		release_sock(sk);

	return 0;		return 0;

			sock_unlink:
			nfc_llcp_put_ssap(local, llcp_sock->ssap);

			nfc_llcp_sock_unlink(&local->connecting_sockets, sk);

	put_dev:		put_dev:
	nfc_put_device(dev);		nfc_put_device(dev);

	@@ -690,7 +684,6 @@ struct sock nfc_llcp_sock_alloc(struct socket sock, int type, gfp_t gfp)
	skb_queue_head_init(&llcp_sock->tx_queue);		skb_queue_head_init(&llcp_sock->tx_queue);
	skb_queue_head_init(&llcp_sock->tx_pending_queue);		skb_queue_head_init(&llcp_sock->tx_pending_queue);
	skb_queue_head_init(&llcp_sock->tx_backlog_queue);		skb_queue_head_init(&llcp_sock->tx_backlog_queue);
	INIT_LIST_HEAD(&llcp_sock->list);
	INIT_LIST_HEAD(&llcp_sock->accept_queue);		INIT_LIST_HEAD(&llcp_sock->accept_queue);

	if (sock != NULL)		if (sock != NULL)
	@@ -708,7 +701,6 @@ void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
	skb_queue_purge(&sock->tx_backlog_queue);		skb_queue_purge(&sock->tx_backlog_queue);

	list_del_init(&sock->accept_queue);		list_del_init(&sock->accept_queue);
	list_del_init(&sock->list);

	sock->parent = NULL;		sock->parent = NULL;