1 files changed, 86 insertions, 4 deletions

diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 6e188d4020ba..40f056debf9a 100644
--- 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)