dirty fix to merging

19 files changed, 6487 insertions, 0 deletions

diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
new file mode 100644
index 00000000..44c865b8
--- /dev/null
+++ b/net/nfc/Kconfig
@@ -0,0 +1,19 @@
+#
+# NFC sybsystem configuration
+#
+
+menuconfig NFC
+	depends on NET && EXPERIMENTAL
+	tristate "NFC subsystem support (EXPERIMENTAL)"
+	default n
+	help
+	  Say Y here if you want to build support for NFC (Near field
+	  communication) devices.
+
+	  To compile this support as a module, choose M here: the module will
+	  be called nfc.
+
+source "net/nfc/nci/Kconfig"
+source "net/nfc/llcp/Kconfig"
+
+source "drivers/nfc/Kconfig"
diff --git a/net/nfc/Makefile b/net/nfc/Makefile
new file mode 100644
index 00000000..7b4a6dcf
--- /dev/null
+++ b/net/nfc/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the Linux NFC subsystem.
+#
+
+obj-$(CONFIG_NFC) += nfc.o
+obj-$(CONFIG_NFC_NCI) += nci/
+
+nfc-objs := core.o netlink.o af_nfc.o rawsock.o
+nfc-$(CONFIG_NFC_LLCP)	+= llcp/llcp.o llcp/commands.o llcp/sock.o
diff --git a/net/nfc/af_nfc.c b/net/nfc/af_nfc.c
new file mode 100644
index 00000000..9d68441e
--- /dev/null
+++ b/net/nfc/af_nfc.c
@@ -0,0 +1,99 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#include <linux/nfc.h>
+#include <linux/module.h>
+
+#include "nfc.h"
+
+static DEFINE_RWLOCK(proto_tab_lock);
+static const struct nfc_protocol *proto_tab[NFC_SOCKPROTO_MAX];
+
+static int nfc_sock_create(struct net *net, struct socket *sock, int proto,
+			   int kern)
+{
+	int rc = -EPROTONOSUPPORT;
+
+	if (net != &init_net)
+		return -EAFNOSUPPORT;
+
+	if (proto < 0 || proto >= NFC_SOCKPROTO_MAX)
+		return -EINVAL;
+
+	read_lock(&proto_tab_lock);
+	if (proto_tab[proto] &&	try_module_get(proto_tab[proto]->owner)) {
+		rc = proto_tab[proto]->create(net, sock, proto_tab[proto]);
+		module_put(proto_tab[proto]->owner);
+	}
+	read_unlock(&proto_tab_lock);
+
+	return rc;
+}
+
+static struct net_proto_family nfc_sock_family_ops = {
+	.owner  = THIS_MODULE,
+	.family = PF_NFC,
+	.create = nfc_sock_create,
+};
+
+int nfc_proto_register(const struct nfc_protocol *nfc_proto)
+{
+	int rc;
+
+	if (nfc_proto->id < 0 || nfc_proto->id >= NFC_SOCKPROTO_MAX)
+		return -EINVAL;
+
+	rc = proto_register(nfc_proto->proto, 0);
+	if (rc)
+		return rc;
+
+	write_lock(&proto_tab_lock);
+	if (proto_tab[nfc_proto->id])
+		rc = -EBUSY;
+	else
+		proto_tab[nfc_proto->id] = nfc_proto;
+	write_unlock(&proto_tab_lock);
+
+	return rc;
+}
+EXPORT_SYMBOL(nfc_proto_register);
+
+void nfc_proto_unregister(const struct nfc_protocol *nfc_proto)
+{
+	write_lock(&proto_tab_lock);
+	proto_tab[nfc_proto->id] = NULL;
+	write_unlock(&proto_tab_lock);
+
+	proto_unregister(nfc_proto->proto);
+}
+EXPORT_SYMBOL(nfc_proto_unregister);
+
+int __init af_nfc_init(void)
+{
+	return sock_register(&nfc_sock_family_ops);
+}
+
+void af_nfc_exit(void)
+{
+	sock_unregister(PF_NFC);
+}
diff --git a/net/nfc/core.c b/net/nfc/core.c
new file mode 100644
index 00000000..295d1298
--- /dev/null
+++ b/net/nfc/core.c
@@ -0,0 +1,659 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/nfc.h>
+
+#include "nfc.h"
+
+#define VERSION "0.1"
+
+int nfc_devlist_generation;
+DEFINE_MUTEX(nfc_devlist_mutex);
+
+/**
+ * nfc_dev_up - turn on the NFC device
+ *
+ * @dev: The nfc device to be turned on
+ *
+ * The device remains up until the nfc_dev_down function is called.
+ */
+int nfc_dev_up(struct nfc_dev *dev)
+{
+	int rc = 0;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (dev->dev_up) {
+		rc = -EALREADY;
+		goto error;
+	}
+
+	if (dev->ops->dev_up)
+		rc = dev->ops->dev_up(dev);
+
+	if (!rc)
+		dev->dev_up = true;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+/**
+ * nfc_dev_down - turn off the NFC device
+ *
+ * @dev: The nfc device to be turned off
+ */
+int nfc_dev_down(struct nfc_dev *dev)
+{
+	int rc = 0;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (!dev->dev_up) {
+		rc = -EALREADY;
+		goto error;
+	}
+
+	if (dev->polling || dev->remote_activated) {
+		rc = -EBUSY;
+		goto error;
+	}
+
+	if (dev->ops->dev_down)
+		dev->ops->dev_down(dev);
+
+	dev->dev_up = false;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+/**
+ * nfc_start_poll - start polling for nfc targets
+ *
+ * @dev: The nfc device that must start polling
+ * @protocols: bitset of nfc protocols that must be used for polling
+ *
+ * The device remains polling for targets until a target is found or
+ * the nfc_stop_poll function is called.
+ */
+int nfc_start_poll(struct nfc_dev *dev, u32 protocols)
+{
+	int rc;
+
+	pr_debug("dev_name=%s protocols=0x%x\n",
+		 dev_name(&dev->dev), protocols);
+
+	if (!protocols)
+		return -EINVAL;
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (dev->polling) {
+		rc = -EBUSY;
+		goto error;
+	}
+
+	rc = dev->ops->start_poll(dev, protocols);
+	if (!rc)
+		dev->polling = true;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+/**
+ * nfc_stop_poll - stop polling for nfc targets
+ *
+ * @dev: The nfc device that must stop polling
+ */
+int nfc_stop_poll(struct nfc_dev *dev)
+{
+	int rc = 0;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (!dev->polling) {
+		rc = -EINVAL;
+		goto error;
+	}
+
+	dev->ops->stop_poll(dev);
+	dev->polling = false;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode)
+{
+	int rc = 0;
+	u8 *gb;
+	size_t gb_len;
+
+	pr_debug("dev_name=%s comm %d\n", dev_name(&dev->dev), comm_mode);
+
+	if (!dev->ops->dep_link_up)
+		return -EOPNOTSUPP;
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (dev->dep_link_up == true) {
+		rc = -EALREADY;
+		goto error;
+	}
+
+	gb = nfc_llcp_general_bytes(dev, &gb_len);
+	if (gb_len > NFC_MAX_GT_LEN) {
+		rc = -EINVAL;
+		goto error;
+	}
+
+	rc = dev->ops->dep_link_up(dev, target_index, comm_mode, gb, gb_len);
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+int nfc_dep_link_down(struct nfc_dev *dev)
+{
+	int rc = 0;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	if (!dev->ops->dep_link_down)
+		return -EOPNOTSUPP;
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (dev->dep_link_up == false) {
+		rc = -EALREADY;
+		goto error;
+	}
+
+	if (dev->dep_rf_mode == NFC_RF_TARGET) {
+		rc = -EOPNOTSUPP;
+		goto error;
+	}
+
+	rc = dev->ops->dep_link_down(dev);
+	if (!rc) {
+		dev->dep_link_up = false;
+		nfc_llcp_mac_is_down(dev);
+		nfc_genl_dep_link_down_event(dev);
+	}
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx,
+		       u8 comm_mode, u8 rf_mode)
+{
+	dev->dep_link_up = true;
+	dev->dep_rf_mode = rf_mode;
+
+	nfc_llcp_mac_is_up(dev, target_idx, comm_mode, rf_mode);
+
+	return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode);
+}
+EXPORT_SYMBOL(nfc_dep_link_is_up);
+
+/**
+ * nfc_activate_target - prepare the target for data exchange
+ *
+ * @dev: The nfc device that found the target
+ * @target_idx: index of the target that must be activated
+ * @protocol: nfc protocol that will be used for data exchange
+ */
+int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol)
+{
+	int rc;
+
+	pr_debug("dev_name=%s target_idx=%u protocol=%u\n",
+		 dev_name(&dev->dev), target_idx, protocol);
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	rc = dev->ops->activate_target(dev, target_idx, protocol);
+	if (!rc)
+		dev->remote_activated = true;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+/**
+ * nfc_deactivate_target - deactivate a nfc target
+ *
+ * @dev: The nfc device that found the target
+ * @target_idx: index of the target that must be deactivated
+ */
+int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx)
+{
+	int rc = 0;
+
+	pr_debug("dev_name=%s target_idx=%u\n",
+		 dev_name(&dev->dev), target_idx);
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	dev->ops->deactivate_target(dev, target_idx);
+	dev->remote_activated = false;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+/**
+ * nfc_data_exchange - transceive data
+ *
+ * @dev: The nfc device that found the target
+ * @target_idx: index of the target
+ * @skb: data to be sent
+ * @cb: callback called when the response is received
+ * @cb_context: parameter for the callback function
+ *
+ * The user must wait for the callback before calling this function again.
+ */
+int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
+		      data_exchange_cb_t cb, void *cb_context)
+{
+	int rc;
+
+	pr_debug("dev_name=%s target_idx=%u skb->len=%u\n",
+		 dev_name(&dev->dev), target_idx, skb->len);
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		kfree_skb(skb);
+		goto error;
+	}
+
+	rc = dev->ops->data_exchange(dev, target_idx, skb, cb, cb_context);
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+int nfc_set_remote_general_bytes(struct nfc_dev *dev, u8 *gb, u8 gb_len)
+{
+	pr_debug("dev_name=%s gb_len=%d\n", dev_name(&dev->dev), gb_len);
+
+	if (gb_len > NFC_MAX_GT_LEN)
+		return -EINVAL;
+
+	return nfc_llcp_set_remote_gb(dev, gb, gb_len);
+}
+EXPORT_SYMBOL(nfc_set_remote_general_bytes);
+
+/**
+ * nfc_alloc_send_skb - allocate a skb for data exchange responses
+ *
+ * @size: size to allocate
+ * @gfp: gfp flags
+ */
+struct sk_buff *nfc_alloc_send_skb(struct nfc_dev *dev, struct sock *sk,
+				   unsigned int flags, unsigned int size,
+				   unsigned int *err)
+{
+	struct sk_buff *skb;
+	unsigned int total_size;
+
+	total_size = size +
+		dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+	skb = sock_alloc_send_skb(sk, total_size, flags & MSG_DONTWAIT, err);
+	if (skb)
+		skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	return skb;
+}
+
+/**
+ * nfc_alloc_recv_skb - allocate a skb for data exchange responses
+ *
+ * @size: size to allocate
+ * @gfp: gfp flags
+ */
+struct sk_buff *nfc_alloc_recv_skb(unsigned int size, gfp_t gfp)
+{
+	struct sk_buff *skb;
+	unsigned int total_size;
+
+	total_size = size + 1;
+	skb = alloc_skb(total_size, gfp);
+
+	if (skb)
+		skb_reserve(skb, 1);
+
+	return skb;
+}
+EXPORT_SYMBOL(nfc_alloc_recv_skb);
+
+/**
+ * nfc_targets_found - inform that targets were found
+ *
+ * @dev: The nfc device that found the targets
+ * @targets: array of nfc targets found
+ * @ntargets: targets array size
+ *
+ * The device driver must call this function when one or many nfc targets
+ * are found. After calling this function, the device driver must stop
+ * polling for targets.
+ */
+int nfc_targets_found(struct nfc_dev *dev,
+		      struct nfc_target *targets, int n_targets)
+{
+	pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets);
+
+	dev->polling = false;
+
+	spin_lock_bh(&dev->targets_lock);
+
+	dev->targets_generation++;
+
+	kfree(dev->targets);
+	dev->targets = kmemdup(targets, n_targets * sizeof(struct nfc_target),
+			       GFP_ATOMIC);
+
+	if (!dev->targets) {
+		dev->n_targets = 0;
+		spin_unlock_bh(&dev->targets_lock);
+		return -ENOMEM;
+	}
+
+	dev->n_targets = n_targets;
+	spin_unlock_bh(&dev->targets_lock);
+
+	nfc_genl_targets_found(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_targets_found);
+
+static void nfc_release(struct device *d)
+{
+	struct nfc_dev *dev = to_nfc_dev(d);
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	nfc_genl_data_exit(&dev->genl_data);
+	kfree(dev->targets);
+	kfree(dev);
+}
+
+struct class nfc_class = {
+	.name = "nfc",
+	.dev_release = nfc_release,
+};
+EXPORT_SYMBOL(nfc_class);
+
+static int match_idx(struct device *d, void *data)
+{
+	struct nfc_dev *dev = to_nfc_dev(d);
+	unsigned *idx = data;
+
+	return dev->idx == *idx;
+}
+
+struct nfc_dev *nfc_get_device(unsigned idx)
+{
+	struct device *d;
+
+	d = class_find_device(&nfc_class, NULL, &idx, match_idx);
+	if (!d)
+		return NULL;
+
+	return to_nfc_dev(d);
+}
+
+/**
+ * nfc_allocate_device - allocate a new nfc device
+ *
+ * @ops: device operations
+ * @supported_protocols: NFC protocols supported by the device
+ */
+struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
+				    u32 supported_protocols,
+				    int tx_headroom, int tx_tailroom)
+{
+	static atomic_t dev_no = ATOMIC_INIT(0);
+	struct nfc_dev *dev;
+
+	if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
+	    !ops->deactivate_target || !ops->data_exchange)
+		return NULL;
+
+	if (!supported_protocols)
+		return NULL;
+
+	dev = kzalloc(sizeof(struct nfc_dev), GFP_KERNEL);
+	if (!dev)
+		return NULL;
+
+	dev->dev.class = &nfc_class;
+	dev->idx = atomic_inc_return(&dev_no) - 1;
+	dev_set_name(&dev->dev, "nfc%d", dev->idx);
+	device_initialize(&dev->dev);
+
+	dev->ops = ops;
+	dev->supported_protocols = supported_protocols;
+	dev->tx_headroom = tx_headroom;
+	dev->tx_tailroom = tx_tailroom;
+
+	spin_lock_init(&dev->targets_lock);
+	nfc_genl_data_init(&dev->genl_data);
+
+	/* first generation must not be 0 */
+	dev->targets_generation = 1;
+
+	return dev;
+}
+EXPORT_SYMBOL(nfc_allocate_device);
+
+/**
+ * nfc_register_device - register a nfc device in the nfc subsystem
+ *
+ * @dev: The nfc device to register
+ */
+int nfc_register_device(struct nfc_dev *dev)
+{
+	int rc;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	mutex_lock(&nfc_devlist_mutex);
+	nfc_devlist_generation++;
+	rc = device_add(&dev->dev);
+	mutex_unlock(&nfc_devlist_mutex);
+
+	if (rc < 0)
+		return rc;
+
+	rc = nfc_llcp_register_device(dev);
+	if (rc)
+		pr_err("Could not register llcp device\n");
+
+	rc = nfc_genl_device_added(dev);
+	if (rc)
+		pr_debug("The userspace won't be notified that the device %s was added\n",
+			 dev_name(&dev->dev));
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_register_device);
+
+/**
+ * nfc_unregister_device - unregister a nfc device in the nfc subsystem
+ *
+ * @dev: The nfc device to unregister
+ */
+void nfc_unregister_device(struct nfc_dev *dev)
+{
+	int rc;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	mutex_lock(&nfc_devlist_mutex);
+	nfc_devlist_generation++;
+
+	/* lock to avoid unregistering a device while an operation
+	   is in progress */
+	device_lock(&dev->dev);
+	device_del(&dev->dev);
+	device_unlock(&dev->dev);
+
+	mutex_unlock(&nfc_devlist_mutex);
+
+	nfc_llcp_unregister_device(dev);
+
+	rc = nfc_genl_device_removed(dev);
+	if (rc)
+		pr_debug("The userspace won't be notified that the device %s was removed\n",
+			 dev_name(&dev->dev));
+
+}
+EXPORT_SYMBOL(nfc_unregister_device);
+
+static int __init nfc_init(void)
+{
+	int rc;
+
+	pr_info("NFC Core ver %s\n", VERSION);
+
+	rc = class_register(&nfc_class);
+	if (rc)
+		return rc;
+
+	rc = nfc_genl_init();
+	if (rc)
+		goto err_genl;
+
+	/* the first generation must not be 0 */
+	nfc_devlist_generation = 1;
+
+	rc = rawsock_init();
+	if (rc)
+		goto err_rawsock;
+
+	rc = nfc_llcp_init();
+	if (rc)
+		goto err_llcp_sock;
+
+	rc = af_nfc_init();
+	if (rc)
+		goto err_af_nfc;
+
+	return 0;
+
+err_af_nfc:
+	nfc_llcp_exit();
+err_llcp_sock:
+	rawsock_exit();
+err_rawsock:
+	nfc_genl_exit();
+err_genl:
+	class_unregister(&nfc_class);
+	return rc;
+}
+
+static void __exit nfc_exit(void)
+{
+	af_nfc_exit();
+	nfc_llcp_exit();
+	rawsock_exit();
+	nfc_genl_exit();
+	class_unregister(&nfc_class);
+}
+
+subsys_initcall(nfc_init);
+module_exit(nfc_exit);
+
+MODULE_AUTHOR("Lauro Ramos Venancio <lauro.venancio@openbossa.org>");
+MODULE_DESCRIPTION("NFC Core ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig
new file mode 100644
index 00000000..fbf5e815
--- /dev/null
+++ b/net/nfc/llcp/Kconfig
@@ -0,0 +1,7 @@
+config NFC_LLCP
+       depends on NFC && EXPERIMENTAL
+       bool "NFC LLCP support (EXPERIMENTAL)"
+       default n
+       help
+	 Say Y here if you want to build support for a kernel NFC LLCP
+	 implementation.
\ No newline at end of file
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
new file mode 100644
index 00000000..ef10ffcb
--- /dev/null
+++ b/net/nfc/llcp/commands.c
@@ -0,0 +1,530 @@
+/*
+ * Copyright (C) 2011  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include <net/nfc/nfc.h>
+
+#include "../nfc.h"
+#include "llcp.h"
+
+static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
+	0,
+	1, /* VERSION */
+	2, /* MIUX */
+	2, /* WKS */
+	1, /* LTO */
+	1, /* RW */
+	0, /* SN */
+	1, /* OPT */
+	0, /* SDREQ */
+	2, /* SDRES */
+
+};
+
+static u8 llcp_tlv8(u8 *tlv, u8 type)
+{
+	if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+		return 0;
+
+	return tlv[2];
+}
+
+static u8 llcp_tlv16(u8 *tlv, u8 type)
+{
+	if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+		return 0;
+
+	return be16_to_cpu(*((__be16 *)(tlv + 2)));
+}
+
+
+static u8 llcp_tlv_version(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_VERSION);
+}
+
+static u16 llcp_tlv_miux(u8 *tlv)
+{
+	return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7f;
+}
+
+static u16 llcp_tlv_wks(u8 *tlv)
+{
+	return llcp_tlv16(tlv, LLCP_TLV_WKS);
+}
+
+static u16 llcp_tlv_lto(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_LTO);
+}
+
+static u8 llcp_tlv_opt(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_OPT);
+}
+
+static u8 llcp_tlv_rw(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
+}
+
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
+{
+	u8 *tlv, length;
+
+	pr_debug("type %d\n", type);
+
+	if (type >= LLCP_TLV_MAX)
+		return NULL;
+
+	length = llcp_tlv_length[type];
+	if (length == 0 && value_length == 0)
+		return NULL;
+	else
+		length = value_length;
+
+	*tlv_length = 2 + length;
+	tlv = kzalloc(2 + length, GFP_KERNEL);
+	if (tlv == NULL)
+		return tlv;
+
+	tlv[0] = type;
+	tlv[1] = length;
+	memcpy(tlv + 2, value, length);
+
+	return tlv;
+}
+
+int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
+		       u8 *tlv_array, u16 tlv_array_len)
+{
+	u8 *tlv = tlv_array, type, length, offset = 0;
+
+	pr_debug("TLV array length %d\n", tlv_array_len);
+
+	if (local == NULL)
+		return -ENODEV;
+
+	while (offset < tlv_array_len) {
+		type = tlv[0];
+		length = tlv[1];
+
+		pr_debug("type 0x%x length %d\n", type, length);
+
+		switch (type) {
+		case LLCP_TLV_VERSION:
+			local->remote_version = llcp_tlv_version(tlv);
+			break;
+		case LLCP_TLV_MIUX:
+			local->remote_miu = llcp_tlv_miux(tlv) + 128;
+			break;
+		case LLCP_TLV_WKS:
+			local->remote_wks = llcp_tlv_wks(tlv);
+			break;
+		case LLCP_TLV_LTO:
+			local->remote_lto = llcp_tlv_lto(tlv) * 10;
+			break;
+		case LLCP_TLV_OPT:
+			local->remote_opt = llcp_tlv_opt(tlv);
+			break;
+		case LLCP_TLV_RW:
+			local->remote_rw = llcp_tlv_rw(tlv);
+			break;
+		case LLCP_TLV_SN:
+			break;
+		default:
+			pr_err("Invalid gt tlv value 0x%x\n", type);
+			break;
+		}
+
+		offset += length + 2;
+		tlv += length + 2;
+	}
+
+	pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x rw %d\n",
+		 local->remote_version, local->remote_miu,
+		 local->remote_lto, local->remote_opt,
+		 local->remote_wks, local->remote_rw);
+
+	return 0;
+}
+
+static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
+				       u8 dsap, u8 ssap, u8 ptype)
+{
+	u8 header[2];
+
+	pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+	header[0] = (u8)((dsap << 2) | (ptype >> 2));
+	header[1] = (u8)((ptype << 6) | ssap);
+
+	pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
+
+	memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
+
+	return pdu;
+}
+
+static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
+				    u8 tlv_length)
+{
+	/* XXX Add an skb length check */
+
+	if (tlv == NULL)
+		return NULL;
+
+	memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
+
+	return pdu;
+}
+
+static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
+					 u8 cmd, u16 size)
+{
+	struct sk_buff *skb;
+	int err;
+
+	if (sock->ssap == 0)
+		return NULL;
+
+	skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+				 size + LLCP_HEADER_SIZE, &err);
+	if (skb == NULL) {
+		pr_err("Could not allocate PDU\n");
+		return NULL;
+	}
+
+	skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
+
+	return skb;
+}
+
+int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	u16 size = 0;
+
+	pr_debug("Sending DISC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	dev = sock->dev;
+	if (dev == NULL)
+		return -ENODEV;
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+	skb = alloc_skb(size, GFP_ATOMIC);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, sock->ssap, sock->dsap, LLCP_PDU_DISC);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_symm(struct nfc_dev *dev)
+{
+	struct sk_buff *skb;
+	struct nfc_llcp_local *local;
+	u16 size = 0;
+
+	pr_debug("Sending SYMM\n");
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL)
+		return -ENODEV;
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
+
+	return nfc_data_exchange(dev, local->target_idx, skb,
+				 nfc_llcp_recv, local);
+}
+
+int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local;
+	struct sk_buff *skb;
+	u8 *service_name_tlv = NULL, service_name_tlv_length;
+	u8 *miux_tlv = NULL, miux_tlv_length;
+	u8 *rw_tlv = NULL, rw_tlv_length, rw;
+	__be16 miux;
+	int err;
+	u16 size = 0;
+
+	pr_debug("Sending CONNECT\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	if (sock->service_name != NULL) {
+		service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
+						      sock->service_name,
+						      sock->service_name_len,
+						      &service_name_tlv_length);
+		size += service_name_tlv_length;
+	}
+
+	miux = cpu_to_be16(LLCP_MAX_MIUX);
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+				      &miux_tlv_length);
+	size += miux_tlv_length;
+
+	rw = LLCP_MAX_RW;
+	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+	size += rw_tlv_length;
+
+	pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
+	if (skb == NULL) {
+		err = -ENOMEM;
+		goto error_tlv;
+	}
+
+	if (service_name_tlv != NULL)
+		skb = llcp_add_tlv(skb, service_name_tlv,
+				   service_name_tlv_length);
+
+	skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+	skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+
+error_tlv:
+	pr_err("error %d\n", err);
+
+	kfree(service_name_tlv);
+	kfree(miux_tlv);
+	kfree(rw_tlv);
+
+	return err;
+}
+
+int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local;
+	struct sk_buff *skb;
+	u8 *miux_tlv = NULL, miux_tlv_length;
+	u8 *rw_tlv = NULL, rw_tlv_length, rw;
+	__be16 miux;
+	int err;
+	u16 size = 0;
+
+	pr_debug("Sending CC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	miux = cpu_to_be16(LLCP_MAX_MIUX);
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+				      &miux_tlv_length);
+	size += miux_tlv_length;
+
+	rw = LLCP_MAX_RW;
+	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+	size += rw_tlv_length;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
+	if (skb == NULL) {
+		err = -ENOMEM;
+		goto error_tlv;
+	}
+
+	skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+	skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+
+error_tlv:
+	pr_err("error %d\n", err);
+
+	kfree(miux_tlv);
+	kfree(rw_tlv);
+
+	return err;
+}
+
+int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	u16 size = 1; /* Reason code */
+
+	pr_debug("Sending DM reason 0x%x\n", reason);
+
+	if (local == NULL)
+		return -ENODEV;
+
+	dev = local->dev;
+	if (dev == NULL)
+		return -ENODEV;
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, ssap, dsap, LLCP_PDU_DM);
+
+	memcpy(skb_put(skb, 1), &reason, 1);
+
+	skb_queue_head(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
+{
+	struct sk_buff *skb;
+	struct nfc_llcp_local *local;
+
+	pr_debug("Send DISC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_queue_head(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
+			  struct msghdr *msg, size_t len)
+{
+	struct sk_buff *pdu;
+	struct sock *sk = &sock->sk;
+	struct nfc_llcp_local *local;
+	size_t frag_len = 0, remaining_len;
+	u8 *msg_data, *msg_ptr;
+
+	pr_debug("Send I frame len %zd\n", len);
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	msg_data = kzalloc(len, GFP_KERNEL);
+	if (msg_data == NULL)
+		return -ENOMEM;
+
+	if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+		kfree(msg_data);
+		return -EFAULT;
+	}
+
+	remaining_len = len;
+	msg_ptr = msg_data;
+
+	while (remaining_len > 0) {
+
+		frag_len = min_t(size_t, local->remote_miu, remaining_len);
+
+		pr_debug("Fragment %zd bytes remaining %zd",
+			 frag_len, remaining_len);
+
+		pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
+					frag_len + LLCP_SEQUENCE_SIZE);
+		if (pdu == NULL)
+			return -ENOMEM;
+
+		skb_put(pdu, LLCP_SEQUENCE_SIZE);
+
+		memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		skb_queue_head(&sock->tx_queue, pdu);
+
+		lock_sock(sk);
+
+		nfc_llcp_queue_i_frames(sock);
+
+		release_sock(sk);
+
+		remaining_len -= frag_len;
+		msg_ptr += frag_len;
+	}
+
+	kfree(msg_data);
+
+	return 0;
+}
+
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
+{
+	struct sk_buff *skb;
+	struct nfc_llcp_local *local;
+
+	pr_debug("Send rr nr %d\n", sock->recv_n);
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_put(skb, LLCP_SEQUENCE_SIZE);
+
+	skb->data[2] = sock->recv_n % 16;
+
+	skb_queue_head(&local->tx_queue, skb);
+
+	return 0;
+}
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
new file mode 100644
index 00000000..17a578f6
--- /dev/null
+++ b/net/nfc/llcp/llcp.c
@@ -0,0 +1,1014 @@
+/*
+ * Copyright (C) 2011  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/nfc.h>
+
+#include "../nfc.h"
+#include "llcp.h"
+
+static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
+
+static struct list_head llcp_devices;
+
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local)
+{
+	struct nfc_llcp_sock *parent, *s, *n;
+	struct sock *sk, *parent_sk;
+	int i;
+
+	mutex_lock(&local->socket_lock);
+
+	for (i = 0; i < LLCP_MAX_SAP; i++) {
+		parent = local->sockets[i];
+		if (parent == NULL)
+			continue;
+
+		/* Release all child sockets */
+		list_for_each_entry_safe(s, n, &parent->list, list) {
+			list_del_init(&s->list);
+			sk = &s->sk;
+
+			lock_sock(sk);
+
+			if (sk->sk_state == LLCP_CONNECTED)
+				nfc_put_device(s->dev);
+
+			sk->sk_state = LLCP_CLOSED;
+
+			release_sock(sk);
+
+			sock_orphan(sk);
+
+			s->local = NULL;
+		}
+
+		parent_sk = &parent->sk;
+
+		lock_sock(parent_sk);
+
+		if (parent_sk->sk_state == LLCP_LISTEN) {
+			struct nfc_llcp_sock *lsk, *n;
+			struct sock *accept_sk;
+
+			list_for_each_entry_safe(lsk, n, &parent->accept_queue,
+						 accept_queue) {
+				accept_sk = &lsk->sk;
+				lock_sock(accept_sk);
+
+				nfc_llcp_accept_unlink(accept_sk);
+
+				accept_sk->sk_state = LLCP_CLOSED;
+
+				release_sock(accept_sk);
+
+				sock_orphan(accept_sk);
+
+				lsk->local = NULL;
+			}
+		}
+
+		if (parent_sk->sk_state == LLCP_CONNECTED)
+			nfc_put_device(parent->dev);
+
+		parent_sk->sk_state = LLCP_CLOSED;
+
+		release_sock(parent_sk);
+
+		sock_orphan(parent_sk);
+
+		parent->local = NULL;
+	}
+
+	mutex_unlock(&local->socket_lock);
+}
+
+static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local)
+{
+	mutex_lock(&local->sdp_lock);
+
+	local->local_wks = 0;
+	local->local_sdp = 0;
+	local->local_sap = 0;
+
+	mutex_unlock(&local->sdp_lock);
+}
+
+static void nfc_llcp_timeout_work(struct work_struct *work)
+{
+	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+						    timeout_work);
+
+	nfc_dep_link_down(local->dev);
+}
+
+static void nfc_llcp_symm_timer(unsigned long data)
+{
+	struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+	pr_err("SYMM timeout\n");
+
+	queue_work(local->timeout_wq, &local->timeout_work);
+}
+
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
+{
+	struct nfc_llcp_local *local, *n;
+
+	list_for_each_entry_safe(local, n, &llcp_devices, list)
+		if (local->dev == dev)
+			return local;
+
+	pr_debug("No device found\n");
+
+	return NULL;
+}
+
+static char *wks[] = {
+	NULL,
+	NULL, /* SDP */
+	"urn:nfc:sn:ip",
+	"urn:nfc:sn:obex",
+	"urn:nfc:sn:snep",
+};
+
+static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
+{
+	int sap, num_wks;
+
+	pr_debug("%s\n", service_name);
+
+	if (service_name == NULL)
+		return -EINVAL;
+
+	num_wks = ARRAY_SIZE(wks);
+
+	for (sap = 0; sap < num_wks; sap++) {
+		if (wks[sap] == NULL)
+			continue;
+
+		if (strncmp(wks[sap], service_name, service_name_len) == 0)
+			return sap;
+	}
+
+	return -EINVAL;
+}
+
+u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
+			 struct nfc_llcp_sock *sock)
+{
+	mutex_lock(&local->sdp_lock);
+
+	if (sock->service_name != NULL && sock->service_name_len > 0) {
+		int ssap = nfc_llcp_wks_sap(sock->service_name,
+					    sock->service_name_len);
+
+		if (ssap > 0) {
+			pr_debug("WKS %d\n", ssap);
+
+			/* This is a WKS, let's check if it's free */
+			if (local->local_wks & BIT(ssap)) {
+				mutex_unlock(&local->sdp_lock);
+
+				return LLCP_SAP_MAX;
+			}
+
+			set_bit(ssap, &local->local_wks);
+			mutex_unlock(&local->sdp_lock);
+
+			return ssap;
+		}
+
+		/*
+		 * This is not a well known service,
+		 * we should try to find a local SDP free spot
+		 */
+		ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
+		if (ssap == LLCP_SDP_NUM_SAP) {
+			mutex_unlock(&local->sdp_lock);
+
+			return LLCP_SAP_MAX;
+		}
+
+		pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
+
+		set_bit(ssap, &local->local_sdp);
+		mutex_unlock(&local->sdp_lock);
+
+		return LLCP_WKS_NUM_SAP + ssap;
+
+	} else if (sock->ssap != 0) {
+		if (sock->ssap < LLCP_WKS_NUM_SAP) {
+			if (!test_bit(sock->ssap, &local->local_wks)) {
+				set_bit(sock->ssap, &local->local_wks);
+				mutex_unlock(&local->sdp_lock);
+
+				return sock->ssap;
+			}
+
+		} else if (sock->ssap < LLCP_SDP_NUM_SAP) {
+			if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP,
+				      &local->local_sdp)) {
+				set_bit(sock->ssap - LLCP_WKS_NUM_SAP,
+					&local->local_sdp);
+				mutex_unlock(&local->sdp_lock);
+
+				return sock->ssap;
+			}
+		}
+	}
+
+	mutex_unlock(&local->sdp_lock);
+
+	return LLCP_SAP_MAX;
+}
+
+u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
+{
+	u8 local_ssap;
+
+	mutex_lock(&local->sdp_lock);
+
+	local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
+	if (local_ssap == LLCP_LOCAL_NUM_SAP) {
+		mutex_unlock(&local->sdp_lock);
+		return LLCP_SAP_MAX;
+	}
+
+	set_bit(local_ssap, &local->local_sap);
+
+	mutex_unlock(&local->sdp_lock);
+
+	return local_ssap + LLCP_LOCAL_SAP_OFFSET;
+}
+
+void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
+{
+	u8 local_ssap;
+	unsigned long *sdp;
+
+	if (ssap < LLCP_WKS_NUM_SAP) {
+		local_ssap = ssap;
+		sdp = &local->local_wks;
+	} else if (ssap < LLCP_LOCAL_NUM_SAP) {
+		local_ssap = ssap - LLCP_WKS_NUM_SAP;
+		sdp = &local->local_sdp;
+	} else if (ssap < LLCP_MAX_SAP) {
+		local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
+		sdp = &local->local_sap;
+	} else {
+		return;
+	}
+
+	mutex_lock(&local->sdp_lock);
+
+	clear_bit(local_ssap, sdp);
+
+	mutex_unlock(&local->sdp_lock);
+}
+
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
+{
+	struct nfc_llcp_local *local;
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL) {
+		*general_bytes_len = 0;
+		return NULL;
+	}
+
+	*general_bytes_len = local->gb_len;
+
+	return local->gb;
+}
+
+static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
+{
+	u8 *gb_cur, *version_tlv, version, version_length;
+	u8 *lto_tlv, lto, lto_length;
+	u8 *wks_tlv, wks_length;
+	u8 gb_len = 0;
+
+	version = LLCP_VERSION_11;
+	version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
+					 1, &version_length);
+	gb_len += version_length;
+
+	/* 1500 ms */
+	lto = 150;
+	lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &lto, 1, &lto_length);
+	gb_len += lto_length;
+
+	pr_debug("Local wks 0x%lx\n", local->local_wks);
+	wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
+				     &wks_length);
+	gb_len += wks_length;
+
+	gb_len += ARRAY_SIZE(llcp_magic);
+
+	if (gb_len > NFC_MAX_GT_LEN) {
+		kfree(version_tlv);
+		return -EINVAL;
+	}
+
+	gb_cur = local->gb;
+
+	memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
+	gb_cur += ARRAY_SIZE(llcp_magic);
+
+	memcpy(gb_cur, version_tlv, version_length);
+	gb_cur += version_length;
+
+	memcpy(gb_cur, lto_tlv, lto_length);
+	gb_cur += lto_length;
+
+	memcpy(gb_cur, wks_tlv, wks_length);
+	gb_cur += wks_length;
+
+	kfree(version_tlv);
+	kfree(lto_tlv);
+
+	local->gb_len = gb_len;
+
+	return 0;
+}
+
+int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
+{
+	struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+	if (local == NULL) {
+		pr_err("No LLCP device\n");
+		return -ENODEV;
+	}
+
+	memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
+	memcpy(local->remote_gb, gb, gb_len);
+	local->remote_gb_len = gb_len;
+
+	if (local->remote_gb == NULL || local->remote_gb_len == 0)
+		return -ENODEV;
+
+	if (memcmp(local->remote_gb, llcp_magic, 3)) {
+		pr_err("MAC does not support LLCP\n");
+		return -EINVAL;
+	}
+
+	return nfc_llcp_parse_tlv(local,
+				  &local->remote_gb[3],
+				  local->remote_gb_len - 3);
+}
+
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+						    tx_work);
+	struct sk_buff *skb;
+
+	skb = skb_dequeue(&local->tx_queue);
+	if (skb != NULL) {
+		pr_debug("Sending pending skb\n");
+		nfc_data_exchange(local->dev, local->target_idx,
+				  skb, nfc_llcp_recv, local);
+	} else {
+		nfc_llcp_send_symm(local->dev);
+	}
+
+	mod_timer(&local->link_timer,
+		  jiffies + msecs_to_jiffies(local->remote_lto));
+}
+
+static u8 nfc_llcp_dsap(struct sk_buff *pdu)
+{
+	return (pdu->data[0] & 0xfc) >> 2;
+}
+
+static u8 nfc_llcp_ptype(struct sk_buff *pdu)
+{
+	return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
+}
+
+static u8 nfc_llcp_ssap(struct sk_buff *pdu)
+{
+	return pdu->data[1] & 0x3f;
+}
+
+static u8 nfc_llcp_ns(struct sk_buff *pdu)
+{
+	return pdu->data[2] >> 4;
+}
+
+static u8 nfc_llcp_nr(struct sk_buff *pdu)
+{
+	return pdu->data[2] & 0xf;
+}
+
+static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
+{
+	pdu->data[2] = (sock->send_n << 4) | (sock->recv_n % 16);
+	sock->send_n = (sock->send_n + 1) % 16;
+	sock->recv_ack_n = (sock->recv_n - 1) % 16;
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
+					       u8 ssap, u8 dsap)
+{
+	struct nfc_llcp_sock *sock, *llcp_sock, *n;
+
+	if (ssap == 0 && dsap == 0)
+		return NULL;
+
+	mutex_lock(&local->socket_lock);
+	sock = local->sockets[ssap];
+	if (sock == NULL) {
+		mutex_unlock(&local->socket_lock);
+		return NULL;
+	}
+
+	pr_debug("root dsap %d (%d)\n", sock->dsap, dsap);
+
+	if (sock->dsap == dsap) {
+		sock_hold(&sock->sk);
+		mutex_unlock(&local->socket_lock);
+		return sock;
+	}
+
+	list_for_each_entry_safe(llcp_sock, n, &sock->list, list) {
+		pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock,
+			 &llcp_sock->sk, llcp_sock->dsap);
+		if (llcp_sock->dsap == dsap) {
+			sock_hold(&llcp_sock->sk);
+			mutex_unlock(&local->socket_lock);
+			return llcp_sock;
+		}
+	}
+
+	pr_err("Could not find socket for %d %d\n", ssap, dsap);
+
+	mutex_unlock(&local->socket_lock);
+
+	return NULL;
+}
+
+static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+{
+	sock_put(&sock->sk);
+}
+
+static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
+{
+	u8 *tlv = &skb->data[2], type, length;
+	size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
+
+	while (offset < tlv_array_len) {
+		type = tlv[0];
+		length = tlv[1];
+
+		pr_debug("type 0x%x length %d\n", type, length);
+
+		if (type == LLCP_TLV_SN) {
+			*sn_len = length;
+			return &tlv[2];
+		}
+
+		offset += length + 2;
+		tlv += length + 2;
+	}
+
+	return NULL;
+}
+
+static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
+				  struct sk_buff *skb)
+{
+	struct sock *new_sk, *parent;
+	struct nfc_llcp_sock *sock, *new_sock;
+	u8 dsap, ssap, bound_sap, reason;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	pr_debug("%d %d\n", dsap, ssap);
+
+	nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
+			   skb->len - LLCP_HEADER_SIZE);
+
+	if (dsap != LLCP_SAP_SDP) {
+		bound_sap = dsap;
+
+		mutex_lock(&local->socket_lock);
+		sock = local->sockets[dsap];
+		if (sock == NULL) {
+			mutex_unlock(&local->socket_lock);
+			reason = LLCP_DM_NOBOUND;
+			goto fail;
+		}
+
+		sock_hold(&sock->sk);
+		mutex_unlock(&local->socket_lock);
+
+		lock_sock(&sock->sk);
+
+		if (sock->dsap == LLCP_SAP_SDP &&
+		    sock->sk.sk_state == LLCP_LISTEN)
+			goto enqueue;
+	} else {
+		u8 *sn;
+		size_t sn_len;
+
+		sn = nfc_llcp_connect_sn(skb, &sn_len);
+		if (sn == NULL) {
+			reason = LLCP_DM_NOBOUND;
+			goto fail;
+		}
+
+		pr_debug("Service name length %zu\n", sn_len);
+
+		mutex_lock(&local->socket_lock);
+		for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET;
+		     bound_sap++) {
+			sock = local->sockets[bound_sap];
+			if (sock == NULL)
+				continue;
+
+			if (sock->service_name == NULL ||
+			    sock->service_name_len == 0)
+					continue;
+
+			if (sock->service_name_len != sn_len)
+				continue;
+
+			if (sock->dsap == LLCP_SAP_SDP &&
+			    sock->sk.sk_state == LLCP_LISTEN &&
+			    !memcmp(sn, sock->service_name, sn_len)) {
+				pr_debug("Found service name at SAP %d\n",
+					 bound_sap);
+				sock_hold(&sock->sk);
+				mutex_unlock(&local->socket_lock);
+
+				lock_sock(&sock->sk);
+
+				goto enqueue;
+			}
+		}
+		mutex_unlock(&local->socket_lock);
+	}
+
+	reason = LLCP_DM_NOBOUND;
+	goto fail;
+
+enqueue:
+	parent = &sock->sk;
+
+	if (sk_acceptq_is_full(parent)) {
+		reason = LLCP_DM_REJ;
+		release_sock(&sock->sk);
+		sock_put(&sock->sk);
+		goto fail;
+	}
+
+	new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
+	if (new_sk == NULL) {
+		reason = LLCP_DM_REJ;
+		release_sock(&sock->sk);
+		sock_put(&sock->sk);
+		goto fail;
+	}
+
+	new_sock = nfc_llcp_sock(new_sk);
+	new_sock->dev = local->dev;
+	new_sock->local = local;
+	new_sock->nfc_protocol = sock->nfc_protocol;
+	new_sock->ssap = bound_sap;
+	new_sock->dsap = ssap;
+	new_sock->parent = parent;
+
+	pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
+
+	list_add_tail(&new_sock->list, &sock->list);
+
+	nfc_llcp_accept_enqueue(&sock->sk, new_sk);
+
+	nfc_get_device(local->dev->idx);
+
+	new_sk->sk_state = LLCP_CONNECTED;
+
+	/* Wake the listening processes */
+	parent->sk_data_ready(parent, 0);
+
+	/* Send CC */
+	nfc_llcp_send_cc(new_sock);
+
+	release_sock(&sock->sk);
+	sock_put(&sock->sk);
+
+	return;
+
+fail:
+	/* Send DM */
+	nfc_llcp_send_dm(local, dsap, ssap, reason);
+
+	return;
+
+}
+
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
+{
+	int nr_frames = 0;
+	struct nfc_llcp_local *local = sock->local;
+
+	pr_debug("Remote ready %d tx queue len %d remote rw %d",
+		 sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
+		 local->remote_rw);
+
+	/* Try to queue some I frames for transmission */
+	while (sock->remote_ready &&
+	       skb_queue_len(&sock->tx_pending_queue) < local->remote_rw) {
+		struct sk_buff *pdu, *pending_pdu;
+
+		pdu = skb_dequeue(&sock->tx_queue);
+		if (pdu == NULL)
+			break;
+
+		/* Update N(S)/N(R) */
+		nfc_llcp_set_nrns(sock, pdu);
+
+		pending_pdu = skb_clone(pdu, GFP_KERNEL);
+
+		skb_queue_tail(&local->tx_queue, pdu);
+		skb_queue_tail(&sock->tx_pending_queue, pending_pdu);
+		nr_frames++;
+	}
+
+	return nr_frames;
+}
+
+static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
+			       struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	struct sock *sk;
+	u8 dsap, ssap, ptype, ns, nr;
+
+	ptype = nfc_llcp_ptype(skb);
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+	ns = nfc_llcp_ns(skb);
+	nr = nfc_llcp_nr(skb);
+
+	pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
+
+	llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+	if (llcp_sock == NULL) {
+		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+		return;
+	}
+
+	sk = &llcp_sock->sk;
+	lock_sock(sk);
+	if (sk->sk_state == LLCP_CLOSED) {
+		release_sock(sk);
+		nfc_llcp_sock_put(llcp_sock);
+	}
+
+	/* Pass the payload upstream */
+	if (ptype == LLCP_PDU_I) {
+		pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
+
+		if (ns == llcp_sock->recv_n)
+			llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
+		else
+			pr_err("Received out of sequence I PDU\n");
+
+		skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
+		if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+			pr_err("receive queue is full\n");
+			skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
+		}
+	}
+
+	/* Remove skbs from the pending queue */
+	if (llcp_sock->send_ack_n != nr) {
+		struct sk_buff *s, *tmp;
+
+		llcp_sock->send_ack_n = nr;
+
+		skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp)
+			if (nfc_llcp_ns(s) <= nr) {
+				skb_unlink(s, &llcp_sock->tx_pending_queue);
+				kfree_skb(s);
+			}
+	}
+
+	if (ptype == LLCP_PDU_RR)
+		llcp_sock->remote_ready = true;
+	else if (ptype == LLCP_PDU_RNR)
+		llcp_sock->remote_ready = false;
+
+	if (nfc_llcp_queue_i_frames(llcp_sock) == 0)
+		nfc_llcp_send_rr(llcp_sock);
+
+	release_sock(sk);
+	nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
+			       struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	struct sock *sk;
+	u8 dsap, ssap;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+	if (llcp_sock == NULL) {
+		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+		return;
+	}
+
+	sk = &llcp_sock->sk;
+	lock_sock(sk);
+	if (sk->sk_state == LLCP_CLOSED) {
+		release_sock(sk);
+		nfc_llcp_sock_put(llcp_sock);
+	}
+
+	if (sk->sk_state == LLCP_CONNECTED) {
+		nfc_put_device(local->dev);
+		sk->sk_state = LLCP_CLOSED;
+		sk->sk_state_change(sk);
+	}
+
+	nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
+
+	release_sock(sk);
+	nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	u8 dsap, ssap;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+
+	if (llcp_sock == NULL)
+		llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+
+	if (llcp_sock == NULL) {
+		pr_err("Invalid CC\n");
+		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+
+		return;
+	}
+
+	llcp_sock->dsap = ssap;
+
+	nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE],
+			   skb->len - LLCP_HEADER_SIZE);
+
+	nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+						    rx_work);
+	u8 dsap, ssap, ptype;
+	struct sk_buff *skb;
+
+	skb = local->rx_pending;
+	if (skb == NULL) {
+		pr_debug("No pending SKB\n");
+		return;
+	}
+
+	ptype = nfc_llcp_ptype(skb);
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+	switch (ptype) {
+	case LLCP_PDU_SYMM:
+		pr_debug("SYMM\n");
+		break;
+
+	case LLCP_PDU_CONNECT:
+		pr_debug("CONNECT\n");
+		nfc_llcp_recv_connect(local, skb);
+		break;
+
+	case LLCP_PDU_DISC:
+		pr_debug("DISC\n");
+		nfc_llcp_recv_disc(local, skb);
+		break;
+
+	case LLCP_PDU_CC:
+		pr_debug("CC\n");
+		nfc_llcp_recv_cc(local, skb);
+		break;
+
+	case LLCP_PDU_I:
+	case LLCP_PDU_RR:
+	case LLCP_PDU_RNR:
+		pr_debug("I frame\n");
+		nfc_llcp_recv_hdlc(local, skb);
+		break;
+
+	}
+
+	queue_work(local->tx_wq, &local->tx_work);
+	kfree_skb(local->rx_pending);
+	local->rx_pending = NULL;
+
+	return;
+}
+
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
+{
+	struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+	pr_debug("Received an LLCP PDU\n");
+	if (err < 0) {
+		pr_err("err %d\n", err);
+		return;
+	}
+
+	local->rx_pending = skb_get(skb);
+	del_timer(&local->link_timer);
+	queue_work(local->rx_wq, &local->rx_work);
+
+	return;
+}
+
+void nfc_llcp_mac_is_down(struct nfc_dev *dev)
+{
+	struct nfc_llcp_local *local;
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL)
+		return;
+
+	nfc_llcp_clear_sdp(local);
+
+	/* Close and purge all existing sockets */
+	nfc_llcp_socket_release(local);
+}
+
+void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+			u8 comm_mode, u8 rf_mode)
+{
+	struct nfc_llcp_local *local;
+
+	pr_debug("rf mode %d\n", rf_mode);
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL)
+		return;
+
+	local->target_idx = target_idx;
+	local->comm_mode = comm_mode;
+	local->rf_mode = rf_mode;
+
+	if (rf_mode == NFC_RF_INITIATOR) {
+		pr_debug("Queueing Tx work\n");
+
+		queue_work(local->tx_wq, &local->tx_work);
+	} else {
+		mod_timer(&local->link_timer,
+			  jiffies + msecs_to_jiffies(local->remote_lto));
+	}
+}
+
+int nfc_llcp_register_device(struct nfc_dev *ndev)
+{
+	struct device *dev = &ndev->dev;
+	struct nfc_llcp_local *local;
+	char name[32];
+	int err;
+
+	local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
+	if (local == NULL)
+		return -ENOMEM;
+
+	local->dev = ndev;
+	INIT_LIST_HEAD(&local->list);
+	mutex_init(&local->sdp_lock);
+	mutex_init(&local->socket_lock);
+	init_timer(&local->link_timer);
+	local->link_timer.data = (unsigned long) local;
+	local->link_timer.function = nfc_llcp_symm_timer;
+
+	skb_queue_head_init(&local->tx_queue);
+	INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
+	snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
+	local->tx_wq =
+		alloc_workqueue(name,
+				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				1);
+	if (local->tx_wq == NULL) {
+		err = -ENOMEM;
+		goto err_local;
+	}
+
+	local->rx_pending = NULL;
+	INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
+	snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
+	local->rx_wq =
+		alloc_workqueue(name,
+				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				1);
+	if (local->rx_wq == NULL) {
+		err = -ENOMEM;
+		goto err_tx_wq;
+	}
+
+	INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
+	snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
+	local->timeout_wq =
+		alloc_workqueue(name,
+				WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
+				1);
+	if (local->timeout_wq == NULL) {
+		err = -ENOMEM;
+		goto err_rx_wq;
+	}
+
+	nfc_llcp_build_gb(local);
+
+	local->remote_miu = LLCP_DEFAULT_MIU;
+	local->remote_lto = LLCP_DEFAULT_LTO;
+	local->remote_rw = LLCP_DEFAULT_RW;
+
+	list_add(&llcp_devices, &local->list);
+
+	return 0;
+
+err_rx_wq:
+	destroy_workqueue(local->rx_wq);
+
+err_tx_wq:
+	destroy_workqueue(local->tx_wq);
+
+err_local:
+	kfree(local);
+
+	return 0;
+}
+
+void nfc_llcp_unregister_device(struct nfc_dev *dev)
+{
+	struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+	if (local == NULL) {
+		pr_debug("No such device\n");
+		return;
+	}
+
+	list_del(&local->list);
+	nfc_llcp_socket_release(local);
+	del_timer_sync(&local->link_timer);
+	skb_queue_purge(&local->tx_queue);
+	destroy_workqueue(local->tx_wq);
+	destroy_workqueue(local->rx_wq);
+	kfree_skb(local->rx_pending);
+	kfree(local);
+}
+
+int __init nfc_llcp_init(void)
+{
+	INIT_LIST_HEAD(&llcp_devices);
+
+	return nfc_llcp_sock_init();
+}
+
+void nfc_llcp_exit(void)
+{
+	nfc_llcp_sock_exit();
+}
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
new file mode 100644
index 00000000..50680ce5
--- /dev/null
+++ b/net/nfc/llcp/llcp.h
@@ -0,0 +1,201 @@
+/*
+ * Copyright (C) 2011  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+enum llcp_state {
+	LLCP_CONNECTED = 1, /* wait_for_packet() wants that */
+	LLCP_CLOSED,
+	LLCP_BOUND,
+	LLCP_LISTEN,
+};
+
+#define LLCP_DEFAULT_LTO 100
+#define LLCP_DEFAULT_RW  1
+#define LLCP_DEFAULT_MIU 128
+
+#define LLCP_MAX_LTO  0xff
+#define LLCP_MAX_RW   15
+#define LLCP_MAX_MIUX 0x7ff
+
+#define LLCP_WKS_NUM_SAP   16
+#define LLCP_SDP_NUM_SAP   16
+#define LLCP_LOCAL_NUM_SAP 32
+#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP)
+#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP)
+
+struct nfc_llcp_sock;
+
+struct nfc_llcp_local {
+	struct list_head list;
+	struct nfc_dev *dev;
+
+	struct mutex sdp_lock;
+	struct mutex socket_lock;
+
+	struct timer_list link_timer;
+	struct sk_buff_head tx_queue;
+	struct workqueue_struct	*tx_wq;
+	struct work_struct	 tx_work;
+	struct workqueue_struct	*rx_wq;
+	struct work_struct	 rx_work;
+	struct sk_buff *rx_pending;
+	struct workqueue_struct	*timeout_wq;
+	struct work_struct	 timeout_work;
+
+	u32 target_idx;
+	u8 rf_mode;
+	u8 comm_mode;
+	unsigned long local_wks;      /* Well known services */
+	unsigned long local_sdp;      /* Local services  */
+	unsigned long local_sap; /* Local SAPs, not available for discovery */
+
+	/* local */
+	u8 gb[NFC_MAX_GT_LEN];
+	u8 gb_len;
+
+	/* remote */
+	u8 remote_gb[NFC_MAX_GT_LEN];
+	u8 remote_gb_len;
+
+	u8  remote_version;
+	u16 remote_miu;
+	u16 remote_lto;
+	u8  remote_opt;
+	u16 remote_wks;
+	u8  remote_rw;
+
+	/* sockets array */
+	struct nfc_llcp_sock *sockets[LLCP_MAX_SAP];
+};
+
+struct nfc_llcp_sock {
+	struct sock sk;
+	struct list_head list;
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	u32 target_idx;
+	u32 nfc_protocol;
+
+	u8 ssap;
+	u8 dsap;
+	char *service_name;
+	size_t service_name_len;
+
+	/* Link variables */
+	u8 send_n;
+	u8 send_ack_n;
+	u8 recv_n;
+	u8 recv_ack_n;
+
+	/* Is the remote peer ready to receive */
+	u8 remote_ready;
+
+	struct sk_buff_head tx_queue;
+	struct sk_buff_head tx_pending_queue;
+	struct sk_buff_head tx_backlog_queue;
+
+	struct list_head accept_queue;
+	struct sock *parent;
+};
+
+#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
+#define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
+
+#define LLCP_HEADER_SIZE   2
+#define LLCP_SEQUENCE_SIZE 1
+
+/* LLCP versions: 1.1 is 1.0 plus SDP */
+#define LLCP_VERSION_10 0x10
+#define LLCP_VERSION_11 0x11
+
+/* LLCP PDU types */
+#define LLCP_PDU_SYMM     0x0
+#define LLCP_PDU_PAX      0x1
+#define LLCP_PDU_AGF      0x2
+#define LLCP_PDU_UI       0x3
+#define LLCP_PDU_CONNECT  0x4
+#define LLCP_PDU_DISC     0x5
+#define LLCP_PDU_CC       0x6
+#define LLCP_PDU_DM       0x7
+#define LLCP_PDU_FRMR     0x8
+#define LLCP_PDU_SNL      0x9
+#define LLCP_PDU_I        0xc
+#define LLCP_PDU_RR       0xd
+#define LLCP_PDU_RNR      0xe
+
+/* Parameters TLV types */
+#define LLCP_TLV_VERSION 0x1
+#define LLCP_TLV_MIUX    0x2
+#define LLCP_TLV_WKS     0x3
+#define LLCP_TLV_LTO     0x4
+#define LLCP_TLV_RW      0x5
+#define LLCP_TLV_SN      0x6
+#define LLCP_TLV_OPT     0x7
+#define LLCP_TLV_SDREQ   0x8
+#define LLCP_TLV_SDRES   0x9
+#define LLCP_TLV_MAX     0xa
+
+/* Well known LLCP SAP */
+#define LLCP_SAP_SDP   0x1
+#define LLCP_SAP_IP    0x2
+#define LLCP_SAP_OBEX  0x3
+#define LLCP_SAP_SNEP  0x4
+#define LLCP_SAP_MAX   0xff
+
+/* Disconnection reason code */
+#define LLCP_DM_DISC    0x00
+#define LLCP_DM_NOCONN  0x01
+#define LLCP_DM_NOBOUND 0x02
+#define LLCP_DM_REJ     0x03
+
+
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
+			 struct nfc_llcp_sock *sock);
+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);
+
+/* Sock API */
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
+void nfc_llcp_sock_free(struct nfc_llcp_sock *sock);
+void nfc_llcp_accept_unlink(struct sock *sk);
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk);
+struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock);
+
+/* TLV API */
+int nfc_llcp_parse_tlv(struct nfc_llcp_local *local,
+		       u8 *tlv_array, u16 tlv_array_len);
+
+/* Commands API */
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
+int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_symm(struct nfc_dev *dev);
+int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
+int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
+			  struct msghdr *msg, size_t len);
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
+
+/* Socket API */
+int __init nfc_llcp_sock_init(void);
+void nfc_llcp_sock_exit(void);
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
new file mode 100644
index 00000000..c13e02eb
--- /dev/null
+++ b/net/nfc/llcp/sock.c
@@ -0,0 +1,703 @@
+/*
+ * Copyright (C) 2011  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include "../nfc.h"
+#include "llcp.h"
+
+static struct proto llcp_sock_proto = {
+	.name     = "NFC_LLCP",
+	.owner    = THIS_MODULE,
+	.obj_size = sizeof(struct nfc_llcp_sock),
+};
+
+static int llcp_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;
+
+	pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+	if (!addr || addr->sa_family != AF_NFC)
+		return -EINVAL;
+
+	memset(&llcp_addr, 0, sizeof(llcp_addr));
+	len = min_t(unsigned int, sizeof(llcp_addr), alen);
+	memcpy(&llcp_addr, addr, len);
+
+	/* This is going to be a listening socket, dsap must be 0 */
+	if (llcp_addr.dsap != 0)
+		return -EINVAL;
+
+	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 = local;
+	llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+	llcp_sock->service_name_len = min_t(unsigned int,
+					    llcp_addr.service_name_len,
+					    NFC_LLCP_MAX_SERVICE_NAME);
+	llcp_sock->service_name = kmemdup(llcp_addr.service_name,
+					  llcp_sock->service_name_len,
+					  GFP_KERNEL);
+
+	llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
+	if (llcp_sock->ssap == LLCP_MAX_SAP)
+		goto put_dev;
+
+	local->sockets[llcp_sock->ssap] = llcp_sock;
+
+	pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
+
+	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;
+	int ret = 0;
+
+	pr_debug("sk %p backlog %d\n", sk, backlog);
+
+	lock_sock(sk);
+
+	if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
+	    || sk->sk_state != LLCP_BOUND) {
+		ret = -EBADFD;
+		goto error;
+	}
+
+	sk->sk_max_ack_backlog = backlog;
+	sk->sk_ack_backlog = 0;
+
+	pr_debug("Socket listening\n");
+	sk->sk_state = LLCP_LISTEN;
+
+error:
+	release_sock(sk);
+
+	return ret;
+}
+
+void nfc_llcp_accept_unlink(struct sock *sk)
+{
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+	pr_debug("state %d\n", sk->sk_state);
+
+	list_del_init(&llcp_sock->accept_queue);
+	sk_acceptq_removed(llcp_sock->parent);
+	llcp_sock->parent = NULL;
+
+	sock_put(sk);
+}
+
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
+{
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
+
+	/* Lock will be free from unlink */
+	sock_hold(sk);
+
+	list_add_tail(&llcp_sock->accept_queue,
+		      &llcp_sock_parent->accept_queue);
+	llcp_sock->parent = parent;
+	sk_acceptq_added(parent);
+}
+
+struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
+				     struct socket *newsock)
+{
+	struct nfc_llcp_sock *lsk, *n, *llcp_parent;
+	struct sock *sk;
+
+	llcp_parent = nfc_llcp_sock(parent);
+
+	list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
+				 accept_queue) {
+		sk = &lsk->sk;
+		lock_sock(sk);
+
+		if (sk->sk_state == LLCP_CLOSED) {
+			release_sock(sk);
+			nfc_llcp_accept_unlink(sk);
+			continue;
+		}
+
+		if (sk->sk_state == LLCP_CONNECTED || !newsock) {
+			nfc_llcp_accept_unlink(sk);
+			if (newsock)
+				sock_graft(sk, newsock);
+
+			release_sock(sk);
+
+			pr_debug("Returning sk state %d\n", sk->sk_state);
+
+			return sk;
+		}
+
+		release_sock(sk);
+	}
+
+	return NULL;
+}
+
+static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
+			    int flags)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	struct sock *sk = sock->sk, *new_sk;
+	long timeo;
+	int ret = 0;
+
+	pr_debug("parent %p\n", sk);
+
+	lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
+
+	if (sk->sk_state != LLCP_LISTEN) {
+		ret = -EBADFD;
+		goto error;
+	}
+
+	timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
+
+	/* Wait for an incoming connection. */
+	add_wait_queue_exclusive(sk_sleep(sk), &wait);
+	while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		if (!timeo) {
+			ret = -EAGAIN;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			ret = sock_intr_errno(timeo);
+			break;
+		}
+
+		release_sock(sk);
+		timeo = schedule_timeout(timeo);
+		lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
+	}
+	__set_current_state(TASK_RUNNING);
+	remove_wait_queue(sk_sleep(sk), &wait);
+
+	if (ret)
+		goto error;
+
+	newsock->state = SS_CONNECTED;
+
+	pr_debug("new socket %p\n", new_sk);
+
+error:
+	release_sock(sk);
+
+	return ret;
+}
+
+static int llcp_sock_getname(struct socket *sock, struct sockaddr *addr,
+			     int *len, int peer)
+{
+	struct sockaddr_nfc_llcp *llcp_addr = (struct sockaddr_nfc_llcp *)addr;
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+	pr_debug("%p\n", sk);
+
+	addr->sa_family = AF_NFC;
+	*len = sizeof(struct sockaddr_nfc_llcp);
+
+	llcp_addr->dev_idx = llcp_sock->dev->idx;
+	llcp_addr->dsap = llcp_sock->dsap;
+	llcp_addr->ssap = llcp_sock->ssap;
+	llcp_addr->service_name_len = llcp_sock->service_name_len;
+	memcpy(llcp_addr->service_name, llcp_sock->service_name,
+	       llcp_addr->service_name_len);
+
+	return 0;
+}
+
+static inline unsigned int llcp_accept_poll(struct sock *parent)
+{
+	struct nfc_llcp_sock *llcp_sock, *n, *parent_sock;
+	struct sock *sk;
+
+	parent_sock = nfc_llcp_sock(parent);
+
+	list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue,
+				 accept_queue) {
+		sk = &llcp_sock->sk;
+
+		if (sk->sk_state == LLCP_CONNECTED)
+			return POLLIN | POLLRDNORM;
+	}
+
+	return 0;
+}
+
+static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
+				   poll_table *wait)
+{
+	struct sock *sk = sock->sk;
+	unsigned int mask = 0;
+
+	pr_debug("%p\n", sk);
+
+	sock_poll_wait(file, sk_sleep(sk), wait);
+
+	if (sk->sk_state == LLCP_LISTEN)
+		return llcp_accept_poll(sk);
+
+	if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
+		mask |= POLLERR;
+
+	if (!skb_queue_empty(&sk->sk_receive_queue))
+		mask |= POLLIN;
+
+	if (sk->sk_state == LLCP_CLOSED)
+		mask |= POLLHUP;
+
+	return mask;
+}
+
+static int llcp_sock_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_local *local;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	int err = 0;
+
+	if (!sk)
+		return 0;
+
+	pr_debug("%p\n", sk);
+
+	local = llcp_sock->local;
+	if (local == NULL) {
+		err = -ENODEV;
+		goto out;
+	}
+
+	mutex_lock(&local->socket_lock);
+
+	if (llcp_sock == local->sockets[llcp_sock->ssap])
+		local->sockets[llcp_sock->ssap] = NULL;
+	else
+		list_del_init(&llcp_sock->list);
+
+	mutex_unlock(&local->socket_lock);
+
+	lock_sock(sk);
+
+	/* Send a DISC */
+	if (sk->sk_state == LLCP_CONNECTED)
+		nfc_llcp_disconnect(llcp_sock);
+
+	if (sk->sk_state == LLCP_LISTEN) {
+		struct nfc_llcp_sock *lsk, *n;
+		struct sock *accept_sk;
+
+		list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
+					 accept_queue) {
+			accept_sk = &lsk->sk;
+			lock_sock(accept_sk);
+
+			nfc_llcp_disconnect(lsk);
+			nfc_llcp_accept_unlink(accept_sk);
+
+			release_sock(accept_sk);
+
+			sock_orphan(accept_sk);
+		}
+	}
+
+	/* Freeing the SAP */
+	if ((sk->sk_state == LLCP_CONNECTED
+	     && llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) ||
+	    sk->sk_state == LLCP_BOUND || sk->sk_state == LLCP_LISTEN)
+		nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
+
+	release_sock(sk);
+
+out:
+	sock_orphan(sk);
+	sock_put(sk);
+
+	return err;
+}
+
+static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
+			     int len, int flags)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	int ret = 0;
+
+	pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
+
+	if (!addr || len < sizeof(struct sockaddr_nfc) ||
+	    addr->sa_family != AF_NFC) {
+		pr_err("Invalid socket\n");
+		return -EINVAL;
+	}
+
+	if (addr->service_name_len == 0 && addr->dsap == 0) {
+		pr_err("Missing service name or dsap\n");
+		return -EINVAL;
+	}
+
+	pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
+		 addr->target_idx, addr->nfc_protocol);
+
+	lock_sock(sk);
+
+	if (sk->sk_state == LLCP_CONNECTED) {
+		ret = -EISCONN;
+		goto error;
+	}
+
+	dev = nfc_get_device(addr->dev_idx);
+	if (dev == NULL) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL) {
+		ret = -ENODEV;
+		goto put_dev;
+	}
+
+	device_lock(&dev->dev);
+	if (dev->dep_link_up == false) {
+		ret = -ENOLINK;
+		device_unlock(&dev->dev);
+		goto put_dev;
+	}
+	device_unlock(&dev->dev);
+
+	if (local->rf_mode == NFC_RF_INITIATOR &&
+	    addr->target_idx != local->target_idx) {
+		ret = -ENOLINK;
+		goto put_dev;
+	}
+
+	llcp_sock->dev = dev;
+	llcp_sock->local = local;
+	llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
+	if (llcp_sock->ssap == LLCP_SAP_MAX) {
+		ret = -ENOMEM;
+		goto put_dev;
+	}
+	if (addr->service_name_len == 0)
+		llcp_sock->dsap = addr->dsap;
+	else
+		llcp_sock->dsap = LLCP_SAP_SDP;
+	llcp_sock->nfc_protocol = addr->nfc_protocol;
+	llcp_sock->service_name_len = min_t(unsigned int,
+					    addr->service_name_len,
+					    NFC_LLCP_MAX_SERVICE_NAME);
+	llcp_sock->service_name = kmemdup(addr->service_name,
+					  llcp_sock->service_name_len,
+					  GFP_KERNEL);
+
+	local->sockets[llcp_sock->ssap] = llcp_sock;
+
+	ret = nfc_llcp_send_connect(llcp_sock);
+	if (ret)
+		goto put_dev;
+
+	sk->sk_state = LLCP_CONNECTED;
+
+	release_sock(sk);
+	return 0;
+
+put_dev:
+	nfc_put_device(dev);
+
+error:
+	release_sock(sk);
+	return ret;
+}
+
+static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
+			     struct msghdr *msg, size_t len)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	int ret;
+
+	pr_debug("sock %p sk %p", sock, sk);
+
+	ret = sock_error(sk);
+	if (ret)
+		return ret;
+
+	if (msg->msg_flags & MSG_OOB)
+		return -EOPNOTSUPP;
+
+	lock_sock(sk);
+
+	if (sk->sk_state != LLCP_CONNECTED) {
+		release_sock(sk);
+		return -ENOTCONN;
+	}
+
+	release_sock(sk);
+
+	return nfc_llcp_send_i_frame(llcp_sock, msg, len);
+}
+
+static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
+			     struct msghdr *msg, size_t len, int flags)
+{
+	int noblock = flags & MSG_DONTWAIT;
+	struct sock *sk = sock->sk;
+	unsigned int copied, rlen;
+	struct sk_buff *skb, *cskb;
+	int err = 0;
+
+	pr_debug("%p %zu\n", sk, len);
+
+	lock_sock(sk);
+
+	if (sk->sk_state == LLCP_CLOSED &&
+	    skb_queue_empty(&sk->sk_receive_queue)) {
+		release_sock(sk);
+		return 0;
+	}
+
+	release_sock(sk);
+
+	if (flags & (MSG_OOB))
+		return -EOPNOTSUPP;
+
+	skb = skb_recv_datagram(sk, flags, noblock, &err);
+	if (!skb) {
+		pr_err("Recv datagram failed state %d %d %d",
+		       sk->sk_state, err, sock_error(sk));
+
+		if (sk->sk_shutdown & RCV_SHUTDOWN)
+			return 0;
+
+		return err;
+	}
+
+	rlen = skb->len;		/* real length of skb */
+	copied = min_t(unsigned int, rlen, len);
+
+	cskb = skb;
+	if (memcpy_toiovec(msg->msg_iov, cskb->data, copied)) {
+		if (!(flags & MSG_PEEK))
+			skb_queue_head(&sk->sk_receive_queue, skb);
+		return -EFAULT;
+	}
+
+	/* Mark read part of skb as used */
+	if (!(flags & MSG_PEEK)) {
+
+		/* SOCK_STREAM: re-queue skb if it contains unreceived data */
+		if (sk->sk_type == SOCK_STREAM) {
+			skb_pull(skb, copied);
+			if (skb->len) {
+				skb_queue_head(&sk->sk_receive_queue, skb);
+				goto done;
+			}
+		}
+
+		kfree_skb(skb);
+	}
+
+	/* XXX Queue backlogged skbs */
+
+done:
+	/* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
+	if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
+		copied = rlen;
+
+	return copied;
+}
+
+static const struct proto_ops llcp_sock_ops = {
+	.family         = PF_NFC,
+	.owner          = THIS_MODULE,
+	.bind           = llcp_sock_bind,
+	.connect        = llcp_sock_connect,
+	.release        = llcp_sock_release,
+	.socketpair     = sock_no_socketpair,
+	.accept         = llcp_sock_accept,
+	.getname        = llcp_sock_getname,
+	.poll           = llcp_sock_poll,
+	.ioctl          = sock_no_ioctl,
+	.listen         = llcp_sock_listen,
+	.shutdown       = sock_no_shutdown,
+	.setsockopt     = sock_no_setsockopt,
+	.getsockopt     = sock_no_getsockopt,
+	.sendmsg        = llcp_sock_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);
+
+	pr_debug("%p\n", sk);
+
+	if (sk->sk_state == LLCP_CONNECTED)
+		nfc_put_device(llcp_sock->dev);
+
+	skb_queue_purge(&sk->sk_receive_queue);
+
+	nfc_llcp_sock_free(llcp_sock);
+
+	if (!sock_flag(sk, SOCK_DEAD)) {
+		pr_err("Freeing alive NFC LLCP socket %p\n", sk);
+		return;
+	}
+}
+
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
+{
+	struct sock *sk;
+	struct nfc_llcp_sock *llcp_sock;
+
+	sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto);
+	if (!sk)
+		return NULL;
+
+	llcp_sock = nfc_llcp_sock(sk);
+
+	sock_init_data(sock, sk);
+	sk->sk_state = LLCP_CLOSED;
+	sk->sk_protocol = NFC_SOCKPROTO_LLCP;
+	sk->sk_type = type;
+	sk->sk_destruct = llcp_sock_destruct;
+
+	llcp_sock->ssap = 0;
+	llcp_sock->dsap = LLCP_SAP_SDP;
+	llcp_sock->send_n = llcp_sock->send_ack_n = 0;
+	llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
+	llcp_sock->remote_ready = 1;
+	skb_queue_head_init(&llcp_sock->tx_queue);
+	skb_queue_head_init(&llcp_sock->tx_pending_queue);
+	skb_queue_head_init(&llcp_sock->tx_backlog_queue);
+	INIT_LIST_HEAD(&llcp_sock->list);
+	INIT_LIST_HEAD(&llcp_sock->accept_queue);
+
+	if (sock != NULL)
+		sock->state = SS_UNCONNECTED;
+
+	return sk;
+}
+
+void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local = sock->local;
+
+	kfree(sock->service_name);
+
+	skb_queue_purge(&sock->tx_queue);
+	skb_queue_purge(&sock->tx_pending_queue);
+	skb_queue_purge(&sock->tx_backlog_queue);
+
+	list_del_init(&sock->accept_queue);
+
+	if (local != NULL && sock == local->sockets[sock->ssap])
+		local->sockets[sock->ssap] = NULL;
+	else
+		list_del_init(&sock->list);
+
+	sock->parent = NULL;
+}
+
+static int llcp_sock_create(struct net *net, struct socket *sock,
+			    const struct nfc_protocol *nfc_proto)
+{
+	struct sock *sk;
+
+	pr_debug("%p\n", sock);
+
+	if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM)
+		return -ESOCKTNOSUPPORT;
+
+	sock->ops = &llcp_sock_ops;
+
+	sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
+	if (sk == NULL)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static const struct nfc_protocol llcp_nfc_proto = {
+	.id	  = NFC_SOCKPROTO_LLCP,
+	.proto    = &llcp_sock_proto,
+	.owner    = THIS_MODULE,
+	.create   = llcp_sock_create
+};
+
+int __init nfc_llcp_sock_init(void)
+{
+	return nfc_proto_register(&llcp_nfc_proto);
+}
+
+void nfc_llcp_sock_exit(void)
+{
+	nfc_proto_unregister(&llcp_nfc_proto);
+}
diff --git a/net/nfc/nci/Kconfig b/net/nfc/nci/Kconfig
new file mode 100644
index 00000000..decdc49b
--- /dev/null
+++ b/net/nfc/nci/Kconfig
@@ -0,0 +1,10 @@
+config NFC_NCI
+	depends on NFC && EXPERIMENTAL
+	tristate "NCI protocol support (EXPERIMENTAL)"
+	default n
+	help
+	  NCI (NFC Controller Interface) is a communication protocol between
+	  an NFC Controller (NFCC) and a Device Host (DH).
+
+	  Say Y here to compile NCI support into the kernel or say M to
+	  compile it as module (nci).
diff --git a/net/nfc/nci/Makefile b/net/nfc/nci/Makefile
new file mode 100644
index 00000000..cdb3a2e4
--- /dev/null
+++ b/net/nfc/nci/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the Linux NFC NCI layer.
+#
+
+obj-$(CONFIG_NFC_NCI) += nci.o
+
+nci-objs := core.o data.o lib.o ntf.o rsp.o
\ No newline at end of file
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
new file mode 100644
index 00000000..9ec065bb
--- /dev/null
+++ b/net/nfc/nci/core.c
@@ -0,0 +1,879 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on hci_core.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/workqueue.h>
+#include <linux/completion.h>
+#include <linux/export.h>
+#include <linux/sched.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+static void nci_cmd_work(struct work_struct *work);
+static void nci_rx_work(struct work_struct *work);
+static void nci_tx_work(struct work_struct *work);
+
+/* ---- NCI requests ---- */
+
+void nci_req_complete(struct nci_dev *ndev, int result)
+{
+	if (ndev->req_status == NCI_REQ_PEND) {
+		ndev->req_result = result;
+		ndev->req_status = NCI_REQ_DONE;
+		complete(&ndev->req_completion);
+	}
+}
+
+static void nci_req_cancel(struct nci_dev *ndev, int err)
+{
+	if (ndev->req_status == NCI_REQ_PEND) {
+		ndev->req_result = err;
+		ndev->req_status = NCI_REQ_CANCELED;
+		complete(&ndev->req_completion);
+	}
+}
+
+/* Execute request and wait for completion. */
+static int __nci_request(struct nci_dev *ndev,
+			 void (*req)(struct nci_dev *ndev, unsigned long opt),
+			 unsigned long opt, __u32 timeout)
+{
+	int rc = 0;
+	long completion_rc;
+
+	ndev->req_status = NCI_REQ_PEND;
+
+	init_completion(&ndev->req_completion);
+	req(ndev, opt);
+	completion_rc =
+		wait_for_completion_interruptible_timeout(&ndev->req_completion,
+							  timeout);
+
+	pr_debug("wait_for_completion return %ld\n", completion_rc);
+
+	if (completion_rc > 0) {
+		switch (ndev->req_status) {
+		case NCI_REQ_DONE:
+			rc = nci_to_errno(ndev->req_result);
+			break;
+
+		case NCI_REQ_CANCELED:
+			rc = -ndev->req_result;
+			break;
+
+		default:
+			rc = -ETIMEDOUT;
+			break;
+		}
+	} else {
+		pr_err("wait_for_completion_interruptible_timeout failed %ld\n",
+		       completion_rc);
+
+		rc = ((completion_rc == 0) ? (-ETIMEDOUT) : (completion_rc));
+	}
+
+	ndev->req_status = ndev->req_result = 0;
+
+	return rc;
+}
+
+static inline int nci_request(struct nci_dev *ndev,
+			      void (*req)(struct nci_dev *ndev,
+					  unsigned long opt),
+			      unsigned long opt, __u32 timeout)
+{
+	int rc;
+
+	if (!test_bit(NCI_UP, &ndev->flags))
+		return -ENETDOWN;
+
+	/* Serialize all requests */
+	mutex_lock(&ndev->req_lock);
+	rc = __nci_request(ndev, req, opt, timeout);
+	mutex_unlock(&ndev->req_lock);
+
+	return rc;
+}
+
+static void nci_reset_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_core_reset_cmd cmd;
+
+	cmd.reset_type = NCI_RESET_TYPE_RESET_CONFIG;
+	nci_send_cmd(ndev, NCI_OP_CORE_RESET_CMD, 1, &cmd);
+}
+
+static void nci_init_req(struct nci_dev *ndev, unsigned long opt)
+{
+	nci_send_cmd(ndev, NCI_OP_CORE_INIT_CMD, 0, NULL);
+}
+
+static void nci_init_complete_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_rf_disc_map_cmd cmd;
+	struct disc_map_config *cfg = cmd.mapping_configs;
+	__u8 *num = &cmd.num_mapping_configs;
+	int i;
+
+	/* set rf mapping configurations */
+	*num = 0;
+
+	/* by default mapping is set to NCI_RF_INTERFACE_FRAME */
+	for (i = 0; i < ndev->num_supported_rf_interfaces; i++) {
+		if (ndev->supported_rf_interfaces[i] ==
+		    NCI_RF_INTERFACE_ISO_DEP) {
+			cfg[*num].rf_protocol = NCI_RF_PROTOCOL_ISO_DEP;
+			cfg[*num].mode = NCI_DISC_MAP_MODE_POLL |
+				NCI_DISC_MAP_MODE_LISTEN;
+			cfg[*num].rf_interface = NCI_RF_INTERFACE_ISO_DEP;
+			(*num)++;
+		} else if (ndev->supported_rf_interfaces[i] ==
+			   NCI_RF_INTERFACE_NFC_DEP) {
+			cfg[*num].rf_protocol = NCI_RF_PROTOCOL_NFC_DEP;
+			cfg[*num].mode = NCI_DISC_MAP_MODE_POLL |
+				NCI_DISC_MAP_MODE_LISTEN;
+			cfg[*num].rf_interface = NCI_RF_INTERFACE_NFC_DEP;
+			(*num)++;
+		}
+
+		if (*num == NCI_MAX_NUM_MAPPING_CONFIGS)
+			break;
+	}
+
+	nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_MAP_CMD,
+		     (1 + ((*num) * sizeof(struct disc_map_config))), &cmd);
+}
+
+static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_rf_disc_cmd cmd;
+	__u32 protocols = opt;
+
+	cmd.num_disc_configs = 0;
+
+	if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+	    (protocols & NFC_PROTO_JEWEL_MASK
+	     || protocols & NFC_PROTO_MIFARE_MASK
+	     || protocols & NFC_PROTO_ISO14443_MASK
+	     || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+		cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+			NCI_NFC_A_PASSIVE_POLL_MODE;
+		cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+		cmd.num_disc_configs++;
+	}
+
+	if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+	    (protocols & NFC_PROTO_ISO14443_MASK)) {
+		cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+			NCI_NFC_B_PASSIVE_POLL_MODE;
+		cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+		cmd.num_disc_configs++;
+	}
+
+	if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+	    (protocols & NFC_PROTO_FELICA_MASK
+	     || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+		cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+			NCI_NFC_F_PASSIVE_POLL_MODE;
+		cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+		cmd.num_disc_configs++;
+	}
+
+	nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_CMD,
+		     (1 + (cmd.num_disc_configs * sizeof(struct disc_config))),
+		     &cmd);
+}
+
+struct nci_rf_discover_select_param {
+	__u8	rf_discovery_id;
+	__u8	rf_protocol;
+};
+
+static void nci_rf_discover_select_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_rf_discover_select_param *param =
+		(struct nci_rf_discover_select_param *)opt;
+	struct nci_rf_discover_select_cmd cmd;
+
+	cmd.rf_discovery_id = param->rf_discovery_id;
+	cmd.rf_protocol = param->rf_protocol;
+
+	switch (cmd.rf_protocol) {
+	case NCI_RF_PROTOCOL_ISO_DEP:
+		cmd.rf_interface = NCI_RF_INTERFACE_ISO_DEP;
+		break;
+
+	case NCI_RF_PROTOCOL_NFC_DEP:
+		cmd.rf_interface = NCI_RF_INTERFACE_NFC_DEP;
+		break;
+
+	default:
+		cmd.rf_interface = NCI_RF_INTERFACE_FRAME;
+		break;
+	}
+
+	nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_SELECT_CMD,
+		     sizeof(struct nci_rf_discover_select_cmd), &cmd);
+}
+
+static void nci_rf_deactivate_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_rf_deactivate_cmd cmd;
+
+	cmd.type = NCI_DEACTIVATE_TYPE_IDLE_MODE;
+
+	nci_send_cmd(ndev, NCI_OP_RF_DEACTIVATE_CMD,
+		     sizeof(struct nci_rf_deactivate_cmd), &cmd);
+}
+
+static int nci_open_device(struct nci_dev *ndev)
+{
+	int rc = 0;
+
+	mutex_lock(&ndev->req_lock);
+
+	if (test_bit(NCI_UP, &ndev->flags)) {
+		rc = -EALREADY;
+		goto done;
+	}
+
+	if (ndev->ops->open(ndev)) {
+		rc = -EIO;
+		goto done;
+	}
+
+	atomic_set(&ndev->cmd_cnt, 1);
+
+	set_bit(NCI_INIT, &ndev->flags);
+
+	rc = __nci_request(ndev, nci_reset_req, 0,
+			   msecs_to_jiffies(NCI_RESET_TIMEOUT));
+
+	if (!rc) {
+		rc = __nci_request(ndev, nci_init_req, 0,
+				   msecs_to_jiffies(NCI_INIT_TIMEOUT));
+	}
+
+	if (!rc) {
+		rc = __nci_request(ndev, nci_init_complete_req, 0,
+				   msecs_to_jiffies(NCI_INIT_TIMEOUT));
+	}
+
+	clear_bit(NCI_INIT, &ndev->flags);
+
+	if (!rc) {
+		set_bit(NCI_UP, &ndev->flags);
+		nci_clear_target_list(ndev);
+		atomic_set(&ndev->state, NCI_IDLE);
+	} else {
+		/* Init failed, cleanup */
+		skb_queue_purge(&ndev->cmd_q);
+		skb_queue_purge(&ndev->rx_q);
+		skb_queue_purge(&ndev->tx_q);
+
+		ndev->ops->close(ndev);
+		ndev->flags = 0;
+	}
+
+done:
+	mutex_unlock(&ndev->req_lock);
+	return rc;
+}
+
+static int nci_close_device(struct nci_dev *ndev)
+{
+	nci_req_cancel(ndev, ENODEV);
+	mutex_lock(&ndev->req_lock);
+
+	if (!test_and_clear_bit(NCI_UP, &ndev->flags)) {
+		del_timer_sync(&ndev->cmd_timer);
+		del_timer_sync(&ndev->data_timer);
+		mutex_unlock(&ndev->req_lock);
+		return 0;
+	}
+
+	/* Drop RX and TX queues */
+	skb_queue_purge(&ndev->rx_q);
+	skb_queue_purge(&ndev->tx_q);
+
+	/* Flush RX and TX wq */
+	flush_workqueue(ndev->rx_wq);
+	flush_workqueue(ndev->tx_wq);
+
+	/* Reset device */
+	skb_queue_purge(&ndev->cmd_q);
+	atomic_set(&ndev->cmd_cnt, 1);
+
+	set_bit(NCI_INIT, &ndev->flags);
+	__nci_request(ndev, nci_reset_req, 0,
+		      msecs_to_jiffies(NCI_RESET_TIMEOUT));
+	clear_bit(NCI_INIT, &ndev->flags);
+
+	/* Flush cmd wq */
+	flush_workqueue(ndev->cmd_wq);
+
+	/* After this point our queues are empty
+	 * and no works are scheduled. */
+	ndev->ops->close(ndev);
+
+	/* Clear flags */
+	ndev->flags = 0;
+
+	mutex_unlock(&ndev->req_lock);
+
+	return 0;
+}
+
+/* NCI command timer function */
+static void nci_cmd_timer(unsigned long arg)
+{
+	struct nci_dev *ndev = (void *) arg;
+
+	atomic_set(&ndev->cmd_cnt, 1);
+	queue_work(ndev->cmd_wq, &ndev->cmd_work);
+}
+
+/* NCI data exchange timer function */
+static void nci_data_timer(unsigned long arg)
+{
+	struct nci_dev *ndev = (void *) arg;
+
+	set_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
+	queue_work(ndev->rx_wq, &ndev->rx_work);
+}
+
+static int nci_dev_up(struct nfc_dev *nfc_dev)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+	return nci_open_device(ndev);
+}
+
+static int nci_dev_down(struct nfc_dev *nfc_dev)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+	return nci_close_device(ndev);
+}
+
+static int nci_start_poll(struct nfc_dev *nfc_dev, __u32 protocols)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	int rc;
+
+	if ((atomic_read(&ndev->state) == NCI_DISCOVERY) ||
+	    (atomic_read(&ndev->state) == NCI_W4_ALL_DISCOVERIES)) {
+		pr_err("unable to start poll, since poll is already active\n");
+		return -EBUSY;
+	}
+
+	if (ndev->target_active_prot) {
+		pr_err("there is an active target\n");
+		return -EBUSY;
+	}
+
+	if ((atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) ||
+	    (atomic_read(&ndev->state) == NCI_POLL_ACTIVE)) {
+		pr_debug("target active or w4 select, implicitly deactivate\n");
+
+		rc = nci_request(ndev, nci_rf_deactivate_req, 0,
+				 msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+		if (rc)
+			return -EBUSY;
+	}
+
+	rc = nci_request(ndev, nci_rf_discover_req, protocols,
+			 msecs_to_jiffies(NCI_RF_DISC_TIMEOUT));
+
+	if (!rc)
+		ndev->poll_prots = protocols;
+
+	return rc;
+}
+
+static void nci_stop_poll(struct nfc_dev *nfc_dev)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+	if ((atomic_read(&ndev->state) != NCI_DISCOVERY) &&
+	    (atomic_read(&ndev->state) != NCI_W4_ALL_DISCOVERIES)) {
+		pr_err("unable to stop poll, since poll is not active\n");
+		return;
+	}
+
+	nci_request(ndev, nci_rf_deactivate_req, 0,
+		    msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+}
+
+static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx,
+			       __u32 protocol)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	struct nci_rf_discover_select_param param;
+	struct nfc_target *target = NULL;
+	int i;
+	int rc = 0;
+
+	pr_debug("target_idx %d, protocol 0x%x\n", target_idx, protocol);
+
+	if ((atomic_read(&ndev->state) != NCI_W4_HOST_SELECT) &&
+	    (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
+		pr_err("there is no available target to activate\n");
+		return -EINVAL;
+	}
+
+	if (ndev->target_active_prot) {
+		pr_err("there is already an active target\n");
+		return -EBUSY;
+	}
+
+	for (i = 0; i < ndev->n_targets; i++) {
+		if (ndev->targets[i].idx == target_idx) {
+			target = &ndev->targets[i];
+			break;
+		}
+	}
+
+	if (!target) {
+		pr_err("unable to find the selected target\n");
+		return -EINVAL;
+	}
+
+	if (!(target->supported_protocols & (1 << protocol))) {
+		pr_err("target does not support the requested protocol 0x%x\n",
+		       protocol);
+		return -EINVAL;
+	}
+
+	if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) {
+		param.rf_discovery_id = target->idx;
+
+		if (protocol == NFC_PROTO_JEWEL)
+			param.rf_protocol = NCI_RF_PROTOCOL_T1T;
+		else if (protocol == NFC_PROTO_MIFARE)
+			param.rf_protocol = NCI_RF_PROTOCOL_T2T;
+		else if (protocol == NFC_PROTO_FELICA)
+			param.rf_protocol = NCI_RF_PROTOCOL_T3T;
+		else if (protocol == NFC_PROTO_ISO14443)
+			param.rf_protocol = NCI_RF_PROTOCOL_ISO_DEP;
+		else
+			param.rf_protocol = NCI_RF_PROTOCOL_NFC_DEP;
+
+		rc = nci_request(ndev, nci_rf_discover_select_req,
+				 (unsigned long)&param,
+				 msecs_to_jiffies(NCI_RF_DISC_SELECT_TIMEOUT));
+	}
+
+	if (!rc)
+		ndev->target_active_prot = protocol;
+
+	return rc;
+}
+
+static void nci_deactivate_target(struct nfc_dev *nfc_dev, __u32 target_idx)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+	pr_debug("target_idx %d\n", target_idx);
+
+	if (!ndev->target_active_prot) {
+		pr_err("unable to deactivate target, no active target\n");
+		return;
+	}
+
+	ndev->target_active_prot = 0;
+
+	if (atomic_read(&ndev->state) == NCI_POLL_ACTIVE) {
+		nci_request(ndev, nci_rf_deactivate_req, 0,
+			    msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+	}
+}
+
+static int nci_data_exchange(struct nfc_dev *nfc_dev, __u32 target_idx,
+			     struct sk_buff *skb,
+			     data_exchange_cb_t cb, void *cb_context)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	int rc;
+
+	pr_debug("target_idx %d, len %d\n", target_idx, skb->len);
+
+	if (!ndev->target_active_prot) {
+		pr_err("unable to exchange data, no active target\n");
+		return -EINVAL;
+	}
+
+	if (test_and_set_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+		return -EBUSY;
+
+	/* store cb and context to be used on receiving data */
+	ndev->data_exchange_cb = cb;
+	ndev->data_exchange_cb_context = cb_context;
+
+	rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb);
+	if (rc)
+		clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
+
+	return rc;
+}
+
+static struct nfc_ops nci_nfc_ops = {
+	.dev_up = nci_dev_up,
+	.dev_down = nci_dev_down,
+	.start_poll = nci_start_poll,
+	.stop_poll = nci_stop_poll,
+	.activate_target = nci_activate_target,
+	.deactivate_target = nci_deactivate_target,
+	.data_exchange = nci_data_exchange,
+};
+
+/* ---- Interface to NCI drivers ---- */
+
+/**
+ * nci_allocate_device - allocate a new nci device
+ *
+ * @ops: device operations
+ * @supported_protocols: NFC protocols supported by the device
+ */
+struct nci_dev *nci_allocate_device(struct nci_ops *ops,
+				    __u32 supported_protocols,
+				    int tx_headroom, int tx_tailroom)
+{
+	struct nci_dev *ndev;
+
+	pr_debug("supported_protocols 0x%x\n", supported_protocols);
+
+	if (!ops->open || !ops->close || !ops->send)
+		return NULL;
+
+	if (!supported_protocols)
+		return NULL;
+
+	ndev = kzalloc(sizeof(struct nci_dev), GFP_KERNEL);
+	if (!ndev)
+		return NULL;
+
+	ndev->ops = ops;
+	ndev->tx_headroom = tx_headroom;
+	ndev->tx_tailroom = tx_tailroom;
+
+	ndev->nfc_dev = nfc_allocate_device(&nci_nfc_ops,
+					    supported_protocols,
+					    tx_headroom + NCI_DATA_HDR_SIZE,
+					    tx_tailroom);
+	if (!ndev->nfc_dev)
+		goto free_exit;
+
+	nfc_set_drvdata(ndev->nfc_dev, ndev);
+
+	return ndev;
+
+free_exit:
+	kfree(ndev);
+	return NULL;
+}
+EXPORT_SYMBOL(nci_allocate_device);
+
+/**
+ * nci_free_device - deallocate nci device
+ *
+ * @ndev: The nci device to deallocate
+ */
+void nci_free_device(struct nci_dev *ndev)
+{
+	nfc_free_device(ndev->nfc_dev);
+	kfree(ndev);
+}
+EXPORT_SYMBOL(nci_free_device);
+
+/**
+ * nci_register_device - register a nci device in the nfc subsystem
+ *
+ * @dev: The nci device to register
+ */
+int nci_register_device(struct nci_dev *ndev)
+{
+	int rc;
+	struct device *dev = &ndev->nfc_dev->dev;
+	char name[32];
+
+	rc = nfc_register_device(ndev->nfc_dev);
+	if (rc)
+		goto exit;
+
+	ndev->flags = 0;
+
+	INIT_WORK(&ndev->cmd_work, nci_cmd_work);
+	snprintf(name, sizeof(name), "%s_nci_cmd_wq", dev_name(dev));
+	ndev->cmd_wq = create_singlethread_workqueue(name);
+	if (!ndev->cmd_wq) {
+		rc = -ENOMEM;
+		goto unreg_exit;
+	}
+
+	INIT_WORK(&ndev->rx_work, nci_rx_work);
+	snprintf(name, sizeof(name), "%s_nci_rx_wq", dev_name(dev));
+	ndev->rx_wq = create_singlethread_workqueue(name);
+	if (!ndev->rx_wq) {
+		rc = -ENOMEM;
+		goto destroy_cmd_wq_exit;
+	}
+
+	INIT_WORK(&ndev->tx_work, nci_tx_work);
+	snprintf(name, sizeof(name), "%s_nci_tx_wq", dev_name(dev));
+	ndev->tx_wq = create_singlethread_workqueue(name);
+	if (!ndev->tx_wq) {
+		rc = -ENOMEM;
+		goto destroy_rx_wq_exit;
+	}
+
+	skb_queue_head_init(&ndev->cmd_q);
+	skb_queue_head_init(&ndev->rx_q);
+	skb_queue_head_init(&ndev->tx_q);
+
+	setup_timer(&ndev->cmd_timer, nci_cmd_timer,
+		    (unsigned long) ndev);
+	setup_timer(&ndev->data_timer, nci_data_timer,
+		    (unsigned long) ndev);
+
+	mutex_init(&ndev->req_lock);
+
+	goto exit;
+
+destroy_rx_wq_exit:
+	destroy_workqueue(ndev->rx_wq);
+
+destroy_cmd_wq_exit:
+	destroy_workqueue(ndev->cmd_wq);
+
+unreg_exit:
+	nfc_unregister_device(ndev->nfc_dev);
+
+exit:
+	return rc;
+}
+EXPORT_SYMBOL(nci_register_device);
+
+/**
+ * nci_unregister_device - unregister a nci device in the nfc subsystem
+ *
+ * @dev: The nci device to unregister
+ */
+void nci_unregister_device(struct nci_dev *ndev)
+{
+	nci_close_device(ndev);
+
+	destroy_workqueue(ndev->cmd_wq);
+	destroy_workqueue(ndev->rx_wq);
+	destroy_workqueue(ndev->tx_wq);
+
+	nfc_unregister_device(ndev->nfc_dev);
+}
+EXPORT_SYMBOL(nci_unregister_device);
+
+/**
+ * nci_recv_frame - receive frame from NCI drivers
+ *
+ * @skb: The sk_buff to receive
+ */
+int nci_recv_frame(struct sk_buff *skb)
+{
+	struct nci_dev *ndev = (struct nci_dev *) skb->dev;
+
+	pr_debug("len %d\n", skb->len);
+
+	if (!ndev || (!test_bit(NCI_UP, &ndev->flags)
+		      && !test_bit(NCI_INIT, &ndev->flags))) {
+		kfree_skb(skb);
+		return -ENXIO;
+	}
+
+	/* Queue frame for rx worker thread */
+	skb_queue_tail(&ndev->rx_q, skb);
+	queue_work(ndev->rx_wq, &ndev->rx_work);
+
+	return 0;
+}
+EXPORT_SYMBOL(nci_recv_frame);
+
+static int nci_send_frame(struct sk_buff *skb)
+{
+	struct nci_dev *ndev = (struct nci_dev *) skb->dev;
+
+	pr_debug("len %d\n", skb->len);
+
+	if (!ndev) {
+		kfree_skb(skb);
+		return -ENODEV;
+	}
+
+	/* Get rid of skb owner, prior to sending to the driver. */
+	skb_orphan(skb);
+
+	return ndev->ops->send(skb);
+}
+
+/* Send NCI command */
+int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
+{
+	struct nci_ctrl_hdr *hdr;
+	struct sk_buff *skb;
+
+	pr_debug("opcode 0x%x, plen %d\n", opcode, plen);
+
+	skb = nci_skb_alloc(ndev, (NCI_CTRL_HDR_SIZE + plen), GFP_KERNEL);
+	if (!skb) {
+		pr_err("no memory for command\n");
+		return -ENOMEM;
+	}
+
+	hdr = (struct nci_ctrl_hdr *) skb_put(skb, NCI_CTRL_HDR_SIZE);
+	hdr->gid = nci_opcode_gid(opcode);
+	hdr->oid = nci_opcode_oid(opcode);
+	hdr->plen = plen;
+
+	nci_mt_set((__u8 *)hdr, NCI_MT_CMD_PKT);
+	nci_pbf_set((__u8 *)hdr, NCI_PBF_LAST);
+
+	if (plen)
+		memcpy(skb_put(skb, plen), payload, plen);
+
+	skb->dev = (void *) ndev;
+
+	skb_queue_tail(&ndev->cmd_q, skb);
+	queue_work(ndev->cmd_wq, &ndev->cmd_work);
+
+	return 0;
+}
+
+/* ---- NCI TX Data worker thread ---- */
+
+static void nci_tx_work(struct work_struct *work)
+{
+	struct nci_dev *ndev = container_of(work, struct nci_dev, tx_work);
+	struct sk_buff *skb;
+
+	pr_debug("credits_cnt %d\n", atomic_read(&ndev->credits_cnt));
+
+	/* Send queued tx data */
+	while (atomic_read(&ndev->credits_cnt)) {
+		skb = skb_dequeue(&ndev->tx_q);
+		if (!skb)
+			return;
+
+		/* Check if data flow control is used */
+		if (atomic_read(&ndev->credits_cnt) !=
+		    NCI_DATA_FLOW_CONTROL_NOT_USED)
+			atomic_dec(&ndev->credits_cnt);
+
+		pr_debug("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
+			 nci_pbf(skb->data),
+			 nci_conn_id(skb->data),
+			 nci_plen(skb->data));
+
+		nci_send_frame(skb);
+
+		mod_timer(&ndev->data_timer,
+			  jiffies + msecs_to_jiffies(NCI_DATA_TIMEOUT));
+	}
+}
+
+/* ----- NCI RX worker thread (data & control) ----- */
+
+static void nci_rx_work(struct work_struct *work)
+{
+	struct nci_dev *ndev = container_of(work, struct nci_dev, rx_work);
+	struct sk_buff *skb;
+
+	while ((skb = skb_dequeue(&ndev->rx_q))) {
+		/* Process frame */
+		switch (nci_mt(skb->data)) {
+		case NCI_MT_RSP_PKT:
+			nci_rsp_packet(ndev, skb);
+			break;
+
+		case NCI_MT_NTF_PKT:
+			nci_ntf_packet(ndev, skb);
+			break;
+
+		case NCI_MT_DATA_PKT:
+			nci_rx_data_packet(ndev, skb);
+			break;
+
+		default:
+			pr_err("unknown MT 0x%x\n", nci_mt(skb->data));
+			kfree_skb(skb);
+			break;
+		}
+	}
+
+	/* check if a data exchange timout has occurred */
+	if (test_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags)) {
+		/* complete the data exchange transaction, if exists */
+		if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+			nci_data_exchange_complete(ndev, NULL, -ETIMEDOUT);
+
+		clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
+	}
+}
+
+/* ----- NCI TX CMD worker thread ----- */
+
+static void nci_cmd_work(struct work_struct *work)
+{
+	struct nci_dev *ndev = container_of(work, struct nci_dev, cmd_work);
+	struct sk_buff *skb;
+
+	pr_debug("cmd_cnt %d\n", atomic_read(&ndev->cmd_cnt));
+
+	/* Send queued command */
+	if (atomic_read(&ndev->cmd_cnt)) {
+		skb = skb_dequeue(&ndev->cmd_q);
+		if (!skb)
+			return;
+
+		atomic_dec(&ndev->cmd_cnt);
+
+		pr_debug("NCI TX: MT=cmd, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
+			 nci_pbf(skb->data),
+			 nci_opcode_gid(nci_opcode(skb->data)),
+			 nci_opcode_oid(nci_opcode(skb->data)),
+			 nci_plen(skb->data));
+
+		nci_send_frame(skb);
+
+		mod_timer(&ndev->cmd_timer,
+			  jiffies + msecs_to_jiffies(NCI_CMD_TIMEOUT));
+	}
+}
diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c
new file mode 100644
index 00000000..a0bc3263
--- /dev/null
+++ b/net/nfc/nci/data.c
@@ -0,0 +1,254 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/wait.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+/* Complete data exchange transaction and forward skb to nfc core */
+void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
+				int err)
+{
+	data_exchange_cb_t cb = ndev->data_exchange_cb;
+	void *cb_context = ndev->data_exchange_cb_context;
+
+	pr_debug("len %d, err %d\n", skb ? skb->len : 0, err);
+
+	/* data exchange is complete, stop the data timer */
+	del_timer_sync(&ndev->data_timer);
+	clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
+
+	if (cb) {
+		ndev->data_exchange_cb = NULL;
+		ndev->data_exchange_cb_context = 0;
+
+		/* forward skb to nfc core */
+		cb(cb_context, skb, err);
+	} else if (skb) {
+		pr_err("no rx callback, dropping rx data...\n");
+
+		/* no waiting callback, free skb */
+		kfree_skb(skb);
+	}
+
+	clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
+}
+
+/* ----------------- NCI TX Data ----------------- */
+
+static inline void nci_push_data_hdr(struct nci_dev *ndev,
+				     __u8 conn_id,
+				     struct sk_buff *skb,
+				     __u8 pbf)
+{
+	struct nci_data_hdr *hdr;
+	int plen = skb->len;
+
+	hdr = (struct nci_data_hdr *) skb_push(skb, NCI_DATA_HDR_SIZE);
+	hdr->conn_id = conn_id;
+	hdr->rfu = 0;
+	hdr->plen = plen;
+
+	nci_mt_set((__u8 *)hdr, NCI_MT_DATA_PKT);
+	nci_pbf_set((__u8 *)hdr, pbf);
+
+	skb->dev = (void *) ndev;
+}
+
+static int nci_queue_tx_data_frags(struct nci_dev *ndev,
+				   __u8 conn_id,
+				   struct sk_buff *skb) {
+	int total_len = skb->len;
+	unsigned char *data = skb->data;
+	unsigned long flags;
+	struct sk_buff_head frags_q;
+	struct sk_buff *skb_frag;
+	int frag_len;
+	int rc = 0;
+
+	pr_debug("conn_id 0x%x, total_len %d\n", conn_id, total_len);
+
+	__skb_queue_head_init(&frags_q);
+
+	while (total_len) {
+		frag_len =
+			min_t(int, total_len, ndev->max_data_pkt_payload_size);
+
+		skb_frag = nci_skb_alloc(ndev,
+					 (NCI_DATA_HDR_SIZE + frag_len),
+					 GFP_KERNEL);
+		if (skb_frag == NULL) {
+			rc = -ENOMEM;
+			goto free_exit;
+		}
+		skb_reserve(skb_frag, NCI_DATA_HDR_SIZE);
+
+		/* first, copy the data */
+		memcpy(skb_put(skb_frag, frag_len), data, frag_len);
+
+		/* second, set the header */
+		nci_push_data_hdr(ndev, conn_id, skb_frag,
+				  ((total_len == frag_len) ?
+				   (NCI_PBF_LAST) : (NCI_PBF_CONT)));
+
+		__skb_queue_tail(&frags_q, skb_frag);
+
+		data += frag_len;
+		total_len -= frag_len;
+
+		pr_debug("frag_len %d, remaining total_len %d\n",
+			 frag_len, total_len);
+	}
+
+	/* queue all fragments atomically */
+	spin_lock_irqsave(&ndev->tx_q.lock, flags);
+
+	while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
+		__skb_queue_tail(&ndev->tx_q, skb_frag);
+
+	spin_unlock_irqrestore(&ndev->tx_q.lock, flags);
+
+	/* free the original skb */
+	kfree_skb(skb);
+
+	goto exit;
+
+free_exit:
+	while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
+		kfree_skb(skb_frag);
+
+exit:
+	return rc;
+}
+
+/* Send NCI data */
+int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
+{
+	int rc = 0;
+
+	pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len);
+
+	/* check if the packet need to be fragmented */
+	if (skb->len <= ndev->max_data_pkt_payload_size) {
+		/* no need to fragment packet */
+		nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST);
+
+		skb_queue_tail(&ndev->tx_q, skb);
+	} else {
+		/* fragment packet and queue the fragments */
+		rc = nci_queue_tx_data_frags(ndev, conn_id, skb);
+		if (rc) {
+			pr_err("failed to fragment tx data packet\n");
+			goto free_exit;
+		}
+	}
+
+	queue_work(ndev->tx_wq, &ndev->tx_work);
+
+	goto exit;
+
+free_exit:
+	kfree_skb(skb);
+
+exit:
+	return rc;
+}
+
+/* ----------------- NCI RX Data ----------------- */
+
+static void nci_add_rx_data_frag(struct nci_dev *ndev,
+				 struct sk_buff *skb,
+				 __u8 pbf)
+{
+	int reassembly_len;
+	int err = 0;
+
+	if (ndev->rx_data_reassembly) {
+		reassembly_len = ndev->rx_data_reassembly->len;
+
+		/* first, make enough room for the already accumulated data */
+		if (skb_cow_head(skb, reassembly_len)) {
+			pr_err("error adding room for accumulated rx data\n");
+
+			kfree_skb(skb);
+			skb = 0;
+
+			kfree_skb(ndev->rx_data_reassembly);
+			ndev->rx_data_reassembly = 0;
+
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		/* second, combine the two fragments */
+		memcpy(skb_push(skb, reassembly_len),
+		       ndev->rx_data_reassembly->data,
+		       reassembly_len);
+
+		/* third, free old reassembly */
+		kfree_skb(ndev->rx_data_reassembly);
+		ndev->rx_data_reassembly = 0;
+	}
+
+	if (pbf == NCI_PBF_CONT) {
+		/* need to wait for next fragment, store skb and exit */
+		ndev->rx_data_reassembly = skb;
+		return;
+	}
+
+exit:
+	nci_data_exchange_complete(ndev, skb, err);
+}
+
+/* Rx Data packet */
+void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u8 pbf = nci_pbf(skb->data);
+
+	pr_debug("len %d\n", skb->len);
+
+	pr_debug("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
+		 nci_pbf(skb->data),
+		 nci_conn_id(skb->data),
+		 nci_plen(skb->data));
+
+	/* strip the nci data header */
+	skb_pull(skb, NCI_DATA_HDR_SIZE);
+
+	if (ndev->target_active_prot == NFC_PROTO_MIFARE) {
+		/* frame I/F => remove the status byte */
+		pr_debug("NFC_PROTO_MIFARE => remove the status byte\n");
+		skb_trim(skb, (skb->len - 1));
+	}
+
+	nci_add_rx_data_frag(ndev, skb, pbf);
+}
diff --git a/net/nfc/nci/lib.c b/net/nfc/nci/lib.c
new file mode 100644
index 00000000..6a63e5eb
--- /dev/null
+++ b/net/nfc/nci/lib.c
@@ -0,0 +1,85 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on lib.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+
+#include <net/nfc/nci.h>
+
+/* NCI status codes to Unix errno mapping */
+int nci_to_errno(__u8 code)
+{
+	switch (code) {
+	case NCI_STATUS_OK:
+		return 0;
+
+	case NCI_STATUS_REJECTED:
+		return -EBUSY;
+
+	case NCI_STATUS_RF_FRAME_CORRUPTED:
+		return -EBADMSG;
+
+	case NCI_STATUS_NOT_INITIALIZED:
+		return -EHOSTDOWN;
+
+	case NCI_STATUS_SYNTAX_ERROR:
+	case NCI_STATUS_SEMANTIC_ERROR:
+	case NCI_STATUS_INVALID_PARAM:
+	case NCI_STATUS_RF_PROTOCOL_ERROR:
+	case NCI_STATUS_NFCEE_PROTOCOL_ERROR:
+		return -EPROTO;
+
+	case NCI_STATUS_UNKNOWN_GID:
+	case NCI_STATUS_UNKNOWN_OID:
+		return -EBADRQC;
+
+	case NCI_STATUS_MESSAGE_SIZE_EXCEEDED:
+		return -EMSGSIZE;
+
+	case NCI_STATUS_DISCOVERY_ALREADY_STARTED:
+		return -EALREADY;
+
+	case NCI_STATUS_DISCOVERY_TARGET_ACTIVATION_FAILED:
+	case NCI_STATUS_NFCEE_INTERFACE_ACTIVATION_FAILED:
+		return -ECONNREFUSED;
+
+	case NCI_STATUS_RF_TRANSMISSION_ERROR:
+	case NCI_STATUS_NFCEE_TRANSMISSION_ERROR:
+		return -ECOMM;
+
+	case NCI_STATUS_RF_TIMEOUT_ERROR:
+	case NCI_STATUS_NFCEE_TIMEOUT_ERROR:
+		return -ETIMEDOUT;
+
+	case NCI_STATUS_FAILED:
+	default:
+		return -ENOSYS;
+	}
+}
+EXPORT_SYMBOL(nci_to_errno);
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
new file mode 100644
index 00000000..e460cf13
--- /dev/null
+++ b/net/nfc/nci/ntf.c
@@ -0,0 +1,555 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on hci_event.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+/* Handle NCI Notification packets */
+
+static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev,
+					     struct sk_buff *skb)
+{
+	struct nci_core_conn_credit_ntf *ntf = (void *) skb->data;
+	int i;
+
+	pr_debug("num_entries %d\n", ntf->num_entries);
+
+	if (ntf->num_entries > NCI_MAX_NUM_CONN)
+		ntf->num_entries = NCI_MAX_NUM_CONN;
+
+	/* update the credits */
+	for (i = 0; i < ntf->num_entries; i++) {
+		ntf->conn_entries[i].conn_id =
+			nci_conn_id(&ntf->conn_entries[i].conn_id);
+
+		pr_debug("entry[%d]: conn_id %d, credits %d\n",
+			 i, ntf->conn_entries[i].conn_id,
+			 ntf->conn_entries[i].credits);
+
+		if (ntf->conn_entries[i].conn_id == NCI_STATIC_RF_CONN_ID) {
+			/* found static rf connection */
+			atomic_add(ntf->conn_entries[i].credits,
+				   &ndev->credits_cnt);
+		}
+	}
+
+	/* trigger the next tx */
+	if (!skb_queue_empty(&ndev->tx_q))
+		queue_work(ndev->tx_wq, &ndev->tx_work);
+}
+
+static void nci_core_generic_error_ntf_packet(struct nci_dev *ndev,
+					      struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) {
+		/* Activation failed, so complete the request
+		   (the state remains the same) */
+		nci_req_complete(ndev, status);
+	}
+}
+
+static void nci_core_conn_intf_error_ntf_packet(struct nci_dev *ndev,
+						struct sk_buff *skb)
+{
+	struct nci_core_intf_error_ntf *ntf = (void *) skb->data;
+
+	ntf->conn_id = nci_conn_id(&ntf->conn_id);
+
+	pr_debug("status 0x%x, conn_id %d\n", ntf->status, ntf->conn_id);
+
+	/* complete the data exchange transaction, if exists */
+	if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+		nci_data_exchange_complete(ndev, NULL, -EIO);
+}
+
+static __u8 *nci_extract_rf_params_nfca_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfca_poll *nfca_poll,
+						     __u8 *data)
+{
+	nfca_poll->sens_res = __le16_to_cpu(*((__u16 *)data));
+	data += 2;
+
+	nfca_poll->nfcid1_len = min_t(__u8, *data++, NFC_NFCID1_MAXSIZE);
+
+	pr_debug("sens_res 0x%x, nfcid1_len %d\n",
+		 nfca_poll->sens_res, nfca_poll->nfcid1_len);
+
+	memcpy(nfca_poll->nfcid1, data, nfca_poll->nfcid1_len);
+	data += nfca_poll->nfcid1_len;
+
+	nfca_poll->sel_res_len = *data++;
+
+	if (nfca_poll->sel_res_len != 0)
+		nfca_poll->sel_res = *data++;
+
+	pr_debug("sel_res_len %d, sel_res 0x%x\n",
+		 nfca_poll->sel_res_len,
+		 nfca_poll->sel_res);
+
+	return data;
+}
+
+static __u8 *nci_extract_rf_params_nfcb_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfcb_poll *nfcb_poll,
+						     __u8 *data)
+{
+	nfcb_poll->sensb_res_len = min_t(__u8, *data++, NFC_SENSB_RES_MAXSIZE);
+
+	pr_debug("sensb_res_len %d\n", nfcb_poll->sensb_res_len);
+
+	memcpy(nfcb_poll->sensb_res, data, nfcb_poll->sensb_res_len);
+	data += nfcb_poll->sensb_res_len;
+
+	return data;
+}
+
+static __u8 *nci_extract_rf_params_nfcf_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfcf_poll *nfcf_poll,
+						     __u8 *data)
+{
+	nfcf_poll->bit_rate = *data++;
+	nfcf_poll->sensf_res_len = min_t(__u8, *data++, NFC_SENSF_RES_MAXSIZE);
+
+	pr_debug("bit_rate %d, sensf_res_len %d\n",
+		 nfcf_poll->bit_rate, nfcf_poll->sensf_res_len);
+
+	memcpy(nfcf_poll->sensf_res, data, nfcf_poll->sensf_res_len);
+	data += nfcf_poll->sensf_res_len;
+
+	return data;
+}
+
+static int nci_add_new_protocol(struct nci_dev *ndev,
+				struct nfc_target *target,
+				__u8 rf_protocol,
+				__u8 rf_tech_and_mode,
+				void *params)
+{
+	struct rf_tech_specific_params_nfca_poll *nfca_poll;
+	struct rf_tech_specific_params_nfcb_poll *nfcb_poll;
+	struct rf_tech_specific_params_nfcf_poll *nfcf_poll;
+	__u32 protocol;
+
+	if (rf_protocol == NCI_RF_PROTOCOL_T2T)
+		protocol = NFC_PROTO_MIFARE_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_ISO_DEP)
+		protocol = NFC_PROTO_ISO14443_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_T3T)
+		protocol = NFC_PROTO_FELICA_MASK;
+	else
+		protocol = 0;
+
+	if (!(protocol & ndev->poll_prots)) {
+		pr_err("the target found does not have the desired protocol\n");
+		return -EPROTO;
+	}
+
+	if (rf_tech_and_mode == NCI_NFC_A_PASSIVE_POLL_MODE) {
+		nfca_poll = (struct rf_tech_specific_params_nfca_poll *)params;
+
+		target->sens_res = nfca_poll->sens_res;
+		target->sel_res = nfca_poll->sel_res;
+		target->nfcid1_len = nfca_poll->nfcid1_len;
+		if (target->nfcid1_len > 0) {
+			memcpy(target->nfcid1, nfca_poll->nfcid1,
+			       target->nfcid1_len);
+		}
+	} else if (rf_tech_and_mode == NCI_NFC_B_PASSIVE_POLL_MODE) {
+		nfcb_poll = (struct rf_tech_specific_params_nfcb_poll *)params;
+
+		target->sensb_res_len = nfcb_poll->sensb_res_len;
+		if (target->sensb_res_len > 0) {
+			memcpy(target->sensb_res, nfcb_poll->sensb_res,
+			       target->sensb_res_len);
+		}
+	} else if (rf_tech_and_mode == NCI_NFC_F_PASSIVE_POLL_MODE) {
+		nfcf_poll = (struct rf_tech_specific_params_nfcf_poll *)params;
+
+		target->sensf_res_len = nfcf_poll->sensf_res_len;
+		if (target->sensf_res_len > 0) {
+			memcpy(target->sensf_res, nfcf_poll->sensf_res,
+			       target->sensf_res_len);
+		}
+	} else {
+		pr_err("unsupported rf_tech_and_mode 0x%x\n", rf_tech_and_mode);
+		return -EPROTO;
+	}
+
+	target->supported_protocols |= protocol;
+
+	pr_debug("protocol 0x%x\n", protocol);
+
+	return 0;
+}
+
+static void nci_add_new_target(struct nci_dev *ndev,
+			       struct nci_rf_discover_ntf *ntf)
+{
+	struct nfc_target *target;
+	int i, rc;
+
+	for (i = 0; i < ndev->n_targets; i++) {
+		target = &ndev->targets[i];
+		if (target->idx == ntf->rf_discovery_id) {
+			/* This target already exists, add the new protocol */
+			nci_add_new_protocol(ndev, target, ntf->rf_protocol,
+					     ntf->rf_tech_and_mode,
+					     &ntf->rf_tech_specific_params);
+			return;
+		}
+	}
+
+	/* This is a new target, check if we've enough room */
+	if (ndev->n_targets == NCI_MAX_DISCOVERED_TARGETS) {
+		pr_debug("not enough room, ignoring new target...\n");
+		return;
+	}
+
+	target = &ndev->targets[ndev->n_targets];
+
+	rc = nci_add_new_protocol(ndev, target, ntf->rf_protocol,
+				  ntf->rf_tech_and_mode,
+				  &ntf->rf_tech_specific_params);
+	if (!rc) {
+		target->idx = ntf->rf_discovery_id;
+		ndev->n_targets++;
+
+		pr_debug("target_idx %d, n_targets %d\n", target->idx,
+			 ndev->n_targets);
+	}
+}
+
+void nci_clear_target_list(struct nci_dev *ndev)
+{
+	memset(ndev->targets, 0,
+	       (sizeof(struct nfc_target)*NCI_MAX_DISCOVERED_TARGETS));
+
+	ndev->n_targets = 0;
+}
+
+static void nci_rf_discover_ntf_packet(struct nci_dev *ndev,
+				       struct sk_buff *skb)
+{
+	struct nci_rf_discover_ntf ntf;
+	__u8 *data = skb->data;
+	bool add_target = true;
+
+	ntf.rf_discovery_id = *data++;
+	ntf.rf_protocol = *data++;
+	ntf.rf_tech_and_mode = *data++;
+	ntf.rf_tech_specific_params_len = *data++;
+
+	pr_debug("rf_discovery_id %d\n", ntf.rf_discovery_id);
+	pr_debug("rf_protocol 0x%x\n", ntf.rf_protocol);
+	pr_debug("rf_tech_and_mode 0x%x\n", ntf.rf_tech_and_mode);
+	pr_debug("rf_tech_specific_params_len %d\n",
+		 ntf.rf_tech_specific_params_len);
+
+	if (ntf.rf_tech_specific_params_len > 0) {
+		switch (ntf.rf_tech_and_mode) {
+		case NCI_NFC_A_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfca_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfca_poll), data);
+			break;
+
+		case NCI_NFC_B_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcb_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcb_poll), data);
+			break;
+
+		case NCI_NFC_F_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcf_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcf_poll), data);
+			break;
+
+		default:
+			pr_err("unsupported rf_tech_and_mode 0x%x\n",
+			       ntf.rf_tech_and_mode);
+			data += ntf.rf_tech_specific_params_len;
+			add_target = false;
+		}
+	}
+
+	ntf.ntf_type = *data++;
+	pr_debug("ntf_type %d\n", ntf.ntf_type);
+
+	if (add_target == true)
+		nci_add_new_target(ndev, &ntf);
+
+	if (ntf.ntf_type == NCI_DISCOVER_NTF_TYPE_MORE) {
+		atomic_set(&ndev->state, NCI_W4_ALL_DISCOVERIES);
+	} else {
+		atomic_set(&ndev->state, NCI_W4_HOST_SELECT);
+		nfc_targets_found(ndev->nfc_dev, ndev->targets,
+				  ndev->n_targets);
+	}
+}
+
+static int nci_extract_activation_params_iso_dep(struct nci_dev *ndev,
+			struct nci_rf_intf_activated_ntf *ntf, __u8 *data)
+{
+	struct activation_params_nfca_poll_iso_dep *nfca_poll;
+	struct activation_params_nfcb_poll_iso_dep *nfcb_poll;
+
+	switch (ntf->activation_rf_tech_and_mode) {
+	case NCI_NFC_A_PASSIVE_POLL_MODE:
+		nfca_poll = &ntf->activation_params.nfca_poll_iso_dep;
+		nfca_poll->rats_res_len = min_t(__u8, *data++, 20);
+		pr_debug("rats_res_len %d\n", nfca_poll->rats_res_len);
+		if (nfca_poll->rats_res_len > 0) {
+			memcpy(nfca_poll->rats_res,
+			       data, nfca_poll->rats_res_len);
+		}
+		break;
+
+	case NCI_NFC_B_PASSIVE_POLL_MODE:
+		nfcb_poll = &ntf->activation_params.nfcb_poll_iso_dep;
+		nfcb_poll->attrib_res_len = min_t(__u8, *data++, 50);
+		pr_debug("attrib_res_len %d\n", nfcb_poll->attrib_res_len);
+		if (nfcb_poll->attrib_res_len > 0) {
+			memcpy(nfcb_poll->attrib_res,
+			       data, nfcb_poll->attrib_res_len);
+		}
+		break;
+
+	default:
+		pr_err("unsupported activation_rf_tech_and_mode 0x%x\n",
+		       ntf->activation_rf_tech_and_mode);
+		return NCI_STATUS_RF_PROTOCOL_ERROR;
+	}
+
+	return NCI_STATUS_OK;
+}
+
+static void nci_target_auto_activated(struct nci_dev *ndev,
+				      struct nci_rf_intf_activated_ntf *ntf)
+{
+	struct nfc_target *target;
+	int rc;
+
+	target = &ndev->targets[ndev->n_targets];
+
+	rc = nci_add_new_protocol(ndev, target, ntf->rf_protocol,
+				  ntf->activation_rf_tech_and_mode,
+				  &ntf->rf_tech_specific_params);
+	if (rc)
+		return;
+
+	target->idx = ntf->rf_discovery_id;
+	ndev->n_targets++;
+
+	pr_debug("target_idx %d, n_targets %d\n", target->idx, ndev->n_targets);
+
+	nfc_targets_found(ndev->nfc_dev, ndev->targets, ndev->n_targets);
+}
+
+static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev,
+					     struct sk_buff *skb)
+{
+	struct nci_rf_intf_activated_ntf ntf;
+	__u8 *data = skb->data;
+	int err = NCI_STATUS_OK;
+
+	ntf.rf_discovery_id = *data++;
+	ntf.rf_interface = *data++;
+	ntf.rf_protocol = *data++;
+	ntf.activation_rf_tech_and_mode = *data++;
+	ntf.max_data_pkt_payload_size = *data++;
+	ntf.initial_num_credits = *data++;
+	ntf.rf_tech_specific_params_len = *data++;
+
+	pr_debug("rf_discovery_id %d\n", ntf.rf_discovery_id);
+	pr_debug("rf_interface 0x%x\n", ntf.rf_interface);
+	pr_debug("rf_protocol 0x%x\n", ntf.rf_protocol);
+	pr_debug("activation_rf_tech_and_mode 0x%x\n",
+		 ntf.activation_rf_tech_and_mode);
+	pr_debug("max_data_pkt_payload_size 0x%x\n",
+		 ntf.max_data_pkt_payload_size);
+	pr_debug("initial_num_credits 0x%x\n",
+		 ntf.initial_num_credits);
+	pr_debug("rf_tech_specific_params_len %d\n",
+		 ntf.rf_tech_specific_params_len);
+
+	if (ntf.rf_tech_specific_params_len > 0) {
+		switch (ntf.activation_rf_tech_and_mode) {
+		case NCI_NFC_A_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfca_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfca_poll), data);
+			break;
+
+		case NCI_NFC_B_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcb_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcb_poll), data);
+			break;
+
+		case NCI_NFC_F_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcf_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcf_poll), data);
+			break;
+
+		default:
+			pr_err("unsupported activation_rf_tech_and_mode 0x%x\n",
+			       ntf.activation_rf_tech_and_mode);
+			err = NCI_STATUS_RF_PROTOCOL_ERROR;
+			goto exit;
+		}
+	}
+
+	ntf.data_exch_rf_tech_and_mode = *data++;
+	ntf.data_exch_tx_bit_rate = *data++;
+	ntf.data_exch_rx_bit_rate = *data++;
+	ntf.activation_params_len = *data++;
+
+	pr_debug("data_exch_rf_tech_and_mode 0x%x\n",
+		 ntf.data_exch_rf_tech_and_mode);
+	pr_debug("data_exch_tx_bit_rate 0x%x\n", ntf.data_exch_tx_bit_rate);
+	pr_debug("data_exch_rx_bit_rate 0x%x\n", ntf.data_exch_rx_bit_rate);
+	pr_debug("activation_params_len %d\n", ntf.activation_params_len);
+
+	if (ntf.activation_params_len > 0) {
+		switch (ntf.rf_interface) {
+		case NCI_RF_INTERFACE_ISO_DEP:
+			err = nci_extract_activation_params_iso_dep(ndev,
+								    &ntf, data);
+			break;
+
+		case NCI_RF_INTERFACE_FRAME:
+			/* no activation params */
+			break;
+
+		default:
+			pr_err("unsupported rf_interface 0x%x\n",
+			       ntf.rf_interface);
+			err = NCI_STATUS_RF_PROTOCOL_ERROR;
+			break;
+		}
+	}
+
+exit:
+	if (err == NCI_STATUS_OK) {
+		ndev->max_data_pkt_payload_size = ntf.max_data_pkt_payload_size;
+		ndev->initial_num_credits = ntf.initial_num_credits;
+
+		/* set the available credits to initial value */
+		atomic_set(&ndev->credits_cnt, ndev->initial_num_credits);
+	}
+
+	if (atomic_read(&ndev->state) == NCI_DISCOVERY) {
+		/* A single target was found and activated automatically */
+		atomic_set(&ndev->state, NCI_POLL_ACTIVE);
+		if (err == NCI_STATUS_OK)
+			nci_target_auto_activated(ndev, &ntf);
+	} else {	/* ndev->state == NCI_W4_HOST_SELECT */
+		/* A selected target was activated, so complete the request */
+		atomic_set(&ndev->state, NCI_POLL_ACTIVE);
+		nci_req_complete(ndev, err);
+	}
+}
+
+static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
+					 struct sk_buff *skb)
+{
+	struct nci_rf_deactivate_ntf *ntf = (void *) skb->data;
+
+	pr_debug("entry, type 0x%x, reason 0x%x\n", ntf->type, ntf->reason);
+
+	/* drop tx data queue */
+	skb_queue_purge(&ndev->tx_q);
+
+	/* drop partial rx data packet */
+	if (ndev->rx_data_reassembly) {
+		kfree_skb(ndev->rx_data_reassembly);
+		ndev->rx_data_reassembly = 0;
+	}
+
+	/* complete the data exchange transaction, if exists */
+	if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+		nci_data_exchange_complete(ndev, NULL, -EIO);
+
+	nci_clear_target_list(ndev);
+	atomic_set(&ndev->state, NCI_IDLE);
+	nci_req_complete(ndev, NCI_STATUS_OK);
+}
+
+void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u16 ntf_opcode = nci_opcode(skb->data);
+
+	pr_debug("NCI RX: MT=ntf, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
+		 nci_pbf(skb->data),
+		 nci_opcode_gid(ntf_opcode),
+		 nci_opcode_oid(ntf_opcode),
+		 nci_plen(skb->data));
+
+	/* strip the nci control header */
+	skb_pull(skb, NCI_CTRL_HDR_SIZE);
+
+	switch (ntf_opcode) {
+	case NCI_OP_CORE_CONN_CREDITS_NTF:
+		nci_core_conn_credits_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_GENERIC_ERROR_NTF:
+		nci_core_generic_error_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_INTF_ERROR_NTF:
+		nci_core_conn_intf_error_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_NTF:
+		nci_rf_discover_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_INTF_ACTIVATED_NTF:
+		nci_rf_intf_activated_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DEACTIVATE_NTF:
+		nci_rf_deactivate_ntf_packet(ndev, skb);
+		break;
+
+	default:
+		pr_err("unknown ntf opcode 0x%x\n", ntf_opcode);
+		break;
+	}
+
+	kfree_skb(skb);
+}
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
new file mode 100644
index 00000000..3003c339
--- /dev/null
+++ b/net/nfc/nci/rsp.c
@@ -0,0 +1,224 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on hci_event.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+
+/* Handle NCI Response packets */
+
+static void nci_core_reset_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct nci_core_reset_rsp *rsp = (void *) skb->data;
+
+	pr_debug("status 0x%x\n", rsp->status);
+
+	if (rsp->status == NCI_STATUS_OK) {
+		ndev->nci_ver = rsp->nci_ver;
+		pr_debug("nci_ver 0x%x, config_status 0x%x\n",
+			 rsp->nci_ver, rsp->config_status);
+	}
+
+	nci_req_complete(ndev, rsp->status);
+}
+
+static void nci_core_init_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct nci_core_init_rsp_1 *rsp_1 = (void *) skb->data;
+	struct nci_core_init_rsp_2 *rsp_2;
+
+	pr_debug("status 0x%x\n", rsp_1->status);
+
+	if (rsp_1->status != NCI_STATUS_OK)
+		goto exit;
+
+	ndev->nfcc_features = __le32_to_cpu(rsp_1->nfcc_features);
+	ndev->num_supported_rf_interfaces = rsp_1->num_supported_rf_interfaces;
+
+	if (ndev->num_supported_rf_interfaces >
+	    NCI_MAX_SUPPORTED_RF_INTERFACES) {
+		ndev->num_supported_rf_interfaces =
+			NCI_MAX_SUPPORTED_RF_INTERFACES;
+	}
+
+	memcpy(ndev->supported_rf_interfaces,
+	       rsp_1->supported_rf_interfaces,
+	       ndev->num_supported_rf_interfaces);
+
+	rsp_2 = (void *) (skb->data + 6 + rsp_1->num_supported_rf_interfaces);
+
+	ndev->max_logical_connections = rsp_2->max_logical_connections;
+	ndev->max_routing_table_size =
+		__le16_to_cpu(rsp_2->max_routing_table_size);
+	ndev->max_ctrl_pkt_payload_len =
+		rsp_2->max_ctrl_pkt_payload_len;
+	ndev->max_size_for_large_params =
+		__le16_to_cpu(rsp_2->max_size_for_large_params);
+	ndev->manufact_id =
+		rsp_2->manufact_id;
+	ndev->manufact_specific_info =
+		__le32_to_cpu(rsp_2->manufact_specific_info);
+
+	pr_debug("nfcc_features 0x%x\n",
+		 ndev->nfcc_features);
+	pr_debug("num_supported_rf_interfaces %d\n",
+		 ndev->num_supported_rf_interfaces);
+	pr_debug("supported_rf_interfaces[0] 0x%x\n",
+		 ndev->supported_rf_interfaces[0]);
+	pr_debug("supported_rf_interfaces[1] 0x%x\n",
+		 ndev->supported_rf_interfaces[1]);
+	pr_debug("supported_rf_interfaces[2] 0x%x\n",
+		 ndev->supported_rf_interfaces[2]);
+	pr_debug("supported_rf_interfaces[3] 0x%x\n",
+		 ndev->supported_rf_interfaces[3]);
+	pr_debug("max_logical_connections %d\n",
+		 ndev->max_logical_connections);
+	pr_debug("max_routing_table_size %d\n",
+		 ndev->max_routing_table_size);
+	pr_debug("max_ctrl_pkt_payload_len %d\n",
+		 ndev->max_ctrl_pkt_payload_len);
+	pr_debug("max_size_for_large_params %d\n",
+		 ndev->max_size_for_large_params);
+	pr_debug("manufact_id 0x%x\n",
+		 ndev->manufact_id);
+	pr_debug("manufact_specific_info 0x%x\n",
+		 ndev->manufact_specific_info);
+
+exit:
+	nci_req_complete(ndev, rsp_1->status);
+}
+
+static void nci_rf_disc_map_rsp_packet(struct nci_dev *ndev,
+				       struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	nci_req_complete(ndev, status);
+}
+
+static void nci_rf_disc_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	if (status == NCI_STATUS_OK)
+		atomic_set(&ndev->state, NCI_DISCOVERY);
+
+	nci_req_complete(ndev, status);
+}
+
+static void nci_rf_disc_select_rsp_packet(struct nci_dev *ndev,
+					  struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	/* Complete the request on intf_activated_ntf or generic_error_ntf */
+	if (status != NCI_STATUS_OK)
+		nci_req_complete(ndev, status);
+}
+
+static void nci_rf_deactivate_rsp_packet(struct nci_dev *ndev,
+					 struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	/* If target was active, complete the request only in deactivate_ntf */
+	if ((status != NCI_STATUS_OK) ||
+	    (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
+		nci_clear_target_list(ndev);
+		atomic_set(&ndev->state, NCI_IDLE);
+		nci_req_complete(ndev, status);
+	}
+}
+
+void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u16 rsp_opcode = nci_opcode(skb->data);
+
+	/* we got a rsp, stop the cmd timer */
+	del_timer(&ndev->cmd_timer);
+
+	pr_debug("NCI RX: MT=rsp, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
+		 nci_pbf(skb->data),
+		 nci_opcode_gid(rsp_opcode),
+		 nci_opcode_oid(rsp_opcode),
+		 nci_plen(skb->data));
+
+	/* strip the nci control header */
+	skb_pull(skb, NCI_CTRL_HDR_SIZE);
+
+	switch (rsp_opcode) {
+	case NCI_OP_CORE_RESET_RSP:
+		nci_core_reset_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_INIT_RSP:
+		nci_core_init_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_MAP_RSP:
+		nci_rf_disc_map_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_RSP:
+		nci_rf_disc_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_SELECT_RSP:
+		nci_rf_disc_select_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DEACTIVATE_RSP:
+		nci_rf_deactivate_rsp_packet(ndev, skb);
+		break;
+
+	default:
+		pr_err("unknown rsp opcode 0x%x\n", rsp_opcode);
+		break;
+	}
+
+	kfree_skb(skb);
+
+	/* trigger the next cmd */
+	atomic_set(&ndev->cmd_cnt, 1);
+	if (!skb_queue_empty(&ndev->cmd_q))
+		queue_work(ndev->cmd_wq, &ndev->cmd_work);
+}
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
new file mode 100644
index 00000000..6404052d
--- /dev/null
+++ b/net/nfc/netlink.c
@@ -0,0 +1,713 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <net/genetlink.h>
+#include <linux/nfc.h>
+#include <linux/slab.h>
+
+#include "nfc.h"
+
+static struct genl_multicast_group nfc_genl_event_mcgrp = {
+	.name = NFC_GENL_MCAST_EVENT_NAME,
+};
+
+struct genl_family nfc_genl_family = {
+	.id = GENL_ID_GENERATE,
+	.hdrsize = 0,
+	.name = NFC_GENL_NAME,
+	.version = NFC_GENL_VERSION,
+	.maxattr = NFC_ATTR_MAX,
+};
+
+static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = {
+	[NFC_ATTR_DEVICE_INDEX] = { .type = NLA_U32 },
+	[NFC_ATTR_DEVICE_NAME] = { .type = NLA_STRING,
+				.len = NFC_DEVICE_NAME_MAXSIZE },
+	[NFC_ATTR_PROTOCOLS] = { .type = NLA_U32 },
+	[NFC_ATTR_COMM_MODE] = { .type = NLA_U8 },
+	[NFC_ATTR_RF_MODE] = { .type = NLA_U8 },
+	[NFC_ATTR_DEVICE_POWERED] = { .type = NLA_U8 },
+};
+
+static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target,
+				struct netlink_callback *cb, int flags)
+{
+	void *hdr;
+
+	hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
+			  &nfc_genl_family, flags, NFC_CMD_GET_TARGET);
+	if (!hdr)
+		return -EMSGSIZE;
+
+	genl_dump_check_consistent(cb, hdr, &nfc_genl_family);
+
+	NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target->idx);
+	NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, target->supported_protocols);
+	NLA_PUT_U16(msg, NFC_ATTR_TARGET_SENS_RES, target->sens_res);
+	NLA_PUT_U8(msg, NFC_ATTR_TARGET_SEL_RES, target->sel_res);
+	if (target->nfcid1_len > 0)
+		NLA_PUT(msg, NFC_ATTR_TARGET_NFCID1, target->nfcid1_len,
+			target->nfcid1);
+	if (target->sensb_res_len > 0)
+		NLA_PUT(msg, NFC_ATTR_TARGET_SENSB_RES, target->sensb_res_len,
+			target->sensb_res);
+	if (target->sensf_res_len > 0)
+		NLA_PUT(msg, NFC_ATTR_TARGET_SENSF_RES, target->sensf_res_len,
+			target->sensf_res);
+
+	return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+	return -EMSGSIZE;
+}
+
+static struct nfc_dev *__get_device_from_cb(struct netlink_callback *cb)
+{
+	struct nfc_dev *dev;
+	int rc;
+	u32 idx;
+
+	rc = nlmsg_parse(cb->nlh, GENL_HDRLEN + nfc_genl_family.hdrsize,
+			 nfc_genl_family.attrbuf,
+			 nfc_genl_family.maxattr,
+			 nfc_genl_policy);
+	if (rc < 0)
+		return ERR_PTR(rc);
+
+	if (!nfc_genl_family.attrbuf[NFC_ATTR_DEVICE_INDEX])
+		return ERR_PTR(-EINVAL);
+
+	idx = nla_get_u32(nfc_genl_family.attrbuf[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return ERR_PTR(-ENODEV);
+
+	return dev;
+}
+
+static int nfc_genl_dump_targets(struct sk_buff *skb,
+				 struct netlink_callback *cb)
+{
+	int i = cb->args[0];
+	struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+	int rc;
+
+	if (!dev) {
+		dev = __get_device_from_cb(cb);
+		if (IS_ERR(dev))
+			return PTR_ERR(dev);
+
+		cb->args[1] = (long) dev;
+	}
+
+	spin_lock_bh(&dev->targets_lock);
+
+	cb->seq = dev->targets_generation;
+
+	while (i < dev->n_targets) {
+		rc = nfc_genl_send_target(skb, &dev->targets[i], cb,
+					  NLM_F_MULTI);
+		if (rc < 0)
+			break;
+
+		i++;
+	}
+
+	spin_unlock_bh(&dev->targets_lock);
+
+	cb->args[0] = i;
+
+	return skb->len;
+}
+
+static int nfc_genl_dump_targets_done(struct netlink_callback *cb)
+{
+	struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+
+	if (dev)
+		nfc_put_device(dev);
+
+	return 0;
+}
+
+int nfc_genl_targets_found(struct nfc_dev *dev)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	dev->genl_data.poll_req_pid = 0;
+
+	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_TARGETS_FOUND);
+	if (!hdr)
+		goto free_msg;
+
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx);
+
+	genlmsg_end(msg, hdr);
+
+	return genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC);
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+free_msg:
+	nlmsg_free(msg);
+	return -EMSGSIZE;
+}
+
+int nfc_genl_device_added(struct nfc_dev *dev)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_DEVICE_ADDED);
+	if (!hdr)
+		goto free_msg;
+
+	NLA_PUT_STRING(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev));
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx);
+	NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols);
+	NLA_PUT_U8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up);
+
+	genlmsg_end(msg, hdr);
+
+	genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_KERNEL);
+
+	return 0;
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+free_msg:
+	nlmsg_free(msg);
+	return -EMSGSIZE;
+}
+
+int nfc_genl_device_removed(struct nfc_dev *dev)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_DEVICE_REMOVED);
+	if (!hdr)
+		goto free_msg;
+
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx);
+
+	genlmsg_end(msg, hdr);
+
+	genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_KERNEL);
+
+	return 0;
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+free_msg:
+	nlmsg_free(msg);
+	return -EMSGSIZE;
+}
+
+static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
+				u32 pid, u32 seq,
+				struct netlink_callback *cb,
+				int flags)
+{
+	void *hdr;
+
+	hdr = genlmsg_put(msg, pid, seq, &nfc_genl_family, flags,
+			  NFC_CMD_GET_DEVICE);
+	if (!hdr)
+		return -EMSGSIZE;
+
+	if (cb)
+		genl_dump_check_consistent(cb, hdr, &nfc_genl_family);
+
+	NLA_PUT_STRING(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev));
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx);
+	NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols);
+	NLA_PUT_U8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up);
+
+	return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+	return -EMSGSIZE;
+}
+
+static int nfc_genl_dump_devices(struct sk_buff *skb,
+				 struct netlink_callback *cb)
+{
+	struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
+	struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+	bool first_call = false;
+
+	if (!iter) {
+		first_call = true;
+		iter = kmalloc(sizeof(struct class_dev_iter), GFP_KERNEL);
+		if (!iter)
+			return -ENOMEM;
+		cb->args[0] = (long) iter;
+	}
+
+	mutex_lock(&nfc_devlist_mutex);
+
+	cb->seq = nfc_devlist_generation;
+
+	if (first_call) {
+		nfc_device_iter_init(iter);
+		dev = nfc_device_iter_next(iter);
+	}
+
+	while (dev) {
+		int rc;
+
+		rc = nfc_genl_send_device(skb, dev, NETLINK_CB(cb->skb).pid,
+					  cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
+		if (rc < 0)
+			break;
+
+		dev = nfc_device_iter_next(iter);
+	}
+
+	mutex_unlock(&nfc_devlist_mutex);
+
+	cb->args[1] = (long) dev;
+
+	return skb->len;
+}
+
+static int nfc_genl_dump_devices_done(struct netlink_callback *cb)
+{
+	struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
+
+	nfc_device_iter_exit(iter);
+	kfree(iter);
+
+	return 0;
+}
+
+int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx,
+			       u8 comm_mode, u8 rf_mode)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	pr_debug("DEP link is up\n");
+
+	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, NFC_CMD_DEP_LINK_UP);
+	if (!hdr)
+		goto free_msg;
+
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx);
+	if (rf_mode == NFC_RF_INITIATOR)
+		NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx);
+	NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, comm_mode);
+	NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, rf_mode);
+
+	genlmsg_end(msg, hdr);
+
+	dev->dep_link_up = true;
+
+	genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC);
+
+	return 0;
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+free_msg:
+	nlmsg_free(msg);
+	return -EMSGSIZE;
+}
+
+int nfc_genl_dep_link_down_event(struct nfc_dev *dev)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	pr_debug("DEP link is down\n");
+
+	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_CMD_DEP_LINK_DOWN);
+	if (!hdr)
+		goto free_msg;
+
+	NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx);
+
+	genlmsg_end(msg, hdr);
+
+	genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC);
+
+	return 0;
+
+nla_put_failure:
+	genlmsg_cancel(msg, hdr);
+free_msg:
+	nlmsg_free(msg);
+	return -EMSGSIZE;
+}
+
+static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info)
+{
+	struct sk_buff *msg;
+	struct nfc_dev *dev;
+	u32 idx;
+	int rc = -ENOBUFS;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!msg) {
+		rc = -ENOMEM;
+		goto out_putdev;
+	}
+
+	rc = nfc_genl_send_device(msg, dev, info->snd_pid, info->snd_seq,
+				  NULL, 0);
+	if (rc < 0)
+		goto out_free;
+
+	nfc_put_device(dev);
+
+	return genlmsg_reply(msg, info);
+
+out_free:
+	nlmsg_free(msg);
+out_putdev:
+	nfc_put_device(dev);
+	return rc;
+}
+
+static int nfc_genl_dev_up(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	int rc;
+	u32 idx;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	rc = nfc_dev_up(dev);
+
+	nfc_put_device(dev);
+	return rc;
+}
+
+static int nfc_genl_dev_down(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	int rc;
+	u32 idx;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	rc = nfc_dev_down(dev);
+
+	nfc_put_device(dev);
+	return rc;
+}
+
+static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	int rc;
+	u32 idx;
+	u32 protocols;
+
+	pr_debug("Poll start\n");
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+	    !info->attrs[NFC_ATTR_PROTOCOLS])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+	protocols = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->genl_data.genl_data_mutex);
+
+	rc = nfc_start_poll(dev, protocols);
+	if (!rc)
+		dev->genl_data.poll_req_pid = info->snd_pid;
+
+	mutex_unlock(&dev->genl_data.genl_data_mutex);
+
+	nfc_put_device(dev);
+	return rc;
+}
+
+static int nfc_genl_stop_poll(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	int rc;
+	u32 idx;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->genl_data.genl_data_mutex);
+
+	if (dev->genl_data.poll_req_pid != info->snd_pid) {
+		rc = -EBUSY;
+		goto out;
+	}
+
+	rc = nfc_stop_poll(dev);
+	dev->genl_data.poll_req_pid = 0;
+
+out:
+	mutex_unlock(&dev->genl_data.genl_data_mutex);
+	nfc_put_device(dev);
+	return rc;
+}
+
+static int nfc_genl_dep_link_up(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	int rc, tgt_idx;
+	u32 idx;
+	u8 comm;
+
+	pr_debug("DEP link up\n");
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+	    !info->attrs[NFC_ATTR_COMM_MODE])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+	if (!info->attrs[NFC_ATTR_TARGET_INDEX])
+		tgt_idx = NFC_TARGET_IDX_ANY;
+	else
+		tgt_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
+
+	comm = nla_get_u8(info->attrs[NFC_ATTR_COMM_MODE]);
+
+	if (comm != NFC_COMM_ACTIVE && comm != NFC_COMM_PASSIVE)
+		return -EINVAL;
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	rc = nfc_dep_link_up(dev, tgt_idx, comm);
+
+	nfc_put_device(dev);
+
+	return rc;
+}
+
+static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	int rc;
+	u32 idx;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	rc = nfc_dep_link_down(dev);
+
+	nfc_put_device(dev);
+	return rc;
+}
+
+static struct genl_ops nfc_genl_ops[] = {
+	{
+		.cmd = NFC_CMD_GET_DEVICE,
+		.doit = nfc_genl_get_device,
+		.dumpit = nfc_genl_dump_devices,
+		.done = nfc_genl_dump_devices_done,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_DEV_UP,
+		.doit = nfc_genl_dev_up,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_DEV_DOWN,
+		.doit = nfc_genl_dev_down,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_START_POLL,
+		.doit = nfc_genl_start_poll,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_STOP_POLL,
+		.doit = nfc_genl_stop_poll,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_DEP_LINK_UP,
+		.doit = nfc_genl_dep_link_up,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_DEP_LINK_DOWN,
+		.doit = nfc_genl_dep_link_down,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_GET_TARGET,
+		.dumpit = nfc_genl_dump_targets,
+		.done = nfc_genl_dump_targets_done,
+		.policy = nfc_genl_policy,
+	},
+};
+
+static int nfc_genl_rcv_nl_event(struct notifier_block *this,
+				 unsigned long event, void *ptr)
+{
+	struct netlink_notify *n = ptr;
+	struct class_dev_iter iter;
+	struct nfc_dev *dev;
+
+	if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC)
+		goto out;
+
+	pr_debug("NETLINK_URELEASE event from id %d\n", n->pid);
+
+	nfc_device_iter_init(&iter);
+	dev = nfc_device_iter_next(&iter);
+
+	while (dev) {
+		if (dev->genl_data.poll_req_pid == n->pid) {
+			nfc_stop_poll(dev);
+			dev->genl_data.poll_req_pid = 0;
+		}
+		dev = nfc_device_iter_next(&iter);
+	}
+
+	nfc_device_iter_exit(&iter);
+
+out:
+	return NOTIFY_DONE;
+}
+
+void nfc_genl_data_init(struct nfc_genl_data *genl_data)
+{
+	genl_data->poll_req_pid = 0;
+	mutex_init(&genl_data->genl_data_mutex);
+}
+
+void nfc_genl_data_exit(struct nfc_genl_data *genl_data)
+{
+	mutex_destroy(&genl_data->genl_data_mutex);
+}
+
+static struct notifier_block nl_notifier = {
+	.notifier_call  = nfc_genl_rcv_nl_event,
+};
+
+/**
+ * nfc_genl_init() - Initialize netlink interface
+ *
+ * This initialization function registers the nfc netlink family.
+ */
+int __init nfc_genl_init(void)
+{
+	int rc;
+
+	rc = genl_register_family_with_ops(&nfc_genl_family, nfc_genl_ops,
+					   ARRAY_SIZE(nfc_genl_ops));
+	if (rc)
+		return rc;
+
+	rc = genl_register_mc_group(&nfc_genl_family, &nfc_genl_event_mcgrp);
+
+	netlink_register_notifier(&nl_notifier);
+
+	return rc;
+}
+
+/**
+ * nfc_genl_exit() - Deinitialize netlink interface
+ *
+ * This exit function unregisters the nfc netlink family.
+ */
+void nfc_genl_exit(void)
+{
+	netlink_unregister_notifier(&nl_notifier);
+	genl_unregister_family(&nfc_genl_family);
+}
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
new file mode 100644
index 00000000..ec8794c1
--- /dev/null
+++ b/net/nfc/nfc.h
@@ -0,0 +1,175 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#ifndef __LOCAL_NFC_H
+#define __LOCAL_NFC_H
+
+#include <net/nfc/nfc.h>
+#include <net/sock.h>
+
+struct nfc_protocol {
+	int id;
+	struct proto *proto;
+	struct module *owner;
+	int (*create)(struct net *net, struct socket *sock,
+		      const struct nfc_protocol *nfc_proto);
+};
+
+struct nfc_rawsock {
+	struct sock sk;
+	struct nfc_dev *dev;
+	u32 target_idx;
+	struct work_struct tx_work;
+	bool tx_work_scheduled;
+};
+#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))
+
+#ifdef CONFIG_NFC_LLCP
+
+void nfc_llcp_mac_is_down(struct nfc_dev *dev);
+void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+			u8 comm_mode, u8 rf_mode);
+int nfc_llcp_register_device(struct nfc_dev *dev);
+void nfc_llcp_unregister_device(struct nfc_dev *dev);
+int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len);
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len);
+int __init nfc_llcp_init(void);
+void nfc_llcp_exit(void);
+
+#else
+
+static inline void nfc_llcp_mac_is_down(struct nfc_dev *dev)
+{
+}
+
+static inline void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+				      u8 comm_mode, u8 rf_mode)
+{
+}
+
+static inline int nfc_llcp_register_device(struct nfc_dev *dev)
+{
+	return 0;
+}
+
+static inline void nfc_llcp_unregister_device(struct nfc_dev *dev)
+{
+}
+
+static inline int nfc_llcp_set_remote_gb(struct nfc_dev *dev,
+					 u8 *gb, u8 gb_len)
+{
+	return 0;
+}
+
+static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *gb_len)
+{
+	*gb_len = 0;
+	return NULL;
+}
+
+static inline int nfc_llcp_init(void)
+{
+	return 0;
+}
+
+static inline void nfc_llcp_exit(void)
+{
+}
+
+#endif
+
+int __init rawsock_init(void);
+void rawsock_exit(void);
+
+int __init af_nfc_init(void);
+void af_nfc_exit(void);
+int nfc_proto_register(const struct nfc_protocol *nfc_proto);
+void nfc_proto_unregister(const struct nfc_protocol *nfc_proto);
+
+extern int nfc_devlist_generation;
+extern struct mutex nfc_devlist_mutex;
+
+int __init nfc_genl_init(void);
+void nfc_genl_exit(void);
+
+void nfc_genl_data_init(struct nfc_genl_data *genl_data);
+void nfc_genl_data_exit(struct nfc_genl_data *genl_data);
+
+int nfc_genl_targets_found(struct nfc_dev *dev);
+
+int nfc_genl_device_added(struct nfc_dev *dev);
+int nfc_genl_device_removed(struct nfc_dev *dev);
+
+int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx,
+			       u8 comm_mode, u8 rf_mode);
+int nfc_genl_dep_link_down_event(struct nfc_dev *dev);
+
+struct nfc_dev *nfc_get_device(unsigned idx);
+
+static inline void nfc_put_device(struct nfc_dev *dev)
+{
+	put_device(&dev->dev);
+}
+
+static inline void nfc_device_iter_init(struct class_dev_iter *iter)
+{
+	class_dev_iter_init(iter, &nfc_class, NULL, NULL);
+}
+
+static inline struct nfc_dev *nfc_device_iter_next(struct class_dev_iter *iter)
+{
+	struct device *d = class_dev_iter_next(iter);
+	if (!d)
+		return NULL;
+
+	return to_nfc_dev(d);
+}
+
+static inline void nfc_device_iter_exit(struct class_dev_iter *iter)
+{
+	class_dev_iter_exit(iter);
+}
+
+int nfc_dev_up(struct nfc_dev *dev);
+
+int nfc_dev_down(struct nfc_dev *dev);
+
+int nfc_start_poll(struct nfc_dev *dev, u32 protocols);
+
+int nfc_stop_poll(struct nfc_dev *dev);
+
+int nfc_dep_link_up(struct nfc_dev *dev, int target_idx, u8 comm_mode);
+
+int nfc_dep_link_down(struct nfc_dev *dev);
+
+int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol);
+
+int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx);
+
+int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
+		      data_exchange_cb_t cb, void *cb_context);
+
+#endif /* __LOCAL_NFC_H */
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
new file mode 100644
index 00000000..e879dce5
--- /dev/null
+++ b/net/nfc/rawsock.c
@@ -0,0 +1,344 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <net/tcp_states.h>
+#include <linux/nfc.h>
+#include <linux/export.h>
+
+#include "nfc.h"
+
+static void rawsock_write_queue_purge(struct sock *sk)
+{
+	pr_debug("sk=%p\n", sk);
+
+	spin_lock_bh(&sk->sk_write_queue.lock);
+	__skb_queue_purge(&sk->sk_write_queue);
+	nfc_rawsock(sk)->tx_work_scheduled = false;
+	spin_unlock_bh(&sk->sk_write_queue.lock);
+}
+
+static void rawsock_report_error(struct sock *sk, int err)
+{
+	pr_debug("sk=%p err=%d\n", sk, err);
+
+	sk->sk_shutdown = SHUTDOWN_MASK;
+	sk->sk_err = -err;
+	sk->sk_error_report(sk);
+
+	rawsock_write_queue_purge(sk);
+}
+
+static int rawsock_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	pr_debug("sock=%p sk=%p\n", sock, sk);
+
+	if (!sk)
+		return 0;
+
+	sock_orphan(sk);
+	sock_put(sk);
+
+	return 0;
+}
+
+static int rawsock_connect(struct socket *sock, struct sockaddr *_addr,
+			   int len, int flags)
+{
+	struct sock *sk = sock->sk;
+	struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr;
+	struct nfc_dev *dev;
+	int rc = 0;
+
+	pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags);
+
+	if (!addr || len < sizeof(struct sockaddr_nfc) ||
+	    addr->sa_family != AF_NFC)
+		return -EINVAL;
+
+	pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n",
+		 addr->dev_idx, addr->target_idx, addr->nfc_protocol);
+
+	lock_sock(sk);
+
+	if (sock->state == SS_CONNECTED) {
+		rc = -EISCONN;
+		goto error;
+	}
+
+	dev = nfc_get_device(addr->dev_idx);
+	if (!dev) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol);
+	if (rc)
+		goto put_dev;
+
+	nfc_rawsock(sk)->dev = dev;
+	nfc_rawsock(sk)->target_idx = addr->target_idx;
+	sock->state = SS_CONNECTED;
+	sk->sk_state = TCP_ESTABLISHED;
+	sk->sk_state_change(sk);
+
+	release_sock(sk);
+	return 0;
+
+put_dev:
+	nfc_put_device(dev);
+error:
+	release_sock(sk);
+	return rc;
+}
+
+static int rawsock_add_header(struct sk_buff *skb)
+{
+	*skb_push(skb, NFC_HEADER_SIZE) = 0;
+
+	return 0;
+}
+
+static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
+					   int err)
+{
+	struct sock *sk = (struct sock *) context;
+
+	BUG_ON(in_irq());
+
+	pr_debug("sk=%p err=%d\n", sk, err);
+
+	if (err)
+		goto error;
+
+	err = rawsock_add_header(skb);
+	if (err)
+		goto error;
+
+	err = sock_queue_rcv_skb(sk, skb);
+	if (err)
+		goto error;
+
+	spin_lock_bh(&sk->sk_write_queue.lock);
+	if (!skb_queue_empty(&sk->sk_write_queue))
+		schedule_work(&nfc_rawsock(sk)->tx_work);
+	else
+		nfc_rawsock(sk)->tx_work_scheduled = false;
+	spin_unlock_bh(&sk->sk_write_queue.lock);
+
+	sock_put(sk);
+	return;
+
+error:
+	rawsock_report_error(sk, err);
+	sock_put(sk);
+}
+
+static void rawsock_tx_work(struct work_struct *work)
+{
+	struct sock *sk = to_rawsock_sk(work);
+	struct nfc_dev *dev = nfc_rawsock(sk)->dev;
+	u32 target_idx = nfc_rawsock(sk)->target_idx;
+	struct sk_buff *skb;
+	int rc;
+
+	pr_debug("sk=%p target_idx=%u\n", sk, target_idx);
+
+	if (sk->sk_shutdown & SEND_SHUTDOWN) {
+		rawsock_write_queue_purge(sk);
+		return;
+	}
+
+	skb = skb_dequeue(&sk->sk_write_queue);
+
+	sock_hold(sk);
+	rc = nfc_data_exchange(dev, target_idx, skb,
+			       rawsock_data_exchange_complete, sk);
+	if (rc) {
+		rawsock_report_error(sk, rc);
+		sock_put(sk);
+	}
+}
+
+static int rawsock_sendmsg(struct kiocb *iocb, struct socket *sock,
+			   struct msghdr *msg, size_t len)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_dev *dev = nfc_rawsock(sk)->dev;
+	struct sk_buff *skb;
+	int rc;
+
+	pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len);
+
+	if (msg->msg_namelen)
+		return -EOPNOTSUPP;
+
+	if (sock->state != SS_CONNECTED)
+		return -ENOTCONN;
+
+	skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc);
+	if (skb == NULL)
+		return rc;
+
+	rc = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
+	if (rc < 0) {
+		kfree_skb(skb);
+		return rc;
+	}
+
+	spin_lock_bh(&sk->sk_write_queue.lock);
+	__skb_queue_tail(&sk->sk_write_queue, skb);
+	if (!nfc_rawsock(sk)->tx_work_scheduled) {
+		schedule_work(&nfc_rawsock(sk)->tx_work);
+		nfc_rawsock(sk)->tx_work_scheduled = true;
+	}
+	spin_unlock_bh(&sk->sk_write_queue.lock);
+
+	return len;
+}
+
+static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock,
+			   struct msghdr *msg, size_t len, int flags)
+{
+	int noblock = flags & MSG_DONTWAIT;
+	struct sock *sk = sock->sk;
+	struct sk_buff *skb;
+	int copied;
+	int rc;
+
+	pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
+
+	skb = skb_recv_datagram(sk, flags, noblock, &rc);
+	if (!skb)
+		return rc;
+
+	msg->msg_namelen = 0;
+
+	copied = skb->len;
+	if (len < copied) {
+		msg->msg_flags |= MSG_TRUNC;
+		copied = len;
+	}
+
+	rc = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
+
+	skb_free_datagram(sk, skb);
+
+	return rc ? : copied;
+}
+
+
+static const struct proto_ops rawsock_ops = {
+	.family         = PF_NFC,
+	.owner          = THIS_MODULE,
+	.release        = rawsock_release,
+	.bind           = sock_no_bind,
+	.connect        = rawsock_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        = rawsock_sendmsg,
+	.recvmsg        = rawsock_recvmsg,
+	.mmap           = sock_no_mmap,
+};
+
+static void rawsock_destruct(struct sock *sk)
+{
+	pr_debug("sk=%p\n", sk);
+
+	if (sk->sk_state == TCP_ESTABLISHED) {
+		nfc_deactivate_target(nfc_rawsock(sk)->dev,
+				      nfc_rawsock(sk)->target_idx);
+		nfc_put_device(nfc_rawsock(sk)->dev);
+	}
+
+	skb_queue_purge(&sk->sk_receive_queue);
+
+	if (!sock_flag(sk, SOCK_DEAD)) {
+		pr_err("Freeing alive NFC raw socket %p\n", sk);
+		return;
+	}
+}
+
+static int rawsock_create(struct net *net, struct socket *sock,
+			  const struct nfc_protocol *nfc_proto)
+{
+	struct sock *sk;
+
+	pr_debug("sock=%p\n", sock);
+
+	if (sock->type != SOCK_SEQPACKET)
+		return -ESOCKTNOSUPPORT;
+
+	sock->ops = &rawsock_ops;
+
+	sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto);
+	if (!sk)
+		return -ENOMEM;
+
+	sock_init_data(sock, sk);
+	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;
+
+	return 0;
+}
+
+static struct proto rawsock_proto = {
+	.name     = "NFC_RAW",
+	.owner    = THIS_MODULE,
+	.obj_size = sizeof(struct nfc_rawsock),
+};
+
+static const struct nfc_protocol rawsock_nfc_proto = {
+	.id	  = NFC_SOCKPROTO_RAW,
+	.proto    = &rawsock_proto,
+	.owner    = THIS_MODULE,
+	.create   = rawsock_create
+};
+
+int __init rawsock_init(void)
+{
+	int rc;
+
+	rc = nfc_proto_register(&rawsock_nfc_proto);
+
+	return rc;
+}
+
+void rawsock_exit(void)
+{
+	nfc_proto_unregister(&rawsock_nfc_proto);
+}