Commit 4463523b authored by Thierry Escande's avatar Thierry Escande Committed by Samuel Ortiz
Browse files

NFC: LLCP raw socket support 



This adds support for socket of type SOCK_RAW to LLCP.
sk_buff are copied and sent to raw sockets with a 2 bytes extra header:
The first byte header contains the nfc adapter index.
The second one contains flags:
- 0x01 - Direction (0=RX, 1=TX)
- 0x02-0x80 - Reserved
A raw socket has to be explicitly bound to a nfc adapter. This is achieved
by specifying the adapter index to be bound to in the dev_idx field of the
sockaddr_nfc_llcp struct passed to bind().

Signed-off-by: default avatarThierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: default avatarSamuel Ortiz <sameo@linux.intel.com>
parent ee5e8d81

include/linux/nfc.h

+11 −0
Original line number Diff line number Diff line
@@ -183,4 +183,15 @@ struct sockaddr_nfc_llcp { 


#define NFC_HEADER_SIZE 1



/**

 * Pseudo-header info for raw socket packets

 * First byte is the adapter index

 * Second byte contains flags

 *  - 0x01 - Direction (0=RX, 1=TX)

 *  - 0x02-0x80 - Reserved

 **/

#define NFC_LLCP_RAW_HEADER_SIZE	2

#define NFC_LLCP_DIRECTION_RX		0x00

#define NFC_LLCP_DIRECTION_TX		0x01



#endif /*__LINUX_NFC_H */

net/nfc/llcp/commands.c

+2 −0
Original line number Diff line number Diff line
@@ -312,6 +312,8 @@ int nfc_llcp_send_symm(struct nfc_dev *dev) 


	skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);



	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX);



	return nfc_data_exchange(dev, local->target_idx, skb,

				 nfc_llcp_recv, local);

}

net/nfc/llcp/llcp.c

+46 −0
Original line number Diff line number Diff line
@@ -558,6 +558,46 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) 
	sock->recv_ack_n = (sock->recv_n - 1) % 16;

}



void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,

			       struct sk_buff *skb, u8 direction)

{

	struct hlist_node *node;

	struct sk_buff *skb_copy = NULL, *nskb;

	struct sock *sk;

	u8 *data;



	read_lock(&local->raw_sockets.lock);



	sk_for_each(sk, node, &local->raw_sockets.head) {

		if (sk->sk_state != LLCP_BOUND)

			continue;



		if (skb_copy == NULL) {

			skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,

					       GFP_ATOMIC);



			if (skb_copy == NULL)

				continue;



			data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);



			data[0] = local->dev ? local->dev->idx : 0xFF;

			data[1] = direction;

		}



		nskb = skb_clone(skb_copy, GFP_ATOMIC);

		if (!nskb)

			continue;



		if (sock_queue_rcv_skb(sk, nskb))

			kfree_skb(nskb);

	}



	read_unlock(&local->raw_sockets.lock);



	kfree_skb(skb_copy);

}



static void nfc_llcp_tx_work(struct work_struct *work)

{

	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
@@ -578,6 +618,9 @@ static void nfc_llcp_tx_work(struct work_struct *work) 
				       DUMP_PREFIX_OFFSET, 16, 1,

				       skb->data, skb->len, true);



			nfc_llcp_send_to_raw_sock(local, skb,

						  NFC_LLCP_DIRECTION_TX);



			ret = nfc_data_exchange(local->dev, local->target_idx,

						skb, nfc_llcp_recv, local);
@@ -1022,6 +1065,8 @@ static void nfc_llcp_rx_work(struct work_struct *work) 
		print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,

			       16, 1, skb->data, skb->len, true);



	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);



	switch (ptype) {

	case LLCP_PDU_SYMM:

		pr_debug("SYMM\n");
@@ -1158,6 +1203,7 @@ int nfc_llcp_register_device(struct nfc_dev *ndev) 


	rwlock_init(&local->sockets.lock);

	rwlock_init(&local->connecting_sockets.lock);

	rwlock_init(&local->raw_sockets.lock);



	nfc_llcp_build_gb(local);

net/nfc/llcp/llcp.h

+3 −0
Original line number Diff line number Diff line
@@ -86,6 +86,7 @@ struct nfc_llcp_local { 
	/* sockets array */

	struct llcp_sock_list sockets;

	struct llcp_sock_list connecting_sockets;

	struct llcp_sock_list raw_sockets;

};



struct nfc_llcp_sock {
@@ -184,6 +185,8 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, 
u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);

void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap);

int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock);

void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,

			       struct sk_buff *skb, u8 direction);



/* Sock API */

struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);

net/nfc/llcp/sock.c

+86 −4
Original line number Diff line number Diff line
@@ -142,6 +142,60 @@ error: 
	return ret;

}



static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,

			      int alen)

{

	struct sock *sk = sock->sk;

	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);

	struct nfc_llcp_local *local;

	struct nfc_dev *dev;

	struct sockaddr_nfc_llcp llcp_addr;

	int len, ret = 0;



	if (!addr || addr->sa_family != AF_NFC)

		return -EINVAL;



	pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);



	memset(&llcp_addr, 0, sizeof(llcp_addr));

	len = min_t(unsigned int, sizeof(llcp_addr), alen);

	memcpy(&llcp_addr, addr, len);



	lock_sock(sk);



	if (sk->sk_state != LLCP_CLOSED) {

		ret = -EBADFD;

		goto error;

	}



	dev = nfc_get_device(llcp_addr.dev_idx);

	if (dev == NULL) {

		ret = -ENODEV;

		goto error;

	}



	local = nfc_llcp_find_local(dev);

	if (local == NULL) {

		ret = -ENODEV;

		goto put_dev;

	}



	llcp_sock->dev = dev;

	llcp_sock->local = nfc_llcp_local_get(local);

	llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;



	nfc_llcp_sock_link(&local->raw_sockets, sk);



	sk->sk_state = LLCP_BOUND;



put_dev:

	nfc_put_device(dev);



error:

	release_sock(sk);

	return ret;

}



static int llcp_sock_listen(struct socket *sock, int backlog)

{

	struct sock *sk = sock->sk;
@@ -418,6 +472,9 @@ static int llcp_sock_release(struct socket *sock) 


	release_sock(sk);



	if (sock->type == SOCK_RAW)

		nfc_llcp_sock_unlink(&local->raw_sockets, sk);

	else

		nfc_llcp_sock_unlink(&local->sockets, sk);



out:
@@ -614,7 +671,7 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock, 
	if (!(flags & MSG_PEEK)) {



		/* SOCK_STREAM: re-queue skb if it contains unreceived data */

		if (sk->sk_type == SOCK_STREAM) {

		if (sk->sk_type == SOCK_STREAM || sk->sk_type == SOCK_RAW) {

			skb_pull(skb, copied);

			if (skb->len) {

				skb_queue_head(&sk->sk_receive_queue, skb);
@@ -655,6 +712,26 @@ static const struct proto_ops llcp_sock_ops = { 
	.mmap           = sock_no_mmap,

};



static const struct proto_ops llcp_rawsock_ops = {

	.family         = PF_NFC,

	.owner          = THIS_MODULE,

	.bind           = llcp_raw_sock_bind,

	.connect        = sock_no_connect,

	.release        = llcp_sock_release,

	.socketpair     = sock_no_socketpair,

	.accept         = sock_no_accept,

	.getname        = llcp_sock_getname,

	.poll           = llcp_sock_poll,

	.ioctl          = sock_no_ioctl,

	.listen         = sock_no_listen,

	.shutdown       = sock_no_shutdown,

	.setsockopt     = sock_no_setsockopt,

	.getsockopt     = sock_no_getsockopt,

	.sendmsg        = sock_no_sendmsg,

	.recvmsg        = llcp_sock_recvmsg,

	.mmap           = sock_no_mmap,

};



static void llcp_sock_destruct(struct sock *sk)

{

	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
@@ -732,9 +809,14 @@ static int llcp_sock_create(struct net *net, struct socket *sock, 


	pr_debug("%p\n", sock);



	if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM)

	if (sock->type != SOCK_STREAM &&

	    sock->type != SOCK_DGRAM &&

	    sock->type != SOCK_RAW)

		return -ESOCKTNOSUPPORT;



	if (sock->type == SOCK_RAW)

		sock->ops = &llcp_rawsock_ops;

	else

		sock->ops = &llcp_sock_ops;



	sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);

CRA Git | Maintained and supported by SUSTech CRA and CCSE