arm: imx: tx6: mfgtool defconfig

[karo-tx-linux.git] / net / nfc / rawsock.c

diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index c27a6e86cae459f0f70c2c4875614edac3518f98..11c3544ea5466f54f8038ca19f500a831d1ca79f 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)
+};
+
+static 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);
+}
+
+static 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_fclone(skb, NFC_RAW_HEADER_SIZE,
+                                                     GFP_ATOMIC, true);
+                       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,