7 files changed, 202 insertions, 25 deletions

diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index d123444404c8..97c440a8cd61 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -716,7 +716,7 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
                                         void *arg, gfp_t flags)
 {
         struct pn533_cmd *cmd;
-        int rc;
+        int rc = 0;
 
         nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
 
@@ -729,16 +729,16 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
                 if (!rc)
                         dev->cmd_pending = 1;
 
-                mutex_unlock(&dev->cmd_lock);
-
-                return rc;
+                goto unlock;
         }
 
         nfc_dev_dbg(&dev->interface->dev, "%s Queueing command", __func__);
 
         cmd = kzalloc(sizeof(struct pn533_cmd), flags);
-        if (!cmd)
-                return -ENOMEM;
+        if (!cmd) {
+                rc = -ENOMEM;
+                goto unlock;
+        }
 
         INIT_LIST_HEAD(&cmd->queue);
         cmd->out_frame = out_frame;
@@ -750,9 +750,10 @@ static int pn533_send_cmd_frame_async(struct pn533 *dev,
 
         list_add_tail(&cmd->queue, &dev->cmd_queue);
 
+unlock:
         mutex_unlock(&dev->cmd_lock);
 
-        return 0;
+        return rc;
 }
 
 struct pn533_sync_cmd_response {
diff --git a/include/linux/nfc.h b/include/linux/nfc.h
index 6189f27e305b..d908d17da56d 100644
--- 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 b982b5b890d7..c45ccd6c094c 100644
--- 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 90ef4a176819..c12c5ef3d036 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -56,7 +56,7 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
         sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
                 llcp_sock = nfc_llcp_sock(sk);
 
-                lock_sock(sk);
+                bh_lock_sock(sk);
 
                 if (sk->sk_state == LLCP_CONNECTED)
                         nfc_put_device(llcp_sock->dev);
@@ -68,26 +68,26 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
                         list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
                                                  accept_queue) {
                                 accept_sk = &lsk->sk;
-                                lock_sock(accept_sk);
+                                bh_lock_sock(accept_sk);
 
                                 nfc_llcp_accept_unlink(accept_sk);
 
                                 accept_sk->sk_state = LLCP_CLOSED;
 
-                                release_sock(accept_sk);
+                                bh_unlock_sock(accept_sk);
 
                                 sock_orphan(accept_sk);
                         }
 
                         if (listen == true) {
-                                release_sock(sk);
+                                bh_unlock_sock(sk);
                                 continue;
                         }
                 }
 
                 sk->sk_state = LLCP_CLOSED;
 
-                release_sock(sk);
+                bh_unlock_sock(sk);
 
                 sock_orphan(sk);
 
@@ -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");
@@ -1156,8 +1201,9 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
 
         INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
 
-        local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
-        local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
+        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 af395c9ceb03..fdb2d24e60bd 100644
--- 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 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)
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 4c51714ee741..baa6af9500df 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -761,31 +761,63 @@ static struct genl_ops nfc_genl_ops[] = {
         },
 };
 
-static int nfc_genl_rcv_nl_event(struct notifier_block *this,
-                                 unsigned long event, void *ptr)
+
+struct urelease_work {
+        struct  work_struct w;
+        int     pid;
+};
+
+static void nfc_urelease_event_work(struct work_struct *work)
 {
-        struct netlink_notify *n = ptr;
+        struct urelease_work *w = container_of(work, struct urelease_work, w);
         struct class_dev_iter iter;
         struct nfc_dev *dev;
 
-        if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC)
-                goto out;
+        pr_debug("pid %d\n", w->pid);
 
-        pr_debug("NETLINK_URELEASE event from id %d\n", n->pid);
+        mutex_lock(&nfc_devlist_mutex);
 
         nfc_device_iter_init(&iter);
         dev = nfc_device_iter_next(&iter);
 
         while (dev) {
-                if (dev->genl_data.poll_req_pid == n->pid) {
+                mutex_lock(&dev->genl_data.genl_data_mutex);
+
+                if (dev->genl_data.poll_req_pid == w->pid) {
                         nfc_stop_poll(dev);
                         dev->genl_data.poll_req_pid = 0;
                 }
+
+                mutex_unlock(&dev->genl_data.genl_data_mutex);
+
                 dev = nfc_device_iter_next(&iter);
         }
 
         nfc_device_iter_exit(&iter);
 
+        mutex_unlock(&nfc_devlist_mutex);
+
+        kfree(w);
+}
+
+static int nfc_genl_rcv_nl_event(struct notifier_block *this,
+                                 unsigned long event, void *ptr)
+{
+        struct netlink_notify *n = ptr;
+        struct urelease_work *w;
+
+        if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC)
+                goto out;
+
+        pr_debug("NETLINK_URELEASE event from id %d\n", n->pid);
+
+        w = kmalloc(sizeof(*w), GFP_ATOMIC);
+        if (w) {
+                INIT_WORK((struct work_struct *) w, nfc_urelease_event_work);
+                w->pid = n->pid;
+                schedule_work((struct work_struct *) w);
+        }
+
 out:
         return NOTIFY_DONE;
 }