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>

4 files changed, 102 insertions, 11 deletions

diff --git a/net/nfc/llcp_commands.c b/net/nfc/llcp_commands.c
index bec6ed15f503..a3ad69a4c648 100644
--- 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);
diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c
index b4671958fcf9..f6278da68763 100644
--- 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);
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index 9d6e74f7e6b3..88d60064890e 100644
--- 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))
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index c27a6e86cae4..8627c75063e2 100644
--- 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,10 +341,13 @@ 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;
 
-        sock->ops = &rawsock_ops;
+        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);
         if (!sk)
@@ -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;
-
-        INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
-        nfc_rawsock(sk)->tx_work_scheduled = false;
+        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,