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: Thierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

diff --git a/include/linux/nfc.h b/include/linux/nfc.h
index 6189f27e305b551a3d47bbbc8d9935041917e246..d908d17da56d677e62a07011c852cc38cdf824cf 100644 (file)
--- a/include/linux/nfc.h
+++ b/include/linux/nfc.h
@@ -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 */

diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index b982b5b890d73da30a315567851d5d91e7ec9285..c45ccd6c094c5b16f258be4ffd7a1cff4433f6dc 100644 (file)
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -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);
 }

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index d649fbf39d580bcbd2936de0e2c81ff29acb56d0..fc43747f0c3366d7ce0320835e621fdc02e8d442 100644 (file)
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -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);
 

diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index af395c9ceb031b739937ff6dee6c95f2809f3820..fdb2d24e60bda5fe85d8225c38b25a1caed7c0de 100644 (file)
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -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);

diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 6e188d4020ba328540916d402200513750e38638..40f056debf9aaecd1fc7993a186a21230a111856 100644 (file)
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -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,7 +472,10 @@ static int llcp_sock_release(struct socket *sock)
 
        release_sock(sk);
 
-       nfc_llcp_sock_unlink(&local->sockets, sk);
+       if (sock->type == SOCK_RAW)
+               nfc_llcp_sock_unlink(&local->raw_sockets, sk);
+       else
+               nfc_llcp_sock_unlink(&local->sockets, sk);
 
 out:
        sock_orphan(sk);
@@ -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,10 +809,15 @@ 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;
 
-       sock->ops = &llcp_sock_ops;
+       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);
        if (sk == NULL)