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>

diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index 2e8b40c16274f73d1ef98d6ee17fa17da172354e..6c583e244de2198d41effbf8a21086296a2c7e40 100644 (file)
--- 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 */

diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h
index 9789dc95b6a8fb775a16e4fff1612101cae879c7..9b19b44619286616b04f1aa9f04d026878730744 100644 (file)
--- 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

diff --git a/net/nfc/llcp_commands.c b/net/nfc/llcp_commands.c
index bec6ed15f5037ef9ffbc75f93e450e9ef1fd81b7..a3ad69a4c648c76779bf496153bfa7eb01f5354c 100644 (file)
--- 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 b4671958fcf935c3c3d984afc18f473bc882eae4..f6278da68763d2545af32d614278ccbe579a3a25 100644 (file)
--- 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 9d6e74f7e6b34ba5ed0d9022a96fdc8061b479c7..88d60064890e3a9ca93db4c88fdcf840e077fabe 100644 (file)
--- 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 c27a6e86cae459f0f70c2c4875614edac3518f98..8627c75063e232d218d813344067347711cb367e 100644 (file)
--- 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,