fishHEADmaster

28 files changed, 11569 insertions, 0 deletions

diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
new file mode 100644
index 0000000..865a6cd
--- /dev/null
+++ b/net/nfc/Kconfig
@@ -0,0 +1,22 @@
+#
+# NFC sybsystem configuration
+#
+
+menuconfig NFC
+	depends on !BACKPORT_KERNEL_3_1
+	depends on NET
+	depends on RFKILL || !RFKILL
+	tristate "NFC subsystem support"
+	depends on m
+	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/hci/Kconfig"
+
+source "drivers/nfc/Kconfig"
diff --git a/net/nfc/Makefile b/net/nfc/Makefile
new file mode 100644
index 0000000..3103074
--- /dev/null
+++ b/net/nfc/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for the Linux NFC subsystem.
+#
+
+obj-$(CPTCFG_NFC) += nfc.o
+obj-$(CPTCFG_NFC_NCI) += nci/
+obj-$(CPTCFG_NFC_HCI) += hci/
+
+nfc-objs := core.o netlink.o af_nfc.o rawsock.o llcp_core.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 0000000..9d68441
--- /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 0000000..40d2527
--- /dev/null
+++ b/net/nfc/core.c
@@ -0,0 +1,981 @@
+/*
+ * 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/rfkill.h>
+#include <linux/nfc.h>
+
+#include <net/genetlink.h>
+
+#include "nfc.h"
+
+#define VERSION "0.1"
+
+#define NFC_CHECK_PRES_FREQ_MS	2000
+
+int nfc_devlist_generation;
+DEFINE_MUTEX(nfc_devlist_mutex);
+
+/* NFC device ID bitmap */
+static DEFINE_IDA(nfc_index_ida);
+
+/**
+ * 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 (dev->rfkill && rfkill_blocked(dev->rfkill)) {
+		rc = -ERFKILL;
+		goto error;
+	}
+
+	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->active_target) {
+		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;
+}
+
+static int nfc_rfkill_set_block(void *data, bool blocked)
+{
+	struct nfc_dev *dev = data;
+
+	pr_debug("%s blocked %d", dev_name(&dev->dev), blocked);
+
+	if (!blocked)
+		return 0;
+
+	nfc_dev_down(dev);
+
+	return 0;
+}
+
+static const struct rfkill_ops nfc_rfkill_ops = {
+	.set_block = nfc_rfkill_set_block,
+};
+
+/**
+ * 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 im_protocols, u32 tm_protocols)
+{
+	int rc;
+
+	pr_debug("dev_name %s initiator protocols 0x%x target protocols 0x%x\n",
+		 dev_name(&dev->dev), im_protocols, tm_protocols);
+
+	if (!im_protocols && !tm_protocols)
+		return -EINVAL;
+
+	device_lock(&dev->dev);
+
+	if (!device_is_registered(&dev->dev)) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (!dev->dev_up) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (dev->polling) {
+		rc = -EBUSY;
+		goto error;
+	}
+
+	rc = dev->ops->start_poll(dev, im_protocols, tm_protocols);
+	if (!rc) {
+		dev->polling = true;
+		dev->rf_mode = NFC_RF_NONE;
+	}
+
+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;
+	dev->rf_mode = NFC_RF_NONE;
+
+error:
+	device_unlock(&dev->dev);
+	return rc;
+}
+
+static struct nfc_target *nfc_find_target(struct nfc_dev *dev, u32 target_idx)
+{
+	int i;
+
+	if (dev->n_targets == 0)
+		return NULL;
+
+	for (i = 0; i < dev->n_targets; i++) {
+		if (dev->targets[i].idx == target_idx)
+			return &dev->targets[i];
+	}
+
+	return NULL;
+}
+
+int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode)
+{
+	int rc = 0;
+	u8 *gb;
+	size_t gb_len;
+	struct nfc_target *target;
+
+	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;
+	}
+
+	target = nfc_find_target(dev, target_index);
+	if (target == NULL) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	rc = dev->ops->dep_link_up(dev, target, comm_mode, gb, gb_len);
+	if (!rc) {
+		dev->active_target = target;
+		dev->rf_mode = NFC_RF_INITIATOR;
+	}
+
+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;
+	}
+
+	rc = dev->ops->dep_link_down(dev);
+	if (!rc) {
+		dev->dep_link_up = false;
+		dev->active_target = NULL;
+		dev->rf_mode = NFC_RF_NONE;
+		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;
+
+	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;
+	struct nfc_target *target;
+
+	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;
+	}
+
+	if (dev->active_target) {
+		rc = -EBUSY;
+		goto error;
+	}
+
+	target = nfc_find_target(dev, target_idx);
+	if (target == NULL) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	rc = dev->ops->activate_target(dev, target, protocol);
+	if (!rc) {
+		dev->active_target = target;
+		dev->rf_mode = NFC_RF_INITIATOR;
+
+		if (dev->ops->check_presence && !dev->shutting_down)
+			mod_timer(&dev->check_pres_timer, jiffies +
+				  msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+	}
+
+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;
+	}
+
+	if (dev->active_target == NULL) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	if (dev->active_target->idx != target_idx) {
+		rc = -ENOTCONN;
+		goto error;
+	}
+
+	if (dev->ops->check_presence)
+		del_timer_sync(&dev->check_pres_timer);
+
+	dev->ops->deactivate_target(dev, dev->active_target);
+	dev->active_target = NULL;
+
+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;
+	}
+
+	if (dev->rf_mode == NFC_RF_INITIATOR && dev->active_target != NULL) {
+		if (dev->active_target->idx != target_idx) {
+			rc = -EADDRNOTAVAIL;
+			kfree_skb(skb);
+			goto error;
+		}
+
+		if (dev->ops->check_presence)
+			del_timer_sync(&dev->check_pres_timer);
+
+		rc = dev->ops->im_transceive(dev, dev->active_target, skb, cb,
+					     cb_context);
+
+		if (!rc && dev->ops->check_presence && !dev->shutting_down)
+			mod_timer(&dev->check_pres_timer, jiffies +
+				  msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+	} else if (dev->rf_mode == NFC_RF_TARGET && dev->ops->tm_send != NULL) {
+		rc = dev->ops->tm_send(dev, skb);
+	} else {
+		rc = -ENOTCONN;
+		kfree_skb(skb);
+		goto error;
+	}
+
+
+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);
+
+u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, size_t *gb_len)
+{
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	return nfc_llcp_general_bytes(dev, gb_len);
+}
+EXPORT_SYMBOL(nfc_get_local_general_bytes);
+
+int nfc_tm_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+	/* Only LLCP target mode for now */
+	if (dev->dep_link_up == false) {
+		kfree_skb(skb);
+		return -ENOLINK;
+	}
+
+	return nfc_llcp_data_received(dev, skb);
+}
+EXPORT_SYMBOL(nfc_tm_data_received);
+
+int nfc_tm_activated(struct nfc_dev *dev, u32 protocol, u8 comm_mode,
+		     u8 *gb, size_t gb_len)
+{
+	int rc;
+
+	device_lock(&dev->dev);
+
+	dev->polling = false;
+
+	if (gb != NULL) {
+		rc = nfc_set_remote_general_bytes(dev, gb, gb_len);
+		if (rc < 0)
+			goto out;
+	}
+
+	dev->rf_mode = NFC_RF_TARGET;
+
+	if (protocol == NFC_PROTO_NFC_DEP_MASK)
+		nfc_dep_link_is_up(dev, 0, comm_mode, NFC_RF_TARGET);
+
+	rc = nfc_genl_tm_activated(dev, protocol);
+
+out:
+	device_unlock(&dev->dev);
+
+	return rc;
+}
+EXPORT_SYMBOL(nfc_tm_activated);
+
+int nfc_tm_deactivated(struct nfc_dev *dev)
+{
+	dev->dep_link_up = false;
+	dev->rf_mode = NFC_RF_NONE;
+
+	return nfc_genl_tm_deactivated(dev);
+}
+EXPORT_SYMBOL(nfc_tm_deactivated);
+
+/**
+ * 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.
+ * NOTE: This function can be called with targets=NULL and n_targets=0 to
+ * notify a driver error, meaning that the polling operation cannot complete.
+ * IMPORTANT: this function must not be called from an atomic context.
+ * In addition, it must also not be called from a context that would prevent
+ * the NFC Core to call other nfc ops entry point concurrently.
+ */
+int nfc_targets_found(struct nfc_dev *dev,
+		      struct nfc_target *targets, int n_targets)
+{
+	int i;
+
+	pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets);
+
+	for (i = 0; i < n_targets; i++)
+		targets[i].idx = dev->target_next_idx++;
+
+	device_lock(&dev->dev);
+
+	if (dev->polling == false) {
+		device_unlock(&dev->dev);
+		return 0;
+	}
+
+	dev->polling = false;
+
+	dev->targets_generation++;
+
+	kfree(dev->targets);
+	dev->targets = NULL;
+
+	if (targets) {
+		dev->targets = kmemdup(targets,
+				       n_targets * sizeof(struct nfc_target),
+				       GFP_ATOMIC);
+
+		if (!dev->targets) {
+			dev->n_targets = 0;
+			device_unlock(&dev->dev);
+			return -ENOMEM;
+		}
+	}
+
+	dev->n_targets = n_targets;
+	device_unlock(&dev->dev);
+
+	nfc_genl_targets_found(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_targets_found);
+
+/**
+ * nfc_target_lost - inform that an activated target went out of field
+ *
+ * @dev: The nfc device that had the activated target in field
+ * @target_idx: the nfc index of the target
+ *
+ * The device driver must call this function when the activated target
+ * goes out of the field.
+ * IMPORTANT: this function must not be called from an atomic context.
+ * In addition, it must also not be called from a context that would prevent
+ * the NFC Core to call other nfc ops entry point concurrently.
+ */
+int nfc_target_lost(struct nfc_dev *dev, u32 target_idx)
+{
+	struct nfc_target *tg;
+	int i;
+
+	pr_debug("dev_name %s n_target %d\n", dev_name(&dev->dev), target_idx);
+
+	device_lock(&dev->dev);
+
+	for (i = 0; i < dev->n_targets; i++) {
+		tg = &dev->targets[i];
+		if (tg->idx == target_idx)
+			break;
+	}
+
+	if (i == dev->n_targets) {
+		device_unlock(&dev->dev);
+		return -EINVAL;
+	}
+
+	dev->targets_generation++;
+	dev->n_targets--;
+	dev->active_target = NULL;
+
+	if (dev->n_targets) {
+		memcpy(&dev->targets[i], &dev->targets[i + 1],
+		       (dev->n_targets - i) * sizeof(struct nfc_target));
+	} else {
+		kfree(dev->targets);
+		dev->targets = NULL;
+	}
+
+	device_unlock(&dev->dev);
+
+	nfc_genl_target_lost(dev, target_idx);
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_target_lost);
+
+inline void nfc_driver_failure(struct nfc_dev *dev, int err)
+{
+	nfc_targets_found(dev, NULL, 0);
+}
+EXPORT_SYMBOL(nfc_driver_failure);
+
+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);
+}
+
+static void nfc_check_pres_work(struct work_struct *work)
+{
+	struct nfc_dev *dev = container_of(work, struct nfc_dev,
+					   check_pres_work);
+	int rc;
+
+	device_lock(&dev->dev);
+
+	if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) {
+		rc = dev->ops->check_presence(dev, dev->active_target);
+		if (rc == -EOPNOTSUPP)
+			goto exit;
+		if (rc) {
+			u32 active_target_idx = dev->active_target->idx;
+			device_unlock(&dev->dev);
+			nfc_target_lost(dev, active_target_idx);
+			return;
+		}
+
+		if (!dev->shutting_down)
+			mod_timer(&dev->check_pres_timer, jiffies +
+				  msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+	}
+
+exit:
+	device_unlock(&dev->dev);
+}
+
+static void nfc_check_pres_timeout(unsigned long data)
+{
+	struct nfc_dev *dev = (struct nfc_dev *)data;
+
+	schedule_work(&dev->check_pres_work);
+}
+
+struct class nfc_class = {
+	.name = "nfc",
+	.dev_release = nfc_release,
+};
+EXPORT_SYMBOL(nfc_class);
+
+static int match_idx(struct device *d, const void *data)
+{
+	struct nfc_dev *dev = to_nfc_dev(d);
+	const unsigned int *idx = data;
+
+	return dev->idx == *idx;
+}
+
+struct nfc_dev *nfc_get_device(unsigned int 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,
+				    u32 supported_se,
+				    int tx_headroom, int tx_tailroom)
+{
+	struct nfc_dev *dev;
+
+	if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
+	    !ops->deactivate_target || !ops->im_transceive)
+		return NULL;
+
+	if (!supported_protocols)
+		return NULL;
+
+	dev = kzalloc(sizeof(struct nfc_dev), GFP_KERNEL);
+	if (!dev)
+		return NULL;
+
+	dev->ops = ops;
+	dev->supported_protocols = supported_protocols;
+	dev->supported_se = supported_se;
+	dev->active_se = NFC_SE_NONE;
+	dev->tx_headroom = tx_headroom;
+	dev->tx_tailroom = tx_tailroom;
+
+	nfc_genl_data_init(&dev->genl_data);
+
+	dev->rf_mode = NFC_RF_NONE;
+
+	/* first generation must not be 0 */
+	dev->targets_generation = 1;
+
+	if (ops->check_presence) {
+		init_timer(&dev->check_pres_timer);
+		dev->check_pres_timer.data = (unsigned long)dev;
+		dev->check_pres_timer.function = nfc_check_pres_timeout;
+
+		INIT_WORK(&dev->check_pres_work, nfc_check_pres_work);
+	}
+
+	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));
+
+	dev->idx = ida_simple_get(&nfc_index_ida, 0, 0, GFP_KERNEL);
+	if (dev->idx < 0)
+		return dev->idx;
+
+	dev->dev.class = &nfc_class;
+	dev_set_name(&dev->dev, "nfc%d", dev->idx);
+	device_initialize(&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));
+
+	dev->rfkill = rfkill_alloc(dev_name(&dev->dev), &dev->dev,
+				   RFKILL_TYPE_NFC, &nfc_rfkill_ops, dev);
+	if (dev->rfkill) {
+		if (rfkill_register(dev->rfkill) < 0) {
+			rfkill_destroy(dev->rfkill);
+			dev->rfkill = NULL;
+		}
+	}
+
+	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, id;
+
+	pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+	id = dev->idx;
+
+	if (dev->rfkill) {
+		rfkill_unregister(dev->rfkill);
+		rfkill_destroy(dev->rfkill);
+	}
+
+	if (dev->ops->check_presence) {
+		device_lock(&dev->dev);
+		dev->shutting_down = true;
+		device_unlock(&dev->dev);
+		del_timer_sync(&dev->check_pres_timer);
+		cancel_work_sync(&dev->check_pres_work);
+	}
+
+	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));
+
+	nfc_llcp_unregister_device(dev);
+
+	mutex_lock(&nfc_devlist_mutex);
+	nfc_devlist_generation++;
+	device_del(&dev->dev);
+	mutex_unlock(&nfc_devlist_mutex);
+
+	ida_simple_remove(&nfc_index_ida, id);
+}
+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");
+MODULE_ALIAS_NETPROTO(PF_NFC);
+MODULE_ALIAS_GENL_FAMILY(NFC_GENL_NAME);
diff --git a/net/nfc/hci/Kconfig b/net/nfc/hci/Kconfig
new file mode 100644
index 0000000..aabfc72
--- /dev/null
+++ b/net/nfc/hci/Kconfig
@@ -0,0 +1,18 @@
+config NFC_HCI
+       depends on NFC
+       tristate "NFC HCI implementation"
+       depends on m
+       default n
+       help
+	 Say Y here if you want to build support for a kernel NFC HCI
+	 implementation. This is mostly needed for devices that only process
+	 HCI frames, like for example the NXP pn544.
+
+config NFC_SHDLC
+	depends on NFC_HCI
+	depends on CRC_CCITT
+	bool "SHDLC link layer for HCI based NFC drivers"
+	default n
+	---help---
+	  Say yes if you use an NFC HCI driver that requires SHDLC link layer.
+	  If unsure, say N here.
diff --git a/net/nfc/hci/Makefile b/net/nfc/hci/Makefile
new file mode 100644
index 0000000..0a13989
--- /dev/null
+++ b/net/nfc/hci/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the Linux NFC HCI layer.
+#
+
+obj-$(CPTCFG_NFC_HCI) += hci.o
+
+hci-y			:= core.o hcp.o command.o llc.o llc_nop.o
+hci-$(CPTCFG_NFC_SHDLC) += llc_shdlc.o
diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c
new file mode 100644
index 0000000..64f922b
--- /dev/null
+++ b/net/nfc/hci/command.c
@@ -0,0 +1,384 @@
+/*
+ * Copyright (C) 2012  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) "hci: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+
+#include <net/nfc/hci.h>
+
+#include "hci.h"
+
+static int nfc_hci_execute_cmd_async(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
+			       const u8 *param, size_t param_len,
+			       data_exchange_cb_t cb, void *cb_context)
+{
+	pr_debug("exec cmd async through pipe=%d, cmd=%d, plen=%zd\n", pipe,
+		 cmd, param_len);
+
+	/* TODO: Define hci cmd execution delay. Should it be the same
+	 * for all commands?
+	 */
+	return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_COMMAND, cmd,
+				      param, param_len, cb, cb_context, 3000);
+}
+
+/*
+ * HCI command execution completion callback.
+ * err will be a standard linux error (may be converted from HCI response)
+ * skb contains the response data and must be disposed, or may be NULL if
+ * an error occured
+ */
+static void nfc_hci_execute_cb(void *context, struct sk_buff *skb, int err)
+{
+	struct hcp_exec_waiter *hcp_ew = (struct hcp_exec_waiter *)context;
+
+	pr_debug("HCI Cmd completed with result=%d\n", err);
+
+	hcp_ew->exec_result = err;
+	if (hcp_ew->exec_result == 0)
+		hcp_ew->result_skb = skb;
+	else
+		kfree_skb(skb);
+	hcp_ew->exec_complete = true;
+
+	wake_up(hcp_ew->wq);
+}
+
+static int nfc_hci_execute_cmd(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
+			       const u8 *param, size_t param_len,
+			       struct sk_buff **skb)
+{
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(ew_wq);
+	struct hcp_exec_waiter hcp_ew;
+	hcp_ew.wq = &ew_wq;
+	hcp_ew.exec_complete = false;
+	hcp_ew.result_skb = NULL;
+
+	pr_debug("exec cmd sync through pipe=%d, cmd=%d, plen=%zd\n", pipe,
+		 cmd, param_len);
+
+	/* TODO: Define hci cmd execution delay. Should it be the same
+	 * for all commands?
+	 */
+	hcp_ew.exec_result = nfc_hci_hcp_message_tx(hdev, pipe,
+						    NFC_HCI_HCP_COMMAND, cmd,
+						    param, param_len,
+						    nfc_hci_execute_cb, &hcp_ew,
+						    3000);
+	if (hcp_ew.exec_result < 0)
+		return hcp_ew.exec_result;
+
+	wait_event(ew_wq, hcp_ew.exec_complete == true);
+
+	if (hcp_ew.exec_result == 0) {
+		if (skb)
+			*skb = hcp_ew.result_skb;
+		else
+			kfree_skb(hcp_ew.result_skb);
+	}
+
+	return hcp_ew.exec_result;
+}
+
+int nfc_hci_send_event(struct nfc_hci_dev *hdev, u8 gate, u8 event,
+		       const u8 *param, size_t param_len)
+{
+	u8 pipe;
+
+	pr_debug("%d to gate %d\n", event, gate);
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_EVENT, event,
+				      param, param_len, NULL, NULL, 0);
+}
+EXPORT_SYMBOL(nfc_hci_send_event);
+
+int nfc_hci_send_response(struct nfc_hci_dev *hdev, u8 gate, u8 response,
+			  const u8 *param, size_t param_len)
+{
+	u8 pipe;
+
+	pr_debug("\n");
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_RESPONSE,
+				      response, param, param_len, NULL, NULL,
+				      0);
+}
+EXPORT_SYMBOL(nfc_hci_send_response);
+
+/*
+ * Execute an hci command sent to gate.
+ * skb will contain response data if success. skb can be NULL if you are not
+ * interested by the response.
+ */
+int nfc_hci_send_cmd(struct nfc_hci_dev *hdev, u8 gate, u8 cmd,
+		     const u8 *param, size_t param_len, struct sk_buff **skb)
+{
+	u8 pipe;
+
+	pr_debug("\n");
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_execute_cmd(hdev, pipe, cmd, param, param_len, skb);
+}
+EXPORT_SYMBOL(nfc_hci_send_cmd);
+
+int nfc_hci_send_cmd_async(struct nfc_hci_dev *hdev, u8 gate, u8 cmd,
+			   const u8 *param, size_t param_len,
+			   data_exchange_cb_t cb, void *cb_context)
+{
+	u8 pipe;
+
+	pr_debug("\n");
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_execute_cmd_async(hdev, pipe, cmd, param, param_len,
+					 cb, cb_context);
+}
+EXPORT_SYMBOL(nfc_hci_send_cmd_async);
+
+int nfc_hci_set_param(struct nfc_hci_dev *hdev, u8 gate, u8 idx,
+		      const u8 *param, size_t param_len)
+{
+	int r;
+	u8 *tmp;
+
+	/* TODO ELa: reg idx must be inserted before param, but we don't want
+	 * to ask the caller to do it to keep a simpler API.
+	 * For now, just create a new temporary param buffer. This is far from
+	 * optimal though, and the plan is to modify APIs to pass idx down to
+	 * nfc_hci_hcp_message_tx where the frame is actually built, thereby
+	 * eliminating the need for the temp allocation-copy here.
+	 */
+
+	pr_debug("idx=%d to gate %d\n", idx, gate);
+
+	tmp = kmalloc(1 + param_len, GFP_KERNEL);
+	if (tmp == NULL)
+		return -ENOMEM;
+
+	*tmp = idx;
+	memcpy(tmp + 1, param, param_len);
+
+	r = nfc_hci_send_cmd(hdev, gate, NFC_HCI_ANY_SET_PARAMETER,
+			     tmp, param_len + 1, NULL);
+
+	kfree(tmp);
+
+	return r;
+}
+EXPORT_SYMBOL(nfc_hci_set_param);
+
+int nfc_hci_get_param(struct nfc_hci_dev *hdev, u8 gate, u8 idx,
+		      struct sk_buff **skb)
+{
+	pr_debug("gate=%d regidx=%d\n", gate, idx);
+
+	return nfc_hci_send_cmd(hdev, gate, NFC_HCI_ANY_GET_PARAMETER,
+				&idx, 1, skb);
+}
+EXPORT_SYMBOL(nfc_hci_get_param);
+
+static int nfc_hci_open_pipe(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	struct sk_buff *skb;
+	int r;
+
+	pr_debug("pipe=%d\n", pipe);
+
+	r = nfc_hci_execute_cmd(hdev, pipe, NFC_HCI_ANY_OPEN_PIPE,
+				NULL, 0, &skb);
+	if (r == 0) {
+		/* dest host other than host controller will send
+		 * number of pipes already open on this gate before
+		 * execution. The number can be found in skb->data[0]
+		 */
+		kfree_skb(skb);
+	}
+
+	return r;
+}
+
+static int nfc_hci_close_pipe(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	pr_debug("\n");
+
+	return nfc_hci_execute_cmd(hdev, pipe, NFC_HCI_ANY_CLOSE_PIPE,
+				   NULL, 0, NULL);
+}
+
+static u8 nfc_hci_create_pipe(struct nfc_hci_dev *hdev, u8 dest_host,
+			      u8 dest_gate, int *result)
+{
+	struct sk_buff *skb;
+	struct hci_create_pipe_params params;
+	struct hci_create_pipe_resp *resp;
+	u8 pipe;
+
+	pr_debug("gate=%d\n", dest_gate);
+
+	params.src_gate = NFC_HCI_ADMIN_GATE;
+	params.dest_host = dest_host;
+	params.dest_gate = dest_gate;
+
+	*result = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				      NFC_HCI_ADM_CREATE_PIPE,
+				      (u8 *) &params, sizeof(params), &skb);
+	if (*result < 0)
+		return NFC_HCI_INVALID_PIPE;
+
+	resp = (struct hci_create_pipe_resp *)skb->data;
+	pipe = resp->pipe;
+	kfree_skb(skb);
+
+	pr_debug("pipe created=%d\n", pipe);
+
+	return pipe;
+}
+
+static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	pr_debug("\n");
+
+	return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				   NFC_HCI_ADM_DELETE_PIPE, &pipe, 1, NULL);
+}
+
+static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
+{
+	u8 param[2];
+	size_t param_len = 2;
+
+	/* TODO: Find out what the identity reference data is
+	 * and fill param with it. HCI spec 6.1.3.5 */
+
+	pr_debug("\n");
+
+	if (test_bit(NFC_HCI_QUIRK_SHORT_CLEAR, &hdev->quirks))
+		param_len = 0;
+
+	return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				   NFC_HCI_ADM_CLEAR_ALL_PIPE, param, param_len,
+				   NULL);
+}
+
+int nfc_hci_disconnect_gate(struct nfc_hci_dev *hdev, u8 gate)
+{
+	int r;
+	u8 pipe = hdev->gate2pipe[gate];
+
+	pr_debug("\n");
+
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	r = nfc_hci_close_pipe(hdev, pipe);
+	if (r < 0)
+		return r;
+
+	if (pipe != NFC_HCI_LINK_MGMT_PIPE && pipe != NFC_HCI_ADMIN_PIPE) {
+		r = nfc_hci_delete_pipe(hdev, pipe);
+		if (r < 0)
+			return r;
+	}
+
+	hdev->gate2pipe[gate] = NFC_HCI_INVALID_PIPE;
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_hci_disconnect_gate);
+
+int nfc_hci_disconnect_all_gates(struct nfc_hci_dev *hdev)
+{
+	int r;
+
+	pr_debug("\n");
+
+	r = nfc_hci_clear_all_pipes(hdev);
+	if (r < 0)
+		return r;
+
+	memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_hci_disconnect_all_gates);
+
+int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
+			 u8 pipe)
+{
+	bool pipe_created = false;
+	int r;
+
+	pr_debug("\n");
+
+	if (hdev->gate2pipe[dest_gate] != NFC_HCI_INVALID_PIPE)
+		return -EADDRINUSE;
+
+	if (pipe != NFC_HCI_INVALID_PIPE)
+		goto open_pipe;
+
+	switch (dest_gate) {
+	case NFC_HCI_LINK_MGMT_GATE:
+		pipe = NFC_HCI_LINK_MGMT_PIPE;
+		break;
+	case NFC_HCI_ADMIN_GATE:
+		pipe = NFC_HCI_ADMIN_PIPE;
+		break;
+	default:
+		pipe = nfc_hci_create_pipe(hdev, dest_host, dest_gate, &r);
+		if (pipe == NFC_HCI_INVALID_PIPE)
+			return r;
+		pipe_created = true;
+		break;
+	}
+
+open_pipe:
+	r = nfc_hci_open_pipe(hdev, pipe);
+	if (r < 0) {
+		if (pipe_created)
+			if (nfc_hci_delete_pipe(hdev, pipe) < 0) {
+				/* TODO: Cannot clean by deleting pipe...
+				 * -> inconsistent state */
+			}
+		return r;
+	}
+
+	hdev->gate2pipe[dest_gate] = pipe;
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_hci_connect_gate);
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
new file mode 100644
index 0000000..91020b2
--- /dev/null
+++ b/net/nfc/hci/core.c
@@ -0,0 +1,954 @@
+/*
+ * Copyright (C) 2012  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) "hci: %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 <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+
+#include "hci.h"
+
+/* Largest headroom needed for outgoing HCI commands */
+#define HCI_CMDS_HEADROOM 1
+
+int nfc_hci_result_to_errno(u8 result)
+{
+	switch (result) {
+	case NFC_HCI_ANY_OK:
+		return 0;
+	case NFC_HCI_ANY_E_REG_PAR_UNKNOWN:
+		return -EOPNOTSUPP;
+	case NFC_HCI_ANY_E_TIMEOUT:
+		return -ETIME;
+	default:
+		return -1;
+	}
+}
+EXPORT_SYMBOL(nfc_hci_result_to_errno);
+
+static void nfc_hci_msg_tx_work(struct work_struct *work)
+{
+	struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev,
+						msg_tx_work);
+	struct hci_msg *msg;
+	struct sk_buff *skb;
+	int r = 0;
+
+	mutex_lock(&hdev->msg_tx_mutex);
+	if (hdev->shutting_down)
+		goto exit;
+
+	if (hdev->cmd_pending_msg) {
+		if (timer_pending(&hdev->cmd_timer) == 0) {
+			if (hdev->cmd_pending_msg->cb)
+				hdev->cmd_pending_msg->cb(hdev->
+							  cmd_pending_msg->
+							  cb_context,
+							  NULL,
+							  -ETIME);
+			kfree(hdev->cmd_pending_msg);
+			hdev->cmd_pending_msg = NULL;
+		} else {
+			goto exit;
+		}
+	}
+
+next_msg:
+	if (list_empty(&hdev->msg_tx_queue))
+		goto exit;
+
+	msg = list_first_entry(&hdev->msg_tx_queue, struct hci_msg, msg_l);
+	list_del(&msg->msg_l);
+
+	pr_debug("msg_tx_queue has a cmd to send\n");
+	while ((skb = skb_dequeue(&msg->msg_frags)) != NULL) {
+		r = nfc_llc_xmit_from_hci(hdev->llc, skb);
+		if (r < 0) {
+			kfree_skb(skb);
+			skb_queue_purge(&msg->msg_frags);
+			if (msg->cb)
+				msg->cb(msg->cb_context, NULL, r);
+			kfree(msg);
+			break;
+		}
+	}
+
+	if (r)
+		goto next_msg;
+
+	if (msg->wait_response == false) {
+		kfree(msg);
+		goto next_msg;
+	}
+
+	hdev->cmd_pending_msg = msg;
+	mod_timer(&hdev->cmd_timer, jiffies +
+		  msecs_to_jiffies(hdev->cmd_pending_msg->completion_delay));
+
+exit:
+	mutex_unlock(&hdev->msg_tx_mutex);
+}
+
+static void nfc_hci_msg_rx_work(struct work_struct *work)
+{
+	struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev,
+						msg_rx_work);
+	struct sk_buff *skb;
+	struct hcp_message *message;
+	u8 pipe;
+	u8 type;
+	u8 instruction;
+
+	while ((skb = skb_dequeue(&hdev->msg_rx_queue)) != NULL) {
+		pipe = skb->data[0];
+		skb_pull(skb, NFC_HCI_HCP_PACKET_HEADER_LEN);
+		message = (struct hcp_message *)skb->data;
+		type = HCP_MSG_GET_TYPE(message->header);
+		instruction = HCP_MSG_GET_CMD(message->header);
+		skb_pull(skb, NFC_HCI_HCP_MESSAGE_HEADER_LEN);
+
+		nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, skb);
+	}
+}
+
+static void __nfc_hci_cmd_completion(struct nfc_hci_dev *hdev, int err,
+				     struct sk_buff *skb)
+{
+	del_timer_sync(&hdev->cmd_timer);
+
+	if (hdev->cmd_pending_msg->cb)
+		hdev->cmd_pending_msg->cb(hdev->cmd_pending_msg->cb_context,
+					  skb, err);
+	else
+		kfree_skb(skb);
+
+	kfree(hdev->cmd_pending_msg);
+	hdev->cmd_pending_msg = NULL;
+
+	schedule_work(&hdev->msg_tx_work);
+}
+
+void nfc_hci_resp_received(struct nfc_hci_dev *hdev, u8 result,
+			   struct sk_buff *skb)
+{
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->cmd_pending_msg == NULL) {
+		kfree_skb(skb);
+		goto exit;
+	}
+
+	__nfc_hci_cmd_completion(hdev, nfc_hci_result_to_errno(result), skb);
+
+exit:
+	mutex_unlock(&hdev->msg_tx_mutex);
+}
+
+void nfc_hci_cmd_received(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
+			  struct sk_buff *skb)
+{
+	kfree_skb(skb);
+}
+
+u32 nfc_hci_sak_to_protocol(u8 sak)
+{
+	switch (NFC_HCI_TYPE_A_SEL_PROT(sak)) {
+	case NFC_HCI_TYPE_A_SEL_PROT_MIFARE:
+		return NFC_PROTO_MIFARE_MASK;
+	case NFC_HCI_TYPE_A_SEL_PROT_ISO14443:
+		return NFC_PROTO_ISO14443_MASK;
+	case NFC_HCI_TYPE_A_SEL_PROT_DEP:
+		return NFC_PROTO_NFC_DEP_MASK;
+	case NFC_HCI_TYPE_A_SEL_PROT_ISO14443_DEP:
+		return NFC_PROTO_ISO14443_MASK | NFC_PROTO_NFC_DEP_MASK;
+	default:
+		return 0xffffffff;
+	}
+}
+EXPORT_SYMBOL(nfc_hci_sak_to_protocol);
+
+int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
+{
+	struct nfc_target *targets;
+	struct sk_buff *atqa_skb = NULL;
+	struct sk_buff *sak_skb = NULL;
+	struct sk_buff *uid_skb = NULL;
+	int r;
+
+	pr_debug("from gate %d\n", gate);
+
+	targets = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
+	if (targets == NULL)
+		return -ENOMEM;
+
+	switch (gate) {
+	case NFC_HCI_RF_READER_A_GATE:
+		r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
+				      NFC_HCI_RF_READER_A_ATQA, &atqa_skb);
+		if (r < 0)
+			goto exit;
+
+		r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
+				      NFC_HCI_RF_READER_A_SAK, &sak_skb);
+		if (r < 0)
+			goto exit;
+
+		if (atqa_skb->len != 2 || sak_skb->len != 1) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		targets->supported_protocols =
+				nfc_hci_sak_to_protocol(sak_skb->data[0]);
+		if (targets->supported_protocols == 0xffffffff) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		targets->sens_res = be16_to_cpu(*(u16 *)atqa_skb->data);
+		targets->sel_res = sak_skb->data[0];
+
+		r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
+				      NFC_HCI_RF_READER_A_UID, &uid_skb);
+		if (r < 0)
+			goto exit;
+
+		if (uid_skb->len == 0 || uid_skb->len > NFC_NFCID1_MAXSIZE) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		memcpy(targets->nfcid1, uid_skb->data, uid_skb->len);
+		targets->nfcid1_len = uid_skb->len;
+
+		if (hdev->ops->complete_target_discovered) {
+			r = hdev->ops->complete_target_discovered(hdev, gate,
+								  targets);
+			if (r < 0)
+				goto exit;
+		}
+		break;
+	case NFC_HCI_RF_READER_B_GATE:
+		targets->supported_protocols = NFC_PROTO_ISO14443_B_MASK;
+		break;
+	default:
+		if (hdev->ops->target_from_gate)
+			r = hdev->ops->target_from_gate(hdev, gate, targets);
+		else
+			r = -EPROTO;
+		if (r < 0)
+			goto exit;
+
+		if (hdev->ops->complete_target_discovered) {
+			r = hdev->ops->complete_target_discovered(hdev, gate,
+								  targets);
+			if (r < 0)
+				goto exit;
+		}
+		break;
+	}
+
+	/* if driver set the new gate, we will skip the old one */
+	if (targets->hci_reader_gate == 0x00)
+		targets->hci_reader_gate = gate;
+
+	r = nfc_targets_found(hdev->ndev, targets, 1);
+
+exit:
+	kfree(targets);
+	kfree_skb(atqa_skb);
+	kfree_skb(sak_skb);
+	kfree_skb(uid_skb);
+
+	return r;
+}
+EXPORT_SYMBOL(nfc_hci_target_discovered);
+
+void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
+			    struct sk_buff *skb)
+{
+	int r = 0;
+	u8 gate = nfc_hci_pipe2gate(hdev, pipe);
+
+	if (gate == 0xff) {
+		pr_err("Discarded event %x to unopened pipe %x\n", event, pipe);
+		goto exit;
+	}
+
+	if (hdev->ops->event_received) {
+		r = hdev->ops->event_received(hdev, gate, event, skb);
+		if (r <= 0)
+			goto exit_noskb;
+	}
+
+	switch (event) {
+	case NFC_HCI_EVT_TARGET_DISCOVERED:
+		if (skb->len < 1) {	/* no status data? */
+			r = -EPROTO;
+			goto exit;
+		}
+
+		if (skb->data[0] == 3) {
+			/* TODO: Multiple targets in field, none activated
+			 * poll is supposedly stopped, but there is no
+			 * single target to activate, so nothing to report
+			 * up.
+			 * if we need to restart poll, we must save the
+			 * protocols from the initial poll and reuse here.
+			 */
+		}
+
+		if (skb->data[0] != 0) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		r = nfc_hci_target_discovered(hdev, gate);
+		break;
+	default:
+		pr_info("Discarded unknown event %x to gate %x\n", event, gate);
+		r = -EINVAL;
+		break;
+	}
+
+exit:
+	kfree_skb(skb);
+
+exit_noskb:
+	if (r) {
+		/* TODO: There was an error dispatching the event,
+		 * how to propagate up to nfc core?
+		 */
+	}
+}
+
+static void nfc_hci_cmd_timeout(unsigned long data)
+{
+	struct nfc_hci_dev *hdev = (struct nfc_hci_dev *)data;
+
+	schedule_work(&hdev->msg_tx_work);
+}
+
+static int hci_dev_connect_gates(struct nfc_hci_dev *hdev, u8 gate_count,
+				 struct nfc_hci_gate *gates)
+{
+	int r;
+	while (gate_count--) {
+		r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID,
+					 gates->gate, gates->pipe);
+		if (r < 0)
+			return r;
+		gates++;
+	}
+
+	return 0;
+}
+
+static int hci_dev_session_init(struct nfc_hci_dev *hdev)
+{
+	struct sk_buff *skb = NULL;
+	int r;
+
+	if (hdev->init_data.gates[0].gate != NFC_HCI_ADMIN_GATE)
+		return -EPROTO;
+
+	r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID,
+				 hdev->init_data.gates[0].gate,
+				 hdev->init_data.gates[0].pipe);
+	if (r < 0)
+		goto exit;
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ADMIN_GATE,
+			      NFC_HCI_ADMIN_SESSION_IDENTITY, &skb);
+	if (r < 0)
+		goto disconnect_all;
+
+	if (skb->len && skb->len == strlen(hdev->init_data.session_id))
+		if (memcmp(hdev->init_data.session_id, skb->data,
+			   skb->len) == 0) {
+			/* TODO ELa: restore gate<->pipe table from
+			 * some TBD location.
+			 * note: it doesn't seem possible to get the chip
+			 * currently open gate/pipe table.
+			 * It is only possible to obtain the supported
+			 * gate list.
+			 */
+
+			/* goto exit
+			 * For now, always do a full initialization */
+		}
+
+	r = nfc_hci_disconnect_all_gates(hdev);
+	if (r < 0)
+		goto exit;
+
+	r = hci_dev_connect_gates(hdev, hdev->init_data.gate_count,
+				  hdev->init_data.gates);
+	if (r < 0)
+		goto disconnect_all;
+
+	r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE,
+			      NFC_HCI_ADMIN_SESSION_IDENTITY,
+			      hdev->init_data.session_id,
+			      strlen(hdev->init_data.session_id));
+	if (r == 0)
+		goto exit;
+
+disconnect_all:
+	nfc_hci_disconnect_all_gates(hdev);
+
+exit:
+	kfree_skb(skb);
+
+	return r;
+}
+
+static int hci_dev_version(struct nfc_hci_dev *hdev)
+{
+	int r;
+	struct sk_buff *skb;
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
+			      NFC_HCI_ID_MGMT_VERSION_SW, &skb);
+	if (r == -EOPNOTSUPP) {
+		pr_info("Software/Hardware info not available\n");
+		return 0;
+	}
+	if (r < 0)
+		return r;
+
+	if (skb->len != 3) {
+		kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	hdev->sw_romlib = (skb->data[0] & 0xf0) >> 4;
+	hdev->sw_patch = skb->data[0] & 0x0f;
+	hdev->sw_flashlib_major = skb->data[1];
+	hdev->sw_flashlib_minor = skb->data[2];
+
+	kfree_skb(skb);
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
+			      NFC_HCI_ID_MGMT_VERSION_HW, &skb);
+	if (r < 0)
+		return r;
+
+	if (skb->len != 3) {
+		kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	hdev->hw_derivative = (skb->data[0] & 0xe0) >> 5;
+	hdev->hw_version = skb->data[0] & 0x1f;
+	hdev->hw_mpw = (skb->data[1] & 0xc0) >> 6;
+	hdev->hw_software = skb->data[1] & 0x3f;
+	hdev->hw_bsid = skb->data[2];
+
+	kfree_skb(skb);
+
+	pr_info("SOFTWARE INFO:\n");
+	pr_info("RomLib         : %d\n", hdev->sw_romlib);
+	pr_info("Patch          : %d\n", hdev->sw_patch);
+	pr_info("FlashLib Major : %d\n", hdev->sw_flashlib_major);
+	pr_info("FlashLib Minor : %d\n", hdev->sw_flashlib_minor);
+	pr_info("HARDWARE INFO:\n");
+	pr_info("Derivative     : %d\n", hdev->hw_derivative);
+	pr_info("HW Version     : %d\n", hdev->hw_version);
+	pr_info("#MPW           : %d\n", hdev->hw_mpw);
+	pr_info("Software       : %d\n", hdev->hw_software);
+	pr_info("BSID Version   : %d\n", hdev->hw_bsid);
+
+	return 0;
+}
+
+static int hci_dev_up(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+	int r = 0;
+
+	if (hdev->ops->open) {
+		r = hdev->ops->open(hdev);
+		if (r < 0)
+			return r;
+	}
+
+	r = nfc_llc_start(hdev->llc);
+	if (r < 0)
+		goto exit_close;
+
+	r = hci_dev_session_init(hdev);
+	if (r < 0)
+		goto exit_llc;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
+	if (r < 0)
+		goto exit_llc;
+
+	if (hdev->ops->hci_ready) {
+		r = hdev->ops->hci_ready(hdev);
+		if (r < 0)
+			goto exit_llc;
+	}
+
+	r = hci_dev_version(hdev);
+	if (r < 0)
+		goto exit_llc;
+
+	return 0;
+
+exit_llc:
+	nfc_llc_stop(hdev->llc);
+
+exit_close:
+	if (hdev->ops->close)
+		hdev->ops->close(hdev);
+
+	return r;
+}
+
+static int hci_dev_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	nfc_llc_stop(hdev->llc);
+
+	if (hdev->ops->close)
+		hdev->ops->close(hdev);
+
+	memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+
+	return 0;
+}
+
+static int hci_start_poll(struct nfc_dev *nfc_dev,
+			  u32 im_protocols, u32 tm_protocols)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->start_poll)
+		return hdev->ops->start_poll(hdev, im_protocols, tm_protocols);
+	else
+		return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+					  NFC_HCI_EVT_READER_REQUESTED,
+					  NULL, 0);
+}
+
+static void hci_stop_poll(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			   NFC_HCI_EVT_END_OPERATION, NULL, 0);
+}
+
+static int hci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+				__u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->dep_link_up)
+		return hdev->ops->dep_link_up(hdev, target, comm_mode,
+						gb, gb_len);
+
+	return 0;
+}
+
+static int hci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->dep_link_down)
+		return hdev->ops->dep_link_down(hdev);
+
+	return 0;
+}
+
+static int hci_activate_target(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target, u32 protocol)
+{
+	return 0;
+}
+
+static void hci_deactivate_target(struct nfc_dev *nfc_dev,
+				  struct nfc_target *target)
+{
+}
+
+#define HCI_CB_TYPE_TRANSCEIVE 1
+
+static void hci_transceive_cb(void *context, struct sk_buff *skb, int err)
+{
+	struct nfc_hci_dev *hdev = context;
+
+	switch (hdev->async_cb_type) {
+	case HCI_CB_TYPE_TRANSCEIVE:
+		/*
+		 * TODO: Check RF Error indicator to make sure data is valid.
+		 * It seems that HCI cmd can complete without error, but data
+		 * can be invalid if an RF error occured? Ignore for now.
+		 */
+		if (err == 0)
+			skb_trim(skb, skb->len - 1); /* RF Err ind */
+
+		hdev->async_cb(hdev->async_cb_context, skb, err);
+		break;
+	default:
+		if (err == 0)
+			kfree_skb(skb);
+		break;
+	}
+}
+
+static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
+			  struct sk_buff *skb, data_exchange_cb_t cb,
+			  void *cb_context)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+	int r;
+
+	pr_debug("target_idx=%d\n", target->idx);
+
+	switch (target->hci_reader_gate) {
+	case NFC_HCI_RF_READER_A_GATE:
+	case NFC_HCI_RF_READER_B_GATE:
+		if (hdev->ops->im_transceive) {
+			r = hdev->ops->im_transceive(hdev, target, skb, cb,
+						     cb_context);
+			if (r <= 0)	/* handled */
+				break;
+		}
+
+		*skb_push(skb, 1) = 0;	/* CTR, see spec:10.2.2.1 */
+
+		hdev->async_cb_type = HCI_CB_TYPE_TRANSCEIVE;
+		hdev->async_cb = cb;
+		hdev->async_cb_context = cb_context;
+
+		r = nfc_hci_send_cmd_async(hdev, target->hci_reader_gate,
+					   NFC_HCI_WR_XCHG_DATA, skb->data,
+					   skb->len, hci_transceive_cb, hdev);
+		break;
+	default:
+		if (hdev->ops->im_transceive) {
+			r = hdev->ops->im_transceive(hdev, target, skb, cb,
+						     cb_context);
+			if (r == 1)
+				r = -ENOTSUPP;
+		} else {
+			r = -ENOTSUPP;
+		}
+		break;
+	}
+
+	kfree_skb(skb);
+
+	return r;
+}
+
+static int hci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->tm_send)
+		return hdev->ops->tm_send(hdev, skb);
+
+	kfree_skb(skb);
+
+	return -ENOTSUPP;
+}
+
+static int hci_check_presence(struct nfc_dev *nfc_dev,
+			      struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->check_presence)
+		return hdev->ops->check_presence(hdev, target);
+
+	return 0;
+}
+
+static void nfc_hci_failure(struct nfc_hci_dev *hdev, int err)
+{
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->cmd_pending_msg == NULL) {
+		nfc_driver_failure(hdev->ndev, err);
+		goto exit;
+	}
+
+	__nfc_hci_cmd_completion(hdev, err, NULL);
+
+exit:
+	mutex_unlock(&hdev->msg_tx_mutex);
+}
+
+static void nfc_hci_llc_failure(struct nfc_hci_dev *hdev, int err)
+{
+	nfc_hci_failure(hdev, err);
+}
+
+static void nfc_hci_recv_from_llc(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+	struct hcp_packet *packet;
+	u8 type;
+	u8 instruction;
+	struct sk_buff *hcp_skb;
+	u8 pipe;
+	struct sk_buff *frag_skb;
+	int msg_len;
+
+	packet = (struct hcp_packet *)skb->data;
+	if ((packet->header & ~NFC_HCI_FRAGMENT) == 0) {
+		skb_queue_tail(&hdev->rx_hcp_frags, skb);
+		return;
+	}
+
+	/* it's the last fragment. Does it need re-aggregation? */
+	if (skb_queue_len(&hdev->rx_hcp_frags)) {
+		pipe = packet->header & NFC_HCI_FRAGMENT;
+		skb_queue_tail(&hdev->rx_hcp_frags, skb);
+
+		msg_len = 0;
+		skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
+			msg_len += (frag_skb->len -
+				    NFC_HCI_HCP_PACKET_HEADER_LEN);
+		}
+
+		hcp_skb = nfc_alloc_recv_skb(NFC_HCI_HCP_PACKET_HEADER_LEN +
+					     msg_len, GFP_KERNEL);
+		if (hcp_skb == NULL) {
+			nfc_hci_failure(hdev, -ENOMEM);
+			return;
+		}
+
+		*skb_put(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN) = pipe;
+
+		skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
+			msg_len = frag_skb->len - NFC_HCI_HCP_PACKET_HEADER_LEN;
+			memcpy(skb_put(hcp_skb, msg_len),
+			       frag_skb->data + NFC_HCI_HCP_PACKET_HEADER_LEN,
+			       msg_len);
+		}
+
+		skb_queue_purge(&hdev->rx_hcp_frags);
+	} else {
+		packet->header &= NFC_HCI_FRAGMENT;
+		hcp_skb = skb;
+	}
+
+	/* if this is a response, dispatch immediately to
+	 * unblock waiting cmd context. Otherwise, enqueue to dispatch
+	 * in separate context where handler can also execute command.
+	 */
+	packet = (struct hcp_packet *)hcp_skb->data;
+	type = HCP_MSG_GET_TYPE(packet->message.header);
+	if (type == NFC_HCI_HCP_RESPONSE) {
+		pipe = packet->header;
+		instruction = HCP_MSG_GET_CMD(packet->message.header);
+		skb_pull(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN +
+			 NFC_HCI_HCP_MESSAGE_HEADER_LEN);
+		nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, hcp_skb);
+	} else {
+		skb_queue_tail(&hdev->msg_rx_queue, hcp_skb);
+		schedule_work(&hdev->msg_rx_work);
+	}
+}
+
+static struct nfc_ops hci_nfc_ops = {
+	.dev_up = hci_dev_up,
+	.dev_down = hci_dev_down,
+	.start_poll = hci_start_poll,
+	.stop_poll = hci_stop_poll,
+	.dep_link_up = hci_dep_link_up,
+	.dep_link_down = hci_dep_link_down,
+	.activate_target = hci_activate_target,
+	.deactivate_target = hci_deactivate_target,
+	.im_transceive = hci_transceive,
+	.tm_send = hci_tm_send,
+	.check_presence = hci_check_presence,
+};
+
+struct nfc_hci_dev *nfc_hci_allocate_device(struct nfc_hci_ops *ops,
+					    struct nfc_hci_init_data *init_data,
+					    unsigned long quirks,
+					    u32 protocols,
+					    u32 supported_se,
+					    const char *llc_name,
+					    int tx_headroom,
+					    int tx_tailroom,
+					    int max_link_payload)
+{
+	struct nfc_hci_dev *hdev;
+
+	if (ops->xmit == NULL)
+		return NULL;
+
+	if (protocols == 0)
+		return NULL;
+
+	hdev = kzalloc(sizeof(struct nfc_hci_dev), GFP_KERNEL);
+	if (hdev == NULL)
+		return NULL;
+
+	hdev->llc = nfc_llc_allocate(llc_name, hdev, ops->xmit,
+				     nfc_hci_recv_from_llc, tx_headroom,
+				     tx_tailroom, nfc_hci_llc_failure);
+	if (hdev->llc == NULL) {
+		kfree(hdev);
+		return NULL;
+	}
+
+	hdev->ndev = nfc_allocate_device(&hci_nfc_ops, protocols, supported_se,
+					 tx_headroom + HCI_CMDS_HEADROOM,
+					 tx_tailroom);
+	if (!hdev->ndev) {
+		nfc_llc_free(hdev->llc);
+		kfree(hdev);
+		return NULL;
+	}
+
+	hdev->ops = ops;
+	hdev->max_data_link_payload = max_link_payload;
+	hdev->init_data = *init_data;
+
+	nfc_set_drvdata(hdev->ndev, hdev);
+
+	memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+
+	hdev->quirks = quirks;
+
+	return hdev;
+}
+EXPORT_SYMBOL(nfc_hci_allocate_device);
+
+void nfc_hci_free_device(struct nfc_hci_dev *hdev)
+{
+	nfc_free_device(hdev->ndev);
+	nfc_llc_free(hdev->llc);
+	kfree(hdev);
+}
+EXPORT_SYMBOL(nfc_hci_free_device);
+
+int nfc_hci_register_device(struct nfc_hci_dev *hdev)
+{
+	mutex_init(&hdev->msg_tx_mutex);
+
+	INIT_LIST_HEAD(&hdev->msg_tx_queue);
+
+	INIT_WORK(&hdev->msg_tx_work, nfc_hci_msg_tx_work);
+
+	init_timer(&hdev->cmd_timer);
+	hdev->cmd_timer.data = (unsigned long)hdev;
+	hdev->cmd_timer.function = nfc_hci_cmd_timeout;
+
+	skb_queue_head_init(&hdev->rx_hcp_frags);
+
+	INIT_WORK(&hdev->msg_rx_work, nfc_hci_msg_rx_work);
+
+	skb_queue_head_init(&hdev->msg_rx_queue);
+
+	return nfc_register_device(hdev->ndev);
+}
+EXPORT_SYMBOL(nfc_hci_register_device);
+
+void nfc_hci_unregister_device(struct nfc_hci_dev *hdev)
+{
+	struct hci_msg *msg, *n;
+
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->cmd_pending_msg) {
+		if (hdev->cmd_pending_msg->cb)
+			hdev->cmd_pending_msg->cb(
+					     hdev->cmd_pending_msg->cb_context,
+					     NULL, -ESHUTDOWN);
+		kfree(hdev->cmd_pending_msg);
+		hdev->cmd_pending_msg = NULL;
+	}
+
+	hdev->shutting_down = true;
+
+	mutex_unlock(&hdev->msg_tx_mutex);
+
+	del_timer_sync(&hdev->cmd_timer);
+	cancel_work_sync(&hdev->msg_tx_work);
+
+	cancel_work_sync(&hdev->msg_rx_work);
+
+	nfc_unregister_device(hdev->ndev);
+
+	skb_queue_purge(&hdev->rx_hcp_frags);
+	skb_queue_purge(&hdev->msg_rx_queue);
+
+	list_for_each_entry_safe(msg, n, &hdev->msg_tx_queue, msg_l) {
+		list_del(&msg->msg_l);
+		skb_queue_purge(&msg->msg_frags);
+		kfree(msg);
+	}
+}
+EXPORT_SYMBOL(nfc_hci_unregister_device);
+
+void nfc_hci_set_clientdata(struct nfc_hci_dev *hdev, void *clientdata)
+{
+	hdev->clientdata = clientdata;
+}
+EXPORT_SYMBOL(nfc_hci_set_clientdata);
+
+void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev)
+{
+	return hdev->clientdata;
+}
+EXPORT_SYMBOL(nfc_hci_get_clientdata);
+
+void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err)
+{
+	nfc_hci_failure(hdev, err);
+}
+EXPORT_SYMBOL(nfc_hci_driver_failure);
+
+void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+	nfc_llc_rcv_from_drv(hdev->llc, skb);
+}
+EXPORT_SYMBOL(nfc_hci_recv_frame);
+
+static int __init nfc_hci_init(void)
+{
+	return nfc_llc_init();
+}
+
+static void __exit nfc_hci_exit(void)
+{
+	nfc_llc_exit();
+}
+
+subsys_initcall(nfc_hci_init);
+module_exit(nfc_hci_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NFC HCI Core");
diff --git a/net/nfc/hci/hci.h b/net/nfc/hci/hci.h
new file mode 100644
index 0000000..b274d12
--- /dev/null
+++ b/net/nfc/hci/hci.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright (C) 2012  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.
+ */
+
+#ifndef __LOCAL_HCI_H
+#define __LOCAL_HCI_H
+
+#include <net/nfc/hci.h>
+
+struct gate_pipe_map {
+	u8 gate;
+	u8 pipe;
+};
+
+struct hcp_message {
+	u8 header;		/* type -cmd,evt,rsp- + instruction */
+	u8 data[];
+} __packed;
+
+struct hcp_packet {
+	u8 header;		/* cbit+pipe */
+	struct hcp_message message;
+} __packed;
+
+struct hcp_exec_waiter {
+	wait_queue_head_t *wq;
+	bool exec_complete;
+	int exec_result;
+	struct sk_buff *result_skb;
+};
+
+struct hci_msg {
+	struct list_head msg_l;
+	struct sk_buff_head msg_frags;
+	bool wait_response;
+	data_exchange_cb_t cb;
+	void *cb_context;
+	unsigned long completion_delay;
+};
+
+struct hci_create_pipe_params {
+	u8 src_gate;
+	u8 dest_host;
+	u8 dest_gate;
+} __packed;
+
+struct hci_create_pipe_resp {
+	u8 src_host;
+	u8 src_gate;
+	u8 dest_host;
+	u8 dest_gate;
+	u8 pipe;
+} __packed;
+
+#define NFC_HCI_FRAGMENT	0x7f
+
+#define HCP_HEADER(type, instr) ((((type) & 0x03) << 6) | ((instr) & 0x3f))
+#define HCP_MSG_GET_TYPE(header) ((header & 0xc0) >> 6)
+#define HCP_MSG_GET_CMD(header) (header & 0x3f)
+
+int nfc_hci_hcp_message_tx(struct nfc_hci_dev *hdev, u8 pipe,
+			   u8 type, u8 instruction,
+			   const u8 *payload, size_t payload_len,
+			   data_exchange_cb_t cb, void *cb_context,
+			   unsigned long completion_delay);
+
+u8 nfc_hci_pipe2gate(struct nfc_hci_dev *hdev, u8 pipe);
+
+void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type,
+			    u8 instruction, struct sk_buff *skb);
+
+/* HCP headers */
+#define NFC_HCI_HCP_PACKET_HEADER_LEN	1
+#define NFC_HCI_HCP_MESSAGE_HEADER_LEN	1
+#define NFC_HCI_HCP_HEADER_LEN		2
+
+/* HCP types */
+#define NFC_HCI_HCP_COMMAND	0x00
+#define NFC_HCI_HCP_EVENT	0x01
+#define NFC_HCI_HCP_RESPONSE	0x02
+
+/* Generic commands */
+#define NFC_HCI_ANY_SET_PARAMETER	0x01
+#define NFC_HCI_ANY_GET_PARAMETER	0x02
+#define NFC_HCI_ANY_OPEN_PIPE		0x03
+#define NFC_HCI_ANY_CLOSE_PIPE		0x04
+
+/* Reader RF commands */
+#define NFC_HCI_WR_XCHG_DATA		0x10
+
+/* Admin commands */
+#define NFC_HCI_ADM_CREATE_PIPE			0x10
+#define NFC_HCI_ADM_DELETE_PIPE			0x11
+#define NFC_HCI_ADM_NOTIFY_PIPE_CREATED		0x12
+#define NFC_HCI_ADM_NOTIFY_PIPE_DELETED		0x13
+#define NFC_HCI_ADM_CLEAR_ALL_PIPE		0x14
+#define NFC_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED	0x15
+
+/* Generic responses */
+#define NFC_HCI_ANY_OK				0x00
+#define NFC_HCI_ANY_E_NOT_CONNECTED		0x01
+#define NFC_HCI_ANY_E_CMD_PAR_UNKNOWN		0x02
+#define NFC_HCI_ANY_E_NOK			0x03
+#define NFC_HCI_ANY_E_PIPES_FULL		0x04
+#define NFC_HCI_ANY_E_REG_PAR_UNKNOWN		0x05
+#define NFC_HCI_ANY_E_PIPE_NOT_OPENED		0x06
+#define NFC_HCI_ANY_E_CMD_NOT_SUPPORTED		0x07
+#define NFC_HCI_ANY_E_INHIBITED			0x08
+#define NFC_HCI_ANY_E_TIMEOUT			0x09
+#define NFC_HCI_ANY_E_REG_ACCESS_DENIED		0x0a
+#define NFC_HCI_ANY_E_PIPE_ACCESS_DENIED	0x0b
+
+#endif /* __LOCAL_HCI_H */
diff --git a/net/nfc/hci/hcp.c b/net/nfc/hci/hcp.c
new file mode 100644
index 0000000..b6b4109
--- /dev/null
+++ b/net/nfc/hci/hcp.c
@@ -0,0 +1,163 @@
+/*
+ * Copyright (C) 2012  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) "hci: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <net/nfc/hci.h>
+
+#include "hci.h"
+
+/*
+ * Payload is the HCP message data only. Instruction will be prepended.
+ * Guarantees that cb will be called upon completion or timeout delay
+ * counted from the moment the cmd is sent to the transport.
+ */
+int nfc_hci_hcp_message_tx(struct nfc_hci_dev *hdev, u8 pipe,
+			   u8 type, u8 instruction,
+			   const u8 *payload, size_t payload_len,
+			   data_exchange_cb_t cb, void *cb_context,
+			   unsigned long completion_delay)
+{
+	struct nfc_dev *ndev = hdev->ndev;
+	struct hci_msg *cmd;
+	const u8 *ptr = payload;
+	int hci_len, err;
+	bool firstfrag = true;
+
+	cmd = kzalloc(sizeof(struct hci_msg), GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&cmd->msg_l);
+	skb_queue_head_init(&cmd->msg_frags);
+	cmd->wait_response = (type == NFC_HCI_HCP_COMMAND) ? true : false;
+	cmd->cb = cb;
+	cmd->cb_context = cb_context;
+	cmd->completion_delay = completion_delay;
+
+	hci_len = payload_len + 1;
+	while (hci_len > 0) {
+		struct sk_buff *skb;
+		int skb_len, data_link_len;
+		struct hcp_packet *packet;
+
+		if (NFC_HCI_HCP_PACKET_HEADER_LEN + hci_len <=
+		    hdev->max_data_link_payload)
+			data_link_len = hci_len;
+		else
+			data_link_len = hdev->max_data_link_payload -
+					NFC_HCI_HCP_PACKET_HEADER_LEN;
+
+		skb_len = ndev->tx_headroom + NFC_HCI_HCP_PACKET_HEADER_LEN +
+			  data_link_len + ndev->tx_tailroom;
+		hci_len -= data_link_len;
+
+		skb = alloc_skb(skb_len, GFP_KERNEL);
+		if (skb == NULL) {
+			err = -ENOMEM;
+			goto out_skb_err;
+		}
+		skb_reserve(skb, ndev->tx_headroom);
+
+		skb_put(skb, NFC_HCI_HCP_PACKET_HEADER_LEN + data_link_len);
+
+		/* Only the last fragment will have the cb bit set to 1 */
+		packet = (struct hcp_packet *)skb->data;
+		packet->header = pipe;
+		if (firstfrag) {
+			firstfrag = false;
+			packet->message.header = HCP_HEADER(type, instruction);
+			if (ptr) {
+				memcpy(packet->message.data, ptr,
+				       data_link_len - 1);
+				ptr += data_link_len - 1;
+			}
+		} else {
+			memcpy(&packet->message, ptr, data_link_len);
+			ptr += data_link_len;
+		}
+
+		/* This is the last fragment, set the cb bit */
+		if (hci_len == 0)
+			packet->header |= ~NFC_HCI_FRAGMENT;
+
+		skb_queue_tail(&cmd->msg_frags, skb);
+	}
+
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->shutting_down) {
+		err = -ESHUTDOWN;
+		mutex_unlock(&hdev->msg_tx_mutex);
+		goto out_skb_err;
+	}
+
+	list_add_tail(&cmd->msg_l, &hdev->msg_tx_queue);
+	mutex_unlock(&hdev->msg_tx_mutex);
+
+	schedule_work(&hdev->msg_tx_work);
+
+	return 0;
+
+out_skb_err:
+	skb_queue_purge(&cmd->msg_frags);
+	kfree(cmd);
+
+	return err;
+}
+
+u8 nfc_hci_pipe2gate(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	int gate;
+
+	for (gate = 0; gate < NFC_HCI_MAX_GATES; gate++)
+		if (hdev->gate2pipe[gate] == pipe)
+			return gate;
+
+	return 0xff;
+}
+
+/*
+ * Receive hcp message for pipe, with type and cmd.
+ * skb contains optional message data only.
+ */
+void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type,
+			    u8 instruction, struct sk_buff *skb)
+{
+	switch (type) {
+	case NFC_HCI_HCP_RESPONSE:
+		nfc_hci_resp_received(hdev, instruction, skb);
+		break;
+	case NFC_HCI_HCP_COMMAND:
+		nfc_hci_cmd_received(hdev, pipe, instruction, skb);
+		break;
+	case NFC_HCI_HCP_EVENT:
+		nfc_hci_event_received(hdev, pipe, instruction, skb);
+		break;
+	default:
+		pr_err("UNKNOWN MSG Type %d, instruction=%d\n",
+		       type, instruction);
+		kfree_skb(skb);
+		break;
+	}
+}
diff --git a/net/nfc/hci/llc.c b/net/nfc/hci/llc.c
new file mode 100644
index 0000000..fe5e966
--- /dev/null
+++ b/net/nfc/hci/llc.c
@@ -0,0 +1,170 @@
+/*
+ * Link Layer Control manager
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions 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 <net/nfc/llc.h>
+
+#include "llc.h"
+
+static struct list_head llc_engines;
+
+int nfc_llc_init(void)
+{
+	int r;
+
+	INIT_LIST_HEAD(&llc_engines);
+
+	r = nfc_llc_nop_register();
+	if (r)
+		goto exit;
+
+	r = nfc_llc_shdlc_register();
+	if (r)
+		goto exit;
+
+	return 0;
+
+exit:
+	nfc_llc_exit();
+	return r;
+}
+
+void nfc_llc_exit(void)
+{
+	struct nfc_llc_engine *llc_engine, *n;
+
+	list_for_each_entry_safe(llc_engine, n, &llc_engines, entry) {
+		list_del(&llc_engine->entry);
+		kfree(llc_engine->name);
+		kfree(llc_engine);
+	}
+}
+
+int nfc_llc_register(const char *name, struct nfc_llc_ops *ops)
+{
+	struct nfc_llc_engine *llc_engine;
+
+	llc_engine = kzalloc(sizeof(struct nfc_llc_engine), GFP_KERNEL);
+	if (llc_engine == NULL)
+		return -ENOMEM;
+
+	llc_engine->name = kstrdup(name, GFP_KERNEL);
+	if (llc_engine->name == NULL) {
+		kfree(llc_engine);
+		return -ENOMEM;
+	}
+	llc_engine->ops = ops;
+
+	INIT_LIST_HEAD(&llc_engine->entry);
+	list_add_tail(&llc_engine->entry, &llc_engines);
+
+	return 0;
+}
+
+static struct nfc_llc_engine *nfc_llc_name_to_engine(const char *name)
+{
+	struct nfc_llc_engine *llc_engine;
+
+	list_for_each_entry(llc_engine, &llc_engines, entry) {
+		if (strcmp(llc_engine->name, name) == 0)
+			return llc_engine;
+	}
+
+	return NULL;
+}
+
+void nfc_llc_unregister(const char *name)
+{
+	struct nfc_llc_engine *llc_engine;
+
+	llc_engine = nfc_llc_name_to_engine(name);
+	if (llc_engine == NULL)
+		return;
+
+	list_del(&llc_engine->entry);
+	kfree(llc_engine->name);
+	kfree(llc_engine);
+}
+
+struct nfc_llc *nfc_llc_allocate(const char *name, struct nfc_hci_dev *hdev,
+				 xmit_to_drv_t xmit_to_drv,
+				 rcv_to_hci_t rcv_to_hci, int tx_headroom,
+				 int tx_tailroom, llc_failure_t llc_failure)
+{
+	struct nfc_llc_engine *llc_engine;
+	struct nfc_llc *llc;
+
+	llc_engine = nfc_llc_name_to_engine(name);
+	if (llc_engine == NULL)
+		return NULL;
+
+	llc = kzalloc(sizeof(struct nfc_llc), GFP_KERNEL);
+	if (llc == NULL)
+		return NULL;
+
+	llc->data = llc_engine->ops->init(hdev, xmit_to_drv, rcv_to_hci,
+					  tx_headroom, tx_tailroom,
+					  &llc->rx_headroom, &llc->rx_tailroom,
+					  llc_failure);
+	if (llc->data == NULL) {
+		kfree(llc);
+		return NULL;
+	}
+	llc->ops = llc_engine->ops;
+
+	return llc;
+}
+
+void nfc_llc_free(struct nfc_llc *llc)
+{
+	llc->ops->deinit(llc);
+	kfree(llc);
+}
+
+inline void nfc_llc_get_rx_head_tail_room(struct nfc_llc *llc, int *rx_headroom,
+					  int *rx_tailroom)
+{
+	*rx_headroom = llc->rx_headroom;
+	*rx_tailroom = llc->rx_tailroom;
+}
+
+inline int nfc_llc_start(struct nfc_llc *llc)
+{
+	return llc->ops->start(llc);
+}
+
+inline int nfc_llc_stop(struct nfc_llc *llc)
+{
+	return llc->ops->stop(llc);
+}
+
+inline void nfc_llc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	llc->ops->rcv_from_drv(llc, skb);
+}
+
+inline int nfc_llc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	return llc->ops->xmit_from_hci(llc, skb);
+}
+
+inline void *nfc_llc_get_data(struct nfc_llc *llc)
+{
+	return llc->data;
+}
diff --git a/net/nfc/hci/llc.h b/net/nfc/hci/llc.h
new file mode 100644
index 0000000..8bb02ca
--- /dev/null
+++ b/net/nfc/hci/llc.h
@@ -0,0 +1,69 @@
+/*
+ * Link Layer Control manager
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions 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.
+ */
+
+#ifndef __LOCAL_LLC_H_
+#define __LOCAL_LLC_H_
+
+#include <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+#include <linux/skbuff.h>
+
+struct nfc_llc_ops {
+	void *(*init) (struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
+		       rcv_to_hci_t rcv_to_hci, int tx_headroom,
+		       int tx_tailroom, int *rx_headroom, int *rx_tailroom,
+		       llc_failure_t llc_failure);
+	void (*deinit) (struct nfc_llc *llc);
+	int (*start) (struct nfc_llc *llc);
+	int (*stop) (struct nfc_llc *llc);
+	void (*rcv_from_drv) (struct nfc_llc *llc, struct sk_buff *skb);
+	int (*xmit_from_hci) (struct nfc_llc *llc, struct sk_buff *skb);
+};
+
+struct nfc_llc_engine {
+	const char *name;
+	struct nfc_llc_ops *ops;
+	struct list_head entry;
+};
+
+struct nfc_llc {
+	void *data;
+	struct nfc_llc_ops *ops;
+	int rx_headroom;
+	int rx_tailroom;
+};
+
+void *nfc_llc_get_data(struct nfc_llc *llc);
+
+int nfc_llc_register(const char *name, struct nfc_llc_ops *ops);
+void nfc_llc_unregister(const char *name);
+
+int nfc_llc_nop_register(void);
+
+#if defined(CPTCFG_NFC_SHDLC)
+int nfc_llc_shdlc_register(void);
+#else
+static inline int nfc_llc_shdlc_register(void)
+{
+	return 0;
+}
+#endif
+
+#endif /* __LOCAL_LLC_H_ */
diff --git a/net/nfc/hci/llc_nop.c b/net/nfc/hci/llc_nop.c
new file mode 100644
index 0000000..87b1029
--- /dev/null
+++ b/net/nfc/hci/llc_nop.c
@@ -0,0 +1,99 @@
+/*
+ * nop (passthrough) Link Layer Control
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions 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/types.h>
+
+#include "llc.h"
+
+struct llc_nop {
+	struct nfc_hci_dev *hdev;
+	xmit_to_drv_t xmit_to_drv;
+	rcv_to_hci_t rcv_to_hci;
+	int tx_headroom;
+	int tx_tailroom;
+	llc_failure_t llc_failure;
+};
+
+static void *llc_nop_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
+			  rcv_to_hci_t rcv_to_hci, int tx_headroom,
+			  int tx_tailroom, int *rx_headroom, int *rx_tailroom,
+			  llc_failure_t llc_failure)
+{
+	struct llc_nop *llc_nop;
+
+	*rx_headroom = 0;
+	*rx_tailroom = 0;
+
+	llc_nop = kzalloc(sizeof(struct llc_nop), GFP_KERNEL);
+	if (llc_nop == NULL)
+		return NULL;
+
+	llc_nop->hdev = hdev;
+	llc_nop->xmit_to_drv = xmit_to_drv;
+	llc_nop->rcv_to_hci = rcv_to_hci;
+	llc_nop->tx_headroom = tx_headroom;
+	llc_nop->tx_tailroom = tx_tailroom;
+	llc_nop->llc_failure = llc_failure;
+
+	return llc_nop;
+}
+
+static void llc_nop_deinit(struct nfc_llc *llc)
+{
+	kfree(nfc_llc_get_data(llc));
+}
+
+static int llc_nop_start(struct nfc_llc *llc)
+{
+	return 0;
+}
+
+static int llc_nop_stop(struct nfc_llc *llc)
+{
+	return 0;
+}
+
+static void llc_nop_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_nop *llc_nop = nfc_llc_get_data(llc);
+
+	llc_nop->rcv_to_hci(llc_nop->hdev, skb);
+}
+
+static int llc_nop_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_nop *llc_nop = nfc_llc_get_data(llc);
+
+	return llc_nop->xmit_to_drv(llc_nop->hdev, skb);
+}
+
+static struct nfc_llc_ops llc_nop_ops = {
+	.init = llc_nop_init,
+	.deinit = llc_nop_deinit,
+	.start = llc_nop_start,
+	.stop = llc_nop_stop,
+	.rcv_from_drv = llc_nop_rcv_from_drv,
+	.xmit_from_hci = llc_nop_xmit_from_hci,
+};
+
+int nfc_llc_nop_register(void)
+{
+	return nfc_llc_register(LLC_NOP_NAME, &llc_nop_ops);
+}
diff --git a/net/nfc/hci/llc_shdlc.c b/net/nfc/hci/llc_shdlc.c
new file mode 100644
index 0000000..27b313b
--- /dev/null
+++ b/net/nfc/hci/llc_shdlc.c
@@ -0,0 +1,856 @@
+/*
+ * shdlc Link Layer Control
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions 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) "shdlc: %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+
+#include "llc.h"
+
+enum shdlc_state {
+	SHDLC_DISCONNECTED = 0,
+	SHDLC_CONNECTING = 1,
+	SHDLC_NEGOTIATING = 2,
+	SHDLC_HALF_CONNECTED = 3,
+	SHDLC_CONNECTED = 4
+};
+
+struct llc_shdlc {
+	struct nfc_hci_dev *hdev;
+	xmit_to_drv_t xmit_to_drv;
+	rcv_to_hci_t rcv_to_hci;
+
+	struct mutex state_mutex;
+	enum shdlc_state state;
+	int hard_fault;
+
+	wait_queue_head_t *connect_wq;
+	int connect_tries;
+	int connect_result;
+	struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
+
+	u8 w;				/* window size */
+	bool srej_support;
+
+	struct timer_list t1_timer;	/* send ack timeout */
+	bool t1_active;
+
+	struct timer_list t2_timer;	/* guard/retransmit timeout */
+	bool t2_active;
+
+	int ns;				/* next seq num for send */
+	int nr;				/* next expected seq num for receive */
+	int dnr;			/* oldest sent unacked seq num */
+
+	struct sk_buff_head rcv_q;
+
+	struct sk_buff_head send_q;
+	bool rnr;			/* other side is not ready to receive */
+
+	struct sk_buff_head ack_pending_q;
+
+	struct work_struct sm_work;
+
+	int tx_headroom;
+	int tx_tailroom;
+
+	llc_failure_t llc_failure;
+};
+
+#define SHDLC_LLC_HEAD_ROOM	2
+
+#define SHDLC_MAX_WINDOW	4
+#define SHDLC_SREJ_SUPPORT	false
+
+#define SHDLC_CONTROL_HEAD_MASK	0xe0
+#define SHDLC_CONTROL_HEAD_I	0x80
+#define SHDLC_CONTROL_HEAD_I2	0xa0
+#define SHDLC_CONTROL_HEAD_S	0xc0
+#define SHDLC_CONTROL_HEAD_U	0xe0
+
+#define SHDLC_CONTROL_NS_MASK	0x38
+#define SHDLC_CONTROL_NR_MASK	0x07
+#define SHDLC_CONTROL_TYPE_MASK	0x18
+
+#define SHDLC_CONTROL_M_MASK	0x1f
+
+enum sframe_type {
+	S_FRAME_RR = 0x00,
+	S_FRAME_REJ = 0x01,
+	S_FRAME_RNR = 0x02,
+	S_FRAME_SREJ = 0x03
+};
+
+enum uframe_modifier {
+	U_FRAME_UA = 0x06,
+	U_FRAME_RSET = 0x19
+};
+
+#define SHDLC_CONNECT_VALUE_MS	5
+#define SHDLC_T1_VALUE_MS(w)	((5 * w) / 4)
+#define SHDLC_T2_VALUE_MS	300
+
+#define SHDLC_DUMP_SKB(info, skb)				  \
+do {								  \
+	pr_debug("%s:\n", info);				  \
+	print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
+		       16, 1, skb->data, skb->len, 0);		  \
+} while (0)
+
+/* checks x < y <= z modulo 8 */
+static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
+{
+	if (x < z)
+		return ((x < y) && (y <= z)) ? true : false;
+	else
+		return ((y > x) || (y <= z)) ? true : false;
+}
+
+/* checks x <= y < z modulo 8 */
+static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
+{
+	if (x <= z)
+		return ((x <= y) && (y < z)) ? true : false;
+	else			/* x > z -> z+8 > x */
+		return ((y >= x) || (y < z)) ? true : false;
+}
+
+static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc,
+					   int payload_len)
+{
+	struct sk_buff *skb;
+
+	skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
+			shdlc->tx_tailroom + payload_len, GFP_KERNEL);
+	if (skb)
+		skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
+
+	return skb;
+}
+
+/* immediately sends an S frame. */
+static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc,
+				  enum sframe_type sframe_type, int nr)
+{
+	int r;
+	struct sk_buff *skb;
+
+	pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
+
+	skb = llc_shdlc_alloc_skb(shdlc, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
+
+	r = shdlc->xmit_to_drv(shdlc->hdev, skb);
+
+	kfree_skb(skb);
+
+	return r;
+}
+
+/* immediately sends an U frame. skb may contain optional payload */
+static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc,
+				  struct sk_buff *skb,
+				  enum uframe_modifier uframe_modifier)
+{
+	int r;
+
+	pr_debug("uframe_modifier=%d\n", uframe_modifier);
+
+	*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
+
+	r = shdlc->xmit_to_drv(shdlc->hdev, skb);
+
+	kfree_skb(skb);
+
+	return r;
+}
+
+/*
+ * Free ack_pending frames until y_nr - 1, and reset t2 according to
+ * the remaining oldest ack_pending frame sent time
+ */
+static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
+{
+	struct sk_buff *skb;
+	int dnr = shdlc->dnr;	/* MUST initially be < y_nr */
+
+	pr_debug("release ack pending up to frame %d excluded\n", y_nr);
+
+	while (dnr != y_nr) {
+		pr_debug("release ack pending frame %d\n", dnr);
+
+		skb = skb_dequeue(&shdlc->ack_pending_q);
+		kfree_skb(skb);
+
+		dnr = (dnr + 1) % 8;
+	}
+
+	if (skb_queue_empty(&shdlc->ack_pending_q)) {
+		if (shdlc->t2_active) {
+			del_timer_sync(&shdlc->t2_timer);
+			shdlc->t2_active = false;
+
+			pr_debug
+			    ("All sent frames acked. Stopped T2(retransmit)\n");
+		}
+	} else {
+		skb = skb_peek(&shdlc->ack_pending_q);
+
+		mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
+			  msecs_to_jiffies(SHDLC_T2_VALUE_MS));
+		shdlc->t2_active = true;
+
+		pr_debug
+		    ("Start T2(retransmit) for remaining unacked sent frames\n");
+	}
+}
+
+/*
+ * Receive validated frames from lower layer. skb contains HCI payload only.
+ * Handle according to algorithm at spec:10.8.2
+ */
+static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
+				  struct sk_buff *skb, int ns, int nr)
+{
+	int x_ns = ns;
+	int y_nr = nr;
+
+	pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
+
+	if (shdlc->state != SHDLC_CONNECTED)
+		goto exit;
+
+	if (x_ns != shdlc->nr) {
+		llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
+		goto exit;
+	}
+
+	if (shdlc->t1_active == false) {
+		shdlc->t1_active = true;
+		mod_timer(&shdlc->t1_timer, jiffies +
+			  msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
+		pr_debug("(re)Start T1(send ack)\n");
+	}
+
+	if (skb->len) {
+		shdlc->rcv_to_hci(shdlc->hdev, skb);
+		skb = NULL;
+	}
+
+	shdlc->nr = (shdlc->nr + 1) % 8;
+
+	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
+		llc_shdlc_reset_t2(shdlc, y_nr);
+
+		shdlc->dnr = y_nr;
+	}
+
+exit:
+	kfree_skb(skb);
+}
+
+static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
+{
+	pr_debug("remote acked up to frame %d excluded\n", y_nr);
+
+	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
+		llc_shdlc_reset_t2(shdlc, y_nr);
+		shdlc->dnr = y_nr;
+	}
+}
+
+static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+
+	pr_debug("ns reset to %d\n", shdlc->dnr);
+
+	while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
+		skb_pull(skb, 1);	/* remove control field */
+		skb_queue_head(&shdlc->send_q, skb);
+	}
+	shdlc->ns = shdlc->dnr;
+}
+
+static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
+{
+	struct sk_buff *skb;
+
+	pr_debug("remote asks retransmition from frame %d\n", y_nr);
+
+	if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
+		if (shdlc->t2_active) {
+			del_timer_sync(&shdlc->t2_timer);
+			shdlc->t2_active = false;
+			pr_debug("Stopped T2(retransmit)\n");
+		}
+
+		if (shdlc->dnr != y_nr) {
+			while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
+				skb = skb_dequeue(&shdlc->ack_pending_q);
+				kfree_skb(skb);
+			}
+		}
+
+		llc_shdlc_requeue_ack_pending(shdlc);
+	}
+}
+
+/* See spec RR:10.8.3 REJ:10.8.4 */
+static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,
+				  enum sframe_type s_frame_type, int nr)
+{
+	struct sk_buff *skb;
+
+	if (shdlc->state != SHDLC_CONNECTED)
+		return;
+
+	switch (s_frame_type) {
+	case S_FRAME_RR:
+		llc_shdlc_rcv_ack(shdlc, nr);
+		if (shdlc->rnr == true) {	/* see SHDLC 10.7.7 */
+			shdlc->rnr = false;
+			if (shdlc->send_q.qlen == 0) {
+				skb = llc_shdlc_alloc_skb(shdlc, 0);
+				if (skb)
+					skb_queue_tail(&shdlc->send_q, skb);
+			}
+		}
+		break;
+	case S_FRAME_REJ:
+		llc_shdlc_rcv_rej(shdlc, nr);
+		break;
+	case S_FRAME_RNR:
+		llc_shdlc_rcv_ack(shdlc, nr);
+		shdlc->rnr = true;
+		break;
+	default:
+		break;
+	}
+}
+
+static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
+{
+	pr_debug("result=%d\n", r);
+
+	del_timer_sync(&shdlc->connect_timer);
+
+	if (r == 0) {
+		shdlc->ns = 0;
+		shdlc->nr = 0;
+		shdlc->dnr = 0;
+
+		shdlc->state = SHDLC_HALF_CONNECTED;
+	} else {
+		shdlc->state = SHDLC_DISCONNECTED;
+	}
+
+	shdlc->connect_result = r;
+
+	wake_up(shdlc->connect_wq);
+}
+
+static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+
+	pr_debug("\n");
+
+	skb = llc_shdlc_alloc_skb(shdlc, 2);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	*skb_put(skb, 1) = SHDLC_MAX_WINDOW;
+	*skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0;
+
+	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
+}
+
+static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+
+	pr_debug("\n");
+
+	skb = llc_shdlc_alloc_skb(shdlc, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
+}
+
+static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
+				  struct sk_buff *skb,
+				  enum uframe_modifier u_frame_modifier)
+{
+	u8 w = SHDLC_MAX_WINDOW;
+	bool srej_support = SHDLC_SREJ_SUPPORT;
+	int r;
+
+	pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
+
+	switch (u_frame_modifier) {
+	case U_FRAME_RSET:
+		switch (shdlc->state) {
+		case SHDLC_NEGOTIATING:
+		case SHDLC_CONNECTING:
+			/*
+			 * We sent RSET, but chip wants to negociate or we
+			 * got RSET before we managed to send out our.
+			 */
+			if (skb->len > 0)
+				w = skb->data[0];
+
+			if (skb->len > 1)
+				srej_support = skb->data[1] & 0x01 ? true :
+					       false;
+
+			if ((w <= SHDLC_MAX_WINDOW) &&
+			    (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
+				shdlc->w = w;
+				shdlc->srej_support = srej_support;
+				r = llc_shdlc_connect_send_ua(shdlc);
+				llc_shdlc_connect_complete(shdlc, r);
+			}
+			break;
+		case SHDLC_HALF_CONNECTED:
+			/*
+			 * Chip resent RSET due to its timeout - Ignote it
+			 * as we already sent UA.
+			 */
+			break;
+		case SHDLC_CONNECTED:
+			/*
+			 * Chip wants to reset link. This is unexpected and
+			 * unsupported.
+			 */
+			shdlc->hard_fault = -ECONNRESET;
+			break;
+		default:
+			break;
+		}
+		break;
+	case U_FRAME_UA:
+		if ((shdlc->state == SHDLC_CONNECTING &&
+		     shdlc->connect_tries > 0) ||
+		    (shdlc->state == SHDLC_NEGOTIATING)) {
+			llc_shdlc_connect_complete(shdlc, 0);
+			shdlc->state = SHDLC_CONNECTED;
+		}
+		break;
+	default:
+		break;
+	}
+
+	kfree_skb(skb);
+}
+
+static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+	u8 control;
+	int nr;
+	int ns;
+	enum sframe_type s_frame_type;
+	enum uframe_modifier u_frame_modifier;
+
+	if (shdlc->rcv_q.qlen)
+		pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
+
+	while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
+		control = skb->data[0];
+		skb_pull(skb, 1);
+		switch (control & SHDLC_CONTROL_HEAD_MASK) {
+		case SHDLC_CONTROL_HEAD_I:
+		case SHDLC_CONTROL_HEAD_I2:
+			if (shdlc->state == SHDLC_HALF_CONNECTED)
+				shdlc->state = SHDLC_CONNECTED;
+
+			ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
+			nr = control & SHDLC_CONTROL_NR_MASK;
+			llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
+			break;
+		case SHDLC_CONTROL_HEAD_S:
+			if (shdlc->state == SHDLC_HALF_CONNECTED)
+				shdlc->state = SHDLC_CONNECTED;
+
+			s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
+			nr = control & SHDLC_CONTROL_NR_MASK;
+			llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
+			kfree_skb(skb);
+			break;
+		case SHDLC_CONTROL_HEAD_U:
+			u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
+			llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
+			break;
+		default:
+			pr_err("UNKNOWN Control=%d\n", control);
+			kfree_skb(skb);
+			break;
+		}
+	}
+}
+
+static int llc_shdlc_w_used(int ns, int dnr)
+{
+	int unack_count;
+
+	if (dnr <= ns)
+		unack_count = ns - dnr;
+	else
+		unack_count = 8 - dnr + ns;
+
+	return unack_count;
+}
+
+/* Send frames according to algorithm at spec:10.8.1 */
+static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+	int r;
+	unsigned long time_sent;
+
+	if (shdlc->send_q.qlen)
+		pr_debug
+		    ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
+		     shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
+		     shdlc->rnr == false ? "false" : "true",
+		     shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
+		     shdlc->ack_pending_q.qlen);
+
+	while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
+	       (shdlc->rnr == false)) {
+
+		if (shdlc->t1_active) {
+			del_timer_sync(&shdlc->t1_timer);
+			shdlc->t1_active = false;
+			pr_debug("Stopped T1(send ack)\n");
+		}
+
+		skb = skb_dequeue(&shdlc->send_q);
+
+		*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
+				    shdlc->nr;
+
+		pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
+			 shdlc->nr);
+		SHDLC_DUMP_SKB("shdlc frame written", skb);
+
+		r = shdlc->xmit_to_drv(shdlc->hdev, skb);
+		if (r < 0) {
+			shdlc->hard_fault = r;
+			break;
+		}
+
+		shdlc->ns = (shdlc->ns + 1) % 8;
+
+		time_sent = jiffies;
+		*(unsigned long *)skb->cb = time_sent;
+
+		skb_queue_tail(&shdlc->ack_pending_q, skb);
+
+		if (shdlc->t2_active == false) {
+			shdlc->t2_active = true;
+			mod_timer(&shdlc->t2_timer, time_sent +
+				  msecs_to_jiffies(SHDLC_T2_VALUE_MS));
+			pr_debug("Started T2 (retransmit)\n");
+		}
+	}
+}
+
+static void llc_shdlc_connect_timeout(unsigned long data)
+{
+	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
+
+	pr_debug("\n");
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_t1_timeout(unsigned long data)
+{
+	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
+
+	pr_debug("SoftIRQ: need to send ack\n");
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_t2_timeout(unsigned long data)
+{
+	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
+
+	pr_debug("SoftIRQ: need to retransmit\n");
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_sm_work(struct work_struct *work)
+{
+	struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
+	int r;
+
+	pr_debug("\n");
+
+	mutex_lock(&shdlc->state_mutex);
+
+	switch (shdlc->state) {
+	case SHDLC_DISCONNECTED:
+		skb_queue_purge(&shdlc->rcv_q);
+		skb_queue_purge(&shdlc->send_q);
+		skb_queue_purge(&shdlc->ack_pending_q);
+		break;
+	case SHDLC_CONNECTING:
+		if (shdlc->hard_fault) {
+			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
+			break;
+		}
+
+		if (shdlc->connect_tries++ < 5)
+			r = llc_shdlc_connect_initiate(shdlc);
+		else
+			r = -ETIME;
+		if (r < 0) {
+			llc_shdlc_connect_complete(shdlc, r);
+		} else {
+			mod_timer(&shdlc->connect_timer, jiffies +
+				  msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
+
+			shdlc->state = SHDLC_NEGOTIATING;
+		}
+		break;
+	case SHDLC_NEGOTIATING:
+		if (timer_pending(&shdlc->connect_timer) == 0) {
+			shdlc->state = SHDLC_CONNECTING;
+			schedule_work(&shdlc->sm_work);
+		}
+
+		llc_shdlc_handle_rcv_queue(shdlc);
+
+		if (shdlc->hard_fault) {
+			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
+			break;
+		}
+		break;
+	case SHDLC_HALF_CONNECTED:
+	case SHDLC_CONNECTED:
+		llc_shdlc_handle_rcv_queue(shdlc);
+		llc_shdlc_handle_send_queue(shdlc);
+
+		if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
+			pr_debug
+			    ("Handle T1(send ack) elapsed (T1 now inactive)\n");
+
+			shdlc->t1_active = false;
+			r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
+						   shdlc->nr);
+			if (r < 0)
+				shdlc->hard_fault = r;
+		}
+
+		if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
+			pr_debug
+			    ("Handle T2(retransmit) elapsed (T2 inactive)\n");
+
+			shdlc->t2_active = false;
+
+			llc_shdlc_requeue_ack_pending(shdlc);
+			llc_shdlc_handle_send_queue(shdlc);
+		}
+
+		if (shdlc->hard_fault)
+			shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
+		break;
+	default:
+		break;
+	}
+	mutex_unlock(&shdlc->state_mutex);
+}
+
+/*
+ * Called from syscall context to establish shdlc link. Sleeps until
+ * link is ready or failure.
+ */
+static int llc_shdlc_connect(struct llc_shdlc *shdlc)
+{
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
+
+	pr_debug("\n");
+
+	mutex_lock(&shdlc->state_mutex);
+
+	shdlc->state = SHDLC_CONNECTING;
+	shdlc->connect_wq = &connect_wq;
+	shdlc->connect_tries = 0;
+	shdlc->connect_result = 1;
+
+	mutex_unlock(&shdlc->state_mutex);
+
+	schedule_work(&shdlc->sm_work);
+
+	wait_event(connect_wq, shdlc->connect_result != 1);
+
+	return shdlc->connect_result;
+}
+
+static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
+{
+	pr_debug("\n");
+
+	mutex_lock(&shdlc->state_mutex);
+
+	shdlc->state = SHDLC_DISCONNECTED;
+
+	mutex_unlock(&shdlc->state_mutex);
+
+	schedule_work(&shdlc->sm_work);
+}
+
+/*
+ * Receive an incoming shdlc frame. Frame has already been crc-validated.
+ * skb contains only LLC header and payload.
+ * If skb == NULL, it is a notification that the link below is dead.
+ */
+static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
+{
+	if (skb == NULL) {
+		pr_err("NULL Frame -> link is dead\n");
+		shdlc->hard_fault = -EREMOTEIO;
+	} else {
+		SHDLC_DUMP_SKB("incoming frame", skb);
+		skb_queue_tail(&shdlc->rcv_q, skb);
+	}
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
+			    rcv_to_hci_t rcv_to_hci, int tx_headroom,
+			    int tx_tailroom, int *rx_headroom, int *rx_tailroom,
+			    llc_failure_t llc_failure)
+{
+	struct llc_shdlc *shdlc;
+
+	*rx_headroom = SHDLC_LLC_HEAD_ROOM;
+	*rx_tailroom = 0;
+
+	shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
+	if (shdlc == NULL)
+		return NULL;
+
+	mutex_init(&shdlc->state_mutex);
+	shdlc->state = SHDLC_DISCONNECTED;
+
+	init_timer(&shdlc->connect_timer);
+	shdlc->connect_timer.data = (unsigned long)shdlc;
+	shdlc->connect_timer.function = llc_shdlc_connect_timeout;
+
+	init_timer(&shdlc->t1_timer);
+	shdlc->t1_timer.data = (unsigned long)shdlc;
+	shdlc->t1_timer.function = llc_shdlc_t1_timeout;
+
+	init_timer(&shdlc->t2_timer);
+	shdlc->t2_timer.data = (unsigned long)shdlc;
+	shdlc->t2_timer.function = llc_shdlc_t2_timeout;
+
+	shdlc->w = SHDLC_MAX_WINDOW;
+	shdlc->srej_support = SHDLC_SREJ_SUPPORT;
+
+	skb_queue_head_init(&shdlc->rcv_q);
+	skb_queue_head_init(&shdlc->send_q);
+	skb_queue_head_init(&shdlc->ack_pending_q);
+
+	INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
+
+	shdlc->hdev = hdev;
+	shdlc->xmit_to_drv = xmit_to_drv;
+	shdlc->rcv_to_hci = rcv_to_hci;
+	shdlc->tx_headroom = tx_headroom;
+	shdlc->tx_tailroom = tx_tailroom;
+	shdlc->llc_failure = llc_failure;
+
+	return shdlc;
+}
+
+static void llc_shdlc_deinit(struct nfc_llc *llc)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	skb_queue_purge(&shdlc->rcv_q);
+	skb_queue_purge(&shdlc->send_q);
+	skb_queue_purge(&shdlc->ack_pending_q);
+
+	kfree(shdlc);
+}
+
+static int llc_shdlc_start(struct nfc_llc *llc)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	return llc_shdlc_connect(shdlc);
+}
+
+static int llc_shdlc_stop(struct nfc_llc *llc)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	llc_shdlc_disconnect(shdlc);
+
+	return 0;
+}
+
+static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	llc_shdlc_recv_frame(shdlc, skb);
+}
+
+static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	skb_queue_tail(&shdlc->send_q, skb);
+
+	schedule_work(&shdlc->sm_work);
+
+	return 0;
+}
+
+static struct nfc_llc_ops llc_shdlc_ops = {
+	.init = llc_shdlc_init,
+	.deinit = llc_shdlc_deinit,
+	.start = llc_shdlc_start,
+	.stop = llc_shdlc_stop,
+	.rcv_from_drv = llc_shdlc_rcv_from_drv,
+	.xmit_from_hci = llc_shdlc_xmit_from_hci,
+};
+
+int nfc_llc_shdlc_register(void)
+{
+	return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
+}
diff --git a/net/nfc/llcp.h b/net/nfc/llcp.h
new file mode 100644
index 0000000..f924dd2
--- /dev/null
+++ b/net/nfc/llcp.h
@@ -0,0 +1,268 @@
+/*
+ * 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_CONNECTING,
+	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_MAX_MIU (LLCP_MAX_MIUX + 128)
+
+#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)
+#define LLCP_SDP_UNBOUND   (LLCP_MAX_SAP + 1)
+
+struct nfc_llcp_sock;
+
+struct llcp_sock_list {
+	struct hlist_head head;
+	rwlock_t          lock;
+};
+
+struct nfc_llcp_sdp_tlv {
+	u8 *tlv;
+	u8 tlv_len;
+
+	char *uri;
+	u8 tid;
+	u8 sap;
+
+	unsigned long time;
+
+	struct hlist_node node;
+};
+
+struct nfc_llcp_local {
+	struct list_head list;
+	struct nfc_dev *dev;
+
+	struct kref ref;
+
+	struct mutex sdp_lock;
+
+	struct timer_list link_timer;
+	struct sk_buff_head tx_queue;
+	struct work_struct	 tx_work;
+	struct work_struct	 rx_work;
+	struct sk_buff *rx_pending;
+	struct work_struct	 timeout_work;
+
+	u32 target_idx;
+	u8 rf_mode;
+	u8 comm_mode;
+	u8 lto;
+	u8 rw;
+	__be16 miux;
+	unsigned long local_wks;      /* Well known services */
+	unsigned long local_sdp;      /* Local services  */
+	unsigned long local_sap; /* Local SAPs, not available for discovery */
+	atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP];
+
+	/* 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;
+
+	struct mutex sdreq_lock;
+	struct hlist_head pending_sdreqs;
+	struct timer_list sdreq_timer;
+	struct work_struct sdreq_timeout_work;
+	u8 sdreq_next_tid;
+
+	/* sockets array */
+	struct llcp_sock_list sockets;
+	struct llcp_sock_list connecting_sockets;
+	struct llcp_sock_list raw_sockets;
+};
+
+struct nfc_llcp_sock {
+	struct sock sk;
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	u32 target_idx;
+	u32 nfc_protocol;
+
+	/* Link parameters */
+	u8 ssap;
+	u8 dsap;
+	char *service_name;
+	size_t service_name_len;
+	u8 rw;
+	__be16 miux;
+
+
+	/* Remote link parameters */
+	u8 remote_rw;
+	u16 remote_miu;
+
+	/* 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;
+
+	/* Reserved source SAP */
+	u8 reserved_ssap;
+
+	struct sk_buff_head tx_queue;
+	struct sk_buff_head tx_pending_queue;
+
+	struct list_head accept_queue;
+	struct sock *parent;
+};
+
+struct nfc_llcp_ui_cb {
+	__u8 dsap;
+	__u8 ssap;
+};
+
+#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
+
+#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
+#define LLCP_AGF_PDU_HEADER_SIZE 2
+
+/* 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
+
+
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local);
+int nfc_llcp_local_put(struct nfc_llcp_local *local);
+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);
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+			       struct sk_buff *skb, u8 direction);
+
+/* Sock API */
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
+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_gb_tlv(struct nfc_llcp_local *local,
+			  u8 *tlv_array, u16 tlv_array_len);
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+				  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);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+						  size_t uri_len);
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head);
+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_snl_sdres(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len);
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len);
+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_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+			   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_commands.c b/net/nfc/llcp_commands.c
new file mode 100644
index 0000000..c1b23ee
--- /dev/null
+++ b/net/nfc/llcp_commands.c
@@ -0,0 +1,817 @@
+/*
+ * 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 u16 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) & 0x7ff;
+}
+
+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 if (length == 0)
+		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;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
+{
+	struct nfc_llcp_sdp_tlv *sdres;
+	u8 value[2];
+
+	sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+	if (sdres == NULL)
+		return NULL;
+
+	value[0] = tid;
+	value[1] = sap;
+
+	sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
+					&sdres->tlv_len);
+	if (sdres->tlv == NULL) {
+		kfree(sdres);
+		return NULL;
+	}
+
+	sdres->tid = tid;
+	sdres->sap = sap;
+
+	INIT_HLIST_NODE(&sdres->node);
+
+	return sdres;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+						  size_t uri_len)
+{
+	struct nfc_llcp_sdp_tlv *sdreq;
+
+	pr_debug("uri: %s, len: %zu\n", uri, uri_len);
+
+	sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+	if (sdreq == NULL)
+		return NULL;
+
+	sdreq->tlv_len = uri_len + 3;
+
+	if (uri[uri_len - 1] == 0)
+		sdreq->tlv_len--;
+
+	sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
+	if (sdreq->tlv == NULL) {
+		kfree(sdreq);
+		return NULL;
+	}
+
+	sdreq->tlv[0] = LLCP_TLV_SDREQ;
+	sdreq->tlv[1] = sdreq->tlv_len - 2;
+	sdreq->tlv[2] = tid;
+
+	sdreq->tid = tid;
+	sdreq->uri = sdreq->tlv + 3;
+	memcpy(sdreq->uri, uri, uri_len);
+
+	sdreq->time = jiffies;
+
+	INIT_HLIST_NODE(&sdreq->node);
+
+	return sdreq;
+}
+
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
+{
+	kfree(sdp->tlv);
+	kfree(sdp);
+}
+
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
+{
+	struct nfc_llcp_sdp_tlv *sdp;
+	struct hlist_node *n;
+
+	hlist_for_each_entry_safe(sdp, n, head, node) {
+		hlist_del(&sdp->node);
+
+		nfc_llcp_free_sdp_tlv(sdp);
+	}
+}
+
+int nfc_llcp_parse_gb_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;
+		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\n",
+		 local->remote_version, local->remote_miu,
+		 local->remote_lto, local->remote_opt,
+		 local->remote_wks);
+
+	return 0;
+}
+
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+				  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 (sock == NULL)
+		return -ENOTCONN;
+
+	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_MIUX:
+			sock->remote_miu = llcp_tlv_miux(tlv) + 128;
+			break;
+		case LLCP_TLV_RW:
+			sock->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("sock %p rw %d miu %d\n", sock,
+		 sock->remote_rw, sock->remote_miu);
+
+	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;
+
+	pr_debug("Sending DISC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	dev = sock->dev;
+	if (dev == NULL)
+		return -ENODEV;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	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);
+
+	__net_timestamp(skb);
+
+	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX);
+
+	return nfc_data_exchange(dev, local->target_idx, skb,
+				 nfc_llcp_recv, local);
+}
+
+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;
+	int err;
+	u16 size = 0, miux;
+
+	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;
+	}
+
+	/* If the socket parameters are not set, use the local ones */
+	miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
+		local->miux : sock->miux;
+	rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
+
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+				      &miux_tlv_length);
+	size += miux_tlv_length;
+
+	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;
+	int err;
+	u16 size = 0, miux;
+
+	pr_debug("Sending CC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	/* If the socket parameters are not set, use the local ones */
+	miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
+		local->miux : sock->miux;
+	rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
+
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+				      &miux_tlv_length);
+	size += miux_tlv_length;
+
+	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;
+}
+
+static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
+					     size_t tlv_length)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	u16 size = 0;
+
+	if (local == NULL)
+		return ERR_PTR(-ENODEV);
+
+	dev = local->dev;
+	if (dev == NULL)
+		return ERR_PTR(-ENODEV);
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+	size += tlv_length;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
+
+	return skb;
+}
+
+int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len)
+{
+	struct nfc_llcp_sdp_tlv *sdp;
+	struct hlist_node *n;
+	struct sk_buff *skb;
+
+	skb = nfc_llcp_allocate_snl(local, tlvs_len);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
+		memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);
+
+		hlist_del(&sdp->node);
+
+		nfc_llcp_free_sdp_tlv(sdp);
+	}
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len)
+{
+	struct nfc_llcp_sdp_tlv *sdreq;
+	struct hlist_node *n;
+	struct sk_buff *skb;
+
+	skb = nfc_llcp_allocate_snl(local, tlvs_len);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	mutex_lock(&local->sdreq_lock);
+
+	if (hlist_empty(&local->pending_sdreqs))
+		mod_timer(&local->sdreq_timer,
+			  jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+	hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
+		pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
+
+		memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv,
+		       sdreq->tlv_len);
+
+		hlist_del(&sdreq->node);
+
+		hlist_add_head(&sdreq->node, &local->pending_sdreqs);
+	}
+
+	mutex_unlock(&local->sdreq_lock);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+}
+
+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, dsap, ssap, 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;
+	u16 remote_miu;
+
+	pr_debug("Send I frame len %zd\n", len);
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	/* Remote is ready but has not acknowledged our frames */
+	if((sock->remote_ready &&
+	    skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw &&
+	    skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
+		pr_err("Pending queue is full %d frames\n",
+		       skb_queue_len(&sock->tx_pending_queue));
+		return -ENOBUFS;
+	}
+
+	/* Remote is not ready and we've been queueing enough frames */
+	if ((!sock->remote_ready &&
+	     skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
+		pr_err("Tx queue is full %d frames\n",
+		       skb_queue_len(&sock->tx_queue));
+		return -ENOBUFS;
+	}
+
+	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;
+
+	do {
+		remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
+				local->remote_miu : sock->remote_miu;
+
+		frag_len = min_t(size_t, 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);
+
+		if (likely(frag_len > 0))
+			memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		skb_queue_tail(&sock->tx_queue, pdu);
+
+		lock_sock(sk);
+
+		nfc_llcp_queue_i_frames(sock);
+
+		release_sock(sk);
+
+		remaining_len -= frag_len;
+		msg_ptr += frag_len;
+	} while (remaining_len > 0);
+
+	kfree(msg_data);
+
+	return len;
+}
+
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+			   struct msghdr *msg, size_t len)
+{
+	struct sk_buff *pdu;
+	struct nfc_llcp_local *local;
+	size_t frag_len = 0, remaining_len;
+	u8 *msg_ptr, *msg_data;
+	u16 remote_miu;
+	int err;
+
+	pr_debug("Send UI 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;
+
+	do {
+		remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
+				local->remote_miu : sock->remote_miu;
+
+		frag_len = min_t(size_t, remote_miu, remaining_len);
+
+		pr_debug("Fragment %zd bytes remaining %zd",
+			 frag_len, remaining_len);
+
+		pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+					 frag_len + LLCP_HEADER_SIZE, &err);
+		if (pdu == NULL) {
+			pr_err("Could not allocate PDU\n");
+			continue;
+		}
+
+		pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+		if (likely(frag_len > 0))
+			memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		/* No need to check for the peer RW for UI frames */
+		skb_queue_tail(&local->tx_queue, pdu);
+
+		remaining_len -= frag_len;
+		msg_ptr += frag_len;
+	} while (remaining_len > 0);
+
+	kfree(msg_data);
+
+	return len;
+}
+
+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;
+
+	skb_queue_head(&local->tx_queue, skb);
+
+	return 0;
+}
diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c
new file mode 100644
index 0000000..158bdbf
--- /dev/null
+++ b/net/nfc/llcp_core.c
@@ -0,0 +1,1624 @@
+/*
+ * 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_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
+
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
+{
+	write_lock(&l->lock);
+	sk_add_node(sk, &l->head);
+	write_unlock(&l->lock);
+}
+
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
+{
+	write_lock(&l->lock);
+	sk_del_node_init(sk);
+	write_unlock(&l->lock);
+}
+
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
+{
+	sock->remote_rw = LLCP_DEFAULT_RW;
+	sock->remote_miu = LLCP_MAX_MIU + 1;
+}
+
+static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local = sock->local;
+	struct sk_buff *s, *tmp;
+
+	pr_debug("%p\n", &sock->sk);
+
+	skb_queue_purge(&sock->tx_queue);
+	skb_queue_purge(&sock->tx_pending_queue);
+
+	if (local == NULL)
+		return;
+
+	/* Search for local pending SKBs that are related to this socket */
+	skb_queue_walk_safe(&local->tx_queue, s, tmp) {
+		if (s->sk != &sock->sk)
+			continue;
+
+		skb_unlink(s, &local->tx_queue);
+		kfree_skb(s);
+	}
+}
+
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
+				    int err)
+{
+	struct sock *sk;
+	struct hlist_node *tmp;
+	struct nfc_llcp_sock *llcp_sock;
+
+	skb_queue_purge(&local->tx_queue);
+
+	write_lock(&local->sockets.lock);
+
+	sk_for_each_safe(sk, tmp, &local->sockets.head) {
+		llcp_sock = nfc_llcp_sock(sk);
+
+		bh_lock_sock(sk);
+
+		nfc_llcp_socket_purge(llcp_sock);
+
+		if (sk->sk_state == LLCP_CONNECTED)
+			nfc_put_device(llcp_sock->dev);
+
+		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;
+				bh_lock_sock(accept_sk);
+
+				nfc_llcp_accept_unlink(accept_sk);
+
+				if (err)
+					accept_sk->sk_err = err;
+				accept_sk->sk_state = LLCP_CLOSED;
+				accept_sk->sk_state_change(sk);
+
+				bh_unlock_sock(accept_sk);
+			}
+		}
+
+		if (err)
+			sk->sk_err = err;
+		sk->sk_state = LLCP_CLOSED;
+		sk->sk_state_change(sk);
+
+		bh_unlock_sock(sk);
+
+		sk_del_node_init(sk);
+	}
+
+	write_unlock(&local->sockets.lock);
+
+	/* If we still have a device, we keep the RAW sockets alive */
+	if (device == true)
+		return;
+
+	write_lock(&local->raw_sockets.lock);
+
+	sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
+		llcp_sock = nfc_llcp_sock(sk);
+
+		bh_lock_sock(sk);
+
+		nfc_llcp_socket_purge(llcp_sock);
+
+		if (err)
+			sk->sk_err = err;
+		sk->sk_state = LLCP_CLOSED;
+		sk->sk_state_change(sk);
+
+		bh_unlock_sock(sk);
+
+		sk_del_node_init(sk);
+	}
+
+	write_unlock(&local->raw_sockets.lock);
+}
+
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
+{
+	kref_get(&local->ref);
+
+	return local;
+}
+
+static void local_cleanup(struct nfc_llcp_local *local)
+{
+	nfc_llcp_socket_release(local, false, ENXIO);
+	del_timer_sync(&local->link_timer);
+	skb_queue_purge(&local->tx_queue);
+	cancel_work_sync(&local->tx_work);
+	cancel_work_sync(&local->rx_work);
+	cancel_work_sync(&local->timeout_work);
+	kfree_skb(local->rx_pending);
+	del_timer_sync(&local->sdreq_timer);
+	cancel_work_sync(&local->sdreq_timeout_work);
+	nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
+}
+
+static void local_release(struct kref *ref)
+{
+	struct nfc_llcp_local *local;
+
+	local = container_of(ref, struct nfc_llcp_local, ref);
+
+	list_del(&local->list);
+	local_cleanup(local);
+	kfree(local);
+}
+
+int nfc_llcp_local_put(struct nfc_llcp_local *local)
+{
+	if (local == NULL)
+		return 0;
+
+	return kref_put(&local->ref, local_release);
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
+					       u8 ssap, u8 dsap)
+{
+	struct sock *sk;
+	struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+	pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
+	if (ssap == 0 && dsap == 0)
+		return NULL;
+
+	read_lock(&local->sockets.lock);
+
+	llcp_sock = NULL;
+
+	sk_for_each(sk, &local->sockets.head) {
+		tmp_sock = nfc_llcp_sock(sk);
+
+		if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
+			llcp_sock = tmp_sock;
+			break;
+		}
+	}
+
+	read_unlock(&local->sockets.lock);
+
+	if (llcp_sock == NULL)
+		return NULL;
+
+	sock_hold(&llcp_sock->sk);
+
+	return llcp_sock;
+}
+
+static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+{
+	sock_put(&sock->sk);
+}
+
+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");
+
+	schedule_work(&local->timeout_work);
+}
+
+static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
+{
+	unsigned long time;
+	HLIST_HEAD(nl_sdres_list);
+	struct hlist_node *n;
+	struct nfc_llcp_sdp_tlv *sdp;
+	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+						    sdreq_timeout_work);
+
+	mutex_lock(&local->sdreq_lock);
+
+	time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
+
+	hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
+		if (time_after(sdp->time, time))
+			continue;
+
+		sdp->sap = LLCP_SDP_UNBOUND;
+
+		hlist_del(&sdp->node);
+
+		hlist_add_head(&sdp->node, &nl_sdres_list);
+	}
+
+	if (!hlist_empty(&local->pending_sdreqs))
+		mod_timer(&local->sdreq_timer,
+			  jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+	mutex_unlock(&local->sdreq_lock);
+
+	if (!hlist_empty(&nl_sdres_list))
+		nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+}
+
+static void nfc_llcp_sdreq_timer(unsigned long data)
+{
+	struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+	schedule_work(&local->sdreq_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;
+}
+
+static
+struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
+					    u8 *sn, size_t sn_len)
+{
+	struct sock *sk;
+	struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+	pr_debug("sn %zd %p\n", sn_len, sn);
+
+	if (sn == NULL || sn_len == 0)
+		return NULL;
+
+	read_lock(&local->sockets.lock);
+
+	llcp_sock = NULL;
+
+	sk_for_each(sk, &local->sockets.head) {
+		tmp_sock = nfc_llcp_sock(sk);
+
+		pr_debug("llcp sock %p\n", tmp_sock);
+
+		if (tmp_sock->sk.sk_type == SOCK_STREAM &&
+		    tmp_sock->sk.sk_state != LLCP_LISTEN)
+			continue;
+
+		if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
+		    tmp_sock->sk.sk_state != LLCP_BOUND)
+			continue;
+
+		if (tmp_sock->service_name == NULL ||
+		    tmp_sock->service_name_len == 0)
+			continue;
+
+		if (tmp_sock->service_name_len != sn_len)
+			continue;
+
+		if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
+			llcp_sock = tmp_sock;
+			break;
+		}
+	}
+
+	read_unlock(&local->sockets.lock);
+
+	pr_debug("Found llcp sock %p\n", llcp_sock);
+
+	return llcp_sock;
+}
+
+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;
+		}
+
+		/*
+		 * Check if there already is a non WKS socket bound
+		 * to this service name.
+		 */
+		if (nfc_llcp_sock_from_sn(local, sock->service_name,
+					  sock->service_name_len) != NULL) {
+			mutex_unlock(&local->sdp_lock);
+
+			return LLCP_SAP_MAX;
+		}
+
+		mutex_unlock(&local->sdp_lock);
+
+		return LLCP_SDP_UNBOUND;
+
+	} else if (sock->ssap != 0 && 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;
+		}
+	}
+
+	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) {
+		atomic_t *client_cnt;
+
+		local_ssap = ssap - LLCP_WKS_NUM_SAP;
+		sdp = &local->local_sdp;
+		client_cnt = &local->local_sdp_cnt[local_ssap];
+
+		pr_debug("%d clients\n", atomic_read(client_cnt));
+
+		mutex_lock(&local->sdp_lock);
+
+		if (atomic_dec_and_test(client_cnt)) {
+			struct nfc_llcp_sock *l_sock;
+
+			pr_debug("No more clients for SAP %d\n", ssap);
+
+			clear_bit(local_ssap, sdp);
+
+			/* Find the listening sock and set it back to UNBOUND */
+			l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
+			if (l_sock) {
+				l_sock->ssap = LLCP_SDP_UNBOUND;
+				nfc_llcp_sock_put(l_sock);
+			}
+		}
+
+		mutex_unlock(&local->sdp_lock);
+
+		return;
+	} 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);
+}
+
+static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
+{
+	u8 ssap;
+
+	mutex_lock(&local->sdp_lock);
+
+	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;
+}
+
+static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
+{
+	u8 *gb_cur, *version_tlv, version, version_length;
+	u8 *lto_tlv, lto_length;
+	u8 *wks_tlv, wks_length;
+	u8 *miux_tlv, miux_length;
+	u8 gb_len = 0;
+	int ret = 0;
+
+	version = LLCP_VERSION_11;
+	version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
+					 1, &version_length);
+	gb_len += version_length;
+
+	lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->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;
+
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
+				      &miux_length);
+	gb_len += miux_length;
+
+	gb_len += ARRAY_SIZE(llcp_magic);
+
+	if (gb_len > NFC_MAX_GT_LEN) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	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;
+
+	memcpy(gb_cur, miux_tlv, miux_length);
+	gb_cur += miux_length;
+
+	local->gb_len = gb_len;
+
+out:
+	kfree(version_tlv);
+	kfree(lto_tlv);
+	kfree(wks_tlv);
+	kfree(miux_tlv);
+
+	return ret;
+}
+
+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;
+	}
+
+	nfc_llcp_build_gb(local);
+
+	*general_bytes_len = local->gb_len;
+
+	return local->gb;
+}
+
+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;
+	}
+	if (gb_len < 3)
+		return -EINVAL;
+
+	memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
+	memcpy(local->remote_gb, gb, gb_len);
+	local->remote_gb_len = gb_len;
+
+	if (memcmp(local->remote_gb, llcp_magic, 3)) {
+		pr_err("MAC does not support LLCP\n");
+		return -EINVAL;
+	}
+
+	return nfc_llcp_parse_gb_tlv(local,
+				     &local->remote_gb[3],
+				     local->remote_gb_len - 3);
+}
+
+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);
+	sock->send_n = (sock->send_n + 1) % 16;
+	sock->recv_ack_n = (sock->recv_n - 1) % 16;
+}
+
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+			       struct sk_buff *skb, u8 direction)
+{
+	struct sk_buff *skb_copy = NULL, *nskb;
+	struct sock *sk;
+	u8 *data;
+
+	read_lock(&local->raw_sockets.lock);
+
+	sk_for_each(sk, &local->raw_sockets.head) {
+		if (sk->sk_state != LLCP_BOUND)
+			continue;
+
+		if (skb_copy == NULL) {
+			skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
+					       GFP_ATOMIC);
+
+			if (skb_copy == NULL)
+				continue;
+
+			data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
+
+			data[0] = local->dev ? local->dev->idx : 0xFF;
+			data[1] = direction;
+		}
+
+		nskb = skb_clone(skb_copy, GFP_ATOMIC);
+		if (!nskb)
+			continue;
+
+		if (sock_queue_rcv_skb(sk, nskb))
+			kfree_skb(nskb);
+	}
+
+	read_unlock(&local->raw_sockets.lock);
+
+	kfree_skb(skb_copy);
+}
+
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+						    tx_work);
+	struct sk_buff *skb;
+	struct sock *sk;
+	struct nfc_llcp_sock *llcp_sock;
+
+	skb = skb_dequeue(&local->tx_queue);
+	if (skb != NULL) {
+		sk = skb->sk;
+		llcp_sock = nfc_llcp_sock(sk);
+
+		if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+			nfc_llcp_send_symm(local->dev);
+		} else {
+			struct sk_buff *copy_skb = NULL;
+			u8 ptype = nfc_llcp_ptype(skb);
+			int ret;
+
+			pr_debug("Sending pending skb\n");
+			print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
+				       DUMP_PREFIX_OFFSET, 16, 1,
+				       skb->data, skb->len, true);
+
+			if (ptype == LLCP_PDU_I)
+				copy_skb = skb_copy(skb, GFP_ATOMIC);
+
+			__net_timestamp(skb);
+
+			nfc_llcp_send_to_raw_sock(local, skb,
+						  NFC_LLCP_DIRECTION_TX);
+
+			ret = nfc_data_exchange(local->dev, local->target_idx,
+						skb, nfc_llcp_recv, local);
+
+			if (ret) {
+				kfree_skb(copy_skb);
+				goto out;
+			}
+
+			if (ptype == LLCP_PDU_I && copy_skb)
+				skb_queue_tail(&llcp_sock->tx_pending_queue,
+					       copy_skb);
+		}
+	} else {
+		nfc_llcp_send_symm(local->dev);
+	}
+
+out:
+	mod_timer(&local->link_timer,
+		  jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
+
+static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
+							  u8 ssap)
+{
+	struct sock *sk;
+	struct nfc_llcp_sock *llcp_sock;
+
+	read_lock(&local->connecting_sockets.lock);
+
+	sk_for_each(sk, &local->connecting_sockets.head) {
+		llcp_sock = nfc_llcp_sock(sk);
+
+		if (llcp_sock->ssap == ssap) {
+			sock_hold(&llcp_sock->sk);
+			goto out;
+		}
+	}
+
+	llcp_sock = NULL;
+
+out:
+	read_unlock(&local->connecting_sockets.lock);
+
+	return llcp_sock;
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
+						  u8 *sn, size_t sn_len)
+{
+	struct nfc_llcp_sock *llcp_sock;
+
+	llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
+
+	if (llcp_sock == NULL)
+		return NULL;
+
+	sock_hold(&llcp_sock->sk);
+
+	return llcp_sock;
+}
+
+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_ui(struct nfc_llcp_local *local,
+			     struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	struct nfc_llcp_ui_cb *ui_cb;
+	u8 dsap, ssap;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	ui_cb = nfc_llcp_ui_skb_cb(skb);
+	ui_cb->dsap = dsap;
+	ui_cb->ssap = ssap;
+
+	pr_debug("%d %d\n", dsap, ssap);
+
+	/* We're looking for a bound socket, not a client one */
+	llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+	if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+		return;
+
+	/* There is no sequence with UI frames */
+	skb_pull(skb, LLCP_HEADER_SIZE);
+	if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+		/*
+		 * UI frames will be freed from the socket layer, so we
+		 * need to keep them alive until someone receives them.
+		 */
+		skb_get(skb);
+	} else {
+		pr_err("Receive queue is full\n");
+	}
+
+	nfc_llcp_sock_put(llcp_sock);
+}
+
+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, reason;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	pr_debug("%d %d\n", dsap, ssap);
+
+	if (dsap != LLCP_SAP_SDP) {
+		sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+		if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
+			reason = LLCP_DM_NOBOUND;
+			goto fail;
+		}
+	} 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);
+
+		sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
+		if (sock == NULL) {
+			reason = LLCP_DM_NOBOUND;
+			goto fail;
+		}
+	}
+
+	lock_sock(&sock->sk);
+
+	parent = &sock->sk;
+
+	if (sk_acceptq_is_full(parent)) {
+		reason = LLCP_DM_REJ;
+		release_sock(&sock->sk);
+		sock_put(&sock->sk);
+		goto fail;
+	}
+
+	if (sock->ssap == LLCP_SDP_UNBOUND) {
+		u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
+
+		pr_debug("First client, reserving %d\n", ssap);
+
+		if (ssap == LLCP_SAP_MAX) {
+			reason = LLCP_DM_REJ;
+			release_sock(&sock->sk);
+			sock_put(&sock->sk);
+			goto fail;
+		}
+
+		sock->ssap = ssap;
+	}
+
+	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 = nfc_llcp_local_get(local);
+	new_sock->rw = sock->rw;
+	new_sock->miux = sock->miux;
+	new_sock->remote_miu = local->remote_miu;
+	new_sock->nfc_protocol = sock->nfc_protocol;
+	new_sock->dsap = ssap;
+	new_sock->target_idx = local->target_idx;
+	new_sock->parent = parent;
+	new_sock->ssap = sock->ssap;
+	if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
+		atomic_t *client_count;
+
+		pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
+
+		client_count =
+			&local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
+
+		atomic_inc(client_count);
+		new_sock->reserved_ssap = sock->ssap;
+	}
+
+	nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
+				      skb->len - LLCP_HEADER_SIZE);
+
+	pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
+
+	nfc_llcp_sock_link(&local->sockets, new_sk);
+
+	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);
+}
+
+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),
+		 sock->remote_rw);
+
+	/* Try to queue some I frames for transmission */
+	while (sock->remote_ready &&
+	       skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
+		struct sk_buff *pdu;
+
+		pdu = skb_dequeue(&sock->tx_queue);
+		if (pdu == NULL)
+			break;
+
+		/* Update N(S)/N(R) */
+		nfc_llcp_set_nrns(sock, pdu);
+
+		skb_queue_tail(&local->tx_queue, 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)) {
+			/*
+			 * I frames will be freed from the socket layer, so we
+			 * need to keep them alive until someone receives them.
+			 */
+			skb_get(skb);
+		} else {
+			pr_err("Receive queue is full\n");
+		}
+	}
+
+	/* Remove skbs from the pending queue */
+	if (llcp_sock->send_ack_n != nr) {
+		struct sk_buff *s, *tmp;
+		u8 n;
+
+		llcp_sock->send_ack_n = nr;
+
+		/* Remove and free all skbs until ns == nr */
+		skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+			n = nfc_llcp_ns(s);
+
+			skb_unlink(s, &llcp_sock->tx_pending_queue);
+			kfree_skb(s);
+
+			if (n == nr)
+				break;
+		}
+
+		/* Re-queue the remaining skbs for transmission */
+		skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+					    s, tmp) {
+			skb_unlink(s, &llcp_sock->tx_pending_queue);
+			skb_queue_head(&local->tx_queue, 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 && ptype == LLCP_PDU_I)
+		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);
+
+	if ((dsap == 0) && (ssap == 0)) {
+		pr_debug("Connection termination");
+		nfc_dep_link_down(local->dev);
+		return;
+	}
+
+	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);
+
+	nfc_llcp_socket_purge(llcp_sock);
+
+	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;
+	struct sock *sk;
+	u8 dsap, ssap;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+	if (llcp_sock == NULL) {
+		pr_err("Invalid CC\n");
+		nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+
+		return;
+	}
+
+	sk = &llcp_sock->sk;
+
+	/* Unlink from connecting and link to the client array */
+	nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+	nfc_llcp_sock_link(&local->sockets, sk);
+	llcp_sock->dsap = ssap;
+
+	nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
+				      skb->len - LLCP_HEADER_SIZE);
+
+	sk->sk_state = LLCP_CONNECTED;
+	sk->sk_state_change(sk);
+
+	nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	struct sock *sk;
+	u8 dsap, ssap, reason;
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+	reason = skb->data[2];
+
+	pr_debug("%d %d reason %d\n", ssap, dsap, reason);
+
+	switch (reason) {
+	case LLCP_DM_NOBOUND:
+	case LLCP_DM_REJ:
+		llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+		break;
+
+	default:
+		llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+		break;
+	}
+
+	if (llcp_sock == NULL) {
+		pr_debug("Already closed\n");
+		return;
+	}
+
+	sk = &llcp_sock->sk;
+
+	sk->sk_err = ENXIO;
+	sk->sk_state = LLCP_CLOSED;
+	sk->sk_state_change(sk);
+
+	nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+			      struct sk_buff *skb)
+{
+	struct nfc_llcp_sock *llcp_sock;
+	u8 dsap, ssap, *tlv, type, length, tid, sap;
+	u16 tlv_len, offset;
+	char *service_name;
+	size_t service_name_len;
+	struct nfc_llcp_sdp_tlv *sdp;
+	HLIST_HEAD(llc_sdres_list);
+	size_t sdres_tlvs_len;
+	HLIST_HEAD(nl_sdres_list);
+
+	dsap = nfc_llcp_dsap(skb);
+	ssap = nfc_llcp_ssap(skb);
+
+	pr_debug("%d %d\n", dsap, ssap);
+
+	if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+		pr_err("Wrong SNL SAP\n");
+		return;
+	}
+
+	tlv = &skb->data[LLCP_HEADER_SIZE];
+	tlv_len = skb->len - LLCP_HEADER_SIZE;
+	offset = 0;
+	sdres_tlvs_len = 0;
+
+	while (offset < tlv_len) {
+		type = tlv[0];
+		length = tlv[1];
+
+		switch (type) {
+		case LLCP_TLV_SDREQ:
+			tid = tlv[2];
+			service_name = (char *) &tlv[3];
+			service_name_len = length - 1;
+
+			pr_debug("Looking for %.16s\n", service_name);
+
+			if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+			    !strncmp(service_name, "urn:nfc:sn:sdp",
+				     service_name_len)) {
+				sap = 1;
+				goto add_snl;
+			}
+
+			llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+							  service_name_len);
+			if (!llcp_sock) {
+				sap = 0;
+				goto add_snl;
+			}
+
+			/*
+			 * We found a socket but its ssap has not been reserved
+			 * yet. We need to assign it for good and send a reply.
+			 * The ssap will be freed when the socket is closed.
+			 */
+			if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+				atomic_t *client_count;
+
+				sap = nfc_llcp_reserve_sdp_ssap(local);
+
+				pr_debug("Reserving %d\n", sap);
+
+				if (sap == LLCP_SAP_MAX) {
+					sap = 0;
+					goto add_snl;
+				}
+
+				client_count =
+					&local->local_sdp_cnt[sap -
+							      LLCP_WKS_NUM_SAP];
+
+				atomic_inc(client_count);
+
+				llcp_sock->ssap = sap;
+				llcp_sock->reserved_ssap = sap;
+			} else {
+				sap = llcp_sock->ssap;
+			}
+
+			pr_debug("%p %d\n", llcp_sock, sap);
+
+add_snl:
+			sdp = nfc_llcp_build_sdres_tlv(tid, sap);
+			if (sdp == NULL)
+				goto exit;
+
+			sdres_tlvs_len += sdp->tlv_len;
+			hlist_add_head(&sdp->node, &llc_sdres_list);
+			break;
+
+		case LLCP_TLV_SDRES:
+			mutex_lock(&local->sdreq_lock);
+
+			pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
+
+			hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
+				if (sdp->tid != tlv[2])
+					continue;
+
+				sdp->sap = tlv[3];
+
+				pr_debug("Found: uri=%s, sap=%d\n",
+					 sdp->uri, sdp->sap);
+
+				hlist_del(&sdp->node);
+
+				hlist_add_head(&sdp->node, &nl_sdres_list);
+
+				break;
+			}
+
+			mutex_unlock(&local->sdreq_lock);
+			break;
+
+		default:
+			pr_err("Invalid SNL tlv value 0x%x\n", type);
+			break;
+		}
+
+		offset += length + 2;
+		tlv += length + 2;
+	}
+
+exit:
+	if (!hlist_empty(&nl_sdres_list))
+		nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+
+	if (!hlist_empty(&llc_sdres_list))
+		nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
+}
+
+static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+	u8 ptype;
+	u16 pdu_len;
+	struct sk_buff *new_skb;
+
+	if (skb->len <= LLCP_HEADER_SIZE) {
+		pr_err("Malformed AGF PDU\n");
+		return;
+	}
+
+	skb_pull(skb, LLCP_HEADER_SIZE);
+
+	while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
+		pdu_len = skb->data[0] << 8 | skb->data[1];
+
+		skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
+
+		if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
+			pr_err("Malformed AGF PDU\n");
+			return;
+		}
+
+		ptype = nfc_llcp_ptype(skb);
+
+		if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
+			goto next;
+
+		new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
+		if (new_skb == NULL) {
+			pr_err("Could not allocate PDU\n");
+			return;
+		}
+
+		memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
+
+		nfc_llcp_rx_skb(local, new_skb);
+
+		kfree_skb(new_skb);
+next:
+		skb_pull(skb, pdu_len);
+	}
+}
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+	u8 dsap, ssap, ptype;
+
+	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);
+
+	if (ptype != LLCP_PDU_SYMM)
+		print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
+			       16, 1, skb->data, skb->len, true);
+
+	switch (ptype) {
+	case LLCP_PDU_SYMM:
+		pr_debug("SYMM\n");
+		break;
+
+	case LLCP_PDU_UI:
+		pr_debug("UI\n");
+		nfc_llcp_recv_ui(local, skb);
+		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_DM:
+		pr_debug("DM\n");
+		nfc_llcp_recv_dm(local, skb);
+		break;
+
+	case LLCP_PDU_SNL:
+		pr_debug("SNL\n");
+		nfc_llcp_recv_snl(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;
+
+	case LLCP_PDU_AGF:
+		pr_debug("AGF frame\n");
+		nfc_llcp_recv_agf(local, skb);
+		break;
+	}
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+	struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+						    rx_work);
+	struct sk_buff *skb;
+
+	skb = local->rx_pending;
+	if (skb == NULL) {
+		pr_debug("No pending SKB\n");
+		return;
+	}
+
+	__net_timestamp(skb);
+
+	nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
+	nfc_llcp_rx_skb(local, skb);
+
+	schedule_work(&local->tx_work);
+	kfree_skb(local->rx_pending);
+	local->rx_pending = NULL;
+}
+
+static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+	local->rx_pending = skb;
+	del_timer(&local->link_timer);
+	schedule_work(&local->rx_work);
+}
+
+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;
+	}
+
+	__nfc_llcp_recv(local, skb);
+}
+
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+	struct nfc_llcp_local *local;
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL)
+		return -ENODEV;
+
+	__nfc_llcp_recv(local, skb);
+
+	return 0;
+}
+
+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;
+
+	local->remote_miu = LLCP_DEFAULT_MIU;
+	local->remote_lto = LLCP_DEFAULT_LTO;
+
+	/* Close and purge all existing sockets */
+	nfc_llcp_socket_release(local, true, 0);
+}
+
+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");
+
+		schedule_work(&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 nfc_llcp_local *local;
+
+	local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
+	if (local == NULL)
+		return -ENOMEM;
+
+	local->dev = ndev;
+	INIT_LIST_HEAD(&local->list);
+	kref_init(&local->ref);
+	mutex_init(&local->sdp_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);
+
+	local->rx_pending = NULL;
+	INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
+
+	INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
+
+	rwlock_init(&local->sockets.lock);
+	rwlock_init(&local->connecting_sockets.lock);
+	rwlock_init(&local->raw_sockets.lock);
+
+	local->lto = 150; /* 1500 ms */
+	local->rw = LLCP_MAX_RW;
+	local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+
+	nfc_llcp_build_gb(local);
+
+	local->remote_miu = LLCP_DEFAULT_MIU;
+	local->remote_lto = LLCP_DEFAULT_LTO;
+
+	mutex_init(&local->sdreq_lock);
+	INIT_HLIST_HEAD(&local->pending_sdreqs);
+	init_timer(&local->sdreq_timer);
+	local->sdreq_timer.data = (unsigned long) local;
+	local->sdreq_timer.function = nfc_llcp_sdreq_timer;
+	INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
+
+	list_add(&local->list, &llcp_devices);
+
+	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;
+	}
+
+	local_cleanup(local);
+
+	nfc_llcp_local_put(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_sock.c b/net/nfc/llcp_sock.c
new file mode 100644
index 0000000..7522c37
--- /dev/null
+++ b/net/nfc/llcp_sock.c
@@ -0,0 +1,1032 @@
+/*
+ * 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 int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
+{
+	DECLARE_WAITQUEUE(wait, current);
+	int err = 0;
+
+	pr_debug("sk %p", sk);
+
+	add_wait_queue(sk_sleep(sk), &wait);
+	set_current_state(TASK_INTERRUPTIBLE);
+
+	while (sk->sk_state != state) {
+		if (!timeo) {
+			err = -EINPROGRESS;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			err = sock_intr_errno(timeo);
+			break;
+		}
+
+		release_sock(sk);
+		timeo = schedule_timeout(timeo);
+		lock_sock(sk);
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		err = sock_error(sk);
+		if (err)
+			break;
+	}
+
+	__set_current_state(TASK_RUNNING);
+	remove_wait_queue(sk_sleep(sk), &wait);
+	return err;
+}
+
+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;
+
+	if (!addr || addr->sa_family != AF_NFC)
+		return -EINVAL;
+
+	pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+	memset(&llcp_addr, 0, sizeof(llcp_addr));
+	len = min_t(unsigned int, sizeof(llcp_addr), alen);
+	memcpy(&llcp_addr, addr, len);
+
+	/* 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 = nfc_llcp_local_get(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_SAP_MAX) {
+		ret = -EADDRINUSE;
+		goto put_dev;
+	}
+
+	llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+	nfc_llcp_sock_link(&local->sockets, sk);
+
+	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_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
+			      int alen)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	struct nfc_llcp_local *local;
+	struct nfc_dev *dev;
+	struct sockaddr_nfc_llcp llcp_addr;
+	int len, ret = 0;
+
+	if (!addr || addr->sa_family != AF_NFC)
+		return -EINVAL;
+
+	pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+	memset(&llcp_addr, 0, sizeof(llcp_addr));
+	len = min_t(unsigned int, sizeof(llcp_addr), alen);
+	memcpy(&llcp_addr, addr, len);
+
+	lock_sock(sk);
+
+	if (sk->sk_state != LLCP_CLOSED) {
+		ret = -EBADFD;
+		goto error;
+	}
+
+	dev = nfc_get_device(llcp_addr.dev_idx);
+	if (dev == NULL) {
+		ret = -ENODEV;
+		goto error;
+	}
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL) {
+		ret = -ENODEV;
+		goto put_dev;
+	}
+
+	llcp_sock->dev = dev;
+	llcp_sock->local = nfc_llcp_local_get(local);
+	llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+
+	nfc_llcp_sock_link(&local->raw_sockets, sk);
+
+	sk->sk_state = LLCP_BOUND;
+
+put_dev:
+	nfc_put_device(dev);
+
+error:
+	release_sock(sk);
+	return ret;
+}
+
+static int llcp_sock_listen(struct socket *sock, int backlog)
+{
+	struct sock *sk = sock->sk;
+	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;
+}
+
+static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
+			       char __user *optval, unsigned int optlen)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	u32 opt;
+	int err = 0;
+
+	pr_debug("%p optname %d\n", sk, optname);
+
+	if (level != SOL_NFC)
+		return -ENOPROTOOPT;
+
+	lock_sock(sk);
+
+	switch (optname) {
+	case NFC_LLCP_RW:
+		if (sk->sk_state == LLCP_CONNECTED ||
+		    sk->sk_state == LLCP_BOUND ||
+		    sk->sk_state == LLCP_LISTEN) {
+			err = -EINVAL;
+			break;
+		}
+
+		if (get_user(opt, (u32 __user *) optval)) {
+			err = -EFAULT;
+			break;
+		}
+
+		if (opt > LLCP_MAX_RW) {
+			err = -EINVAL;
+			break;
+		}
+
+		llcp_sock->rw = (u8) opt;
+
+		break;
+
+	case NFC_LLCP_MIUX:
+		if (sk->sk_state == LLCP_CONNECTED ||
+		    sk->sk_state == LLCP_BOUND ||
+		    sk->sk_state == LLCP_LISTEN) {
+			err = -EINVAL;
+			break;
+		}
+
+		if (get_user(opt, (u32 __user *) optval)) {
+			err = -EFAULT;
+			break;
+		}
+
+		if (opt > LLCP_MAX_MIUX) {
+			err = -EINVAL;
+			break;
+		}
+
+		llcp_sock->miux = cpu_to_be16((u16) opt);
+
+		break;
+
+	default:
+		err = -ENOPROTOOPT;
+		break;
+	}
+
+	release_sock(sk);
+
+	pr_debug("%p rw %d miux %d\n", llcp_sock,
+		 llcp_sock->rw, llcp_sock->miux);
+
+	return err;
+}
+
+static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
+			       char __user *optval, int __user *optlen)
+{
+	struct nfc_llcp_local *local;
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	int len, err = 0;
+	u16 miux, remote_miu;
+	u8 rw;
+
+	pr_debug("%p optname %d\n", sk, optname);
+
+	if (level != SOL_NFC)
+		return -ENOPROTOOPT;
+
+	if (get_user(len, optlen))
+		return -EFAULT;
+
+	local = llcp_sock->local;
+	if (!local)
+		return -ENODEV;
+
+	len = min_t(u32, len, sizeof(u32));
+
+	lock_sock(sk);
+
+	switch (optname) {
+	case NFC_LLCP_RW:
+		rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
+		if (put_user(rw, (u32 __user *) optval))
+			err = -EFAULT;
+
+		break;
+
+	case NFC_LLCP_MIUX:
+		miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
+			be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
+
+		if (put_user(miux, (u32 __user *) optval))
+			err = -EFAULT;
+
+		break;
+
+	case NFC_LLCP_REMOTE_MIU:
+		remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
+				local->remote_miu : llcp_sock->remote_miu;
+
+		if (put_user(remote_miu, (u32 __user *) optval))
+			err = -EFAULT;
+
+		break;
+
+	case NFC_LLCP_REMOTE_LTO:
+		if (put_user(local->remote_lto / 10, (u32 __user *) optval))
+			err = -EFAULT;
+
+		break;
+
+	case NFC_LLCP_REMOTE_RW:
+		if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
+			err = -EFAULT;
+
+		break;
+
+	default:
+		err = -ENOPROTOOPT;
+		break;
+	}
+
+	release_sock(sk);
+
+	if (put_user(len, optlen))
+		return -EFAULT;
+
+	return err;
+}
+
+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) {
+			list_del_init(&lsk->accept_queue);
+			sock_put(sk);
+
+			if (newsock)
+				sock_graft(sk, newsock);
+
+			release_sock(sk);
+
+			pr_debug("Returning sk state %d\n", sk->sk_state);
+
+			sk_acceptq_removed(parent);
+
+			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 *uaddr,
+			     int *len, int peer)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+	DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr);
+
+	if (llcp_sock == NULL || llcp_sock->dev == NULL)
+		return -EBADFD;
+
+	pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx,
+		 llcp_sock->dsap, llcp_sock->ssap);
+
+	memset(llcp_addr, 0, sizeof(*llcp_addr));
+	*len = sizeof(struct sockaddr_nfc_llcp);
+
+	llcp_addr->sa_family = AF_NFC;
+	llcp_addr->dev_idx = llcp_sock->dev->idx;
+	llcp_addr->target_idx = llcp_sock->target_idx;
+	llcp_addr->nfc_protocol = llcp_sock->nfc_protocol;
+	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 |
+			(sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0);
+
+	if (!skb_queue_empty(&sk->sk_receive_queue))
+		mask |= POLLIN | POLLRDNORM;
+
+	if (sk->sk_state == LLCP_CLOSED)
+		mask |= POLLHUP;
+
+	if (sk->sk_shutdown & RCV_SHUTDOWN)
+		mask |= POLLRDHUP | POLLIN | POLLRDNORM;
+
+	if (sk->sk_shutdown == SHUTDOWN_MASK)
+		mask |= POLLHUP;
+
+	if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED)
+		mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
+	else
+		set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
+
+	pr_debug("mask 0x%x\n", mask);
+
+	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;
+	}
+
+	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);
+		}
+	}
+
+	if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
+		nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
+
+	release_sock(sk);
+
+	if (sock->type == SOCK_RAW)
+		nfc_llcp_sock_unlink(&local->raw_sockets, sk);
+	else
+		nfc_llcp_sock_unlink(&local->sockets, sk);
+
+out:
+	sock_orphan(sk);
+	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)
+		return -EINVAL;
+
+	if (addr->service_name_len == 0 && addr->dsap == 0)
+		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 = nfc_llcp_local_get(local);
+	llcp_sock->remote_miu = llcp_sock->local->remote_miu;
+	llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
+	if (llcp_sock->ssap == LLCP_SAP_MAX) {
+		ret = -ENOMEM;
+		goto put_dev;
+	}
+
+	llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+	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);
+
+	nfc_llcp_sock_link(&local->connecting_sockets, sk);
+
+	ret = nfc_llcp_send_connect(llcp_sock);
+	if (ret)
+		goto sock_unlink;
+
+	sk->sk_state = LLCP_CONNECTING;
+
+	ret = sock_wait_state(sk, LLCP_CONNECTED,
+			      sock_sndtimeo(sk, flags & O_NONBLOCK));
+	if (ret && ret != -EINPROGRESS)
+		goto sock_unlink;
+
+	release_sock(sk);
+
+	return ret;
+
+sock_unlink:
+	nfc_llcp_put_ssap(local, llcp_sock->ssap);
+
+	nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+
+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_type == SOCK_DGRAM) {
+		struct sockaddr_nfc_llcp *addr =
+			(struct sockaddr_nfc_llcp *)msg->msg_name;
+
+		if (msg->msg_namelen < sizeof(*addr)) {
+			release_sock(sk);
+			return -EINVAL;
+		}
+
+		release_sock(sk);
+
+		return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
+					      msg, len);
+	}
+
+	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);
+
+	msg->msg_namelen = 0;
+
+	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 (skb_copy_datagram_iovec(cskb, 0, msg->msg_iov, copied)) {
+		if (!(flags & MSG_PEEK))
+			skb_queue_head(&sk->sk_receive_queue, skb);
+		return -EFAULT;
+	}
+
+	sock_recv_timestamp(msg, sk, skb);
+
+	if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
+		struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
+		struct sockaddr_nfc_llcp *sockaddr =
+			(struct sockaddr_nfc_llcp *) msg->msg_name;
+
+		msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
+
+		pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
+
+		memset(sockaddr, 0, sizeof(*sockaddr));
+		sockaddr->sa_family = AF_NFC;
+		sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
+		sockaddr->dsap = ui_cb->dsap;
+		sockaddr->ssap = ui_cb->ssap;
+	}
+
+	/* 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 ||
+		    sk->sk_type == SOCK_DGRAM ||
+		    sk->sk_type == SOCK_RAW) {
+			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     = nfc_llcp_setsockopt,
+	.getsockopt     = nfc_llcp_getsockopt,
+	.sendmsg        = llcp_sock_sendmsg,
+	.recvmsg        = llcp_sock_recvmsg,
+	.mmap           = sock_no_mmap,
+};
+
+static const struct proto_ops llcp_rawsock_ops = {
+	.family         = PF_NFC,
+	.owner          = THIS_MODULE,
+	.bind           = llcp_raw_sock_bind,
+	.connect        = sock_no_connect,
+	.release        = llcp_sock_release,
+	.socketpair     = sock_no_socketpair,
+	.accept         = sock_no_accept,
+	.getname        = llcp_sock_getname,
+	.poll           = llcp_sock_poll,
+	.ioctl          = sock_no_ioctl,
+	.listen         = sock_no_listen,
+	.shutdown       = sock_no_shutdown,
+	.setsockopt     = sock_no_setsockopt,
+	.getsockopt     = sock_no_getsockopt,
+	.sendmsg        = sock_no_sendmsg,
+	.recvmsg        = llcp_sock_recvmsg,
+	.mmap           = sock_no_mmap,
+};
+
+static void llcp_sock_destruct(struct sock *sk)
+{
+	struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+	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->rw = LLCP_MAX_RW + 1;
+	llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
+	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;
+	llcp_sock->reserved_ssap = LLCP_SAP_MAX;
+	nfc_llcp_socket_remote_param_init(llcp_sock);
+	skb_queue_head_init(&llcp_sock->tx_queue);
+	skb_queue_head_init(&llcp_sock->tx_pending_queue);
+	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)
+{
+	kfree(sock->service_name);
+
+	skb_queue_purge(&sock->tx_queue);
+	skb_queue_purge(&sock->tx_pending_queue);
+
+	list_del_init(&sock->accept_queue);
+
+	sock->parent = NULL;
+
+	nfc_llcp_local_put(sock->local);
+}
+
+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 &&
+	    sock->type != SOCK_RAW)
+		return -ESOCKTNOSUPPORT;
+
+	if (sock->type == SOCK_RAW)
+		sock->ops = &llcp_rawsock_ops;
+	else
+		sock->ops = &llcp_sock_ops;
+
+	sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
+	if (sk == NULL)
+		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 0000000..67f7b64
--- /dev/null
+++ b/net/nfc/nci/Kconfig
@@ -0,0 +1,11 @@
+config NFC_NCI
+	depends on NFC
+	tristate "NCI protocol support"
+	depends on m
+	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 0000000..261b52e
--- /dev/null
+++ b/net/nfc/nci/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for the Linux NFC NCI layer.
+#
+
+obj-$(CPTCFG_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 0000000..48ada0e
--- /dev/null
+++ b/net/nfc/nci/core.c
@@ -0,0 +1,973 @@
+/*
+ *  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/module.h>
+#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);
+}
+
+struct nci_set_config_param {
+	__u8	id;
+	size_t	len;
+	__u8	*val;
+};
+
+static void nci_set_config_req(struct nci_dev *ndev, unsigned long opt)
+{
+	struct nci_set_config_param *param = (struct nci_set_config_param *)opt;
+	struct nci_core_set_config_cmd cmd;
+
+	BUG_ON(param->len > NCI_MAX_PARAM_LEN);
+
+	cmd.num_params = 1;
+	cmd.param.id = param->id;
+	cmd.param.len = param->len;
+	memcpy(cmd.param.val, param->val, param->len);
+
+	nci_send_cmd(ndev, NCI_OP_CORE_SET_CONFIG_CMD, (3 + param->len), &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_B_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_set_local_general_bytes(struct nfc_dev *nfc_dev)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	struct nci_set_config_param param;
+	__u8 local_gb[NFC_MAX_GT_LEN];
+	int i;
+
+	param.val = nfc_get_local_general_bytes(nfc_dev, &param.len);
+	if ((param.val == NULL) || (param.len == 0))
+		return 0;
+
+	if (param.len > NFC_MAX_GT_LEN)
+		return -EINVAL;
+
+	for (i = 0; i < param.len; i++)
+		local_gb[param.len-1-i] = param.val[i];
+
+	param.id = NCI_PN_ATR_REQ_GEN_BYTES;
+	param.val = local_gb;
+
+	return nci_request(ndev, nci_set_config_req, (unsigned long)&param,
+			   msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
+}
+
+static int nci_start_poll(struct nfc_dev *nfc_dev,
+			  __u32 im_protocols, __u32 tm_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;
+	}
+
+	if (im_protocols & NFC_PROTO_NFC_DEP_MASK) {
+		rc = nci_set_local_general_bytes(nfc_dev);
+		if (rc) {
+			pr_err("failed to set local general bytes\n");
+			return rc;
+		}
+	}
+
+	rc = nci_request(ndev, nci_rf_discover_req, im_protocols,
+			 msecs_to_jiffies(NCI_RF_DISC_TIMEOUT));
+
+	if (!rc)
+		ndev->poll_prots = im_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,
+			       struct nfc_target *target, __u32 protocol)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	struct nci_rf_discover_select_param param;
+	struct nfc_target *nci_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) {
+			nci_target = &ndev->targets[i];
+			break;
+		}
+	}
+
+	if (!nci_target) {
+		pr_err("unable to find the selected target\n");
+		return -EINVAL;
+	}
+
+	if (!(nci_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 = nci_target->logical_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 ||
+			 protocol == NFC_PROTO_ISO14443_B)
+			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,
+				  struct nfc_target *target)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+	pr_debug("entry\n");
+
+	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_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+			   __u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	int rc;
+
+	pr_debug("target_idx %d, comm_mode %d\n", target->idx, comm_mode);
+
+	rc = nci_activate_target(nfc_dev, target, NFC_PROTO_NFC_DEP);
+	if (rc)
+		return rc;
+
+	rc = nfc_set_remote_general_bytes(nfc_dev, ndev->remote_gb,
+					  ndev->remote_gb_len);
+	if (!rc)
+		rc = nfc_dep_link_is_up(nfc_dev, target->idx, NFC_COMM_PASSIVE,
+					NFC_RF_INITIATOR);
+
+	return rc;
+}
+
+static int nci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+	pr_debug("entry\n");
+
+	nci_deactivate_target(nfc_dev, NULL);
+
+	return 0;
+}
+
+
+static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
+			  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,
+	.dep_link_up = nci_dep_link_up,
+	.dep_link_down = nci_dep_link_down,
+	.activate_target = nci_activate_target,
+	.deactivate_target = nci_deactivate_target,
+	.im_transceive = nci_transceive,
+};
+
+/* ---- 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,
+				    __u32 supported_se,
+				    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,
+					    supported_se,
+					    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));
+	}
+}
+
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c
new file mode 100644
index 0000000..76c48c5
--- /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 = NULL;
+
+		/* 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 = NULL;
+
+			kfree_skb(ndev->rx_data_reassembly);
+			ndev->rx_data_reassembly = NULL;
+
+			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 = NULL;
+	}
+
+	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 0000000..6b7fd26
--- /dev/null
+++ b/net/nfc/nci/lib.c
@@ -0,0 +1,86 @@
+/*
+ *  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>
+#include <net/nfc/nci_core.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 0000000..b2aa98e
--- /dev/null
+++ b/net/nfc/nci/ntf.c
@@ -0,0 +1,611 @@
+/*
+ *  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)
+		if (rf_tech_and_mode == NCI_NFC_A_PASSIVE_POLL_MODE)
+			protocol = NFC_PROTO_ISO14443_MASK;
+		else
+			protocol = NFC_PROTO_ISO14443_B_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_T3T)
+		protocol = NFC_PROTO_FELICA_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_NFC_DEP)
+		protocol = NFC_PROTO_NFC_DEP_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->logical_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->logical_idx = ntf->rf_discovery_id;
+		ndev->n_targets++;
+
+		pr_debug("logical idx %d, n_targets %d\n", target->logical_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 int nci_extract_activation_params_nfc_dep(struct nci_dev *ndev,
+			struct nci_rf_intf_activated_ntf *ntf, __u8 *data)
+{
+	struct activation_params_poll_nfc_dep *poll;
+	int i;
+
+	switch (ntf->activation_rf_tech_and_mode) {
+	case NCI_NFC_A_PASSIVE_POLL_MODE:
+	case NCI_NFC_F_PASSIVE_POLL_MODE:
+		poll = &ntf->activation_params.poll_nfc_dep;
+		poll->atr_res_len = min_t(__u8, *data++, 63);
+		pr_debug("atr_res_len %d\n", poll->atr_res_len);
+		if (poll->atr_res_len > 0) {
+			for (i = 0; i < poll->atr_res_len; i++)
+				poll->atr_res[poll->atr_res_len-1-i] = data[i];
+		}
+		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->logical_idx = ntf->rf_discovery_id;
+	ndev->n_targets++;
+
+	pr_debug("logical idx %d, n_targets %d\n",
+		 target->logical_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_NFC_DEP:
+			err = nci_extract_activation_params_nfc_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);
+
+		/* store general bytes to be reported later in dep_link_up */
+		if (ntf.rf_interface == NCI_RF_INTERFACE_NFC_DEP) {
+			ndev->remote_gb_len = 0;
+
+			if (ntf.activation_params_len > 0) {
+				/* ATR_RES general bytes at offset 15 */
+				ndev->remote_gb_len = min_t(__u8,
+					(ntf.activation_params
+					.poll_nfc_dep.atr_res_len
+					- NFC_ATR_RES_GT_OFFSET),
+					NFC_MAX_GT_LEN);
+				memcpy(ndev->remote_gb,
+				       (ntf.activation_params.poll_nfc_dep
+				       .atr_res + NFC_ATR_RES_GT_OFFSET),
+				       ndev->remote_gb_len);
+			}
+		}
+	}
+
+	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 = NULL;
+	}
+
+	/* 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 0000000..dd072f3
--- /dev/null
+++ b/net/nfc/nci/rsp.c
@@ -0,0 +1,238 @@
+/*
+ *  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_core_set_config_rsp_packet(struct nci_dev *ndev,
+					   struct sk_buff *skb)
+{
+	struct nci_core_set_config_rsp *rsp = (void *) skb->data;
+
+	pr_debug("status 0x%x\n", rsp->status);
+
+	nci_req_complete(ndev, rsp->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_CORE_SET_CONFIG_RSP:
+		nci_core_set_config_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 0000000..8594d0e
--- /dev/null
+++ b/net/nfc/netlink.c
@@ -0,0 +1,1195 @@
+/*
+ * 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"
+#include "llcp.h"
+
+static struct genl_multicast_group nfc_genl_event_mcgrp = {
+	.name = NFC_GENL_MCAST_EVENT_NAME,
+};
+
+static 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 },
+	[NFC_ATTR_IM_PROTOCOLS] = { .type = NLA_U32 },
+	[NFC_ATTR_TM_PROTOCOLS] = { .type = NLA_U32 },
+	[NFC_ATTR_LLC_PARAM_LTO] = { .type = NLA_U8 },
+	[NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 },
+	[NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 },
+	[NFC_ATTR_LLC_SDP] = { .type = NLA_NESTED },
+};
+
+static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = {
+	[NFC_SDP_ATTR_URI] = { .type = NLA_STRING },
+	[NFC_SDP_ATTR_SAP] = { .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_PORTID(cb->skb), 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);
+
+	if (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))
+		goto nla_put_failure;
+	if (target->nfcid1_len > 0 &&
+	    nla_put(msg, NFC_ATTR_TARGET_NFCID1, target->nfcid1_len,
+		    target->nfcid1))
+		goto nla_put_failure;
+	if (target->sensb_res_len > 0 &&
+	    nla_put(msg, NFC_ATTR_TARGET_SENSB_RES, target->sensb_res_len,
+		    target->sensb_res))
+		goto nla_put_failure;
+	if (target->sensf_res_len > 0 &&
+	    nla_put(msg, NFC_ATTR_TARGET_SENSF_RES, target->sensf_res_len,
+		    target->sensf_res))
+		goto nla_put_failure;
+
+	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;
+	}
+
+	device_lock(&dev->dev);
+
+	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++;
+	}
+
+	device_unlock(&dev->dev);
+
+	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_portid = 0;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, 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;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+
+	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_target_lost(struct nfc_dev *dev, u32 target_idx)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_TARGET_LOST);
+	if (!hdr)
+		goto free_msg;
+
+	if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
+	    nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx))
+		goto nla_put_failure;
+
+	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_tm_activated(struct nfc_dev *dev, u32 protocol)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_TM_ACTIVATED);
+	if (!hdr)
+		goto free_msg;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+	if (nla_put_u32(msg, NFC_ATTR_TM_PROTOCOLS, protocol))
+		goto nla_put_failure;
+
+	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_tm_deactivated(struct nfc_dev *dev)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_TM_DEACTIVATED);
+	if (!hdr)
+		goto free_msg;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+
+	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_added(struct nfc_dev *dev)
+{
+	struct sk_buff *msg;
+	void *hdr;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, 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;
+
+	if (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))
+		goto nla_put_failure;
+
+	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_DEFAULT_SIZE, 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;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+
+	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_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list)
+{
+	struct sk_buff *msg;
+	struct nlattr *sdp_attr, *uri_attr;
+	struct nfc_llcp_sdp_tlv *sdres;
+	struct hlist_node *n;
+	void *hdr;
+	int rc = -EMSGSIZE;
+	int i;
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
+
+	hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+			  NFC_EVENT_LLC_SDRES);
+	if (!hdr)
+		goto free_msg;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+
+	sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP);
+	if (sdp_attr == NULL) {
+		rc = -ENOMEM;
+		goto nla_put_failure;
+	}
+
+	i = 1;
+	hlist_for_each_entry_safe(sdres, n, sdres_list, node) {
+		pr_debug("uri: %s, sap: %d\n", sdres->uri, sdres->sap);
+
+		uri_attr = nla_nest_start(msg, i++);
+		if (uri_attr == NULL) {
+			rc = -ENOMEM;
+			goto nla_put_failure;
+		}
+
+		if (nla_put_u8(msg, NFC_SDP_ATTR_SAP, sdres->sap))
+			goto nla_put_failure;
+
+		if (nla_put_string(msg, NFC_SDP_ATTR_URI, sdres->uri))
+			goto nla_put_failure;
+
+		nla_nest_end(msg, uri_attr);
+
+		hlist_del(&sdres->node);
+
+		nfc_llcp_free_sdp_tlv(sdres);
+	}
+
+	nla_nest_end(msg, sdp_attr);
+
+	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);
+
+	nfc_llcp_free_sdp_tlv_list(sdres_list);
+
+	return rc;
+}
+
+static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
+				u32 portid, u32 seq,
+				struct netlink_callback *cb,
+				int flags)
+{
+	void *hdr;
+
+	hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, flags,
+			  NFC_CMD_GET_DEVICE);
+	if (!hdr)
+		return -EMSGSIZE;
+
+	if (cb)
+		genl_dump_check_consistent(cb, hdr, &nfc_genl_family);
+
+	if (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_u32(msg, NFC_ATTR_SE, dev->supported_se) ||
+	    nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) ||
+	    nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode))
+		goto nla_put_failure;
+
+	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_PORTID(cb->skb),
+					  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_DEFAULT_SIZE, 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;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+	if (rf_mode == NFC_RF_INITIATOR &&
+	    nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx))
+		goto nla_put_failure;
+	if (nla_put_u8(msg, NFC_ATTR_COMM_MODE, comm_mode) ||
+	    nla_put_u8(msg, NFC_ATTR_RF_MODE, rf_mode))
+		goto nla_put_failure;
+
+	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_DEFAULT_SIZE, 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;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+		goto nla_put_failure;
+
+	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_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg) {
+		rc = -ENOMEM;
+		goto out_putdev;
+	}
+
+	rc = nfc_genl_send_device(msg, dev, genl_info_snd_portid(info), 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 im_protocols = 0, tm_protocols = 0;
+
+	pr_debug("Poll start\n");
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+	    ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
+	      !info->attrs[NFC_ATTR_PROTOCOLS]) &&
+	      !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	if (info->attrs[NFC_ATTR_TM_PROTOCOLS])
+		tm_protocols = nla_get_u32(info->attrs[NFC_ATTR_TM_PROTOCOLS]);
+
+	if (info->attrs[NFC_ATTR_IM_PROTOCOLS])
+		im_protocols = nla_get_u32(info->attrs[NFC_ATTR_IM_PROTOCOLS]);
+	else if (info->attrs[NFC_ATTR_PROTOCOLS])
+		im_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, im_protocols, tm_protocols);
+	if (!rc)
+		dev->genl_data.poll_req_portid = genl_info_snd_portid(info);
+
+	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;
+
+	device_lock(&dev->dev);
+
+	if (!dev->polling) {
+		device_unlock(&dev->dev);
+		return -EINVAL;
+	}
+
+	device_unlock(&dev->dev);
+
+	mutex_lock(&dev->genl_data.genl_data_mutex);
+
+	if (dev->genl_data.poll_req_portid != genl_info_snd_portid(info)) {
+		rc = -EBUSY;
+		goto out;
+	}
+
+	rc = nfc_stop_poll(dev);
+	dev->genl_data.poll_req_portid = 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 int nfc_genl_send_params(struct sk_buff *msg,
+				struct nfc_llcp_local *local,
+				u32 portid, u32 seq)
+{
+	void *hdr;
+
+	hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, 0,
+			  NFC_CMD_LLC_GET_PARAMS);
+	if (!hdr)
+		return -EMSGSIZE;
+
+	if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, local->dev->idx) ||
+	    nla_put_u8(msg, NFC_ATTR_LLC_PARAM_LTO, local->lto) ||
+	    nla_put_u8(msg, NFC_ATTR_LLC_PARAM_RW, local->rw) ||
+	    nla_put_u16(msg, NFC_ATTR_LLC_PARAM_MIUX, be16_to_cpu(local->miux)))
+		goto nla_put_failure;
+
+	return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+
+	genlmsg_cancel(msg, hdr);
+	return -EMSGSIZE;
+}
+
+static int nfc_genl_llc_get_params(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	int rc = 0;
+	struct sk_buff *msg = NULL;
+	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;
+
+	device_lock(&dev->dev);
+
+	local = nfc_llcp_find_local(dev);
+	if (!local) {
+		rc = -ENODEV;
+		goto exit;
+	}
+
+	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+	if (!msg) {
+		rc = -ENOMEM;
+		goto exit;
+	}
+
+	rc = nfc_genl_send_params(msg, local, genl_info_snd_portid(info), info->snd_seq);
+
+exit:
+	device_unlock(&dev->dev);
+
+	nfc_put_device(dev);
+
+	if (rc < 0) {
+		if (msg)
+			nlmsg_free(msg);
+
+		return rc;
+	}
+
+	return genlmsg_reply(msg, info);
+}
+
+static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	u8 rw = 0;
+	u16 miux = 0;
+	u32 idx;
+	int rc = 0;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+	    (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] &&
+	     !info->attrs[NFC_ATTR_LLC_PARAM_RW] &&
+	     !info->attrs[NFC_ATTR_LLC_PARAM_MIUX]))
+		return -EINVAL;
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) {
+		rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]);
+
+		if (rw > LLCP_MAX_RW)
+			return -EINVAL;
+	}
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) {
+		miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]);
+
+		if (miux > LLCP_MAX_MIUX)
+			return -EINVAL;
+	}
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev)
+		return -ENODEV;
+
+	device_lock(&dev->dev);
+
+	local = nfc_llcp_find_local(dev);
+	if (!local) {
+		nfc_put_device(dev);
+		rc = -ENODEV;
+		goto exit;
+	}
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) {
+		if (dev->dep_link_up) {
+			rc = -EINPROGRESS;
+			goto exit;
+		}
+
+		local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]);
+	}
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_RW])
+		local->rw = rw;
+
+	if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX])
+		local->miux = cpu_to_be16(miux);
+
+exit:
+	device_unlock(&dev->dev);
+
+	nfc_put_device(dev);
+
+	return rc;
+}
+
+static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info)
+{
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	struct nlattr *attr, *sdp_attrs[NFC_SDP_ATTR_MAX+1];
+	u32 idx;
+	u8 tid;
+	char *uri;
+	int rc = 0, rem;
+	size_t uri_len, tlvs_len;
+	struct hlist_head sdreq_list;
+	struct nfc_llcp_sdp_tlv *sdreq;
+
+	if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+	    !info->attrs[NFC_ATTR_LLC_SDP])
+		return -EINVAL;
+
+	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+	dev = nfc_get_device(idx);
+	if (!dev) {
+		rc = -ENODEV;
+		goto exit;
+	}
+
+	device_lock(&dev->dev);
+
+	if (dev->dep_link_up == false) {
+		rc = -ENOLINK;
+		goto exit;
+	}
+
+	local = nfc_llcp_find_local(dev);
+	if (!local) {
+		nfc_put_device(dev);
+		rc = -ENODEV;
+		goto exit;
+	}
+
+	INIT_HLIST_HEAD(&sdreq_list);
+
+	tlvs_len = 0;
+
+	nla_for_each_nested(attr, info->attrs[NFC_ATTR_LLC_SDP], rem) {
+		rc = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX, attr,
+				      nfc_sdp_genl_policy);
+
+		if (rc != 0) {
+			rc = -EINVAL;
+			goto exit;
+		}
+
+		if (!sdp_attrs[NFC_SDP_ATTR_URI])
+			continue;
+
+		uri_len = nla_len(sdp_attrs[NFC_SDP_ATTR_URI]);
+		if (uri_len == 0)
+			continue;
+
+		uri = nla_data(sdp_attrs[NFC_SDP_ATTR_URI]);
+		if (uri == NULL || *uri == 0)
+			continue;
+
+		tid = local->sdreq_next_tid++;
+
+		sdreq = nfc_llcp_build_sdreq_tlv(tid, uri, uri_len);
+		if (sdreq == NULL) {
+			rc = -ENOMEM;
+			goto exit;
+		}
+
+		tlvs_len += sdreq->tlv_len;
+
+		hlist_add_head(&sdreq->node, &sdreq_list);
+	}
+
+	if (hlist_empty(&sdreq_list)) {
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	rc = nfc_llcp_send_snl_sdreq(local, &sdreq_list, tlvs_len);
+exit:
+	device_unlock(&dev->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,
+	},
+	{
+		.cmd = NFC_CMD_LLC_GET_PARAMS,
+		.doit = nfc_genl_llc_get_params,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_LLC_SET_PARAMS,
+		.doit = nfc_genl_llc_set_params,
+		.policy = nfc_genl_policy,
+	},
+	{
+		.cmd = NFC_CMD_LLC_SDREQ,
+		.doit = nfc_genl_llc_sdreq,
+		.policy = nfc_genl_policy,
+	},
+};
+
+
+struct urelease_work {
+	struct	work_struct w;
+	int	portid;
+};
+
+static void nfc_urelease_event_work(struct work_struct *work)
+{
+	struct urelease_work *w = container_of(work, struct urelease_work, w);
+	struct class_dev_iter iter;
+	struct nfc_dev *dev;
+
+	pr_debug("portid %d\n", w->portid);
+
+	mutex_lock(&nfc_devlist_mutex);
+
+	nfc_device_iter_init(&iter);
+	dev = nfc_device_iter_next(&iter);
+
+	while (dev) {
+		mutex_lock(&dev->genl_data.genl_data_mutex);
+
+		if (dev->genl_data.poll_req_portid == w->portid) {
+			nfc_stop_poll(dev);
+			dev->genl_data.poll_req_portid = 0;
+		}
+
+		mutex_unlock(&dev->genl_data.genl_data_mutex);
+
+		dev = nfc_device_iter_next(&iter);
+	}
+
+	nfc_device_iter_exit(&iter);
+
+	mutex_unlock(&nfc_devlist_mutex);
+
+	kfree(w);
+}
+
+static int nfc_genl_rcv_nl_event(struct notifier_block *this,
+				 unsigned long event, void *ptr)
+{
+	struct netlink_notify *n = ptr;
+	struct urelease_work *w;
+
+	if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC)
+		goto out;
+
+	pr_debug("NETLINK_URELEASE event from id %d\n", netlink_notify_portid(n));
+
+	w = kmalloc(sizeof(*w), GFP_ATOMIC);
+	if (w) {
+		INIT_WORK((struct work_struct *) w, nfc_urelease_event_work);
+		w->portid = netlink_notify_portid(n);
+		schedule_work((struct work_struct *) w);
+	}
+
+out:
+	return NOTIFY_DONE;
+}
+
+void nfc_genl_data_init(struct nfc_genl_data *genl_data)
+{
+	genl_data->poll_req_portid = 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 0000000..afa1f84
--- /dev/null
+++ b/net/nfc/nfc.h
@@ -0,0 +1,142 @@
+/*
+ * 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))
+
+struct nfc_llcp_sdp_tlv;
+
+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 nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+int __init nfc_llcp_init(void);
+void nfc_llcp_exit(void);
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head);
+
+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_target_lost(struct nfc_dev *dev, u32 target_idx);
+
+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);
+
+int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol);
+int nfc_genl_tm_deactivated(struct nfc_dev *dev);
+
+int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list);
+
+struct nfc_dev *nfc_get_device(unsigned int 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 im_protocols, u32 tm_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 0000000..313bf1b
--- /dev/null
+++ b/net/nfc/rawsock.c
@@ -0,0 +1,349 @@
+/*
+ * 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;
+	}
+
+	if (addr->target_idx > dev->target_next_idx - 1 ||
+	    addr->target_idx < dev->target_next_idx - dev->n_targets) {
+		rc = -EINVAL;
+		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);
+}