Fixed MTP to work with TWRP

net/nfc/Kconfig

@@ -0,0 +1,34 @@
+#
+# NFC sybsystem configuration
+#
+
+menuconfig NFC
+	depends on NET
+	depends on RFKILL || !RFKILL
+	tristate "NFC subsystem support"
+	default n
+	help
+	  Say Y here if you want to build support for NFC (Near field
+	  communication) devices.
+
+	  To compile this support as a module, choose M here: the module will
+	  be called nfc.
+
+config NFC_DIGITAL
+	depends on NFC
+	select CRC_CCITT
+	select CRC_ITU_T
+	tristate "NFC Digital Protocol stack support"
+	default n
+	help
+	  Say Y if you want to build NFC digital protocol stack support.
+	  This is needed by NFC chipsets whose firmware only implement
+	  the NFC analog layer.
+
+	  To compile this support as a module, choose M here: the module will
+	  be called nfc_digital.
+
+source "net/nfc/nci/Kconfig"
+source "net/nfc/hci/Kconfig"
+
+source "drivers/nfc/Kconfig"

net/nfc/Makefile

@@ -0,0 +1,13 @@
+#
+# Makefile for the Linux NFC subsystem.
+#
+
+obj-$(CONFIG_NFC) += nfc.o
+obj-$(CONFIG_NFC_NCI) += nci/
+obj-$(CONFIG_NFC_HCI) += hci/
+obj-$(CONFIG_NFC_DIGITAL) += nfc_digital.o
+
+nfc-objs := core.o netlink.o af_nfc.o rawsock.o llcp_core.o llcp_commands.o \
+		llcp_sock.o
+
+nfc_digital-objs := digital_core.o digital_technology.o digital_dep.o

net/nfc/af_nfc.c

@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/nfc.h>
+#include <linux/module.h>
+
+#include "nfc.h"
+
+static DEFINE_RWLOCK(proto_tab_lock);
+static const struct nfc_protocol *proto_tab[NFC_SOCKPROTO_MAX];
+
+static int nfc_sock_create(struct net *net, struct socket *sock, int proto,
+			   int kern)
+{
+	int rc = -EPROTONOSUPPORT;
+
+	if (net != &init_net)
+		return -EAFNOSUPPORT;
+
+	if (proto < 0 || proto >= NFC_SOCKPROTO_MAX)
+		return -EINVAL;
+
+	read_lock(&proto_tab_lock);
+	if (proto_tab[proto] &&	try_module_get(proto_tab[proto]->owner)) {
+		rc = proto_tab[proto]->create(net, sock, proto_tab[proto]);
+		module_put(proto_tab[proto]->owner);
+	}
+	read_unlock(&proto_tab_lock);
+
+	return rc;
+}
+
+static struct net_proto_family nfc_sock_family_ops = {
+	.owner  = THIS_MODULE,
+	.family = PF_NFC,
+	.create = nfc_sock_create,
+};
+
+int nfc_proto_register(const struct nfc_protocol *nfc_proto)
+{
+	int rc;
+
+	if (nfc_proto->id < 0 || nfc_proto->id >= NFC_SOCKPROTO_MAX)
+		return -EINVAL;
+
+	rc = proto_register(nfc_proto->proto, 0);
+	if (rc)
+		return rc;
+
+	write_lock(&proto_tab_lock);
+	if (proto_tab[nfc_proto->id])
+		rc = -EBUSY;
+	else
+		proto_tab[nfc_proto->id] = nfc_proto;
+	write_unlock(&proto_tab_lock);
+
+	return rc;
+}
+EXPORT_SYMBOL(nfc_proto_register);
+
+void nfc_proto_unregister(const struct nfc_protocol *nfc_proto)
+{
+	write_lock(&proto_tab_lock);
+	proto_tab[nfc_proto->id] = NULL;
+	write_unlock(&proto_tab_lock);
+
+	proto_unregister(nfc_proto->proto);
+}
+EXPORT_SYMBOL(nfc_proto_unregister);
+
+int __init af_nfc_init(void)
+{
+	return sock_register(&nfc_sock_family_ops);
+}
+
+void af_nfc_exit(void)
+{
+	sock_unregister(PF_NFC);
+}

net/nfc/digital.h

@@ -0,0 +1,180 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * 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 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 __DIGITAL_H
+#define __DIGITAL_H
+
+#include <net/nfc/nfc.h>
+#include <net/nfc/digital.h>
+
+#include <linux/crc-ccitt.h>
+#include <linux/crc-itu-t.h>
+
+#define PROTOCOL_ERR(req) pr_err("%d: NFC Digital Protocol error: %s\n", \
+				 __LINE__, req)
+
+#define DIGITAL_CMD_IN_SEND        0
+#define DIGITAL_CMD_TG_SEND        1
+#define DIGITAL_CMD_TG_LISTEN      2
+#define DIGITAL_CMD_TG_LISTEN_MDAA 3
+#define DIGITAL_CMD_TG_LISTEN_MD   4
+
+#define DIGITAL_MAX_HEADER_LEN 7
+#define DIGITAL_CRC_LEN        2
+
+#define DIGITAL_SENSF_NFCID2_NFC_DEP_B1 0x01
+#define DIGITAL_SENSF_NFCID2_NFC_DEP_B2 0xFE
+
+#define DIGITAL_SENS_RES_NFC_DEP 0x0100
+#define DIGITAL_SEL_RES_NFC_DEP  0x40
+#define DIGITAL_SENSF_FELICA_SC  0xFFFF
+
+#define DIGITAL_DRV_CAPS_IN_CRC(ddev) \
+	((ddev)->driver_capabilities & NFC_DIGITAL_DRV_CAPS_IN_CRC)
+#define DIGITAL_DRV_CAPS_TG_CRC(ddev) \
+	((ddev)->driver_capabilities & NFC_DIGITAL_DRV_CAPS_TG_CRC)
+
+struct digital_data_exch {
+	data_exchange_cb_t cb;
+	void *cb_context;
+};
+
+struct sk_buff *digital_skb_alloc(struct nfc_digital_dev *ddev,
+				  unsigned int len);
+
+int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
+		     struct sk_buff *skb, struct digital_tg_mdaa_params *params,
+		     u16 timeout, nfc_digital_cmd_complete_t cmd_cb,
+		     void *cb_context);
+
+int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param);
+static inline int digital_in_send_cmd(struct nfc_digital_dev *ddev,
+				      struct sk_buff *skb, u16 timeout,
+				      nfc_digital_cmd_complete_t cmd_cb,
+				      void *cb_context)
+{
+	return digital_send_cmd(ddev, DIGITAL_CMD_IN_SEND, skb, NULL, timeout,
+				cmd_cb, cb_context);
+}
+
+void digital_poll_next_tech(struct nfc_digital_dev *ddev);
+
+int digital_in_send_sens_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_in_send_sensb_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_in_send_sensf_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_in_send_iso15693_inv_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+
+int digital_in_iso_dep_pull_sod(struct nfc_digital_dev *ddev,
+				struct sk_buff *skb);
+int digital_in_iso_dep_push_sod(struct nfc_digital_dev *ddev,
+				struct sk_buff *skb);
+
+int digital_target_found(struct nfc_digital_dev *ddev,
+			 struct nfc_target *target, u8 protocol);
+
+int digital_in_recv_mifare_res(struct sk_buff *resp);
+
+int digital_in_send_atr_req(struct nfc_digital_dev *ddev,
+			    struct nfc_target *target, __u8 comm_mode, __u8 *gb,
+			    size_t gb_len);
+int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
+			    struct nfc_target *target, struct sk_buff *skb,
+			    struct digital_data_exch *data_exch);
+
+int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param);
+static inline int digital_tg_send_cmd(struct nfc_digital_dev *ddev,
+			struct sk_buff *skb, u16 timeout,
+			nfc_digital_cmd_complete_t cmd_cb, void *cb_context)
+{
+	return digital_send_cmd(ddev, DIGITAL_CMD_TG_SEND, skb, NULL, timeout,
+				cmd_cb, cb_context);
+}
+
+void digital_tg_recv_sens_req(struct nfc_digital_dev *ddev, void *arg,
+			      struct sk_buff *resp);
+
+void digital_tg_recv_sensf_req(struct nfc_digital_dev *ddev, void *arg,
+			       struct sk_buff *resp);
+
+static inline int digital_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
+				    nfc_digital_cmd_complete_t cb, void *arg)
+{
+	return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN, NULL, NULL,
+				timeout, cb, arg);
+}
+
+void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg,
+			     struct sk_buff *resp);
+
+int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb);
+
+int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech);
+void digital_tg_recv_md_req(struct nfc_digital_dev *ddev, void *arg,
+			    struct sk_buff *resp);
+
+typedef u16 (*crc_func_t)(u16, const u8 *, size_t);
+
+#define CRC_A_INIT 0x6363
+#define CRC_B_INIT 0xFFFF
+#define CRC_F_INIT 0x0000
+
+void digital_skb_add_crc(struct sk_buff *skb, crc_func_t crc_func, u16 init,
+			 u8 bitwise_inv, u8 msb_first);
+
+static inline void digital_skb_add_crc_a(struct sk_buff *skb)
+{
+	digital_skb_add_crc(skb, crc_ccitt, CRC_A_INIT, 0, 0);
+}
+
+static inline void digital_skb_add_crc_b(struct sk_buff *skb)
+{
+	digital_skb_add_crc(skb, crc_ccitt, CRC_B_INIT, 1, 0);
+}
+
+static inline void digital_skb_add_crc_f(struct sk_buff *skb)
+{
+	digital_skb_add_crc(skb, crc_itu_t, CRC_F_INIT, 0, 1);
+}
+
+static inline void digital_skb_add_crc_none(struct sk_buff *skb)
+{
+	return;
+}
+
+int digital_skb_check_crc(struct sk_buff *skb, crc_func_t crc_func,
+			  u16 crc_init, u8 bitwise_inv, u8 msb_first);
+
+static inline int digital_skb_check_crc_a(struct sk_buff *skb)
+{
+	return digital_skb_check_crc(skb, crc_ccitt, CRC_A_INIT, 0, 0);
+}
+
+static inline int digital_skb_check_crc_b(struct sk_buff *skb)
+{
+	return digital_skb_check_crc(skb, crc_ccitt, CRC_B_INIT, 1, 0);
+}
+
+static inline int digital_skb_check_crc_f(struct sk_buff *skb)
+{
+	return digital_skb_check_crc(skb, crc_itu_t, CRC_F_INIT, 0, 1);
+}
+
+static inline int digital_skb_check_crc_none(struct sk_buff *skb)
+{
+	return 0;
+}
+
+#endif /* __DIGITAL_H */

net/nfc/digital_core.c

@@ -0,0 +1,845 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * 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 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.
+ *
+ */
+
+#define pr_fmt(fmt) "digital: %s: " fmt, __func__
+
+#include <linux/module.h>
+
+#include "digital.h"
+
+#define DIGITAL_PROTO_NFCA_RF_TECH \
+	(NFC_PROTO_JEWEL_MASK | NFC_PROTO_MIFARE_MASK | NFC_PROTO_NFC_DEP_MASK)
+
+#define DIGITAL_PROTO_NFCB_RF_TECH	NFC_PROTO_ISO14443_B_MASK
+
+#define DIGITAL_PROTO_NFCF_RF_TECH \
+	(NFC_PROTO_FELICA_MASK | NFC_PROTO_NFC_DEP_MASK)
+
+#define DIGITAL_PROTO_ISO15693_RF_TECH	NFC_PROTO_ISO15693_MASK
+
+struct digital_cmd {
+	struct list_head queue;
+
+	u8 type;
+	u8 pending;
+
+	u16 timeout;
+	struct sk_buff *req;
+	struct sk_buff *resp;
+	struct digital_tg_mdaa_params *mdaa_params;
+
+	nfc_digital_cmd_complete_t cmd_cb;
+	void *cb_context;
+};
+
+struct sk_buff *digital_skb_alloc(struct nfc_digital_dev *ddev,
+				  unsigned int len)
+{
+	struct sk_buff *skb;
+
+	skb = alloc_skb(len + ddev->tx_headroom + ddev->tx_tailroom,
+			GFP_KERNEL);
+	if (skb)
+		skb_reserve(skb, ddev->tx_headroom);
+
+	return skb;
+}
+
+void digital_skb_add_crc(struct sk_buff *skb, crc_func_t crc_func, u16 init,
+			 u8 bitwise_inv, u8 msb_first)
+{
+	u16 crc;
+
+	crc = crc_func(init, skb->data, skb->len);
+
+	if (bitwise_inv)
+		crc = ~crc;
+
+	if (msb_first)
+		crc = __fswab16(crc);
+
+	*skb_put(skb, 1) = crc & 0xFF;
+	*skb_put(skb, 1) = (crc >> 8) & 0xFF;
+}
+
+int digital_skb_check_crc(struct sk_buff *skb, crc_func_t crc_func,
+			  u16 crc_init, u8 bitwise_inv, u8 msb_first)
+{
+	int rc;
+	u16 crc;
+
+	if (skb->len <= 2)
+		return -EIO;
+
+	crc = crc_func(crc_init, skb->data, skb->len - 2);
+
+	if (bitwise_inv)
+		crc = ~crc;
+
+	if (msb_first)
+		crc = __swab16(crc);
+
+	rc = (skb->data[skb->len - 2] - (crc & 0xFF)) +
+	     (skb->data[skb->len - 1] - ((crc >> 8) & 0xFF));
+
+	if (rc)
+		return -EIO;
+
+	skb_trim(skb, skb->len - 2);
+
+	return 0;
+}
+
+static inline void digital_switch_rf(struct nfc_digital_dev *ddev, bool on)
+{
+	ddev->ops->switch_rf(ddev, on);
+}
+
+static inline void digital_abort_cmd(struct nfc_digital_dev *ddev)
+{
+	ddev->ops->abort_cmd(ddev);
+}
+
+static void digital_wq_cmd_complete(struct work_struct *work)
+{
+	struct digital_cmd *cmd;
+	struct nfc_digital_dev *ddev = container_of(work,
+						    struct nfc_digital_dev,
+						    cmd_complete_work);
+
+	mutex_lock(&ddev->cmd_lock);
+
+	cmd = list_first_entry_or_null(&ddev->cmd_queue, struct digital_cmd,
+				       queue);
+	if (!cmd) {
+		mutex_unlock(&ddev->cmd_lock);
+		return;
+	}
+
+	list_del(&cmd->queue);
+
+	mutex_unlock(&ddev->cmd_lock);
+
+	if (!IS_ERR(cmd->resp))
+		print_hex_dump_debug("DIGITAL RX: ", DUMP_PREFIX_NONE, 16, 1,
+				     cmd->resp->data, cmd->resp->len, false);
+
+	cmd->cmd_cb(ddev, cmd->cb_context, cmd->resp);
+
+	kfree(cmd->mdaa_params);
+	kfree(cmd);
+
+	schedule_work(&ddev->cmd_work);
+}
+
+static void digital_send_cmd_complete(struct nfc_digital_dev *ddev,
+				      void *arg, struct sk_buff *resp)
+{
+	struct digital_cmd *cmd = arg;
+
+	cmd->resp = resp;
+
+	schedule_work(&ddev->cmd_complete_work);
+}
+
+static void digital_wq_cmd(struct work_struct *work)
+{
+	int rc;
+	struct digital_cmd *cmd;
+	struct digital_tg_mdaa_params *params;
+	struct nfc_digital_dev *ddev = container_of(work,
+						    struct nfc_digital_dev,
+						    cmd_work);
+
+	mutex_lock(&ddev->cmd_lock);
+
+	cmd = list_first_entry_or_null(&ddev->cmd_queue, struct digital_cmd,
+				       queue);
+	if (!cmd || cmd->pending) {
+		mutex_unlock(&ddev->cmd_lock);
+		return;
+	}
+
+	mutex_unlock(&ddev->cmd_lock);
+
+	if (cmd->req)
+		print_hex_dump_debug("DIGITAL TX: ", DUMP_PREFIX_NONE, 16, 1,
+				     cmd->req->data, cmd->req->len, false);
+
+	switch (cmd->type) {
+	case DIGITAL_CMD_IN_SEND:
+		rc = ddev->ops->in_send_cmd(ddev, cmd->req, cmd->timeout,
+					    digital_send_cmd_complete, cmd);
+		break;
+
+	case DIGITAL_CMD_TG_SEND:
+		rc = ddev->ops->tg_send_cmd(ddev, cmd->req, cmd->timeout,
+					    digital_send_cmd_complete, cmd);
+		break;
+
+	case DIGITAL_CMD_TG_LISTEN:
+		rc = ddev->ops->tg_listen(ddev, cmd->timeout,
+					  digital_send_cmd_complete, cmd);
+		break;
+
+	case DIGITAL_CMD_TG_LISTEN_MDAA:
+		params = cmd->mdaa_params;
+
+		rc = ddev->ops->tg_listen_mdaa(ddev, params, cmd->timeout,
+					       digital_send_cmd_complete, cmd);
+		break;
+
+	case DIGITAL_CMD_TG_LISTEN_MD:
+		rc = ddev->ops->tg_listen_md(ddev, cmd->timeout,
+					       digital_send_cmd_complete, cmd);
+		break;
+
+	default:
+		pr_err("Unknown cmd type %d\n", cmd->type);
+		return;
+	}
+
+	if (!rc)
+		return;
+
+	pr_err("in_send_command returned err %d\n", rc);
+
+	mutex_lock(&ddev->cmd_lock);
+	list_del(&cmd->queue);
+	mutex_unlock(&ddev->cmd_lock);
+
+	kfree_skb(cmd->req);
+	kfree(cmd->mdaa_params);
+	kfree(cmd);
+
+	schedule_work(&ddev->cmd_work);
+}
+
+int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
+		     struct sk_buff *skb, struct digital_tg_mdaa_params *params,
+		     u16 timeout, nfc_digital_cmd_complete_t cmd_cb,
+		     void *cb_context)
+{
+	struct digital_cmd *cmd;
+
+	cmd = kzalloc(sizeof(struct digital_cmd), GFP_KERNEL);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->type = cmd_type;
+	cmd->timeout = timeout;
+	cmd->req = skb;
+	cmd->mdaa_params = params;
+	cmd->cmd_cb = cmd_cb;
+	cmd->cb_context = cb_context;
+	INIT_LIST_HEAD(&cmd->queue);
+
+	mutex_lock(&ddev->cmd_lock);
+	list_add_tail(&cmd->queue, &ddev->cmd_queue);
+	mutex_unlock(&ddev->cmd_lock);
+
+	schedule_work(&ddev->cmd_work);
+
+	return 0;
+}
+
+int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param)
+{
+	int rc;
+
+	rc = ddev->ops->in_configure_hw(ddev, type, param);
+	if (rc)
+		pr_err("in_configure_hw failed: %d\n", rc);
+
+	return rc;
+}
+
+int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param)
+{
+	int rc;
+
+	rc = ddev->ops->tg_configure_hw(ddev, type, param);
+	if (rc)
+		pr_err("tg_configure_hw failed: %d\n", rc);
+
+	return rc;
+}
+
+static int digital_tg_listen_mdaa(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+	struct digital_tg_mdaa_params *params;
+
+	params = kzalloc(sizeof(struct digital_tg_mdaa_params), GFP_KERNEL);
+	if (!params)
+		return -ENOMEM;
+
+	params->sens_res = DIGITAL_SENS_RES_NFC_DEP;
+	get_random_bytes(params->nfcid1, sizeof(params->nfcid1));
+	params->sel_res = DIGITAL_SEL_RES_NFC_DEP;
+
+	params->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
+	params->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
+	get_random_bytes(params->nfcid2 + 2, NFC_NFCID2_MAXSIZE - 2);
+	params->sc = DIGITAL_SENSF_FELICA_SC;
+
+	return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MDAA, NULL, params,
+				500, digital_tg_recv_atr_req, NULL);
+}
+
+static int digital_tg_listen_md(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+	return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MD, NULL, NULL, 500,
+				digital_tg_recv_md_req, NULL);
+}
+
+int digital_target_found(struct nfc_digital_dev *ddev,
+			 struct nfc_target *target, u8 protocol)
+{
+	int rc;
+	u8 framing;
+	u8 rf_tech;
+	u8 poll_tech_count;
+	int (*check_crc)(struct sk_buff *skb);
+	void (*add_crc)(struct sk_buff *skb);
+
+	rf_tech = ddev->poll_techs[ddev->poll_tech_index].rf_tech;
+
+	switch (protocol) {
+	case NFC_PROTO_JEWEL:
+		framing = NFC_DIGITAL_FRAMING_NFCA_T1T;
+		check_crc = digital_skb_check_crc_b;
+		add_crc = digital_skb_add_crc_b;
+		break;
+
+	case NFC_PROTO_MIFARE:
+		framing = NFC_DIGITAL_FRAMING_NFCA_T2T;
+		check_crc = digital_skb_check_crc_a;
+		add_crc = digital_skb_add_crc_a;
+		break;
+
+	case NFC_PROTO_FELICA:
+		framing = NFC_DIGITAL_FRAMING_NFCF_T3T;
+		check_crc = digital_skb_check_crc_f;
+		add_crc = digital_skb_add_crc_f;
+		break;
+
+	case NFC_PROTO_NFC_DEP:
+		if (rf_tech == NFC_DIGITAL_RF_TECH_106A) {
+			framing = NFC_DIGITAL_FRAMING_NFCA_NFC_DEP;
+			check_crc = digital_skb_check_crc_a;
+			add_crc = digital_skb_add_crc_a;
+		} else {
+			framing = NFC_DIGITAL_FRAMING_NFCF_NFC_DEP;
+			check_crc = digital_skb_check_crc_f;
+			add_crc = digital_skb_add_crc_f;
+		}
+		break;
+
+	case NFC_PROTO_ISO15693:
+		framing = NFC_DIGITAL_FRAMING_ISO15693_T5T;
+		check_crc = digital_skb_check_crc_b;
+		add_crc = digital_skb_add_crc_b;
+		break;
+
+	case NFC_PROTO_ISO14443:
+		framing = NFC_DIGITAL_FRAMING_NFCA_T4T;
+		check_crc = digital_skb_check_crc_a;
+		add_crc = digital_skb_add_crc_a;
+		break;
+
+	case NFC_PROTO_ISO14443_B:
+		framing = NFC_DIGITAL_FRAMING_NFCB_T4T;
+		check_crc = digital_skb_check_crc_b;
+		add_crc = digital_skb_add_crc_b;
+		break;
+
+	default:
+		pr_err("Invalid protocol %d\n", protocol);
+		return -EINVAL;
+	}
+
+	pr_debug("rf_tech=%d, protocol=%d\n", rf_tech, protocol);
+
+	ddev->curr_rf_tech = rf_tech;
+
+	if (DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+		ddev->skb_add_crc = digital_skb_add_crc_none;
+		ddev->skb_check_crc = digital_skb_check_crc_none;
+	} else {
+		ddev->skb_add_crc = add_crc;
+		ddev->skb_check_crc = check_crc;
+	}
+
+	rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING, framing);
+	if (rc)
+		return rc;
+
+	target->supported_protocols = (1 << protocol);
+
+	poll_tech_count = ddev->poll_tech_count;
+	ddev->poll_tech_count = 0;
+
+	rc = nfc_targets_found(ddev->nfc_dev, target, 1);
+	if (rc) {
+		ddev->poll_tech_count = poll_tech_count;
+		return rc;
+	}
+
+	return 0;
+}
+
+void digital_poll_next_tech(struct nfc_digital_dev *ddev)
+{
+	u8 rand_mod;
+
+	digital_switch_rf(ddev, 0);
+
+	mutex_lock(&ddev->poll_lock);
+
+	if (!ddev->poll_tech_count) {
+		mutex_unlock(&ddev->poll_lock);
+		return;
+	}
+
+	get_random_bytes(&rand_mod, sizeof(rand_mod));
+	ddev->poll_tech_index = rand_mod % ddev->poll_tech_count;
+
+	mutex_unlock(&ddev->poll_lock);
+
+	schedule_work(&ddev->poll_work);
+}
+
+static void digital_wq_poll(struct work_struct *work)
+{
+	int rc;
+	struct digital_poll_tech *poll_tech;
+	struct nfc_digital_dev *ddev = container_of(work,
+						    struct nfc_digital_dev,
+						    poll_work);
+	mutex_lock(&ddev->poll_lock);
+
+	if (!ddev->poll_tech_count) {
+		mutex_unlock(&ddev->poll_lock);
+		return;
+	}
+
+	poll_tech = &ddev->poll_techs[ddev->poll_tech_index];
+
+	mutex_unlock(&ddev->poll_lock);
+
+	rc = poll_tech->poll_func(ddev, poll_tech->rf_tech);
+	if (rc)
+		digital_poll_next_tech(ddev);
+}
+
+static void digital_add_poll_tech(struct nfc_digital_dev *ddev, u8 rf_tech,
+				  digital_poll_t poll_func)
+{
+	struct digital_poll_tech *poll_tech;
+
+	if (ddev->poll_tech_count >= NFC_DIGITAL_POLL_MODE_COUNT_MAX)
+		return;
+
+	poll_tech = &ddev->poll_techs[ddev->poll_tech_count++];
+
+	poll_tech->rf_tech = rf_tech;
+	poll_tech->poll_func = poll_func;
+}
+
+/**
+ * start_poll operation
+ *
+ * For every supported protocol, the corresponding polling function is added
+ * to the table of polling technologies (ddev->poll_techs[]) using
+ * digital_add_poll_tech().
+ * When a polling function fails (by timeout or protocol error) the next one is
+ * schedule by digital_poll_next_tech() on the poll workqueue (ddev->poll_work).
+ */
+static int digital_start_poll(struct nfc_dev *nfc_dev, __u32 im_protocols,
+			      __u32 tm_protocols)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+	u32 matching_im_protocols, matching_tm_protocols;
+
+	pr_debug("protocols: im 0x%x, tm 0x%x, supported 0x%x\n", im_protocols,
+		 tm_protocols, ddev->protocols);
+
+	matching_im_protocols = ddev->protocols & im_protocols;
+	matching_tm_protocols = ddev->protocols & tm_protocols;
+
+	if (!matching_im_protocols && !matching_tm_protocols) {
+		pr_err("Unknown protocol\n");
+		return -EINVAL;
+	}
+
+	if (ddev->poll_tech_count) {
+		pr_err("Already polling\n");
+		return -EBUSY;
+	}
+
+	if (ddev->curr_protocol) {
+		pr_err("A target is already active\n");
+		return -EBUSY;
+	}
+
+	ddev->poll_tech_count = 0;
+	ddev->poll_tech_index = 0;
+
+	if (matching_im_protocols & DIGITAL_PROTO_NFCA_RF_TECH)
+		digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
+				      digital_in_send_sens_req);
+
+	if (matching_im_protocols & DIGITAL_PROTO_NFCB_RF_TECH)
+		digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106B,
+				      digital_in_send_sensb_req);
+
+	if (matching_im_protocols & DIGITAL_PROTO_NFCF_RF_TECH) {
+		digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_212F,
+				      digital_in_send_sensf_req);
+
+		digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_424F,
+				      digital_in_send_sensf_req);
+	}
+
+	if (matching_im_protocols & DIGITAL_PROTO_ISO15693_RF_TECH)
+		digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_ISO15693,
+				      digital_in_send_iso15693_inv_req);
+
+	if (matching_tm_protocols & NFC_PROTO_NFC_DEP_MASK) {
+		if (ddev->ops->tg_listen_mdaa) {
+			digital_add_poll_tech(ddev, 0,
+					      digital_tg_listen_mdaa);
+		} else if (ddev->ops->tg_listen_md) {
+			digital_add_poll_tech(ddev, 0,
+					      digital_tg_listen_md);
+		} else {
+			digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
+					      digital_tg_listen_nfca);
+
+			digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_212F,
+					      digital_tg_listen_nfcf);
+
+			digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_424F,
+					      digital_tg_listen_nfcf);
+		}
+	}
+
+	if (!ddev->poll_tech_count) {
+		pr_err("Unsupported protocols: im=0x%x, tm=0x%x\n",
+		       matching_im_protocols, matching_tm_protocols);
+		return -EINVAL;
+	}
+
+	schedule_work(&ddev->poll_work);
+
+	return 0;
+}
+
+static void digital_stop_poll(struct nfc_dev *nfc_dev)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+	mutex_lock(&ddev->poll_lock);
+
+	if (!ddev->poll_tech_count) {
+		pr_err("Polling operation was not running\n");
+		mutex_unlock(&ddev->poll_lock);
+		return;
+	}
+
+	ddev->poll_tech_count = 0;
+
+	mutex_unlock(&ddev->poll_lock);
+
+	cancel_work_sync(&ddev->poll_work);
+
+	digital_abort_cmd(ddev);
+}
+
+static int digital_dev_up(struct nfc_dev *nfc_dev)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+	digital_switch_rf(ddev, 1);
+
+	return 0;
+}
+
+static int digital_dev_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+	digital_switch_rf(ddev, 0);
+
+	return 0;
+}
+
+static int digital_dep_link_up(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target,
+			       __u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+	int rc;
+
+	rc = digital_in_send_atr_req(ddev, target, comm_mode, gb, gb_len);
+
+	if (!rc)
+		ddev->curr_protocol = NFC_PROTO_NFC_DEP;
+
+	return rc;
+}
+
+static int digital_dep_link_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+	ddev->curr_protocol = 0;
+
+	return 0;
+}
+
+static int digital_activate_target(struct nfc_dev *nfc_dev,
+				   struct nfc_target *target, __u32 protocol)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+	if (ddev->poll_tech_count) {
+		pr_err("Can't activate a target while polling\n");
+		return -EBUSY;
+	}
+
+	if (ddev->curr_protocol) {
+		pr_err("A target is already active\n");
+		return -EBUSY;
+	}
+
+	ddev->curr_protocol = protocol;
+
+	return 0;
+}
+
+static void digital_deactivate_target(struct nfc_dev *nfc_dev,
+				      struct nfc_target *target)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+	if (!ddev->curr_protocol) {
+		pr_err("No active target\n");
+		return;
+	}
+
+	ddev->curr_protocol = 0;
+}
+
+static int digital_tg_send(struct nfc_dev *dev, struct sk_buff *skb)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(dev);
+
+	return digital_tg_send_dep_res(ddev, skb);
+}
+
+static void digital_in_send_complete(struct nfc_digital_dev *ddev, void *arg,
+				     struct sk_buff *resp)
+{
+	struct digital_data_exch *data_exch = arg;
+	int rc;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto done;
+	}
+
+	if (ddev->curr_protocol == NFC_PROTO_MIFARE) {
+		rc = digital_in_recv_mifare_res(resp);
+		/* crc check is done in digital_in_recv_mifare_res() */
+		goto done;
+	}
+
+	if ((ddev->curr_protocol == NFC_PROTO_ISO14443) ||
+	    (ddev->curr_protocol == NFC_PROTO_ISO14443_B)) {
+		rc = digital_in_iso_dep_pull_sod(ddev, resp);
+		if (rc)
+			goto done;
+	}
+
+	rc = ddev->skb_check_crc(resp);
+
+done:
+	if (rc) {
+		kfree_skb(resp);
+		resp = NULL;
+	}
+
+	data_exch->cb(data_exch->cb_context, resp, rc);
+
+	kfree(data_exch);
+}
+
+static int digital_in_send(struct nfc_dev *nfc_dev, struct nfc_target *target,
+			   struct sk_buff *skb, data_exchange_cb_t cb,
+			   void *cb_context)
+{
+	struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+	struct digital_data_exch *data_exch;
+	int rc;
+
+	data_exch = kzalloc(sizeof(struct digital_data_exch), GFP_KERNEL);
+	if (!data_exch) {
+		pr_err("Failed to allocate data_exch struct\n");
+		return -ENOMEM;
+	}
+
+	data_exch->cb = cb;
+	data_exch->cb_context = cb_context;
+
+	if (ddev->curr_protocol == NFC_PROTO_NFC_DEP) {
+		rc = digital_in_send_dep_req(ddev, target, skb, data_exch);
+		goto exit;
+	}
+
+	if ((ddev->curr_protocol == NFC_PROTO_ISO14443) ||
+	    (ddev->curr_protocol == NFC_PROTO_ISO14443_B)) {
+		rc = digital_in_iso_dep_push_sod(ddev, skb);
+		if (rc)
+			goto exit;
+	}
+
+	ddev->skb_add_crc(skb);
+
+	rc = digital_in_send_cmd(ddev, skb, 500, digital_in_send_complete,
+				 data_exch);
+
+exit:
+	if (rc)
+		kfree(data_exch);
+
+	return rc;
+}
+
+static struct nfc_ops digital_nfc_ops = {
+	.dev_up = digital_dev_up,
+	.dev_down = digital_dev_down,
+	.start_poll = digital_start_poll,
+	.stop_poll = digital_stop_poll,
+	.dep_link_up = digital_dep_link_up,
+	.dep_link_down = digital_dep_link_down,
+	.activate_target = digital_activate_target,
+	.deactivate_target = digital_deactivate_target,
+	.tm_send = digital_tg_send,
+	.im_transceive = digital_in_send,
+};
+
+struct nfc_digital_dev *nfc_digital_allocate_device(struct nfc_digital_ops *ops,
+					    __u32 supported_protocols,
+					    __u32 driver_capabilities,
+					    int tx_headroom, int tx_tailroom)
+{
+	struct nfc_digital_dev *ddev;
+
+	if (!ops->in_configure_hw || !ops->in_send_cmd || !ops->tg_listen ||
+	    !ops->tg_configure_hw || !ops->tg_send_cmd || !ops->abort_cmd ||
+	    !ops->switch_rf || (ops->tg_listen_md && !ops->tg_get_rf_tech))
+		return NULL;
+
+	ddev = kzalloc(sizeof(struct nfc_digital_dev), GFP_KERNEL);
+	if (!ddev)
+		return NULL;
+
+	ddev->driver_capabilities = driver_capabilities;
+	ddev->ops = ops;
+
+	mutex_init(&ddev->cmd_lock);
+	INIT_LIST_HEAD(&ddev->cmd_queue);
+
+	INIT_WORK(&ddev->cmd_work, digital_wq_cmd);
+	INIT_WORK(&ddev->cmd_complete_work, digital_wq_cmd_complete);
+
+	mutex_init(&ddev->poll_lock);
+	INIT_WORK(&ddev->poll_work, digital_wq_poll);
+
+	if (supported_protocols & NFC_PROTO_JEWEL_MASK)
+		ddev->protocols |= NFC_PROTO_JEWEL_MASK;
+	if (supported_protocols & NFC_PROTO_MIFARE_MASK)
+		ddev->protocols |= NFC_PROTO_MIFARE_MASK;
+	if (supported_protocols & NFC_PROTO_FELICA_MASK)
+		ddev->protocols |= NFC_PROTO_FELICA_MASK;
+	if (supported_protocols & NFC_PROTO_NFC_DEP_MASK)
+		ddev->protocols |= NFC_PROTO_NFC_DEP_MASK;
+	if (supported_protocols & NFC_PROTO_ISO15693_MASK)
+		ddev->protocols |= NFC_PROTO_ISO15693_MASK;
+	if (supported_protocols & NFC_PROTO_ISO14443_MASK)
+		ddev->protocols |= NFC_PROTO_ISO14443_MASK;
+	if (supported_protocols & NFC_PROTO_ISO14443_B_MASK)
+		ddev->protocols |= NFC_PROTO_ISO14443_B_MASK;
+
+	ddev->tx_headroom = tx_headroom + DIGITAL_MAX_HEADER_LEN;
+	ddev->tx_tailroom = tx_tailroom + DIGITAL_CRC_LEN;
+
+	ddev->nfc_dev = nfc_allocate_device(&digital_nfc_ops, ddev->protocols,
+					    ddev->tx_headroom,
+					    ddev->tx_tailroom);
+	if (!ddev->nfc_dev) {
+		pr_err("nfc_allocate_device failed\n");
+		goto free_dev;
+	}
+
+	nfc_set_drvdata(ddev->nfc_dev, ddev);
+
+	return ddev;
+
+free_dev:
+	kfree(ddev);
+
+	return NULL;
+}
+EXPORT_SYMBOL(nfc_digital_allocate_device);
+
+void nfc_digital_free_device(struct nfc_digital_dev *ddev)
+{
+	nfc_free_device(ddev->nfc_dev);
+	kfree(ddev);
+}
+EXPORT_SYMBOL(nfc_digital_free_device);
+
+int nfc_digital_register_device(struct nfc_digital_dev *ddev)
+{
+	return nfc_register_device(ddev->nfc_dev);
+}
+EXPORT_SYMBOL(nfc_digital_register_device);
+
+void nfc_digital_unregister_device(struct nfc_digital_dev *ddev)
+{
+	struct digital_cmd *cmd, *n;
+
+	nfc_unregister_device(ddev->nfc_dev);
+
+	mutex_lock(&ddev->poll_lock);
+	ddev->poll_tech_count = 0;
+	mutex_unlock(&ddev->poll_lock);
+
+	cancel_work_sync(&ddev->poll_work);
+	cancel_work_sync(&ddev->cmd_work);
+	cancel_work_sync(&ddev->cmd_complete_work);
+
+	list_for_each_entry_safe(cmd, n, &ddev->cmd_queue, queue) {
+		list_del(&cmd->queue);
+		kfree(cmd->mdaa_params);
+		kfree(cmd);
+	}
+}
+EXPORT_SYMBOL(nfc_digital_unregister_device);
+
+MODULE_LICENSE("GPL");

net/nfc/digital_dep.c

@@ -0,0 +1,850 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * 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 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.
+ *
+ */
+
+#define pr_fmt(fmt) "digital: %s: " fmt, __func__
+
+#include "digital.h"
+
+#define DIGITAL_NFC_DEP_FRAME_DIR_OUT 0xD4
+#define DIGITAL_NFC_DEP_FRAME_DIR_IN  0xD5
+
+#define DIGITAL_NFC_DEP_NFCA_SOD_SB   0xF0
+
+#define DIGITAL_CMD_ATR_REQ 0x00
+#define DIGITAL_CMD_ATR_RES 0x01
+#define DIGITAL_CMD_PSL_REQ 0x04
+#define DIGITAL_CMD_PSL_RES 0x05
+#define DIGITAL_CMD_DEP_REQ 0x06
+#define DIGITAL_CMD_DEP_RES 0x07
+
+#define DIGITAL_ATR_REQ_MIN_SIZE 16
+#define DIGITAL_ATR_REQ_MAX_SIZE 64
+
+#define DIGITAL_LR_BITS_PAYLOAD_SIZE_254B 0x30
+#define DIGITAL_FSL_BITS_PAYLOAD_SIZE_254B \
+				(DIGITAL_LR_BITS_PAYLOAD_SIZE_254B >> 4)
+#define DIGITAL_GB_BIT	0x02
+
+#define DIGITAL_NFC_DEP_PFB_TYPE(pfb) ((pfb) & 0xE0)
+
+#define DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT 0x10
+
+#define DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb) \
+				((pfb) & DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT)
+#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb)  ((pfb) & 0x10)
+#define DIGITAL_NFC_DEP_NAD_BIT_SET(pfb) ((pfb) & 0x08)
+#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & 0x04)
+#define DIGITAL_NFC_DEP_PFB_PNI(pfb)     ((pfb) & 0x03)
+
+#define DIGITAL_NFC_DEP_PFB_I_PDU          0x00
+#define DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU   0x40
+#define DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU 0x80
+
+struct digital_atr_req {
+	u8 dir;
+	u8 cmd;
+	u8 nfcid3[10];
+	u8 did;
+	u8 bs;
+	u8 br;
+	u8 pp;
+	u8 gb[0];
+} __packed;
+
+struct digital_atr_res {
+	u8 dir;
+	u8 cmd;
+	u8 nfcid3[10];
+	u8 did;
+	u8 bs;
+	u8 br;
+	u8 to;
+	u8 pp;
+	u8 gb[0];
+} __packed;
+
+struct digital_psl_req {
+	u8 dir;
+	u8 cmd;
+	u8 did;
+	u8 brs;
+	u8 fsl;
+} __packed;
+
+struct digital_psl_res {
+	u8 dir;
+	u8 cmd;
+	u8 did;
+} __packed;
+
+struct digital_dep_req_res {
+	u8 dir;
+	u8 cmd;
+	u8 pfb;
+} __packed;
+
+static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
+				    struct sk_buff *resp);
+
+static void digital_skb_push_dep_sod(struct nfc_digital_dev *ddev,
+				     struct sk_buff *skb)
+{
+	skb_push(skb, sizeof(u8));
+
+	skb->data[0] = skb->len;
+
+	if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
+		*skb_push(skb, sizeof(u8)) = DIGITAL_NFC_DEP_NFCA_SOD_SB;
+}
+
+static int digital_skb_pull_dep_sod(struct nfc_digital_dev *ddev,
+				    struct sk_buff *skb)
+{
+	u8 size;
+
+	if (skb->len < 2)
+		return -EIO;
+
+	if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
+		skb_pull(skb, sizeof(u8));
+
+	size = skb->data[0];
+	if (size != skb->len)
+		return -EIO;
+
+	skb_pull(skb, sizeof(u8));
+
+	return 0;
+}
+
+static void digital_in_recv_psl_res(struct nfc_digital_dev *ddev, void *arg,
+				    struct sk_buff *resp)
+{
+	struct nfc_target *target = arg;
+	struct digital_psl_res *psl_res;
+	int rc;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto exit;
+	}
+
+	rc = ddev->skb_check_crc(resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.6");
+		goto exit;
+	}
+
+	rc = digital_skb_pull_dep_sod(ddev, resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.2");
+		goto exit;
+	}
+
+	psl_res = (struct digital_psl_res *)resp->data;
+
+	if ((resp->len != sizeof(*psl_res)) ||
+	    (psl_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN) ||
+	    (psl_res->cmd != DIGITAL_CMD_PSL_RES)) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+				     NFC_DIGITAL_RF_TECH_424F);
+	if (rc)
+		goto exit;
+
+	rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+				     NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
+	if (rc)
+		goto exit;
+
+	if (!DIGITAL_DRV_CAPS_IN_CRC(ddev) &&
+	    (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)) {
+		ddev->skb_add_crc = digital_skb_add_crc_f;
+		ddev->skb_check_crc = digital_skb_check_crc_f;
+	}
+
+	ddev->curr_rf_tech = NFC_DIGITAL_RF_TECH_424F;
+
+	nfc_dep_link_is_up(ddev->nfc_dev, target->idx, NFC_COMM_ACTIVE,
+			   NFC_RF_INITIATOR);
+
+	ddev->curr_nfc_dep_pni = 0;
+
+exit:
+	dev_kfree_skb(resp);
+
+	if (rc)
+		ddev->curr_protocol = 0;
+}
+
+static int digital_in_send_psl_req(struct nfc_digital_dev *ddev,
+				   struct nfc_target *target)
+{
+	struct sk_buff *skb;
+	struct digital_psl_req *psl_req;
+
+	skb = digital_skb_alloc(ddev, sizeof(*psl_req));
+	if (!skb)
+		return -ENOMEM;
+
+	skb_put(skb, sizeof(*psl_req));
+
+	psl_req = (struct digital_psl_req *)skb->data;
+
+	psl_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+	psl_req->cmd = DIGITAL_CMD_PSL_REQ;
+	psl_req->did = 0;
+	psl_req->brs = (0x2 << 3) | 0x2; /* 424F both directions */
+	psl_req->fsl = DIGITAL_FSL_BITS_PAYLOAD_SIZE_254B;
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	return digital_in_send_cmd(ddev, skb, 500, digital_in_recv_psl_res,
+				   target);
+}
+
+static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg,
+				 struct sk_buff *resp)
+{
+	struct nfc_target *target = arg;
+	struct digital_atr_res *atr_res;
+	u8 gb_len;
+	int rc;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto exit;
+	}
+
+	rc = ddev->skb_check_crc(resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.6");
+		goto exit;
+	}
+
+	rc = digital_skb_pull_dep_sod(ddev, resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.2");
+		goto exit;
+	}
+
+	if (resp->len < sizeof(struct digital_atr_res)) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	gb_len = resp->len - sizeof(struct digital_atr_res);
+
+	atr_res = (struct digital_atr_res *)resp->data;
+
+	rc = nfc_set_remote_general_bytes(ddev->nfc_dev, atr_res->gb, gb_len);
+	if (rc)
+		goto exit;
+
+	if ((ddev->protocols & NFC_PROTO_FELICA_MASK) &&
+	    (ddev->curr_rf_tech != NFC_DIGITAL_RF_TECH_424F)) {
+		rc = digital_in_send_psl_req(ddev, target);
+		if (!rc)
+			goto exit;
+	}
+
+	rc = nfc_dep_link_is_up(ddev->nfc_dev, target->idx, NFC_COMM_ACTIVE,
+				NFC_RF_INITIATOR);
+
+	ddev->curr_nfc_dep_pni = 0;
+
+exit:
+	dev_kfree_skb(resp);
+
+	if (rc)
+		ddev->curr_protocol = 0;
+}
+
+int digital_in_send_atr_req(struct nfc_digital_dev *ddev,
+			    struct nfc_target *target, __u8 comm_mode, __u8 *gb,
+			    size_t gb_len)
+{
+	struct sk_buff *skb;
+	struct digital_atr_req *atr_req;
+	uint size;
+
+	size = DIGITAL_ATR_REQ_MIN_SIZE + gb_len;
+
+	if (size > DIGITAL_ATR_REQ_MAX_SIZE) {
+		PROTOCOL_ERR("14.6.1.1");
+		return -EINVAL;
+	}
+
+	skb = digital_skb_alloc(ddev, size);
+	if (!skb)
+		return -ENOMEM;
+
+	skb_put(skb, sizeof(struct digital_atr_req));
+
+	atr_req = (struct digital_atr_req *)skb->data;
+	memset(atr_req, 0, sizeof(struct digital_atr_req));
+
+	atr_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+	atr_req->cmd = DIGITAL_CMD_ATR_REQ;
+	if (target->nfcid2_len)
+		memcpy(atr_req->nfcid3, target->nfcid2, NFC_NFCID2_MAXSIZE);
+	else
+		get_random_bytes(atr_req->nfcid3, NFC_NFCID3_MAXSIZE);
+
+	atr_req->did = 0;
+	atr_req->bs = 0;
+	atr_req->br = 0;
+
+	atr_req->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B;
+
+	if (gb_len) {
+		atr_req->pp |= DIGITAL_GB_BIT;
+		memcpy(skb_put(skb, gb_len), gb, gb_len);
+	}
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	return digital_in_send_cmd(ddev, skb, 500, digital_in_recv_atr_res,
+				   target);
+}
+
+static int digital_in_send_rtox(struct nfc_digital_dev *ddev,
+				struct digital_data_exch *data_exch, u8 rtox)
+{
+	struct digital_dep_req_res *dep_req;
+	struct sk_buff *skb;
+	int rc;
+
+	skb = digital_skb_alloc(ddev, 1);
+	if (!skb)
+		return -ENOMEM;
+
+	*skb_put(skb, 1) = rtox;
+
+	skb_push(skb, sizeof(struct digital_dep_req_res));
+
+	dep_req = (struct digital_dep_req_res *)skb->data;
+
+	dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+	dep_req->cmd = DIGITAL_CMD_DEP_REQ;
+	dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU |
+		       DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT;
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
+				 data_exch);
+
+	return rc;
+}
+
+static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
+				    struct sk_buff *resp)
+{
+	struct digital_data_exch *data_exch = arg;
+	struct digital_dep_req_res *dep_res;
+	u8 pfb;
+	uint size;
+	int rc;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto exit;
+	}
+
+	rc = ddev->skb_check_crc(resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.6");
+		goto error;
+	}
+
+	rc = digital_skb_pull_dep_sod(ddev, resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.2");
+		goto exit;
+	}
+
+	dep_res = (struct digital_dep_req_res *)resp->data;
+
+	if (resp->len < sizeof(struct digital_dep_req_res) ||
+	    dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN ||
+	    dep_res->cmd != DIGITAL_CMD_DEP_RES) {
+		rc = -EIO;
+		goto error;
+	}
+
+	pfb = dep_res->pfb;
+
+	switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) {
+	case DIGITAL_NFC_DEP_PFB_I_PDU:
+		if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
+			PROTOCOL_ERR("14.12.3.3");
+			rc = -EIO;
+			goto error;
+		}
+
+		ddev->curr_nfc_dep_pni =
+			DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
+		rc = 0;
+		break;
+
+	case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
+		pr_err("Received a ACK/NACK PDU\n");
+		rc = -EIO;
+		goto error;
+
+	case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
+		if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) {
+			rc = -EINVAL;
+			goto error;
+		}
+
+		rc = digital_in_send_rtox(ddev, data_exch, resp->data[3]);
+		if (rc)
+			goto error;
+
+		kfree_skb(resp);
+		return;
+	}
+
+	if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) {
+		pr_err("MI bit set. Chained PDU not supported\n");
+		rc = -EIO;
+		goto error;
+	}
+
+	size = sizeof(struct digital_dep_req_res);
+
+	if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb))
+		size++;
+
+	if (size > resp->len) {
+		rc = -EIO;
+		goto error;
+	}
+
+	skb_pull(resp, size);
+
+exit:
+	data_exch->cb(data_exch->cb_context, resp, rc);
+
+error:
+	kfree(data_exch);
+
+	if (rc)
+		kfree_skb(resp);
+}
+
+int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
+			    struct nfc_target *target, struct sk_buff *skb,
+			    struct digital_data_exch *data_exch)
+{
+	struct digital_dep_req_res *dep_req;
+
+	skb_push(skb, sizeof(struct digital_dep_req_res));
+
+	dep_req = (struct digital_dep_req_res *)skb->data;
+	dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+	dep_req->cmd = DIGITAL_CMD_DEP_REQ;
+	dep_req->pfb = ddev->curr_nfc_dep_pni;
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	return digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
+				   data_exch);
+}
+
+static void digital_tg_set_rf_tech(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+	ddev->curr_rf_tech = rf_tech;
+
+	ddev->skb_add_crc = digital_skb_add_crc_none;
+	ddev->skb_check_crc = digital_skb_check_crc_none;
+
+	if (DIGITAL_DRV_CAPS_TG_CRC(ddev))
+		return;
+
+	switch (ddev->curr_rf_tech) {
+	case NFC_DIGITAL_RF_TECH_106A:
+		ddev->skb_add_crc = digital_skb_add_crc_a;
+		ddev->skb_check_crc = digital_skb_check_crc_a;
+		break;
+
+	case NFC_DIGITAL_RF_TECH_212F:
+	case NFC_DIGITAL_RF_TECH_424F:
+		ddev->skb_add_crc = digital_skb_add_crc_f;
+		ddev->skb_check_crc = digital_skb_check_crc_f;
+		break;
+
+	default:
+		break;
+	}
+}
+
+static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
+				    struct sk_buff *resp)
+{
+	int rc;
+	struct digital_dep_req_res *dep_req;
+	size_t size;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto exit;
+	}
+
+	rc = ddev->skb_check_crc(resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.6");
+		goto exit;
+	}
+
+	rc = digital_skb_pull_dep_sod(ddev, resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.2");
+		goto exit;
+	}
+
+	size = sizeof(struct digital_dep_req_res);
+	dep_req = (struct digital_dep_req_res *)resp->data;
+
+	if (resp->len < size || dep_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
+	    dep_req->cmd != DIGITAL_CMD_DEP_REQ) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	if (DIGITAL_NFC_DEP_DID_BIT_SET(dep_req->pfb))
+		size++;
+
+	if (resp->len < size) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	switch (DIGITAL_NFC_DEP_PFB_TYPE(dep_req->pfb)) {
+	case DIGITAL_NFC_DEP_PFB_I_PDU:
+		pr_debug("DIGITAL_NFC_DEP_PFB_I_PDU\n");
+		ddev->curr_nfc_dep_pni = DIGITAL_NFC_DEP_PFB_PNI(dep_req->pfb);
+		break;
+	case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
+		pr_err("Received a ACK/NACK PDU\n");
+		rc = -EINVAL;
+		goto exit;
+	case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
+		pr_err("Received a SUPERVISOR PDU\n");
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	skb_pull(resp, size);
+
+	rc = nfc_tm_data_received(ddev->nfc_dev, resp);
+
+exit:
+	if (rc)
+		kfree_skb(resp);
+}
+
+int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb)
+{
+	struct digital_dep_req_res *dep_res;
+
+	skb_push(skb, sizeof(struct digital_dep_req_res));
+	dep_res = (struct digital_dep_req_res *)skb->data;
+
+	dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
+	dep_res->cmd = DIGITAL_CMD_DEP_RES;
+	dep_res->pfb = ddev->curr_nfc_dep_pni;
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	return digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
+				   NULL);
+}
+
+static void digital_tg_send_psl_res_complete(struct nfc_digital_dev *ddev,
+					     void *arg, struct sk_buff *resp)
+{
+	u8 rf_tech = (unsigned long)arg;
+
+	if (IS_ERR(resp))
+		return;
+
+	digital_tg_set_rf_tech(ddev, rf_tech);
+
+	digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+
+	digital_tg_listen(ddev, 1500, digital_tg_recv_dep_req, NULL);
+
+	dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_psl_res(struct nfc_digital_dev *ddev, u8 did,
+				   u8 rf_tech)
+{
+	struct digital_psl_res *psl_res;
+	struct sk_buff *skb;
+	int rc;
+
+	skb = digital_skb_alloc(ddev, sizeof(struct digital_psl_res));
+	if (!skb)
+		return -ENOMEM;
+
+	skb_put(skb, sizeof(struct digital_psl_res));
+
+	psl_res = (struct digital_psl_res *)skb->data;
+
+	psl_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
+	psl_res->cmd = DIGITAL_CMD_PSL_RES;
+	psl_res->did = did;
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	rc = digital_tg_send_cmd(ddev, skb, 0, digital_tg_send_psl_res_complete,
+				 (void *)(unsigned long)rf_tech);
+
+	if (rc)
+		kfree_skb(skb);
+
+	return rc;
+}
+
+static void digital_tg_recv_psl_req(struct nfc_digital_dev *ddev, void *arg,
+				    struct sk_buff *resp)
+{
+	int rc;
+	struct digital_psl_req *psl_req;
+	u8 rf_tech;
+	u8 dsi;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto exit;
+	}
+
+	rc = ddev->skb_check_crc(resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.6");
+		goto exit;
+	}
+
+	rc = digital_skb_pull_dep_sod(ddev, resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.2");
+		goto exit;
+	}
+
+	psl_req = (struct digital_psl_req *)resp->data;
+
+	if (resp->len != sizeof(struct digital_psl_req) ||
+	    psl_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
+	    psl_req->cmd != DIGITAL_CMD_PSL_REQ) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	dsi = (psl_req->brs >> 3) & 0x07;
+	switch (dsi) {
+	case 0:
+		rf_tech = NFC_DIGITAL_RF_TECH_106A;
+		break;
+	case 1:
+		rf_tech = NFC_DIGITAL_RF_TECH_212F;
+		break;
+	case 2:
+		rf_tech = NFC_DIGITAL_RF_TECH_424F;
+		break;
+	default:
+		pr_err("Unsupported dsi value %d\n", dsi);
+		goto exit;
+	}
+
+	rc = digital_tg_send_psl_res(ddev, psl_req->did, rf_tech);
+
+exit:
+	kfree_skb(resp);
+}
+
+static void digital_tg_send_atr_res_complete(struct nfc_digital_dev *ddev,
+					     void *arg, struct sk_buff *resp)
+{
+	int offset;
+
+	if (IS_ERR(resp)) {
+		digital_poll_next_tech(ddev);
+		return;
+	}
+
+	offset = 2;
+	if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB)
+		offset++;
+
+	if (resp->data[offset] == DIGITAL_CMD_PSL_REQ)
+		digital_tg_recv_psl_req(ddev, arg, resp);
+	else
+		digital_tg_recv_dep_req(ddev, arg, resp);
+}
+
+static int digital_tg_send_atr_res(struct nfc_digital_dev *ddev,
+				   struct digital_atr_req *atr_req)
+{
+	struct digital_atr_res *atr_res;
+	struct sk_buff *skb;
+	u8 *gb;
+	size_t gb_len;
+	int rc;
+
+	gb = nfc_get_local_general_bytes(ddev->nfc_dev, &gb_len);
+	if (!gb)
+		gb_len = 0;
+
+	skb = digital_skb_alloc(ddev, sizeof(struct digital_atr_res) + gb_len);
+	if (!skb)
+		return -ENOMEM;
+
+	skb_put(skb, sizeof(struct digital_atr_res));
+	atr_res = (struct digital_atr_res *)skb->data;
+
+	memset(atr_res, 0, sizeof(struct digital_atr_res));
+
+	atr_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
+	atr_res->cmd = DIGITAL_CMD_ATR_RES;
+	memcpy(atr_res->nfcid3, atr_req->nfcid3, sizeof(atr_req->nfcid3));
+	atr_res->to = 8;
+	atr_res->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B;
+	if (gb_len) {
+		skb_put(skb, gb_len);
+
+		atr_res->pp |= DIGITAL_GB_BIT;
+		memcpy(atr_res->gb, gb, gb_len);
+	}
+
+	digital_skb_push_dep_sod(ddev, skb);
+
+	ddev->skb_add_crc(skb);
+
+	rc = digital_tg_send_cmd(ddev, skb, 999,
+				 digital_tg_send_atr_res_complete, NULL);
+	if (rc) {
+		kfree_skb(skb);
+		return rc;
+	}
+
+	return rc;
+}
+
+void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg,
+			     struct sk_buff *resp)
+{
+	int rc;
+	struct digital_atr_req *atr_req;
+	size_t gb_len, min_size;
+	u8 poll_tech_count;
+
+	if (IS_ERR(resp)) {
+		rc = PTR_ERR(resp);
+		resp = NULL;
+		goto exit;
+	}
+
+	if (!resp->len) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB) {
+		min_size = DIGITAL_ATR_REQ_MIN_SIZE + 2;
+		digital_tg_set_rf_tech(ddev, NFC_DIGITAL_RF_TECH_106A);
+	} else {
+		min_size = DIGITAL_ATR_REQ_MIN_SIZE + 1;
+		digital_tg_set_rf_tech(ddev, NFC_DIGITAL_RF_TECH_212F);
+	}
+
+	if (resp->len < min_size) {
+		rc = -EIO;
+		goto exit;
+	}
+
+	ddev->curr_protocol = NFC_PROTO_NFC_DEP_MASK;
+
+	rc = ddev->skb_check_crc(resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.6");
+		goto exit;
+	}
+
+	rc = digital_skb_pull_dep_sod(ddev, resp);
+	if (rc) {
+		PROTOCOL_ERR("14.4.1.2");
+		goto exit;
+	}
+
+	atr_req = (struct digital_atr_req *)resp->data;
+
+	if (atr_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
+	    atr_req->cmd != DIGITAL_CMD_ATR_REQ) {
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+				     NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED);
+	if (rc)
+		goto exit;
+
+	rc = digital_tg_send_atr_res(ddev, atr_req);
+	if (rc)
+		goto exit;
+
+	gb_len = resp->len - sizeof(struct digital_atr_req);
+
+	poll_tech_count = ddev->poll_tech_count;
+	ddev->poll_tech_count = 0;
+
+	rc = nfc_tm_activated(ddev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
+			      NFC_COMM_PASSIVE, atr_req->gb, gb_len);
+	if (rc) {
+		ddev->poll_tech_count = poll_tech_count;
+		goto exit;
+	}
+
+	rc = 0;
+exit:
+	if (rc)
+		digital_poll_next_tech(ddev);
+
+	dev_kfree_skb(resp);
+}

net/nfc/hci/Kconfig

@@ -0,0 +1,17 @@
+config NFC_HCI
+       depends on NFC
+       tristate "NFC HCI implementation"
+       default n
+       help
+	 Say Y here if you want to build support for a kernel NFC HCI
+	 implementation. This is mostly needed for devices that only process
+	 HCI frames, like for example the NXP pn544.
+
+config NFC_SHDLC
+	depends on NFC_HCI
+	select CRC_CCITT
+	bool "SHDLC link layer for HCI based NFC drivers"
+	default n
+	---help---
+	  Say yes if you use an NFC HCI driver that requires SHDLC link layer.
+	  If unsure, say N here.

net/nfc/hci/Makefile

@@ -0,0 +1,8 @@
+#
+# Makefile for the Linux NFC HCI layer.
+#
+
+obj-$(CONFIG_NFC_HCI) += hci.o
+
+hci-y			:= core.o hcp.o command.o llc.o llc_nop.o
+hci-$(CONFIG_NFC_SHDLC) += llc_shdlc.o

net/nfc/hci/command.c

@@ -0,0 +1,384 @@
+/*
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) "hci: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+
+#include <net/nfc/hci.h>
+
+#include "hci.h"
+
+#define MAX_FWI 4949
+
+static int nfc_hci_execute_cmd_async(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
+			       const u8 *param, size_t param_len,
+			       data_exchange_cb_t cb, void *cb_context)
+{
+	pr_debug("exec cmd async through pipe=%d, cmd=%d, plen=%zd\n", pipe,
+		 cmd, param_len);
+
+	/* TODO: Define hci cmd execution delay. Should it be the same
+	 * for all commands?
+	 */
+	return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_COMMAND, cmd,
+				      param, param_len, cb, cb_context, MAX_FWI);
+}
+
+/*
+ * HCI command execution completion callback.
+ * err will be a standard linux error (may be converted from HCI response)
+ * skb contains the response data and must be disposed, or may be NULL if
+ * an error occured
+ */
+static void nfc_hci_execute_cb(void *context, struct sk_buff *skb, int err)
+{
+	struct hcp_exec_waiter *hcp_ew = (struct hcp_exec_waiter *)context;
+
+	pr_debug("HCI Cmd completed with result=%d\n", err);
+
+	hcp_ew->exec_result = err;
+	if (hcp_ew->exec_result == 0)
+		hcp_ew->result_skb = skb;
+	else
+		kfree_skb(skb);
+	hcp_ew->exec_complete = true;
+
+	wake_up(hcp_ew->wq);
+}
+
+static int nfc_hci_execute_cmd(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
+			       const u8 *param, size_t param_len,
+			       struct sk_buff **skb)
+{
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(ew_wq);
+	struct hcp_exec_waiter hcp_ew;
+	hcp_ew.wq = &ew_wq;
+	hcp_ew.exec_complete = false;
+	hcp_ew.result_skb = NULL;
+
+	pr_debug("exec cmd sync through pipe=%d, cmd=%d, plen=%zd\n", pipe,
+		 cmd, param_len);
+
+	/* TODO: Define hci cmd execution delay. Should it be the same
+	 * for all commands?
+	 */
+	hcp_ew.exec_result = nfc_hci_hcp_message_tx(hdev, pipe,
+						    NFC_HCI_HCP_COMMAND, cmd,
+						    param, param_len,
+						    nfc_hci_execute_cb, &hcp_ew,
+						    MAX_FWI);
+	if (hcp_ew.exec_result < 0)
+		return hcp_ew.exec_result;
+
+	wait_event(ew_wq, hcp_ew.exec_complete == true);
+
+	if (hcp_ew.exec_result == 0) {
+		if (skb)
+			*skb = hcp_ew.result_skb;
+		else
+			kfree_skb(hcp_ew.result_skb);
+	}
+
+	return hcp_ew.exec_result;
+}
+
+int nfc_hci_send_event(struct nfc_hci_dev *hdev, u8 gate, u8 event,
+		       const u8 *param, size_t param_len)
+{
+	u8 pipe;
+
+	pr_debug("%d to gate %d\n", event, gate);
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_EVENT, event,
+				      param, param_len, NULL, NULL, 0);
+}
+EXPORT_SYMBOL(nfc_hci_send_event);
+
+int nfc_hci_send_response(struct nfc_hci_dev *hdev, u8 gate, u8 response,
+			  const u8 *param, size_t param_len)
+{
+	u8 pipe;
+
+	pr_debug("\n");
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_RESPONSE,
+				      response, param, param_len, NULL, NULL,
+				      0);
+}
+EXPORT_SYMBOL(nfc_hci_send_response);
+
+/*
+ * Execute an hci command sent to gate.
+ * skb will contain response data if success. skb can be NULL if you are not
+ * interested by the response.
+ */
+int nfc_hci_send_cmd(struct nfc_hci_dev *hdev, u8 gate, u8 cmd,
+		     const u8 *param, size_t param_len, struct sk_buff **skb)
+{
+	u8 pipe;
+
+	pr_debug("\n");
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_execute_cmd(hdev, pipe, cmd, param, param_len, skb);
+}
+EXPORT_SYMBOL(nfc_hci_send_cmd);
+
+int nfc_hci_send_cmd_async(struct nfc_hci_dev *hdev, u8 gate, u8 cmd,
+			   const u8 *param, size_t param_len,
+			   data_exchange_cb_t cb, void *cb_context)
+{
+	u8 pipe;
+
+	pr_debug("\n");
+
+	pipe = hdev->gate2pipe[gate];
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	return nfc_hci_execute_cmd_async(hdev, pipe, cmd, param, param_len,
+					 cb, cb_context);
+}
+EXPORT_SYMBOL(nfc_hci_send_cmd_async);
+
+int nfc_hci_set_param(struct nfc_hci_dev *hdev, u8 gate, u8 idx,
+		      const u8 *param, size_t param_len)
+{
+	int r;
+	u8 *tmp;
+
+	/* TODO ELa: reg idx must be inserted before param, but we don't want
+	 * to ask the caller to do it to keep a simpler API.
+	 * For now, just create a new temporary param buffer. This is far from
+	 * optimal though, and the plan is to modify APIs to pass idx down to
+	 * nfc_hci_hcp_message_tx where the frame is actually built, thereby
+	 * eliminating the need for the temp allocation-copy here.
+	 */
+
+	pr_debug("idx=%d to gate %d\n", idx, gate);
+
+	tmp = kmalloc(1 + param_len, GFP_KERNEL);
+	if (tmp == NULL)
+		return -ENOMEM;
+
+	*tmp = idx;
+	memcpy(tmp + 1, param, param_len);
+
+	r = nfc_hci_send_cmd(hdev, gate, NFC_HCI_ANY_SET_PARAMETER,
+			     tmp, param_len + 1, NULL);
+
+	kfree(tmp);
+
+	return r;
+}
+EXPORT_SYMBOL(nfc_hci_set_param);
+
+int nfc_hci_get_param(struct nfc_hci_dev *hdev, u8 gate, u8 idx,
+		      struct sk_buff **skb)
+{
+	pr_debug("gate=%d regidx=%d\n", gate, idx);
+
+	return nfc_hci_send_cmd(hdev, gate, NFC_HCI_ANY_GET_PARAMETER,
+				&idx, 1, skb);
+}
+EXPORT_SYMBOL(nfc_hci_get_param);
+
+static int nfc_hci_open_pipe(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	struct sk_buff *skb;
+	int r;
+
+	pr_debug("pipe=%d\n", pipe);
+
+	r = nfc_hci_execute_cmd(hdev, pipe, NFC_HCI_ANY_OPEN_PIPE,
+				NULL, 0, &skb);
+	if (r == 0) {
+		/* dest host other than host controller will send
+		 * number of pipes already open on this gate before
+		 * execution. The number can be found in skb->data[0]
+		 */
+		kfree_skb(skb);
+	}
+
+	return r;
+}
+
+static int nfc_hci_close_pipe(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	pr_debug("\n");
+
+	return nfc_hci_execute_cmd(hdev, pipe, NFC_HCI_ANY_CLOSE_PIPE,
+				   NULL, 0, NULL);
+}
+
+static u8 nfc_hci_create_pipe(struct nfc_hci_dev *hdev, u8 dest_host,
+			      u8 dest_gate, int *result)
+{
+	struct sk_buff *skb;
+	struct hci_create_pipe_params params;
+	struct hci_create_pipe_resp *resp;
+	u8 pipe;
+
+	pr_debug("gate=%d\n", dest_gate);
+
+	params.src_gate = NFC_HCI_ADMIN_GATE;
+	params.dest_host = dest_host;
+	params.dest_gate = dest_gate;
+
+	*result = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				      NFC_HCI_ADM_CREATE_PIPE,
+				      (u8 *) &params, sizeof(params), &skb);
+	if (*result < 0)
+		return NFC_HCI_INVALID_PIPE;
+
+	resp = (struct hci_create_pipe_resp *)skb->data;
+	pipe = resp->pipe;
+	kfree_skb(skb);
+
+	pr_debug("pipe created=%d\n", pipe);
+
+	return pipe;
+}
+
+static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	pr_debug("\n");
+
+	return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				   NFC_HCI_ADM_DELETE_PIPE, &pipe, 1, NULL);
+}
+
+static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
+{
+	u8 param[2];
+	size_t param_len = 2;
+
+	/* TODO: Find out what the identity reference data is
+	 * and fill param with it. HCI spec 6.1.3.5 */
+
+	pr_debug("\n");
+
+	if (test_bit(NFC_HCI_QUIRK_SHORT_CLEAR, &hdev->quirks))
+		param_len = 0;
+
+	return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+				   NFC_HCI_ADM_CLEAR_ALL_PIPE, param, param_len,
+				   NULL);
+}
+
+int nfc_hci_disconnect_gate(struct nfc_hci_dev *hdev, u8 gate)
+{
+	int r;
+	u8 pipe = hdev->gate2pipe[gate];
+
+	pr_debug("\n");
+
+	if (pipe == NFC_HCI_INVALID_PIPE)
+		return -EADDRNOTAVAIL;
+
+	r = nfc_hci_close_pipe(hdev, pipe);
+	if (r < 0)
+		return r;
+
+	if (pipe != NFC_HCI_LINK_MGMT_PIPE && pipe != NFC_HCI_ADMIN_PIPE) {
+		r = nfc_hci_delete_pipe(hdev, pipe);
+		if (r < 0)
+			return r;
+	}
+
+	hdev->gate2pipe[gate] = NFC_HCI_INVALID_PIPE;
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_hci_disconnect_gate);
+
+int nfc_hci_disconnect_all_gates(struct nfc_hci_dev *hdev)
+{
+	int r;
+
+	pr_debug("\n");
+
+	r = nfc_hci_clear_all_pipes(hdev);
+	if (r < 0)
+		return r;
+
+	memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_hci_disconnect_all_gates);
+
+int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate,
+			 u8 pipe)
+{
+	bool pipe_created = false;
+	int r;
+
+	pr_debug("\n");
+
+	if (hdev->gate2pipe[dest_gate] != NFC_HCI_INVALID_PIPE)
+		return -EADDRINUSE;
+
+	if (pipe != NFC_HCI_INVALID_PIPE)
+		goto open_pipe;
+
+	switch (dest_gate) {
+	case NFC_HCI_LINK_MGMT_GATE:
+		pipe = NFC_HCI_LINK_MGMT_PIPE;
+		break;
+	case NFC_HCI_ADMIN_GATE:
+		pipe = NFC_HCI_ADMIN_PIPE;
+		break;
+	default:
+		pipe = nfc_hci_create_pipe(hdev, dest_host, dest_gate, &r);
+		if (pipe == NFC_HCI_INVALID_PIPE)
+			return r;
+		pipe_created = true;
+		break;
+	}
+
+open_pipe:
+	r = nfc_hci_open_pipe(hdev, pipe);
+	if (r < 0) {
+		if (pipe_created)
+			if (nfc_hci_delete_pipe(hdev, pipe) < 0) {
+				/* TODO: Cannot clean by deleting pipe...
+				 * -> inconsistent state */
+			}
+		return r;
+	}
+
+	hdev->gate2pipe[dest_gate] = pipe;
+
+	return 0;
+}
+EXPORT_SYMBOL(nfc_hci_connect_gate);

net/nfc/hci/core.c

@@ -0,0 +1,992 @@
+/*
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) "hci: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include <net/nfc/nfc.h>
+#include <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+
+#include "hci.h"
+
+/* Largest headroom needed for outgoing HCI commands */
+#define HCI_CMDS_HEADROOM 1
+
+int nfc_hci_result_to_errno(u8 result)
+{
+	switch (result) {
+	case NFC_HCI_ANY_OK:
+		return 0;
+	case NFC_HCI_ANY_E_REG_PAR_UNKNOWN:
+		return -EOPNOTSUPP;
+	case NFC_HCI_ANY_E_TIMEOUT:
+		return -ETIME;
+	default:
+		return -1;
+	}
+}
+EXPORT_SYMBOL(nfc_hci_result_to_errno);
+
+static void nfc_hci_msg_tx_work(struct work_struct *work)
+{
+	struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev,
+						msg_tx_work);
+	struct hci_msg *msg;
+	struct sk_buff *skb;
+	int r = 0;
+
+	mutex_lock(&hdev->msg_tx_mutex);
+	if (hdev->shutting_down)
+		goto exit;
+
+	if (hdev->cmd_pending_msg) {
+		if (timer_pending(&hdev->cmd_timer) == 0) {
+			if (hdev->cmd_pending_msg->cb)
+				hdev->cmd_pending_msg->cb(hdev->
+							  cmd_pending_msg->
+							  cb_context,
+							  NULL,
+							  -ETIME);
+			kfree(hdev->cmd_pending_msg);
+			hdev->cmd_pending_msg = NULL;
+		} else {
+			goto exit;
+		}
+	}
+
+next_msg:
+	if (list_empty(&hdev->msg_tx_queue))
+		goto exit;
+
+	msg = list_first_entry(&hdev->msg_tx_queue, struct hci_msg, msg_l);
+	list_del(&msg->msg_l);
+
+	pr_debug("msg_tx_queue has a cmd to send\n");
+	while ((skb = skb_dequeue(&msg->msg_frags)) != NULL) {
+		r = nfc_llc_xmit_from_hci(hdev->llc, skb);
+		if (r < 0) {
+			kfree_skb(skb);
+			skb_queue_purge(&msg->msg_frags);
+			if (msg->cb)
+				msg->cb(msg->cb_context, NULL, r);
+			kfree(msg);
+			break;
+		}
+	}
+
+	if (r)
+		goto next_msg;
+
+	if (msg->wait_response == false) {
+		kfree(msg);
+		goto next_msg;
+	}
+
+	hdev->cmd_pending_msg = msg;
+	mod_timer(&hdev->cmd_timer, jiffies +
+		  msecs_to_jiffies(hdev->cmd_pending_msg->completion_delay));
+
+exit:
+	mutex_unlock(&hdev->msg_tx_mutex);
+}
+
+static void nfc_hci_msg_rx_work(struct work_struct *work)
+{
+	struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev,
+						msg_rx_work);
+	struct sk_buff *skb;
+	struct hcp_message *message;
+	u8 pipe;
+	u8 type;
+	u8 instruction;
+
+	while ((skb = skb_dequeue(&hdev->msg_rx_queue)) != NULL) {
+		pipe = skb->data[0];
+		skb_pull(skb, NFC_HCI_HCP_PACKET_HEADER_LEN);
+		message = (struct hcp_message *)skb->data;
+		type = HCP_MSG_GET_TYPE(message->header);
+		instruction = HCP_MSG_GET_CMD(message->header);
+		skb_pull(skb, NFC_HCI_HCP_MESSAGE_HEADER_LEN);
+
+		nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, skb);
+	}
+}
+
+static void __nfc_hci_cmd_completion(struct nfc_hci_dev *hdev, int err,
+				     struct sk_buff *skb)
+{
+	del_timer_sync(&hdev->cmd_timer);
+
+	if (hdev->cmd_pending_msg->cb)
+		hdev->cmd_pending_msg->cb(hdev->cmd_pending_msg->cb_context,
+					  skb, err);
+	else
+		kfree_skb(skb);
+
+	kfree(hdev->cmd_pending_msg);
+	hdev->cmd_pending_msg = NULL;
+
+	schedule_work(&hdev->msg_tx_work);
+}
+
+void nfc_hci_resp_received(struct nfc_hci_dev *hdev, u8 result,
+			   struct sk_buff *skb)
+{
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->cmd_pending_msg == NULL) {
+		kfree_skb(skb);
+		goto exit;
+	}
+
+	__nfc_hci_cmd_completion(hdev, nfc_hci_result_to_errno(result), skb);
+
+exit:
+	mutex_unlock(&hdev->msg_tx_mutex);
+}
+
+void nfc_hci_cmd_received(struct nfc_hci_dev *hdev, u8 pipe, u8 cmd,
+			  struct sk_buff *skb)
+{
+	kfree_skb(skb);
+}
+
+u32 nfc_hci_sak_to_protocol(u8 sak)
+{
+	switch (NFC_HCI_TYPE_A_SEL_PROT(sak)) {
+	case NFC_HCI_TYPE_A_SEL_PROT_MIFARE:
+		return NFC_PROTO_MIFARE_MASK;
+	case NFC_HCI_TYPE_A_SEL_PROT_ISO14443:
+		return NFC_PROTO_ISO14443_MASK;
+	case NFC_HCI_TYPE_A_SEL_PROT_DEP:
+		return NFC_PROTO_NFC_DEP_MASK;
+	case NFC_HCI_TYPE_A_SEL_PROT_ISO14443_DEP:
+		return NFC_PROTO_ISO14443_MASK | NFC_PROTO_NFC_DEP_MASK;
+	default:
+		return 0xffffffff;
+	}
+}
+EXPORT_SYMBOL(nfc_hci_sak_to_protocol);
+
+int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
+{
+	struct nfc_target *targets;
+	struct sk_buff *atqa_skb = NULL;
+	struct sk_buff *sak_skb = NULL;
+	struct sk_buff *uid_skb = NULL;
+	int r;
+
+	pr_debug("from gate %d\n", gate);
+
+	targets = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
+	if (targets == NULL)
+		return -ENOMEM;
+
+	switch (gate) {
+	case NFC_HCI_RF_READER_A_GATE:
+		r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
+				      NFC_HCI_RF_READER_A_ATQA, &atqa_skb);
+		if (r < 0)
+			goto exit;
+
+		r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
+				      NFC_HCI_RF_READER_A_SAK, &sak_skb);
+		if (r < 0)
+			goto exit;
+
+		if (atqa_skb->len != 2 || sak_skb->len != 1) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		targets->supported_protocols =
+				nfc_hci_sak_to_protocol(sak_skb->data[0]);
+		if (targets->supported_protocols == 0xffffffff) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		targets->sens_res = be16_to_cpu(*(__be16 *)atqa_skb->data);
+		targets->sel_res = sak_skb->data[0];
+
+		r = nfc_hci_get_param(hdev, NFC_HCI_RF_READER_A_GATE,
+				      NFC_HCI_RF_READER_A_UID, &uid_skb);
+		if (r < 0)
+			goto exit;
+
+		if (uid_skb->len == 0 || uid_skb->len > NFC_NFCID1_MAXSIZE) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		memcpy(targets->nfcid1, uid_skb->data, uid_skb->len);
+		targets->nfcid1_len = uid_skb->len;
+
+		if (hdev->ops->complete_target_discovered) {
+			r = hdev->ops->complete_target_discovered(hdev, gate,
+								  targets);
+			if (r < 0)
+				goto exit;
+		}
+		break;
+	case NFC_HCI_RF_READER_B_GATE:
+		targets->supported_protocols = NFC_PROTO_ISO14443_B_MASK;
+		break;
+	default:
+		if (hdev->ops->target_from_gate)
+			r = hdev->ops->target_from_gate(hdev, gate, targets);
+		else
+			r = -EPROTO;
+		if (r < 0)
+			goto exit;
+
+		if (hdev->ops->complete_target_discovered) {
+			r = hdev->ops->complete_target_discovered(hdev, gate,
+								  targets);
+			if (r < 0)
+				goto exit;
+		}
+		break;
+	}
+
+	/* if driver set the new gate, we will skip the old one */
+	if (targets->hci_reader_gate == 0x00)
+		targets->hci_reader_gate = gate;
+
+	r = nfc_targets_found(hdev->ndev, targets, 1);
+
+exit:
+	kfree(targets);
+	kfree_skb(atqa_skb);
+	kfree_skb(sak_skb);
+	kfree_skb(uid_skb);
+
+	return r;
+}
+EXPORT_SYMBOL(nfc_hci_target_discovered);
+
+void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
+			    struct sk_buff *skb)
+{
+	int r = 0;
+	u8 gate = nfc_hci_pipe2gate(hdev, pipe);
+
+	if (gate == 0xff) {
+		pr_err("Discarded event %x to unopened pipe %x\n", event, pipe);
+		goto exit;
+	}
+
+	if (hdev->ops->event_received) {
+		r = hdev->ops->event_received(hdev, gate, event, skb);
+		if (r <= 0)
+			goto exit_noskb;
+	}
+
+	switch (event) {
+	case NFC_HCI_EVT_TARGET_DISCOVERED:
+		if (skb->len < 1) {	/* no status data? */
+			r = -EPROTO;
+			goto exit;
+		}
+
+		if (skb->data[0] == 3) {
+			/* TODO: Multiple targets in field, none activated
+			 * poll is supposedly stopped, but there is no
+			 * single target to activate, so nothing to report
+			 * up.
+			 * if we need to restart poll, we must save the
+			 * protocols from the initial poll and reuse here.
+			 */
+		}
+
+		if (skb->data[0] != 0) {
+			r = -EPROTO;
+			goto exit;
+		}
+
+		r = nfc_hci_target_discovered(hdev, gate);
+		break;
+	default:
+		pr_info("Discarded unknown event %x to gate %x\n", event, gate);
+		r = -EINVAL;
+		break;
+	}
+
+exit:
+	kfree_skb(skb);
+
+exit_noskb:
+	if (r)
+		nfc_hci_driver_failure(hdev, r);
+}
+
+static void nfc_hci_cmd_timeout(unsigned long data)
+{
+	struct nfc_hci_dev *hdev = (struct nfc_hci_dev *)data;
+
+	schedule_work(&hdev->msg_tx_work);
+}
+
+static int hci_dev_connect_gates(struct nfc_hci_dev *hdev, u8 gate_count,
+				 struct nfc_hci_gate *gates)
+{
+	int r;
+	while (gate_count--) {
+		r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID,
+					 gates->gate, gates->pipe);
+		if (r < 0)
+			return r;
+		gates++;
+	}
+
+	return 0;
+}
+
+static int hci_dev_session_init(struct nfc_hci_dev *hdev)
+{
+	struct sk_buff *skb = NULL;
+	int r;
+
+	if (hdev->init_data.gates[0].gate != NFC_HCI_ADMIN_GATE)
+		return -EPROTO;
+
+	r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID,
+				 hdev->init_data.gates[0].gate,
+				 hdev->init_data.gates[0].pipe);
+	if (r < 0)
+		goto exit;
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ADMIN_GATE,
+			      NFC_HCI_ADMIN_SESSION_IDENTITY, &skb);
+	if (r < 0)
+		goto disconnect_all;
+
+	if (skb->len && skb->len == strlen(hdev->init_data.session_id) &&
+		(memcmp(hdev->init_data.session_id, skb->data,
+			   skb->len) == 0) && hdev->ops->load_session) {
+		/* Restore gate<->pipe table from some proprietary location. */
+
+		r = hdev->ops->load_session(hdev);
+
+		if (r < 0)
+			goto disconnect_all;
+	} else {
+
+		r = nfc_hci_disconnect_all_gates(hdev);
+		if (r < 0)
+			goto exit;
+
+		r = hci_dev_connect_gates(hdev, hdev->init_data.gate_count,
+					  hdev->init_data.gates);
+		if (r < 0)
+			goto disconnect_all;
+
+		r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE,
+				NFC_HCI_ADMIN_SESSION_IDENTITY,
+				hdev->init_data.session_id,
+				strlen(hdev->init_data.session_id));
+	}
+	if (r == 0)
+		goto exit;
+
+disconnect_all:
+	nfc_hci_disconnect_all_gates(hdev);
+
+exit:
+	kfree_skb(skb);
+
+	return r;
+}
+
+static int hci_dev_version(struct nfc_hci_dev *hdev)
+{
+	int r;
+	struct sk_buff *skb;
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
+			      NFC_HCI_ID_MGMT_VERSION_SW, &skb);
+	if (r == -EOPNOTSUPP) {
+		pr_info("Software/Hardware info not available\n");
+		return 0;
+	}
+	if (r < 0)
+		return r;
+
+	if (skb->len != 3) {
+		kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	hdev->sw_romlib = (skb->data[0] & 0xf0) >> 4;
+	hdev->sw_patch = skb->data[0] & 0x0f;
+	hdev->sw_flashlib_major = skb->data[1];
+	hdev->sw_flashlib_minor = skb->data[2];
+
+	kfree_skb(skb);
+
+	r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE,
+			      NFC_HCI_ID_MGMT_VERSION_HW, &skb);
+	if (r < 0)
+		return r;
+
+	if (skb->len != 3) {
+		kfree_skb(skb);
+		return -EINVAL;
+	}
+
+	hdev->hw_derivative = (skb->data[0] & 0xe0) >> 5;
+	hdev->hw_version = skb->data[0] & 0x1f;
+	hdev->hw_mpw = (skb->data[1] & 0xc0) >> 6;
+	hdev->hw_software = skb->data[1] & 0x3f;
+	hdev->hw_bsid = skb->data[2];
+
+	kfree_skb(skb);
+
+	pr_info("SOFTWARE INFO:\n");
+	pr_info("RomLib         : %d\n", hdev->sw_romlib);
+	pr_info("Patch          : %d\n", hdev->sw_patch);
+	pr_info("FlashLib Major : %d\n", hdev->sw_flashlib_major);
+	pr_info("FlashLib Minor : %d\n", hdev->sw_flashlib_minor);
+	pr_info("HARDWARE INFO:\n");
+	pr_info("Derivative     : %d\n", hdev->hw_derivative);
+	pr_info("HW Version     : %d\n", hdev->hw_version);
+	pr_info("#MPW           : %d\n", hdev->hw_mpw);
+	pr_info("Software       : %d\n", hdev->hw_software);
+	pr_info("BSID Version   : %d\n", hdev->hw_bsid);
+
+	return 0;
+}
+
+static int hci_dev_up(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+	int r = 0;
+
+	if (hdev->ops->open) {
+		r = hdev->ops->open(hdev);
+		if (r < 0)
+			return r;
+	}
+
+	r = nfc_llc_start(hdev->llc);
+	if (r < 0)
+		goto exit_close;
+
+	r = hci_dev_session_init(hdev);
+	if (r < 0)
+		goto exit_llc;
+
+	r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+			       NFC_HCI_EVT_END_OPERATION, NULL, 0);
+	if (r < 0)
+		goto exit_llc;
+
+	if (hdev->ops->hci_ready) {
+		r = hdev->ops->hci_ready(hdev);
+		if (r < 0)
+			goto exit_llc;
+	}
+
+	r = hci_dev_version(hdev);
+	if (r < 0)
+		goto exit_llc;
+
+	return 0;
+
+exit_llc:
+	nfc_llc_stop(hdev->llc);
+
+exit_close:
+	if (hdev->ops->close)
+		hdev->ops->close(hdev);
+
+	return r;
+}
+
+static int hci_dev_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	nfc_llc_stop(hdev->llc);
+
+	if (hdev->ops->close)
+		hdev->ops->close(hdev);
+
+	memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+
+	return 0;
+}
+
+static int hci_start_poll(struct nfc_dev *nfc_dev,
+			  u32 im_protocols, u32 tm_protocols)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->start_poll)
+		return hdev->ops->start_poll(hdev, im_protocols, tm_protocols);
+	else
+		return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+					  NFC_HCI_EVT_READER_REQUESTED,
+					  NULL, 0);
+}
+
+static void hci_stop_poll(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->stop_poll)
+		hdev->ops->stop_poll(hdev);
+	else
+		nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
+				   NFC_HCI_EVT_END_OPERATION, NULL, 0);
+}
+
+static int hci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+				__u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (!hdev->ops->dep_link_up)
+		return 0;
+
+	return hdev->ops->dep_link_up(hdev, target, comm_mode,
+				      gb, gb_len);
+}
+
+static int hci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (!hdev->ops->dep_link_down)
+		return 0;
+
+	return hdev->ops->dep_link_down(hdev);
+}
+
+static int hci_activate_target(struct nfc_dev *nfc_dev,
+			       struct nfc_target *target, u32 protocol)
+{
+	return 0;
+}
+
+static void hci_deactivate_target(struct nfc_dev *nfc_dev,
+				  struct nfc_target *target)
+{
+}
+
+#define HCI_CB_TYPE_TRANSCEIVE 1
+
+static void hci_transceive_cb(void *context, struct sk_buff *skb, int err)
+{
+	struct nfc_hci_dev *hdev = context;
+
+	switch (hdev->async_cb_type) {
+	case HCI_CB_TYPE_TRANSCEIVE:
+		/*
+		 * TODO: Check RF Error indicator to make sure data is valid.
+		 * It seems that HCI cmd can complete without error, but data
+		 * can be invalid if an RF error occured? Ignore for now.
+		 */
+		if (err == 0)
+			skb_trim(skb, skb->len - 1); /* RF Err ind */
+
+		hdev->async_cb(hdev->async_cb_context, skb, err);
+		break;
+	default:
+		if (err == 0)
+			kfree_skb(skb);
+		break;
+	}
+}
+
+static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
+			  struct sk_buff *skb, data_exchange_cb_t cb,
+			  void *cb_context)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+	int r;
+
+	pr_debug("target_idx=%d\n", target->idx);
+
+	switch (target->hci_reader_gate) {
+	case NFC_HCI_RF_READER_A_GATE:
+	case NFC_HCI_RF_READER_B_GATE:
+		if (hdev->ops->im_transceive) {
+			r = hdev->ops->im_transceive(hdev, target, skb, cb,
+						     cb_context);
+			if (r <= 0)	/* handled */
+				break;
+		}
+
+		*skb_push(skb, 1) = 0;	/* CTR, see spec:10.2.2.1 */
+
+		hdev->async_cb_type = HCI_CB_TYPE_TRANSCEIVE;
+		hdev->async_cb = cb;
+		hdev->async_cb_context = cb_context;
+
+		r = nfc_hci_send_cmd_async(hdev, target->hci_reader_gate,
+					   NFC_HCI_WR_XCHG_DATA, skb->data,
+					   skb->len, hci_transceive_cb, hdev);
+		break;
+	default:
+		if (hdev->ops->im_transceive) {
+			r = hdev->ops->im_transceive(hdev, target, skb, cb,
+						     cb_context);
+			if (r == 1)
+				r = -ENOTSUPP;
+		} else {
+			r = -ENOTSUPP;
+		}
+		break;
+	}
+
+	kfree_skb(skb);
+
+	return r;
+}
+
+static int hci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (!hdev->ops->tm_send) {
+		kfree_skb(skb);
+		return -ENOTSUPP;
+	}
+
+	return hdev->ops->tm_send(hdev, skb);
+}
+
+static int hci_check_presence(struct nfc_dev *nfc_dev,
+			      struct nfc_target *target)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (!hdev->ops->check_presence)
+		return 0;
+
+	return hdev->ops->check_presence(hdev, target);
+}
+
+static int hci_discover_se(struct nfc_dev *nfc_dev)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->discover_se)
+		return hdev->ops->discover_se(hdev);
+
+	return 0;
+}
+
+static int hci_enable_se(struct nfc_dev *nfc_dev, u32 se_idx)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->enable_se)
+		return hdev->ops->enable_se(hdev, se_idx);
+
+	return 0;
+}
+
+static int hci_disable_se(struct nfc_dev *nfc_dev, u32 se_idx)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (hdev->ops->disable_se)
+		return hdev->ops->disable_se(hdev, se_idx);
+
+	return 0;
+}
+
+static void nfc_hci_failure(struct nfc_hci_dev *hdev, int err)
+{
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->cmd_pending_msg == NULL) {
+		nfc_driver_failure(hdev->ndev, err);
+		goto exit;
+	}
+
+	__nfc_hci_cmd_completion(hdev, err, NULL);
+
+exit:
+	mutex_unlock(&hdev->msg_tx_mutex);
+}
+
+static void nfc_hci_llc_failure(struct nfc_hci_dev *hdev, int err)
+{
+	nfc_hci_failure(hdev, err);
+}
+
+static void nfc_hci_recv_from_llc(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+	struct hcp_packet *packet;
+	u8 type;
+	u8 instruction;
+	struct sk_buff *hcp_skb;
+	u8 pipe;
+	struct sk_buff *frag_skb;
+	int msg_len;
+
+	packet = (struct hcp_packet *)skb->data;
+	if ((packet->header & ~NFC_HCI_FRAGMENT) == 0) {
+		skb_queue_tail(&hdev->rx_hcp_frags, skb);
+		return;
+	}
+
+	/* it's the last fragment. Does it need re-aggregation? */
+	if (skb_queue_len(&hdev->rx_hcp_frags)) {
+		pipe = packet->header & NFC_HCI_FRAGMENT;
+		skb_queue_tail(&hdev->rx_hcp_frags, skb);
+
+		msg_len = 0;
+		skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
+			msg_len += (frag_skb->len -
+				    NFC_HCI_HCP_PACKET_HEADER_LEN);
+		}
+
+		hcp_skb = nfc_alloc_recv_skb(NFC_HCI_HCP_PACKET_HEADER_LEN +
+					     msg_len, GFP_KERNEL);
+		if (hcp_skb == NULL) {
+			nfc_hci_failure(hdev, -ENOMEM);
+			return;
+		}
+
+		*skb_put(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN) = pipe;
+
+		skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
+			msg_len = frag_skb->len - NFC_HCI_HCP_PACKET_HEADER_LEN;
+			memcpy(skb_put(hcp_skb, msg_len),
+			       frag_skb->data + NFC_HCI_HCP_PACKET_HEADER_LEN,
+			       msg_len);
+		}
+
+		skb_queue_purge(&hdev->rx_hcp_frags);
+	} else {
+		packet->header &= NFC_HCI_FRAGMENT;
+		hcp_skb = skb;
+	}
+
+	/* if this is a response, dispatch immediately to
+	 * unblock waiting cmd context. Otherwise, enqueue to dispatch
+	 * in separate context where handler can also execute command.
+	 */
+	packet = (struct hcp_packet *)hcp_skb->data;
+	type = HCP_MSG_GET_TYPE(packet->message.header);
+	if (type == NFC_HCI_HCP_RESPONSE) {
+		pipe = packet->header;
+		instruction = HCP_MSG_GET_CMD(packet->message.header);
+		skb_pull(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN +
+			 NFC_HCI_HCP_MESSAGE_HEADER_LEN);
+		nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, hcp_skb);
+	} else {
+		skb_queue_tail(&hdev->msg_rx_queue, hcp_skb);
+		schedule_work(&hdev->msg_rx_work);
+	}
+}
+
+static int hci_fw_download(struct nfc_dev *nfc_dev, const char *firmware_name)
+{
+	struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+	if (!hdev->ops->fw_download)
+		return -ENOTSUPP;
+
+	return hdev->ops->fw_download(hdev, firmware_name);
+}
+
+static struct nfc_ops hci_nfc_ops = {
+	.dev_up = hci_dev_up,
+	.dev_down = hci_dev_down,
+	.start_poll = hci_start_poll,
+	.stop_poll = hci_stop_poll,
+	.dep_link_up = hci_dep_link_up,
+	.dep_link_down = hci_dep_link_down,
+	.activate_target = hci_activate_target,
+	.deactivate_target = hci_deactivate_target,
+	.im_transceive = hci_transceive,
+	.tm_send = hci_tm_send,
+	.check_presence = hci_check_presence,
+	.fw_download = hci_fw_download,
+	.discover_se = hci_discover_se,
+	.enable_se = hci_enable_se,
+	.disable_se = hci_disable_se,
+};
+
+struct nfc_hci_dev *nfc_hci_allocate_device(struct nfc_hci_ops *ops,
+					    struct nfc_hci_init_data *init_data,
+					    unsigned long quirks,
+					    u32 protocols,
+					    const char *llc_name,
+					    int tx_headroom,
+					    int tx_tailroom,
+					    int max_link_payload)
+{
+	struct nfc_hci_dev *hdev;
+
+	if (ops->xmit == NULL)
+		return NULL;
+
+	if (protocols == 0)
+		return NULL;
+
+	hdev = kzalloc(sizeof(struct nfc_hci_dev), GFP_KERNEL);
+	if (hdev == NULL)
+		return NULL;
+
+	hdev->llc = nfc_llc_allocate(llc_name, hdev, ops->xmit,
+				     nfc_hci_recv_from_llc, tx_headroom,
+				     tx_tailroom, nfc_hci_llc_failure);
+	if (hdev->llc == NULL) {
+		kfree(hdev);
+		return NULL;
+	}
+
+	hdev->ndev = nfc_allocate_device(&hci_nfc_ops, protocols,
+					 tx_headroom + HCI_CMDS_HEADROOM,
+					 tx_tailroom);
+	if (!hdev->ndev) {
+		nfc_llc_free(hdev->llc);
+		kfree(hdev);
+		return NULL;
+	}
+
+	hdev->ops = ops;
+	hdev->max_data_link_payload = max_link_payload;
+	hdev->init_data = *init_data;
+
+	nfc_set_drvdata(hdev->ndev, hdev);
+
+	memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+
+	hdev->quirks = quirks;
+
+	return hdev;
+}
+EXPORT_SYMBOL(nfc_hci_allocate_device);
+
+void nfc_hci_free_device(struct nfc_hci_dev *hdev)
+{
+	nfc_free_device(hdev->ndev);
+	nfc_llc_free(hdev->llc);
+	kfree(hdev);
+}
+EXPORT_SYMBOL(nfc_hci_free_device);
+
+int nfc_hci_register_device(struct nfc_hci_dev *hdev)
+{
+	mutex_init(&hdev->msg_tx_mutex);
+
+	INIT_LIST_HEAD(&hdev->msg_tx_queue);
+
+	INIT_WORK(&hdev->msg_tx_work, nfc_hci_msg_tx_work);
+
+	init_timer(&hdev->cmd_timer);
+	hdev->cmd_timer.data = (unsigned long)hdev;
+	hdev->cmd_timer.function = nfc_hci_cmd_timeout;
+
+	skb_queue_head_init(&hdev->rx_hcp_frags);
+
+	INIT_WORK(&hdev->msg_rx_work, nfc_hci_msg_rx_work);
+
+	skb_queue_head_init(&hdev->msg_rx_queue);
+
+	return nfc_register_device(hdev->ndev);
+}
+EXPORT_SYMBOL(nfc_hci_register_device);
+
+void nfc_hci_unregister_device(struct nfc_hci_dev *hdev)
+{
+	struct hci_msg *msg, *n;
+
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->cmd_pending_msg) {
+		if (hdev->cmd_pending_msg->cb)
+			hdev->cmd_pending_msg->cb(
+					     hdev->cmd_pending_msg->cb_context,
+					     NULL, -ESHUTDOWN);
+		kfree(hdev->cmd_pending_msg);
+		hdev->cmd_pending_msg = NULL;
+	}
+
+	hdev->shutting_down = true;
+
+	mutex_unlock(&hdev->msg_tx_mutex);
+
+	del_timer_sync(&hdev->cmd_timer);
+	cancel_work_sync(&hdev->msg_tx_work);
+
+	cancel_work_sync(&hdev->msg_rx_work);
+
+	nfc_unregister_device(hdev->ndev);
+
+	skb_queue_purge(&hdev->rx_hcp_frags);
+	skb_queue_purge(&hdev->msg_rx_queue);
+
+	list_for_each_entry_safe(msg, n, &hdev->msg_tx_queue, msg_l) {
+		list_del(&msg->msg_l);
+		skb_queue_purge(&msg->msg_frags);
+		kfree(msg);
+	}
+}
+EXPORT_SYMBOL(nfc_hci_unregister_device);
+
+void nfc_hci_set_clientdata(struct nfc_hci_dev *hdev, void *clientdata)
+{
+	hdev->clientdata = clientdata;
+}
+EXPORT_SYMBOL(nfc_hci_set_clientdata);
+
+void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev)
+{
+	return hdev->clientdata;
+}
+EXPORT_SYMBOL(nfc_hci_get_clientdata);
+
+void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err)
+{
+	nfc_hci_failure(hdev, err);
+}
+EXPORT_SYMBOL(nfc_hci_driver_failure);
+
+void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+	nfc_llc_rcv_from_drv(hdev->llc, skb);
+}
+EXPORT_SYMBOL(nfc_hci_recv_frame);
+
+static int __init nfc_hci_init(void)
+{
+	return nfc_llc_init();
+}
+
+static void __exit nfc_hci_exit(void)
+{
+	nfc_llc_exit();
+}
+
+subsys_initcall(nfc_hci_init);
+module_exit(nfc_hci_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NFC HCI Core");

net/nfc/hci/hci.h

@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __LOCAL_HCI_H
+#define __LOCAL_HCI_H
+
+#include <net/nfc/hci.h>
+
+struct gate_pipe_map {
+	u8 gate;
+	u8 pipe;
+};
+
+struct hcp_message {
+	u8 header;		/* type -cmd,evt,rsp- + instruction */
+	u8 data[];
+} __packed;
+
+struct hcp_packet {
+	u8 header;		/* cbit+pipe */
+	struct hcp_message message;
+} __packed;
+
+struct hcp_exec_waiter {
+	wait_queue_head_t *wq;
+	bool exec_complete;
+	int exec_result;
+	struct sk_buff *result_skb;
+};
+
+struct hci_msg {
+	struct list_head msg_l;
+	struct sk_buff_head msg_frags;
+	bool wait_response;
+	data_exchange_cb_t cb;
+	void *cb_context;
+	unsigned long completion_delay;
+};
+
+struct hci_create_pipe_params {
+	u8 src_gate;
+	u8 dest_host;
+	u8 dest_gate;
+} __packed;
+
+struct hci_create_pipe_resp {
+	u8 src_host;
+	u8 src_gate;
+	u8 dest_host;
+	u8 dest_gate;
+	u8 pipe;
+} __packed;
+
+#define NFC_HCI_FRAGMENT	0x7f
+
+#define HCP_HEADER(type, instr) ((((type) & 0x03) << 6) | ((instr) & 0x3f))
+#define HCP_MSG_GET_TYPE(header) ((header & 0xc0) >> 6)
+#define HCP_MSG_GET_CMD(header) (header & 0x3f)
+
+int nfc_hci_hcp_message_tx(struct nfc_hci_dev *hdev, u8 pipe,
+			   u8 type, u8 instruction,
+			   const u8 *payload, size_t payload_len,
+			   data_exchange_cb_t cb, void *cb_context,
+			   unsigned long completion_delay);
+
+u8 nfc_hci_pipe2gate(struct nfc_hci_dev *hdev, u8 pipe);
+
+void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type,
+			    u8 instruction, struct sk_buff *skb);
+
+/* HCP headers */
+#define NFC_HCI_HCP_PACKET_HEADER_LEN	1
+#define NFC_HCI_HCP_MESSAGE_HEADER_LEN	1
+#define NFC_HCI_HCP_HEADER_LEN		2
+
+/* HCP types */
+#define NFC_HCI_HCP_COMMAND	0x00
+#define NFC_HCI_HCP_EVENT	0x01
+#define NFC_HCI_HCP_RESPONSE	0x02
+
+/* Generic commands */
+#define NFC_HCI_ANY_SET_PARAMETER	0x01
+#define NFC_HCI_ANY_GET_PARAMETER	0x02
+#define NFC_HCI_ANY_OPEN_PIPE		0x03
+#define NFC_HCI_ANY_CLOSE_PIPE		0x04
+
+/* Reader RF commands */
+#define NFC_HCI_WR_XCHG_DATA		0x10
+
+/* Admin commands */
+#define NFC_HCI_ADM_CREATE_PIPE			0x10
+#define NFC_HCI_ADM_DELETE_PIPE			0x11
+#define NFC_HCI_ADM_NOTIFY_PIPE_CREATED		0x12
+#define NFC_HCI_ADM_NOTIFY_PIPE_DELETED		0x13
+#define NFC_HCI_ADM_CLEAR_ALL_PIPE		0x14
+#define NFC_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED	0x15
+
+/* Generic responses */
+#define NFC_HCI_ANY_OK				0x00
+#define NFC_HCI_ANY_E_NOT_CONNECTED		0x01
+#define NFC_HCI_ANY_E_CMD_PAR_UNKNOWN		0x02
+#define NFC_HCI_ANY_E_NOK			0x03
+#define NFC_HCI_ANY_E_PIPES_FULL		0x04
+#define NFC_HCI_ANY_E_REG_PAR_UNKNOWN		0x05
+#define NFC_HCI_ANY_E_PIPE_NOT_OPENED		0x06
+#define NFC_HCI_ANY_E_CMD_NOT_SUPPORTED		0x07
+#define NFC_HCI_ANY_E_INHIBITED			0x08
+#define NFC_HCI_ANY_E_TIMEOUT			0x09
+#define NFC_HCI_ANY_E_REG_ACCESS_DENIED		0x0a
+#define NFC_HCI_ANY_E_PIPE_ACCESS_DENIED	0x0b
+
+#endif /* __LOCAL_HCI_H */

net/nfc/hci/hcp.c

@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) "hci: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include <net/nfc/hci.h>
+
+#include "hci.h"
+
+/*
+ * Payload is the HCP message data only. Instruction will be prepended.
+ * Guarantees that cb will be called upon completion or timeout delay
+ * counted from the moment the cmd is sent to the transport.
+ */
+int nfc_hci_hcp_message_tx(struct nfc_hci_dev *hdev, u8 pipe,
+			   u8 type, u8 instruction,
+			   const u8 *payload, size_t payload_len,
+			   data_exchange_cb_t cb, void *cb_context,
+			   unsigned long completion_delay)
+{
+	struct nfc_dev *ndev = hdev->ndev;
+	struct hci_msg *cmd;
+	const u8 *ptr = payload;
+	int hci_len, err;
+	bool firstfrag = true;
+
+	cmd = kzalloc(sizeof(struct hci_msg), GFP_KERNEL);
+	if (cmd == NULL)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&cmd->msg_l);
+	skb_queue_head_init(&cmd->msg_frags);
+	cmd->wait_response = (type == NFC_HCI_HCP_COMMAND) ? true : false;
+	cmd->cb = cb;
+	cmd->cb_context = cb_context;
+	cmd->completion_delay = completion_delay;
+
+	hci_len = payload_len + 1;
+	while (hci_len > 0) {
+		struct sk_buff *skb;
+		int skb_len, data_link_len;
+		struct hcp_packet *packet;
+
+		if (NFC_HCI_HCP_PACKET_HEADER_LEN + hci_len <=
+		    hdev->max_data_link_payload)
+			data_link_len = hci_len;
+		else
+			data_link_len = hdev->max_data_link_payload -
+					NFC_HCI_HCP_PACKET_HEADER_LEN;
+
+		skb_len = ndev->tx_headroom + NFC_HCI_HCP_PACKET_HEADER_LEN +
+			  data_link_len + ndev->tx_tailroom;
+		hci_len -= data_link_len;
+
+		skb = alloc_skb(skb_len, GFP_KERNEL);
+		if (skb == NULL) {
+			err = -ENOMEM;
+			goto out_skb_err;
+		}
+		skb_reserve(skb, ndev->tx_headroom);
+
+		skb_put(skb, NFC_HCI_HCP_PACKET_HEADER_LEN + data_link_len);
+
+		/* Only the last fragment will have the cb bit set to 1 */
+		packet = (struct hcp_packet *)skb->data;
+		packet->header = pipe;
+		if (firstfrag) {
+			firstfrag = false;
+			packet->message.header = HCP_HEADER(type, instruction);
+			if (ptr) {
+				memcpy(packet->message.data, ptr,
+				       data_link_len - 1);
+				ptr += data_link_len - 1;
+			}
+		} else {
+			memcpy(&packet->message, ptr, data_link_len);
+			ptr += data_link_len;
+		}
+
+		/* This is the last fragment, set the cb bit */
+		if (hci_len == 0)
+			packet->header |= ~NFC_HCI_FRAGMENT;
+
+		skb_queue_tail(&cmd->msg_frags, skb);
+	}
+
+	mutex_lock(&hdev->msg_tx_mutex);
+
+	if (hdev->shutting_down) {
+		err = -ESHUTDOWN;
+		mutex_unlock(&hdev->msg_tx_mutex);
+		goto out_skb_err;
+	}
+
+	list_add_tail(&cmd->msg_l, &hdev->msg_tx_queue);
+	mutex_unlock(&hdev->msg_tx_mutex);
+
+	schedule_work(&hdev->msg_tx_work);
+
+	return 0;
+
+out_skb_err:
+	skb_queue_purge(&cmd->msg_frags);
+	kfree(cmd);
+
+	return err;
+}
+
+u8 nfc_hci_pipe2gate(struct nfc_hci_dev *hdev, u8 pipe)
+{
+	int gate;
+
+	for (gate = 0; gate < NFC_HCI_MAX_GATES; gate++)
+		if (hdev->gate2pipe[gate] == pipe)
+			return gate;
+
+	return 0xff;
+}
+
+/*
+ * Receive hcp message for pipe, with type and cmd.
+ * skb contains optional message data only.
+ */
+void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type,
+			    u8 instruction, struct sk_buff *skb)
+{
+	switch (type) {
+	case NFC_HCI_HCP_RESPONSE:
+		nfc_hci_resp_received(hdev, instruction, skb);
+		break;
+	case NFC_HCI_HCP_COMMAND:
+		nfc_hci_cmd_received(hdev, pipe, instruction, skb);
+		break;
+	case NFC_HCI_HCP_EVENT:
+		nfc_hci_event_received(hdev, pipe, instruction, skb);
+		break;
+	default:
+		pr_err("UNKNOWN MSG Type %d, instruction=%d\n",
+		       type, instruction);
+		kfree_skb(skb);
+		break;
+	}
+}

net/nfc/hci/llc.c

@@ -0,0 +1,166 @@
+/*
+ * Link Layer Control manager
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <net/nfc/llc.h>
+
+#include "llc.h"
+
+static LIST_HEAD(llc_engines);
+
+int nfc_llc_init(void)
+{
+	int r;
+
+	r = nfc_llc_nop_register();
+	if (r)
+		goto exit;
+
+	r = nfc_llc_shdlc_register();
+	if (r)
+		goto exit;
+
+	return 0;
+
+exit:
+	nfc_llc_exit();
+	return r;
+}
+
+void nfc_llc_exit(void)
+{
+	struct nfc_llc_engine *llc_engine, *n;
+
+	list_for_each_entry_safe(llc_engine, n, &llc_engines, entry) {
+		list_del(&llc_engine->entry);
+		kfree(llc_engine->name);
+		kfree(llc_engine);
+	}
+}
+
+int nfc_llc_register(const char *name, struct nfc_llc_ops *ops)
+{
+	struct nfc_llc_engine *llc_engine;
+
+	llc_engine = kzalloc(sizeof(struct nfc_llc_engine), GFP_KERNEL);
+	if (llc_engine == NULL)
+		return -ENOMEM;
+
+	llc_engine->name = kstrdup(name, GFP_KERNEL);
+	if (llc_engine->name == NULL) {
+		kfree(llc_engine);
+		return -ENOMEM;
+	}
+	llc_engine->ops = ops;
+
+	INIT_LIST_HEAD(&llc_engine->entry);
+	list_add_tail(&llc_engine->entry, &llc_engines);
+
+	return 0;
+}
+
+static struct nfc_llc_engine *nfc_llc_name_to_engine(const char *name)
+{
+	struct nfc_llc_engine *llc_engine;
+
+	list_for_each_entry(llc_engine, &llc_engines, entry) {
+		if (strcmp(llc_engine->name, name) == 0)
+			return llc_engine;
+	}
+
+	return NULL;
+}
+
+void nfc_llc_unregister(const char *name)
+{
+	struct nfc_llc_engine *llc_engine;
+
+	llc_engine = nfc_llc_name_to_engine(name);
+	if (llc_engine == NULL)
+		return;
+
+	list_del(&llc_engine->entry);
+	kfree(llc_engine->name);
+	kfree(llc_engine);
+}
+
+struct nfc_llc *nfc_llc_allocate(const char *name, struct nfc_hci_dev *hdev,
+				 xmit_to_drv_t xmit_to_drv,
+				 rcv_to_hci_t rcv_to_hci, int tx_headroom,
+				 int tx_tailroom, llc_failure_t llc_failure)
+{
+	struct nfc_llc_engine *llc_engine;
+	struct nfc_llc *llc;
+
+	llc_engine = nfc_llc_name_to_engine(name);
+	if (llc_engine == NULL)
+		return NULL;
+
+	llc = kzalloc(sizeof(struct nfc_llc), GFP_KERNEL);
+	if (llc == NULL)
+		return NULL;
+
+	llc->data = llc_engine->ops->init(hdev, xmit_to_drv, rcv_to_hci,
+					  tx_headroom, tx_tailroom,
+					  &llc->rx_headroom, &llc->rx_tailroom,
+					  llc_failure);
+	if (llc->data == NULL) {
+		kfree(llc);
+		return NULL;
+	}
+	llc->ops = llc_engine->ops;
+
+	return llc;
+}
+
+void nfc_llc_free(struct nfc_llc *llc)
+{
+	llc->ops->deinit(llc);
+	kfree(llc);
+}
+
+inline void nfc_llc_get_rx_head_tail_room(struct nfc_llc *llc, int *rx_headroom,
+					  int *rx_tailroom)
+{
+	*rx_headroom = llc->rx_headroom;
+	*rx_tailroom = llc->rx_tailroom;
+}
+
+inline int nfc_llc_start(struct nfc_llc *llc)
+{
+	return llc->ops->start(llc);
+}
+
+inline int nfc_llc_stop(struct nfc_llc *llc)
+{
+	return llc->ops->stop(llc);
+}
+
+inline void nfc_llc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	llc->ops->rcv_from_drv(llc, skb);
+}
+
+inline int nfc_llc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	return llc->ops->xmit_from_hci(llc, skb);
+}
+
+inline void *nfc_llc_get_data(struct nfc_llc *llc)
+{
+	return llc->data;
+}

net/nfc/hci/llc.h

@@ -0,0 +1,67 @@
+/*
+ * Link Layer Control manager
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __LOCAL_LLC_H_
+#define __LOCAL_LLC_H_
+
+#include <net/nfc/hci.h>
+#include <net/nfc/llc.h>
+#include <linux/skbuff.h>
+
+struct nfc_llc_ops {
+	void *(*init) (struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
+		       rcv_to_hci_t rcv_to_hci, int tx_headroom,
+		       int tx_tailroom, int *rx_headroom, int *rx_tailroom,
+		       llc_failure_t llc_failure);
+	void (*deinit) (struct nfc_llc *llc);
+	int (*start) (struct nfc_llc *llc);
+	int (*stop) (struct nfc_llc *llc);
+	void (*rcv_from_drv) (struct nfc_llc *llc, struct sk_buff *skb);
+	int (*xmit_from_hci) (struct nfc_llc *llc, struct sk_buff *skb);
+};
+
+struct nfc_llc_engine {
+	const char *name;
+	struct nfc_llc_ops *ops;
+	struct list_head entry;
+};
+
+struct nfc_llc {
+	void *data;
+	struct nfc_llc_ops *ops;
+	int rx_headroom;
+	int rx_tailroom;
+};
+
+void *nfc_llc_get_data(struct nfc_llc *llc);
+
+int nfc_llc_register(const char *name, struct nfc_llc_ops *ops);
+void nfc_llc_unregister(const char *name);
+
+int nfc_llc_nop_register(void);
+
+#if defined(CONFIG_NFC_SHDLC)
+int nfc_llc_shdlc_register(void);
+#else
+static inline int nfc_llc_shdlc_register(void)
+{
+	return 0;
+}
+#endif
+
+#endif /* __LOCAL_LLC_H_ */

net/nfc/hci/llc_nop.c

@@ -0,0 +1,97 @@
+/*
+ * nop (passthrough) Link Layer Control
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/types.h>
+
+#include "llc.h"
+
+struct llc_nop {
+	struct nfc_hci_dev *hdev;
+	xmit_to_drv_t xmit_to_drv;
+	rcv_to_hci_t rcv_to_hci;
+	int tx_headroom;
+	int tx_tailroom;
+	llc_failure_t llc_failure;
+};
+
+static void *llc_nop_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
+			  rcv_to_hci_t rcv_to_hci, int tx_headroom,
+			  int tx_tailroom, int *rx_headroom, int *rx_tailroom,
+			  llc_failure_t llc_failure)
+{
+	struct llc_nop *llc_nop;
+
+	*rx_headroom = 0;
+	*rx_tailroom = 0;
+
+	llc_nop = kzalloc(sizeof(struct llc_nop), GFP_KERNEL);
+	if (llc_nop == NULL)
+		return NULL;
+
+	llc_nop->hdev = hdev;
+	llc_nop->xmit_to_drv = xmit_to_drv;
+	llc_nop->rcv_to_hci = rcv_to_hci;
+	llc_nop->tx_headroom = tx_headroom;
+	llc_nop->tx_tailroom = tx_tailroom;
+	llc_nop->llc_failure = llc_failure;
+
+	return llc_nop;
+}
+
+static void llc_nop_deinit(struct nfc_llc *llc)
+{
+	kfree(nfc_llc_get_data(llc));
+}
+
+static int llc_nop_start(struct nfc_llc *llc)
+{
+	return 0;
+}
+
+static int llc_nop_stop(struct nfc_llc *llc)
+{
+	return 0;
+}
+
+static void llc_nop_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_nop *llc_nop = nfc_llc_get_data(llc);
+
+	llc_nop->rcv_to_hci(llc_nop->hdev, skb);
+}
+
+static int llc_nop_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_nop *llc_nop = nfc_llc_get_data(llc);
+
+	return llc_nop->xmit_to_drv(llc_nop->hdev, skb);
+}
+
+static struct nfc_llc_ops llc_nop_ops = {
+	.init = llc_nop_init,
+	.deinit = llc_nop_deinit,
+	.start = llc_nop_start,
+	.stop = llc_nop_stop,
+	.rcv_from_drv = llc_nop_rcv_from_drv,
+	.xmit_from_hci = llc_nop_xmit_from_hci,
+};
+
+int nfc_llc_nop_register(void)
+{
+	return nfc_llc_register(LLC_NOP_NAME, &llc_nop_ops);
+}

net/nfc/hci/llc_shdlc.c

@@ -0,0 +1,854 @@
+/*
+ * shdlc Link Layer Control
+ *
+ * Copyright (C) 2012  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+#include <linux/skbuff.h>
+
+#include "llc.h"
+
+enum shdlc_state {
+	SHDLC_DISCONNECTED = 0,
+	SHDLC_CONNECTING = 1,
+	SHDLC_NEGOTIATING = 2,
+	SHDLC_HALF_CONNECTED = 3,
+	SHDLC_CONNECTED = 4
+};
+
+struct llc_shdlc {
+	struct nfc_hci_dev *hdev;
+	xmit_to_drv_t xmit_to_drv;
+	rcv_to_hci_t rcv_to_hci;
+
+	struct mutex state_mutex;
+	enum shdlc_state state;
+	int hard_fault;
+
+	wait_queue_head_t *connect_wq;
+	int connect_tries;
+	int connect_result;
+	struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
+
+	u8 w;				/* window size */
+	bool srej_support;
+
+	struct timer_list t1_timer;	/* send ack timeout */
+	bool t1_active;
+
+	struct timer_list t2_timer;	/* guard/retransmit timeout */
+	bool t2_active;
+
+	int ns;				/* next seq num for send */
+	int nr;				/* next expected seq num for receive */
+	int dnr;			/* oldest sent unacked seq num */
+
+	struct sk_buff_head rcv_q;
+
+	struct sk_buff_head send_q;
+	bool rnr;			/* other side is not ready to receive */
+
+	struct sk_buff_head ack_pending_q;
+
+	struct work_struct sm_work;
+
+	int tx_headroom;
+	int tx_tailroom;
+
+	llc_failure_t llc_failure;
+};
+
+#define SHDLC_LLC_HEAD_ROOM	2
+
+#define SHDLC_MAX_WINDOW	4
+#define SHDLC_SREJ_SUPPORT	false
+
+#define SHDLC_CONTROL_HEAD_MASK	0xe0
+#define SHDLC_CONTROL_HEAD_I	0x80
+#define SHDLC_CONTROL_HEAD_I2	0xa0
+#define SHDLC_CONTROL_HEAD_S	0xc0
+#define SHDLC_CONTROL_HEAD_U	0xe0
+
+#define SHDLC_CONTROL_NS_MASK	0x38
+#define SHDLC_CONTROL_NR_MASK	0x07
+#define SHDLC_CONTROL_TYPE_MASK	0x18
+
+#define SHDLC_CONTROL_M_MASK	0x1f
+
+enum sframe_type {
+	S_FRAME_RR = 0x00,
+	S_FRAME_REJ = 0x01,
+	S_FRAME_RNR = 0x02,
+	S_FRAME_SREJ = 0x03
+};
+
+enum uframe_modifier {
+	U_FRAME_UA = 0x06,
+	U_FRAME_RSET = 0x19
+};
+
+#define SHDLC_CONNECT_VALUE_MS	5
+#define SHDLC_T1_VALUE_MS(w)	((5 * w) / 4)
+#define SHDLC_T2_VALUE_MS	300
+
+#define SHDLC_DUMP_SKB(info, skb)				  \
+do {								  \
+	pr_debug("%s:\n", info);				  \
+	print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
+		       16, 1, skb->data, skb->len, 0);		  \
+} while (0)
+
+/* checks x < y <= z modulo 8 */
+static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
+{
+	if (x < z)
+		return ((x < y) && (y <= z)) ? true : false;
+	else
+		return ((y > x) || (y <= z)) ? true : false;
+}
+
+/* checks x <= y < z modulo 8 */
+static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
+{
+	if (x <= z)
+		return ((x <= y) && (y < z)) ? true : false;
+	else			/* x > z -> z+8 > x */
+		return ((y >= x) || (y < z)) ? true : false;
+}
+
+static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc,
+					   int payload_len)
+{
+	struct sk_buff *skb;
+
+	skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
+			shdlc->tx_tailroom + payload_len, GFP_KERNEL);
+	if (skb)
+		skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
+
+	return skb;
+}
+
+/* immediately sends an S frame. */
+static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc,
+				  enum sframe_type sframe_type, int nr)
+{
+	int r;
+	struct sk_buff *skb;
+
+	pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
+
+	skb = llc_shdlc_alloc_skb(shdlc, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
+
+	r = shdlc->xmit_to_drv(shdlc->hdev, skb);
+
+	kfree_skb(skb);
+
+	return r;
+}
+
+/* immediately sends an U frame. skb may contain optional payload */
+static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc,
+				  struct sk_buff *skb,
+				  enum uframe_modifier uframe_modifier)
+{
+	int r;
+
+	pr_debug("uframe_modifier=%d\n", uframe_modifier);
+
+	*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
+
+	r = shdlc->xmit_to_drv(shdlc->hdev, skb);
+
+	kfree_skb(skb);
+
+	return r;
+}
+
+/*
+ * Free ack_pending frames until y_nr - 1, and reset t2 according to
+ * the remaining oldest ack_pending frame sent time
+ */
+static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
+{
+	struct sk_buff *skb;
+	int dnr = shdlc->dnr;	/* MUST initially be < y_nr */
+
+	pr_debug("release ack pending up to frame %d excluded\n", y_nr);
+
+	while (dnr != y_nr) {
+		pr_debug("release ack pending frame %d\n", dnr);
+
+		skb = skb_dequeue(&shdlc->ack_pending_q);
+		kfree_skb(skb);
+
+		dnr = (dnr + 1) % 8;
+	}
+
+	if (skb_queue_empty(&shdlc->ack_pending_q)) {
+		if (shdlc->t2_active) {
+			del_timer_sync(&shdlc->t2_timer);
+			shdlc->t2_active = false;
+
+			pr_debug
+			    ("All sent frames acked. Stopped T2(retransmit)\n");
+		}
+	} else {
+		skb = skb_peek(&shdlc->ack_pending_q);
+
+		mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
+			  msecs_to_jiffies(SHDLC_T2_VALUE_MS));
+		shdlc->t2_active = true;
+
+		pr_debug
+		    ("Start T2(retransmit) for remaining unacked sent frames\n");
+	}
+}
+
+/*
+ * Receive validated frames from lower layer. skb contains HCI payload only.
+ * Handle according to algorithm at spec:10.8.2
+ */
+static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
+				  struct sk_buff *skb, int ns, int nr)
+{
+	int x_ns = ns;
+	int y_nr = nr;
+
+	pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
+
+	if (shdlc->state != SHDLC_CONNECTED)
+		goto exit;
+
+	if (x_ns != shdlc->nr) {
+		llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
+		goto exit;
+	}
+
+	if (shdlc->t1_active == false) {
+		shdlc->t1_active = true;
+		mod_timer(&shdlc->t1_timer, jiffies +
+			  msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
+		pr_debug("(re)Start T1(send ack)\n");
+	}
+
+	if (skb->len) {
+		shdlc->rcv_to_hci(shdlc->hdev, skb);
+		skb = NULL;
+	}
+
+	shdlc->nr = (shdlc->nr + 1) % 8;
+
+	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
+		llc_shdlc_reset_t2(shdlc, y_nr);
+
+		shdlc->dnr = y_nr;
+	}
+
+exit:
+	kfree_skb(skb);
+}
+
+static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
+{
+	pr_debug("remote acked up to frame %d excluded\n", y_nr);
+
+	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
+		llc_shdlc_reset_t2(shdlc, y_nr);
+		shdlc->dnr = y_nr;
+	}
+}
+
+static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+
+	pr_debug("ns reset to %d\n", shdlc->dnr);
+
+	while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
+		skb_pull(skb, 1);	/* remove control field */
+		skb_queue_head(&shdlc->send_q, skb);
+	}
+	shdlc->ns = shdlc->dnr;
+}
+
+static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
+{
+	struct sk_buff *skb;
+
+	pr_debug("remote asks retransmission from frame %d\n", y_nr);
+
+	if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
+		if (shdlc->t2_active) {
+			del_timer_sync(&shdlc->t2_timer);
+			shdlc->t2_active = false;
+			pr_debug("Stopped T2(retransmit)\n");
+		}
+
+		if (shdlc->dnr != y_nr) {
+			while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
+				skb = skb_dequeue(&shdlc->ack_pending_q);
+				kfree_skb(skb);
+			}
+		}
+
+		llc_shdlc_requeue_ack_pending(shdlc);
+	}
+}
+
+/* See spec RR:10.8.3 REJ:10.8.4 */
+static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,
+				  enum sframe_type s_frame_type, int nr)
+{
+	struct sk_buff *skb;
+
+	if (shdlc->state != SHDLC_CONNECTED)
+		return;
+
+	switch (s_frame_type) {
+	case S_FRAME_RR:
+		llc_shdlc_rcv_ack(shdlc, nr);
+		if (shdlc->rnr == true) {	/* see SHDLC 10.7.7 */
+			shdlc->rnr = false;
+			if (shdlc->send_q.qlen == 0) {
+				skb = llc_shdlc_alloc_skb(shdlc, 0);
+				if (skb)
+					skb_queue_tail(&shdlc->send_q, skb);
+			}
+		}
+		break;
+	case S_FRAME_REJ:
+		llc_shdlc_rcv_rej(shdlc, nr);
+		break;
+	case S_FRAME_RNR:
+		llc_shdlc_rcv_ack(shdlc, nr);
+		shdlc->rnr = true;
+		break;
+	default:
+		break;
+	}
+}
+
+static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
+{
+	pr_debug("result=%d\n", r);
+
+	del_timer_sync(&shdlc->connect_timer);
+
+	if (r == 0) {
+		shdlc->ns = 0;
+		shdlc->nr = 0;
+		shdlc->dnr = 0;
+
+		shdlc->state = SHDLC_HALF_CONNECTED;
+	} else {
+		shdlc->state = SHDLC_DISCONNECTED;
+	}
+
+	shdlc->connect_result = r;
+
+	wake_up(shdlc->connect_wq);
+}
+
+static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+
+	pr_debug("\n");
+
+	skb = llc_shdlc_alloc_skb(shdlc, 2);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	*skb_put(skb, 1) = SHDLC_MAX_WINDOW;
+	*skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0;
+
+	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
+}
+
+static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+
+	pr_debug("\n");
+
+	skb = llc_shdlc_alloc_skb(shdlc, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
+}
+
+static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
+				  struct sk_buff *skb,
+				  enum uframe_modifier u_frame_modifier)
+{
+	u8 w = SHDLC_MAX_WINDOW;
+	bool srej_support = SHDLC_SREJ_SUPPORT;
+	int r;
+
+	pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
+
+	switch (u_frame_modifier) {
+	case U_FRAME_RSET:
+		switch (shdlc->state) {
+		case SHDLC_NEGOTIATING:
+		case SHDLC_CONNECTING:
+			/*
+			 * We sent RSET, but chip wants to negociate or we
+			 * got RSET before we managed to send out our.
+			 */
+			if (skb->len > 0)
+				w = skb->data[0];
+
+			if (skb->len > 1)
+				srej_support = skb->data[1] & 0x01 ? true :
+					       false;
+
+			if ((w <= SHDLC_MAX_WINDOW) &&
+			    (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
+				shdlc->w = w;
+				shdlc->srej_support = srej_support;
+				r = llc_shdlc_connect_send_ua(shdlc);
+				llc_shdlc_connect_complete(shdlc, r);
+			}
+			break;
+		case SHDLC_HALF_CONNECTED:
+			/*
+			 * Chip resent RSET due to its timeout - Ignote it
+			 * as we already sent UA.
+			 */
+			break;
+		case SHDLC_CONNECTED:
+			/*
+			 * Chip wants to reset link. This is unexpected and
+			 * unsupported.
+			 */
+			shdlc->hard_fault = -ECONNRESET;
+			break;
+		default:
+			break;
+		}
+		break;
+	case U_FRAME_UA:
+		if ((shdlc->state == SHDLC_CONNECTING &&
+		     shdlc->connect_tries > 0) ||
+		    (shdlc->state == SHDLC_NEGOTIATING)) {
+			llc_shdlc_connect_complete(shdlc, 0);
+			shdlc->state = SHDLC_CONNECTED;
+		}
+		break;
+	default:
+		break;
+	}
+
+	kfree_skb(skb);
+}
+
+static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+	u8 control;
+	int nr;
+	int ns;
+	enum sframe_type s_frame_type;
+	enum uframe_modifier u_frame_modifier;
+
+	if (shdlc->rcv_q.qlen)
+		pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
+
+	while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
+		control = skb->data[0];
+		skb_pull(skb, 1);
+		switch (control & SHDLC_CONTROL_HEAD_MASK) {
+		case SHDLC_CONTROL_HEAD_I:
+		case SHDLC_CONTROL_HEAD_I2:
+			if (shdlc->state == SHDLC_HALF_CONNECTED)
+				shdlc->state = SHDLC_CONNECTED;
+
+			ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
+			nr = control & SHDLC_CONTROL_NR_MASK;
+			llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
+			break;
+		case SHDLC_CONTROL_HEAD_S:
+			if (shdlc->state == SHDLC_HALF_CONNECTED)
+				shdlc->state = SHDLC_CONNECTED;
+
+			s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
+			nr = control & SHDLC_CONTROL_NR_MASK;
+			llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
+			kfree_skb(skb);
+			break;
+		case SHDLC_CONTROL_HEAD_U:
+			u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
+			llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
+			break;
+		default:
+			pr_err("UNKNOWN Control=%d\n", control);
+			kfree_skb(skb);
+			break;
+		}
+	}
+}
+
+static int llc_shdlc_w_used(int ns, int dnr)
+{
+	int unack_count;
+
+	if (dnr <= ns)
+		unack_count = ns - dnr;
+	else
+		unack_count = 8 - dnr + ns;
+
+	return unack_count;
+}
+
+/* Send frames according to algorithm at spec:10.8.1 */
+static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
+{
+	struct sk_buff *skb;
+	int r;
+	unsigned long time_sent;
+
+	if (shdlc->send_q.qlen)
+		pr_debug
+		    ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
+		     shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
+		     shdlc->rnr == false ? "false" : "true",
+		     shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
+		     shdlc->ack_pending_q.qlen);
+
+	while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
+	       (shdlc->rnr == false)) {
+
+		if (shdlc->t1_active) {
+			del_timer_sync(&shdlc->t1_timer);
+			shdlc->t1_active = false;
+			pr_debug("Stopped T1(send ack)\n");
+		}
+
+		skb = skb_dequeue(&shdlc->send_q);
+
+		*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
+				    shdlc->nr;
+
+		pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
+			 shdlc->nr);
+		SHDLC_DUMP_SKB("shdlc frame written", skb);
+
+		r = shdlc->xmit_to_drv(shdlc->hdev, skb);
+		if (r < 0) {
+			shdlc->hard_fault = r;
+			break;
+		}
+
+		shdlc->ns = (shdlc->ns + 1) % 8;
+
+		time_sent = jiffies;
+		*(unsigned long *)skb->cb = time_sent;
+
+		skb_queue_tail(&shdlc->ack_pending_q, skb);
+
+		if (shdlc->t2_active == false) {
+			shdlc->t2_active = true;
+			mod_timer(&shdlc->t2_timer, time_sent +
+				  msecs_to_jiffies(SHDLC_T2_VALUE_MS));
+			pr_debug("Started T2 (retransmit)\n");
+		}
+	}
+}
+
+static void llc_shdlc_connect_timeout(unsigned long data)
+{
+	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
+
+	pr_debug("\n");
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_t1_timeout(unsigned long data)
+{
+	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
+
+	pr_debug("SoftIRQ: need to send ack\n");
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_t2_timeout(unsigned long data)
+{
+	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;
+
+	pr_debug("SoftIRQ: need to retransmit\n");
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_sm_work(struct work_struct *work)
+{
+	struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
+	int r;
+
+	pr_debug("\n");
+
+	mutex_lock(&shdlc->state_mutex);
+
+	switch (shdlc->state) {
+	case SHDLC_DISCONNECTED:
+		skb_queue_purge(&shdlc->rcv_q);
+		skb_queue_purge(&shdlc->send_q);
+		skb_queue_purge(&shdlc->ack_pending_q);
+		break;
+	case SHDLC_CONNECTING:
+		if (shdlc->hard_fault) {
+			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
+			break;
+		}
+
+		if (shdlc->connect_tries++ < 5)
+			r = llc_shdlc_connect_initiate(shdlc);
+		else
+			r = -ETIME;
+		if (r < 0) {
+			llc_shdlc_connect_complete(shdlc, r);
+		} else {
+			mod_timer(&shdlc->connect_timer, jiffies +
+				  msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
+
+			shdlc->state = SHDLC_NEGOTIATING;
+		}
+		break;
+	case SHDLC_NEGOTIATING:
+		if (timer_pending(&shdlc->connect_timer) == 0) {
+			shdlc->state = SHDLC_CONNECTING;
+			schedule_work(&shdlc->sm_work);
+		}
+
+		llc_shdlc_handle_rcv_queue(shdlc);
+
+		if (shdlc->hard_fault) {
+			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
+			break;
+		}
+		break;
+	case SHDLC_HALF_CONNECTED:
+	case SHDLC_CONNECTED:
+		llc_shdlc_handle_rcv_queue(shdlc);
+		llc_shdlc_handle_send_queue(shdlc);
+
+		if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
+			pr_debug
+			    ("Handle T1(send ack) elapsed (T1 now inactive)\n");
+
+			shdlc->t1_active = false;
+			r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
+						   shdlc->nr);
+			if (r < 0)
+				shdlc->hard_fault = r;
+		}
+
+		if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
+			pr_debug
+			    ("Handle T2(retransmit) elapsed (T2 inactive)\n");
+
+			shdlc->t2_active = false;
+
+			llc_shdlc_requeue_ack_pending(shdlc);
+			llc_shdlc_handle_send_queue(shdlc);
+		}
+
+		if (shdlc->hard_fault)
+			shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
+		break;
+	default:
+		break;
+	}
+	mutex_unlock(&shdlc->state_mutex);
+}
+
+/*
+ * Called from syscall context to establish shdlc link. Sleeps until
+ * link is ready or failure.
+ */
+static int llc_shdlc_connect(struct llc_shdlc *shdlc)
+{
+	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
+
+	pr_debug("\n");
+
+	mutex_lock(&shdlc->state_mutex);
+
+	shdlc->state = SHDLC_CONNECTING;
+	shdlc->connect_wq = &connect_wq;
+	shdlc->connect_tries = 0;
+	shdlc->connect_result = 1;
+
+	mutex_unlock(&shdlc->state_mutex);
+
+	schedule_work(&shdlc->sm_work);
+
+	wait_event(connect_wq, shdlc->connect_result != 1);
+
+	return shdlc->connect_result;
+}
+
+static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
+{
+	pr_debug("\n");
+
+	mutex_lock(&shdlc->state_mutex);
+
+	shdlc->state = SHDLC_DISCONNECTED;
+
+	mutex_unlock(&shdlc->state_mutex);
+
+	schedule_work(&shdlc->sm_work);
+}
+
+/*
+ * Receive an incoming shdlc frame. Frame has already been crc-validated.
+ * skb contains only LLC header and payload.
+ * If skb == NULL, it is a notification that the link below is dead.
+ */
+static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
+{
+	if (skb == NULL) {
+		pr_err("NULL Frame -> link is dead\n");
+		shdlc->hard_fault = -EREMOTEIO;
+	} else {
+		SHDLC_DUMP_SKB("incoming frame", skb);
+		skb_queue_tail(&shdlc->rcv_q, skb);
+	}
+
+	schedule_work(&shdlc->sm_work);
+}
+
+static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
+			    rcv_to_hci_t rcv_to_hci, int tx_headroom,
+			    int tx_tailroom, int *rx_headroom, int *rx_tailroom,
+			    llc_failure_t llc_failure)
+{
+	struct llc_shdlc *shdlc;
+
+	*rx_headroom = SHDLC_LLC_HEAD_ROOM;
+	*rx_tailroom = 0;
+
+	shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
+	if (shdlc == NULL)
+		return NULL;
+
+	mutex_init(&shdlc->state_mutex);
+	shdlc->state = SHDLC_DISCONNECTED;
+
+	init_timer(&shdlc->connect_timer);
+	shdlc->connect_timer.data = (unsigned long)shdlc;
+	shdlc->connect_timer.function = llc_shdlc_connect_timeout;
+
+	init_timer(&shdlc->t1_timer);
+	shdlc->t1_timer.data = (unsigned long)shdlc;
+	shdlc->t1_timer.function = llc_shdlc_t1_timeout;
+
+	init_timer(&shdlc->t2_timer);
+	shdlc->t2_timer.data = (unsigned long)shdlc;
+	shdlc->t2_timer.function = llc_shdlc_t2_timeout;
+
+	shdlc->w = SHDLC_MAX_WINDOW;
+	shdlc->srej_support = SHDLC_SREJ_SUPPORT;
+
+	skb_queue_head_init(&shdlc->rcv_q);
+	skb_queue_head_init(&shdlc->send_q);
+	skb_queue_head_init(&shdlc->ack_pending_q);
+
+	INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
+
+	shdlc->hdev = hdev;
+	shdlc->xmit_to_drv = xmit_to_drv;
+	shdlc->rcv_to_hci = rcv_to_hci;
+	shdlc->tx_headroom = tx_headroom;
+	shdlc->tx_tailroom = tx_tailroom;
+	shdlc->llc_failure = llc_failure;
+
+	return shdlc;
+}
+
+static void llc_shdlc_deinit(struct nfc_llc *llc)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	skb_queue_purge(&shdlc->rcv_q);
+	skb_queue_purge(&shdlc->send_q);
+	skb_queue_purge(&shdlc->ack_pending_q);
+
+	kfree(shdlc);
+}
+
+static int llc_shdlc_start(struct nfc_llc *llc)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	return llc_shdlc_connect(shdlc);
+}
+
+static int llc_shdlc_stop(struct nfc_llc *llc)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	llc_shdlc_disconnect(shdlc);
+
+	return 0;
+}
+
+static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	llc_shdlc_recv_frame(shdlc, skb);
+}
+
+static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+	skb_queue_tail(&shdlc->send_q, skb);
+
+	schedule_work(&shdlc->sm_work);
+
+	return 0;
+}
+
+static struct nfc_llc_ops llc_shdlc_ops = {
+	.init = llc_shdlc_init,
+	.deinit = llc_shdlc_deinit,
+	.start = llc_shdlc_start,
+	.stop = llc_shdlc_stop,
+	.rcv_from_drv = llc_shdlc_rcv_from_drv,
+	.xmit_from_hci = llc_shdlc_xmit_from_hci,
+};
+
+int nfc_llc_shdlc_register(void)
+{
+	return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
+}

net/nfc/llcp.h

@@ -0,0 +1,266 @@
+/*
+ * Copyright (C) 2011  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+enum llcp_state {
+	LLCP_CONNECTED = 1, /* wait_for_packet() wants that */
+	LLCP_CONNECTING,
+	LLCP_DISCONNECTING,
+	LLCP_CLOSED,
+	LLCP_BOUND,
+	LLCP_LISTEN,
+};
+
+#define LLCP_DEFAULT_LTO 100
+#define LLCP_DEFAULT_RW  1
+#define LLCP_DEFAULT_MIU 128
+
+#define LLCP_MAX_LTO  0xff
+#define LLCP_MAX_RW   15
+#define LLCP_MAX_MIUX 0x7ff
+#define LLCP_MAX_MIU (LLCP_MAX_MIUX + 128)
+
+#define LLCP_WKS_NUM_SAP   16
+#define LLCP_SDP_NUM_SAP   16
+#define LLCP_LOCAL_NUM_SAP 32
+#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP)
+#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP)
+#define LLCP_SDP_UNBOUND   (LLCP_MAX_SAP + 1)
+
+struct nfc_llcp_sock;
+
+struct llcp_sock_list {
+	struct hlist_head head;
+	rwlock_t          lock;
+};
+
+struct nfc_llcp_sdp_tlv {
+	u8 *tlv;
+	u8 tlv_len;
+
+	char *uri;
+	u8 tid;
+	u8 sap;
+
+	unsigned long time;
+
+	struct hlist_node node;
+};
+
+struct nfc_llcp_local {
+	struct list_head list;
+	struct nfc_dev *dev;
+
+	struct kref ref;
+
+	struct mutex sdp_lock;
+
+	struct timer_list link_timer;
+	struct sk_buff_head tx_queue;
+	struct work_struct	 tx_work;
+	struct work_struct	 rx_work;
+	struct sk_buff *rx_pending;
+	struct work_struct	 timeout_work;
+
+	u32 target_idx;
+	u8 rf_mode;
+	u8 comm_mode;
+	u8 lto;
+	u8 rw;
+	__be16 miux;
+	unsigned long local_wks;      /* Well known services */
+	unsigned long local_sdp;      /* Local services  */
+	unsigned long local_sap; /* Local SAPs, not available for discovery */
+	atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP];
+
+	/* local */
+	u8 gb[NFC_MAX_GT_LEN];
+	u8 gb_len;
+
+	/* remote */
+	u8 remote_gb[NFC_MAX_GT_LEN];
+	u8 remote_gb_len;
+
+	u8  remote_version;
+	u16 remote_miu;
+	u16 remote_lto;
+	u8  remote_opt;
+	u16 remote_wks;
+
+	struct mutex sdreq_lock;
+	struct hlist_head pending_sdreqs;
+	struct timer_list sdreq_timer;
+	struct work_struct sdreq_timeout_work;
+	u8 sdreq_next_tid;
+
+	/* sockets array */
+	struct llcp_sock_list sockets;
+	struct llcp_sock_list connecting_sockets;
+	struct llcp_sock_list raw_sockets;
+};
+
+struct nfc_llcp_sock {
+	struct sock sk;
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+	u32 target_idx;
+	u32 nfc_protocol;
+
+	/* Link parameters */
+	u8 ssap;
+	u8 dsap;
+	char *service_name;
+	size_t service_name_len;
+	u8 rw;
+	__be16 miux;
+
+
+	/* Remote link parameters */
+	u8 remote_rw;
+	u16 remote_miu;
+
+	/* Link variables */
+	u8 send_n;
+	u8 send_ack_n;
+	u8 recv_n;
+	u8 recv_ack_n;
+
+	/* Is the remote peer ready to receive */
+	u8 remote_ready;
+
+	/* Reserved source SAP */
+	u8 reserved_ssap;
+
+	struct sk_buff_head tx_queue;
+	struct sk_buff_head tx_pending_queue;
+
+	struct list_head accept_queue;
+	struct sock *parent;
+};
+
+struct nfc_llcp_ui_cb {
+	__u8 dsap;
+	__u8 ssap;
+};
+
+#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
+
+#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
+#define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
+
+#define LLCP_HEADER_SIZE   2
+#define LLCP_SEQUENCE_SIZE 1
+#define LLCP_AGF_PDU_HEADER_SIZE 2
+
+/* LLCP versions: 1.1 is 1.0 plus SDP */
+#define LLCP_VERSION_10 0x10
+#define LLCP_VERSION_11 0x11
+
+/* LLCP PDU types */
+#define LLCP_PDU_SYMM     0x0
+#define LLCP_PDU_PAX      0x1
+#define LLCP_PDU_AGF      0x2
+#define LLCP_PDU_UI       0x3
+#define LLCP_PDU_CONNECT  0x4
+#define LLCP_PDU_DISC     0x5
+#define LLCP_PDU_CC       0x6
+#define LLCP_PDU_DM       0x7
+#define LLCP_PDU_FRMR     0x8
+#define LLCP_PDU_SNL      0x9
+#define LLCP_PDU_I        0xc
+#define LLCP_PDU_RR       0xd
+#define LLCP_PDU_RNR      0xe
+
+/* Parameters TLV types */
+#define LLCP_TLV_VERSION 0x1
+#define LLCP_TLV_MIUX    0x2
+#define LLCP_TLV_WKS     0x3
+#define LLCP_TLV_LTO     0x4
+#define LLCP_TLV_RW      0x5
+#define LLCP_TLV_SN      0x6
+#define LLCP_TLV_OPT     0x7
+#define LLCP_TLV_SDREQ   0x8
+#define LLCP_TLV_SDRES   0x9
+#define LLCP_TLV_MAX     0xa
+
+/* Well known LLCP SAP */
+#define LLCP_SAP_SDP   0x1
+#define LLCP_SAP_IP    0x2
+#define LLCP_SAP_OBEX  0x3
+#define LLCP_SAP_SNEP  0x4
+#define LLCP_SAP_MAX   0xff
+
+/* Disconnection reason code */
+#define LLCP_DM_DISC    0x00
+#define LLCP_DM_NOCONN  0x01
+#define LLCP_DM_NOBOUND 0x02
+#define LLCP_DM_REJ     0x03
+
+
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local);
+int nfc_llcp_local_put(struct nfc_llcp_local *local);
+u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
+			 struct nfc_llcp_sock *sock);
+u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);
+void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap);
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock);
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+			       struct sk_buff *skb, u8 direction);
+
+/* Sock API */
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
+void nfc_llcp_sock_free(struct nfc_llcp_sock *sock);
+void nfc_llcp_accept_unlink(struct sock *sk);
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk);
+struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock);
+
+/* TLV API */
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+			  u8 *tlv_array, u16 tlv_array_len);
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+				  u8 *tlv_array, u16 tlv_array_len);
+
+/* Commands API */
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+						  size_t uri_len);
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head);
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
+int nfc_llcp_send_symm(struct nfc_dev *dev);
+int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len);
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len);
+int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
+int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
+			  struct msghdr *msg, size_t len);
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+			   struct msghdr *msg, size_t len);
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
+
+/* Socket API */
+int __init nfc_llcp_sock_init(void);
+void nfc_llcp_sock_exit(void);

net/nfc/llcp_commands.c

@@ -0,0 +1,797 @@
+/*
+ * Copyright (C) 2011  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include <net/nfc/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
+	0,
+	1, /* VERSION */
+	2, /* MIUX */
+	2, /* WKS */
+	1, /* LTO */
+	1, /* RW */
+	0, /* SN */
+	1, /* OPT */
+	0, /* SDREQ */
+	2, /* SDRES */
+
+};
+
+static u8 llcp_tlv8(u8 *tlv, u8 type)
+{
+	if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+		return 0;
+
+	return tlv[2];
+}
+
+static u16 llcp_tlv16(u8 *tlv, u8 type)
+{
+	if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+		return 0;
+
+	return be16_to_cpu(*((__be16 *)(tlv + 2)));
+}
+
+
+static u8 llcp_tlv_version(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_VERSION);
+}
+
+static u16 llcp_tlv_miux(u8 *tlv)
+{
+	return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
+}
+
+static u16 llcp_tlv_wks(u8 *tlv)
+{
+	return llcp_tlv16(tlv, LLCP_TLV_WKS);
+}
+
+static u16 llcp_tlv_lto(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_LTO);
+}
+
+static u8 llcp_tlv_opt(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_OPT);
+}
+
+static u8 llcp_tlv_rw(u8 *tlv)
+{
+	return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
+}
+
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
+{
+	u8 *tlv, length;
+
+	pr_debug("type %d\n", type);
+
+	if (type >= LLCP_TLV_MAX)
+		return NULL;
+
+	length = llcp_tlv_length[type];
+	if (length == 0 && value_length == 0)
+		return NULL;
+	else if (length == 0)
+		length = value_length;
+
+	*tlv_length = 2 + length;
+	tlv = kzalloc(2 + length, GFP_KERNEL);
+	if (tlv == NULL)
+		return tlv;
+
+	tlv[0] = type;
+	tlv[1] = length;
+	memcpy(tlv + 2, value, length);
+
+	return tlv;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
+{
+	struct nfc_llcp_sdp_tlv *sdres;
+	u8 value[2];
+
+	sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+	if (sdres == NULL)
+		return NULL;
+
+	value[0] = tid;
+	value[1] = sap;
+
+	sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
+					&sdres->tlv_len);
+	if (sdres->tlv == NULL) {
+		kfree(sdres);
+		return NULL;
+	}
+
+	sdres->tid = tid;
+	sdres->sap = sap;
+
+	INIT_HLIST_NODE(&sdres->node);
+
+	return sdres;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+						  size_t uri_len)
+{
+	struct nfc_llcp_sdp_tlv *sdreq;
+
+	pr_debug("uri: %s, len: %zu\n", uri, uri_len);
+
+	sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+	if (sdreq == NULL)
+		return NULL;
+
+	sdreq->tlv_len = uri_len + 3;
+
+	if (uri[uri_len - 1] == 0)
+		sdreq->tlv_len--;
+
+	sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
+	if (sdreq->tlv == NULL) {
+		kfree(sdreq);
+		return NULL;
+	}
+
+	sdreq->tlv[0] = LLCP_TLV_SDREQ;
+	sdreq->tlv[1] = sdreq->tlv_len - 2;
+	sdreq->tlv[2] = tid;
+
+	sdreq->tid = tid;
+	sdreq->uri = sdreq->tlv + 3;
+	memcpy(sdreq->uri, uri, uri_len);
+
+	sdreq->time = jiffies;
+
+	INIT_HLIST_NODE(&sdreq->node);
+
+	return sdreq;
+}
+
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
+{
+	kfree(sdp->tlv);
+	kfree(sdp);
+}
+
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
+{
+	struct nfc_llcp_sdp_tlv *sdp;
+	struct hlist_node *n;
+
+	hlist_for_each_entry_safe(sdp, n, head, node) {
+		hlist_del(&sdp->node);
+
+		nfc_llcp_free_sdp_tlv(sdp);
+	}
+}
+
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+			  u8 *tlv_array, u16 tlv_array_len)
+{
+	u8 *tlv = tlv_array, type, length, offset = 0;
+
+	pr_debug("TLV array length %d\n", tlv_array_len);
+
+	if (local == NULL)
+		return -ENODEV;
+
+	while (offset < tlv_array_len) {
+		type = tlv[0];
+		length = tlv[1];
+
+		pr_debug("type 0x%x length %d\n", type, length);
+
+		switch (type) {
+		case LLCP_TLV_VERSION:
+			local->remote_version = llcp_tlv_version(tlv);
+			break;
+		case LLCP_TLV_MIUX:
+			local->remote_miu = llcp_tlv_miux(tlv) + 128;
+			break;
+		case LLCP_TLV_WKS:
+			local->remote_wks = llcp_tlv_wks(tlv);
+			break;
+		case LLCP_TLV_LTO:
+			local->remote_lto = llcp_tlv_lto(tlv) * 10;
+			break;
+		case LLCP_TLV_OPT:
+			local->remote_opt = llcp_tlv_opt(tlv);
+			break;
+		default:
+			pr_err("Invalid gt tlv value 0x%x\n", type);
+			break;
+		}
+
+		offset += length + 2;
+		tlv += length + 2;
+	}
+
+	pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
+		 local->remote_version, local->remote_miu,
+		 local->remote_lto, local->remote_opt,
+		 local->remote_wks);
+
+	return 0;
+}
+
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+				  u8 *tlv_array, u16 tlv_array_len)
+{
+	u8 *tlv = tlv_array, type, length, offset = 0;
+
+	pr_debug("TLV array length %d\n", tlv_array_len);
+
+	if (sock == NULL)
+		return -ENOTCONN;
+
+	while (offset < tlv_array_len) {
+		type = tlv[0];
+		length = tlv[1];
+
+		pr_debug("type 0x%x length %d\n", type, length);
+
+		switch (type) {
+		case LLCP_TLV_MIUX:
+			sock->remote_miu = llcp_tlv_miux(tlv) + 128;
+			break;
+		case LLCP_TLV_RW:
+			sock->remote_rw = llcp_tlv_rw(tlv);
+			break;
+		case LLCP_TLV_SN:
+			break;
+		default:
+			pr_err("Invalid gt tlv value 0x%x\n", type);
+			break;
+		}
+
+		offset += length + 2;
+		tlv += length + 2;
+	}
+
+	pr_debug("sock %p rw %d miu %d\n", sock,
+		 sock->remote_rw, sock->remote_miu);
+
+	return 0;
+}
+
+static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
+				       u8 dsap, u8 ssap, u8 ptype)
+{
+	u8 header[2];
+
+	pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+	header[0] = (u8)((dsap << 2) | (ptype >> 2));
+	header[1] = (u8)((ptype << 6) | ssap);
+
+	pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
+
+	memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
+
+	return pdu;
+}
+
+static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
+				    u8 tlv_length)
+{
+	/* XXX Add an skb length check */
+
+	if (tlv == NULL)
+		return NULL;
+
+	memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
+
+	return pdu;
+}
+
+static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
+					 u8 cmd, u16 size)
+{
+	struct sk_buff *skb;
+	int err;
+
+	if (sock->ssap == 0)
+		return NULL;
+
+	skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+				 size + LLCP_HEADER_SIZE, &err);
+	if (skb == NULL) {
+		pr_err("Could not allocate PDU\n");
+		return NULL;
+	}
+
+	skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
+
+	return skb;
+}
+
+int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	struct nfc_llcp_local *local;
+
+	pr_debug("Sending DISC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	dev = sock->dev;
+	if (dev == NULL)
+		return -ENODEV;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_symm(struct nfc_dev *dev)
+{
+	struct sk_buff *skb;
+	struct nfc_llcp_local *local;
+	u16 size = 0;
+
+	pr_debug("Sending SYMM\n");
+
+	local = nfc_llcp_find_local(dev);
+	if (local == NULL)
+		return -ENODEV;
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
+
+	__net_timestamp(skb);
+
+	nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_TX);
+
+	return nfc_data_exchange(dev, local->target_idx, skb,
+				 nfc_llcp_recv, local);
+}
+
+int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local;
+	struct sk_buff *skb;
+	u8 *service_name_tlv = NULL, service_name_tlv_length;
+	u8 *miux_tlv = NULL, miux_tlv_length;
+	u8 *rw_tlv = NULL, rw_tlv_length, rw;
+	int err;
+	u16 size = 0, miux;
+
+	pr_debug("Sending CONNECT\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	if (sock->service_name != NULL) {
+		service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
+						      sock->service_name,
+						      sock->service_name_len,
+						      &service_name_tlv_length);
+		size += service_name_tlv_length;
+	}
+
+	/* If the socket parameters are not set, use the local ones */
+	miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
+		local->miux : sock->miux;
+	rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
+
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+				      &miux_tlv_length);
+	size += miux_tlv_length;
+
+	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+	size += rw_tlv_length;
+
+	pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
+	if (skb == NULL) {
+		err = -ENOMEM;
+		goto error_tlv;
+	}
+
+	if (service_name_tlv != NULL)
+		skb = llcp_add_tlv(skb, service_name_tlv,
+				   service_name_tlv_length);
+
+	skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+	skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+
+error_tlv:
+	pr_err("error %d\n", err);
+
+	kfree(service_name_tlv);
+	kfree(miux_tlv);
+	kfree(rw_tlv);
+
+	return err;
+}
+
+int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
+{
+	struct nfc_llcp_local *local;
+	struct sk_buff *skb;
+	u8 *miux_tlv = NULL, miux_tlv_length;
+	u8 *rw_tlv = NULL, rw_tlv_length, rw;
+	int err;
+	u16 size = 0, miux;
+
+	pr_debug("Sending CC\n");
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	/* If the socket parameters are not set, use the local ones */
+	miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
+		local->miux : sock->miux;
+	rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
+
+	miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+				      &miux_tlv_length);
+	size += miux_tlv_length;
+
+	rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+	size += rw_tlv_length;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
+	if (skb == NULL) {
+		err = -ENOMEM;
+		goto error_tlv;
+	}
+
+	skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+	skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+
+error_tlv:
+	pr_err("error %d\n", err);
+
+	kfree(miux_tlv);
+	kfree(rw_tlv);
+
+	return err;
+}
+
+static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
+					     size_t tlv_length)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	u16 size = 0;
+
+	if (local == NULL)
+		return ERR_PTR(-ENODEV);
+
+	dev = local->dev;
+	if (dev == NULL)
+		return ERR_PTR(-ENODEV);
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+	size += tlv_length;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
+
+	return skb;
+}
+
+int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len)
+{
+	struct nfc_llcp_sdp_tlv *sdp;
+	struct hlist_node *n;
+	struct sk_buff *skb;
+
+	skb = nfc_llcp_allocate_snl(local, tlvs_len);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
+		memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);
+
+		hlist_del(&sdp->node);
+
+		nfc_llcp_free_sdp_tlv(sdp);
+	}
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+			    struct hlist_head *tlv_list, size_t tlvs_len)
+{
+	struct nfc_llcp_sdp_tlv *sdreq;
+	struct hlist_node *n;
+	struct sk_buff *skb;
+
+	skb = nfc_llcp_allocate_snl(local, tlvs_len);
+	if (IS_ERR(skb))
+		return PTR_ERR(skb);
+
+	mutex_lock(&local->sdreq_lock);
+
+	if (hlist_empty(&local->pending_sdreqs))
+		mod_timer(&local->sdreq_timer,
+			  jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+	hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
+		pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
+
+		memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv,
+		       sdreq->tlv_len);
+
+		hlist_del(&sdreq->node);
+
+		hlist_add_head(&sdreq->node, &local->pending_sdreqs);
+	}
+
+	mutex_unlock(&local->sdreq_lock);
+
+	skb_queue_tail(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
+{
+	struct sk_buff *skb;
+	struct nfc_dev *dev;
+	u16 size = 1; /* Reason code */
+
+	pr_debug("Sending DM reason 0x%x\n", reason);
+
+	if (local == NULL)
+		return -ENODEV;
+
+	dev = local->dev;
+	if (dev == NULL)
+		return -ENODEV;
+
+	size += LLCP_HEADER_SIZE;
+	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+	skb = alloc_skb(size, GFP_KERNEL);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+	skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM);
+
+	memcpy(skb_put(skb, 1), &reason, 1);
+
+	skb_queue_head(&local->tx_queue, skb);
+
+	return 0;
+}
+
+int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
+			  struct msghdr *msg, size_t len)
+{
+	struct sk_buff *pdu;
+	struct sock *sk = &sock->sk;
+	struct nfc_llcp_local *local;
+	size_t frag_len = 0, remaining_len;
+	u8 *msg_data, *msg_ptr;
+	u16 remote_miu;
+
+	pr_debug("Send I frame len %zd\n", len);
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	/* Remote is ready but has not acknowledged our frames */
+	if((sock->remote_ready &&
+	    skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw &&
+	    skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
+		pr_err("Pending queue is full %d frames\n",
+		       skb_queue_len(&sock->tx_pending_queue));
+		return -ENOBUFS;
+	}
+
+	/* Remote is not ready and we've been queueing enough frames */
+	if ((!sock->remote_ready &&
+	     skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
+		pr_err("Tx queue is full %d frames\n",
+		       skb_queue_len(&sock->tx_queue));
+		return -ENOBUFS;
+	}
+
+	msg_data = kzalloc(len, GFP_KERNEL);
+	if (msg_data == NULL)
+		return -ENOMEM;
+
+	if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+		kfree(msg_data);
+		return -EFAULT;
+	}
+
+	remaining_len = len;
+	msg_ptr = msg_data;
+
+	do {
+		remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
+				LLCP_DEFAULT_MIU : sock->remote_miu;
+
+		frag_len = min_t(size_t, remote_miu, remaining_len);
+
+		pr_debug("Fragment %zd bytes remaining %zd",
+			 frag_len, remaining_len);
+
+		pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
+					frag_len + LLCP_SEQUENCE_SIZE);
+		if (pdu == NULL) {
+			kfree(msg_data);
+			return -ENOMEM;
+		}
+
+		skb_put(pdu, LLCP_SEQUENCE_SIZE);
+
+		if (likely(frag_len > 0))
+			memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		skb_queue_tail(&sock->tx_queue, pdu);
+
+		lock_sock(sk);
+
+		nfc_llcp_queue_i_frames(sock);
+
+		release_sock(sk);
+
+		remaining_len -= frag_len;
+		msg_ptr += frag_len;
+	} while (remaining_len > 0);
+
+	kfree(msg_data);
+
+	return len;
+}
+
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+			   struct msghdr *msg, size_t len)
+{
+	struct sk_buff *pdu;
+	struct nfc_llcp_local *local;
+	size_t frag_len = 0, remaining_len;
+	u8 *msg_ptr, *msg_data;
+	u16 remote_miu;
+	int err;
+
+	pr_debug("Send UI frame len %zd\n", len);
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	msg_data = kzalloc(len, GFP_KERNEL);
+	if (msg_data == NULL)
+		return -ENOMEM;
+
+	if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+		kfree(msg_data);
+		return -EFAULT;
+	}
+
+	remaining_len = len;
+	msg_ptr = msg_data;
+
+	do {
+		remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
+				local->remote_miu : sock->remote_miu;
+
+		frag_len = min_t(size_t, remote_miu, remaining_len);
+
+		pr_debug("Fragment %zd bytes remaining %zd",
+			 frag_len, remaining_len);
+
+		pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+					 frag_len + LLCP_HEADER_SIZE, &err);
+		if (pdu == NULL) {
+			pr_err("Could not allocate PDU\n");
+			continue;
+		}
+
+		pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+		if (likely(frag_len > 0))
+			memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		/* No need to check for the peer RW for UI frames */
+		skb_queue_tail(&local->tx_queue, pdu);
+
+		remaining_len -= frag_len;
+		msg_ptr += frag_len;
+	} while (remaining_len > 0);
+
+	kfree(msg_data);
+
+	return len;
+}
+
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
+{
+	struct sk_buff *skb;
+	struct nfc_llcp_local *local;
+
+	pr_debug("Send rr nr %d\n", sock->recv_n);
+
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
+	if (skb == NULL)
+		return -ENOMEM;
+
+	skb_put(skb, LLCP_SEQUENCE_SIZE);
+
+	skb->data[2] = sock->recv_n;
+
+	skb_queue_head(&local->tx_queue, skb);
+
+	return 0;
+}

net/nfc/nci/Kconfig

@@ -0,0 +1,21 @@
+config NFC_NCI
+	depends on NFC
+	tristate "NCI protocol support"
+	default n
+	help
+	  NCI (NFC Controller Interface) is a communication protocol between
+	  an NFC Controller (NFCC) and a Device Host (DH).
+
+	  Say Y here to compile NCI support into the kernel or say M to
+	  compile it as module (nci).
+
+config NFC_NCI_SPI
+	depends on NFC_NCI && SPI
+	select CRC_CCITT
+	bool "NCI over SPI protocol support"
+	default n
+	help
+	  NCI (NFC Controller Interface) is a communication protocol between
+	  an NFC Controller (NFCC) and a Device Host (DH).
+
+	  Say yes if you use an NCI driver that requires SPI link layer.

net/nfc/nci/Makefile

@@ -0,0 +1,9 @@
+#
+# Makefile for the Linux NFC NCI layer.
+#
+
+obj-$(CONFIG_NFC_NCI) += nci.o
+
+nci-objs := core.o data.o lib.o ntf.o rsp.o
+
+nci-$(CONFIG_NFC_NCI_SPI) += spi.o

net/nfc/nci/data.c

@@ -0,0 +1,254 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/wait.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+/* Complete data exchange transaction and forward skb to nfc core */
+void nci_data_exchange_complete(struct nci_dev *ndev, struct sk_buff *skb,
+				int err)
+{
+	data_exchange_cb_t cb = ndev->data_exchange_cb;
+	void *cb_context = ndev->data_exchange_cb_context;
+
+	pr_debug("len %d, err %d\n", skb ? skb->len : 0, err);
+
+	/* data exchange is complete, stop the data timer */
+	del_timer_sync(&ndev->data_timer);
+	clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
+
+	if (cb) {
+		ndev->data_exchange_cb = NULL;
+		ndev->data_exchange_cb_context = NULL;
+
+		/* forward skb to nfc core */
+		cb(cb_context, skb, err);
+	} else if (skb) {
+		pr_err("no rx callback, dropping rx data...\n");
+
+		/* no waiting callback, free skb */
+		kfree_skb(skb);
+	}
+
+	clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
+}
+
+/* ----------------- NCI TX Data ----------------- */
+
+static inline void nci_push_data_hdr(struct nci_dev *ndev,
+				     __u8 conn_id,
+				     struct sk_buff *skb,
+				     __u8 pbf)
+{
+	struct nci_data_hdr *hdr;
+	int plen = skb->len;
+
+	hdr = (struct nci_data_hdr *) skb_push(skb, NCI_DATA_HDR_SIZE);
+	hdr->conn_id = conn_id;
+	hdr->rfu = 0;
+	hdr->plen = plen;
+
+	nci_mt_set((__u8 *)hdr, NCI_MT_DATA_PKT);
+	nci_pbf_set((__u8 *)hdr, pbf);
+}
+
+static int nci_queue_tx_data_frags(struct nci_dev *ndev,
+				   __u8 conn_id,
+				   struct sk_buff *skb) {
+	int total_len = skb->len;
+	unsigned char *data = skb->data;
+	unsigned long flags;
+	struct sk_buff_head frags_q;
+	struct sk_buff *skb_frag;
+	int frag_len;
+	int rc = 0;
+
+	pr_debug("conn_id 0x%x, total_len %d\n", conn_id, total_len);
+
+	__skb_queue_head_init(&frags_q);
+
+	while (total_len) {
+		frag_len =
+			min_t(int, total_len, ndev->max_data_pkt_payload_size);
+
+		skb_frag = nci_skb_alloc(ndev,
+					 (NCI_DATA_HDR_SIZE + frag_len),
+					 GFP_KERNEL);
+		if (skb_frag == NULL) {
+			rc = -ENOMEM;
+			goto free_exit;
+		}
+		skb_reserve(skb_frag, NCI_DATA_HDR_SIZE);
+
+		/* first, copy the data */
+		memcpy(skb_put(skb_frag, frag_len), data, frag_len);
+
+		/* second, set the header */
+		nci_push_data_hdr(ndev, conn_id, skb_frag,
+				  ((total_len == frag_len) ?
+				   (NCI_PBF_LAST) : (NCI_PBF_CONT)));
+
+		__skb_queue_tail(&frags_q, skb_frag);
+
+		data += frag_len;
+		total_len -= frag_len;
+
+		pr_debug("frag_len %d, remaining total_len %d\n",
+			 frag_len, total_len);
+	}
+
+	/* queue all fragments atomically */
+	spin_lock_irqsave(&ndev->tx_q.lock, flags);
+
+	while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
+		__skb_queue_tail(&ndev->tx_q, skb_frag);
+
+	spin_unlock_irqrestore(&ndev->tx_q.lock, flags);
+
+	/* free the original skb */
+	kfree_skb(skb);
+
+	goto exit;
+
+free_exit:
+	while ((skb_frag = __skb_dequeue(&frags_q)) != NULL)
+		kfree_skb(skb_frag);
+
+exit:
+	return rc;
+}
+
+/* Send NCI data */
+int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb)
+{
+	int rc = 0;
+
+	pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len);
+
+	/* check if the packet need to be fragmented */
+	if (skb->len <= ndev->max_data_pkt_payload_size) {
+		/* no need to fragment packet */
+		nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST);
+
+		skb_queue_tail(&ndev->tx_q, skb);
+	} else {
+		/* fragment packet and queue the fragments */
+		rc = nci_queue_tx_data_frags(ndev, conn_id, skb);
+		if (rc) {
+			pr_err("failed to fragment tx data packet\n");
+			goto free_exit;
+		}
+	}
+
+	queue_work(ndev->tx_wq, &ndev->tx_work);
+
+	goto exit;
+
+free_exit:
+	kfree_skb(skb);
+
+exit:
+	return rc;
+}
+
+/* ----------------- NCI RX Data ----------------- */
+
+static void nci_add_rx_data_frag(struct nci_dev *ndev,
+				 struct sk_buff *skb,
+				 __u8 pbf)
+{
+	int reassembly_len;
+	int err = 0;
+
+	if (ndev->rx_data_reassembly) {
+		reassembly_len = ndev->rx_data_reassembly->len;
+
+		/* first, make enough room for the already accumulated data */
+		if (skb_cow_head(skb, reassembly_len)) {
+			pr_err("error adding room for accumulated rx data\n");
+
+			kfree_skb(skb);
+			skb = NULL;
+
+			kfree_skb(ndev->rx_data_reassembly);
+			ndev->rx_data_reassembly = NULL;
+
+			err = -ENOMEM;
+			goto exit;
+		}
+
+		/* second, combine the two fragments */
+		memcpy(skb_push(skb, reassembly_len),
+		       ndev->rx_data_reassembly->data,
+		       reassembly_len);
+
+		/* third, free old reassembly */
+		kfree_skb(ndev->rx_data_reassembly);
+		ndev->rx_data_reassembly = NULL;
+	}
+
+	if (pbf == NCI_PBF_CONT) {
+		/* need to wait for next fragment, store skb and exit */
+		ndev->rx_data_reassembly = skb;
+		return;
+	}
+
+exit:
+	nci_data_exchange_complete(ndev, skb, err);
+}
+
+/* Rx Data packet */
+void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u8 pbf = nci_pbf(skb->data);
+
+	pr_debug("len %d\n", skb->len);
+
+	pr_debug("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
+		 nci_pbf(skb->data),
+		 nci_conn_id(skb->data),
+		 nci_plen(skb->data));
+
+	/* strip the nci data header */
+	skb_pull(skb, NCI_DATA_HDR_SIZE);
+
+	if (ndev->target_active_prot == NFC_PROTO_MIFARE ||
+	    ndev->target_active_prot == NFC_PROTO_JEWEL ||
+	    ndev->target_active_prot == NFC_PROTO_FELICA ||
+	    ndev->target_active_prot == NFC_PROTO_ISO15693) {
+		/* frame I/F => remove the status byte */
+		pr_debug("frame I/F => remove the status byte\n");
+		skb_trim(skb, (skb->len - 1));
+	}
+
+	nci_add_rx_data_frag(ndev, skb, pbf);
+}

net/nfc/nci/lib.c

@@ -0,0 +1,85 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on lib.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+
+/* NCI status codes to Unix errno mapping */
+int nci_to_errno(__u8 code)
+{
+	switch (code) {
+	case NCI_STATUS_OK:
+		return 0;
+
+	case NCI_STATUS_REJECTED:
+		return -EBUSY;
+
+	case NCI_STATUS_RF_FRAME_CORRUPTED:
+		return -EBADMSG;
+
+	case NCI_STATUS_NOT_INITIALIZED:
+		return -EHOSTDOWN;
+
+	case NCI_STATUS_SYNTAX_ERROR:
+	case NCI_STATUS_SEMANTIC_ERROR:
+	case NCI_STATUS_INVALID_PARAM:
+	case NCI_STATUS_RF_PROTOCOL_ERROR:
+	case NCI_STATUS_NFCEE_PROTOCOL_ERROR:
+		return -EPROTO;
+
+	case NCI_STATUS_UNKNOWN_GID:
+	case NCI_STATUS_UNKNOWN_OID:
+		return -EBADRQC;
+
+	case NCI_STATUS_MESSAGE_SIZE_EXCEEDED:
+		return -EMSGSIZE;
+
+	case NCI_STATUS_DISCOVERY_ALREADY_STARTED:
+		return -EALREADY;
+
+	case NCI_STATUS_DISCOVERY_TARGET_ACTIVATION_FAILED:
+	case NCI_STATUS_NFCEE_INTERFACE_ACTIVATION_FAILED:
+		return -ECONNREFUSED;
+
+	case NCI_STATUS_RF_TRANSMISSION_ERROR:
+	case NCI_STATUS_NFCEE_TRANSMISSION_ERROR:
+		return -ECOMM;
+
+	case NCI_STATUS_RF_TIMEOUT_ERROR:
+	case NCI_STATUS_NFCEE_TIMEOUT_ERROR:
+		return -ETIMEDOUT;
+
+	case NCI_STATUS_FAILED:
+	default:
+		return -ENOSYS;
+	}
+}
+EXPORT_SYMBOL(nci_to_errno);

net/nfc/nci/ntf.c

@@ -0,0 +1,647 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2014 Marvell International Ltd.
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on hci_event.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+/* Handle NCI Notification packets */
+
+static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev,
+					     struct sk_buff *skb)
+{
+	struct nci_core_conn_credit_ntf *ntf = (void *) skb->data;
+	int i;
+
+	pr_debug("num_entries %d\n", ntf->num_entries);
+
+	if (ntf->num_entries > NCI_MAX_NUM_CONN)
+		ntf->num_entries = NCI_MAX_NUM_CONN;
+
+	/* update the credits */
+	for (i = 0; i < ntf->num_entries; i++) {
+		ntf->conn_entries[i].conn_id =
+			nci_conn_id(&ntf->conn_entries[i].conn_id);
+
+		pr_debug("entry[%d]: conn_id %d, credits %d\n",
+			 i, ntf->conn_entries[i].conn_id,
+			 ntf->conn_entries[i].credits);
+
+		if (ntf->conn_entries[i].conn_id == NCI_STATIC_RF_CONN_ID) {
+			/* found static rf connection */
+			atomic_add(ntf->conn_entries[i].credits,
+				   &ndev->credits_cnt);
+		}
+	}
+
+	/* trigger the next tx */
+	if (!skb_queue_empty(&ndev->tx_q))
+		queue_work(ndev->tx_wq, &ndev->tx_work);
+}
+
+static void nci_core_generic_error_ntf_packet(struct nci_dev *ndev,
+					      struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) {
+		/* Activation failed, so complete the request
+		   (the state remains the same) */
+		nci_req_complete(ndev, status);
+	}
+}
+
+static void nci_core_conn_intf_error_ntf_packet(struct nci_dev *ndev,
+						struct sk_buff *skb)
+{
+	struct nci_core_intf_error_ntf *ntf = (void *) skb->data;
+
+	ntf->conn_id = nci_conn_id(&ntf->conn_id);
+
+	pr_debug("status 0x%x, conn_id %d\n", ntf->status, ntf->conn_id);
+
+	/* complete the data exchange transaction, if exists */
+	if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+		nci_data_exchange_complete(ndev, NULL, -EIO);
+}
+
+static __u8 *nci_extract_rf_params_nfca_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfca_poll *nfca_poll,
+						     __u8 *data)
+{
+	nfca_poll->sens_res = __le16_to_cpu(*((__u16 *)data));
+	data += 2;
+
+	nfca_poll->nfcid1_len = min_t(__u8, *data++, NFC_NFCID1_MAXSIZE);
+
+	pr_debug("sens_res 0x%x, nfcid1_len %d\n",
+		 nfca_poll->sens_res, nfca_poll->nfcid1_len);
+
+	memcpy(nfca_poll->nfcid1, data, nfca_poll->nfcid1_len);
+	data += nfca_poll->nfcid1_len;
+
+	nfca_poll->sel_res_len = *data++;
+
+	if (nfca_poll->sel_res_len != 0)
+		nfca_poll->sel_res = *data++;
+
+	pr_debug("sel_res_len %d, sel_res 0x%x\n",
+		 nfca_poll->sel_res_len,
+		 nfca_poll->sel_res);
+
+	return data;
+}
+
+static __u8 *nci_extract_rf_params_nfcb_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfcb_poll *nfcb_poll,
+						     __u8 *data)
+{
+	nfcb_poll->sensb_res_len = min_t(__u8, *data++, NFC_SENSB_RES_MAXSIZE);
+
+	pr_debug("sensb_res_len %d\n", nfcb_poll->sensb_res_len);
+
+	memcpy(nfcb_poll->sensb_res, data, nfcb_poll->sensb_res_len);
+	data += nfcb_poll->sensb_res_len;
+
+	return data;
+}
+
+static __u8 *nci_extract_rf_params_nfcf_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfcf_poll *nfcf_poll,
+						     __u8 *data)
+{
+	nfcf_poll->bit_rate = *data++;
+	nfcf_poll->sensf_res_len = min_t(__u8, *data++, NFC_SENSF_RES_MAXSIZE);
+
+	pr_debug("bit_rate %d, sensf_res_len %d\n",
+		 nfcf_poll->bit_rate, nfcf_poll->sensf_res_len);
+
+	memcpy(nfcf_poll->sensf_res, data, nfcf_poll->sensf_res_len);
+	data += nfcf_poll->sensf_res_len;
+
+	return data;
+}
+
+static __u8 *nci_extract_rf_params_nfcv_passive_poll(struct nci_dev *ndev,
+			struct rf_tech_specific_params_nfcv_poll *nfcv_poll,
+						     __u8 *data)
+{
+	++data;
+	nfcv_poll->dsfid = *data++;
+	memcpy(nfcv_poll->uid, data, NFC_ISO15693_UID_MAXSIZE);
+	data += NFC_ISO15693_UID_MAXSIZE;
+	return data;
+}
+
+__u32 nci_get_prop_rf_protocol(struct nci_dev *ndev, __u8 rf_protocol)
+{
+	if (ndev->ops->get_rfprotocol)
+		return ndev->ops->get_rfprotocol(ndev, rf_protocol);
+	return 0;
+}
+
+static int nci_add_new_protocol(struct nci_dev *ndev,
+				struct nfc_target *target,
+				__u8 rf_protocol,
+				__u8 rf_tech_and_mode,
+				void *params)
+{
+	struct rf_tech_specific_params_nfca_poll *nfca_poll;
+	struct rf_tech_specific_params_nfcb_poll *nfcb_poll;
+	struct rf_tech_specific_params_nfcf_poll *nfcf_poll;
+	struct rf_tech_specific_params_nfcv_poll *nfcv_poll;
+	__u32 protocol;
+
+	if (rf_protocol == NCI_RF_PROTOCOL_T1T)
+		protocol = NFC_PROTO_JEWEL_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_T2T)
+		protocol = NFC_PROTO_MIFARE_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_ISO_DEP)
+		if (rf_tech_and_mode == NCI_NFC_A_PASSIVE_POLL_MODE)
+			protocol = NFC_PROTO_ISO14443_MASK;
+		else
+			protocol = NFC_PROTO_ISO14443_B_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_T3T)
+		protocol = NFC_PROTO_FELICA_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_NFC_DEP)
+		protocol = NFC_PROTO_NFC_DEP_MASK;
+	else if (rf_protocol == NCI_RF_PROTOCOL_T5T)
+		protocol = NFC_PROTO_ISO15693_MASK;
+	else
+		protocol = nci_get_prop_rf_protocol(ndev, rf_protocol);
+
+	if (!(protocol & ndev->poll_prots)) {
+		pr_err("the target found does not have the desired protocol\n");
+		return -EPROTO;
+	}
+
+	if (rf_tech_and_mode == NCI_NFC_A_PASSIVE_POLL_MODE) {
+		nfca_poll = (struct rf_tech_specific_params_nfca_poll *)params;
+
+		target->sens_res = nfca_poll->sens_res;
+		target->sel_res = nfca_poll->sel_res;
+		target->nfcid1_len = nfca_poll->nfcid1_len;
+		if (target->nfcid1_len > 0) {
+			memcpy(target->nfcid1, nfca_poll->nfcid1,
+			       target->nfcid1_len);
+		}
+	} else if (rf_tech_and_mode == NCI_NFC_B_PASSIVE_POLL_MODE) {
+		nfcb_poll = (struct rf_tech_specific_params_nfcb_poll *)params;
+
+		target->sensb_res_len = nfcb_poll->sensb_res_len;
+		if (target->sensb_res_len > 0) {
+			memcpy(target->sensb_res, nfcb_poll->sensb_res,
+			       target->sensb_res_len);
+		}
+	} else if (rf_tech_and_mode == NCI_NFC_F_PASSIVE_POLL_MODE) {
+		nfcf_poll = (struct rf_tech_specific_params_nfcf_poll *)params;
+
+		target->sensf_res_len = nfcf_poll->sensf_res_len;
+		if (target->sensf_res_len > 0) {
+			memcpy(target->sensf_res, nfcf_poll->sensf_res,
+			       target->sensf_res_len);
+		}
+	} else if (rf_tech_and_mode == NCI_NFC_V_PASSIVE_POLL_MODE) {
+		nfcv_poll = (struct rf_tech_specific_params_nfcv_poll *)params;
+
+		target->is_iso15693 = 1;
+		target->iso15693_dsfid = nfcv_poll->dsfid;
+		memcpy(target->iso15693_uid, nfcv_poll->uid, NFC_ISO15693_UID_MAXSIZE);
+	} else {
+		pr_err("unsupported rf_tech_and_mode 0x%x\n", rf_tech_and_mode);
+		return -EPROTO;
+	}
+
+	target->supported_protocols |= protocol;
+
+	pr_debug("protocol 0x%x\n", protocol);
+
+	return 0;
+}
+
+static void nci_add_new_target(struct nci_dev *ndev,
+			       struct nci_rf_discover_ntf *ntf)
+{
+	struct nfc_target *target;
+	int i, rc;
+
+	for (i = 0; i < ndev->n_targets; i++) {
+		target = &ndev->targets[i];
+		if (target->logical_idx == ntf->rf_discovery_id) {
+			/* This target already exists, add the new protocol */
+			nci_add_new_protocol(ndev, target, ntf->rf_protocol,
+					     ntf->rf_tech_and_mode,
+					     &ntf->rf_tech_specific_params);
+			return;
+		}
+	}
+
+	/* This is a new target, check if we've enough room */
+	if (ndev->n_targets == NCI_MAX_DISCOVERED_TARGETS) {
+		pr_debug("not enough room, ignoring new target...\n");
+		return;
+	}
+
+	target = &ndev->targets[ndev->n_targets];
+
+	rc = nci_add_new_protocol(ndev, target, ntf->rf_protocol,
+				  ntf->rf_tech_and_mode,
+				  &ntf->rf_tech_specific_params);
+	if (!rc) {
+		target->logical_idx = ntf->rf_discovery_id;
+		ndev->n_targets++;
+
+		pr_debug("logical idx %d, n_targets %d\n", target->logical_idx,
+			 ndev->n_targets);
+	}
+}
+
+void nci_clear_target_list(struct nci_dev *ndev)
+{
+	memset(ndev->targets, 0,
+	       (sizeof(struct nfc_target)*NCI_MAX_DISCOVERED_TARGETS));
+
+	ndev->n_targets = 0;
+}
+
+static void nci_rf_discover_ntf_packet(struct nci_dev *ndev,
+				       struct sk_buff *skb)
+{
+	struct nci_rf_discover_ntf ntf;
+	__u8 *data = skb->data;
+	bool add_target = true;
+
+	ntf.rf_discovery_id = *data++;
+	ntf.rf_protocol = *data++;
+	ntf.rf_tech_and_mode = *data++;
+	ntf.rf_tech_specific_params_len = *data++;
+
+	pr_debug("rf_discovery_id %d\n", ntf.rf_discovery_id);
+	pr_debug("rf_protocol 0x%x\n", ntf.rf_protocol);
+	pr_debug("rf_tech_and_mode 0x%x\n", ntf.rf_tech_and_mode);
+	pr_debug("rf_tech_specific_params_len %d\n",
+		 ntf.rf_tech_specific_params_len);
+
+	if (ntf.rf_tech_specific_params_len > 0) {
+		switch (ntf.rf_tech_and_mode) {
+		case NCI_NFC_A_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfca_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfca_poll), data);
+			break;
+
+		case NCI_NFC_B_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcb_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcb_poll), data);
+			break;
+
+		case NCI_NFC_F_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcf_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcf_poll), data);
+			break;
+
+		case NCI_NFC_V_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcv_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcv_poll), data);
+			break;
+
+		default:
+			pr_err("unsupported rf_tech_and_mode 0x%x\n",
+			       ntf.rf_tech_and_mode);
+			data += ntf.rf_tech_specific_params_len;
+			add_target = false;
+		}
+	}
+
+	ntf.ntf_type = *data++;
+	pr_debug("ntf_type %d\n", ntf.ntf_type);
+
+	if (add_target == true)
+		nci_add_new_target(ndev, &ntf);
+
+	if (ntf.ntf_type == NCI_DISCOVER_NTF_TYPE_MORE) {
+		atomic_set(&ndev->state, NCI_W4_ALL_DISCOVERIES);
+	} else {
+		atomic_set(&ndev->state, NCI_W4_HOST_SELECT);
+		nfc_targets_found(ndev->nfc_dev, ndev->targets,
+				  ndev->n_targets);
+	}
+}
+
+static int nci_extract_activation_params_iso_dep(struct nci_dev *ndev,
+			struct nci_rf_intf_activated_ntf *ntf, __u8 *data)
+{
+	struct activation_params_nfca_poll_iso_dep *nfca_poll;
+	struct activation_params_nfcb_poll_iso_dep *nfcb_poll;
+
+	switch (ntf->activation_rf_tech_and_mode) {
+	case NCI_NFC_A_PASSIVE_POLL_MODE:
+		nfca_poll = &ntf->activation_params.nfca_poll_iso_dep;
+		nfca_poll->rats_res_len = min_t(__u8, *data++, 20);
+		pr_debug("rats_res_len %d\n", nfca_poll->rats_res_len);
+		if (nfca_poll->rats_res_len > 0) {
+			memcpy(nfca_poll->rats_res,
+			       data, nfca_poll->rats_res_len);
+		}
+		break;
+
+	case NCI_NFC_B_PASSIVE_POLL_MODE:
+		nfcb_poll = &ntf->activation_params.nfcb_poll_iso_dep;
+		nfcb_poll->attrib_res_len = min_t(__u8, *data++, 50);
+		pr_debug("attrib_res_len %d\n", nfcb_poll->attrib_res_len);
+		if (nfcb_poll->attrib_res_len > 0) {
+			memcpy(nfcb_poll->attrib_res,
+			       data, nfcb_poll->attrib_res_len);
+		}
+		break;
+
+	default:
+		pr_err("unsupported activation_rf_tech_and_mode 0x%x\n",
+		       ntf->activation_rf_tech_and_mode);
+		return NCI_STATUS_RF_PROTOCOL_ERROR;
+	}
+
+	return NCI_STATUS_OK;
+}
+
+static int nci_extract_activation_params_nfc_dep(struct nci_dev *ndev,
+			struct nci_rf_intf_activated_ntf *ntf, __u8 *data)
+{
+	struct activation_params_poll_nfc_dep *poll;
+
+	switch (ntf->activation_rf_tech_and_mode) {
+	case NCI_NFC_A_PASSIVE_POLL_MODE:
+	case NCI_NFC_F_PASSIVE_POLL_MODE:
+		poll = &ntf->activation_params.poll_nfc_dep;
+		poll->atr_res_len = min_t(__u8, *data++, 63);
+		pr_debug("atr_res_len %d\n", poll->atr_res_len);
+		if (poll->atr_res_len > 0)
+			memcpy(poll->atr_res, data, poll->atr_res_len);
+		break;
+
+	default:
+		pr_err("unsupported activation_rf_tech_and_mode 0x%x\n",
+		       ntf->activation_rf_tech_and_mode);
+		return NCI_STATUS_RF_PROTOCOL_ERROR;
+	}
+
+	return NCI_STATUS_OK;
+}
+
+static void nci_target_auto_activated(struct nci_dev *ndev,
+				      struct nci_rf_intf_activated_ntf *ntf)
+{
+	struct nfc_target *target;
+	int rc;
+
+	target = &ndev->targets[ndev->n_targets];
+
+	rc = nci_add_new_protocol(ndev, target, ntf->rf_protocol,
+				  ntf->activation_rf_tech_and_mode,
+				  &ntf->rf_tech_specific_params);
+	if (rc)
+		return;
+
+	target->logical_idx = ntf->rf_discovery_id;
+	ndev->n_targets++;
+
+	pr_debug("logical idx %d, n_targets %d\n",
+		 target->logical_idx, ndev->n_targets);
+
+	nfc_targets_found(ndev->nfc_dev, ndev->targets, ndev->n_targets);
+}
+
+static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev,
+					     struct sk_buff *skb)
+{
+	struct nci_rf_intf_activated_ntf ntf;
+	__u8 *data = skb->data;
+	int err = NCI_STATUS_OK;
+
+	ntf.rf_discovery_id = *data++;
+	ntf.rf_interface = *data++;
+	ntf.rf_protocol = *data++;
+	ntf.activation_rf_tech_and_mode = *data++;
+	ntf.max_data_pkt_payload_size = *data++;
+	ntf.initial_num_credits = *data++;
+	ntf.rf_tech_specific_params_len = *data++;
+
+	pr_debug("rf_discovery_id %d\n", ntf.rf_discovery_id);
+	pr_debug("rf_interface 0x%x\n", ntf.rf_interface);
+	pr_debug("rf_protocol 0x%x\n", ntf.rf_protocol);
+	pr_debug("activation_rf_tech_and_mode 0x%x\n",
+		 ntf.activation_rf_tech_and_mode);
+	pr_debug("max_data_pkt_payload_size 0x%x\n",
+		 ntf.max_data_pkt_payload_size);
+	pr_debug("initial_num_credits 0x%x\n",
+		 ntf.initial_num_credits);
+	pr_debug("rf_tech_specific_params_len %d\n",
+		 ntf.rf_tech_specific_params_len);
+
+	if (ntf.rf_tech_specific_params_len > 0) {
+		switch (ntf.activation_rf_tech_and_mode) {
+		case NCI_NFC_A_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfca_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfca_poll), data);
+			break;
+
+		case NCI_NFC_B_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcb_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcb_poll), data);
+			break;
+
+		case NCI_NFC_F_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcf_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcf_poll), data);
+			break;
+
+		case NCI_NFC_V_PASSIVE_POLL_MODE:
+			data = nci_extract_rf_params_nfcv_passive_poll(ndev,
+				&(ntf.rf_tech_specific_params.nfcv_poll), data);
+			break;
+
+		default:
+			pr_err("unsupported activation_rf_tech_and_mode 0x%x\n",
+			       ntf.activation_rf_tech_and_mode);
+			err = NCI_STATUS_RF_PROTOCOL_ERROR;
+			goto exit;
+		}
+	}
+
+	ntf.data_exch_rf_tech_and_mode = *data++;
+	ntf.data_exch_tx_bit_rate = *data++;
+	ntf.data_exch_rx_bit_rate = *data++;
+	ntf.activation_params_len = *data++;
+
+	pr_debug("data_exch_rf_tech_and_mode 0x%x\n",
+		 ntf.data_exch_rf_tech_and_mode);
+	pr_debug("data_exch_tx_bit_rate 0x%x\n", ntf.data_exch_tx_bit_rate);
+	pr_debug("data_exch_rx_bit_rate 0x%x\n", ntf.data_exch_rx_bit_rate);
+	pr_debug("activation_params_len %d\n", ntf.activation_params_len);
+
+	if (ntf.activation_params_len > 0) {
+		switch (ntf.rf_interface) {
+		case NCI_RF_INTERFACE_ISO_DEP:
+			err = nci_extract_activation_params_iso_dep(ndev,
+								    &ntf, data);
+			break;
+
+		case NCI_RF_INTERFACE_NFC_DEP:
+			err = nci_extract_activation_params_nfc_dep(ndev,
+								    &ntf, data);
+			break;
+
+		case NCI_RF_INTERFACE_FRAME:
+			/* no activation params */
+			break;
+
+		default:
+			pr_err("unsupported rf_interface 0x%x\n",
+			       ntf.rf_interface);
+			err = NCI_STATUS_RF_PROTOCOL_ERROR;
+			break;
+		}
+	}
+
+exit:
+	if (err == NCI_STATUS_OK) {
+		ndev->max_data_pkt_payload_size = ntf.max_data_pkt_payload_size;
+		ndev->initial_num_credits = ntf.initial_num_credits;
+
+		/* set the available credits to initial value */
+		atomic_set(&ndev->credits_cnt, ndev->initial_num_credits);
+
+		/* store general bytes to be reported later in dep_link_up */
+		if (ntf.rf_interface == NCI_RF_INTERFACE_NFC_DEP) {
+			ndev->remote_gb_len = 0;
+
+			if (ntf.activation_params_len > 0) {
+				/* ATR_RES general bytes at offset 15 */
+				ndev->remote_gb_len = min_t(__u8,
+					(ntf.activation_params
+					.poll_nfc_dep.atr_res_len
+					- NFC_ATR_RES_GT_OFFSET),
+					NFC_MAX_GT_LEN);
+				memcpy(ndev->remote_gb,
+				       (ntf.activation_params.poll_nfc_dep
+				       .atr_res + NFC_ATR_RES_GT_OFFSET),
+				       ndev->remote_gb_len);
+			}
+		}
+	}
+
+	if (atomic_read(&ndev->state) == NCI_DISCOVERY) {
+		/* A single target was found and activated automatically */
+		atomic_set(&ndev->state, NCI_POLL_ACTIVE);
+		if (err == NCI_STATUS_OK)
+			nci_target_auto_activated(ndev, &ntf);
+	} else {	/* ndev->state == NCI_W4_HOST_SELECT */
+		/* A selected target was activated, so complete the request */
+		atomic_set(&ndev->state, NCI_POLL_ACTIVE);
+		nci_req_complete(ndev, err);
+	}
+}
+
+static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
+					 struct sk_buff *skb)
+{
+	struct nci_rf_deactivate_ntf *ntf = (void *) skb->data;
+
+	pr_debug("entry, type 0x%x, reason 0x%x\n", ntf->type, ntf->reason);
+
+	/* drop tx data queue */
+	skb_queue_purge(&ndev->tx_q);
+
+	/* drop partial rx data packet */
+	if (ndev->rx_data_reassembly) {
+		kfree_skb(ndev->rx_data_reassembly);
+		ndev->rx_data_reassembly = NULL;
+	}
+
+	/* complete the data exchange transaction, if exists */
+	if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+		nci_data_exchange_complete(ndev, NULL, -EIO);
+
+	nci_clear_target_list(ndev);
+	atomic_set(&ndev->state, NCI_IDLE);
+	nci_req_complete(ndev, NCI_STATUS_OK);
+}
+
+void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u16 ntf_opcode = nci_opcode(skb->data);
+
+	pr_debug("NCI RX: MT=ntf, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
+		 nci_pbf(skb->data),
+		 nci_opcode_gid(ntf_opcode),
+		 nci_opcode_oid(ntf_opcode),
+		 nci_plen(skb->data));
+
+	/* strip the nci control header */
+	skb_pull(skb, NCI_CTRL_HDR_SIZE);
+
+	switch (ntf_opcode) {
+	case NCI_OP_CORE_CONN_CREDITS_NTF:
+		nci_core_conn_credits_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_GENERIC_ERROR_NTF:
+		nci_core_generic_error_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_INTF_ERROR_NTF:
+		nci_core_conn_intf_error_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_NTF:
+		nci_rf_discover_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_INTF_ACTIVATED_NTF:
+		nci_rf_intf_activated_ntf_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DEACTIVATE_NTF:
+		nci_rf_deactivate_ntf_packet(ndev, skb);
+		break;
+
+	default:
+		pr_err("unknown ntf opcode 0x%x\n", ntf_opcode);
+		break;
+	}
+
+	kfree_skb(skb);
+}

net/nfc/nci/rsp.c

@@ -0,0 +1,237 @@
+/*
+ *  The NFC Controller Interface is the communication protocol between an
+ *  NFC Controller (NFCC) and a Device Host (DH).
+ *
+ *  Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ *  Written by Ilan Elias <ilane@ti.com>
+ *
+ *  Acknowledgements:
+ *  This file is based on hci_event.c, which was written
+ *  by Maxim Krasnyansky.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2
+ *  as published by the Free Software Foundation
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+
+/* Handle NCI Response packets */
+
+static void nci_core_reset_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct nci_core_reset_rsp *rsp = (void *) skb->data;
+
+	pr_debug("status 0x%x\n", rsp->status);
+
+	if (rsp->status == NCI_STATUS_OK) {
+		ndev->nci_ver = rsp->nci_ver;
+		pr_debug("nci_ver 0x%x, config_status 0x%x\n",
+			 rsp->nci_ver, rsp->config_status);
+	}
+
+	nci_req_complete(ndev, rsp->status);
+}
+
+static void nci_core_init_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	struct nci_core_init_rsp_1 *rsp_1 = (void *) skb->data;
+	struct nci_core_init_rsp_2 *rsp_2;
+
+	pr_debug("status 0x%x\n", rsp_1->status);
+
+	if (rsp_1->status != NCI_STATUS_OK)
+		goto exit;
+
+	ndev->nfcc_features = __le32_to_cpu(rsp_1->nfcc_features);
+	ndev->num_supported_rf_interfaces = rsp_1->num_supported_rf_interfaces;
+
+	if (ndev->num_supported_rf_interfaces >
+	    NCI_MAX_SUPPORTED_RF_INTERFACES) {
+		ndev->num_supported_rf_interfaces =
+			NCI_MAX_SUPPORTED_RF_INTERFACES;
+	}
+
+	memcpy(ndev->supported_rf_interfaces,
+	       rsp_1->supported_rf_interfaces,
+	       ndev->num_supported_rf_interfaces);
+
+	rsp_2 = (void *) (skb->data + 6 + rsp_1->num_supported_rf_interfaces);
+
+	ndev->max_logical_connections = rsp_2->max_logical_connections;
+	ndev->max_routing_table_size =
+		__le16_to_cpu(rsp_2->max_routing_table_size);
+	ndev->max_ctrl_pkt_payload_len =
+		rsp_2->max_ctrl_pkt_payload_len;
+	ndev->max_size_for_large_params =
+		__le16_to_cpu(rsp_2->max_size_for_large_params);
+	ndev->manufact_id =
+		rsp_2->manufact_id;
+	ndev->manufact_specific_info =
+		__le32_to_cpu(rsp_2->manufact_specific_info);
+
+	pr_debug("nfcc_features 0x%x\n",
+		 ndev->nfcc_features);
+	pr_debug("num_supported_rf_interfaces %d\n",
+		 ndev->num_supported_rf_interfaces);
+	pr_debug("supported_rf_interfaces[0] 0x%x\n",
+		 ndev->supported_rf_interfaces[0]);
+	pr_debug("supported_rf_interfaces[1] 0x%x\n",
+		 ndev->supported_rf_interfaces[1]);
+	pr_debug("supported_rf_interfaces[2] 0x%x\n",
+		 ndev->supported_rf_interfaces[2]);
+	pr_debug("supported_rf_interfaces[3] 0x%x\n",
+		 ndev->supported_rf_interfaces[3]);
+	pr_debug("max_logical_connections %d\n",
+		 ndev->max_logical_connections);
+	pr_debug("max_routing_table_size %d\n",
+		 ndev->max_routing_table_size);
+	pr_debug("max_ctrl_pkt_payload_len %d\n",
+		 ndev->max_ctrl_pkt_payload_len);
+	pr_debug("max_size_for_large_params %d\n",
+		 ndev->max_size_for_large_params);
+	pr_debug("manufact_id 0x%x\n",
+		 ndev->manufact_id);
+	pr_debug("manufact_specific_info 0x%x\n",
+		 ndev->manufact_specific_info);
+
+exit:
+	nci_req_complete(ndev, rsp_1->status);
+}
+
+static void nci_core_set_config_rsp_packet(struct nci_dev *ndev,
+					   struct sk_buff *skb)
+{
+	struct nci_core_set_config_rsp *rsp = (void *) skb->data;
+
+	pr_debug("status 0x%x\n", rsp->status);
+
+	nci_req_complete(ndev, rsp->status);
+}
+
+static void nci_rf_disc_map_rsp_packet(struct nci_dev *ndev,
+				       struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	nci_req_complete(ndev, status);
+}
+
+static void nci_rf_disc_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	if (status == NCI_STATUS_OK)
+		atomic_set(&ndev->state, NCI_DISCOVERY);
+
+	nci_req_complete(ndev, status);
+}
+
+static void nci_rf_disc_select_rsp_packet(struct nci_dev *ndev,
+					  struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	/* Complete the request on intf_activated_ntf or generic_error_ntf */
+	if (status != NCI_STATUS_OK)
+		nci_req_complete(ndev, status);
+}
+
+static void nci_rf_deactivate_rsp_packet(struct nci_dev *ndev,
+					 struct sk_buff *skb)
+{
+	__u8 status = skb->data[0];
+
+	pr_debug("status 0x%x\n", status);
+
+	/* If target was active, complete the request only in deactivate_ntf */
+	if ((status != NCI_STATUS_OK) ||
+	    (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
+		nci_clear_target_list(ndev);
+		atomic_set(&ndev->state, NCI_IDLE);
+		nci_req_complete(ndev, status);
+	}
+}
+
+void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+	__u16 rsp_opcode = nci_opcode(skb->data);
+
+	/* we got a rsp, stop the cmd timer */
+	del_timer(&ndev->cmd_timer);
+
+	pr_debug("NCI RX: MT=rsp, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
+		 nci_pbf(skb->data),
+		 nci_opcode_gid(rsp_opcode),
+		 nci_opcode_oid(rsp_opcode),
+		 nci_plen(skb->data));
+
+	/* strip the nci control header */
+	skb_pull(skb, NCI_CTRL_HDR_SIZE);
+
+	switch (rsp_opcode) {
+	case NCI_OP_CORE_RESET_RSP:
+		nci_core_reset_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_INIT_RSP:
+		nci_core_init_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_CORE_SET_CONFIG_RSP:
+		nci_core_set_config_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_MAP_RSP:
+		nci_rf_disc_map_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_RSP:
+		nci_rf_disc_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DISCOVER_SELECT_RSP:
+		nci_rf_disc_select_rsp_packet(ndev, skb);
+		break;
+
+	case NCI_OP_RF_DEACTIVATE_RSP:
+		nci_rf_deactivate_rsp_packet(ndev, skb);
+		break;
+
+	default:
+		pr_err("unknown rsp opcode 0x%x\n", rsp_opcode);
+		break;
+	}
+
+	kfree_skb(skb);
+
+	/* trigger the next cmd */
+	atomic_set(&ndev->cmd_cnt, 1);
+	if (!skb_queue_empty(&ndev->cmd_q))
+		queue_work(ndev->cmd_wq, &ndev->cmd_work);
+}

net/nfc/nci/spi.c

@@ -0,0 +1,322 @@
+/*
+ * Copyright (C) 2013  Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+#define pr_fmt(fmt) "nci_spi: %s: " fmt, __func__
+
+#include <linux/export.h>
+#include <linux/spi/spi.h>
+#include <linux/crc-ccitt.h>
+#include <net/nfc/nci_core.h>
+
+#define NCI_SPI_ACK_SHIFT		6
+#define NCI_SPI_MSB_PAYLOAD_MASK	0x3F
+
+#define NCI_SPI_SEND_TIMEOUT	(NCI_CMD_TIMEOUT > NCI_DATA_TIMEOUT ? \
+					NCI_CMD_TIMEOUT : NCI_DATA_TIMEOUT)
+
+#define NCI_SPI_DIRECT_WRITE	0x01
+#define NCI_SPI_DIRECT_READ	0x02
+
+#define ACKNOWLEDGE_NONE	0
+#define ACKNOWLEDGE_ACK		1
+#define ACKNOWLEDGE_NACK	2
+
+#define CRC_INIT		0xFFFF
+
+static int __nci_spi_send(struct nci_spi *nspi, struct sk_buff *skb,
+			  int cs_change)
+{
+	struct spi_message m;
+	struct spi_transfer t;
+
+	memset(&t, 0, sizeof(struct spi_transfer));
+	/* a NULL skb means we just want the SPI chip select line to raise */
+	if (skb) {
+		t.tx_buf = skb->data;
+		t.len = skb->len;
+	} else {
+		/* still set tx_buf non NULL to make the driver happy */
+		t.tx_buf = &t;
+		t.len = 0;
+	}
+	t.cs_change = cs_change;
+	t.delay_usecs = nspi->xfer_udelay;
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t, &m);
+
+	return spi_sync(nspi->spi, &m);
+}
+
+int nci_spi_send(struct nci_spi *nspi,
+		 struct completion *write_handshake_completion,
+		 struct sk_buff *skb)
+{
+	unsigned int payload_len = skb->len;
+	unsigned char *hdr;
+	int ret;
+	long completion_rc;
+
+	/* add the NCI SPI header to the start of the buffer */
+	hdr = skb_push(skb, NCI_SPI_HDR_LEN);
+	hdr[0] = NCI_SPI_DIRECT_WRITE;
+	hdr[1] = nspi->acknowledge_mode;
+	hdr[2] = payload_len >> 8;
+	hdr[3] = payload_len & 0xFF;
+
+	if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
+		u16 crc;
+
+		crc = crc_ccitt(CRC_INIT, skb->data, skb->len);
+		*skb_put(skb, 1) = crc >> 8;
+		*skb_put(skb, 1) = crc & 0xFF;
+	}
+
+	if (write_handshake_completion)	{
+		/* Trick SPI driver to raise chip select */
+		ret = __nci_spi_send(nspi, NULL, 1);
+		if (ret)
+			goto done;
+
+		/* wait for NFC chip hardware handshake to complete */
+		if (wait_for_completion_timeout(write_handshake_completion,
+						msecs_to_jiffies(1000)) == 0) {
+			ret = -ETIME;
+			goto done;
+		}
+	}
+
+	ret = __nci_spi_send(nspi, skb, 0);
+	if (ret != 0 || nspi->acknowledge_mode == NCI_SPI_CRC_DISABLED)
+		goto done;
+
+	reinit_completion(&nspi->req_completion);
+	completion_rc =	wait_for_completion_interruptible_timeout(
+							&nspi->req_completion,
+							NCI_SPI_SEND_TIMEOUT);
+
+	if (completion_rc <= 0 || nspi->req_result == ACKNOWLEDGE_NACK)
+		ret = -EIO;
+
+done:
+	kfree_skb(skb);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(nci_spi_send);
+
+/* ---- Interface to NCI SPI drivers ---- */
+
+/**
+ * nci_spi_allocate_spi - allocate a new nci spi
+ *
+ * @spi: SPI device
+ * @acknowledge_mode: Acknowledge mode used by the NFC device
+ * @delay: delay between transactions in us
+ * @ndev: nci dev to send incoming nci frames to
+ */
+struct nci_spi *nci_spi_allocate_spi(struct spi_device *spi,
+				     u8 acknowledge_mode, unsigned int delay,
+				     struct nci_dev *ndev)
+{
+	struct nci_spi *nspi;
+
+	nspi = devm_kzalloc(&spi->dev, sizeof(struct nci_spi), GFP_KERNEL);
+	if (!nspi)
+		return NULL;
+
+	nspi->acknowledge_mode = acknowledge_mode;
+	nspi->xfer_udelay = delay;
+
+	nspi->spi = spi;
+	nspi->ndev = ndev;
+	init_completion(&nspi->req_completion);
+
+	return nspi;
+}
+EXPORT_SYMBOL_GPL(nci_spi_allocate_spi);
+
+static int send_acknowledge(struct nci_spi *nspi, u8 acknowledge)
+{
+	struct sk_buff *skb;
+	unsigned char *hdr;
+	u16 crc;
+	int ret;
+
+	skb = nci_skb_alloc(nspi->ndev, 0, GFP_KERNEL);
+
+	/* add the NCI SPI header to the start of the buffer */
+	hdr = skb_push(skb, NCI_SPI_HDR_LEN);
+	hdr[0] = NCI_SPI_DIRECT_WRITE;
+	hdr[1] = NCI_SPI_CRC_ENABLED;
+	hdr[2] = acknowledge << NCI_SPI_ACK_SHIFT;
+	hdr[3] = 0;
+
+	crc = crc_ccitt(CRC_INIT, skb->data, skb->len);
+	*skb_put(skb, 1) = crc >> 8;
+	*skb_put(skb, 1) = crc & 0xFF;
+
+	ret = __nci_spi_send(nspi, skb, 0);
+
+	kfree_skb(skb);
+
+	return ret;
+}
+
+static struct sk_buff *__nci_spi_read(struct nci_spi *nspi)
+{
+	struct sk_buff *skb;
+	struct spi_message m;
+	unsigned char req[2], resp_hdr[2];
+	struct spi_transfer tx, rx;
+	unsigned short rx_len = 0;
+	int ret;
+
+	spi_message_init(&m);
+
+	memset(&tx, 0, sizeof(struct spi_transfer));
+	req[0] = NCI_SPI_DIRECT_READ;
+	req[1] = nspi->acknowledge_mode;
+	tx.tx_buf = req;
+	tx.len = 2;
+	tx.cs_change = 0;
+	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;
+	spi_message_add_tail(&rx, &m);
+
+	ret = spi_sync(nspi->spi, &m);
+	if (ret)
+		return NULL;
+
+	if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED)
+		rx_len = ((resp_hdr[0] & NCI_SPI_MSB_PAYLOAD_MASK) << 8) +
+				resp_hdr[1] + NCI_SPI_CRC_LEN;
+	else
+		rx_len = (resp_hdr[0] << 8) | resp_hdr[1];
+
+	skb = nci_skb_alloc(nspi->ndev, rx_len, GFP_KERNEL);
+	if (!skb)
+		return NULL;
+
+	spi_message_init(&m);
+
+	memset(&rx, 0, sizeof(struct spi_transfer));
+	rx.rx_buf = skb_put(skb, rx_len);
+	rx.len = rx_len;
+	rx.cs_change = 0;
+	rx.delay_usecs = nspi->xfer_udelay;
+	spi_message_add_tail(&rx, &m);
+
+	ret = spi_sync(nspi->spi, &m);
+	if (ret)
+		goto receive_error;
+
+	if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
+		*skb_push(skb, 1) = resp_hdr[1];
+		*skb_push(skb, 1) = resp_hdr[0];
+	}
+
+	return skb;
+
+receive_error:
+	kfree_skb(skb);
+
+	return NULL;
+}
+
+static int nci_spi_check_crc(struct sk_buff *skb)
+{
+	u16 crc_data = (skb->data[skb->len - 2] << 8) |
+			skb->data[skb->len - 1];
+	int ret;
+
+	ret = (crc_ccitt(CRC_INIT, skb->data, skb->len - NCI_SPI_CRC_LEN)
+			== crc_data);
+
+	skb_trim(skb, skb->len - NCI_SPI_CRC_LEN);
+
+	return ret;
+}
+
+static u8 nci_spi_get_ack(struct sk_buff *skb)
+{
+	u8 ret;
+
+	ret = skb->data[0] >> NCI_SPI_ACK_SHIFT;
+
+	/* Remove NFCC part of the header: ACK, NACK and MSB payload len */
+	skb_pull(skb, 2);
+
+	return ret;
+}
+
+/**
+ * nci_spi_read - read frame from NCI SPI drivers
+ *
+ * @nspi: The nci spi
+ * Context: can sleep
+ *
+ * This call may only be used from a context that may sleep.  The sleep
+ * is non-interruptible, and has no timeout.
+ *
+ * It returns an allocated skb containing the frame on success, or NULL.
+ */
+struct sk_buff *nci_spi_read(struct nci_spi *nspi)
+{
+	struct sk_buff *skb;
+
+	/* Retrieve frame from SPI */
+	skb = __nci_spi_read(nspi);
+	if (!skb)
+		goto done;
+
+	if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
+		if (!nci_spi_check_crc(skb)) {
+			send_acknowledge(nspi, ACKNOWLEDGE_NACK);
+			goto done;
+		}
+
+		/* In case of acknowledged mode: if ACK or NACK received,
+		 * unblock completion of latest frame sent.
+		 */
+		nspi->req_result = nci_spi_get_ack(skb);
+		if (nspi->req_result)
+			complete(&nspi->req_completion);
+	}
+
+	/* If there is no payload (ACK/NACK only frame),
+	 * free the socket buffer
+	 */
+	if (!skb->len) {
+		kfree_skb(skb);
+		skb = NULL;
+		goto done;
+	}
+
+	if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED)
+		send_acknowledge(nspi, ACKNOWLEDGE_ACK);
+
+done:
+
+	return skb;
+}
+EXPORT_SYMBOL_GPL(nci_spi_read);

net/nfc/nfc.h

@@ -0,0 +1,156 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __LOCAL_NFC_H
+#define __LOCAL_NFC_H
+
+#include <net/nfc/nfc.h>
+#include <net/sock.h>
+
+struct nfc_protocol {
+	int id;
+	struct proto *proto;
+	struct module *owner;
+	int (*create)(struct net *net, struct socket *sock,
+		      const struct nfc_protocol *nfc_proto);
+};
+
+struct nfc_rawsock {
+	struct sock sk;
+	struct nfc_dev *dev;
+	u32 target_idx;
+	struct work_struct tx_work;
+	bool tx_work_scheduled;
+};
+
+struct nfc_sock_list {
+	struct hlist_head head;
+	rwlock_t          lock;
+};
+
+#define nfc_rawsock(sk) ((struct nfc_rawsock *) sk)
+#define to_rawsock_sk(_tx_work) \
+	((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work))
+
+struct nfc_llcp_sdp_tlv;
+
+void nfc_llcp_mac_is_down(struct nfc_dev *dev);
+void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+			u8 comm_mode, u8 rf_mode);
+int nfc_llcp_register_device(struct nfc_dev *dev);
+void nfc_llcp_unregister_device(struct nfc_dev *dev);
+int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len);
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len);
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+int __init nfc_llcp_init(void);
+void nfc_llcp_exit(void);
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head);
+
+int __init rawsock_init(void);
+void rawsock_exit(void);
+
+int __init af_nfc_init(void);
+void af_nfc_exit(void);
+int nfc_proto_register(const struct nfc_protocol *nfc_proto);
+void nfc_proto_unregister(const struct nfc_protocol *nfc_proto);
+
+extern int nfc_devlist_generation;
+extern struct mutex nfc_devlist_mutex;
+
+int __init nfc_genl_init(void);
+void nfc_genl_exit(void);
+
+void nfc_genl_data_init(struct nfc_genl_data *genl_data);
+void nfc_genl_data_exit(struct nfc_genl_data *genl_data);
+
+int nfc_genl_targets_found(struct nfc_dev *dev);
+int nfc_genl_target_lost(struct nfc_dev *dev, u32 target_idx);
+
+int nfc_genl_device_added(struct nfc_dev *dev);
+int nfc_genl_device_removed(struct nfc_dev *dev);
+
+int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx,
+			       u8 comm_mode, u8 rf_mode);
+int nfc_genl_dep_link_down_event(struct nfc_dev *dev);
+
+int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol);
+int nfc_genl_tm_deactivated(struct nfc_dev *dev);
+
+int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list);
+
+int nfc_genl_se_added(struct nfc_dev *dev, u32 se_idx, u16 type);
+int nfc_genl_se_removed(struct nfc_dev *dev, u32 se_idx);
+
+struct nfc_dev *nfc_get_device(unsigned int idx);
+
+static inline void nfc_put_device(struct nfc_dev *dev)
+{
+	put_device(&dev->dev);
+}
+
+static inline void nfc_device_iter_init(struct class_dev_iter *iter)
+{
+	class_dev_iter_init(iter, &nfc_class, NULL, NULL);
+}
+
+static inline struct nfc_dev *nfc_device_iter_next(struct class_dev_iter *iter)
+{
+	struct device *d = class_dev_iter_next(iter);
+	if (!d)
+		return NULL;
+
+	return to_nfc_dev(d);
+}
+
+static inline void nfc_device_iter_exit(struct class_dev_iter *iter)
+{
+	class_dev_iter_exit(iter);
+}
+
+int nfc_fw_download(struct nfc_dev *dev, const char *firmware_name);
+int nfc_genl_fw_download_done(struct nfc_dev *dev, const char *firmware_name,
+			      u32 result);
+
+int nfc_dev_up(struct nfc_dev *dev);
+
+int nfc_dev_down(struct nfc_dev *dev);
+
+int nfc_start_poll(struct nfc_dev *dev, u32 im_protocols, u32 tm_protocols);
+
+int nfc_stop_poll(struct nfc_dev *dev);
+
+int nfc_dep_link_up(struct nfc_dev *dev, int target_idx, u8 comm_mode);
+
+int nfc_dep_link_down(struct nfc_dev *dev);
+
+int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol);
+
+int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx);
+
+int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
+		      data_exchange_cb_t cb, void *cb_context);
+
+int nfc_enable_se(struct nfc_dev *dev, u32 se_idx);
+int nfc_disable_se(struct nfc_dev *dev, u32 se_idx);
+
+#endif /* __LOCAL_NFC_H */

net/nfc/rawsock.c

@@ -0,0 +1,432 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <net/tcp_states.h>
+#include <linux/nfc.h>
+#include <linux/export.h>
+
+#include "nfc.h"
+
+static struct nfc_sock_list raw_sk_list = {
+	.lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock)
+};
+
+static void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk)
+{
+	write_lock(&l->lock);
+	sk_add_node(sk, &l->head);
+	write_unlock(&l->lock);
+}
+
+static void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk)
+{
+	write_lock(&l->lock);
+	sk_del_node_init(sk);
+	write_unlock(&l->lock);
+}
+
+static void rawsock_write_queue_purge(struct sock *sk)
+{
+	pr_debug("sk=%p\n", sk);
+
+	spin_lock_bh(&sk->sk_write_queue.lock);
+	__skb_queue_purge(&sk->sk_write_queue);
+	nfc_rawsock(sk)->tx_work_scheduled = false;
+	spin_unlock_bh(&sk->sk_write_queue.lock);
+}
+
+static void rawsock_report_error(struct sock *sk, int err)
+{
+	pr_debug("sk=%p err=%d\n", sk, err);
+
+	sk->sk_shutdown = SHUTDOWN_MASK;
+	sk->sk_err = -err;
+	sk->sk_error_report(sk);
+
+	rawsock_write_queue_purge(sk);
+}
+
+static int rawsock_release(struct socket *sock)
+{
+	struct sock *sk = sock->sk;
+
+	pr_debug("sock=%p sk=%p\n", sock, sk);
+
+	if (!sk)
+		return 0;
+
+	if (sock->type == SOCK_RAW)
+		nfc_sock_unlink(&raw_sk_list, sk);
+
+	sock_orphan(sk);
+	sock_put(sk);
+
+	return 0;
+}
+
+static int rawsock_connect(struct socket *sock, struct sockaddr *_addr,
+			   int len, int flags)
+{
+	struct sock *sk = sock->sk;
+	struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr;
+	struct nfc_dev *dev;
+	int rc = 0;
+
+	pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags);
+
+	if (!addr || len < sizeof(struct sockaddr_nfc) ||
+	    addr->sa_family != AF_NFC)
+		return -EINVAL;
+
+	pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n",
+		 addr->dev_idx, addr->target_idx, addr->nfc_protocol);
+
+	lock_sock(sk);
+
+	if (sock->state == SS_CONNECTED) {
+		rc = -EISCONN;
+		goto error;
+	}
+
+	dev = nfc_get_device(addr->dev_idx);
+	if (!dev) {
+		rc = -ENODEV;
+		goto error;
+	}
+
+	if (addr->target_idx > dev->target_next_idx - 1 ||
+	    addr->target_idx < dev->target_next_idx - dev->n_targets) {
+		rc = -EINVAL;
+		goto error;
+	}
+
+	rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol);
+	if (rc)
+		goto put_dev;
+
+	nfc_rawsock(sk)->dev = dev;
+	nfc_rawsock(sk)->target_idx = addr->target_idx;
+	sock->state = SS_CONNECTED;
+	sk->sk_state = TCP_ESTABLISHED;
+	sk->sk_state_change(sk);
+
+	release_sock(sk);
+	return 0;
+
+put_dev:
+	nfc_put_device(dev);
+error:
+	release_sock(sk);
+	return rc;
+}
+
+static int rawsock_add_header(struct sk_buff *skb)
+{
+	*skb_push(skb, NFC_HEADER_SIZE) = 0;
+
+	return 0;
+}
+
+static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
+					   int err)
+{
+	struct sock *sk = (struct sock *) context;
+
+	BUG_ON(in_irq());
+
+	pr_debug("sk=%p err=%d\n", sk, err);
+
+	if (err)
+		goto error;
+
+	err = rawsock_add_header(skb);
+	if (err)
+		goto error_skb;
+
+	err = sock_queue_rcv_skb(sk, skb);
+	if (err)
+		goto error_skb;
+
+	spin_lock_bh(&sk->sk_write_queue.lock);
+	if (!skb_queue_empty(&sk->sk_write_queue))
+		schedule_work(&nfc_rawsock(sk)->tx_work);
+	else
+		nfc_rawsock(sk)->tx_work_scheduled = false;
+	spin_unlock_bh(&sk->sk_write_queue.lock);
+
+	sock_put(sk);
+	return;
+
+error_skb:
+	kfree_skb(skb);
+
+error:
+	rawsock_report_error(sk, err);
+	sock_put(sk);
+}
+
+static void rawsock_tx_work(struct work_struct *work)
+{
+	struct sock *sk = to_rawsock_sk(work);
+	struct nfc_dev *dev = nfc_rawsock(sk)->dev;
+	u32 target_idx = nfc_rawsock(sk)->target_idx;
+	struct sk_buff *skb;
+	int rc;
+
+	pr_debug("sk=%p target_idx=%u\n", sk, target_idx);
+
+	if (sk->sk_shutdown & SEND_SHUTDOWN) {
+		rawsock_write_queue_purge(sk);
+		return;
+	}
+
+	skb = skb_dequeue(&sk->sk_write_queue);
+
+	sock_hold(sk);
+	rc = nfc_data_exchange(dev, target_idx, skb,
+			       rawsock_data_exchange_complete, sk);
+	if (rc) {
+		rawsock_report_error(sk, rc);
+		sock_put(sk);
+	}
+}
+
+static int rawsock_sendmsg(struct kiocb *iocb, struct socket *sock,
+			   struct msghdr *msg, size_t len)
+{
+	struct sock *sk = sock->sk;
+	struct nfc_dev *dev = nfc_rawsock(sk)->dev;
+	struct sk_buff *skb;
+	int rc;
+
+	pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len);
+
+	if (msg->msg_namelen)
+		return -EOPNOTSUPP;
+
+	if (sock->state != SS_CONNECTED)
+		return -ENOTCONN;
+
+	skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc);
+	if (skb == NULL)
+		return rc;
+
+	rc = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len);
+	if (rc < 0) {
+		kfree_skb(skb);
+		return rc;
+	}
+
+	spin_lock_bh(&sk->sk_write_queue.lock);
+	__skb_queue_tail(&sk->sk_write_queue, skb);
+	if (!nfc_rawsock(sk)->tx_work_scheduled) {
+		schedule_work(&nfc_rawsock(sk)->tx_work);
+		nfc_rawsock(sk)->tx_work_scheduled = true;
+	}
+	spin_unlock_bh(&sk->sk_write_queue.lock);
+
+	return len;
+}
+
+static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock,
+			   struct msghdr *msg, size_t len, int flags)
+{
+	int noblock = flags & MSG_DONTWAIT;
+	struct sock *sk = sock->sk;
+	struct sk_buff *skb;
+	int copied;
+	int rc;
+
+	pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
+
+	skb = skb_recv_datagram(sk, flags, noblock, &rc);
+	if (!skb)
+		return rc;
+
+	copied = skb->len;
+	if (len < copied) {
+		msg->msg_flags |= MSG_TRUNC;
+		copied = len;
+	}
+
+	rc = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
+
+	skb_free_datagram(sk, skb);
+
+	return rc ? : copied;
+}
+
+static const struct proto_ops rawsock_ops = {
+	.family         = PF_NFC,
+	.owner          = THIS_MODULE,
+	.release        = rawsock_release,
+	.bind           = sock_no_bind,
+	.connect        = rawsock_connect,
+	.socketpair     = sock_no_socketpair,
+	.accept         = sock_no_accept,
+	.getname        = sock_no_getname,
+	.poll           = datagram_poll,
+	.ioctl          = sock_no_ioctl,
+	.listen         = sock_no_listen,
+	.shutdown       = sock_no_shutdown,
+	.setsockopt     = sock_no_setsockopt,
+	.getsockopt     = sock_no_getsockopt,
+	.sendmsg        = rawsock_sendmsg,
+	.recvmsg        = rawsock_recvmsg,
+	.mmap           = sock_no_mmap,
+};
+
+static const struct proto_ops rawsock_raw_ops = {
+	.family         = PF_NFC,
+	.owner          = THIS_MODULE,
+	.release        = rawsock_release,
+	.bind           = sock_no_bind,
+	.connect        = sock_no_connect,
+	.socketpair     = sock_no_socketpair,
+	.accept         = sock_no_accept,
+	.getname        = sock_no_getname,
+	.poll           = datagram_poll,
+	.ioctl          = sock_no_ioctl,
+	.listen         = sock_no_listen,
+	.shutdown       = sock_no_shutdown,
+	.setsockopt     = sock_no_setsockopt,
+	.getsockopt     = sock_no_getsockopt,
+	.sendmsg        = sock_no_sendmsg,
+	.recvmsg        = rawsock_recvmsg,
+	.mmap           = sock_no_mmap,
+};
+
+static void rawsock_destruct(struct sock *sk)
+{
+	pr_debug("sk=%p\n", sk);
+
+	if (sk->sk_state == TCP_ESTABLISHED) {
+		nfc_deactivate_target(nfc_rawsock(sk)->dev,
+				      nfc_rawsock(sk)->target_idx);
+		nfc_put_device(nfc_rawsock(sk)->dev);
+	}
+
+	skb_queue_purge(&sk->sk_receive_queue);
+
+	if (!sock_flag(sk, SOCK_DEAD)) {
+		pr_err("Freeing alive NFC raw socket %p\n", sk);
+		return;
+	}
+}
+
+static int rawsock_create(struct net *net, struct socket *sock,
+			  const struct nfc_protocol *nfc_proto)
+{
+	struct sock *sk;
+
+	pr_debug("sock=%p\n", sock);
+
+	if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
+		return -ESOCKTNOSUPPORT;
+
+	if (sock->type == SOCK_RAW)
+		sock->ops = &rawsock_raw_ops;
+	else
+		sock->ops = &rawsock_ops;
+
+	sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto);
+	if (!sk)
+		return -ENOMEM;
+
+	sock_init_data(sock, sk);
+	sk->sk_protocol = nfc_proto->id;
+	sk->sk_destruct = rawsock_destruct;
+	sock->state = SS_UNCONNECTED;
+	if (sock->type == SOCK_RAW)
+		nfc_sock_link(&raw_sk_list, sk);
+	else {
+		INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
+		nfc_rawsock(sk)->tx_work_scheduled = false;
+	}
+
+	return 0;
+}
+
+void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
+			  u8 payload_type, u8 direction)
+{
+	struct sk_buff *skb_copy = NULL, *nskb;
+	struct sock *sk;
+	u8 *data;
+
+	read_lock(&raw_sk_list.lock);
+
+	sk_for_each(sk, &raw_sk_list.head) {
+		if (!skb_copy) {
+			skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
+						      GFP_ATOMIC, true);
+			if (!skb_copy)
+				continue;
+
+			data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
+
+			data[0] = dev ? dev->idx : 0xFF;
+			data[1] = direction & 0x01;
+			data[1] |= (payload_type << 1);
+		}
+
+		nskb = skb_clone(skb_copy, GFP_ATOMIC);
+		if (!nskb)
+			continue;
+
+		if (sock_queue_rcv_skb(sk, nskb))
+			kfree_skb(nskb);
+	}
+
+	read_unlock(&raw_sk_list.lock);
+
+	kfree_skb(skb_copy);
+}
+EXPORT_SYMBOL(nfc_send_to_raw_sock);
+
+static struct proto rawsock_proto = {
+	.name     = "NFC_RAW",
+	.owner    = THIS_MODULE,
+	.obj_size = sizeof(struct nfc_rawsock),
+};
+
+static const struct nfc_protocol rawsock_nfc_proto = {
+	.id	  = NFC_SOCKPROTO_RAW,
+	.proto    = &rawsock_proto,
+	.owner    = THIS_MODULE,
+	.create   = rawsock_create
+};
+
+int __init rawsock_init(void)
+{
+	int rc;
+
+	rc = nfc_proto_register(&rawsock_nfc_proto);
+
+	return rc;
+}
+
+void rawsock_exit(void)
+{
+	nfc_proto_unregister(&rawsock_nfc_proto);
+}