Merge tag 'nfc-next-4.4-2' of git://git.kernel.org/pub/scm/linux/kernel/git/sameo/nfc-next

Samuel Ortiz says:

====================
NFC 4.4 pull request

This is the NFC pull request for 4.4.

It's a bit bigger than usual, the 3 main culprits being:

- A new driver for Intel's Fields Peak NCI chipset. In order to
  support this chipset we had to export a few NCI routines and
  extend the driver NCI ops to not only support proprietary
  commands but also core ones.

- Support for vendor commands for both STM drivers, st-nci
  and st21nfca. Those vendor commands allow to run factory tests
  through the NFC netlink interface.

- New i2c and SPI support for the Marvell driver, together with
  firmware download support for this driver's core.

Besides that we also have:

- A few file renames in the STM drivers, to keep the naming
  consistent between drivers.

- Some improvements and fixes on the NCI HCI layer, mostly to
  properly reach a secure element over a legacy HCI link.

- A few fixes for the s3fwrn5 and trf7970a drivers.
====================

Signed-off-by: David S. Miller <davem@davemloft.net>

74 files changed, 4380 insertions, 481 deletions

diff --git a/Documentation/devicetree/bindings/net/nfc/nfcmrvl.txt b/Documentation/devicetree/bindings/net/nfc/nfcmrvl.txt
index 7c4a0cc370cf..76df9173825a 100644
--- a/Documentation/devicetree/bindings/net/nfc/nfcmrvl.txt
+++ b/Documentation/devicetree/bindings/net/nfc/nfcmrvl.txt
@@ -1,7 +1,10 @@
 * Marvell International Ltd. NCI NFC Controller
 
 Required properties:
-- compatible: Should be "mrvl,nfc-uart".
+- compatible: Should be:
+  - "marvell,nfc-uart" or "mrvl,nfc-uart" for UART devices
+  - "marvell,nfc-i2c" for I2C devices
+  - "marvell,nfc-spi" for SPI devices
 
 Optional SoC specific properties:
 - pinctrl-names: Contains only one value - "default".
@@ -13,13 +16,19 @@ Optional UART-based chip specific properties:
 - flow-control: Specifies that the chip is using RTS/CTS.
 - break-control: Specifies that the chip needs specific break management.
 
+Optional I2C-based chip specific properties:
+- i2c-int-falling: Specifies that the chip read event shall be trigged on
+  		   falling edge.
+- i2c-int-rising: Specifies that the chip read event shall be trigged on
+  		  rising edge.
+
 Example (for ARM-based BeagleBoard Black with 88W8887 on UART5):
 
 &uart5 {
 	status = "okay";
 
 	nfcmrvluart: nfcmrvluart@5 {
-		compatible = "mrvl,nfc-uart";
+		compatible = "marvell,nfc-uart";
 
 		reset-n-io = <&gpio3 16 0>;
 
@@ -27,3 +36,51 @@ Example (for ARM-based BeagleBoard Black with 88W8887 on UART5):
 		flow-control;
         }
 };
+
+
+Example (for ARM-based BeagleBoard Black with 88W8887 on I2C1):
+
+&i2c1 {
+	status = "okay";
+	clock-frequency = <400000>;
+
+	nfcmrvli2c0: i2c@1 {
+		compatible = "marvell,nfc-i2c";
+
+		reg = <0x8>;
+
+		/* I2C INT configuration */
+		interrupt-parent = <&gpio3>;
+		interrupts = <21 0>;
+
+		/* I2C INT trigger configuration */
+		i2c-int-rising;
+
+		/* Reset IO */
+		reset-n-io = <&gpio3 19 0>;
+	};
+};
+
+
+Example (for ARM-based BeagleBoard Black on SPI0):
+
+&spi0 {
+
+	mrvlnfcspi0: spi@0 {
+		compatible = "marvell,nfc-spi";
+
+		reg = <0>;
+
+		/* SPI Bus configuration */
+		spi-max-frequency = <3000000>;
+		spi-cpha;
+		spi-cpol;
+
+		/* SPI INT configuration */
+		interrupt-parent = <&gpio1>;
+		interrupts = <17 0>;
+
+		/* Reset IO */
+       		reset-n-io = <&gpio3 19 0>;
+	};
+};
diff --git a/Documentation/devicetree/bindings/net/nfc/st-nci-i2c.txt b/Documentation/devicetree/bindings/net/nfc/st-nci-i2c.txt
index d707588ed734..263732e8879f 100644
--- a/Documentation/devicetree/bindings/net/nfc/st-nci-i2c.txt
+++ b/Documentation/devicetree/bindings/net/nfc/st-nci-i2c.txt
@@ -11,6 +11,10 @@ Required properties:
 Optional SoC Specific Properties:
 - pinctrl-names: Contains only one value - "default".
 - pintctrl-0: Specifies the pin control groups used for this controller.
+- ese-present: Specifies that an ese is physically connected to the nfc
+controller.
+- uicc-present: Specifies that the uicc swp signal can be physically
+connected to the nfc controller.
 
 Example (for ARM-based BeagleBoard xM with ST21NFCB on I2C2):
 
@@ -29,5 +33,8 @@ Example (for ARM-based BeagleBoard xM with ST21NFCB on I2C2):
 		interrupts = <2 IRQ_TYPE_LEVEL_HIGH>;
 
 		reset-gpios = <&gpio5 29 GPIO_ACTIVE_HIGH>;
+
+		ese-present;
+		uicc-present;
 	};
 };
diff --git a/Documentation/devicetree/bindings/net/nfc/st-nci-spi.txt b/Documentation/devicetree/bindings/net/nfc/st-nci-spi.txt
index 525681b6dc39..711ca85a363d 100644
--- a/Documentation/devicetree/bindings/net/nfc/st-nci-spi.txt
+++ b/Documentation/devicetree/bindings/net/nfc/st-nci-spi.txt
@@ -2,7 +2,7 @@
 
 Required properties:
 - compatible: Should be "st,st21nfcb-spi"
-- spi-max-frequency: Maximum SPI frequency (<= 10000000).
+- spi-max-frequency: Maximum SPI frequency (<= 4000000).
 - interrupt-parent: phandle for the interrupt gpio controller
 - interrupts: GPIO interrupt to which the chip is connected
 - reset-gpios: Output GPIO pin used to reset the ST21NFCB
@@ -10,6 +10,10 @@ Required properties:
 Optional SoC Specific Properties:
 - pinctrl-names: Contains only one value - "default".
 - pintctrl-0: Specifies the pin control groups used for this controller.
+- ese-present: Specifies that an ese is physically connected to the nfc
+controller.
+- uicc-present: Specifies that the uicc swp signal can be physically
+connected to the nfc controller.
 
 Example (for ARM-based BeagleBoard xM with ST21NFCB on SPI4):
 
@@ -27,5 +31,8 @@ Example (for ARM-based BeagleBoard xM with ST21NFCB on SPI4):
 		interrupts = <2 IRQ_TYPE_EDGE_RISING>;
 
 		reset-gpios = <&gpio5 29 GPIO_ACTIVE_HIGH>;
+
+		ese-present;
+		uicc-present;
 	};
 };
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index 6639cd1cae36..0d6003dee3af 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -68,6 +68,7 @@ config NFC_PORT100
 
 	  If unsure, say N.
 
+source "drivers/nfc/fdp/Kconfig"
 source "drivers/nfc/pn544/Kconfig"
 source "drivers/nfc/microread/Kconfig"
 source "drivers/nfc/nfcmrvl/Kconfig"
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index 2757fe1b8aa5..e3621416a48e 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -2,6 +2,7 @@
 # Makefile for nfc devices
 #
 
+obj-$(CONFIG_NFC_FDP)		+= fdp/
 obj-$(CONFIG_NFC_PN544)		+= pn544/
 obj-$(CONFIG_NFC_MICROREAD)	+= microread/
 obj-$(CONFIG_NFC_PN533)		+= pn533.o
diff --git a/drivers/nfc/fdp/Kconfig b/drivers/nfc/fdp/Kconfig
new file mode 100644
index 000000000000..fbccd9dd887d
--- /dev/null
+++ b/drivers/nfc/fdp/Kconfig
@@ -0,0 +1,23 @@
+config NFC_FDP
+	tristate "Intel FDP NFC driver"
+	depends on NFC_NCI
+	select CRC_CCITT
+	default n
+	---help---
+	  Intel Fields Peak NFC controller core driver.
+	  This is a driver based on the NCI NFC kernel layers.
+
+	  To compile this driver as a module, choose m here. The module will
+	  be called fdp.
+	  Say N if unsure.
+
+config NFC_FDP_I2C
+	tristate "NFC FDP i2c support"
+	depends on NFC_FDP && I2C
+	---help---
+	  This module adds support for the Intel Fields Peak NFC controller
+	  i2c interface.
+	  Select this if your platform is using the i2c bus.
+
+	  If you choose to build a module, it'll be called fdp_i2c.
+	  Say N if unsure.
diff --git a/drivers/nfc/fdp/Makefile b/drivers/nfc/fdp/Makefile
new file mode 100644
index 000000000000..e79d51bdeec7
--- /dev/null
+++ b/drivers/nfc/fdp/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for FDP NCI based NFC driver
+#
+
+obj-$(CONFIG_NFC_FDP)     += fdp.o
+obj-$(CONFIG_NFC_FDP_I2C) += fdp_i2c.o
+
+fdp_i2c-objs  = i2c.o
+
diff --git a/drivers/nfc/fdp/fdp.c b/drivers/nfc/fdp/fdp.c
new file mode 100644
index 000000000000..ccb07a1b153d
--- /dev/null
+++ b/drivers/nfc/fdp/fdp.c
@@ -0,0 +1,817 @@
+/* -------------------------------------------------------------------------
+ * Copyright (C) 2014-2016, Intel Corporation
+ *
+ *  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.
+ * -------------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/nfc.h>
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <net/nfc/nci_core.h>
+
+#include "fdp.h"
+
+#define FDP_OTP_PATCH_NAME			"otp.bin"
+#define FDP_RAM_PATCH_NAME			"ram.bin"
+#define FDP_FW_HEADER_SIZE			576
+#define FDP_FW_UPDATE_SLEEP			1000
+
+#define NCI_GET_VERSION_TIMEOUT			8000
+#define NCI_PATCH_REQUEST_TIMEOUT		8000
+#define FDP_PATCH_CONN_DEST			0xC2
+#define FDP_PATCH_CONN_PARAM_TYPE		0xA0
+
+#define NCI_PATCH_TYPE_RAM			0x00
+#define NCI_PATCH_TYPE_OTP			0x01
+#define NCI_PATCH_TYPE_EOT			0xFF
+
+#define NCI_PARAM_ID_FW_RAM_VERSION		0xA0
+#define NCI_PARAM_ID_FW_OTP_VERSION		0xA1
+#define NCI_PARAM_ID_OTP_LIMITED_VERSION	0xC5
+#define NCI_PARAM_ID_KEY_INDEX_ID		0xC6
+
+#define NCI_GID_PROP				0x0F
+#define NCI_OP_PROP_PATCH_OID			0x08
+#define NCI_OP_PROP_SET_PDATA_OID		0x23
+
+struct fdp_nci_info {
+	struct nfc_phy_ops *phy_ops;
+	struct fdp_i2c_phy *phy;
+	struct nci_dev *ndev;
+
+	const struct firmware *otp_patch;
+	const struct firmware *ram_patch;
+	u32 otp_patch_version;
+	u32 ram_patch_version;
+
+	u32 otp_version;
+	u32 ram_version;
+	u32 limited_otp_version;
+	u8 key_index;
+
+	u8 *fw_vsc_cfg;
+	u8 clock_type;
+	u32 clock_freq;
+
+	atomic_t data_pkt_counter;
+	void (*data_pkt_counter_cb)(struct nci_dev *ndev);
+	u8 setup_patch_sent;
+	u8 setup_patch_ntf;
+	u8 setup_patch_status;
+	u8 setup_reset_ntf;
+	wait_queue_head_t setup_wq;
+};
+
+static u8 nci_core_get_config_otp_ram_version[5] = {
+	0x04,
+	NCI_PARAM_ID_FW_RAM_VERSION,
+	NCI_PARAM_ID_FW_OTP_VERSION,
+	NCI_PARAM_ID_OTP_LIMITED_VERSION,
+	NCI_PARAM_ID_KEY_INDEX_ID
+};
+
+struct nci_core_get_config_rsp {
+	u8 status;
+	u8 count;
+	u8 data[0];
+};
+
+static int fdp_nci_create_conn(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct core_conn_create_dest_spec_params param;
+	int r;
+
+	/* proprietary destination specific paramerer without value */
+	param.type = FDP_PATCH_CONN_PARAM_TYPE;
+	param.length = 0x00;
+
+	r = nci_core_conn_create(info->ndev, FDP_PATCH_CONN_DEST, 1,
+				 sizeof(param), &param);
+	if (r)
+		return r;
+
+	return nci_get_conn_info_by_id(ndev, 0);
+}
+
+static inline int fdp_nci_get_versions(struct nci_dev *ndev)
+{
+	return nci_core_cmd(ndev, NCI_OP_CORE_GET_CONFIG_CMD,
+			    sizeof(nci_core_get_config_otp_ram_version),
+			    (__u8 *) &nci_core_get_config_otp_ram_version);
+}
+
+static inline int fdp_nci_patch_cmd(struct nci_dev *ndev, u8 type)
+{
+	return nci_prop_cmd(ndev, NCI_OP_PROP_PATCH_OID, sizeof(type), &type);
+}
+
+static inline int fdp_nci_set_production_data(struct nci_dev *ndev, u8 len,
+					      char *data)
+{
+	return nci_prop_cmd(ndev, NCI_OP_PROP_SET_PDATA_OID, len, data);
+}
+
+static int fdp_nci_set_clock(struct nci_dev *ndev, u8 clock_type,
+			     u32 clock_freq)
+{
+	u32 fc = 13560;
+	u32 nd, num, delta;
+	char data[9];
+
+	nd = (24 * fc) / clock_freq;
+	delta = 24 * fc - nd * clock_freq;
+	num = (32768 * delta) / clock_freq;
+
+	data[0] = 0x00;
+	data[1] = 0x00;
+	data[2] = 0x00;
+
+	data[3] = 0x10;
+	data[4] = 0x04;
+	data[5] = num & 0xFF;
+	data[6] = (num >> 8) & 0xff;
+	data[7] = nd;
+	data[8] = clock_type;
+
+	return fdp_nci_set_production_data(ndev, 9, data);
+}
+
+static void fdp_nci_send_patch_cb(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+
+	info->setup_patch_sent = 1;
+	wake_up(&info->setup_wq);
+}
+
+/**
+ * Register a packet sent counter and a callback
+ *
+ * We have no other way of knowing when all firmware packets were sent out
+ * on the i2c bus. We need to know that in order to close the connection and
+ * send the patch end message.
+ */
+static void fdp_nci_set_data_pkt_counter(struct nci_dev *ndev,
+				  void (*cb)(struct nci_dev *ndev), int count)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "NCI data pkt counter %d\n", count);
+	atomic_set(&info->data_pkt_counter, count);
+	info->data_pkt_counter_cb = cb;
+}
+
+/**
+ * The device is expecting a stream of packets. All packets need to
+ * have the PBF flag set to 0x0 (last packet) even if the firmware
+ * file is segmented and there are multiple packets. If we give the
+ * whole firmware to nci_send_data it will segment it and it will set
+ * the PBF flag to 0x01 so we need to do the segmentation here.
+ *
+ * The firmware will be analyzed and applied when we send NCI_OP_PROP_PATCH_CMD
+ * command with NCI_PATCH_TYPE_EOT parameter. The device will send a
+ * NFCC_PATCH_NTF packaet and a NCI_OP_CORE_RESET_NTF packet.
+ */
+static int fdp_nci_send_patch(struct nci_dev *ndev, u8 conn_id, u8 type)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	const struct firmware *fw;
+	struct sk_buff *skb;
+	unsigned long len;
+	u8 max_size, payload_size;
+	int rc = 0;
+
+	if ((type == NCI_PATCH_TYPE_OTP && !info->otp_patch) ||
+	    (type == NCI_PATCH_TYPE_RAM && !info->ram_patch))
+		return -EINVAL;
+
+	if (type == NCI_PATCH_TYPE_OTP)
+		fw = info->otp_patch;
+	else
+		fw = info->ram_patch;
+
+	max_size = nci_conn_max_data_pkt_payload_size(ndev, conn_id);
+	if (max_size <= 0)
+		return -EINVAL;
+
+	len = fw->size;
+
+	fdp_nci_set_data_pkt_counter(ndev, fdp_nci_send_patch_cb,
+				     DIV_ROUND_UP(fw->size, max_size));
+
+	while (len) {
+
+		payload_size = min_t(unsigned long, (unsigned long) max_size,
+				     len);
+
+		skb = nci_skb_alloc(ndev, (NCI_CTRL_HDR_SIZE + payload_size),
+				    GFP_KERNEL);
+		if (!skb) {
+			fdp_nci_set_data_pkt_counter(ndev, NULL, 0);
+			return -ENOMEM;
+		}
+
+
+		skb_reserve(skb, NCI_CTRL_HDR_SIZE);
+
+		memcpy(skb_put(skb, payload_size), fw->data + (fw->size - len),
+		       payload_size);
+
+		rc = nci_send_data(ndev, conn_id, skb);
+
+		if (rc) {
+			fdp_nci_set_data_pkt_counter(ndev, NULL, 0);
+			return rc;
+		}
+
+		len -= payload_size;
+	}
+
+	return rc;
+}
+
+static int fdp_nci_open(struct nci_dev *ndev)
+{
+	int r;
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	r = info->phy_ops->enable(info->phy);
+
+	return r;
+}
+
+static int fdp_nci_close(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+	return 0;
+}
+
+static int fdp_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (atomic_dec_and_test(&info->data_pkt_counter))
+		info->data_pkt_counter_cb(ndev);
+
+	return info->phy_ops->write(info->phy, skb);
+}
+
+int fdp_nci_recv_frame(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+	return nci_recv_frame(ndev, skb);
+}
+EXPORT_SYMBOL(fdp_nci_recv_frame);
+
+static int fdp_nci_request_firmware(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	u8 *data;
+	int r;
+
+	r = request_firmware(&info->ram_patch, FDP_RAM_PATCH_NAME, dev);
+	if (r < 0) {
+		nfc_err(dev, "RAM patch request error\n");
+		goto error;
+	}
+
+	data = (u8 *) info->ram_patch->data;
+	info->ram_patch_version =
+		data[FDP_FW_HEADER_SIZE] |
+		(data[FDP_FW_HEADER_SIZE + 1] << 8) |
+		(data[FDP_FW_HEADER_SIZE + 2] << 16) |
+		(data[FDP_FW_HEADER_SIZE + 3] << 24);
+
+	dev_dbg(dev, "RAM patch version: %d, size: %d\n",
+		  info->ram_patch_version, (int) info->ram_patch->size);
+
+
+	r = request_firmware(&info->otp_patch, FDP_OTP_PATCH_NAME, dev);
+	if (r < 0) {
+		nfc_err(dev, "OTP patch request error\n");
+		goto out;
+	}
+
+	data = (u8 *) info->otp_patch->data;
+	info->otp_patch_version =
+		data[FDP_FW_HEADER_SIZE] |
+		(data[FDP_FW_HEADER_SIZE + 1] << 8) |
+		(data[FDP_FW_HEADER_SIZE+2] << 16) |
+		(data[FDP_FW_HEADER_SIZE+3] << 24);
+
+	dev_dbg(dev, "OTP patch version: %d, size: %d\n",
+		 info->otp_patch_version, (int) info->otp_patch->size);
+out:
+	return 0;
+error:
+	return r;
+}
+
+static void fdp_nci_release_firmware(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+
+	if (info->otp_patch) {
+		release_firmware(info->otp_patch);
+		info->otp_patch = NULL;
+	}
+
+	if (info->ram_patch) {
+		release_firmware(info->ram_patch);
+		info->otp_patch = NULL;
+	}
+}
+
+static int fdp_nci_patch_otp(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	u8 conn_id;
+	int r = 0;
+
+	if (info->otp_version >= info->otp_patch_version)
+		goto out;
+
+	info->setup_patch_sent = 0;
+	info->setup_reset_ntf = 0;
+	info->setup_patch_ntf = 0;
+
+	/* Patch init request */
+	r = fdp_nci_patch_cmd(ndev, NCI_PATCH_TYPE_OTP);
+	if (r)
+		goto out;
+
+	/* Patch data connection creation */
+	conn_id = fdp_nci_create_conn(ndev);
+	if (conn_id < 0) {
+		r = conn_id;
+		goto out;
+	}
+
+	/* Send the patch over the data connection */
+	r = fdp_nci_send_patch(ndev, conn_id, NCI_PATCH_TYPE_OTP);
+	if (r)
+		goto out;
+
+	/* Wait for all the packets to be send over i2c */
+	wait_event_interruptible(info->setup_wq,
+				 info->setup_patch_sent == 1);
+
+	/* make sure that the NFCC processed the last data packet */
+	msleep(FDP_FW_UPDATE_SLEEP);
+
+	/* Close the data connection */
+	r = nci_core_conn_close(info->ndev, conn_id);
+	if (r)
+		goto out;
+
+	/* Patch finish message */
+	if (fdp_nci_patch_cmd(ndev, NCI_PATCH_TYPE_EOT)) {
+		nfc_err(dev, "OTP patch error 0x%x\n", r);
+		r = -EINVAL;
+		goto out;
+	}
+
+	/* If the patch notification didn't arrive yet, wait for it */
+	wait_event_interruptible(info->setup_wq, info->setup_patch_ntf);
+
+	/* Check if the patching was successful */
+	r = info->setup_patch_status;
+	if (r) {
+		nfc_err(dev, "OTP patch error 0x%x\n", r);
+		r = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * We need to wait for the reset notification before we
+	 * can continue
+	 */
+	wait_event_interruptible(info->setup_wq, info->setup_reset_ntf);
+
+out:
+	return r;
+}
+
+static int fdp_nci_patch_ram(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	u8 conn_id;
+	int r = 0;
+
+	if (info->ram_version >= info->ram_patch_version)
+		goto out;
+
+	info->setup_patch_sent = 0;
+	info->setup_reset_ntf = 0;
+	info->setup_patch_ntf = 0;
+
+	/* Patch init request */
+	r = fdp_nci_patch_cmd(ndev, NCI_PATCH_TYPE_RAM);
+	if (r)
+		goto out;
+
+	/* Patch data connection creation */
+	conn_id = fdp_nci_create_conn(ndev);
+	if (conn_id < 0) {
+		r = conn_id;
+		goto out;
+	}
+
+	/* Send the patch over the data connection */
+	r = fdp_nci_send_patch(ndev, conn_id, NCI_PATCH_TYPE_RAM);
+	if (r)
+		goto out;
+
+	/* Wait for all the packets to be send over i2c */
+	wait_event_interruptible(info->setup_wq,
+				 info->setup_patch_sent == 1);
+
+	/* make sure that the NFCC processed the last data packet */
+	msleep(FDP_FW_UPDATE_SLEEP);
+
+	/* Close the data connection */
+	r = nci_core_conn_close(info->ndev, conn_id);
+	if (r)
+		goto out;
+
+	/* Patch finish message */
+	if (fdp_nci_patch_cmd(ndev, NCI_PATCH_TYPE_EOT)) {
+		nfc_err(dev, "RAM patch error 0x%x\n", r);
+		r = -EINVAL;
+		goto out;
+	}
+
+	/* If the patch notification didn't arrive yet, wait for it */
+	wait_event_interruptible(info->setup_wq, info->setup_patch_ntf);
+
+	/* Check if the patching was successful */
+	r = info->setup_patch_status;
+	if (r) {
+		nfc_err(dev, "RAM patch error 0x%x\n", r);
+		r = -EINVAL;
+		goto out;
+	}
+
+	/*
+	 * We need to wait for the reset notification before we
+	 * can continue
+	 */
+	wait_event_interruptible(info->setup_wq, info->setup_reset_ntf);
+
+out:
+	return r;
+}
+
+static int fdp_nci_setup(struct nci_dev *ndev)
+{
+	/* Format: total length followed by an NCI packet */
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	int r;
+	u8 patched = 0;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	r = nci_core_init(ndev);
+	if (r)
+		goto error;
+
+	/* Get RAM and OTP version */
+	r = fdp_nci_get_versions(ndev);
+	if (r)
+		goto error;
+
+	/* Load firmware from disk */
+	r = fdp_nci_request_firmware(ndev);
+	if (r)
+		goto error;
+
+	/* Update OTP */
+	if (info->otp_version < info->otp_patch_version) {
+		r = fdp_nci_patch_otp(ndev);
+		if (r)
+			goto error;
+		patched = 1;
+	}
+
+	/* Update RAM */
+	if (info->ram_version < info->ram_patch_version) {
+		r = fdp_nci_patch_ram(ndev);
+		if (r)
+			goto error;
+		patched = 1;
+	}
+
+	/* Release the firmware buffers */
+	fdp_nci_release_firmware(ndev);
+
+	/* If a patch was applied the new version is checked */
+	if (patched) {
+		r = nci_core_init(ndev);
+		if (r)
+			goto error;
+
+		r = fdp_nci_get_versions(ndev);
+		if (r)
+			goto error;
+
+		if (info->otp_version != info->otp_patch_version ||
+		    info->ram_version != info->ram_patch_version) {
+			nfc_err(dev, "Firmware update failed");
+			r = -EINVAL;
+			goto error;
+		}
+	}
+
+	/*
+	 * We initialized the devices but the NFC subsystem expects
+	 * it to not be initialized.
+	 */
+	return nci_core_reset(ndev);
+
+error:
+	fdp_nci_release_firmware(ndev);
+	nfc_err(dev, "Setup error %d\n", r);
+	return r;
+}
+
+static int fdp_nci_post_setup(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	int r;
+
+	/* Check if the device has VSC */
+	if (info->fw_vsc_cfg && info->fw_vsc_cfg[0]) {
+
+		/* Set the vendor specific configuration */
+		r = fdp_nci_set_production_data(ndev, info->fw_vsc_cfg[3],
+						&info->fw_vsc_cfg[4]);
+		if (r) {
+			nfc_err(dev, "Vendor specific config set error %d\n",
+				r);
+			return r;
+		}
+	}
+
+	/* Set clock type and frequency */
+	r = fdp_nci_set_clock(ndev, info->clock_type, info->clock_freq);
+	if (r) {
+		nfc_err(dev, "Clock set error %d\n", r);
+		return r;
+	}
+
+	/*
+	 * In order to apply the VSC FDP needs a reset
+	 */
+	r = nci_core_reset(ndev);
+	if (r)
+		return r;
+
+	/**
+	 * The nci core was initialized when post setup was called
+	 * so we leave it like that
+	 */
+	return nci_core_init(ndev);
+}
+
+static int fdp_nci_core_reset_ntf_packet(struct nci_dev *ndev,
+					  struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+	info->setup_reset_ntf = 1;
+	wake_up(&info->setup_wq);
+
+	return 0;
+}
+
+static int fdp_nci_prop_patch_ntf_packet(struct nci_dev *ndev,
+					  struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+	info->setup_patch_ntf = 1;
+	info->setup_patch_status = skb->data[0];
+	wake_up(&info->setup_wq);
+
+	return 0;
+}
+
+static int fdp_nci_prop_patch_rsp_packet(struct nci_dev *ndev,
+					  struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	u8 status = skb->data[0];
+
+	dev_dbg(dev, "%s: status 0x%x\n", __func__, status);
+	nci_req_complete(ndev, status);
+
+	return 0;
+}
+
+static int fdp_nci_prop_set_production_data_rsp_packet(struct nci_dev *ndev,
+							struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	u8 status = skb->data[0];
+
+	dev_dbg(dev, "%s: status 0x%x\n", __func__, status);
+	nci_req_complete(ndev, status);
+
+	return 0;
+}
+
+static int fdp_nci_core_get_config_rsp_packet(struct nci_dev *ndev,
+						struct sk_buff *skb)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+	struct nci_core_get_config_rsp *rsp = (void *) skb->data;
+	u8 i, *p;
+
+	if (rsp->status == NCI_STATUS_OK) {
+
+		p = rsp->data;
+		for (i = 0; i < 4; i++) {
+
+			switch (*p++) {
+			case NCI_PARAM_ID_FW_RAM_VERSION:
+				p++;
+				info->ram_version = le32_to_cpup((__le32 *) p);
+				p += 4;
+				break;
+			case NCI_PARAM_ID_FW_OTP_VERSION:
+				p++;
+				info->otp_version = le32_to_cpup((__le32 *) p);
+				p += 4;
+				break;
+			case NCI_PARAM_ID_OTP_LIMITED_VERSION:
+				p++;
+				info->otp_version = le32_to_cpup((__le32 *) p);
+				p += 4;
+				break;
+			case NCI_PARAM_ID_KEY_INDEX_ID:
+				p++;
+				info->key_index = *p++;
+			}
+		}
+	}
+
+	dev_dbg(dev, "OTP version %d\n", info->otp_version);
+	dev_dbg(dev, "RAM version %d\n", info->ram_version);
+	dev_dbg(dev, "key index %d\n", info->key_index);
+	dev_dbg(dev, "%s: status 0x%x\n", __func__, rsp->status);
+
+	nci_req_complete(ndev, rsp->status);
+
+	return 0;
+}
+
+static struct nci_driver_ops fdp_core_ops[] = {
+	{
+		.opcode = NCI_OP_CORE_GET_CONFIG_RSP,
+		.rsp = fdp_nci_core_get_config_rsp_packet,
+	},
+	{
+		.opcode = NCI_OP_CORE_RESET_NTF,
+		.ntf = fdp_nci_core_reset_ntf_packet,
+	},
+};
+
+static struct nci_driver_ops fdp_prop_ops[] = {
+	{
+		.opcode = nci_opcode_pack(NCI_GID_PROP, NCI_OP_PROP_PATCH_OID),
+		.rsp = fdp_nci_prop_patch_rsp_packet,
+		.ntf = fdp_nci_prop_patch_ntf_packet,
+	},
+	{
+		.opcode = nci_opcode_pack(NCI_GID_PROP,
+					  NCI_OP_PROP_SET_PDATA_OID),
+		.rsp = fdp_nci_prop_set_production_data_rsp_packet,
+	},
+};
+
+struct nci_ops nci_ops = {
+	.open = fdp_nci_open,
+	.close = fdp_nci_close,
+	.send = fdp_nci_send,
+	.setup = fdp_nci_setup,
+	.post_setup = fdp_nci_post_setup,
+	.prop_ops = fdp_prop_ops,
+	.n_prop_ops = ARRAY_SIZE(fdp_prop_ops),
+	.core_ops = fdp_core_ops,
+	.n_core_ops = ARRAY_SIZE(fdp_core_ops),
+};
+
+int fdp_nci_probe(struct fdp_i2c_phy *phy, struct nfc_phy_ops *phy_ops,
+			struct nci_dev **ndevp, int tx_headroom,
+			int tx_tailroom, u8 clock_type, u32 clock_freq,
+			u8 *fw_vsc_cfg)
+{
+	struct device *dev = &phy->i2c_dev->dev;
+	struct fdp_nci_info *info;
+	struct nci_dev *ndev;
+	u32 protocols;
+	int r;
+
+	info = kzalloc(sizeof(struct fdp_nci_info), GFP_KERNEL);
+	if (!info) {
+		r = -ENOMEM;
+		goto err_info_alloc;
+	}
+
+	info->phy = phy;
+	info->phy_ops = phy_ops;
+	info->clock_type = clock_type;
+	info->clock_freq = clock_freq;
+	info->fw_vsc_cfg = fw_vsc_cfg;
+
+	init_waitqueue_head(&info->setup_wq);
+
+	protocols = NFC_PROTO_JEWEL_MASK |
+		    NFC_PROTO_MIFARE_MASK |
+		    NFC_PROTO_FELICA_MASK |
+		    NFC_PROTO_ISO14443_MASK |
+		    NFC_PROTO_ISO14443_B_MASK |
+		    NFC_PROTO_NFC_DEP_MASK |
+		    NFC_PROTO_ISO15693_MASK;
+
+	ndev = nci_allocate_device(&nci_ops, protocols, tx_headroom,
+				   tx_tailroom);
+	if (!ndev) {
+		nfc_err(dev, "Cannot allocate nfc ndev\n");
+		r = -ENOMEM;
+		goto err_alloc_ndev;
+	}
+
+	r = nci_register_device(ndev);
+	if (r)
+		goto err_regdev;
+
+	*ndevp = ndev;
+	info->ndev = ndev;
+
+	nci_set_drvdata(ndev, info);
+
+	return 0;
+
+err_regdev:
+	nci_free_device(ndev);
+err_alloc_ndev:
+	kfree(info);
+err_info_alloc:
+	return r;
+}
+EXPORT_SYMBOL(fdp_nci_probe);
+
+void fdp_nci_remove(struct nci_dev *ndev)
+{
+	struct fdp_nci_info *info = nci_get_drvdata(ndev);
+	struct device *dev = &info->phy->i2c_dev->dev;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	nci_unregister_device(ndev);
+	nci_free_device(ndev);
+	kfree(info);
+}
+EXPORT_SYMBOL(fdp_nci_remove);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NFC NCI driver for Intel Fields Peak NFC controller");
+MODULE_AUTHOR("Robert Dolca <robert.dolca@intel.com>");
diff --git a/drivers/nfc/fdp/fdp.h b/drivers/nfc/fdp/fdp.h
new file mode 100644
index 000000000000..0bd36c00535d
--- /dev/null
+++ b/drivers/nfc/fdp/fdp.h
@@ -0,0 +1,38 @@
+/* -------------------------------------------------------------------------
+ * Copyright (C) 2014-2016, Intel Corporation
+ *
+ *  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.
+ * -------------------------------------------------------------------------
+ */
+
+#ifndef __LOCAL_FDP_H_
+#define __LOCAL_FDP_H_
+
+#include <net/nfc/nci_core.h>
+#include <linux/gpio/consumer.h>
+
+struct fdp_i2c_phy {
+	struct i2c_client *i2c_dev;
+	struct gpio_desc *power_gpio;
+	struct nci_dev *ndev;
+
+	/* < 0 if i2c error occurred */
+	int hard_fault;
+	uint16_t next_read_size;
+};
+
+int fdp_nci_probe(struct fdp_i2c_phy *phy, struct nfc_phy_ops *phy_ops,
+		  struct nci_dev **ndev, int tx_headroom, int tx_tailroom,
+		  u8 clock_type, u32 clock_freq, u8 *fw_vsc_cfg);
+void fdp_nci_remove(struct nci_dev *ndev);
+int fdp_nci_recv_frame(struct nci_dev *ndev, struct sk_buff *skb);
+
+#endif /* __LOCAL_FDP_H_ */
diff --git a/drivers/nfc/fdp/i2c.c b/drivers/nfc/fdp/i2c.c
new file mode 100644
index 000000000000..532db28145c7
--- /dev/null
+++ b/drivers/nfc/fdp/i2c.c
@@ -0,0 +1,388 @@
+/* -------------------------------------------------------------------------
+ * Copyright (C) 2014-2016, Intel Corporation
+ *
+ *  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.
+ * -------------------------------------------------------------------------
+ */
+
+#include <linux/module.h>
+#include <linux/acpi.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/nfc.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <net/nfc/nfc.h>
+#include <net/nfc/nci_core.h>
+
+#include "fdp.h"
+
+#define FDP_I2C_DRIVER_NAME	"fdp_nci_i2c"
+
+#define FDP_DP_POWER_GPIO_NAME	"power"
+#define FDP_DP_CLOCK_TYPE_NAME	"clock-type"
+#define FDP_DP_CLOCK_FREQ_NAME	"clock-freq"
+#define FDP_DP_FW_VSC_CFG_NAME	"fw-vsc-cfg"
+
+#define FDP_FRAME_HEADROOM	2
+#define FDP_FRAME_TAILROOM	1
+
+#define FDP_NCI_I2C_MIN_PAYLOAD	5
+#define FDP_NCI_I2C_MAX_PAYLOAD	261
+
+#define FDP_POWER_OFF		0
+#define FDP_POWER_ON		1
+
+#define fdp_nci_i2c_dump_skb(dev, prefix, skb)				\
+	print_hex_dump(KERN_DEBUG, prefix": ", DUMP_PREFIX_OFFSET,	\
+		       16, 1, (skb)->data, (skb)->len, 0)
+
+static void fdp_nci_i2c_reset(struct fdp_i2c_phy *phy)
+{
+	/* Reset RST/WakeUP for at least 100 micro-second */
+	gpiod_set_value_cansleep(phy->power_gpio, FDP_POWER_OFF);
+	usleep_range(1000, 4000);
+	gpiod_set_value_cansleep(phy->power_gpio, FDP_POWER_ON);
+	usleep_range(10000, 14000);
+}
+
+static int fdp_nci_i2c_enable(void *phy_id)
+{
+	struct fdp_i2c_phy *phy = phy_id;
+
+	dev_dbg(&phy->i2c_dev->dev, "%s\n", __func__);
+	fdp_nci_i2c_reset(phy);
+
+	return 0;
+}
+
+static void fdp_nci_i2c_disable(void *phy_id)
+{
+	struct fdp_i2c_phy *phy = phy_id;
+
+	dev_dbg(&phy->i2c_dev->dev, "%s\n", __func__);
+	fdp_nci_i2c_reset(phy);
+}
+
+static void fdp_nci_i2c_add_len_lrc(struct sk_buff *skb)
+{
+	u8 lrc = 0;
+	u16 len, i;
+
+	/* Add length header */
+	len = skb->len;
+	*skb_push(skb, 1) = len & 0xff;
+	*skb_push(skb, 1) = len >> 8;
+
+	/* Compute and add lrc */
+	for (i = 0; i < len + 2; i++)
+		lrc ^= skb->data[i];
+
+	*skb_put(skb, 1) = lrc;
+}
+
+static void fdp_nci_i2c_remove_len_lrc(struct sk_buff *skb)
+{
+	skb_pull(skb, FDP_FRAME_HEADROOM);
+	skb_trim(skb, skb->len - FDP_FRAME_TAILROOM);
+}
+
+static int fdp_nci_i2c_write(void *phy_id, struct sk_buff *skb)
+{
+	struct fdp_i2c_phy *phy = phy_id;
+	struct i2c_client *client = phy->i2c_dev;
+	int r;
+
+	if (phy->hard_fault != 0)
+		return phy->hard_fault;
+
+	fdp_nci_i2c_add_len_lrc(skb);
+	fdp_nci_i2c_dump_skb(&client->dev, "fdp_wr", skb);
+
+	r = i2c_master_send(client, skb->data, skb->len);
+	if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
+		usleep_range(1000, 4000);
+		r = i2c_master_send(client, skb->data, skb->len);
+	}
+
+	if (r < 0 || r != skb->len)
+		dev_dbg(&client->dev, "%s: error err=%d len=%d\n",
+			__func__, r, skb->len);
+
+	if (r >= 0) {
+		if (r != skb->len) {
+			phy->hard_fault = r;
+			r = -EREMOTEIO;
+		} else {
+			r = 0;
+		}
+	}
+
+	fdp_nci_i2c_remove_len_lrc(skb);
+
+	return r;
+}
+
+static struct nfc_phy_ops i2c_phy_ops = {
+	.write = fdp_nci_i2c_write,
+	.enable = fdp_nci_i2c_enable,
+	.disable = fdp_nci_i2c_disable,
+};
+
+static int fdp_nci_i2c_read(struct fdp_i2c_phy *phy, struct sk_buff **skb)
+{
+	int r, len;
+	u8 tmp[FDP_NCI_I2C_MAX_PAYLOAD], lrc, k;
+	u16 i;
+	struct i2c_client *client = phy->i2c_dev;
+
+	*skb = NULL;
+
+	/* Read the length packet and the data packet */
+	for (k = 0; k < 2; k++) {
+
+		len = phy->next_read_size;
+
+		r = i2c_master_recv(client, tmp, len);
+		if (r != len) {
+			dev_dbg(&client->dev, "%s: i2c recv err: %d\n",
+				__func__, r);
+			goto flush;
+		}
+
+		/* Check packet integruty */
+		for (lrc = i = 0; i < r; i++)
+			lrc ^= tmp[i];
+
+		/*
+		 * LRC check failed. This may due to transmission error or
+		 * desynchronization between driver and FDP. Drop the paquet
+		 * and force resynchronization
+		 */
+		if (lrc) {
+			dev_dbg(&client->dev, "%s: corrupted packet\n",
+				__func__);
+			phy->next_read_size = 5;
+			goto flush;
+		}
+
+		/* Packet that contains a length */
+		if (tmp[0] == 0 && tmp[1] == 0) {
+			phy->next_read_size = (tmp[2] << 8) + tmp[3] + 3;
+		} else {
+			phy->next_read_size = FDP_NCI_I2C_MIN_PAYLOAD;
+
+			*skb = alloc_skb(len, GFP_KERNEL);
+			if (*skb == NULL) {
+				r = -ENOMEM;
+				goto flush;
+			}
+
+			memcpy(skb_put(*skb, len), tmp, len);
+			fdp_nci_i2c_dump_skb(&client->dev, "fdp_rd", *skb);
+
+			fdp_nci_i2c_remove_len_lrc(*skb);
+		}
+	}
+
+	return 0;
+
+flush:
+	/* Flush the remaining data */
+	if (i2c_master_recv(client, tmp, sizeof(tmp)) < 0)
+		r = -EREMOTEIO;
+
+	return r;
+}
+
+static irqreturn_t fdp_nci_i2c_irq_thread_fn(int irq, void *phy_id)
+{
+	struct fdp_i2c_phy *phy = phy_id;
+	struct i2c_client *client;
+	struct sk_buff *skb;
+	int r;
+
+	client = phy->i2c_dev;
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	if (!phy || irq != phy->i2c_dev->irq) {
+		WARN_ON_ONCE(1);
+		return IRQ_NONE;
+	}
+
+	r = fdp_nci_i2c_read(phy, &skb);
+
+	if (r == -EREMOTEIO)
+		return IRQ_HANDLED;
+	else if (r == -ENOMEM || r == -EBADMSG)
+		return IRQ_HANDLED;
+
+	if (skb != NULL)
+		fdp_nci_recv_frame(phy->ndev, skb);
+
+	return IRQ_HANDLED;
+}
+
+static void fdp_nci_i2c_read_device_properties(struct device *dev,
+					       u8 *clock_type, u32 *clock_freq,
+					       u8 **fw_vsc_cfg)
+{
+	int r;
+	u8 len;
+
+	r = device_property_read_u8(dev, FDP_DP_CLOCK_TYPE_NAME, clock_type);
+	if (r) {
+		dev_dbg(dev, "Using default clock type");
+		*clock_type = 0;
+	}
+
+	r = device_property_read_u32(dev, FDP_DP_CLOCK_FREQ_NAME, clock_freq);
+	if (r) {
+		dev_dbg(dev, "Using default clock frequency\n");
+		*clock_freq = 26000;
+	}
+
+	if (device_property_present(dev, FDP_DP_FW_VSC_CFG_NAME)) {
+		r = device_property_read_u8(dev, FDP_DP_FW_VSC_CFG_NAME,
+					    &len);
+
+		if (r || len <= 0)
+			goto vsc_read_err;
+
+		/* Add 1 to the length to inclue the length byte itself */
+		len++;
+
+		*fw_vsc_cfg = devm_kmalloc(dev,
+					   len * sizeof(**fw_vsc_cfg),
+					   GFP_KERNEL);
+
+		r = device_property_read_u8_array(dev, FDP_DP_FW_VSC_CFG_NAME,
+						  *fw_vsc_cfg, len);
+
+		if (r) {
+			devm_kfree(dev, fw_vsc_cfg);
+			goto vsc_read_err;
+		}
+	} else {
+vsc_read_err:
+		dev_dbg(dev, "FW vendor specific commands not present\n");
+		*fw_vsc_cfg = NULL;
+	}
+
+	dev_dbg(dev, "Clock type: %d, clock frequency: %d, VSC: %s",
+		*clock_type, *clock_freq, *fw_vsc_cfg != NULL ? "yes" : "no");
+}
+
+static int fdp_nci_i2c_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct fdp_i2c_phy *phy;
+	struct device *dev = &client->dev;
+	u8 *fw_vsc_cfg;
+	u8 clock_type;
+	u32 clock_freq;
+	int r = 0;
+
+	dev_dbg(dev, "%s\n", __func__);
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		nfc_err(dev, "No I2C_FUNC_I2C support\n");
+		return -ENODEV;
+	}
+
+	phy = devm_kzalloc(dev, sizeof(struct fdp_i2c_phy),
+			   GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	phy->i2c_dev = client;
+	phy->next_read_size = FDP_NCI_I2C_MIN_PAYLOAD;
+	i2c_set_clientdata(client, phy);
+
+	/* Checking if we have an irq */
+	if (client->irq <= 0) {
+		dev_err(dev, "IRQ not present\n");
+		return -ENODEV;
+	}
+
+	r = request_threaded_irq(client->irq, NULL, fdp_nci_i2c_irq_thread_fn,
+				 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+				 FDP_I2C_DRIVER_NAME, phy);
+
+	if (r < 0) {
+		nfc_err(&client->dev, "Unable to register IRQ handler\n");
+		return r;
+	}
+
+	/* Requesting the power gpio */
+	phy->power_gpio = devm_gpiod_get(dev, FDP_DP_POWER_GPIO_NAME,
+					 GPIOD_OUT_LOW);
+
+	if (IS_ERR(phy->power_gpio)) {
+		nfc_err(dev, "Power GPIO request failed\n");
+		return PTR_ERR(phy->power_gpio);
+	}
+
+	/* read device properties to get the clock and production settings */
+	fdp_nci_i2c_read_device_properties(dev, &clock_type, &clock_freq,
+					   &fw_vsc_cfg);
+
+	/* Call the NFC specific probe function */
+	r = fdp_nci_probe(phy, &i2c_phy_ops, &phy->ndev,
+			  FDP_FRAME_HEADROOM, FDP_FRAME_TAILROOM,
+			  clock_type, clock_freq, fw_vsc_cfg);
+	if (r < 0) {
+		nfc_err(dev, "NCI probing error\n");
+		return r;
+	}
+
+	dev_dbg(dev, "I2C driver loaded\n");
+	return 0;
+}
+
+static int fdp_nci_i2c_remove(struct i2c_client *client)
+{
+	struct fdp_i2c_phy *phy = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "%s\n", __func__);
+
+	fdp_nci_remove(phy->ndev);
+	fdp_nci_i2c_disable(phy);
+
+	return 0;
+}
+
+static struct i2c_device_id fdp_nci_i2c_id_table[] = {
+	{"int339a", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, fdp_nci_i2c_id_table);
+
+static const struct acpi_device_id fdp_nci_i2c_acpi_match[] = {
+	{"INT339A", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, fdp_nci_i2c_acpi_match);
+
+static struct i2c_driver fdp_nci_i2c_driver = {
+	.driver = {
+		   .name = FDP_I2C_DRIVER_NAME,
+		   .acpi_match_table = ACPI_PTR(fdp_nci_i2c_acpi_match),
+		  },
+	.id_table = fdp_nci_i2c_id_table,
+	.probe = fdp_nci_i2c_probe,
+	.remove = fdp_nci_i2c_remove,
+};
+module_i2c_driver(fdp_nci_i2c_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("I2C driver for Intel Fields Peak NFC controller");
+MODULE_AUTHOR("Robert Dolca <robert.dolca@intel.com>");
diff --git a/drivers/nfc/microread/Kconfig b/drivers/nfc/microread/Kconfig
index 951d5542f6bc..2c6dbc9f6781 100644
--- a/drivers/nfc/microread/Kconfig
+++ b/drivers/nfc/microread/Kconfig
@@ -1,20 +1,15 @@
 config NFC_MICROREAD
-	tristate "Inside Secure microread NFC driver"
-	depends on NFC_HCI
+	tristate
 	select CRC_CCITT
-	default n
 	---help---
 	  This module contains the main code for Inside Secure microread
 	  NFC chipsets. It implements the chipset HCI logic and hooks into
 	  the NFC kernel APIs. Physical layers will register against it.
 
-	  To compile this driver as a module, choose m here. The module will
-	  be called microread.
-	  Say N if unsure.
-
 config NFC_MICROREAD_I2C
-	tristate "NFC Microread i2c support"
-	depends on NFC_MICROREAD && I2C && NFC_SHDLC
+	tristate "Inside Secure Microread device support (I2C)"
+	depends on NFC_HCI && I2C && NFC_SHDLC
+	select NFC_MICROREAD
 	---help---
 	  This module adds support for the i2c interface of adapters using
 	  Inside microread chipsets.  Select this if your platform is using
@@ -24,8 +19,9 @@ config NFC_MICROREAD_I2C
 	  Say N if unsure.
 
 config NFC_MICROREAD_MEI
-	tristate "NFC Microread MEI support"
-	depends on NFC_MICROREAD && NFC_MEI_PHY
+	tristate "Inside Secure Microread device support (MEI)"
+	depends on NFC_HCI && NFC_MEI_PHY
+	select NFC_MICROREAD
 	---help---
 	  This module adds support for the mei interface of adapters using
 	  Inside microread chipsets.  Select this if your microread chipset
diff --git a/drivers/nfc/nfcmrvl/Kconfig b/drivers/nfc/nfcmrvl/Kconfig
index 796be2411440..444ca94697d9 100644
--- a/drivers/nfc/nfcmrvl/Kconfig
+++ b/drivers/nfc/nfcmrvl/Kconfig
@@ -1,18 +1,15 @@
 config NFC_MRVL
-	tristate "Marvell NFC driver support"
-	depends on NFC_NCI
+	tristate
 	help
 	  The core driver to support Marvell NFC devices.
 
 	  This driver is required if you want to support
 	  Marvell NFC device 8897.
 
-	  Say Y here to compile Marvell NFC driver into the kernel or
-	  say M to compile it as module.
-
 config NFC_MRVL_USB
 	tristate "Marvell NFC-over-USB driver"
-	depends on NFC_MRVL && USB
+	depends on NFC_NCI && USB
+	select NFC_MRVL
 	help
 	  Marvell NFC-over-USB driver.
 
@@ -24,7 +21,8 @@ config NFC_MRVL_USB
 
 config NFC_MRVL_UART
 	tristate "Marvell NFC-over-UART driver"
-	depends on NFC_MRVL && NFC_NCI_UART
+	depends on NFC_NCI && NFC_NCI_UART
+	select NFC_MRVL
 	help
 	  Marvell NFC-over-UART driver.
 
@@ -32,3 +30,25 @@ config NFC_MRVL_UART
 
 	  Say Y here to compile support for Marvell NFC-over-UART driver
 	  into the kernel or say M to compile it as module.
+
+config NFC_MRVL_I2C
+	tristate "Marvell NFC-over-I2C driver"
+	depends on NFC_MRVL && I2C
+	help
+	  Marvell NFC-over-I2C driver.
+
+	  This driver provides support for Marvell NFC-over-I2C devices.
+
+	  Say Y here to compile support for Marvell NFC-over-I2C driver
+	  into the kernel or say M to compile it as module.
+
+config NFC_MRVL_SPI
+	tristate "Marvell NFC-over-SPI driver"
+	depends on NFC_MRVL && SPI
+	help
+	  Marvell NFC-over-SPI driver.
+
+	  This driver provides support for Marvell NFC-over-SPI devices.
+
+	  Say Y here to compile support for Marvell NFC-over-SPI driver
+	  into the kernel or say M to compile it as module.
diff --git a/drivers/nfc/nfcmrvl/Makefile b/drivers/nfc/nfcmrvl/Makefile
index 775196274d1f..fa07c7806492 100644
--- a/drivers/nfc/nfcmrvl/Makefile
+++ b/drivers/nfc/nfcmrvl/Makefile
@@ -2,7 +2,7 @@
 # Makefile for NFCMRVL NCI based NFC driver
 #
 
-nfcmrvl-y += main.o
+nfcmrvl-y += main.o fw_dnld.o
 obj-$(CONFIG_NFC_MRVL) += nfcmrvl.o
 
 nfcmrvl_usb-y += usb.o
@@ -10,3 +10,9 @@ obj-$(CONFIG_NFC_MRVL_USB) += nfcmrvl_usb.o
 
 nfcmrvl_uart-y += uart.o
 obj-$(CONFIG_NFC_MRVL_UART) += nfcmrvl_uart.o
+
+nfcmrvl_i2c-y += i2c.o
+obj-$(CONFIG_NFC_MRVL_I2C) += nfcmrvl_i2c.o
+
+nfcmrvl_spi-y += spi.o
+obj-$(CONFIG_NFC_MRVL_SPI) += nfcmrvl_spi.o
diff --git a/drivers/nfc/nfcmrvl/fw_dnld.c b/drivers/nfc/nfcmrvl/fw_dnld.c
new file mode 100644
index 000000000000..bfa771392b1f
--- /dev/null
+++ b/drivers/nfc/nfcmrvl/fw_dnld.c
@@ -0,0 +1,553 @@
+/*
+ * Marvell NFC driver: Firmware downloader
+ *
+ * Copyright (C) 2015, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+#include <linux/module.h>
+#include <linux/unaligned/access_ok.h>
+#include <linux/firmware.h>
+#include <linux/nfc.h>
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include "nfcmrvl.h"
+
+#define FW_DNLD_TIMEOUT			15000
+
+#define NCI_OP_PROPRIETARY_BOOT_CMD	nci_opcode_pack(NCI_GID_PROPRIETARY, \
+							NCI_OP_PROP_BOOT_CMD)
+
+/* FW download states */
+
+enum {
+	STATE_RESET = 0,
+	STATE_INIT,
+	STATE_SET_REF_CLOCK,
+	STATE_SET_HI_CONFIG,
+	STATE_OPEN_LC,
+	STATE_FW_DNLD,
+	STATE_CLOSE_LC,
+	STATE_BOOT
+};
+
+enum {
+	SUBSTATE_WAIT_COMMAND = 0,
+	SUBSTATE_WAIT_ACK_CREDIT,
+	SUBSTATE_WAIT_NACK_CREDIT,
+	SUBSTATE_WAIT_DATA_CREDIT,
+};
+
+/*
+** Patterns for responses
+*/
+
+static const uint8_t nci_pattern_core_reset_ntf[] = {
+	0x60, 0x00, 0x02, 0xA0, 0x01
+};
+
+static const uint8_t nci_pattern_core_init_rsp[] = {
+	0x40, 0x01, 0x11
+};
+
+static const uint8_t nci_pattern_core_set_config_rsp[] = {
+	0x40, 0x02, 0x02, 0x00, 0x00
+};
+
+static const uint8_t nci_pattern_core_conn_create_rsp[] = {
+	0x40, 0x04, 0x04, 0x00
+};
+
+static const uint8_t nci_pattern_core_conn_close_rsp[] = {
+	0x40, 0x05, 0x01, 0x00
+};
+
+static const uint8_t nci_pattern_core_conn_credits_ntf[] = {
+	0x60, 0x06, 0x03, 0x01, NCI_CORE_LC_CONNID_PROP_FW_DL, 0x01
+};
+
+static const uint8_t nci_pattern_proprietary_boot_rsp[] = {
+	0x4F, 0x3A, 0x01, 0x00
+};
+
+static struct sk_buff *alloc_lc_skb(struct nfcmrvl_private *priv, uint8_t plen)
+{
+	struct sk_buff *skb;
+	struct nci_data_hdr *hdr;
+
+	skb = nci_skb_alloc(priv->ndev, (NCI_DATA_HDR_SIZE + plen), GFP_KERNEL);
+	if (!skb) {
+		pr_err("no memory for data\n");
+		return NULL;
+	}
+
+	hdr = (struct nci_data_hdr *) skb_put(skb, NCI_DATA_HDR_SIZE);
+	hdr->conn_id = NCI_CORE_LC_CONNID_PROP_FW_DL;
+	hdr->rfu = 0;
+	hdr->plen = plen;
+
+	nci_mt_set((__u8 *)hdr, NCI_MT_DATA_PKT);
+	nci_pbf_set((__u8 *)hdr, NCI_PBF_LAST);
+
+	return skb;
+}
+
+static void fw_dnld_over(struct nfcmrvl_private *priv, u32 error)
+{
+	if (priv->fw_dnld.fw) {
+		release_firmware(priv->fw_dnld.fw);
+		priv->fw_dnld.fw = NULL;
+		priv->fw_dnld.header = NULL;
+		priv->fw_dnld.binary_config = NULL;
+	}
+
+	atomic_set(&priv->ndev->cmd_cnt, 0);
+	del_timer_sync(&priv->ndev->cmd_timer);
+
+	del_timer_sync(&priv->fw_dnld.timer);
+
+	nfc_info(priv->dev, "FW loading over (%d)]\n", error);
+
+	if (error != 0) {
+		/* failed, halt the chip to avoid power consumption */
+		nfcmrvl_chip_halt(priv);
+	}
+
+	nfc_fw_download_done(priv->ndev->nfc_dev, priv->fw_dnld.name, error);
+}
+
+static void fw_dnld_timeout(unsigned long arg)
+{
+	struct nfcmrvl_private *priv = (struct nfcmrvl_private *) arg;
+
+	nfc_err(priv->dev, "FW loading timeout");
+	priv->fw_dnld.state = STATE_RESET;
+	fw_dnld_over(priv, -ETIMEDOUT);
+}
+
+static int process_state_reset(struct nfcmrvl_private *priv,
+			       struct sk_buff *skb)
+{
+	if (sizeof(nci_pattern_core_reset_ntf) != skb->len ||
+	    memcmp(skb->data, nci_pattern_core_reset_ntf,
+		   sizeof(nci_pattern_core_reset_ntf)))
+		return -EINVAL;
+
+	nfc_info(priv->dev, "BootROM reset, start fw download\n");
+
+	/* Start FW download state machine */
+	priv->fw_dnld.state = STATE_INIT;
+	nci_send_cmd(priv->ndev, NCI_OP_CORE_INIT_CMD, 0, NULL);
+
+	return 0;
+}
+
+static int process_state_init(struct nfcmrvl_private *priv, struct sk_buff *skb)
+{
+	struct nci_core_set_config_cmd cmd;
+
+	if (sizeof(nci_pattern_core_init_rsp) >= skb->len ||
+	    memcmp(skb->data, nci_pattern_core_init_rsp,
+		   sizeof(nci_pattern_core_init_rsp)))
+		return -EINVAL;
+
+	cmd.num_params = 1;
+	cmd.param.id = NFCMRVL_PROP_REF_CLOCK;
+	cmd.param.len = 4;
+	memcpy(cmd.param.val, &priv->fw_dnld.header->ref_clock, 4);
+
+	nci_send_cmd(priv->ndev, NCI_OP_CORE_SET_CONFIG_CMD, 3 + cmd.param.len,
+		     &cmd);
+
+	priv->fw_dnld.state = STATE_SET_REF_CLOCK;
+	return 0;
+}
+
+static void create_lc(struct nfcmrvl_private *priv)
+{
+	uint8_t param[2] = { NCI_CORE_LC_PROP_FW_DL, 0x0 };
+
+	priv->fw_dnld.state = STATE_OPEN_LC;
+	nci_send_cmd(priv->ndev, NCI_OP_CORE_CONN_CREATE_CMD, 2, param);
+}
+
+static int process_state_set_ref_clock(struct nfcmrvl_private *priv,
+				       struct sk_buff *skb)
+{
+	struct nci_core_set_config_cmd cmd;
+
+	if (sizeof(nci_pattern_core_set_config_rsp) != skb->len ||
+	    memcmp(skb->data, nci_pattern_core_set_config_rsp, skb->len))
+		return -EINVAL;
+
+	cmd.num_params = 1;
+	cmd.param.id = NFCMRVL_PROP_SET_HI_CONFIG;
+
+	switch (priv->phy) {
+	case NFCMRVL_PHY_UART:
+		cmd.param.len = 5;
+		memcpy(cmd.param.val,
+		       &priv->fw_dnld.binary_config->uart.baudrate,
+		       4);
+		cmd.param.val[4] =
+			priv->fw_dnld.binary_config->uart.flow_control;
+		break;
+	case NFCMRVL_PHY_I2C:
+		cmd.param.len = 5;
+		memcpy(cmd.param.val,
+		       &priv->fw_dnld.binary_config->i2c.clk,
+		       4);
+		cmd.param.val[4] = 0;
+		break;
+	case NFCMRVL_PHY_SPI:
+		cmd.param.len = 5;
+		memcpy(cmd.param.val,
+		       &priv->fw_dnld.binary_config->spi.clk,
+		       4);
+		cmd.param.val[4] = 0;
+		break;
+	default:
+		create_lc(priv);
+		return 0;
+	}
+
+	priv->fw_dnld.state = STATE_SET_HI_CONFIG;
+	nci_send_cmd(priv->ndev, NCI_OP_CORE_SET_CONFIG_CMD, 3 + cmd.param.len,
+		     &cmd);
+	return 0;
+}
+
+static int process_state_set_hi_config(struct nfcmrvl_private *priv,
+				       struct sk_buff *skb)
+{
+	if (sizeof(nci_pattern_core_set_config_rsp) != skb->len ||
+	    memcmp(skb->data, nci_pattern_core_set_config_rsp, skb->len))
+		return -EINVAL;
+
+	create_lc(priv);
+	return 0;
+}
+
+static int process_state_open_lc(struct nfcmrvl_private *priv,
+				 struct sk_buff *skb)
+{
+	if (sizeof(nci_pattern_core_conn_create_rsp) >= skb->len ||
+	    memcmp(skb->data, nci_pattern_core_conn_create_rsp,
+		   sizeof(nci_pattern_core_conn_create_rsp)))
+		return -EINVAL;
+
+	priv->fw_dnld.state = STATE_FW_DNLD;
+	priv->fw_dnld.substate = SUBSTATE_WAIT_COMMAND;
+	priv->fw_dnld.offset = priv->fw_dnld.binary_config->offset;
+	return 0;
+}
+
+static int process_state_fw_dnld(struct nfcmrvl_private *priv,
+				 struct sk_buff *skb)
+{
+	uint16_t len;
+	uint16_t comp_len;
+	struct sk_buff *out_skb;
+
+	switch (priv->fw_dnld.substate) {
+	case SUBSTATE_WAIT_COMMAND:
+		/*
+		 * Command format:
+		 * B0..2: NCI header
+		 * B3   : Helper command (0xA5)
+		 * B4..5: le16 data size
+		 * B6..7: le16 data size complement (~)
+		 * B8..N: payload
+		 */
+
+		/* Remove NCI HDR */
+		skb_pull(skb, 3);
+		if (skb->data[0] != HELPER_CMD_PACKET_FORMAT || skb->len != 5) {
+			nfc_err(priv->dev, "bad command");
+			return -EINVAL;
+		}
+		skb_pull(skb, 1);
+		memcpy(&len, skb->data, 2);
+		skb_pull(skb, 2);
+		memcpy(&comp_len, skb->data, 2);
+		skb_pull(skb, 2);
+		len = get_unaligned_le16(&len);
+		comp_len = get_unaligned_le16(&comp_len);
+		if (((~len) & 0xFFFF) != comp_len) {
+			nfc_err(priv->dev, "bad len complement: %x %x %x",
+				len, comp_len, (~len & 0xFFFF));
+			out_skb = alloc_lc_skb(priv, 1);
+			if (!out_skb)
+				return -ENOMEM;
+			*skb_put(out_skb, 1) = 0xBF;
+			nci_send_frame(priv->ndev, out_skb);
+			priv->fw_dnld.substate = SUBSTATE_WAIT_NACK_CREDIT;
+			return 0;
+		}
+		priv->fw_dnld.chunk_len = len;
+		out_skb = alloc_lc_skb(priv, 1);
+		if (!out_skb)
+			return -ENOMEM;
+		*skb_put(out_skb, 1) = HELPER_ACK_PACKET_FORMAT;
+		nci_send_frame(priv->ndev, out_skb);
+		priv->fw_dnld.substate = SUBSTATE_WAIT_ACK_CREDIT;
+		break;
+
+	case SUBSTATE_WAIT_ACK_CREDIT:
+		if (sizeof(nci_pattern_core_conn_credits_ntf) != skb->len ||
+		    memcmp(nci_pattern_core_conn_credits_ntf, skb->data,
+			   skb->len)) {
+			nfc_err(priv->dev, "bad packet: waiting for credit");
+			return -EINVAL;
+		}
+		if (priv->fw_dnld.chunk_len == 0) {
+			/* FW Loading is done */
+			uint8_t conn_id = NCI_CORE_LC_CONNID_PROP_FW_DL;
+
+			priv->fw_dnld.state = STATE_CLOSE_LC;
+			nci_send_cmd(priv->ndev, NCI_OP_CORE_CONN_CLOSE_CMD,
+				     1, &conn_id);
+		} else {
+			out_skb = alloc_lc_skb(priv, priv->fw_dnld.chunk_len);
+			if (!out_skb)
+				return -ENOMEM;
+			memcpy(skb_put(out_skb, priv->fw_dnld.chunk_len),
+			       ((uint8_t *)priv->fw_dnld.fw->data) +
+			       priv->fw_dnld.offset,
+			       priv->fw_dnld.chunk_len);
+			nci_send_frame(priv->ndev, out_skb);
+			priv->fw_dnld.substate = SUBSTATE_WAIT_DATA_CREDIT;
+		}
+		break;
+
+	case SUBSTATE_WAIT_DATA_CREDIT:
+		if (sizeof(nci_pattern_core_conn_credits_ntf) != skb->len ||
+		    memcmp(nci_pattern_core_conn_credits_ntf, skb->data,
+			    skb->len)) {
+			nfc_err(priv->dev, "bad packet: waiting for credit");
+			return -EINVAL;
+		}
+		priv->fw_dnld.offset += priv->fw_dnld.chunk_len;
+		priv->fw_dnld.chunk_len = 0;
+		priv->fw_dnld.substate = SUBSTATE_WAIT_COMMAND;
+		break;
+
+	case SUBSTATE_WAIT_NACK_CREDIT:
+		if (sizeof(nci_pattern_core_conn_credits_ntf) != skb->len ||
+		    memcmp(nci_pattern_core_conn_credits_ntf, skb->data,
+			    skb->len)) {
+			nfc_err(priv->dev, "bad packet: waiting for credit");
+			return -EINVAL;
+		}
+		priv->fw_dnld.substate = SUBSTATE_WAIT_COMMAND;
+		break;
+	}
+	return 0;
+}
+
+static int process_state_close_lc(struct nfcmrvl_private *priv,
+				  struct sk_buff *skb)
+{
+	if (sizeof(nci_pattern_core_conn_close_rsp) != skb->len ||
+	    memcmp(skb->data, nci_pattern_core_conn_close_rsp, skb->len))
+		return -EINVAL;
+
+	priv->fw_dnld.state = STATE_BOOT;
+	nci_send_cmd(priv->ndev, NCI_OP_PROPRIETARY_BOOT_CMD, 0, NULL);
+	return 0;
+}
+
+static int process_state_boot(struct nfcmrvl_private *priv, struct sk_buff *skb)
+{
+	if (sizeof(nci_pattern_proprietary_boot_rsp) != skb->len ||
+	    memcmp(skb->data, nci_pattern_proprietary_boot_rsp, skb->len))
+		return -EINVAL;
+
+	/*
+	 * Update HI config to use the right configuration for the next
+	 * data exchanges.
+	 */
+	priv->if_ops->nci_update_config(priv,
+					&priv->fw_dnld.binary_config->config);
+
+	if (priv->fw_dnld.binary_config == &priv->fw_dnld.header->helper) {
+		/*
+		 * This is the case where an helper was needed and we have
+		 * uploaded it. Now we have to wait the next RESET NTF to start
+		 * FW download.
+		 */
+		priv->fw_dnld.state = STATE_RESET;
+		priv->fw_dnld.binary_config = &priv->fw_dnld.header->firmware;
+		nfc_info(priv->dev, "FW loading: helper loaded");
+	} else {
+		nfc_info(priv->dev, "FW loading: firmware loaded");
+		fw_dnld_over(priv, 0);
+	}
+	return 0;
+}
+
+static void fw_dnld_rx_work(struct work_struct *work)
+{
+	int ret;
+	struct sk_buff *skb;
+	struct nfcmrvl_fw_dnld *fw_dnld = container_of(work,
+						       struct nfcmrvl_fw_dnld,
+						       rx_work);
+	struct nfcmrvl_private *priv = container_of(fw_dnld,
+						    struct nfcmrvl_private,
+						    fw_dnld);
+
+	while ((skb = skb_dequeue(&fw_dnld->rx_q))) {
+		nfc_send_to_raw_sock(priv->ndev->nfc_dev, skb,
+				     RAW_PAYLOAD_NCI, NFC_DIRECTION_RX);
+		switch (fw_dnld->state) {
+		case STATE_RESET:
+			ret = process_state_reset(priv, skb);
+			break;
+		case STATE_INIT:
+			ret = process_state_init(priv, skb);
+			break;
+		case STATE_SET_REF_CLOCK:
+			ret = process_state_set_ref_clock(priv, skb);
+			break;
+		case STATE_SET_HI_CONFIG:
+			ret = process_state_set_hi_config(priv, skb);
+			break;
+		case STATE_OPEN_LC:
+			ret = process_state_open_lc(priv, skb);
+			break;
+		case STATE_FW_DNLD:
+			ret = process_state_fw_dnld(priv, skb);
+			break;
+		case STATE_CLOSE_LC:
+			ret = process_state_close_lc(priv, skb);
+			break;
+		case STATE_BOOT:
+			ret = process_state_boot(priv, skb);
+			break;
+		default:
+			ret = -EFAULT;
+		}
+
+		kfree_skb(skb);
+
+		if (ret != 0) {
+			nfc_err(priv->dev, "FW loading error");
+			fw_dnld_over(priv, ret);
+			break;
+		}
+	}
+}
+
+int	nfcmrvl_fw_dnld_init(struct nfcmrvl_private *priv)
+{
+	char name[32];
+
+	INIT_WORK(&priv->fw_dnld.rx_work, fw_dnld_rx_work);
+	snprintf(name, sizeof(name), "%s_nfcmrvl_fw_dnld_rx_wq",
+		 dev_name(priv->dev));
+	priv->fw_dnld.rx_wq = create_singlethread_workqueue(name);
+	if (!priv->fw_dnld.rx_wq)
+		return -ENOMEM;
+	skb_queue_head_init(&priv->fw_dnld.rx_q);
+	return 0;
+}
+
+void	nfcmrvl_fw_dnld_deinit(struct nfcmrvl_private *priv)
+{
+	destroy_workqueue(priv->fw_dnld.rx_wq);
+}
+
+void	nfcmrvl_fw_dnld_recv_frame(struct nfcmrvl_private *priv,
+				   struct sk_buff *skb)
+{
+	/* Allow next command */
+	atomic_set(&priv->ndev->cmd_cnt, 1);
+	del_timer_sync(&priv->ndev->cmd_timer);
+
+	/* Queue and trigger rx work */
+	skb_queue_tail(&priv->fw_dnld.rx_q, skb);
+	queue_work(priv->fw_dnld.rx_wq, &priv->fw_dnld.rx_work);
+}
+
+void nfcmrvl_fw_dnld_abort(struct nfcmrvl_private *priv)
+{
+	fw_dnld_over(priv, -EHOSTDOWN);
+}
+
+int nfcmrvl_fw_dnld_start(struct nci_dev *ndev, const char *firmware_name)
+{
+	struct nfcmrvl_private *priv = nci_get_drvdata(ndev);
+	struct nfcmrvl_fw_dnld *fw_dnld = &priv->fw_dnld;
+
+	if (!priv->support_fw_dnld)
+		return -ENOTSUPP;
+
+	if (!firmware_name || !firmware_name[0])
+		return -EINVAL;
+
+	strcpy(fw_dnld->name, firmware_name);
+
+	/*
+	 * Retrieve FW binary file and parse it to initialize FW download
+	 * state machine.
+	 */
+
+	/* Retrieve FW binary */
+	if (request_firmware(&fw_dnld->fw, firmware_name, priv->dev) < 0) {
+		nfc_err(priv->dev, "failed to retrieve FW %s", firmware_name);
+		return -ENOENT;
+	}
+
+	fw_dnld->header = (const struct nfcmrvl_fw *) priv->fw_dnld.fw->data;
+
+	if (fw_dnld->header->magic != NFCMRVL_FW_MAGIC ||
+	    fw_dnld->header->phy != priv->phy) {
+		nfc_err(priv->dev, "bad firmware binary %s magic=0x%x phy=%d",
+			firmware_name, fw_dnld->header->magic,
+			fw_dnld->header->phy);
+		release_firmware(fw_dnld->fw);
+		fw_dnld->header = NULL;
+		return -EINVAL;
+	}
+
+	if (fw_dnld->header->helper.offset != 0) {
+		nfc_info(priv->dev, "loading helper");
+		fw_dnld->binary_config = &fw_dnld->header->helper;
+	} else {
+		nfc_info(priv->dev, "loading firmware");
+		fw_dnld->binary_config = &fw_dnld->header->firmware;
+	}
+
+	/* Configure a timer for timeout */
+	setup_timer(&priv->fw_dnld.timer, fw_dnld_timeout,
+		    (unsigned long) priv);
+	mod_timer(&priv->fw_dnld.timer,
+		  jiffies + msecs_to_jiffies(FW_DNLD_TIMEOUT));
+
+	/* Ronfigure HI to be sure that it is the bootrom values */
+	priv->if_ops->nci_update_config(priv,
+					&fw_dnld->header->bootrom.config);
+
+	/* Allow first command */
+	atomic_set(&priv->ndev->cmd_cnt, 1);
+
+	/* First, reset the chip */
+	priv->fw_dnld.state = STATE_RESET;
+	nfcmrvl_chip_reset(priv);
+
+	/* Now wait for CORE_RESET_NTF or timeout */
+
+	return 0;
+}
diff --git a/drivers/nfc/nfcmrvl/fw_dnld.h b/drivers/nfc/nfcmrvl/fw_dnld.h
new file mode 100644
index 000000000000..ee4a339c05fd
--- /dev/null
+++ b/drivers/nfc/nfcmrvl/fw_dnld.h
@@ -0,0 +1,98 @@
+/**
+ * Marvell NFC driver: Firmware downloader
+ *
+ * Copyright (C) 2015, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ **/
+
+#ifndef __NFCMRVL_FW_DNLD_H__
+#define __NFCMRVL_FW_DNLD_H__
+
+#include <linux/workqueue.h>
+
+#define NFCMRVL_FW_MAGIC		0x88888888
+
+#define NCI_OP_PROP_BOOT_CMD		0x3A
+
+#define NCI_CORE_LC_PROP_FW_DL		0xFD
+#define NCI_CORE_LC_CONNID_PROP_FW_DL	0x02
+
+#define HELPER_CMD_ENTRY_POINT		0x04
+#define HELPER_CMD_PACKET_FORMAT	0xA5
+#define HELPER_ACK_PACKET_FORMAT	0x5A
+#define HELPER_RETRY_REQUESTED		(1 << 15)
+
+struct nfcmrvl_private;
+
+struct nfcmrvl_fw_uart_config {
+	uint8_t flow_control;
+	uint32_t baudrate;
+} __packed;
+
+struct nfcmrvl_fw_i2c_config {
+	uint32_t clk;
+} __packed;
+
+struct nfcmrvl_fw_spi_config {
+	uint32_t clk;
+} __packed;
+
+struct nfcmrvl_fw_binary_config {
+	uint32_t offset;
+	union {
+		void *config;
+		struct nfcmrvl_fw_uart_config uart;
+		struct nfcmrvl_fw_i2c_config i2c;
+		struct nfcmrvl_fw_spi_config spi;
+		uint8_t reserved[64];
+	};
+} __packed;
+
+struct nfcmrvl_fw {
+	uint32_t magic;
+	uint32_t ref_clock;
+	uint32_t phy;
+	struct nfcmrvl_fw_binary_config bootrom;
+	struct nfcmrvl_fw_binary_config helper;
+	struct nfcmrvl_fw_binary_config firmware;
+	uint8_t reserved[64];
+} __packed;
+
+struct nfcmrvl_fw_dnld {
+	char name[NFC_FIRMWARE_NAME_MAXSIZE + 1];
+	const struct firmware *fw;
+
+	const struct nfcmrvl_fw *header;
+	const struct nfcmrvl_fw_binary_config *binary_config;
+
+	int state;
+	int substate;
+	int offset;
+	int chunk_len;
+
+	struct workqueue_struct	*rx_wq;
+	struct work_struct rx_work;
+	struct sk_buff_head rx_q;
+
+	struct timer_list timer;
+};
+
+int nfcmrvl_fw_dnld_init(struct nfcmrvl_private *priv);
+void nfcmrvl_fw_dnld_deinit(struct nfcmrvl_private *priv);
+void nfcmrvl_fw_dnld_abort(struct nfcmrvl_private *priv);
+int nfcmrvl_fw_dnld_start(struct nci_dev *ndev, const char *firmware_name);
+void nfcmrvl_fw_dnld_recv_frame(struct nfcmrvl_private *priv,
+				struct sk_buff *skb);
+
+#endif
diff --git a/drivers/nfc/nfcmrvl/i2c.c b/drivers/nfc/nfcmrvl/i2c.c
new file mode 100644
index 000000000000..78b7aa835c81
--- /dev/null
+++ b/drivers/nfc/nfcmrvl/i2c.c
@@ -0,0 +1,290 @@
+/**
+ * Marvell NFC-over-I2C driver: I2C interface related functions
+ *
+ * Copyright (C) 2015, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ **/
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+#include <linux/nfc.h>
+#include <linux/gpio.h>
+#include <linux/delay.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include "nfcmrvl.h"
+
+struct nfcmrvl_i2c_drv_data {
+	unsigned long flags;
+	struct device *dev;
+	struct i2c_client *i2c;
+	struct nfcmrvl_private *priv;
+};
+
+static int nfcmrvl_i2c_read(struct nfcmrvl_i2c_drv_data *drv_data,
+			    struct sk_buff **skb)
+{
+	int ret;
+	struct nci_ctrl_hdr nci_hdr;
+
+	/* Read NCI header to know the payload size */
+	ret = i2c_master_recv(drv_data->i2c, (u8 *)&nci_hdr, NCI_CTRL_HDR_SIZE);
+	if (ret != NCI_CTRL_HDR_SIZE) {
+		nfc_err(&drv_data->i2c->dev, "cannot read NCI header\n");
+		return -EBADMSG;
+	}
+
+	if (nci_hdr.plen > NCI_MAX_PAYLOAD_SIZE) {
+		nfc_err(&drv_data->i2c->dev, "invalid packet payload size\n");
+		return -EBADMSG;
+	}
+
+	*skb = nci_skb_alloc(drv_data->priv->ndev,
+			     nci_hdr.plen + NCI_CTRL_HDR_SIZE, GFP_KERNEL);
+	if (!*skb)
+		return -ENOMEM;
+
+	/* Copy NCI header into the SKB */
+	memcpy(skb_put(*skb, NCI_CTRL_HDR_SIZE), &nci_hdr, NCI_CTRL_HDR_SIZE);
+
+	if (nci_hdr.plen) {
+		/* Read the NCI payload */
+		ret = i2c_master_recv(drv_data->i2c,
+				      skb_put(*skb, nci_hdr.plen),
+				      nci_hdr.plen);
+
+		if (ret != nci_hdr.plen) {
+			nfc_err(&drv_data->i2c->dev,
+				"Invalid frame payload length: %u (expected %u)\n",
+				ret, nci_hdr.plen);
+			kfree_skb(*skb);
+			return -EBADMSG;
+		}
+	}
+
+	return 0;
+}
+
+static irqreturn_t nfcmrvl_i2c_int_irq_thread_fn(int irq, void *drv_data_ptr)
+{
+	struct nfcmrvl_i2c_drv_data *drv_data = drv_data_ptr;
+	struct sk_buff *skb = NULL;
+	int ret;
+
+	if (!drv_data->priv)
+		return IRQ_HANDLED;
+
+	if (test_bit(NFCMRVL_PHY_ERROR, &drv_data->priv->flags))
+		return IRQ_HANDLED;
+
+	ret = nfcmrvl_i2c_read(drv_data, &skb);
+
+	switch (ret) {
+	case -EREMOTEIO:
+		set_bit(NFCMRVL_PHY_ERROR, &drv_data->priv->flags);
+		break;
+	case -ENOMEM:
+	case -EBADMSG:
+		nfc_err(&drv_data->i2c->dev, "read failed %d\n", ret);
+		break;
+	default:
+		if (nfcmrvl_nci_recv_frame(drv_data->priv, skb) < 0)
+			nfc_err(&drv_data->i2c->dev, "corrupted RX packet\n");
+		break;
+	}
+	return IRQ_HANDLED;
+}
+
+static int nfcmrvl_i2c_nci_open(struct nfcmrvl_private *priv)
+{
+	struct nfcmrvl_i2c_drv_data *drv_data = priv->drv_data;
+
+	if (!drv_data)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int nfcmrvl_i2c_nci_close(struct nfcmrvl_private *priv)
+{
+	return 0;
+}
+
+static int nfcmrvl_i2c_nci_send(struct nfcmrvl_private *priv,
+				struct sk_buff *skb)
+{
+	struct nfcmrvl_i2c_drv_data *drv_data = priv->drv_data;
+	int ret;
+
+	if (test_bit(NFCMRVL_PHY_ERROR, &priv->flags))
+		return -EREMOTEIO;
+
+	ret = i2c_master_send(drv_data->i2c, skb->data, skb->len);
+
+	/* Retry if chip was in standby */
+	if (ret == -EREMOTEIO) {
+		nfc_info(drv_data->dev, "chip may sleep, retry\n");
+		usleep_range(6000, 10000);
+		ret = i2c_master_send(drv_data->i2c, skb->data, skb->len);
+	}
+
+	if (ret >= 0) {
+		if (ret != skb->len) {
+			nfc_err(drv_data->dev,
+				"Invalid length sent: %u (expected %u)\n",
+				ret, skb->len);
+			ret = -EREMOTEIO;
+		} else
+			ret = 0;
+		kfree_skb(skb);
+	}
+
+	return ret;
+}
+
+static void nfcmrvl_i2c_nci_update_config(struct nfcmrvl_private *priv,
+					  const void *param)
+{
+}
+
+static struct nfcmrvl_if_ops i2c_ops = {
+	.nci_open = nfcmrvl_i2c_nci_open,
+	.nci_close = nfcmrvl_i2c_nci_close,
+	.nci_send = nfcmrvl_i2c_nci_send,
+	.nci_update_config = nfcmrvl_i2c_nci_update_config,
+};
+
+static int nfcmrvl_i2c_parse_dt(struct device_node *node,
+				struct nfcmrvl_platform_data *pdata)
+{
+	int ret;
+
+	ret = nfcmrvl_parse_dt(node, pdata);
+	if (ret < 0) {
+		pr_err("Failed to get generic entries\n");
+		return ret;
+	}
+
+	if (of_find_property(node, "i2c-int-falling", NULL))
+		pdata->irq_polarity = IRQF_TRIGGER_FALLING;
+	else
+		pdata->irq_polarity = IRQF_TRIGGER_RISING;
+
+	ret = irq_of_parse_and_map(node, 0);
+	if (ret < 0) {
+		pr_err("Unable to get irq, error: %d\n", ret);
+		return ret;
+	}
+	pdata->irq = ret;
+
+	return 0;
+}
+
+static int nfcmrvl_i2c_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct nfcmrvl_i2c_drv_data *drv_data;
+	struct nfcmrvl_platform_data *pdata;
+	struct nfcmrvl_platform_data config;
+	int ret;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		nfc_err(&client->dev, "Need I2C_FUNC_I2C\n");
+		return -ENODEV;
+	}
+
+	drv_data = devm_kzalloc(&client->dev, sizeof(*drv_data), GFP_KERNEL);
+	if (!drv_data)
+		return -ENOMEM;
+
+	drv_data->i2c = client;
+	drv_data->dev = &client->dev;
+	drv_data->priv = NULL;
+
+	i2c_set_clientdata(client, drv_data);
+
+	pdata = client->dev.platform_data;
+
+	if (!pdata && client->dev.of_node)
+		if (nfcmrvl_i2c_parse_dt(client->dev.of_node, &config) == 0)
+			pdata = &config;
+
+	if (!pdata)
+		return -EINVAL;
+
+	/* Request the read IRQ */
+	ret = devm_request_threaded_irq(&drv_data->i2c->dev, pdata->irq,
+					NULL, nfcmrvl_i2c_int_irq_thread_fn,
+					pdata->irq_polarity | IRQF_ONESHOT,
+					"nfcmrvl_i2c_int", drv_data);
+	if (ret < 0) {
+		nfc_err(&drv_data->i2c->dev,
+			"Unable to register IRQ handler\n");
+		return ret;
+	}
+
+	drv_data->priv = nfcmrvl_nci_register_dev(NFCMRVL_PHY_I2C,
+						  drv_data, &i2c_ops,
+						  &drv_data->i2c->dev, pdata);
+
+	if (IS_ERR(drv_data->priv))
+		return PTR_ERR(drv_data->priv);
+
+	drv_data->priv->support_fw_dnld = true;
+
+	return 0;
+}
+
+static int nfcmrvl_i2c_remove(struct i2c_client *client)
+{
+	struct nfcmrvl_i2c_drv_data *drv_data = i2c_get_clientdata(client);
+
+	nfcmrvl_nci_unregister_dev(drv_data->priv);
+
+	return 0;
+}
+
+
+static const struct of_device_id of_nfcmrvl_i2c_match[] = {
+	{ .compatible = "marvell,nfc-i2c", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_nfcmrvl_i2c_match);
+
+static struct i2c_device_id nfcmrvl_i2c_id_table[] = {
+	{ "nfcmrvl_i2c", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, nfcmrvl_i2c_id_table);
+
+static struct i2c_driver nfcmrvl_i2c_driver = {
+	.probe = nfcmrvl_i2c_probe,
+	.id_table = nfcmrvl_i2c_id_table,
+	.remove = nfcmrvl_i2c_remove,
+	.driver = {
+		.name		= "nfcmrvl_i2c",
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(of_nfcmrvl_i2c_match),
+	},
+};
+
+module_i2c_driver(nfcmrvl_i2c_driver);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("Marvell NFC-over-I2C driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/nfc/nfcmrvl/main.c b/drivers/nfc/nfcmrvl/main.c
index 4a8866d62941..8079ae0de21e 100644
--- a/drivers/nfc/nfcmrvl/main.c
+++ b/drivers/nfc/nfcmrvl/main.c
@@ -1,7 +1,7 @@
 /*
  * Marvell NFC driver: major functions
  *
- * Copyright (C) 2014, Marvell International Ltd.
+ * Copyright (C) 2014-2015 Marvell International Ltd.
  *
  * This software file (the "File") is distributed by Marvell International
  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
@@ -25,8 +25,6 @@
 #include <net/nfc/nci_core.h>
 #include "nfcmrvl.h"
 
-#define VERSION "1.0"
-
 static int nfcmrvl_nci_open(struct nci_dev *ndev)
 {
 	struct nfcmrvl_private *priv = nci_get_drvdata(ndev);
@@ -35,6 +33,9 @@ static int nfcmrvl_nci_open(struct nci_dev *ndev)
 	if (test_and_set_bit(NFCMRVL_NCI_RUNNING, &priv->flags))
 		return 0;
 
+	/* Reset possible fault of previous session */
+	clear_bit(NFCMRVL_PHY_ERROR, &priv->flags);
+
 	err = priv->if_ops->nci_open(priv);
 
 	if (err)
@@ -63,9 +64,6 @@ static int nfcmrvl_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
 
 	skb->dev = (void *)ndev;
 
-	if (!test_bit(NFCMRVL_NCI_RUNNING, &priv->flags))
-		return -EBUSY;
-
 	if (priv->config.hci_muxed) {
 		unsigned char *hdr;
 		unsigned char len = skb->len;
@@ -88,21 +86,30 @@ static int nfcmrvl_nci_setup(struct nci_dev *ndev)
 	return 0;
 }
 
+static int nfcmrvl_nci_fw_download(struct nci_dev *ndev,
+				   const char *firmware_name)
+{
+	return nfcmrvl_fw_dnld_start(ndev, firmware_name);
+}
+
 static struct nci_ops nfcmrvl_nci_ops = {
 	.open = nfcmrvl_nci_open,
 	.close = nfcmrvl_nci_close,
 	.send = nfcmrvl_nci_send,
 	.setup = nfcmrvl_nci_setup,
+	.fw_download = nfcmrvl_nci_fw_download,
 };
 
-struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
+struct nfcmrvl_private *nfcmrvl_nci_register_dev(enum nfcmrvl_phy phy,
+				void *drv_data,
 				struct nfcmrvl_if_ops *ops,
 				struct device *dev,
 				struct nfcmrvl_platform_data *pdata)
 {
 	struct nfcmrvl_private *priv;
 	int rc;
-	int headroom = 0;
+	int headroom;
+	int tailroom;
 	u32 protocols;
 
 	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
@@ -112,6 +119,7 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 	priv->drv_data = drv_data;
 	priv->if_ops = ops;
 	priv->dev = dev;
+	priv->phy = phy;
 
 	memcpy(&priv->config, pdata, sizeof(*pdata));
 
@@ -124,8 +132,14 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 			nfc_err(dev, "failed to request reset_n io\n");
 	}
 
+	if (phy == NFCMRVL_PHY_SPI) {
+		headroom = NCI_SPI_HDR_LEN;
+		tailroom = 1;
+	} else
+		headroom = tailroom = 0;
+
 	if (priv->config.hci_muxed)
-		headroom = NFCMRVL_HCI_EVENT_HEADER_SIZE;
+		headroom += NFCMRVL_HCI_EVENT_HEADER_SIZE;
 
 	protocols = NFC_PROTO_JEWEL_MASK
 		| NFC_PROTO_MIFARE_MASK
@@ -136,7 +150,7 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 		| NFC_PROTO_NFC_DEP_MASK;
 
 	priv->ndev = nci_allocate_device(&nfcmrvl_nci_ops, protocols,
-					 headroom, 0);
+					 headroom, tailroom);
 	if (!priv->ndev) {
 		nfc_err(dev, "nci_allocate_device failed\n");
 		rc = -ENOMEM;
@@ -145,18 +159,26 @@ struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
 
 	nci_set_drvdata(priv->ndev, priv);
 
-	nfcmrvl_chip_reset(priv);
-
 	rc = nci_register_device(priv->ndev);
 	if (rc) {
 		nfc_err(dev, "nci_register_device failed %d\n", rc);
-		nci_free_device(priv->ndev);
-		goto error;
+		goto error_free_dev;
+	}
+
+	/* Ensure that controller is powered off */
+	nfcmrvl_chip_halt(priv);
+
+	rc = nfcmrvl_fw_dnld_init(priv);
+	if (rc) {
+		nfc_err(dev, "failed to initialize FW download %d\n", rc);
+		goto error_free_dev;
 	}
 
 	nfc_info(dev, "registered with nci successfully\n");
 	return priv;
 
+error_free_dev:
+	nci_free_device(priv->ndev);
 error:
 	kfree(priv);
 	return ERR_PTR(rc);
@@ -167,6 +189,11 @@ void nfcmrvl_nci_unregister_dev(struct nfcmrvl_private *priv)
 {
 	struct nci_dev *ndev = priv->ndev;
 
+	if (priv->ndev->nfc_dev->fw_download_in_progress)
+		nfcmrvl_fw_dnld_abort(priv);
+
+	nfcmrvl_fw_dnld_deinit(priv);
+
 	nci_unregister_device(ndev);
 	nci_free_device(ndev);
 	kfree(priv);
@@ -187,6 +214,11 @@ int nfcmrvl_nci_recv_frame(struct nfcmrvl_private *priv, struct sk_buff *skb)
 		}
 	}
 
+	if (priv->ndev->nfc_dev->fw_download_in_progress) {
+		nfcmrvl_fw_dnld_recv_frame(priv, skb);
+		return 0;
+	}
+
 	if (test_bit(NFCMRVL_NCI_RUNNING, &priv->flags))
 		nci_recv_frame(priv->ndev, skb);
 	else {
@@ -201,10 +233,8 @@ EXPORT_SYMBOL_GPL(nfcmrvl_nci_recv_frame);
 
 void nfcmrvl_chip_reset(struct nfcmrvl_private *priv)
 {
-	/*
-	 * This function does not take care if someone is using the device.
-	 * To be improved.
-	 */
+	/* Reset possible fault of previous session */
+	clear_bit(NFCMRVL_PHY_ERROR, &priv->flags);
 
 	if (priv->config.reset_n_io) {
 		nfc_info(priv->dev, "reset the chip\n");
@@ -215,6 +245,12 @@ void nfcmrvl_chip_reset(struct nfcmrvl_private *priv)
 		nfc_info(priv->dev, "no reset available on this interface\n");
 }
 
+void nfcmrvl_chip_halt(struct nfcmrvl_private *priv)
+{
+	if (priv->config.reset_n_io)
+		gpio_set_value(priv->config.reset_n_io, 0);
+}
+
 #ifdef CONFIG_OF
 
 int nfcmrvl_parse_dt(struct device_node *node,
@@ -252,6 +288,5 @@ int nfcmrvl_parse_dt(struct device_node *node,
 EXPORT_SYMBOL_GPL(nfcmrvl_parse_dt);
 
 MODULE_AUTHOR("Marvell International Ltd.");
-MODULE_DESCRIPTION("Marvell NFC driver ver " VERSION);
-MODULE_VERSION(VERSION);
+MODULE_DESCRIPTION("Marvell NFC driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/nfc/nfcmrvl/nfcmrvl.h b/drivers/nfc/nfcmrvl/nfcmrvl.h
index e5a7e5464f2e..de68ff45e49a 100644
--- a/drivers/nfc/nfcmrvl/nfcmrvl.h
+++ b/drivers/nfc/nfcmrvl/nfcmrvl.h
@@ -1,7 +1,7 @@
 /**
  * Marvell NFC driver
  *
- * Copyright (C) 2014, Marvell International Ltd.
+ * Copyright (C) 2014-2015, Marvell International Ltd.
  *
  * This software file (the "File") is distributed by Marvell International
  * Ltd. under the terms of the GNU General Public License Version 2, June 1991
@@ -21,8 +21,11 @@
 
 #include <linux/platform_data/nfcmrvl.h>
 
+#include "fw_dnld.h"
+
 /* Define private flags: */
 #define NFCMRVL_NCI_RUNNING			1
+#define NFCMRVL_PHY_ERROR			2
 
 #define NFCMRVL_EXT_COEX_ID			0xE0
 #define NFCMRVL_NOT_ALLOWED_ID			0xE1
@@ -37,6 +40,8 @@
 */
 
 #define NFCMRVL_PB_BAIL_OUT			0x11
+#define NFCMRVL_PROP_REF_CLOCK			0xF0
+#define NFCMRVL_PROP_SET_HI_CONFIG		0xF1
 
 /*
 ** HCI defines
@@ -52,9 +57,10 @@
 enum nfcmrvl_phy {
 	NFCMRVL_PHY_USB		= 0,
 	NFCMRVL_PHY_UART	= 1,
+	NFCMRVL_PHY_I2C		= 2,
+	NFCMRVL_PHY_SPI		= 3,
 };
 
-
 struct nfcmrvl_private {
 
 	unsigned long flags;
@@ -62,8 +68,15 @@ struct nfcmrvl_private {
 	/* Platform configuration */
 	struct nfcmrvl_platform_data config;
 
+	/* Parent dev */
 	struct nci_dev *ndev;
 
+	/* FW download context */
+	struct nfcmrvl_fw_dnld fw_dnld;
+
+	/* FW download support */
+	bool support_fw_dnld;
+
 	/*
 	** PHY related information
 	*/
@@ -82,17 +95,21 @@ struct nfcmrvl_if_ops {
 	int (*nci_open) (struct nfcmrvl_private *priv);
 	int (*nci_close) (struct nfcmrvl_private *priv);
 	int (*nci_send) (struct nfcmrvl_private *priv, struct sk_buff *skb);
+	void (*nci_update_config)(struct nfcmrvl_private *priv,
+				  const void *param);
 };
 
 void nfcmrvl_nci_unregister_dev(struct nfcmrvl_private *priv);
 int nfcmrvl_nci_recv_frame(struct nfcmrvl_private *priv, struct sk_buff *skb);
-struct nfcmrvl_private *nfcmrvl_nci_register_dev(void *drv_data,
+struct nfcmrvl_private *nfcmrvl_nci_register_dev(enum nfcmrvl_phy phy,
+				void *drv_data,
 				struct nfcmrvl_if_ops *ops,
 				struct device *dev,
 				struct nfcmrvl_platform_data *pdata);
 
 
 void nfcmrvl_chip_reset(struct nfcmrvl_private *priv);
+void nfcmrvl_chip_halt(struct nfcmrvl_private *priv);
 
 int nfcmrvl_parse_dt(struct device_node *node,
 		     struct nfcmrvl_platform_data *pdata);
diff --git a/drivers/nfc/nfcmrvl/spi.c b/drivers/nfc/nfcmrvl/spi.c
new file mode 100644
index 000000000000..a7faa0bcc01e
--- /dev/null
+++ b/drivers/nfc/nfcmrvl/spi.c
@@ -0,0 +1,228 @@
+/**
+ * Marvell NFC-over-SPI driver: SPI interface related functions
+ *
+ * Copyright (C) 2015, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ **/
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/nfc.h>
+#include <linux/gpio.h>
+#include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/spi/spi.h>
+#include <linux/gpio.h>
+#include "nfcmrvl.h"
+
+#define SPI_WAIT_HANDSHAKE	1
+
+struct nfcmrvl_spi_drv_data {
+	unsigned long flags;
+	struct spi_device *spi;
+	struct nci_spi *nci_spi;
+	struct completion handshake_completion;
+	struct nfcmrvl_private *priv;
+};
+
+static irqreturn_t nfcmrvl_spi_int_irq_thread_fn(int irq, void *drv_data_ptr)
+{
+	struct nfcmrvl_spi_drv_data *drv_data = drv_data_ptr;
+	struct sk_buff *skb;
+
+	/*
+	 * Special case where we are waiting for SPI_INT deassertion to start a
+	 * transfer.
+	 */
+	if (test_and_clear_bit(SPI_WAIT_HANDSHAKE, &drv_data->flags)) {
+		complete(&drv_data->handshake_completion);
+		return IRQ_HANDLED;
+	}
+
+	/* Normal case, SPI_INT deasserted by slave to trigger a master read */
+
+	skb = nci_spi_read(drv_data->nci_spi);
+	if (!skb) {
+		nfc_err(&drv_data->spi->dev, "failed to read spi packet");
+		return IRQ_HANDLED;
+	}
+
+	if (nfcmrvl_nci_recv_frame(drv_data->priv, skb) < 0)
+		nfc_err(&drv_data->spi->dev, "corrupted RX packet");
+
+	return IRQ_HANDLED;
+}
+
+static int nfcmrvl_spi_nci_open(struct nfcmrvl_private *priv)
+{
+	return 0;
+}
+
+static int nfcmrvl_spi_nci_close(struct nfcmrvl_private *priv)
+{
+	return 0;
+}
+
+static int nfcmrvl_spi_nci_send(struct nfcmrvl_private *priv,
+				struct sk_buff *skb)
+{
+	struct nfcmrvl_spi_drv_data *drv_data = priv->drv_data;
+	int err;
+
+	/* Reinit completion for slave handshake */
+	reinit_completion(&drv_data->handshake_completion);
+	set_bit(SPI_WAIT_HANDSHAKE, &drv_data->flags);
+
+	/*
+	 * Append a dummy byte at the end of SPI frame. This is due to a
+	 * specific DMA implementation in the controller
+	 */
+	skb_put(skb, 1);
+
+	/* Send the SPI packet */
+	err = nci_spi_send(drv_data->nci_spi, &drv_data->handshake_completion,
+			   skb);
+	if (err != 0) {
+		nfc_err(priv->dev, "spi_send failed %d", err);
+		kfree_skb(skb);
+	}
+	return err;
+}
+
+static void nfcmrvl_spi_nci_update_config(struct nfcmrvl_private *priv,
+					  const void *param)
+{
+	struct nfcmrvl_spi_drv_data *drv_data = priv->drv_data;
+	const struct nfcmrvl_fw_spi_config *config = param;
+
+	drv_data->nci_spi->xfer_speed_hz = config->clk;
+}
+
+static struct nfcmrvl_if_ops spi_ops = {
+	.nci_open = nfcmrvl_spi_nci_open,
+	.nci_close = nfcmrvl_spi_nci_close,
+	.nci_send = nfcmrvl_spi_nci_send,
+	.nci_update_config = nfcmrvl_spi_nci_update_config,
+};
+
+static int nfcmrvl_spi_parse_dt(struct device_node *node,
+				struct nfcmrvl_platform_data *pdata)
+{
+	int ret;
+
+	ret = nfcmrvl_parse_dt(node, pdata);
+	if (ret < 0) {
+		pr_err("Failed to get generic entries\n");
+		return ret;
+	}
+
+	ret = irq_of_parse_and_map(node, 0);
+	if (ret < 0) {
+		pr_err("Unable to get irq, error: %d\n", ret);
+		return ret;
+	}
+	pdata->irq = ret;
+
+	return 0;
+}
+
+static int nfcmrvl_spi_probe(struct spi_device *spi)
+{
+	struct nfcmrvl_platform_data *pdata;
+	struct nfcmrvl_platform_data config;
+	struct nfcmrvl_spi_drv_data *drv_data;
+	int ret = 0;
+
+	drv_data = devm_kzalloc(&spi->dev, sizeof(*drv_data), GFP_KERNEL);
+	if (!drv_data)
+		return -ENOMEM;
+
+	drv_data->spi = spi;
+	drv_data->priv = NULL;
+	spi_set_drvdata(spi, drv_data);
+
+	pdata = spi->dev.platform_data;
+
+	if (!pdata && spi->dev.of_node)
+		if (nfcmrvl_spi_parse_dt(spi->dev.of_node, &config) == 0)
+			pdata = &config;
+
+	if (!pdata)
+		return -EINVAL;
+
+	ret = devm_request_threaded_irq(&drv_data->spi->dev, pdata->irq,
+					NULL, nfcmrvl_spi_int_irq_thread_fn,
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					"nfcmrvl_spi_int", drv_data);
+	if (ret < 0) {
+		nfc_err(&drv_data->spi->dev, "Unable to register IRQ handler");
+		return -ENODEV;
+	}
+
+	drv_data->priv = nfcmrvl_nci_register_dev(NFCMRVL_PHY_SPI,
+						  drv_data, &spi_ops,
+						  &drv_data->spi->dev,
+						  pdata);
+	if (IS_ERR(drv_data->priv))
+		return PTR_ERR(drv_data->priv);
+
+	drv_data->priv->support_fw_dnld = true;
+
+	drv_data->nci_spi = nci_spi_allocate_spi(drv_data->spi, 0, 10,
+						 drv_data->priv->ndev);
+
+	/* Init completion for slave handshake */
+	init_completion(&drv_data->handshake_completion);
+	return 0;
+}
+
+static int nfcmrvl_spi_remove(struct spi_device *spi)
+{
+	struct nfcmrvl_spi_drv_data *drv_data = spi_get_drvdata(spi);
+
+	nfcmrvl_nci_unregister_dev(drv_data->priv);
+	return 0;
+}
+
+static const struct of_device_id of_nfcmrvl_spi_match[] = {
+	{ .compatible = "marvell,nfc-spi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_nfcmrvl_spi_match);
+
+static const struct spi_device_id nfcmrvl_spi_id_table[] = {
+	{ "nfcmrvl_spi", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, nfcmrvl_spi_id_table);
+
+static struct spi_driver nfcmrvl_spi_driver = {
+	.probe		= nfcmrvl_spi_probe,
+	.remove		= nfcmrvl_spi_remove,
+	.id_table	= nfcmrvl_spi_id_table,
+	.driver		= {
+		.name		= "nfcmrvl_spi",
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match_ptr(of_nfcmrvl_spi_match),
+	},
+};
+
+module_spi_driver(nfcmrvl_spi_driver);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("Marvell NFC-over-SPI driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/nfc/nfcmrvl/uart.c b/drivers/nfc/nfcmrvl/uart.c
index 61442d6528a6..f3d041c4f249 100644
--- a/drivers/nfc/nfcmrvl/uart.c
+++ b/drivers/nfc/nfcmrvl/uart.c
@@ -50,10 +50,21 @@ static int nfcmrvl_uart_nci_send(struct nfcmrvl_private *priv,
 	return nu->ops.send(nu, skb);
 }
 
+static void nfcmrvl_uart_nci_update_config(struct nfcmrvl_private *priv,
+					   const void *param)
+{
+	struct nci_uart *nu = priv->drv_data;
+	const struct nfcmrvl_fw_uart_config *config = param;
+
+	nci_uart_set_config(nu, le32_to_cpu(config->baudrate),
+			    config->flow_control);
+}
+
 static struct nfcmrvl_if_ops uart_ops = {
 	.nci_open = nfcmrvl_uart_nci_open,
 	.nci_close = nfcmrvl_uart_nci_close,
 	.nci_send = nfcmrvl_uart_nci_send,
+	.nci_update_config = nfcmrvl_uart_nci_update_config
 };
 
 #ifdef CONFIG_OF
@@ -64,9 +75,13 @@ static int nfcmrvl_uart_parse_dt(struct device_node *node,
 	struct device_node *matched_node;
 	int ret;
 
-	matched_node = of_find_compatible_node(node, NULL, "mrvl,nfc-uart");
-	if (!matched_node)
-		return -ENODEV;
+	matched_node = of_find_compatible_node(node, NULL, "marvell,nfc-uart");
+	if (!matched_node) {
+		matched_node = of_find_compatible_node(node, NULL,
+						       "mrvl,nfc-uart");
+		if (!matched_node)
+			return -ENODEV;
+	}
 
 	ret = nfcmrvl_parse_dt(matched_node, pdata);
 	if (ret < 0) {
@@ -127,11 +142,12 @@ static int nfcmrvl_nci_uart_open(struct nci_uart *nu)
 		pdata = &config;
 	}
 
-	priv = nfcmrvl_nci_register_dev(nu, &uart_ops, nu->tty->dev, pdata);
+	priv = nfcmrvl_nci_register_dev(NFCMRVL_PHY_UART, nu, &uart_ops,
+					nu->tty->dev, pdata);
 	if (IS_ERR(priv))
 		return PTR_ERR(priv);
 
-	priv->phy = NFCMRVL_PHY_UART;
+	priv->support_fw_dnld = true;
 
 	nu->drv_data = priv;
 	nu->ndev = priv->ndev;
diff --git a/drivers/nfc/nfcmrvl/usb.c b/drivers/nfc/nfcmrvl/usb.c
index 7d1fe436c9f6..585a0f20835b 100644
--- a/drivers/nfc/nfcmrvl/usb.c
+++ b/drivers/nfc/nfcmrvl/usb.c
@@ -23,8 +23,6 @@
 #include <net/nfc/nci_core.h>
 #include "nfcmrvl.h"
 
-#define VERSION "1.0"
-
 static struct usb_device_id nfcmrvl_table[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x1286, 0x2046,
 					USB_CLASS_VENDOR_SPEC, 4, 1) },
@@ -342,13 +340,14 @@ static int nfcmrvl_probe(struct usb_interface *intf,
 	init_usb_anchor(&drv_data->bulk_anchor);
 	init_usb_anchor(&drv_data->deferred);
 
-	priv = nfcmrvl_nci_register_dev(drv_data, &usb_ops,
+	priv = nfcmrvl_nci_register_dev(NFCMRVL_PHY_USB, drv_data, &usb_ops,
 					&drv_data->udev->dev, &config);
 	if (IS_ERR(priv))
 		return PTR_ERR(priv);
 
 	drv_data->priv = priv;
-	drv_data->priv->phy = NFCMRVL_PHY_USB;
+	drv_data->priv->support_fw_dnld = false;
+
 	priv->dev = &drv_data->udev->dev;
 
 	usb_set_intfdata(intf, drv_data);
@@ -469,6 +468,5 @@ static struct usb_driver nfcmrvl_usb_driver = {
 module_usb_driver(nfcmrvl_usb_driver);
 
 MODULE_AUTHOR("Marvell International Ltd.");
-MODULE_DESCRIPTION("Marvell NFC-over-USB driver ver " VERSION);
-MODULE_VERSION(VERSION);
+MODULE_DESCRIPTION("Marvell NFC-over-USB driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/nfc/nfcsim.c b/drivers/nfc/nfcsim.c
index 93111fa8d282..26ac9e5fa1ab 100644
--- a/drivers/nfc/nfcsim.c
+++ b/drivers/nfc/nfcsim.c
@@ -246,7 +246,7 @@ static int nfcsim_activate_target(struct nfc_dev *nfc_dev,
 }
 
 static void nfcsim_deactivate_target(struct nfc_dev *nfc_dev,
-				     struct nfc_target *target)
+				     struct nfc_target *target, u8 mode)
 {
 	struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
diff --git a/drivers/nfc/nfcwilink.c b/drivers/nfc/nfcwilink.c
index ce2e2cf54fbc..f81e500e7650 100644
--- a/drivers/nfc/nfcwilink.c
+++ b/drivers/nfc/nfcwilink.c
@@ -497,7 +497,7 @@ static struct nci_ops nfcwilink_ops = {
 
 static int nfcwilink_probe(struct platform_device *pdev)
 {
-	static struct nfcwilink *drv;
+	struct nfcwilink *drv;
 	int rc;
 	__u32 protocols;
 
diff --git a/drivers/nfc/nxp-nci/core.c b/drivers/nfc/nxp-nci/core.c
index 8979636d48ea..2e4b004a96aa 100644
--- a/drivers/nfc/nxp-nci/core.c
+++ b/drivers/nfc/nxp-nci/core.c
@@ -109,7 +109,8 @@ static struct nci_ops nxp_nci_ops = {
 };
 
 int nxp_nci_probe(void *phy_id, struct device *pdev,
-		  struct nxp_nci_phy_ops *phy_ops, unsigned int max_payload,
+		  const struct nxp_nci_phy_ops *phy_ops,
+		  unsigned int max_payload,
 		  struct nci_dev **ndev)
 {
 	struct nxp_nci_info *info;
diff --git a/drivers/nfc/nxp-nci/i2c.c b/drivers/nfc/nxp-nci/i2c.c
index fac80c691914..df4333c7ee0f 100644
--- a/drivers/nfc/nxp-nci/i2c.c
+++ b/drivers/nfc/nxp-nci/i2c.c
@@ -106,7 +106,7 @@ static int nxp_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 	return r;
 }
 
-static struct nxp_nci_phy_ops i2c_phy_ops = {
+static const struct nxp_nci_phy_ops i2c_phy_ops = {
 	.set_mode = nxp_nci_i2c_set_mode,
 	.write = nxp_nci_i2c_write,
 };
diff --git a/drivers/nfc/nxp-nci/nxp-nci.h b/drivers/nfc/nxp-nci/nxp-nci.h
index f1fecc4e2457..20408cbff4f1 100644
--- a/drivers/nfc/nxp-nci/nxp-nci.h
+++ b/drivers/nfc/nxp-nci/nxp-nci.h
@@ -68,7 +68,7 @@ struct nxp_nci_info {
 
 	enum nxp_nci_mode mode;
 
-	struct nxp_nci_phy_ops *phy_ops;
+	const struct nxp_nci_phy_ops *phy_ops;
 	unsigned int max_payload;
 
 	struct mutex info_lock;
@@ -82,7 +82,8 @@ void nxp_nci_fw_recv_frame(struct nci_dev *ndev, struct sk_buff *skb);
 void nxp_nci_fw_work_complete(struct nxp_nci_info *info, int result);
 
 int nxp_nci_probe(void *phy_id, struct device *pdev,
-		  struct nxp_nci_phy_ops *phy_ops, unsigned int max_payload,
+		  const struct nxp_nci_phy_ops *phy_ops,
+		  unsigned int max_payload,
 		  struct nci_dev **ndev);
 void nxp_nci_remove(struct nci_dev *ndev);
 
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index a03e4eb5fe29..bb3d5ea9869c 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -2263,7 +2263,7 @@ static int pn533_activate_target(struct nfc_dev *nfc_dev,
 }
 
 static void pn533_deactivate_target(struct nfc_dev *nfc_dev,
-				    struct nfc_target *target)
+				    struct nfc_target *target, u8 mode)
 {
 	struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 	struct sk_buff *skb;
diff --git a/drivers/nfc/pn544/Kconfig b/drivers/nfc/pn544/Kconfig
index ccf06f5f6ebb..2b8bde39540d 100644
--- a/drivers/nfc/pn544/Kconfig
+++ b/drivers/nfc/pn544/Kconfig
@@ -1,20 +1,15 @@
 config NFC_PN544
-	tristate "NXP PN544 NFC driver"
-	depends on NFC_HCI
+	tristate
 	select CRC_CCITT
-	default n
 	---help---
 	  NXP PN544 core driver.
 	  This is a driver based on the HCI NFC kernel layers and
 	  will thus not work with NXP libnfc library.
 
-	  To compile this driver as a module, choose m here. The module will
-	  be called pn544.
-	  Say N if unsure.
-
 config NFC_PN544_I2C
-	tristate "NFC PN544 i2c support"
-	depends on NFC_PN544 && I2C && NFC_SHDLC
+	tristate "NXP PN544 device support (I2C)"
+	depends on NFC_HCI && I2C && NFC_SHDLC
+	select NFC_PN544
 	---help---
 	  This module adds support for the NXP pn544 i2c interface.
 	  Select this if your platform is using the i2c bus.
@@ -23,8 +18,9 @@ config NFC_PN544_I2C
 	  Say N if unsure.
 
 config NFC_PN544_MEI
-	tristate "NFC PN544 MEI support"
-	depends on NFC_PN544 && NFC_MEI_PHY
+	tristate "NXP PN544 device support (MEI)"
+	depends on NFC_HCI && NFC_MEI_PHY
+	select NFC_PN544
 	---help---
 	  This module adds support for the mei interface of adapters using
 	  NXP pn544 chipsets.  Select this if your pn544 chipset
diff --git a/drivers/nfc/s3fwrn5/Kconfig b/drivers/nfc/s3fwrn5/Kconfig
index 7e3b255b3f99..1eef9199486e 100644
--- a/drivers/nfc/s3fwrn5/Kconfig
+++ b/drivers/nfc/s3fwrn5/Kconfig
@@ -1,5 +1,6 @@
 config NFC_S3FWRN5
 	tristate
+	select CRYPTO
 	---help---
 	  Core driver for Samsung S3FWRN5 NFC chip. Contains core utilities
 	  of chip. It's intended to be used by PHYs to avoid duplicating lots
diff --git a/drivers/nfc/s3fwrn5/Makefile b/drivers/nfc/s3fwrn5/Makefile
index 3381c34faf62..ddfa7be7dd05 100644
--- a/drivers/nfc/s3fwrn5/Makefile
+++ b/drivers/nfc/s3fwrn5/Makefile
@@ -7,5 +7,3 @@ s3fwrn5_i2c-objs = i2c.o
 
 obj-$(CONFIG_NFC_S3FWRN5) += s3fwrn5.o
 obj-$(CONFIG_NFC_S3FWRN5_I2C) += s3fwrn5_i2c.o
-
-ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/s3fwrn5/i2c.c b/drivers/nfc/s3fwrn5/i2c.c
index b4dd7dd47473..c61d8a308da4 100644
--- a/drivers/nfc/s3fwrn5/i2c.c
+++ b/drivers/nfc/s3fwrn5/i2c.c
@@ -258,7 +258,7 @@ static int s3fwrn5_i2c_probe(struct i2c_client *client,
 	if (ret < 0)
 		return ret;
 
-	ret = request_threaded_irq(phy->i2c_dev->irq, NULL,
+	ret = devm_request_threaded_irq(&client->dev, phy->i2c_dev->irq, NULL,
 		s3fwrn5_i2c_irq_thread_fn, IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
 		S3FWRN5_I2C_DRIVER_NAME, phy);
 	if (ret)
diff --git a/drivers/nfc/s3fwrn5/nci.c b/drivers/nfc/s3fwrn5/nci.c
index ace0071c5339..075e4e877b33 100644
--- a/drivers/nfc/s3fwrn5/nci.c
+++ b/drivers/nfc/s3fwrn5/nci.c
@@ -31,7 +31,7 @@ static int s3fwrn5_nci_prop_rsp(struct nci_dev *ndev, struct sk_buff *skb)
 	return 0;
 }
 
-static struct nci_prop_ops s3fwrn5_nci_prop_ops[] = {
+static struct nci_driver_ops s3fwrn5_nci_prop_ops[] = {
 	{
 		.opcode = nci_opcode_pack(NCI_GID_PROPRIETARY,
 				NCI_PROP_AGAIN),
@@ -79,7 +79,7 @@ static struct nci_prop_ops s3fwrn5_nci_prop_ops[] = {
 	},
 };
 
-void s3fwrn5_nci_get_prop_ops(struct nci_prop_ops **ops, size_t *n)
+void s3fwrn5_nci_get_prop_ops(struct nci_driver_ops **ops, size_t *n)
 {
 	*ops = s3fwrn5_nci_prop_ops;
 	*n = ARRAY_SIZE(s3fwrn5_nci_prop_ops);
diff --git a/drivers/nfc/s3fwrn5/nci.h b/drivers/nfc/s3fwrn5/nci.h
index 0e68d439dde6..60c7fb575b66 100644
--- a/drivers/nfc/s3fwrn5/nci.h
+++ b/drivers/nfc/s3fwrn5/nci.h
@@ -83,7 +83,7 @@ struct nci_prop_fw_cfg_rsp {
 
 #define NCI_PROP_WR_RESET	0x2f
 
-void s3fwrn5_nci_get_prop_ops(struct nci_prop_ops **ops, size_t *n);
+void s3fwrn5_nci_get_prop_ops(struct nci_driver_ops **ops, size_t *n);
 int s3fwrn5_nci_rf_configure(struct s3fwrn5_info *info, const char *fw_name);
 
 #endif /* __LOCAL_S3FWRN5_NCI_H_ */
diff --git a/drivers/nfc/st-nci/Makefile b/drivers/nfc/st-nci/Makefile
index 348ce76f2177..439b2fa8654a 100644
--- a/drivers/nfc/st-nci/Makefile
+++ b/drivers/nfc/st-nci/Makefile
@@ -1,8 +1,8 @@
 #
-# Makefile for ST21NFCB NCI based NFC driver
+# Makefile for ST_NCI NCI based NFC driver
 #
 
-st-nci-objs = ndlc.o core.o st-nci_se.o
+st-nci-objs = ndlc.o core.o se.o vendor_cmds.o
 obj-$(CONFIG_NFC_ST_NCI)     += st-nci.o
 
 st-nci_i2c-objs = i2c.o
diff --git a/drivers/nfc/st-nci/core.c b/drivers/nfc/st-nci/core.c
index c419d3943973..c693128ee6fb 100644
--- a/drivers/nfc/st-nci/core.c
+++ b/drivers/nfc/st-nci/core.c
@@ -24,7 +24,6 @@
 #include <linux/delay.h>
 
 #include "st-nci.h"
-#include "st-nci_se.h"
 
 #define DRIVER_DESC "NCI NFC driver for ST_NCI"
 
@@ -98,7 +97,7 @@ static int st_nci_prop_rsp_packet(struct nci_dev *ndev,
 	return 0;
 }
 
-static struct nci_prop_ops st_nci_prop_ops[] = {
+static struct nci_driver_ops st_nci_prop_ops[] = {
 	{
 		.opcode = nci_opcode_pack(NCI_GID_PROPRIETARY,
 					  ST_NCI_CORE_PROP),
@@ -124,7 +123,7 @@ static struct nci_ops st_nci_ops = {
 };
 
 int st_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
-		       int phy_tailroom)
+		 int phy_tailroom, struct st_nci_se_status *se_status)
 {
 	struct st_nci_info *info;
 	int r;
@@ -153,14 +152,23 @@ int st_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
 
 	nci_set_drvdata(ndlc->ndev, info);
 
+	r = st_nci_vendor_cmds_init(ndlc->ndev);
+	if (r) {
+		pr_err("Cannot register proprietary vendor cmds\n");
+		goto err_reg_dev;
+	}
+
 	r = nci_register_device(ndlc->ndev);
 	if (r) {
 		pr_err("Cannot register nfc device to nci core\n");
-		nci_free_device(ndlc->ndev);
-		return r;
+		goto err_reg_dev;
 	}
 
-	return st_nci_se_init(ndlc->ndev);
+	return st_nci_se_init(ndlc->ndev, se_status);
+
+err_reg_dev:
+	nci_free_device(ndlc->ndev);
+	return r;
 }
 EXPORT_SYMBOL_GPL(st_nci_probe);
 
diff --git a/drivers/nfc/st-nci/i2c.c b/drivers/nfc/st-nci/i2c.c
index 707ed2eb5936..15e3ce2d274c 100644
--- a/drivers/nfc/st-nci/i2c.c
+++ b/drivers/nfc/st-nci/i2c.c
@@ -27,12 +27,12 @@
 #include <linux/nfc.h>
 #include <linux/platform_data/st-nci.h>
 
-#include "ndlc.h"
+#include "st-nci.h"
 
 #define DRIVER_DESC "NCI NFC driver for ST_NCI"
 
 /* ndlc header */
-#define ST_NCI_FRAME_HEADROOM	1
+#define ST_NCI_FRAME_HEADROOM 1
 #define ST_NCI_FRAME_TAILROOM 0
 
 #define ST_NCI_I2C_MIN_SIZE 4   /* PCB(1) + NCI Packet header(3) */
@@ -50,16 +50,13 @@ struct st_nci_i2c_phy {
 	struct i2c_client *i2c_dev;
 	struct llt_ndlc *ndlc;
 
+	bool irq_active;
+
 	unsigned int gpio_reset;
 	unsigned int irq_polarity;
-};
 
-#define I2C_DUMP_SKB(info, skb)					\
-do {								\
-	pr_debug("%s:\n", info);				\
-	print_hex_dump(KERN_DEBUG, "i2c: ", DUMP_PREFIX_OFFSET,	\
-		       16, 1, (skb)->data, (skb)->len, 0);	\
-} while (0)
+	struct st_nci_se_status se_status;
+};
 
 static int st_nci_i2c_enable(void *phy_id)
 {
@@ -70,8 +67,10 @@ static int st_nci_i2c_enable(void *phy_id)
 	gpio_set_value(phy->gpio_reset, 1);
 	usleep_range(80000, 85000);
 
-	if (phy->ndlc->powered == 0)
+	if (phy->ndlc->powered == 0 && phy->irq_active == 0) {
 		enable_irq(phy->i2c_dev->irq);
+		phy->irq_active = true;
+	}
 
 	return 0;
 }
@@ -81,6 +80,7 @@ static void st_nci_i2c_disable(void *phy_id)
 	struct st_nci_i2c_phy *phy = phy_id;
 
 	disable_irq_nosync(phy->i2c_dev->irq);
+	phy->irq_active = false;
 }
 
 /*
@@ -94,8 +94,6 @@ static int st_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 	struct st_nci_i2c_phy *phy = phy_id;
 	struct i2c_client *client = phy->i2c_dev;
 
-	I2C_DUMP_SKB("st_nci_i2c_write", skb);
-
 	if (phy->ndlc->hard_fault != 0)
 		return phy->ndlc->hard_fault;
 
@@ -166,8 +164,6 @@ static int st_nci_i2c_read(struct st_nci_i2c_phy *phy,
 	skb_put(*skb, len);
 	memcpy((*skb)->data + ST_NCI_I2C_MIN_SIZE, buf, len);
 
-	I2C_DUMP_SKB("i2c frame read", *skb);
-
 	return 0;
 }
 
@@ -245,6 +241,11 @@ static int st_nci_i2c_of_request_resources(struct i2c_client *client)
 
 	phy->irq_polarity = irq_get_trigger_type(client->irq);
 
+	phy->se_status.is_ese_present =
+				of_property_read_bool(pp, "ese-present");
+	phy->se_status.is_uicc_present =
+				of_property_read_bool(pp, "uicc-present");
+
 	return 0;
 }
 #else
@@ -277,6 +278,9 @@ static int st_nci_i2c_request_resources(struct i2c_client *client)
 		return r;
 	}
 
+	phy->se_status.is_ese_present = pdata->is_ese_present;
+	phy->se_status.is_uicc_present = pdata->is_uicc_present;
+
 	return 0;
 }
 
@@ -326,12 +330,13 @@ static int st_nci_i2c_probe(struct i2c_client *client,
 
 	r = ndlc_probe(phy, &i2c_phy_ops, &client->dev,
 			ST_NCI_FRAME_HEADROOM, ST_NCI_FRAME_TAILROOM,
-			&phy->ndlc);
+			&phy->ndlc, &phy->se_status);
 	if (r < 0) {
 		nfc_err(&client->dev, "Unable to register ndlc layer\n");
 		return r;
 	}
 
+	phy->irq_active = true;
 	r = devm_request_threaded_irq(&client->dev, client->irq, NULL,
 				st_nci_irq_thread_fn,
 				phy->irq_polarity | IRQF_ONESHOT,
diff --git a/drivers/nfc/st-nci/ndlc.c b/drivers/nfc/st-nci/ndlc.c
index d2cf84e680c6..0884b11001ef 100644
--- a/drivers/nfc/st-nci/ndlc.c
+++ b/drivers/nfc/st-nci/ndlc.c
@@ -19,8 +19,8 @@
 #include <linux/sched.h>
 #include <net/nfc/nci_core.h>
 
-#include "ndlc.h"
 #include "st-nci.h"
+#include "ndlc.h"
 
 #define NDLC_TIMER_T1		100
 #define NDLC_TIMER_T1_WAIT	400
@@ -266,7 +266,8 @@ static void ndlc_t2_timeout(unsigned long data)
 }
 
 int ndlc_probe(void *phy_id, struct nfc_phy_ops *phy_ops, struct device *dev,
-	       int phy_headroom, int phy_tailroom, struct llt_ndlc **ndlc_id)
+	       int phy_headroom, int phy_tailroom, struct llt_ndlc **ndlc_id,
+	       struct st_nci_se_status *se_status)
 {
 	struct llt_ndlc *ndlc;
 
@@ -296,7 +297,7 @@ int ndlc_probe(void *phy_id, struct nfc_phy_ops *phy_ops, struct device *dev,
 
 	INIT_WORK(&ndlc->sm_work, llt_ndlc_sm_work);
 
-	return st_nci_probe(ndlc, phy_headroom, phy_tailroom);
+	return st_nci_probe(ndlc, phy_headroom, phy_tailroom, se_status);
 }
 EXPORT_SYMBOL(ndlc_probe);
 
diff --git a/drivers/nfc/st-nci/ndlc.h b/drivers/nfc/st-nci/ndlc.h
index 6361005ef003..bdf78ffd5bb7 100644
--- a/drivers/nfc/st-nci/ndlc.h
+++ b/drivers/nfc/st-nci/ndlc.h
@@ -22,6 +22,8 @@
 #include <linux/skbuff.h>
 #include <net/nfc/nfc.h>
 
+struct st_nci_se_status;
+
 /* Low Level Transport description */
 struct llt_ndlc {
 	struct nci_dev *ndev;
@@ -55,6 +57,7 @@ void ndlc_close(struct llt_ndlc *ndlc);
 int ndlc_send(struct llt_ndlc *ndlc, struct sk_buff *skb);
 void ndlc_recv(struct llt_ndlc *ndlc, struct sk_buff *skb);
 int ndlc_probe(void *phy_id, struct nfc_phy_ops *phy_ops, struct device *dev,
-	int phy_headroom, int phy_tailroom, struct llt_ndlc **ndlc_id);
+	       int phy_headroom, int phy_tailroom, struct llt_ndlc **ndlc_id,
+	       struct st_nci_se_status *se_status);
 void ndlc_remove(struct llt_ndlc *ndlc);
 #endif /* __LOCAL_NDLC_H__ */
diff --git a/drivers/nfc/st-nci/st-nci_se.c b/drivers/nfc/st-nci/se.c
index c742ef65a05a..dbab722a0654 100644
--- a/drivers/nfc/st-nci/st-nci_se.c
+++ b/drivers/nfc/st-nci/se.c
@@ -23,7 +23,6 @@
 #include <net/nfc/nci_core.h>
 
 #include "st-nci.h"
-#include "st-nci_se.h"
 
 struct st_nci_pipe_info {
 	u8 pipe_state;
@@ -40,7 +39,6 @@ struct st_nci_pipe_info {
 #define ST_NCI_ESE_HOST_ID            0xc0
 
 /* Gates */
-#define ST_NCI_DEVICE_MGNT_GATE       0x01
 #define ST_NCI_APDU_READER_GATE       0xf0
 #define ST_NCI_CONNECTIVITY_GATE      0x41
 
@@ -64,7 +62,7 @@ struct st_nci_pipe_info {
 
 #define ST_NCI_EVT_SE_HARD_RESET		0x20
 #define ST_NCI_EVT_TRANSMIT_DATA		0x10
-#define ST_NCI_EVT_WTX_REQUEST		0x11
+#define ST_NCI_EVT_WTX_REQUEST			0x11
 #define ST_NCI_EVT_SE_SOFT_RESET		0x11
 #define ST_NCI_EVT_SE_END_OF_APDU_TRANSFER	0x21
 #define ST_NCI_EVT_HOT_PLUG			0x03
@@ -113,6 +111,11 @@ static struct nci_hci_gate st_nci_gates[] = {
 	{ST_NCI_DEVICE_MGNT_GATE, ST_NCI_DEVICE_MGNT_PIPE,
 					ST_NCI_HOST_CONTROLLER_ID},
 
+	{NCI_HCI_IDENTITY_MGMT_GATE, NCI_HCI_INVALID_PIPE,
+					ST_NCI_HOST_CONTROLLER_ID},
+	{NCI_HCI_LOOPBACK_GATE, NCI_HCI_INVALID_PIPE,
+					ST_NCI_HOST_CONTROLLER_ID},
+
 	/* Secure element pipes are created by secure element host */
 	{ST_NCI_CONNECTIVITY_GATE, NCI_HCI_DO_NOT_OPEN_PIPE,
 					ST_NCI_HOST_CONTROLLER_ID},
@@ -226,27 +229,32 @@ int st_nci_hci_load_session(struct nci_dev *ndev)
 			continue;
 		}
 
-		for (j = 0; (j < ARRAY_SIZE(st_nci_gates)) &&
+		for (j = 3; (j < ARRAY_SIZE(st_nci_gates)) &&
 		     (st_nci_gates[j].gate != dm_pipe_info->dst_gate_id); j++)
 			;
 
 		if (j < ARRAY_SIZE(st_nci_gates) &&
 		    st_nci_gates[j].gate == dm_pipe_info->dst_gate_id &&
 		    ST_NCI_DM_IS_PIPE_OPEN(dm_pipe_info->pipe_state)) {
-			st_nci_gates[j].pipe = pipe_info[2];
+			ndev->hci_dev->init_data.gates[j].pipe = pipe_info[2];
 
 			ndev->hci_dev->gate2pipe[st_nci_gates[j].gate] =
-						st_nci_gates[j].pipe;
-			ndev->hci_dev->pipes[st_nci_gates[j].pipe].gate =
+						pipe_info[2];
+			ndev->hci_dev->pipes[pipe_info[2]].gate =
 						st_nci_gates[j].gate;
-			ndev->hci_dev->pipes[st_nci_gates[j].pipe].host =
+			ndev->hci_dev->pipes[pipe_info[2]].host =
 						dm_pipe_info->src_host_id;
 		}
 		kfree_skb(skb_pipe_info);
 	}
 
-	memcpy(ndev->hci_dev->init_data.gates, st_nci_gates,
-	       sizeof(st_nci_gates));
+	/*
+	 * 3 gates have a well known pipe ID. Only NCI_HCI_LINK_MGMT_GATE
+	 * is not yet open at this stage.
+	 */
+	r = nci_hci_connect_gate(ndev, ST_NCI_HOST_CONTROLLER_ID,
+				 NCI_HCI_LINK_MGMT_GATE,
+				 NCI_HCI_LINK_MGMT_PIPE);
 
 	kfree_skb(skb_pipe_list);
 	return r;
@@ -272,6 +280,8 @@ static void st_nci_hci_admin_event_received(struct nci_dev *ndev,
 			}
 		}
 	break;
+	default:
+		nfc_err(&ndev->nfc_dev->dev, "Unexpected event on admin gate\n");
 	}
 }
 
@@ -295,6 +305,9 @@ static int st_nci_hci_apdu_reader_event_received(struct nci_dev *ndev,
 		mod_timer(&info->se_info.bwi_timer, jiffies +
 			  msecs_to_jiffies(info->se_info.wt_timeout));
 	break;
+	default:
+		nfc_err(&ndev->nfc_dev->dev, "Unexpected event on apdu reader gate\n");
+		return 1;
 	}
 
 	kfree_skb(skb);
@@ -349,6 +362,7 @@ static int st_nci_hci_connectivity_event_received(struct nci_dev *ndev,
 		r = nfc_se_transaction(ndev->nfc_dev, host, transaction);
 		break;
 	default:
+		nfc_err(&ndev->nfc_dev->dev, "Unexpected event on connectivity gate\n");
 		return 1;
 	}
 	kfree_skb(skb);
@@ -369,8 +383,10 @@ void st_nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
 		st_nci_hci_apdu_reader_event_received(ndev, event, skb);
 	break;
 	case ST_NCI_CONNECTIVITY_GATE:
-		st_nci_hci_connectivity_event_received(ndev, host, event,
-							 skb);
+		st_nci_hci_connectivity_event_received(ndev, host, event, skb);
+	break;
+	case NCI_HCI_LOOPBACK_GATE:
+		st_nci_hci_loopback_event_received(ndev, event, skb);
 	break;
 	}
 }
@@ -403,15 +419,11 @@ void st_nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
 }
 EXPORT_SYMBOL_GPL(st_nci_hci_cmd_received);
 
-/*
- * Remarks: On some early st_nci firmware, nci_nfcee_mode_set(0)
- * is rejected
- */
 static int st_nci_control_se(struct nci_dev *ndev, u8 se_idx,
-				   u8 state)
+			     u8 state)
 {
 	struct st_nci_info *info = nci_get_drvdata(ndev);
-	int r;
+	int r, i;
 	struct sk_buff *sk_host_list;
 	u8 host_id;
 
@@ -433,7 +445,7 @@ static int st_nci_control_se(struct nci_dev *ndev, u8 se_idx,
 	 * retrieve a relevant host list.
 	 */
 	reinit_completion(&info->se_info.req_completion);
-	r = nci_nfcee_mode_set(ndev, se_idx, NCI_NFCEE_ENABLE);
+	r = nci_nfcee_mode_set(ndev, se_idx, state);
 	if (r != NCI_STATUS_OK)
 		return r;
 
@@ -449,14 +461,19 @@ static int st_nci_control_se(struct nci_dev *ndev, u8 se_idx,
 	 * There is no possible synchronization to prevent this.
 	 * Adding a small delay is the only way to solve the issue.
 	 */
-	usleep_range(3000, 5000);
+	if (info->se_info.se_status->is_ese_present &&
+	    info->se_info.se_status->is_uicc_present)
+		usleep_range(15000, 20000);
 
 	r = nci_hci_get_param(ndev, NCI_HCI_ADMIN_GATE,
 			NCI_HCI_ADMIN_PARAM_HOST_LIST, &sk_host_list);
 	if (r != NCI_HCI_ANY_OK)
 		return r;
 
-	host_id = sk_host_list->data[sk_host_list->len - 1];
+	for (i = 0; i < sk_host_list->len &&
+		sk_host_list->data[i] != se_idx; i++)
+		;
+	host_id = sk_host_list->data[i];
 	kfree_skb(sk_host_list);
 	if (state == ST_NCI_SE_MODE_ON && host_id == se_idx)
 		return se_idx;
@@ -472,11 +489,20 @@ int st_nci_disable_se(struct nci_dev *ndev, u32 se_idx)
 
 	pr_debug("st_nci_disable_se\n");
 
-	if (se_idx == NFC_SE_EMBEDDED) {
-		r = nci_hci_send_event(ndev, ST_NCI_APDU_READER_GATE,
-				ST_NCI_EVT_SE_END_OF_APDU_TRANSFER, NULL, 0);
-		if (r < 0)
-			return r;
+	/*
+	 * According to upper layer, se_idx == NFC_SE_UICC when
+	 * info->se_info.se_status->is_uicc_enable is true should never happen
+	 * Same for eSE.
+	 */
+	r = st_nci_control_se(ndev, se_idx, ST_NCI_SE_MODE_OFF);
+	if (r < 0) {
+		/* Do best effort to release SWP */
+		if (se_idx == NFC_SE_EMBEDDED) {
+			r = nci_hci_send_event(ndev, ST_NCI_APDU_READER_GATE,
+					ST_NCI_EVT_SE_END_OF_APDU_TRANSFER,
+					NULL, 0);
+		}
+		return r;
 	}
 
 	return 0;
@@ -489,11 +515,25 @@ int st_nci_enable_se(struct nci_dev *ndev, u32 se_idx)
 
 	pr_debug("st_nci_enable_se\n");
 
-	if (se_idx == ST_NCI_HCI_HOST_ID_ESE) {
+	/*
+	 * According to upper layer, se_idx == NFC_SE_UICC when
+	 * info->se_info.se_status->is_uicc_enable is true should never happen.
+	 * Same for eSE.
+	 */
+	r = st_nci_control_se(ndev, se_idx, ST_NCI_SE_MODE_ON);
+	if (r == ST_NCI_HCI_HOST_ID_ESE) {
+		st_nci_se_get_atr(ndev);
 		r = nci_hci_send_event(ndev, ST_NCI_APDU_READER_GATE,
 				ST_NCI_EVT_SE_SOFT_RESET, NULL, 0);
-		if (r < 0)
-			return r;
+	}
+
+	if (r < 0) {
+		/*
+		 * The activation procedure failed, the secure element
+		 * is not connected. Remove from the list.
+		 */
+		nfc_remove_se(ndev->nfc_dev, se_idx);
+		return r;
 	}
 
 	return 0;
@@ -502,6 +542,7 @@ EXPORT_SYMBOL_GPL(st_nci_enable_se);
 
 static int st_nci_hci_network_init(struct nci_dev *ndev)
 {
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 	struct core_conn_create_dest_spec_params *dest_params;
 	struct dest_spec_params spec_params;
 	struct nci_conn_info    *conn_info;
@@ -532,6 +573,7 @@ static int st_nci_hci_network_init(struct nci_dev *ndev)
 	if (!conn_info)
 		goto free_dest_params;
 
+	ndev->hci_dev->init_data.gate_count = ARRAY_SIZE(st_nci_gates);
 	memcpy(ndev->hci_dev->init_data.gates, st_nci_gates,
 	       sizeof(st_nci_gates));
 
@@ -553,10 +595,17 @@ static int st_nci_hci_network_init(struct nci_dev *ndev)
 	if (r != NCI_HCI_ANY_OK)
 		goto free_dest_params;
 
-	r = nci_nfcee_mode_set(ndev, ndev->hci_dev->conn_info->id,
-			       NCI_NFCEE_ENABLE);
-	if (r != NCI_STATUS_OK)
-		goto free_dest_params;
+	/*
+	 * In factory mode, we prevent secure elements activation
+	 * by disabling nfcee on the current HCI connection id.
+	 * HCI will be used here only for proprietary commands.
+	 */
+	if (test_bit(ST_NCI_FACTORY_MODE, &info->flags))
+		r = nci_nfcee_mode_set(ndev, ndev->hci_dev->conn_info->id,
+				       NCI_NFCEE_DISABLE);
+	else
+		r = nci_nfcee_mode_set(ndev, ndev->hci_dev->conn_info->id,
+				       NCI_NFCEE_ENABLE);
 
 free_dest_params:
 	kfree(dest_params);
@@ -567,9 +616,10 @@ exit:
 
 int st_nci_discover_se(struct nci_dev *ndev)
 {
-	u8 param[2];
-	int r;
+	u8 white_list[2];
+	int r, wl_size = 0;
 	int se_count = 0;
+	struct st_nci_info *info = nci_get_drvdata(ndev);
 
 	pr_debug("st_nci_discover_se\n");
 
@@ -577,29 +627,37 @@ int st_nci_discover_se(struct nci_dev *ndev)
 	if (r != 0)
 		return r;
 
-	param[0] = ST_NCI_UICC_HOST_ID;
-	param[1] = ST_NCI_HCI_HOST_ID_ESE;
-	r = nci_hci_set_param(ndev, NCI_HCI_ADMIN_GATE,
-				NCI_HCI_ADMIN_PARAM_WHITELIST,
-				param, sizeof(param));
-	if (r != NCI_HCI_ANY_OK)
-		return r;
+	if (test_bit(ST_NCI_FACTORY_MODE, &info->flags))
+		return 0;
+
+	if (info->se_info.se_status->is_ese_present &&
+	    info->se_info.se_status->is_uicc_present) {
+		white_list[wl_size++] = ST_NCI_UICC_HOST_ID;
+		white_list[wl_size++] = ST_NCI_ESE_HOST_ID;
+	} else if (!info->se_info.se_status->is_ese_present &&
+		   info->se_info.se_status->is_uicc_present) {
+		white_list[wl_size++] = ST_NCI_UICC_HOST_ID;
+	} else if (info->se_info.se_status->is_ese_present &&
+		   !info->se_info.se_status->is_uicc_present) {
+		white_list[wl_size++] = ST_NCI_ESE_HOST_ID;
+	}
 
-	r = st_nci_control_se(ndev, ST_NCI_UICC_HOST_ID,
-				ST_NCI_SE_MODE_ON);
-	if (r == ST_NCI_UICC_HOST_ID) {
+	if (wl_size) {
+		r = nci_hci_set_param(ndev, NCI_HCI_ADMIN_GATE,
+				      NCI_HCI_ADMIN_PARAM_WHITELIST,
+				      white_list, wl_size);
+		if (r != NCI_HCI_ANY_OK)
+			return r;
+	}
+
+	if (info->se_info.se_status->is_uicc_present) {
 		nfc_add_se(ndev->nfc_dev, ST_NCI_UICC_HOST_ID, NFC_SE_UICC);
 		se_count++;
 	}
 
-	/* Try to enable eSE in order to check availability */
-	r = st_nci_control_se(ndev, ST_NCI_HCI_HOST_ID_ESE,
-				ST_NCI_SE_MODE_ON);
-	if (r == ST_NCI_HCI_HOST_ID_ESE) {
-		nfc_add_se(ndev->nfc_dev, ST_NCI_HCI_HOST_ID_ESE,
-			   NFC_SE_EMBEDDED);
+	if (info->se_info.se_status->is_ese_present) {
+		nfc_add_se(ndev->nfc_dev, ST_NCI_ESE_HOST_ID, NFC_SE_EMBEDDED);
 		se_count++;
-		st_nci_se_get_atr(ndev);
 	}
 
 	return !se_count;
@@ -672,7 +730,7 @@ static void st_nci_se_activation_timeout(unsigned long data)
 	complete(&info->se_info.req_completion);
 }
 
-int st_nci_se_init(struct nci_dev *ndev)
+int st_nci_se_init(struct nci_dev *ndev, struct st_nci_se_status *se_status)
 {
 	struct st_nci_info *info = nci_get_drvdata(ndev);
 
@@ -694,6 +752,8 @@ int st_nci_se_init(struct nci_dev *ndev)
 	info->se_info.wt_timeout =
 		ST_NCI_BWI_TO_TIMEOUT(ST_NCI_ATR_DEFAULT_BWI);
 
+	info->se_info.se_status = se_status;
+
 	return 0;
 }
 EXPORT_SYMBOL(st_nci_se_init);
diff --git a/drivers/nfc/st-nci/spi.c b/drivers/nfc/st-nci/spi.c
index 598a58c4d6d1..cf7ad8121e11 100644
--- a/drivers/nfc/st-nci/spi.c
+++ b/drivers/nfc/st-nci/spi.c
@@ -25,9 +25,10 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
+#include <net/nfc/nci.h>
 #include <linux/platform_data/st-nci.h>
 
-#include "ndlc.h"
+#include "st-nci.h"
 
 #define DRIVER_DESC "NCI NFC driver for ST_NCI"
 
@@ -50,16 +51,13 @@ struct st_nci_spi_phy {
 	struct spi_device *spi_dev;
 	struct llt_ndlc *ndlc;
 
+	bool irq_active;
+
 	unsigned int gpio_reset;
 	unsigned int irq_polarity;
-};
 
-#define SPI_DUMP_SKB(info, skb)					\
-do {								\
-	pr_debug("%s:\n", info);				\
-	print_hex_dump(KERN_DEBUG, "spi: ", DUMP_PREFIX_OFFSET,	\
-		       16, 1, (skb)->data, (skb)->len, 0);	\
-} while (0)
+	struct st_nci_se_status se_status;
+};
 
 static int st_nci_spi_enable(void *phy_id)
 {
@@ -70,8 +68,10 @@ static int st_nci_spi_enable(void *phy_id)
 	gpio_set_value(phy->gpio_reset, 1);
 	usleep_range(80000, 85000);
 
-	if (phy->ndlc->powered == 0)
+	if (phy->ndlc->powered == 0 && phy->irq_active == 0) {
 		enable_irq(phy->spi_dev->irq);
+		phy->irq_active = true;
+	}
 
 	return 0;
 }
@@ -81,6 +81,7 @@ static void st_nci_spi_disable(void *phy_id)
 	struct st_nci_spi_phy *phy = phy_id;
 
 	disable_irq_nosync(phy->spi_dev->irq);
+	phy->irq_active = false;
 }
 
 /*
@@ -94,15 +95,14 @@ static int st_nci_spi_write(void *phy_id, struct sk_buff *skb)
 	struct st_nci_spi_phy *phy = phy_id;
 	struct spi_device *dev = phy->spi_dev;
 	struct sk_buff *skb_rx;
-	u8 buf[ST_NCI_SPI_MAX_SIZE];
+	u8 buf[ST_NCI_SPI_MAX_SIZE + NCI_DATA_HDR_SIZE +
+	       ST_NCI_FRAME_HEADROOM + ST_NCI_FRAME_TAILROOM];
 	struct spi_transfer spi_xfer = {
 		.tx_buf = skb->data,
 		.rx_buf = buf,
 		.len = skb->len,
 	};
 
-	SPI_DUMP_SKB("st_nci_spi_write", skb);
-
 	if (phy->ndlc->hard_fault != 0)
 		return phy->ndlc->hard_fault;
 
@@ -179,8 +179,6 @@ static int st_nci_spi_read(struct st_nci_spi_phy *phy,
 	skb_put(*skb, len);
 	memcpy((*skb)->data + ST_NCI_SPI_MIN_SIZE, buf, len);
 
-	SPI_DUMP_SKB("spi frame read", *skb);
-
 	return 0;
 }
 
@@ -258,6 +256,11 @@ static int st_nci_spi_of_request_resources(struct spi_device *dev)
 
 	phy->irq_polarity = irq_get_trigger_type(dev->irq);
 
+	phy->se_status.is_ese_present =
+				of_property_read_bool(pp, "ese-present");
+	phy->se_status.is_uicc_present =
+				of_property_read_bool(pp, "uicc-present");
+
 	return 0;
 }
 #else
@@ -290,6 +293,9 @@ static int st_nci_spi_request_resources(struct spi_device *dev)
 		return r;
 	}
 
+	phy->se_status.is_ese_present = pdata->is_ese_present;
+	phy->se_status.is_uicc_present = pdata->is_uicc_present;
+
 	return 0;
 }
 
@@ -340,12 +346,13 @@ static int st_nci_spi_probe(struct spi_device *dev)
 
 	r = ndlc_probe(phy, &spi_phy_ops, &dev->dev,
 			ST_NCI_FRAME_HEADROOM, ST_NCI_FRAME_TAILROOM,
-			&phy->ndlc);
+			&phy->ndlc, &phy->se_status);
 	if (r < 0) {
 		nfc_err(&dev->dev, "Unable to register ndlc layer\n");
 		return r;
 	}
 
+	phy->irq_active = true;
 	r = devm_request_threaded_irq(&dev->dev, dev->irq, NULL,
 				st_nci_irq_thread_fn,
 				phy->irq_polarity | IRQF_ONESHOT,
diff --git a/drivers/nfc/st-nci/st-nci.h b/drivers/nfc/st-nci/st-nci.h
index 850a2395deb7..8b9f77b0249c 100644
--- a/drivers/nfc/st-nci/st-nci.h
+++ b/drivers/nfc/st-nci/st-nci.h
@@ -19,7 +19,6 @@
 #ifndef __LOCAL_ST_NCI_H_
 #define __LOCAL_ST_NCI_H_
 
-#include "st-nci_se.h"
 #include "ndlc.h"
 
 /* Define private flags: */
@@ -28,6 +27,18 @@
 #define ST_NCI_CORE_PROP                0x01
 #define ST_NCI_SET_NFC_MODE             0x02
 
+/*
+ * ref ISO7816-3 chap 8.1. the initial character TS is followed by a
+ * sequence of at most 32 characters.
+ */
+#define ST_NCI_ESE_MAX_LENGTH  33
+#define ST_NCI_HCI_HOST_ID_ESE 0xc0
+
+#define ST_NCI_DEVICE_MGNT_GATE		0x01
+
+#define ST_NCI_VENDOR_OUI 0x0080E1 /* STMicroelectronics */
+#define ST_NCI_FACTORY_MODE 2
+
 struct nci_mode_set_cmd {
 	u8 cmd_type;
 	u8 mode;
@@ -37,14 +48,116 @@ struct nci_mode_set_rsp {
 	u8 status;
 } __packed;
 
+struct st_nci_se_status {
+	bool is_ese_present;
+	bool is_uicc_present;
+};
+
+struct st_nci_se_info {
+	struct st_nci_se_status *se_status;
+	u8 atr[ST_NCI_ESE_MAX_LENGTH];
+	struct completion req_completion;
+
+	struct timer_list bwi_timer;
+	int wt_timeout; /* in msecs */
+	bool bwi_active;
+
+	struct timer_list se_active_timer;
+	bool se_active;
+
+	bool xch_error;
+
+	se_io_cb_t cb;
+	void *cb_context;
+};
+
+/**
+ * enum nfc_vendor_cmds - supported nfc vendor commands
+ *
+ * @FACTORY_MODE: Allow to set the driver into a mode where no secure element
+ *	are activated. It does not consider any NFC_ATTR_VENDOR_DATA.
+ * @HCI_CLEAR_ALL_PIPES: Allow to execute a HCI clear all pipes command.
+ *	It does not consider any NFC_ATTR_VENDOR_DATA.
+ * @HCI_DM_PUT_DATA: Allow to configure specific CLF registry as for example
+ *	RF trimmings or low level drivers configurations (I2C, SPI, SWP).
+ * @HCI_DM_UPDATE_AID: Allow to configure an AID routing into the CLF routing
+ *	table following RF technology, CLF mode or protocol.
+ * @HCI_DM_GET_INFO: Allow to retrieve CLF information.
+ * @HCI_DM_GET_DATA: Allow to retrieve CLF configurable data such as low
+ *	level drivers configurations or RF trimmings.
+ * @HCI_DM_DIRECT_LOAD: Allow to load a firmware into the CLF. A complete
+ *	packet can be more than 8KB.
+ * @HCI_DM_RESET: Allow to run a CLF reset in order to "commit" CLF
+ *	configuration changes without CLF power off.
+ * @HCI_GET_PARAM: Allow to retrieve an HCI CLF parameter (for example the
+ *	white list).
+ * @HCI_DM_FIELD_GENERATOR: Allow to generate different kind of RF
+ *	technology. When using this command to anti-collision is done.
+ * @HCI_LOOPBACK: Allow to echo a command and test the Dh to CLF
+ *	connectivity.
+ * @HCI_DM_VDC_MEASUREMENT_VALUE: Allow to measure the field applied on the
+ *	CLF antenna. A value between 0 and 0x0f is returned. 0 is maximum.
+ * @HCI_DM_FWUPD_START: Allow to put CLF into firmware update mode. It is a
+ *	specific CLF command as there is no GPIO for this.
+ * @HCI_DM_FWUPD_END:  Allow to complete firmware update.
+ * @HCI_DM_VDC_VALUE_COMPARISON: Allow to compare the field applied on the
+ *	CLF antenna to a reference value.
+ * @MANUFACTURER_SPECIFIC: Allow to retrieve manufacturer specific data
+ *	received during a NCI_CORE_INIT_CMD.
+ */
+enum nfc_vendor_cmds {
+	FACTORY_MODE,
+	HCI_CLEAR_ALL_PIPES,
+	HCI_DM_PUT_DATA,
+	HCI_DM_UPDATE_AID,
+	HCI_DM_GET_INFO,
+	HCI_DM_GET_DATA,
+	HCI_DM_DIRECT_LOAD,
+	HCI_DM_RESET,
+	HCI_GET_PARAM,
+	HCI_DM_FIELD_GENERATOR,
+	HCI_LOOPBACK,
+	HCI_DM_FWUPD_START,
+	HCI_DM_FWUPD_END,
+	HCI_DM_VDC_MEASUREMENT_VALUE,
+	HCI_DM_VDC_VALUE_COMPARISON,
+	MANUFACTURER_SPECIFIC,
+};
+
+struct st_nci_vendor_info {
+	struct completion req_completion;
+	struct sk_buff *rx_skb;
+};
+
 struct st_nci_info {
 	struct llt_ndlc *ndlc;
 	unsigned long flags;
+
 	struct st_nci_se_info se_info;
+	struct st_nci_vendor_info vendor_info;
 };
 
 void st_nci_remove(struct nci_dev *ndev);
 int st_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
-		int phy_tailroom);
+		 int phy_tailroom, struct st_nci_se_status *se_status);
+
+int st_nci_se_init(struct nci_dev *ndev, struct st_nci_se_status *se_status);
+void st_nci_se_deinit(struct nci_dev *ndev);
+
+int st_nci_discover_se(struct nci_dev *ndev);
+int st_nci_enable_se(struct nci_dev *ndev, u32 se_idx);
+int st_nci_disable_se(struct nci_dev *ndev, u32 se_idx);
+int st_nci_se_io(struct nci_dev *ndev, u32 se_idx,
+				u8 *apdu, size_t apdu_length,
+				se_io_cb_t cb, void *cb_context);
+int st_nci_hci_load_session(struct nci_dev *ndev);
+void st_nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
+					u8 event, struct sk_buff *skb);
+void st_nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
+						struct sk_buff *skb);
+
+void st_nci_hci_loopback_event_received(struct nci_dev *ndev, u8 event,
+					struct sk_buff *skb);
+int st_nci_vendor_cmds_init(struct nci_dev *ndev);
 
 #endif /* __LOCAL_ST_NCI_H_ */
diff --git a/drivers/nfc/st-nci/st-nci_se.h b/drivers/nfc/st-nci/st-nci_se.h
deleted file mode 100644
index ea66e879d67f..000000000000
--- a/drivers/nfc/st-nci/st-nci_se.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * Secure Element Driver for STMicroelectronics NFC NCI Chip
- *
- * Copyright (C) 2014-2015 STMicroelectronics SAS. 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, see <http://www.gnu.org/licenses/>.
- */
-#ifndef __LOCAL_ST_NCI_SE_H_
-#define __LOCAL_ST_NCI_SE_H_
-
-/*
- * ref ISO7816-3 chap 8.1. the initial character TS is followed by a
- * sequence of at most 32 characters.
- */
-#define ST_NCI_ESE_MAX_LENGTH	33
-#define ST_NCI_HCI_HOST_ID_ESE	0xc0
-
-struct st_nci_se_info {
-	u8 atr[ST_NCI_ESE_MAX_LENGTH];
-	struct completion req_completion;
-
-	struct timer_list bwi_timer;
-	int wt_timeout; /* in msecs */
-	bool bwi_active;
-
-	struct timer_list se_active_timer;
-	bool se_active;
-
-	bool xch_error;
-
-	se_io_cb_t cb;
-	void *cb_context;
-};
-
-int st_nci_se_init(struct nci_dev *ndev);
-void st_nci_se_deinit(struct nci_dev *ndev);
-
-int st_nci_discover_se(struct nci_dev *ndev);
-int st_nci_enable_se(struct nci_dev *ndev, u32 se_idx);
-int st_nci_disable_se(struct nci_dev *ndev, u32 se_idx);
-int st_nci_se_io(struct nci_dev *ndev, u32 se_idx,
-		u8 *apdu, size_t apdu_length,
-		se_io_cb_t cb, void *cb_context);
-int st_nci_hci_load_session(struct nci_dev *ndev);
-void st_nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
-			u8 event, struct sk_buff *skb);
-void st_nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
-			struct sk_buff *skb);
-
-
-#endif /* __LOCAL_ST_NCI_SE_H_ */
diff --git a/drivers/nfc/st-nci/vendor_cmds.c b/drivers/nfc/st-nci/vendor_cmds.c
new file mode 100644
index 000000000000..b5debce4ae0b
--- /dev/null
+++ b/drivers/nfc/st-nci/vendor_cmds.c
@@ -0,0 +1,516 @@
+/*
+ * Proprietary commands extension for STMicroelectronics NFC NCI Chip
+ *
+ * Copyright (C) 2014-2015  STMicroelectronics SAS. 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <net/genetlink.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+#include <linux/delay.h>
+#include <net/nfc/nci_core.h>
+
+#include "st-nci.h"
+
+#define ST_NCI_HCI_DM_GETDATA			0x10
+#define ST_NCI_HCI_DM_PUTDATA			0x11
+#define ST_NCI_HCI_DM_LOAD			0x12
+#define ST_NCI_HCI_DM_GETINFO			0x13
+#define ST_NCI_HCI_DM_FWUPD_START		0x14
+#define ST_NCI_HCI_DM_FWUPD_STOP		0x15
+#define ST_NCI_HCI_DM_UPDATE_AID		0x20
+#define ST_NCI_HCI_DM_RESET			0x3e
+
+#define ST_NCI_HCI_DM_FIELD_GENERATOR		0x32
+#define ST_NCI_HCI_DM_VDC_MEASUREMENT_VALUE	0x33
+#define ST_NCI_HCI_DM_VDC_VALUE_COMPARISON	0x34
+
+#define ST_NCI_FACTORY_MODE_ON			1
+#define ST_NCI_FACTORY_MODE_OFF			0
+
+#define ST_NCI_EVT_POST_DATA			0x02
+
+struct get_param_data {
+	u8 gate;
+	u8 data;
+} __packed;
+
+static int st_nci_factory_mode(struct nfc_dev *dev, void *data,
+			       size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	if (data_len != 1)
+		return -EINVAL;
+
+	pr_debug("factory mode: %x\n", ((u8 *)data)[0]);
+
+	switch (((u8 *)data)[0]) {
+	case ST_NCI_FACTORY_MODE_ON:
+		test_and_set_bit(ST_NCI_FACTORY_MODE, &info->flags);
+	break;
+	case ST_NCI_FACTORY_MODE_OFF:
+		clear_bit(ST_NCI_FACTORY_MODE, &info->flags);
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int st_nci_hci_clear_all_pipes(struct nfc_dev *dev, void *data,
+				      size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	return nci_hci_clear_all_pipes(ndev);
+}
+
+static int st_nci_hci_dm_put_data(struct nfc_dev *dev, void *data,
+				  size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	return nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+				ST_NCI_HCI_DM_PUTDATA, data,
+				data_len, NULL);
+}
+
+static int st_nci_hci_dm_update_aid(struct nfc_dev *dev, void *data,
+				    size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	return nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			ST_NCI_HCI_DM_UPDATE_AID, data, data_len, NULL);
+}
+
+static int st_nci_hci_dm_get_info(struct nfc_dev *dev, void *data,
+				  size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE, ST_NCI_HCI_DM_GETINFO,
+			     data, data_len, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST_NCI_VENDOR_OUI,
+					     HCI_DM_GET_INFO, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st_nci_hci_dm_get_data(struct nfc_dev *dev, void *data,
+				  size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE, ST_NCI_HCI_DM_GETDATA,
+			     data, data_len, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST_NCI_VENDOR_OUI,
+					     HCI_DM_GET_DATA, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st_nci_hci_dm_fwupd_start(struct nfc_dev *dev, void *data,
+				     size_t data_len)
+{
+	int r;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	dev->fw_download_in_progress = true;
+	r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			ST_NCI_HCI_DM_FWUPD_START, data, data_len, NULL);
+	if (r)
+		dev->fw_download_in_progress = false;
+
+	return r;
+}
+
+static int st_nci_hci_dm_fwupd_end(struct nfc_dev *dev, void *data,
+				   size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	return nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			ST_NCI_HCI_DM_FWUPD_STOP, data, data_len, NULL);
+}
+
+static int st_nci_hci_dm_direct_load(struct nfc_dev *dev, void *data,
+				     size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	if (dev->fw_download_in_progress) {
+		dev->fw_download_in_progress = false;
+		return nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+				ST_NCI_HCI_DM_LOAD, data, data_len, NULL);
+	}
+	return -EPROTO;
+}
+
+static int st_nci_hci_dm_reset(struct nfc_dev *dev, void *data,
+			       size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			ST_NCI_HCI_DM_RESET, data, data_len, NULL);
+	msleep(200);
+
+	return 0;
+}
+
+static int st_nci_hci_get_param(struct nfc_dev *dev, void *data,
+				size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+	struct get_param_data *param = (struct get_param_data *)data;
+
+	if (data_len < sizeof(struct get_param_data))
+		return -EPROTO;
+
+	r = nci_hci_get_param(ndev, param->gate, param->data, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST_NCI_VENDOR_OUI,
+					     HCI_GET_PARAM, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st_nci_hci_dm_field_generator(struct nfc_dev *dev, void *data,
+					 size_t data_len)
+{
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	return nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+				ST_NCI_HCI_DM_FIELD_GENERATOR, data, data_len, NULL);
+}
+
+static int st_nci_hci_dm_vdc_measurement_value(struct nfc_dev *dev, void *data,
+					       size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	if (data_len != 4)
+		return -EPROTO;
+
+	r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			     ST_NCI_HCI_DM_VDC_MEASUREMENT_VALUE,
+			     data, data_len, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST_NCI_VENDOR_OUI,
+				HCI_DM_VDC_MEASUREMENT_VALUE, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st_nci_hci_dm_vdc_value_comparison(struct nfc_dev *dev, void *data,
+					      size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	if (data_len != 2)
+		return -EPROTO;
+
+	r = nci_hci_send_cmd(ndev, ST_NCI_DEVICE_MGNT_GATE,
+			     ST_NCI_HCI_DM_VDC_VALUE_COMPARISON,
+			     data, data_len, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST_NCI_VENDOR_OUI,
+					HCI_DM_VDC_VALUE_COMPARISON, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+void st_nci_hci_loopback_event_received(struct nci_dev *ndev, u8 event,
+					struct sk_buff *skb)
+{
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	switch (event) {
+	case ST_NCI_EVT_POST_DATA:
+		info->vendor_info.rx_skb = skb;
+	break;
+	default:
+		nfc_err(&ndev->nfc_dev->dev, "Unexpected event on loopback gate\n");
+	}
+	complete(&info->vendor_info.req_completion);
+}
+EXPORT_SYMBOL(st_nci_hci_loopback_event_received);
+
+static int st_nci_hci_loopback(struct nfc_dev *dev, void *data,
+			       size_t data_len)
+{
+	int r;
+	struct sk_buff *msg;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	if (data_len <= 0)
+		return -EPROTO;
+
+	reinit_completion(&info->vendor_info.req_completion);
+	info->vendor_info.rx_skb = NULL;
+
+	r = nci_hci_send_event(ndev, NCI_HCI_LOOPBACK_GATE,
+			       ST_NCI_EVT_POST_DATA, data, data_len);
+	if (r != data_len) {
+		r = -EPROTO;
+		goto exit;
+	}
+
+	wait_for_completion_interruptible(&info->vendor_info.req_completion);
+
+	if (!info->vendor_info.rx_skb ||
+	    info->vendor_info.rx_skb->len != data_len) {
+		r = -EPROTO;
+		goto exit;
+	}
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(ndev->nfc_dev,
+					ST_NCI_VENDOR_OUI,
+					HCI_LOOPBACK,
+					info->vendor_info.rx_skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, info->vendor_info.rx_skb->len,
+		    info->vendor_info.rx_skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+free_skb:
+	kfree_skb(info->vendor_info.rx_skb);
+exit:
+	return r;
+}
+
+static int st_nci_manufacturer_specific(struct nfc_dev *dev, void *data,
+					size_t data_len)
+{
+	struct sk_buff *msg;
+	struct nci_dev *ndev = nfc_get_drvdata(dev);
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST_NCI_VENDOR_OUI,
+					MANUFACTURER_SPECIFIC,
+					sizeof(ndev->manufact_specific_info));
+	if (!msg)
+		return -ENOMEM;
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, sizeof(ndev->manufact_specific_info),
+		    &ndev->manufact_specific_info)) {
+		kfree_skb(msg);
+		return -ENOBUFS;
+	}
+
+	return nfc_vendor_cmd_reply(msg);
+}
+
+static struct nfc_vendor_cmd st_nci_vendor_cmds[] = {
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = FACTORY_MODE,
+		.doit = st_nci_factory_mode,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_CLEAR_ALL_PIPES,
+		.doit = st_nci_hci_clear_all_pipes,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_PUT_DATA,
+		.doit = st_nci_hci_dm_put_data,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_UPDATE_AID,
+		.doit = st_nci_hci_dm_update_aid,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_GET_INFO,
+		.doit = st_nci_hci_dm_get_info,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_GET_DATA,
+		.doit = st_nci_hci_dm_get_data,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_DIRECT_LOAD,
+		.doit = st_nci_hci_dm_direct_load,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_RESET,
+		.doit = st_nci_hci_dm_reset,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_GET_PARAM,
+		.doit = st_nci_hci_get_param,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_FIELD_GENERATOR,
+		.doit = st_nci_hci_dm_field_generator,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_FWUPD_START,
+		.doit = st_nci_hci_dm_fwupd_start,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_FWUPD_END,
+		.doit = st_nci_hci_dm_fwupd_end,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_LOOPBACK,
+		.doit = st_nci_hci_loopback,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_VDC_MEASUREMENT_VALUE,
+		.doit = st_nci_hci_dm_vdc_measurement_value,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = HCI_DM_VDC_VALUE_COMPARISON,
+		.doit = st_nci_hci_dm_vdc_value_comparison,
+	},
+	{
+		.vendor_id = ST_NCI_VENDOR_OUI,
+		.subcmd = MANUFACTURER_SPECIFIC,
+		.doit = st_nci_manufacturer_specific,
+	},
+};
+
+int st_nci_vendor_cmds_init(struct nci_dev *ndev)
+{
+	struct st_nci_info *info = nci_get_drvdata(ndev);
+
+	init_completion(&info->vendor_info.req_completion);
+	return nfc_set_vendor_cmds(ndev->nfc_dev, st_nci_vendor_cmds,
+				   sizeof(st_nci_vendor_cmds));
+}
+EXPORT_SYMBOL(st_nci_vendor_cmds_init);
diff --git a/drivers/nfc/st21nfca/Makefile b/drivers/nfc/st21nfca/Makefile
index 97edab4bbdf8..ded6489c3eeb 100644
--- a/drivers/nfc/st21nfca/Makefile
+++ b/drivers/nfc/st21nfca/Makefile
@@ -2,7 +2,7 @@
 # Makefile for ST21NFCA HCI based NFC driver
 #
 
-st21nfca_hci-objs = st21nfca.o st21nfca_dep.o st21nfca_se.o
+st21nfca_hci-objs = core.o dep.o se.o vendor_cmds.o
 obj-$(CONFIG_NFC_ST21NFCA)     += st21nfca_hci.o
 
 st21nfca_i2c-objs  = i2c.o
diff --git a/drivers/nfc/st21nfca/st21nfca.c b/drivers/nfc/st21nfca/core.c
index 051286562fab..dd8b150fbffa 100644
--- a/drivers/nfc/st21nfca/st21nfca.c
+++ b/drivers/nfc/st21nfca/core.c
@@ -22,8 +22,6 @@
 #include <net/nfc/llc.h>
 
 #include "st21nfca.h"
-#include "st21nfca_dep.h"
-#include "st21nfca_se.h"
 
 #define DRIVER_DESC "HCI NFC driver for ST21NFCA"
 
@@ -87,12 +85,13 @@ static DECLARE_BITMAP(dev_mask, ST21NFCA_NUM_DEVICES);
 
 static struct nfc_hci_gate st21nfca_gates[] = {
 	{NFC_HCI_ADMIN_GATE, NFC_HCI_ADMIN_PIPE},
+	{NFC_HCI_LINK_MGMT_GATE, NFC_HCI_LINK_MGMT_PIPE},
+	{ST21NFCA_DEVICE_MGNT_GATE, ST21NFCA_DEVICE_MGNT_PIPE},
+
 	{NFC_HCI_LOOPBACK_GATE, NFC_HCI_INVALID_PIPE},
 	{NFC_HCI_ID_MGMT_GATE, NFC_HCI_INVALID_PIPE},
-	{NFC_HCI_LINK_MGMT_GATE, NFC_HCI_LINK_MGMT_PIPE},
 	{NFC_HCI_RF_READER_B_GATE, NFC_HCI_INVALID_PIPE},
 	{NFC_HCI_RF_READER_A_GATE, NFC_HCI_INVALID_PIPE},
-	{ST21NFCA_DEVICE_MGNT_GATE, ST21NFCA_DEVICE_MGNT_PIPE},
 	{ST21NFCA_RF_READER_F_GATE, NFC_HCI_INVALID_PIPE},
 	{ST21NFCA_RF_READER_14443_3_A_GATE, NFC_HCI_INVALID_PIPE},
 	{ST21NFCA_RF_READER_ISO15693_GATE, NFC_HCI_INVALID_PIPE},
@@ -163,7 +162,6 @@ static int st21nfca_hci_load_session(struct nfc_hci_dev *hdev)
 		r = nfc_hci_send_cmd(hdev, ST21NFCA_DEVICE_MGNT_GATE,
 					ST21NFCA_DM_GETINFO, pipe_info,
 					sizeof(pipe_info), &skb_pipe_info);
-
 		if (r)
 			continue;
 
@@ -185,43 +183,33 @@ static int st21nfca_hci_load_session(struct nfc_hci_dev *hdev)
 			continue;
 		}
 
-		for (j = 0; (j < ARRAY_SIZE(st21nfca_gates)) &&
+		for (j = 3; (j < ARRAY_SIZE(st21nfca_gates)) &&
 			(st21nfca_gates[j].gate != info->dst_gate_id) ; j++)
 			;
 
 		if (j < ARRAY_SIZE(st21nfca_gates) &&
 			st21nfca_gates[j].gate == info->dst_gate_id &&
 			ST21NFCA_DM_IS_PIPE_OPEN(info->pipe_state)) {
-			st21nfca_gates[j].pipe = pipe_info[2];
+			hdev->init_data.gates[j].pipe = pipe_info[2];
 
 			hdev->gate2pipe[st21nfca_gates[j].gate] =
-							st21nfca_gates[j].pipe;
-			hdev->pipes[st21nfca_gates[j].pipe].gate =
-							st21nfca_gates[j].gate;
-			hdev->pipes[st21nfca_gates[j].pipe].dest_host =
-							info->src_host_id;
+						pipe_info[2];
+			hdev->pipes[pipe_info[2]].gate =
+						st21nfca_gates[j].gate;
+			hdev->pipes[pipe_info[2]].dest_host =
+						info->src_host_id;
 		}
 		kfree_skb(skb_pipe_info);
 	}
 
 	/*
-	 * 3 gates have a well known pipe ID.
-	 * They will never appear in the pipe list
+	 * 3 gates have a well known pipe ID. Only NFC_HCI_LINK_MGMT_GATE
+	 * is not yet open at this stage.
 	 */
-	if (skb_pipe_list->len + 3 < ARRAY_SIZE(st21nfca_gates)) {
-		for (i = skb_pipe_list->len + 3;
-				i < ARRAY_SIZE(st21nfca_gates) - 2; i++) {
-			r = nfc_hci_connect_gate(hdev,
-					NFC_HCI_HOST_CONTROLLER_ID,
-					st21nfca_gates[i].gate,
-					st21nfca_gates[i].pipe);
-			if (r < 0)
-				goto free_list;
-		}
-	}
+	r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID,
+				 NFC_HCI_LINK_MGMT_GATE,
+				 NFC_HCI_LINK_MGMT_PIPE);
 
-	memcpy(hdev->init_data.gates, st21nfca_gates, sizeof(st21nfca_gates));
-free_list:
 	kfree_skb(skb_pipe_list);
 	return r;
 }
@@ -905,6 +893,8 @@ static int st21nfca_admin_event_received(struct nfc_hci_dev *hdev, u8 event,
 			}
 		}
 	break;
+	default:
+		nfc_err(&hdev->ndev->dev, "Unexpected event on admin gate\n");
 	}
 	kfree_skb(skb);
 	return 0;
@@ -933,6 +923,8 @@ static int st21nfca_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe,
 							event, skb);
 	case ST21NFCA_APDU_READER_GATE:
 		return st21nfca_apdu_reader_event_received(hdev, event, skb);
+	case NFC_HCI_LOOPBACK_GATE:
+		return st21nfca_hci_loopback_event_received(hdev, event, skb);
 	default:
 		return 1;
 	}
@@ -993,7 +985,6 @@ int st21nfca_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops,
 	 * persistent info to discriminate 2 identical chips
 	 */
 	dev_num = find_first_zero_bit(dev_mask, ST21NFCA_NUM_DEVICES);
-
 	if (dev_num >= ST21NFCA_NUM_DEVICES)
 		return -ENODEV;
 
@@ -1035,6 +1026,7 @@ int st21nfca_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops,
 	*hdev = info->hdev;
 	st21nfca_dep_init(info->hdev);
 	st21nfca_se_init(info->hdev);
+	st21nfca_vendor_cmds_init(info->hdev);
 
 	return 0;
 
diff --git a/drivers/nfc/st21nfca/st21nfca_dep.c b/drivers/nfc/st21nfca/dep.c
index 8882181d65de..798a32bbac5d 100644
--- a/drivers/nfc/st21nfca/st21nfca_dep.c
+++ b/drivers/nfc/st21nfca/dep.c
@@ -17,7 +17,6 @@
 #include <net/nfc/hci.h>
 
 #include "st21nfca.h"
-#include "st21nfca_dep.h"
 
 #define ST21NFCA_NFCIP1_INITIATOR 0x00
 #define ST21NFCA_NFCIP1_REQ 0xd4
@@ -436,6 +435,7 @@ int st21nfca_dep_event_received(struct nfc_hci_dev *hdev,
 			return r;
 		return 0;
 	default:
+		nfc_err(&hdev->ndev->dev, "Unexpected event on card f gate\n");
 		return 1;
 	}
 	kfree_skb(skb);
diff --git a/drivers/nfc/st21nfca/i2c.c b/drivers/nfc/st21nfca/i2c.c
index a32143951616..a98da33e680a 100644
--- a/drivers/nfc/st21nfca/i2c.c
+++ b/drivers/nfc/st21nfca/i2c.c
@@ -94,6 +94,7 @@ struct st21nfca_i2c_phy {
 	int hard_fault;
 	struct mutex phy_lock;
 };
+
 static u8 len_seq[] = { 16, 24, 12, 29 };
 static u16 wait_tab[] = { 2, 3, 5, 15, 20, 40};
 
diff --git a/drivers/nfc/st21nfca/st21nfca_se.c b/drivers/nfc/st21nfca/se.c
index 3197e9bb66f7..c79d99b24c96 100644
--- a/drivers/nfc/st21nfca/st21nfca_se.c
+++ b/drivers/nfc/st21nfca/se.c
@@ -17,10 +17,9 @@
 #include <net/nfc/hci.h>
 
 #include "st21nfca.h"
-#include "st21nfca_se.h"
 
 #define ST21NFCA_EVT_UICC_ACTIVATE		0x10
-#define ST21NFCA_EVT_UICC_DEACTIVATE	0x13
+#define ST21NFCA_EVT_UICC_DEACTIVATE		0x13
 #define ST21NFCA_EVT_SE_HARD_RESET		0x20
 #define ST21NFCA_EVT_SE_SOFT_RESET		0x11
 #define ST21NFCA_EVT_SE_END_OF_APDU_TRANSFER	0x21
@@ -101,7 +100,7 @@ static int st21nfca_hci_control_se(struct nfc_hci_dev *hdev, u32 se_idx,
 				u8 state)
 {
 	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
-	int r;
+	int r, i;
 	struct sk_buff *sk_host_list;
 	u8 se_event, host_id;
 
@@ -149,7 +148,10 @@ static int st21nfca_hci_control_se(struct nfc_hci_dev *hdev, u32 se_idx,
 	if (r < 0)
 		return r;
 
-	host_id = sk_host_list->data[sk_host_list->len - 1];
+	for (i = 0; i < sk_host_list->len &&
+		sk_host_list->data[i] != se_idx; i++)
+		;
+	host_id = sk_host_list->data[i];
 	kfree_skb(sk_host_list);
 
 	if (state == ST21NFCA_SE_MODE_ON && host_id == se_idx)
@@ -165,6 +167,9 @@ int st21nfca_hci_discover_se(struct nfc_hci_dev *hdev)
 	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
 	int se_count = 0;
 
+	if (test_bit(ST21NFCA_FACTORY_MODE, &hdev->quirks))
+		return 0;
+
 	if (info->se_status->is_uicc_present) {
 		nfc_add_se(hdev->ndev, NFC_HCI_UICC_HOST_ID, NFC_SE_UICC);
 		se_count++;
@@ -189,7 +194,6 @@ int st21nfca_hci_enable_se(struct nfc_hci_dev *hdev, u32 se_idx)
 	 * Same for eSE.
 	 */
 	r = st21nfca_hci_control_se(hdev, se_idx, ST21NFCA_SE_MODE_ON);
-
 	if (r == ST21NFCA_ESE_HOST_ID) {
 		st21nfca_se_get_atr(hdev);
 		r = nfc_hci_send_event(hdev, ST21NFCA_APDU_READER_GATE,
@@ -340,6 +344,7 @@ int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
 		r = nfc_se_transaction(hdev->ndev, host, transaction);
 		break;
 	default:
+		nfc_err(&hdev->ndev->dev, "Unexpected event on connectivity gate\n");
 		return 1;
 	}
 	kfree_skb(skb);
@@ -371,6 +376,9 @@ int st21nfca_apdu_reader_event_received(struct nfc_hci_dev *hdev,
 		mod_timer(&info->se_info.bwi_timer, jiffies +
 				msecs_to_jiffies(info->se_info.wt_timeout));
 		break;
+	default:
+		nfc_err(&hdev->ndev->dev, "Unexpected event on apdu reader gate\n");
+		return 1;
 	}
 
 exit:
diff --git a/drivers/nfc/st21nfca/st21nfca.h b/drivers/nfc/st21nfca/st21nfca.h
index 15a78d330a9f..94ffb0501e87 100644
--- a/drivers/nfc/st21nfca/st21nfca.h
+++ b/drivers/nfc/st21nfca/st21nfca.h
@@ -18,9 +18,8 @@
 #define __LOCAL_ST21NFCA_H_
 
 #include <net/nfc/hci.h>
-
-#include "st21nfca_dep.h"
-#include "st21nfca_se.h"
+#include <linux/skbuff.h>
+#include <linux/workqueue.h>
 
 #define HCI_MODE 0
 
@@ -46,28 +45,115 @@
 #define ST21NFCA_HCI_LLC_MAX_SIZE       (ST21NFCA_HCI_LLC_LEN_CRC + 1 + \
 					ST21NFCA_HCI_LLC_MAX_PAYLOAD)
 
+/* Reader RF commands */
+#define ST21NFCA_WR_XCHG_DATA           0x10
+
+#define ST21NFCA_DEVICE_MGNT_GATE       0x01
+#define ST21NFCA_RF_READER_F_GATE       0x14
+#define ST21NFCA_RF_CARD_F_GATE		0x24
+#define ST21NFCA_APDU_READER_GATE	0xf0
+#define ST21NFCA_CONNECTIVITY_GATE	0x41
+
+/*
+ * ref ISO7816-3 chap 8.1. the initial character TS is followed by a
+ * sequence of at most 32 characters.
+ */
+#define ST21NFCA_ESE_MAX_LENGTH		33
+#define ST21NFCA_ESE_HOST_ID		0xc0
+
 #define DRIVER_DESC "HCI NFC driver for ST21NFCA"
 
-#define ST21NFCA_HCI_MODE 0
+#define ST21NFCA_HCI_MODE		0
+#define ST21NFCA_NUM_DEVICES		256
 
-#define ST21NFCA_NUM_DEVICES 256
+#define ST21NFCA_VENDOR_OUI		0x0080E1 /* STMicroelectronics */
+#define ST21NFCA_FACTORY_MODE		2
 
 struct st21nfca_se_status {
 	bool is_ese_present;
 	bool is_uicc_present;
 };
 
-int st21nfca_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops,
-		       char *llc_name, int phy_headroom, int phy_tailroom,
-		       int phy_payload, struct nfc_hci_dev **hdev,
-			   struct st21nfca_se_status *se_status);
-void st21nfca_hci_remove(struct nfc_hci_dev *hdev);
-
 enum st21nfca_state {
 	ST21NFCA_ST_COLD,
 	ST21NFCA_ST_READY,
 };
 
+/**
+ * enum nfc_vendor_cmds - supported nfc vendor commands
+ *
+ * @FACTORY_MODE: Allow to set the driver into a mode where no secure element
+ *	are activated. It does not consider any NFC_ATTR_VENDOR_DATA.
+ * @HCI_CLEAR_ALL_PIPES: Allow to execute a HCI clear all pipes command.
+ *	It does not consider any NFC_ATTR_VENDOR_DATA.
+ * @HCI_DM_PUT_DATA: Allow to configure specific CLF registry as for example
+ *	RF trimmings or low level drivers configurations (I2C, SPI, SWP).
+ * @HCI_DM_UPDATE_AID: Allow to configure an AID routing into the CLF routing
+ *	table following RF technology, CLF mode or protocol.
+ * @HCI_DM_GET_INFO: Allow to retrieve CLF information.
+ * @HCI_DM_GET_DATA: Allow to retrieve CLF configurable data such as low
+ *	level drivers configurations or RF trimmings.
+ * @HCI_DM_LOAD: Allow to load a firmware into the CLF. A complete
+ *	packet can be more than 8KB.
+ * @HCI_DM_RESET: Allow to run a CLF reset in order to "commit" CLF
+ *	configuration changes without CLF power off.
+ * @HCI_GET_PARAM: Allow to retrieve an HCI CLF parameter (for example the
+ *	white list).
+ * @HCI_DM_FIELD_GENERATOR: Allow to generate different kind of RF
+ *	technology. When using this command to anti-collision is done.
+ * @HCI_LOOPBACK: Allow to echo a command and test the Dh to CLF
+ *	connectivity.
+ */
+enum nfc_vendor_cmds {
+	FACTORY_MODE,
+	HCI_CLEAR_ALL_PIPES,
+	HCI_DM_PUT_DATA,
+	HCI_DM_UPDATE_AID,
+	HCI_DM_GET_INFO,
+	HCI_DM_GET_DATA,
+	HCI_DM_LOAD,
+	HCI_DM_RESET,
+	HCI_GET_PARAM,
+	HCI_DM_FIELD_GENERATOR,
+	HCI_LOOPBACK,
+};
+
+struct st21nfca_vendor_info {
+	struct completion req_completion;
+	struct sk_buff *rx_skb;
+};
+
+struct st21nfca_dep_info {
+	struct sk_buff *tx_pending;
+	struct work_struct tx_work;
+	u8 curr_nfc_dep_pni;
+	u32 idx;
+	u8 to;
+	u8 did;
+	u8 bsi;
+	u8 bri;
+	u8 lri;
+} __packed;
+
+struct st21nfca_se_info {
+	u8 atr[ST21NFCA_ESE_MAX_LENGTH];
+	struct completion req_completion;
+
+	struct timer_list bwi_timer;
+	int wt_timeout; /* in msecs */
+	bool bwi_active;
+
+	struct timer_list se_active_timer;
+	bool se_active;
+	int expected_pipes;
+	int count_pipes;
+
+	bool xch_error;
+
+	se_io_cb_t cb;
+	void *cb_context;
+};
+
 struct st21nfca_hci_info {
 	struct nfc_phy_ops *phy_ops;
 	void *phy_id;
@@ -85,15 +171,41 @@ struct st21nfca_hci_info {
 
 	struct st21nfca_dep_info dep_info;
 	struct st21nfca_se_info se_info;
+	struct st21nfca_vendor_info vendor_info;
 };
 
-/* Reader RF commands */
-#define ST21NFCA_WR_XCHG_DATA           0x10
+int st21nfca_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops,
+		       char *llc_name, int phy_headroom, int phy_tailroom,
+		       int phy_payload, struct nfc_hci_dev **hdev,
+		       struct st21nfca_se_status *se_status);
+void st21nfca_hci_remove(struct nfc_hci_dev *hdev);
 
-#define ST21NFCA_DEVICE_MGNT_GATE       0x01
-#define ST21NFCA_RF_READER_F_GATE       0x14
-#define ST21NFCA_RF_CARD_F_GATE			0x24
-#define ST21NFCA_APDU_READER_GATE		0xf0
-#define ST21NFCA_CONNECTIVITY_GATE		0x41
+int st21nfca_dep_event_received(struct nfc_hci_dev *hdev,
+				u8 event, struct sk_buff *skb);
+int st21nfca_tm_send_dep_res(struct nfc_hci_dev *hdev, struct sk_buff *skb);
+
+int st21nfca_im_send_atr_req(struct nfc_hci_dev *hdev, u8 *gb, size_t gb_len);
+int st21nfca_im_send_dep_req(struct nfc_hci_dev *hdev, struct sk_buff *skb);
+void st21nfca_dep_init(struct nfc_hci_dev *hdev);
+void st21nfca_dep_deinit(struct nfc_hci_dev *hdev);
+
+int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
+					u8 event, struct sk_buff *skb);
+int st21nfca_apdu_reader_event_received(struct nfc_hci_dev *hdev,
+					u8 event, struct sk_buff *skb);
+
+int st21nfca_hci_discover_se(struct nfc_hci_dev *hdev);
+int st21nfca_hci_enable_se(struct nfc_hci_dev *hdev, u32 se_idx);
+int st21nfca_hci_disable_se(struct nfc_hci_dev *hdev, u32 se_idx);
+int st21nfca_hci_se_io(struct nfc_hci_dev *hdev, u32 se_idx,
+		u8 *apdu, size_t apdu_length,
+		se_io_cb_t cb, void *cb_context);
+
+void st21nfca_se_init(struct nfc_hci_dev *hdev);
+void st21nfca_se_deinit(struct nfc_hci_dev *hdev);
+
+int st21nfca_hci_loopback_event_received(struct nfc_hci_dev *ndev, u8 event,
+					 struct sk_buff *skb);
+int st21nfca_vendor_cmds_init(struct nfc_hci_dev *ndev);
 
 #endif /* __LOCAL_ST21NFCA_H_ */
diff --git a/drivers/nfc/st21nfca/st21nfca_dep.h b/drivers/nfc/st21nfca/st21nfca_dep.h
deleted file mode 100644
index baf4664b4fc4..000000000000
--- a/drivers/nfc/st21nfca/st21nfca_dep.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/*
- * Copyright (C) 2014  STMicroelectronics SAS. 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, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __ST21NFCA_DEP_H
-#define __ST21NFCA_DEP_H
-
-#include <linux/skbuff.h>
-#include <linux/workqueue.h>
-
-struct st21nfca_dep_info {
-	struct sk_buff *tx_pending;
-	struct work_struct tx_work;
-	u8 curr_nfc_dep_pni;
-	u32 idx;
-	u8 to;
-	u8 did;
-	u8 bsi;
-	u8 bri;
-	u8 lri;
-} __packed;
-
-int st21nfca_dep_event_received(struct nfc_hci_dev *hdev,
-				u8 event, struct sk_buff *skb);
-int st21nfca_tm_send_dep_res(struct nfc_hci_dev *hdev, struct sk_buff *skb);
-
-int st21nfca_im_send_atr_req(struct nfc_hci_dev *hdev, u8 *gb, size_t gb_len);
-int st21nfca_im_send_dep_req(struct nfc_hci_dev *hdev, struct sk_buff *skb);
-void st21nfca_dep_init(struct nfc_hci_dev *hdev);
-void st21nfca_dep_deinit(struct nfc_hci_dev *hdev);
-#endif /* __ST21NFCA_DEP_H */
diff --git a/drivers/nfc/st21nfca/st21nfca_se.h b/drivers/nfc/st21nfca/st21nfca_se.h
deleted file mode 100644
index b172cfcaeb90..000000000000
--- a/drivers/nfc/st21nfca/st21nfca_se.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * Copyright (C) 2014  STMicroelectronics SAS. 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, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __ST21NFCA_SE_H
-#define __ST21NFCA_SE_H
-
-#include <linux/skbuff.h>
-#include <linux/workqueue.h>
-
-/*
- * ref ISO7816-3 chap 8.1. the initial character TS is followed by a
- * sequence of at most 32 characters.
- */
-#define ST21NFCA_ESE_MAX_LENGTH			33
-#define ST21NFCA_ESE_HOST_ID			0xc0
-
-struct st21nfca_se_info {
-	u8 atr[ST21NFCA_ESE_MAX_LENGTH];
-	struct completion req_completion;
-
-	struct timer_list bwi_timer;
-	int wt_timeout; /* in msecs */
-	bool bwi_active;
-
-	struct timer_list se_active_timer;
-	bool se_active;
-	int expected_pipes;
-	int count_pipes;
-
-	bool xch_error;
-
-	se_io_cb_t cb;
-	void *cb_context;
-};
-
-int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
-					u8 event, struct sk_buff *skb);
-int st21nfca_apdu_reader_event_received(struct nfc_hci_dev *hdev,
-					u8 event, struct sk_buff *skb);
-
-int st21nfca_hci_discover_se(struct nfc_hci_dev *hdev);
-int st21nfca_hci_enable_se(struct nfc_hci_dev *hdev, u32 se_idx);
-int st21nfca_hci_disable_se(struct nfc_hci_dev *hdev, u32 se_idx);
-int st21nfca_hci_se_io(struct nfc_hci_dev *hdev, u32 se_idx,
-		u8 *apdu, size_t apdu_length,
-		se_io_cb_t cb, void *cb_context);
-
-void st21nfca_se_init(struct nfc_hci_dev *hdev);
-void st21nfca_se_deinit(struct nfc_hci_dev *hdev);
-#endif /* __ST21NFCA_SE_H */
diff --git a/drivers/nfc/st21nfca/vendor_cmds.c b/drivers/nfc/st21nfca/vendor_cmds.c
new file mode 100644
index 000000000000..ab765e5478c0
--- /dev/null
+++ b/drivers/nfc/st21nfca/vendor_cmds.c
@@ -0,0 +1,375 @@
+/*
+ * Proprietary commands extension for STMicroelectronics NFC Chip
+ *
+ * Copyright (C) 2014-2015  STMicroelectronics SAS. 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <net/genetlink.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+#include <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+
+#include "st21nfca.h"
+
+#define ST21NFCA_HCI_DM_GETDATA			0x10
+#define ST21NFCA_HCI_DM_PUTDATA			0x11
+#define ST21NFCA_HCI_DM_LOAD			0x12
+#define ST21NFCA_HCI_DM_GETINFO			0x13
+#define ST21NFCA_HCI_DM_UPDATE_AID		0x20
+#define ST21NFCA_HCI_DM_RESET			0x3e
+
+#define ST21NFCA_HCI_DM_FIELD_GENERATOR		0x32
+
+#define ST21NFCA_FACTORY_MODE_ON		1
+#define ST21NFCA_FACTORY_MODE_OFF		0
+
+#define ST21NFCA_EVT_POST_DATA			0x02
+
+struct get_param_data {
+	u8 gate;
+	u8 data;
+} __packed;
+
+static int st21nfca_factory_mode(struct nfc_dev *dev, void *data,
+			       size_t data_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	if (data_len != 1)
+		return -EINVAL;
+
+	pr_debug("factory mode: %x\n", ((u8 *)data)[0]);
+
+	switch (((u8 *)data)[0]) {
+	case ST21NFCA_FACTORY_MODE_ON:
+		test_and_set_bit(ST21NFCA_FACTORY_MODE, &hdev->quirks);
+	break;
+	case ST21NFCA_FACTORY_MODE_OFF:
+		clear_bit(ST21NFCA_FACTORY_MODE, &hdev->quirks);
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int st21nfca_hci_clear_all_pipes(struct nfc_dev *dev, void *data,
+				      size_t data_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	return nfc_hci_disconnect_all_gates(hdev);
+}
+
+static int st21nfca_hci_dm_put_data(struct nfc_dev *dev, void *data,
+				  size_t data_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	return nfc_hci_send_cmd(hdev, ST21NFCA_DEVICE_MGNT_GATE,
+				ST21NFCA_HCI_DM_PUTDATA, data,
+				data_len, NULL);
+}
+
+static int st21nfca_hci_dm_update_aid(struct nfc_dev *dev, void *data,
+				    size_t data_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	return nfc_hci_send_cmd(hdev, ST21NFCA_DEVICE_MGNT_GATE,
+			ST21NFCA_HCI_DM_UPDATE_AID, data, data_len, NULL);
+}
+
+static int st21nfca_hci_dm_get_info(struct nfc_dev *dev, void *data,
+				    size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	r = nfc_hci_send_cmd(hdev,
+			     ST21NFCA_DEVICE_MGNT_GATE,
+			     ST21NFCA_HCI_DM_GETINFO,
+			     data, data_len, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST21NFCA_VENDOR_OUI,
+					     HCI_DM_GET_INFO, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st21nfca_hci_dm_get_data(struct nfc_dev *dev, void *data,
+				    size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	r = nfc_hci_send_cmd(hdev,
+			     ST21NFCA_DEVICE_MGNT_GATE,
+			     ST21NFCA_HCI_DM_GETDATA,
+			     data, data_len, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST21NFCA_VENDOR_OUI,
+					     HCI_DM_GET_DATA, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st21nfca_hci_dm_load(struct nfc_dev *dev, void *data,
+				size_t data_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	return nfc_hci_send_cmd(hdev, ST21NFCA_DEVICE_MGNT_GATE,
+				ST21NFCA_HCI_DM_LOAD, data, data_len, NULL);
+}
+
+static int st21nfca_hci_dm_reset(struct nfc_dev *dev, void *data,
+				 size_t data_len)
+{
+	int r;
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	r = nfc_hci_send_cmd_async(hdev, ST21NFCA_DEVICE_MGNT_GATE,
+			ST21NFCA_HCI_DM_RESET, data, data_len, NULL, NULL);
+	if (r < 0)
+		return r;
+
+	r = nfc_llc_stop(hdev->llc);
+	if (r < 0)
+		return r;
+
+	return nfc_llc_start(hdev->llc);
+}
+
+static int st21nfca_hci_get_param(struct nfc_dev *dev, void *data,
+				  size_t data_len)
+{
+	int r;
+	struct sk_buff *msg, *skb;
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+	struct get_param_data *param = (struct get_param_data *)data;
+
+	if (data_len < sizeof(struct get_param_data))
+		return -EPROTO;
+
+	r = nfc_hci_get_param(hdev, param->gate, param->data, &skb);
+	if (r)
+		goto exit;
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(dev, ST21NFCA_VENDOR_OUI,
+					     HCI_GET_PARAM, skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, skb->len, skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+
+free_skb:
+	kfree_skb(skb);
+exit:
+	return r;
+}
+
+static int st21nfca_hci_dm_field_generator(struct nfc_dev *dev, void *data,
+					   size_t data_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+
+	return nfc_hci_send_cmd(hdev,
+				ST21NFCA_DEVICE_MGNT_GATE,
+				ST21NFCA_HCI_DM_FIELD_GENERATOR,
+				data, data_len, NULL);
+}
+
+int st21nfca_hci_loopback_event_received(struct nfc_hci_dev *hdev, u8 event,
+					 struct sk_buff *skb)
+{
+	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
+
+	switch (event) {
+	case ST21NFCA_EVT_POST_DATA:
+		info->vendor_info.rx_skb = skb;
+	break;
+	default:
+		nfc_err(&hdev->ndev->dev, "Unexpected event on loopback gate\n");
+	}
+	complete(&info->vendor_info.req_completion);
+	return 0;
+}
+EXPORT_SYMBOL(st21nfca_hci_loopback_event_received);
+
+static int st21nfca_hci_loopback(struct nfc_dev *dev, void *data,
+				 size_t data_len)
+{
+	int r;
+	struct sk_buff *msg;
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(dev);
+	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
+
+	if (data_len <= 0)
+		return -EPROTO;
+
+	reinit_completion(&info->vendor_info.req_completion);
+	info->vendor_info.rx_skb = NULL;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_LOOPBACK_GATE,
+			       ST21NFCA_EVT_POST_DATA, data, data_len);
+	if (r < 0) {
+		r = -EPROTO;
+		goto exit;
+	}
+
+	wait_for_completion_interruptible(&info->vendor_info.req_completion);
+	if (!info->vendor_info.rx_skb ||
+	    info->vendor_info.rx_skb->len != data_len) {
+		r = -EPROTO;
+		goto exit;
+	}
+
+	msg = nfc_vendor_cmd_alloc_reply_skb(hdev->ndev,
+					ST21NFCA_VENDOR_OUI,
+					HCI_LOOPBACK,
+					info->vendor_info.rx_skb->len);
+	if (!msg) {
+		r = -ENOMEM;
+		goto free_skb;
+	}
+
+	if (nla_put(msg, NFC_ATTR_VENDOR_DATA, info->vendor_info.rx_skb->len,
+		    info->vendor_info.rx_skb->data)) {
+		kfree_skb(msg);
+		r = -ENOBUFS;
+		goto free_skb;
+	}
+
+	r = nfc_vendor_cmd_reply(msg);
+free_skb:
+	kfree_skb(info->vendor_info.rx_skb);
+exit:
+	return r;
+}
+
+static struct nfc_vendor_cmd st21nfca_vendor_cmds[] = {
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = FACTORY_MODE,
+		.doit = st21nfca_factory_mode,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_CLEAR_ALL_PIPES,
+		.doit = st21nfca_hci_clear_all_pipes,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_PUT_DATA,
+		.doit = st21nfca_hci_dm_put_data,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_UPDATE_AID,
+		.doit = st21nfca_hci_dm_update_aid,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_GET_INFO,
+		.doit = st21nfca_hci_dm_get_info,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_GET_DATA,
+		.doit = st21nfca_hci_dm_get_data,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_LOAD,
+		.doit = st21nfca_hci_dm_load,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_RESET,
+		.doit = st21nfca_hci_dm_reset,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_GET_PARAM,
+		.doit = st21nfca_hci_get_param,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_DM_FIELD_GENERATOR,
+		.doit = st21nfca_hci_dm_field_generator,
+	},
+	{
+		.vendor_id = ST21NFCA_VENDOR_OUI,
+		.subcmd = HCI_LOOPBACK,
+		.doit = st21nfca_hci_loopback,
+	},
+};
+
+int st21nfca_vendor_cmds_init(struct nfc_hci_dev *hdev)
+{
+	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
+
+	init_completion(&info->vendor_info.req_completion);
+	return nfc_set_vendor_cmds(hdev->ndev, st21nfca_vendor_cmds,
+				   sizeof(st21nfca_vendor_cmds));
+}
+EXPORT_SYMBOL(st21nfca_vendor_cmds_init);
diff --git a/drivers/nfc/trf7970a.c b/drivers/nfc/trf7970a.c
index 70b0707fd9a9..123aa981c9d8 100644
--- a/drivers/nfc/trf7970a.c
+++ b/drivers/nfc/trf7970a.c
@@ -2211,6 +2211,12 @@ static const struct dev_pm_ops trf7970a_pm_ops = {
 			trf7970a_pm_runtime_resume, NULL)
 };
 
+static const struct of_device_id trf7970a_of_match[] = {
+	{ .compatible = "ti,trf7970a", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, trf7970a_of_match);
+
 static const struct spi_device_id trf7970a_id_table[] = {
 	{ "trf7970a", 0 },
 	{ }
@@ -2223,6 +2229,7 @@ static struct spi_driver trf7970a_spi_driver = {
 	.id_table	= trf7970a_id_table,
 	.driver		= {
 		.name	= "trf7970a",
+		.of_match_table = of_match_ptr(trf7970a_of_match),
 		.owner	= THIS_MODULE,
 		.pm	= &trf7970a_pm_ops,
 	},
diff --git a/include/linux/platform_data/nfcmrvl.h b/include/linux/platform_data/nfcmrvl.h
index ac91707dabcb..a6f9d633f5be 100644
--- a/include/linux/platform_data/nfcmrvl.h
+++ b/include/linux/platform_data/nfcmrvl.h
@@ -35,6 +35,14 @@ struct nfcmrvl_platform_data {
 	unsigned int flow_control;
 	/* Tell if firmware supports break control for power management */
 	unsigned int break_control;
+
+
+	/*
+	 * I2C specific
+	 */
+
+	unsigned int irq;
+	unsigned int irq_polarity;
 };
 
 #endif /* _NFCMRVL_PTF_H_ */
diff --git a/include/linux/platform_data/st-nci.h b/include/linux/platform_data/st-nci.h
index d9d400a297bd..f6494b347c06 100644
--- a/include/linux/platform_data/st-nci.h
+++ b/include/linux/platform_data/st-nci.h
@@ -24,6 +24,8 @@
 struct st_nci_nfc_platform_data {
 	unsigned int gpio_reset;
 	unsigned int irq_polarity;
+	bool is_ese_present;
+	bool is_uicc_present;
 };
 
 #endif /* _ST_NCI_H_ */
diff --git a/include/net/nfc/nci.h b/include/net/nfc/nci.h
index 75d2e1880059..707e3ab816c2 100644
--- a/include/net/nfc/nci.h
+++ b/include/net/nfc/nci.h
@@ -35,6 +35,7 @@
 #define NCI_MAX_NUM_RF_CONFIGS					10
 #define NCI_MAX_NUM_CONN					10
 #define NCI_MAX_PARAM_LEN					251
+#define NCI_MAX_PAYLOAD_SIZE					255
 #define NCI_MAX_PACKET_SIZE					258
 
 /* NCI Status Codes */
@@ -315,6 +316,8 @@ struct nci_nfcee_mode_set_cmd {
 	__u8	nfcee_mode;
 } __packed;
 
+#define NCI_OP_CORE_GET_CONFIG_CMD	nci_opcode_pack(NCI_GID_CORE, 0x03)
+
 /* ----------------------- */
 /* ---- NCI Responses ---- */
 /* ----------------------- */
@@ -375,6 +378,9 @@ struct nci_nfcee_discover_rsp {
 } __packed;
 
 #define NCI_OP_NFCEE_MODE_SET_RSP nci_opcode_pack(NCI_GID_NFCEE_MGMT, 0x01)
+
+#define NCI_OP_CORE_GET_CONFIG_RSP	nci_opcode_pack(NCI_GID_CORE, 0x03)
+
 /* --------------------------- */
 /* ---- NCI Notifications ---- */
 /* --------------------------- */
@@ -528,4 +534,6 @@ struct nci_nfcee_discover_ntf {
 	struct nci_nfcee_information_tlv	information_tlv;
 } __packed;
 
+#define NCI_OP_CORE_RESET_NTF		nci_opcode_pack(NCI_GID_CORE, 0x00)
+
 #endif /* __NCI_H */
diff --git a/include/net/nfc/nci_core.h b/include/net/nfc/nci_core.h
index d0d0f1e53bb9..57ce24fb0047 100644
--- a/include/net/nfc/nci_core.h
+++ b/include/net/nfc/nci_core.h
@@ -67,7 +67,7 @@ enum nci_state {
 
 struct nci_dev;
 
-struct nci_prop_ops {
+struct nci_driver_ops {
 	__u16 opcode;
 	int (*rsp)(struct nci_dev *dev, struct sk_buff *skb);
 	int (*ntf)(struct nci_dev *dev, struct sk_buff *skb);
@@ -94,8 +94,11 @@ struct nci_ops {
 	void  (*hci_cmd_received)(struct nci_dev *ndev, u8 pipe, u8 cmd,
 				  struct sk_buff *skb);
 
-	struct nci_prop_ops *prop_ops;
+	struct nci_driver_ops *prop_ops;
 	size_t n_prop_ops;
+
+	struct nci_driver_ops *core_ops;
+	size_t n_core_ops;
 };
 
 #define NCI_MAX_SUPPORTED_RF_INTERFACES		4
@@ -125,6 +128,8 @@ struct nci_conn_info {
 
 /* Gates */
 #define NCI_HCI_ADMIN_GATE         0x00
+#define NCI_HCI_LOOPBACK_GATE	   0x04
+#define NCI_HCI_IDENTITY_MGMT_GATE 0x05
 #define NCI_HCI_LINK_MGMT_GATE     0x06
 
 /* Pipes */
@@ -278,10 +283,12 @@ int nci_request(struct nci_dev *ndev,
 			    unsigned long opt),
 		unsigned long opt, __u32 timeout);
 int nci_prop_cmd(struct nci_dev *ndev, __u8 oid, size_t len, __u8 *payload);
+int nci_core_cmd(struct nci_dev *ndev, __u16 opcode, size_t len, __u8 *payload);
 int nci_core_reset(struct nci_dev *ndev);
 int nci_core_init(struct nci_dev *ndev);
 
 int nci_recv_frame(struct nci_dev *ndev, struct sk_buff *skb);
+int nci_send_frame(struct nci_dev *ndev, struct sk_buff *skb);
 int nci_set_config(struct nci_dev *ndev, __u8 id, size_t len, __u8 *val);
 
 int nci_nfcee_discover(struct nci_dev *ndev, u8 action);
@@ -305,6 +312,7 @@ int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx,
 		      const u8 *param, size_t param_len);
 int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx,
 		      struct sk_buff **skb);
+int nci_hci_clear_all_pipes(struct nci_dev *ndev);
 int nci_hci_dev_session_init(struct nci_dev *ndev);
 
 static inline struct sk_buff *nci_skb_alloc(struct nci_dev *ndev,
@@ -348,9 +356,14 @@ int nci_prop_rsp_packet(struct nci_dev *ndev, __u16 opcode,
 			struct sk_buff *skb);
 int nci_prop_ntf_packet(struct nci_dev *ndev, __u16 opcode,
 			struct sk_buff *skb);
+int nci_core_rsp_packet(struct nci_dev *ndev, __u16 opcode,
+			struct sk_buff *skb);
+int nci_core_ntf_packet(struct nci_dev *ndev, __u16 opcode,
+			struct sk_buff *skb);
 void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb);
 int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload);
 int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb);
+int nci_conn_max_data_pkt_payload_size(struct nci_dev *ndev, __u8 conn_id);
 void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
 				__u8 conn_id, int err);
 void nci_hci_data_received_cb(void *context, struct sk_buff *skb, int err);
@@ -365,6 +378,7 @@ void nci_clear_target_list(struct nci_dev *ndev);
 void nci_req_complete(struct nci_dev *ndev, int result);
 struct nci_conn_info *nci_get_conn_info_by_conn_id(struct nci_dev *ndev,
 						   int conn_id);
+int nci_get_conn_info_by_id(struct nci_dev *ndev, u8 id);
 
 /* ----- NCI status code ----- */
 int nci_to_errno(__u8 code);
@@ -380,6 +394,12 @@ struct nci_spi {
 
 	unsigned int		xfer_udelay;	/* microseconds delay between
 						  transactions */
+
+	unsigned int		xfer_speed_hz; /*
+						* SPI clock frequency
+						* 0 => default clock
+						*/
+
 	u8			acknowledge_mode;
 
 	struct completion	req_completion;
diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index 30afc9a6718c..dcfcfc9c00bf 100644
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@@ -68,7 +68,7 @@ struct nfc_ops {
 	int (*activate_target)(struct nfc_dev *dev, struct nfc_target *target,
 			       u32 protocol);
 	void (*deactivate_target)(struct nfc_dev *dev,
-				  struct nfc_target *target);
+				  struct nfc_target *target, u8 mode);
 	int (*im_transceive)(struct nfc_dev *dev, struct nfc_target *target,
 			     struct sk_buff *skb, data_exchange_cb_t cb,
 			     void *cb_context);
diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h
index dd3f75389076..399f39ff8048 100644
--- a/include/uapi/linux/nfc.h
+++ b/include/uapi/linux/nfc.h
@@ -86,6 +86,7 @@
  *	for this event is the application ID (AID).
  * @NFC_CMD_GET_SE: Dump all discovered secure elements from an NFC controller.
  * @NFC_CMD_SE_IO: Send/Receive APDUs to/from the selected secure element.
+ * @NFC_CMD_ACTIVATE_TARGET: Request NFC controller to reactivate target.
  * @NFC_CMD_VENDOR: Vendor specific command, to be implemented directly
  *	from the driver in order to support hardware specific operations.
  */
@@ -156,6 +157,7 @@ enum nfc_commands {
  * @NFC_ATTR_APDU: Secure element APDU
  * @NFC_ATTR_TARGET_ISO15693_DSFID: ISO 15693 Data Storage Format Identifier
  * @NFC_ATTR_TARGET_ISO15693_UID: ISO 15693 Unique Identifier
+ * @NFC_ATTR_SE_PARAMS: Parameters data from an evt_transaction
  * @NFC_ATTR_VENDOR_ID: NFC manufacturer unique ID, typically an OUI
  * @NFC_ATTR_VENDOR_SUBCMD: Vendor specific sub command
  * @NFC_ATTR_VENDOR_DATA: Vendor specific data, to be optionally passed
diff --git a/net/nfc/core.c b/net/nfc/core.c
index cff3f1614ad4..1fe3d3b362c0 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -449,7 +449,7 @@ error:
  * @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 nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx, u8 mode)
 {
 	int rc = 0;
 
@@ -476,7 +476,7 @@ int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx)
 	if (dev->ops->check_presence)
 		del_timer_sync(&dev->check_pres_timer);
 
-	dev->ops->deactivate_target(dev, dev->active_target);
+	dev->ops->deactivate_target(dev, dev->active_target, mode);
 	dev->active_target = NULL;
 
 error:
diff --git a/net/nfc/digital_core.c b/net/nfc/digital_core.c
index 009bcf317101..23c2a118ac9f 100644
--- a/net/nfc/digital_core.c
+++ b/net/nfc/digital_core.c
@@ -631,7 +631,8 @@ static int digital_activate_target(struct nfc_dev *nfc_dev,
 }
 
 static void digital_deactivate_target(struct nfc_dev *nfc_dev,
-				      struct nfc_target *target)
+				      struct nfc_target *target,
+				      u8 mode)
 {
 	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
 
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index 6e061da2258a..2b0f0ac498d2 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -678,7 +678,8 @@ static int hci_activate_target(struct nfc_dev *nfc_dev,
 }
 
 static void hci_deactivate_target(struct nfc_dev *nfc_dev,
-				  struct nfc_target *target)
+				  struct nfc_target *target,
+				  u8 mode)
 {
 }
 
diff --git a/net/nfc/hci/llc.c b/net/nfc/hci/llc.c
index 1b90c0531852..1399a03fa6e6 100644
--- a/net/nfc/hci/llc.c
+++ b/net/nfc/hci/llc.c
@@ -144,11 +144,13 @@ inline int nfc_llc_start(struct nfc_llc *llc)
 {
 	return llc->ops->start(llc);
 }
+EXPORT_SYMBOL(nfc_llc_start);
 
 inline int nfc_llc_stop(struct nfc_llc *llc)
 {
 	return llc->ops->stop(llc);
 }
+EXPORT_SYMBOL(nfc_llc_stop);
 
 inline void nfc_llc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
 {
diff --git a/net/nfc/nci/Kconfig b/net/nfc/nci/Kconfig
index 901c1ddba841..85d4819ab657 100644
--- a/net/nfc/nci/Kconfig
+++ b/net/nfc/nci/Kconfig
@@ -12,7 +12,7 @@ config NFC_NCI
 config NFC_NCI_SPI
 	depends on NFC_NCI && SPI
 	select CRC_CCITT
-	bool "NCI over SPI protocol support"
+	tristate "NCI over SPI protocol support"
 	default n
 	help
 	  NCI (NFC Controller Interface) is a communication protocol between
diff --git a/net/nfc/nci/Makefile b/net/nfc/nci/Makefile
index b4b85b82e988..0ca31d9bf741 100644
--- a/net/nfc/nci/Makefile
+++ b/net/nfc/nci/Makefile
@@ -6,7 +6,8 @@ obj-$(CONFIG_NFC_NCI) += nci.o
 
 nci-objs := core.o data.o lib.o ntf.o rsp.o hci.o
 
-nci-$(CONFIG_NFC_NCI_SPI) += spi.o
+nci_spi-y += spi.o
+obj-$(CONFIG_NFC_NCI_SPI) += nci_spi.o
 
 nci_uart-y += uart.o
 obj-$(CONFIG_NFC_NCI_UART) += nci_uart.o
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index 943889b87a34..10c99a578421 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -64,6 +64,19 @@ struct nci_conn_info *nci_get_conn_info_by_conn_id(struct nci_dev *ndev,
 	return NULL;
 }
 
+int nci_get_conn_info_by_id(struct nci_dev *ndev, u8 id)
+{
+	struct nci_conn_info *conn_info;
+
+	list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
+		if (conn_info->id == id)
+			return conn_info->conn_id;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL(nci_get_conn_info_by_id);
+
 /* ---- NCI requests ---- */
 
 void nci_req_complete(struct nci_dev *ndev, int result)
@@ -325,32 +338,46 @@ static void nci_rf_deactivate_req(struct nci_dev *ndev, unsigned long opt)
 		     sizeof(struct nci_rf_deactivate_cmd), &cmd);
 }
 
-struct nci_prop_cmd_param {
+struct nci_cmd_param {
 	__u16 opcode;
 	size_t len;
 	__u8 *payload;
 };
 
-static void nci_prop_cmd_req(struct nci_dev *ndev, unsigned long opt)
+static void nci_generic_req(struct nci_dev *ndev, unsigned long opt)
 {
-	struct nci_prop_cmd_param *param = (struct nci_prop_cmd_param *)opt;
+	struct nci_cmd_param *param =
+		(struct nci_cmd_param *)opt;
 
 	nci_send_cmd(ndev, param->opcode, param->len, param->payload);
 }
 
 int nci_prop_cmd(struct nci_dev *ndev, __u8 oid, size_t len, __u8 *payload)
 {
-	struct nci_prop_cmd_param param;
+	struct nci_cmd_param param;
 
 	param.opcode = nci_opcode_pack(NCI_GID_PROPRIETARY, oid);
 	param.len = len;
 	param.payload = payload;
 
-	return __nci_request(ndev, nci_prop_cmd_req, (unsigned long)&param,
+	return __nci_request(ndev, nci_generic_req, (unsigned long)&param,
 			     msecs_to_jiffies(NCI_CMD_TIMEOUT));
 }
 EXPORT_SYMBOL(nci_prop_cmd);
 
+int nci_core_cmd(struct nci_dev *ndev, __u16 opcode, size_t len, __u8 *payload)
+{
+	struct nci_cmd_param param;
+
+	param.opcode = opcode;
+	param.len = len;
+	param.payload = payload;
+
+	return __nci_request(ndev, nci_generic_req, (unsigned long)&param,
+			     msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_cmd);
+
 int nci_core_reset(struct nci_dev *ndev)
 {
 	return __nci_request(ndev, nci_reset_req, 0,
@@ -402,9 +429,8 @@ static int nci_open_device(struct nci_dev *ndev)
 				   msecs_to_jiffies(NCI_INIT_TIMEOUT));
 	}
 
-	if (ndev->ops->post_setup) {
+	if (!rc && ndev->ops->post_setup)
 		rc = ndev->ops->post_setup(ndev);
-	}
 
 	if (!rc) {
 		rc = __nci_request(ndev, nci_init_complete_req, 0,
@@ -540,7 +566,7 @@ static void nci_nfcee_discover_req(struct nci_dev *ndev, unsigned long opt)
 
 int nci_nfcee_discover(struct nci_dev *ndev, u8 action)
 {
-	return nci_request(ndev, nci_nfcee_discover_req, action,
+	return __nci_request(ndev, nci_nfcee_discover_req, action,
 				msecs_to_jiffies(NCI_CMD_TIMEOUT));
 }
 EXPORT_SYMBOL(nci_nfcee_discover);
@@ -561,8 +587,9 @@ int nci_nfcee_mode_set(struct nci_dev *ndev, u8 nfcee_id, u8 nfcee_mode)
 	cmd.nfcee_id = nfcee_id;
 	cmd.nfcee_mode = nfcee_mode;
 
-	return nci_request(ndev, nci_nfcee_mode_set_req, (unsigned long)&cmd,
-			   msecs_to_jiffies(NCI_CMD_TIMEOUT));
+	return __nci_request(ndev, nci_nfcee_mode_set_req,
+			     (unsigned long)&cmd,
+			     msecs_to_jiffies(NCI_CMD_TIMEOUT));
 }
 EXPORT_SYMBOL(nci_nfcee_mode_set);
 
@@ -588,12 +615,19 @@ int nci_core_conn_create(struct nci_dev *ndev, u8 destination_type,
 	if (!cmd)
 		return -ENOMEM;
 
+	if (!number_destination_params)
+		return -EINVAL;
+
 	cmd->destination_type = destination_type;
 	cmd->number_destination_params = number_destination_params;
 	memcpy(cmd->params, params, params_len);
 
 	data.cmd = cmd;
-	ndev->cur_id = params->value[DEST_SPEC_PARAMS_ID_INDEX];
+
+	if (params->length > 0)
+		ndev->cur_id = params->value[DEST_SPEC_PARAMS_ID_INDEX];
+	else
+		ndev->cur_id = 0;
 
 	r = __nci_request(ndev, nci_core_conn_create_req,
 			  (unsigned long)&data,
@@ -612,8 +646,8 @@ static void nci_core_conn_close_req(struct nci_dev *ndev, unsigned long opt)
 
 int nci_core_conn_close(struct nci_dev *ndev, u8 conn_id)
 {
-	return nci_request(ndev, nci_core_conn_close_req, conn_id,
-				msecs_to_jiffies(NCI_CMD_TIMEOUT));
+	return __nci_request(ndev, nci_core_conn_close_req, conn_id,
+			     msecs_to_jiffies(NCI_CMD_TIMEOUT));
 }
 EXPORT_SYMBOL(nci_core_conn_close);
 
@@ -801,9 +835,11 @@ static int nci_activate_target(struct nfc_dev *nfc_dev,
 }
 
 static void nci_deactivate_target(struct nfc_dev *nfc_dev,
-				  struct nfc_target *target)
+				  struct nfc_target *target,
+				  __u8 mode)
 {
 	struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+	u8 nci_mode = NCI_DEACTIVATE_TYPE_IDLE_MODE;
 
 	pr_debug("entry\n");
 
@@ -814,9 +850,14 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev,
 
 	ndev->target_active_prot = 0;
 
+	switch (mode) {
+	case NFC_TARGET_MODE_SLEEP:
+		nci_mode = NCI_DEACTIVATE_TYPE_SLEEP_MODE;
+		break;
+	}
+
 	if (atomic_read(&ndev->state) == NCI_POLL_ACTIVE) {
-		nci_request(ndev, nci_rf_deactivate_req,
-			    NCI_DEACTIVATE_TYPE_IDLE_MODE,
+		nci_request(ndev, nci_rf_deactivate_req, nci_mode,
 			    msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
 	}
 }
@@ -850,7 +891,7 @@ static int nci_dep_link_down(struct nfc_dev *nfc_dev)
 	pr_debug("entry\n");
 
 	if (nfc_dev->rf_mode == NFC_RF_INITIATOR) {
-		nci_deactivate_target(nfc_dev, NULL);
+		nci_deactivate_target(nfc_dev, NULL, NCI_DEACTIVATE_TYPE_IDLE_MODE);
 	} else {
 		if (atomic_read(&ndev->state) == NCI_LISTEN_ACTIVE ||
 		    atomic_read(&ndev->state) == NCI_DISCOVERY) {
@@ -1177,7 +1218,7 @@ int nci_recv_frame(struct nci_dev *ndev, struct sk_buff *skb)
 }
 EXPORT_SYMBOL(nci_recv_frame);
 
-static int nci_send_frame(struct nci_dev *ndev, struct sk_buff *skb)
+int nci_send_frame(struct nci_dev *ndev, struct sk_buff *skb)
 {
 	pr_debug("len %d\n", skb->len);
 
@@ -1195,6 +1236,7 @@ static int nci_send_frame(struct nci_dev *ndev, struct sk_buff *skb)
 
 	return ndev->ops->send(ndev, skb);
 }
+EXPORT_SYMBOL(nci_send_frame);
 
 /* Send NCI command */
 int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
@@ -1226,48 +1268,80 @@ int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
 
 	return 0;
 }
+EXPORT_SYMBOL(nci_send_cmd);
 
 /* Proprietary commands API */
-static struct nci_prop_ops *prop_cmd_lookup(struct nci_dev *ndev,
-					    __u16 opcode)
+static struct nci_driver_ops *ops_cmd_lookup(struct nci_driver_ops *ops,
+					     size_t n_ops,
+					     __u16 opcode)
 {
 	size_t i;
-	struct nci_prop_ops *prop_op;
+	struct nci_driver_ops *op;
 
-	if (!ndev->ops->prop_ops || !ndev->ops->n_prop_ops)
+	if (!ops || !n_ops)
 		return NULL;
 
-	for (i = 0; i < ndev->ops->n_prop_ops; i++) {
-		prop_op = &ndev->ops->prop_ops[i];
-		if (prop_op->opcode == opcode)
-			return prop_op;
+	for (i = 0; i < n_ops; i++) {
+		op = &ops[i];
+		if (op->opcode == opcode)
+			return op;
 	}
 
 	return NULL;
 }
 
-int nci_prop_rsp_packet(struct nci_dev *ndev, __u16 rsp_opcode,
-			struct sk_buff *skb)
+static int nci_op_rsp_packet(struct nci_dev *ndev, __u16 rsp_opcode,
+			     struct sk_buff *skb, struct nci_driver_ops *ops,
+			     size_t n_ops)
 {
-	struct nci_prop_ops *prop_op;
+	struct nci_driver_ops *op;
 
-	prop_op = prop_cmd_lookup(ndev, rsp_opcode);
-	if (!prop_op || !prop_op->rsp)
+	op = ops_cmd_lookup(ops, n_ops, rsp_opcode);
+	if (!op || !op->rsp)
 		return -ENOTSUPP;
 
-	return prop_op->rsp(ndev, skb);
+	return op->rsp(ndev, skb);
 }
 
-int nci_prop_ntf_packet(struct nci_dev *ndev, __u16 ntf_opcode,
-			struct sk_buff *skb)
+static int nci_op_ntf_packet(struct nci_dev *ndev, __u16 ntf_opcode,
+			     struct sk_buff *skb, struct nci_driver_ops *ops,
+			     size_t n_ops)
 {
-	struct nci_prop_ops *prop_op;
+	struct nci_driver_ops *op;
 
-	prop_op = prop_cmd_lookup(ndev, ntf_opcode);
-	if (!prop_op || !prop_op->ntf)
+	op = ops_cmd_lookup(ops, n_ops, ntf_opcode);
+	if (!op || !op->ntf)
 		return -ENOTSUPP;
 
-	return prop_op->ntf(ndev, skb);
+	return op->ntf(ndev, skb);
+}
+
+int nci_prop_rsp_packet(struct nci_dev *ndev, __u16 opcode,
+			struct sk_buff *skb)
+{
+	return nci_op_rsp_packet(ndev, opcode, skb, ndev->ops->prop_ops,
+				 ndev->ops->n_prop_ops);
+}
+
+int nci_prop_ntf_packet(struct nci_dev *ndev, __u16 opcode,
+			struct sk_buff *skb)
+{
+	return nci_op_ntf_packet(ndev, opcode, skb, ndev->ops->prop_ops,
+				 ndev->ops->n_prop_ops);
+}
+
+int nci_core_rsp_packet(struct nci_dev *ndev, __u16 opcode,
+			struct sk_buff *skb)
+{
+	return nci_op_rsp_packet(ndev, opcode, skb, ndev->ops->core_ops,
+				  ndev->ops->n_core_ops);
+}
+
+int nci_core_ntf_packet(struct nci_dev *ndev, __u16 opcode,
+			struct sk_buff *skb)
+{
+	return nci_op_ntf_packet(ndev, opcode, skb, ndev->ops->core_ops,
+				 ndev->ops->n_core_ops);
 }
 
 /* ---- NCI TX Data worker thread ---- */
diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c
index 566466d90048..dbd24254412a 100644
--- a/net/nfc/nci/data.c
+++ b/net/nfc/nci/data.c
@@ -90,6 +90,18 @@ static inline void nci_push_data_hdr(struct nci_dev *ndev,
 	nci_pbf_set((__u8 *)hdr, pbf);
 }
 
+int nci_conn_max_data_pkt_payload_size(struct nci_dev *ndev, __u8 conn_id)
+{
+	struct nci_conn_info *conn_info;
+
+	conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+	if (!conn_info)
+		return -EPROTO;
+
+	return conn_info->max_pkt_payload_len;
+}
+EXPORT_SYMBOL(nci_conn_max_data_pkt_payload_size);
+
 static int nci_queue_tx_data_frags(struct nci_dev *ndev,
 				   __u8 conn_id,
 				   struct sk_buff *skb) {
@@ -203,6 +215,7 @@ free_exit:
 exit:
 	return rc;
 }
+EXPORT_SYMBOL(nci_send_data);
 
 /* ----------------- NCI RX Data ----------------- */
 
diff --git a/net/nfc/nci/hci.c b/net/nfc/nci/hci.c
index 609f92283d1b..2aedac15cb59 100644
--- a/net/nfc/nci/hci.c
+++ b/net/nfc/nci/hci.c
@@ -70,6 +70,7 @@ struct nci_hcp_packet {
 #define NCI_HCI_ANY_SET_PARAMETER  0x01
 #define NCI_HCI_ANY_GET_PARAMETER  0x02
 #define NCI_HCI_ANY_CLOSE_PIPE     0x04
+#define NCI_HCI_ADM_CLEAR_ALL_PIPE 0x14
 
 #define NCI_HFP_NO_CHAINING        0x80
 
@@ -78,6 +79,8 @@ struct nci_hcp_packet {
 #define NCI_EVT_HOT_PLUG           0x03
 
 #define NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY       0x01
+#define NCI_HCI_ADM_CREATE_PIPE			0x10
+#define NCI_HCI_ADM_DELETE_PIPE			0x11
 
 /* HCP headers */
 #define NCI_HCI_HCP_PACKET_HEADER_LEN      1
@@ -101,6 +104,20 @@ struct nci_hcp_packet {
 #define NCI_HCP_MSG_GET_CMD(header)  (header & 0x3f)
 #define NCI_HCP_MSG_GET_PIPE(header) (header & 0x7f)
 
+static int nci_hci_result_to_errno(u8 result)
+{
+	switch (result) {
+	case NCI_HCI_ANY_OK:
+		return 0;
+	case NCI_HCI_ANY_E_REG_PAR_UNKNOWN:
+		return -EOPNOTSUPP;
+	case NCI_HCI_ANY_E_TIMEOUT:
+		return -ETIME;
+	default:
+		return -1;
+	}
+}
+
 /* HCI core */
 static void nci_hci_reset_pipes(struct nci_hci_dev *hdev)
 {
@@ -146,18 +163,18 @@ static int nci_hci_send_data(struct nci_dev *ndev, u8 pipe,
 	if (!conn_info)
 		return -EPROTO;
 
-	skb = nci_skb_alloc(ndev, 2 + conn_info->max_pkt_payload_len +
+	i = 0;
+	skb = nci_skb_alloc(ndev, conn_info->max_pkt_payload_len +
 			    NCI_DATA_HDR_SIZE, GFP_KERNEL);
 	if (!skb)
 		return -ENOMEM;
 
-	skb_reserve(skb, 2 + NCI_DATA_HDR_SIZE);
+	skb_reserve(skb, NCI_DATA_HDR_SIZE + 2);
 	*skb_push(skb, 1) = data_type;
 
-	i = 0;
-	len = conn_info->max_pkt_payload_len;
-
 	do {
+		len = conn_info->max_pkt_payload_len;
+
 		/* If last packet add NCI_HFP_NO_CHAINING */
 		if (i + conn_info->max_pkt_payload_len -
 		    (skb->len + 1) >= data_len) {
@@ -177,9 +194,15 @@ static int nci_hci_send_data(struct nci_dev *ndev, u8 pipe,
 			return r;
 
 		i += len;
+
 		if (i < data_len) {
-			skb_trim(skb, 0);
-			skb_pull(skb, len);
+			skb = nci_skb_alloc(ndev,
+					    conn_info->max_pkt_payload_len +
+					    NCI_DATA_HDR_SIZE, GFP_KERNEL);
+			if (!skb)
+				return -ENOMEM;
+
+			skb_reserve(skb, NCI_DATA_HDR_SIZE + 1);
 		}
 	} while (i < data_len);
 
@@ -212,7 +235,8 @@ int nci_hci_send_cmd(struct nci_dev *ndev, u8 gate, u8 cmd,
 		     const u8 *param, size_t param_len,
 		     struct sk_buff **skb)
 {
-	struct nci_conn_info    *conn_info;
+	struct nci_hcp_message *message;
+	struct nci_conn_info   *conn_info;
 	struct nci_data data;
 	int r;
 	u8 pipe = ndev->hci_dev->gate2pipe[gate];
@@ -232,14 +256,34 @@ int nci_hci_send_cmd(struct nci_dev *ndev, u8 gate, u8 cmd,
 
 	r = nci_request(ndev, nci_hci_send_data_req, (unsigned long)&data,
 			msecs_to_jiffies(NCI_DATA_TIMEOUT));
-
-	if (r == NCI_STATUS_OK && skb)
-		*skb = conn_info->rx_skb;
+	if (r == NCI_STATUS_OK) {
+		message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+		r = nci_hci_result_to_errno(
+			NCI_HCP_MSG_GET_CMD(message->header));
+		skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+		if (!r && skb)
+			*skb = conn_info->rx_skb;
+	}
 
 	return r;
 }
 EXPORT_SYMBOL(nci_hci_send_cmd);
 
+int nci_hci_clear_all_pipes(struct nci_dev *ndev)
+{
+	int r;
+
+	r = nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE,
+			     NCI_HCI_ADM_CLEAR_ALL_PIPE, NULL, 0, NULL);
+	if (r < 0)
+		return r;
+
+	nci_hci_reset_pipes(ndev->hci_dev);
+	return r;
+}
+EXPORT_SYMBOL(nci_hci_clear_all_pipes);
+
 static void nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
 				   u8 event, struct sk_buff *skb)
 {
@@ -328,9 +372,6 @@ static void nci_hci_resp_received(struct nci_dev *ndev, u8 pipe,
 	struct nci_conn_info    *conn_info;
 	u8 status = result;
 
-	if (result != NCI_HCI_ANY_OK)
-		goto exit;
-
 	conn_info = ndev->hci_dev->conn_info;
 	if (!conn_info) {
 		status = NCI_STATUS_REJECTED;
@@ -340,7 +381,7 @@ static void nci_hci_resp_received(struct nci_dev *ndev, u8 pipe,
 	conn_info->rx_skb = skb;
 
 exit:
-	nci_req_complete(ndev, status);
+	nci_req_complete(ndev, NCI_STATUS_OK);
 }
 
 /* Receive hcp message for pipe, with type and cmd.
@@ -366,7 +407,7 @@ static void nci_hci_hcp_message_rx(struct nci_dev *ndev, u8 pipe,
 		break;
 	}
 
-	nci_req_complete(ndev, 0);
+	nci_req_complete(ndev, NCI_STATUS_OK);
 }
 
 static void nci_hci_msg_rx_work(struct work_struct *work)
@@ -378,7 +419,7 @@ static void nci_hci_msg_rx_work(struct work_struct *work)
 	u8 pipe, type, instruction;
 
 	while ((skb = skb_dequeue(&hdev->msg_rx_queue)) != NULL) {
-		pipe = skb->data[0];
+		pipe = NCI_HCP_MSG_GET_PIPE(skb->data[0]);
 		skb_pull(skb, NCI_HCI_HCP_PACKET_HEADER_LEN);
 		message = (struct nci_hcp_message *)skb->data;
 		type = NCI_HCP_MSG_GET_TYPE(message->header);
@@ -395,7 +436,7 @@ void nci_hci_data_received_cb(void *context,
 {
 	struct nci_dev *ndev = (struct nci_dev *)context;
 	struct nci_hcp_packet *packet;
-	u8 pipe, type, instruction;
+	u8 pipe, type;
 	struct sk_buff *hcp_skb;
 	struct sk_buff *frag_skb;
 	int msg_len;
@@ -415,7 +456,7 @@ void nci_hci_data_received_cb(void *context,
 
 	/* it's the last fragment. Does it need re-aggregation? */
 	if (skb_queue_len(&ndev->hci_dev->rx_hcp_frags)) {
-		pipe = packet->header & NCI_HCI_FRAGMENT;
+		pipe = NCI_HCP_MSG_GET_PIPE(packet->header);
 		skb_queue_tail(&ndev->hci_dev->rx_hcp_frags, skb);
 
 		msg_len = 0;
@@ -434,7 +475,7 @@ void nci_hci_data_received_cb(void *context,
 		*skb_put(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN) = pipe;
 
 		skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) {
-		       msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN;
+			msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN;
 			memcpy(skb_put(hcp_skb, msg_len), frag_skb->data +
 			       NCI_HCI_HCP_PACKET_HEADER_LEN, msg_len);
 		}
@@ -452,11 +493,10 @@ void nci_hci_data_received_cb(void *context,
 	packet = (struct nci_hcp_packet *)hcp_skb->data;
 	type = NCI_HCP_MSG_GET_TYPE(packet->message.header);
 	if (type == NCI_HCI_HCP_RESPONSE) {
-		pipe = packet->header;
-		instruction = NCI_HCP_MSG_GET_CMD(packet->message.header);
-		skb_pull(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN +
-			 NCI_HCI_HCP_MESSAGE_HEADER_LEN);
-		nci_hci_hcp_message_rx(ndev, pipe, type, instruction, hcp_skb);
+		pipe = NCI_HCP_MSG_GET_PIPE(packet->header);
+		skb_pull(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN);
+		nci_hci_hcp_message_rx(ndev, pipe, type,
+				       NCI_STATUS_OK, hcp_skb);
 	} else {
 		skb_queue_tail(&ndev->hci_dev->msg_rx_queue, hcp_skb);
 		schedule_work(&ndev->hci_dev->msg_rx_work);
@@ -485,9 +525,47 @@ int nci_hci_open_pipe(struct nci_dev *ndev, u8 pipe)
 }
 EXPORT_SYMBOL(nci_hci_open_pipe);
 
+static u8 nci_hci_create_pipe(struct nci_dev *ndev, u8 dest_host,
+			      u8 dest_gate, int *result)
+{
+	u8 pipe;
+	struct sk_buff *skb;
+	struct nci_hci_create_pipe_params params;
+	struct nci_hci_create_pipe_resp *resp;
+
+	pr_debug("gate=%d\n", dest_gate);
+
+	params.src_gate = NCI_HCI_ADMIN_GATE;
+	params.dest_host = dest_host;
+	params.dest_gate = dest_gate;
+
+	*result = nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE,
+				   NCI_HCI_ADM_CREATE_PIPE,
+				   (u8 *)&params, sizeof(params), &skb);
+	if (*result < 0)
+		return NCI_HCI_INVALID_PIPE;
+
+	resp = (struct nci_hci_create_pipe_resp *)skb->data;
+	pipe = resp->pipe;
+	kfree_skb(skb);
+
+	pr_debug("pipe created=%d\n", pipe);
+
+	return pipe;
+}
+
+static int nci_hci_delete_pipe(struct nci_dev *ndev, u8 pipe)
+{
+	pr_debug("\n");
+
+	return nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE,
+				NCI_HCI_ADM_DELETE_PIPE, &pipe, 1, NULL);
+}
+
 int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx,
 		      const u8 *param, size_t param_len)
 {
+	struct nci_hcp_message *message;
 	struct nci_conn_info *conn_info;
 	struct nci_data data;
 	int r;
@@ -520,6 +598,12 @@ int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx,
 	r = nci_request(ndev, nci_hci_send_data_req,
 			(unsigned long)&data,
 			msecs_to_jiffies(NCI_DATA_TIMEOUT));
+	if (r == NCI_STATUS_OK) {
+		message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+		r = nci_hci_result_to_errno(
+			NCI_HCP_MSG_GET_CMD(message->header));
+		skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+	}
 
 	kfree(tmp);
 	return r;
@@ -529,6 +613,7 @@ EXPORT_SYMBOL(nci_hci_set_param);
 int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx,
 		      struct sk_buff **skb)
 {
+	struct nci_hcp_message *message;
 	struct nci_conn_info    *conn_info;
 	struct nci_data data;
 	int r;
@@ -553,8 +638,15 @@ int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx,
 	r = nci_request(ndev, nci_hci_send_data_req, (unsigned long)&data,
 			msecs_to_jiffies(NCI_DATA_TIMEOUT));
 
-	if (r == NCI_STATUS_OK)
-		*skb = conn_info->rx_skb;
+	if (r == NCI_STATUS_OK) {
+		message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+		r = nci_hci_result_to_errno(
+			NCI_HCP_MSG_GET_CMD(message->header));
+		skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+		if (!r && skb)
+			*skb = conn_info->rx_skb;
+	}
 
 	return r;
 }
@@ -563,6 +655,7 @@ EXPORT_SYMBOL(nci_hci_get_param);
 int nci_hci_connect_gate(struct nci_dev *ndev,
 			 u8 dest_host, u8 dest_gate, u8 pipe)
 {
+	bool pipe_created = false;
 	int r;
 
 	if (pipe == NCI_HCI_DO_NOT_OPEN_PIPE)
@@ -581,12 +674,26 @@ int nci_hci_connect_gate(struct nci_dev *ndev,
 	case NCI_HCI_ADMIN_GATE:
 		pipe = NCI_HCI_ADMIN_PIPE;
 	break;
+	default:
+		pipe = nci_hci_create_pipe(ndev, dest_host, dest_gate, &r);
+		if (pipe < 0)
+			return r;
+		pipe_created = true;
+		break;
 	}
 
 open_pipe:
 	r = nci_hci_open_pipe(ndev, pipe);
-	if (r < 0)
+	if (r < 0) {
+		if (pipe_created) {
+			if (nci_hci_delete_pipe(ndev, pipe) < 0) {
+				/* TODO: Cannot clean by deleting pipe...
+				 * -> inconsistent state
+				 */
+			}
+		}
 		return r;
+	}
 
 	ndev->hci_dev->pipes[pipe].gate = dest_gate;
 	ndev->hci_dev->pipes[pipe].host = dest_host;
@@ -653,6 +760,10 @@ int nci_hci_dev_session_init(struct nci_dev *ndev)
 		/* Restore gate<->pipe table from some proprietary location. */
 		r = ndev->ops->hci_load_session(ndev);
 	} else {
+		r = nci_hci_clear_all_pipes(ndev);
+		if (r < 0)
+			goto exit;
+
 		r = nci_hci_dev_connect_gates(ndev,
 					      ndev->hci_dev->init_data.gate_count,
 					      ndev->hci_dev->init_data.gates);
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
index 5d1c2e391c56..2ada2b39e355 100644
--- a/net/nfc/nci/ntf.c
+++ b/net/nfc/nci/ntf.c
@@ -759,7 +759,7 @@ void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb)
 	skb_pull(skb, NCI_CTRL_HDR_SIZE);
 
 	if (nci_opcode_gid(ntf_opcode) == NCI_GID_PROPRIETARY) {
-		if (nci_prop_ntf_packet(ndev, ntf_opcode, skb)) {
+		if (nci_prop_ntf_packet(ndev, ntf_opcode, skb) == -ENOTSUPP) {
 			pr_err("unsupported ntf opcode 0x%x\n",
 			       ntf_opcode);
 		}
@@ -805,6 +805,7 @@ void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb)
 		break;
 	}
 
+	nci_core_ntf_packet(ndev, ntf_opcode, skb);
 end:
 	kfree_skb(skb);
 }
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
index 408bd8f857ab..9b6eb913d801 100644
--- a/net/nfc/nci/rsp.c
+++ b/net/nfc/nci/rsp.c
@@ -355,6 +355,7 @@ void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
 		break;
 	}
 
+	nci_core_rsp_packet(ndev, rsp_opcode, skb);
 end:
 	kfree_skb(skb);
 
diff --git a/net/nfc/nci/spi.c b/net/nfc/nci/spi.c
index ec250e77763a..d904cd2f1442 100644
--- a/net/nfc/nci/spi.c
+++ b/net/nfc/nci/spi.c
@@ -18,6 +18,8 @@
 
 #define pr_fmt(fmt) "nci_spi: %s: " fmt, __func__
 
+#include <linux/module.h>
+
 #include <linux/export.h>
 #include <linux/spi/spi.h>
 #include <linux/crc-ccitt.h>
@@ -56,6 +58,7 @@ static int __nci_spi_send(struct nci_spi *nspi, struct sk_buff *skb,
 	}
 	t.cs_change = cs_change;
 	t.delay_usecs = nspi->xfer_udelay;
+	t.speed_hz = nspi->xfer_speed_hz;
 
 	spi_message_init(&m);
 	spi_message_add_tail(&t, &m);
@@ -142,7 +145,8 @@ struct nci_spi *nci_spi_allocate_spi(struct spi_device *spi,
 
 	nspi->acknowledge_mode = acknowledge_mode;
 	nspi->xfer_udelay = delay;
-
+	/* Use controller max SPI speed by default */
+	nspi->xfer_speed_hz = 0;
 	nspi->spi = spi;
 	nspi->ndev = ndev;
 	init_completion(&nspi->req_completion);
@@ -195,12 +199,14 @@ static struct sk_buff *__nci_spi_read(struct nci_spi *nspi)
 	tx.tx_buf = req;
 	tx.len = 2;
 	tx.cs_change = 0;
+	tx.speed_hz = nspi->xfer_speed_hz;
 	spi_message_add_tail(&tx, &m);
 
 	memset(&rx, 0, sizeof(struct spi_transfer));
 	rx.rx_buf = resp_hdr;
 	rx.len = 2;
 	rx.cs_change = 1;
+	rx.speed_hz = nspi->xfer_speed_hz;
 	spi_message_add_tail(&rx, &m);
 
 	ret = spi_sync(nspi->spi, &m);
@@ -224,6 +230,7 @@ static struct sk_buff *__nci_spi_read(struct nci_spi *nspi)
 	rx.len = rx_len;
 	rx.cs_change = 0;
 	rx.delay_usecs = nspi->xfer_udelay;
+	rx.speed_hz = nspi->xfer_speed_hz;
 	spi_message_add_tail(&rx, &m);
 
 	ret = spi_sync(nspi->spi, &m);
@@ -320,3 +327,5 @@ done:
 	return skb;
 }
 EXPORT_SYMBOL_GPL(nci_spi_read);
+
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 853172c27f68..f58c1fba1026 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -885,7 +885,7 @@ static int nfc_genl_activate_target(struct sk_buff *skb, struct genl_info *info)
 	target_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
 	protocol = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
 
-	nfc_deactivate_target(dev, target_idx);
+	nfc_deactivate_target(dev, target_idx, NFC_TARGET_MODE_SLEEP);
 	rc = nfc_activate_target(dev, target_idx, protocol);
 
 	nfc_put_device(dev);
@@ -1109,10 +1109,8 @@ static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info)
 	idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
 
 	dev = nfc_get_device(idx);
-	if (!dev) {
-		rc = -ENODEV;
-		goto exit;
-	}
+	if (!dev)
+		return -ENODEV;
 
 	device_lock(&dev->dev);
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index 5c93e8412a26..c20b784ad720 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -25,6 +25,9 @@
 #include <net/nfc/nfc.h>
 #include <net/sock.h>
 
+#define NFC_TARGET_MODE_IDLE 0
+#define NFC_TARGET_MODE_SLEEP 1
+
 struct nfc_protocol {
 	int id;
 	struct proto *proto;
@@ -147,7 +150,7 @@ 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_deactivate_target(struct nfc_dev *dev, u32 target_idx, u8 mode);
 
 int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
 		      data_exchange_cb_t cb, void *cb_context);
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index e9a91488fe3d..e386e6c90b17 100644
--- a/net/nfc/rawsock.c
+++ b/net/nfc/rawsock.c
@@ -321,7 +321,8 @@ static void rawsock_destruct(struct sock *sk)
 
 	if (sk->sk_state == TCP_ESTABLISHED) {
 		nfc_deactivate_target(nfc_rawsock(sk)->dev,
-				      nfc_rawsock(sk)->target_idx);
+				      nfc_rawsock(sk)->target_idx,
+				      NFC_TARGET_MODE_IDLE);
 		nfc_put_device(nfc_rawsock(sk)->dev);
 	}