NFC: Add RAW socket type support for SOCKPROTO_RAW

This allows for a more generic NFC sniffing by using SOCKPROTO_RAW SOCK_RAW to read RAW NFC frames. This is for sniffing anything but LLCP (HCI, NCI, etc...). Signed-off-by: Hiren Tandel <hirent@marvell.com> Signed-off-by: Rahul Tank <rahult@marvell.com> Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>
2014-05-05 19:43:31 +09:00 · 2014-05-05 19:43:31 +09:00 · 57be1f3f3e
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@ -264,4 +264,7 @@ int nfc_add_se(struct nfc_dev *dev, u32 se_idx, u16 type);
 int nfc_remove_se(struct nfc_dev *dev, u32 se_idx);
 struct nfc_se *nfc_find_se(struct nfc_dev *dev, u32 se_idx);

+void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
+			  u8 payload_type, u8 direction);
+
 #endif /* __NET_NFC_H */
--- a/include/uapi/linux/nfc.h
+++ b/include/uapi/linux/nfc.h
@ -273,11 +273,19 @@ struct sockaddr_nfc_llcp {
 * First byte is the adapter index
 * Second byte contains flags
 *  - 0x01 - Direction (0=RX, 1=TX)
- *  - 0x02-0x80 - Reserved
+ *  - 0x02-0x04 - Payload type (000=LLCP, 001=NCI, 010=HCI, 011=Digital,
+ *                              100=Proprietary)
+ *  - 0x05-0x80 - Reserved
 **/
-#define NFC_LLCP_RAW_HEADER_SIZE	2
-#define NFC_LLCP_DIRECTION_RX		0x00
-#define NFC_LLCP_DIRECTION_TX		0x01
+#define NFC_RAW_HEADER_SIZE	2
+#define NFC_DIRECTION_RX		0x00
+#define NFC_DIRECTION_TX		0x01
+
+#define RAW_PAYLOAD_LLCP 0
+#define RAW_PAYLOAD_NCI	1
+#define RAW_PAYLOAD_HCI	2
+#define RAW_PAYLOAD_DIGITAL	3
+#define RAW_PAYLOAD_PROPRIETARY	4

 /* socket option names */
 #define NFC_LLCP_RW		0
--- a/net/nfc/llcp_commands.c
+++ b/net/nfc/llcp_commands.c
@ -387,7 +387,7 @@ int nfc_llcp_send_symm(struct nfc_dev *dev)

 	__net_timestamp(skb);

-	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX);
+	nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_TX);

 	return nfc_data_exchange(dev, local->target_idx, skb,
 				 nfc_llcp_recv, local);
--- a/net/nfc/llcp_core.c
+++ b/net/nfc/llcp_core.c
@ -680,16 +680,17 @@ void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
 			continue;

 		if (skb_copy == NULL) {
-			skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
+			skb_copy = __pskb_copy(skb, NFC_RAW_HEADER_SIZE,
 					       GFP_ATOMIC);

 			if (skb_copy == NULL)
 				continue;

-			data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
+			data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);

 			data[0] = local->dev ? local->dev->idx : 0xFF;
-			data[1] = direction;
+			data[1] = direction & 0x01;
+			data[1] |= (RAW_PAYLOAD_LLCP << 1);
 		}

 		nskb = skb_clone(skb_copy, GFP_ATOMIC);
@ -747,7 +748,7 @@ static void nfc_llcp_tx_work(struct work_struct *work)
 			__net_timestamp(skb);

 			nfc_llcp_send_to_raw_sock(local, skb,
-						  NFC_LLCP_DIRECTION_TX);
+						  NFC_DIRECTION_TX);

 			ret = nfc_data_exchange(local->dev, local->target_idx,
 						skb, nfc_llcp_recv, local);
@ -1476,7 +1477,7 @@ static void nfc_llcp_rx_work(struct work_struct *work)

 	__net_timestamp(skb);

-	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+	nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_RX);

 	nfc_llcp_rx_skb(local, skb);

--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@ -40,6 +40,12 @@ struct nfc_rawsock {
 	struct work_struct tx_work;
 	bool tx_work_scheduled;
 };
+
+struct nfc_sock_list {
+	struct hlist_head head;
+	rwlock_t          lock;
+};
+
 #define nfc_rawsock(sk) ((struct nfc_rawsock *) sk)
 #define to_rawsock_sk(_tx_work) \
 	((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work))
--- a/net/nfc/rawsock.c
+++ b/net/nfc/rawsock.c
@ -27,6 +27,24 @@

 #include "nfc.h"

+static struct nfc_sock_list raw_sk_list = {
+	.lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock)
+};
+
+void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk)
+{
+	write_lock(&l->lock);
+	sk_add_node(sk, &l->head);
+	write_unlock(&l->lock);
+}
+
+void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk)
+{
+	write_lock(&l->lock);
+	sk_del_node_init(sk);
+	write_unlock(&l->lock);
+}
+
 static void rawsock_write_queue_purge(struct sock *sk)
 {
 	pr_debug("sk=%p\n", sk);
@ -57,6 +75,9 @@ static int rawsock_release(struct socket *sock)
 	if (!sk)
 		return 0;

+	if (sock->type == SOCK_RAW)
+		nfc_sock_unlink(&raw_sk_list, sk);
+
 	sock_orphan(sk);
 	sock_put(sk);

@ -275,6 +296,26 @@ static const struct proto_ops rawsock_ops = {
 	.mmap           = sock_no_mmap,
 };

+static const struct proto_ops rawsock_raw_ops = {
+	.family         = PF_NFC,
+	.owner          = THIS_MODULE,
+	.release        = rawsock_release,
+	.bind           = sock_no_bind,
+	.connect        = sock_no_connect,
+	.socketpair     = sock_no_socketpair,
+	.accept         = sock_no_accept,
+	.getname        = sock_no_getname,
+	.poll           = datagram_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        = rawsock_recvmsg,
+	.mmap           = sock_no_mmap,
+};
+
 static void rawsock_destruct(struct sock *sk)
 {
 	pr_debug("sk=%p\n", sk);
@ -300,9 +341,12 @@ static int rawsock_create(struct net *net, struct socket *sock,

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

-	if (sock->type != SOCK_SEQPACKET)
+	if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
 		return -ESOCKTNOSUPPORT;

+	if (sock->type == SOCK_RAW)
+		sock->ops = &rawsock_raw_ops;
+	else
 		sock->ops = &rawsock_ops;

 	sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto);
@ -313,13 +357,53 @@ static int rawsock_create(struct net *net, struct socket *sock,
 	sk->sk_protocol = nfc_proto->id;
 	sk->sk_destruct = rawsock_destruct;
 	sock->state = SS_UNCONNECTED;
-
+	if (sock->type == SOCK_RAW)
+		nfc_sock_link(&raw_sk_list, sk);
+	else {
 		INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
 		nfc_rawsock(sk)->tx_work_scheduled = false;
+	}

 	return 0;
 }

+void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
+			  u8 payload_type, u8 direction)
+{
+	struct sk_buff *skb_copy = NULL, *nskb;
+	struct sock *sk;
+	u8 *data;
+
+	read_lock(&raw_sk_list.lock);
+
+	sk_for_each(sk, &raw_sk_list.head) {
+		if (!skb_copy) {
+			skb_copy = __pskb_copy(skb, NFC_RAW_HEADER_SIZE,
+				     GFP_ATOMIC);
+			if (!skb_copy)
+				continue;
+
+			data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
+
+			data[0] = dev ? dev->idx : 0xFF;
+			data[1] = direction & 0x01;
+			data[1] |= (payload_type << 1);
+		}
+
+		nskb = skb_clone(skb_copy, GFP_ATOMIC);
+		if (!nskb)
+			continue;
+
+		if (sock_queue_rcv_skb(sk, nskb))
+			kfree_skb(nskb);
+	}
+
+	read_unlock(&raw_sk_list.lock);
+
+	kfree_skb(skb_copy);
+}
+EXPORT_SYMBOL(nfc_send_to_raw_sock);
+
 static struct proto rawsock_proto = {
 	.name     = "NFC_RAW",
 	.owner    = THIS_MODULE,