v4.19.13 snapshot.
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
new file mode 100644
index 0000000..6e0fa0c
--- /dev/null
+++ b/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"
diff --git a/net/nfc/Makefile b/net/nfc/Makefile
new file mode 100644
index 0000000..2ffc69b
--- /dev/null
+++ b/net/nfc/Makefile
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# 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
diff --git a/net/nfc/af_nfc.c b/net/nfc/af_nfc.c
new file mode 100644
index 0000000..d3e594e
--- /dev/null
+++ b/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], kern);
+ module_put(proto_tab[proto]->owner);
+ }
+ read_unlock(&proto_tab_lock);
+
+ return rc;
+}
+
+static const struct net_proto_family nfc_sock_family_ops = {
+ .owner = THIS_MODULE,
+ .family = PF_NFC,
+ .create = nfc_sock_create,
+};
+
+int nfc_proto_register(const struct nfc_protocol *nfc_proto)
+{
+ int rc;
+
+ if (nfc_proto->id < 0 || nfc_proto->id >= NFC_SOCKPROTO_MAX)
+ return -EINVAL;
+
+ rc = proto_register(nfc_proto->proto, 0);
+ if (rc)
+ return rc;
+
+ write_lock(&proto_tab_lock);
+ if (proto_tab[nfc_proto->id])
+ rc = -EBUSY;
+ else
+ proto_tab[nfc_proto->id] = nfc_proto;
+ write_unlock(&proto_tab_lock);
+
+ return rc;
+}
+EXPORT_SYMBOL(nfc_proto_register);
+
+void nfc_proto_unregister(const struct nfc_protocol *nfc_proto)
+{
+ write_lock(&proto_tab_lock);
+ proto_tab[nfc_proto->id] = NULL;
+ write_unlock(&proto_tab_lock);
+
+ proto_unregister(nfc_proto->proto);
+}
+EXPORT_SYMBOL(nfc_proto_unregister);
+
+int __init af_nfc_init(void)
+{
+ return sock_register(&nfc_sock_family_ops);
+}
+
+void af_nfc_exit(void)
+{
+ sock_unregister(PF_NFC);
+}
diff --git a/net/nfc/core.c b/net/nfc/core.c
new file mode 100644
index 0000000..947a470
--- /dev/null
+++ b/net/nfc/core.c
@@ -0,0 +1,1248 @@
+/*
+ * 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/>.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/rfkill.h>
+#include <linux/nfc.h>
+
+#include <net/genetlink.h>
+
+#include "nfc.h"
+
+#define VERSION "0.1"
+
+#define NFC_CHECK_PRES_FREQ_MS 2000
+
+int nfc_devlist_generation;
+DEFINE_MUTEX(nfc_devlist_mutex);
+
+/* NFC device ID bitmap */
+static DEFINE_IDA(nfc_index_ida);
+
+int nfc_fw_download(struct nfc_dev *dev, const char *firmware_name)
+{
+ int rc = 0;
+
+ pr_debug("%s do firmware %s\n", dev_name(&dev->dev), firmware_name);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->dev_up) {
+ rc = -EBUSY;
+ goto error;
+ }
+
+ if (!dev->ops->fw_download) {
+ rc = -EOPNOTSUPP;
+ goto error;
+ }
+
+ dev->fw_download_in_progress = true;
+ rc = dev->ops->fw_download(dev, firmware_name);
+ if (rc)
+ dev->fw_download_in_progress = false;
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+/**
+ * nfc_fw_download_done - inform that a firmware download was completed
+ *
+ * @dev: The nfc device to which firmware was downloaded
+ * @firmware_name: The firmware filename
+ * @result: The positive value of a standard errno value
+ */
+int nfc_fw_download_done(struct nfc_dev *dev, const char *firmware_name,
+ u32 result)
+{
+ dev->fw_download_in_progress = false;
+
+ return nfc_genl_fw_download_done(dev, firmware_name, result);
+}
+EXPORT_SYMBOL(nfc_fw_download_done);
+
+/**
+ * nfc_dev_up - turn on the NFC device
+ *
+ * @dev: The nfc device to be turned on
+ *
+ * The device remains up until the nfc_dev_down function is called.
+ */
+int nfc_dev_up(struct nfc_dev *dev)
+{
+ int rc = 0;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ device_lock(&dev->dev);
+
+ if (dev->rfkill && rfkill_blocked(dev->rfkill)) {
+ rc = -ERFKILL;
+ goto error;
+ }
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->fw_download_in_progress) {
+ rc = -EBUSY;
+ goto error;
+ }
+
+ if (dev->dev_up) {
+ rc = -EALREADY;
+ goto error;
+ }
+
+ if (dev->ops->dev_up)
+ rc = dev->ops->dev_up(dev);
+
+ if (!rc)
+ dev->dev_up = true;
+
+ /* We have to enable the device before discovering SEs */
+ if (dev->ops->discover_se && dev->ops->discover_se(dev))
+ pr_err("SE discovery failed\n");
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+/**
+ * nfc_dev_down - turn off the NFC device
+ *
+ * @dev: The nfc device to be turned off
+ */
+int nfc_dev_down(struct nfc_dev *dev)
+{
+ int rc = 0;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->dev_up) {
+ rc = -EALREADY;
+ goto error;
+ }
+
+ if (dev->polling || dev->active_target) {
+ rc = -EBUSY;
+ goto error;
+ }
+
+ if (dev->ops->dev_down)
+ dev->ops->dev_down(dev);
+
+ dev->dev_up = false;
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+static int nfc_rfkill_set_block(void *data, bool blocked)
+{
+ struct nfc_dev *dev = data;
+
+ pr_debug("%s blocked %d", dev_name(&dev->dev), blocked);
+
+ if (!blocked)
+ return 0;
+
+ nfc_dev_down(dev);
+
+ return 0;
+}
+
+static const struct rfkill_ops nfc_rfkill_ops = {
+ .set_block = nfc_rfkill_set_block,
+};
+
+/**
+ * nfc_start_poll - start polling for nfc targets
+ *
+ * @dev: The nfc device that must start polling
+ * @protocols: bitset of nfc protocols that must be used for polling
+ *
+ * The device remains polling for targets until a target is found or
+ * the nfc_stop_poll function is called.
+ */
+int nfc_start_poll(struct nfc_dev *dev, u32 im_protocols, u32 tm_protocols)
+{
+ int rc;
+
+ pr_debug("dev_name %s initiator protocols 0x%x target protocols 0x%x\n",
+ dev_name(&dev->dev), im_protocols, tm_protocols);
+
+ if (!im_protocols && !tm_protocols)
+ return -EINVAL;
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->dev_up) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->polling) {
+ rc = -EBUSY;
+ goto error;
+ }
+
+ rc = dev->ops->start_poll(dev, im_protocols, tm_protocols);
+ if (!rc) {
+ dev->polling = true;
+ dev->rf_mode = NFC_RF_NONE;
+ }
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+/**
+ * nfc_stop_poll - stop polling for nfc targets
+ *
+ * @dev: The nfc device that must stop polling
+ */
+int nfc_stop_poll(struct nfc_dev *dev)
+{
+ int rc = 0;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->polling) {
+ rc = -EINVAL;
+ goto error;
+ }
+
+ dev->ops->stop_poll(dev);
+ dev->polling = false;
+ dev->rf_mode = NFC_RF_NONE;
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+static struct nfc_target *nfc_find_target(struct nfc_dev *dev, u32 target_idx)
+{
+ int i;
+
+ for (i = 0; i < dev->n_targets; i++) {
+ if (dev->targets[i].idx == target_idx)
+ return &dev->targets[i];
+ }
+
+ return NULL;
+}
+
+int nfc_dep_link_up(struct nfc_dev *dev, int target_index, u8 comm_mode)
+{
+ int rc = 0;
+ u8 *gb;
+ size_t gb_len;
+ struct nfc_target *target;
+
+ pr_debug("dev_name=%s comm %d\n", dev_name(&dev->dev), comm_mode);
+
+ if (!dev->ops->dep_link_up)
+ return -EOPNOTSUPP;
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->dep_link_up == true) {
+ rc = -EALREADY;
+ goto error;
+ }
+
+ gb = nfc_llcp_general_bytes(dev, &gb_len);
+ if (gb_len > NFC_MAX_GT_LEN) {
+ rc = -EINVAL;
+ goto error;
+ }
+
+ target = nfc_find_target(dev, target_index);
+ if (target == NULL) {
+ rc = -ENOTCONN;
+ goto error;
+ }
+
+ rc = dev->ops->dep_link_up(dev, target, comm_mode, gb, gb_len);
+ if (!rc) {
+ dev->active_target = target;
+ dev->rf_mode = NFC_RF_INITIATOR;
+ }
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+int nfc_dep_link_down(struct nfc_dev *dev)
+{
+ int rc = 0;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ if (!dev->ops->dep_link_down)
+ return -EOPNOTSUPP;
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->dep_link_up == false) {
+ rc = -EALREADY;
+ goto error;
+ }
+
+ rc = dev->ops->dep_link_down(dev);
+ if (!rc) {
+ dev->dep_link_up = false;
+ dev->active_target = NULL;
+ dev->rf_mode = NFC_RF_NONE;
+ nfc_llcp_mac_is_down(dev);
+ nfc_genl_dep_link_down_event(dev);
+ }
+
+error:
+ device_unlock(&dev->dev);
+
+ return rc;
+}
+
+int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx,
+ u8 comm_mode, u8 rf_mode)
+{
+ dev->dep_link_up = true;
+
+ if (!dev->active_target && rf_mode == NFC_RF_INITIATOR) {
+ struct nfc_target *target;
+
+ target = nfc_find_target(dev, target_idx);
+ if (target == NULL)
+ return -ENOTCONN;
+
+ dev->active_target = target;
+ }
+
+ dev->polling = false;
+ dev->rf_mode = rf_mode;
+
+ nfc_llcp_mac_is_up(dev, target_idx, comm_mode, rf_mode);
+
+ return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode);
+}
+EXPORT_SYMBOL(nfc_dep_link_is_up);
+
+/**
+ * nfc_activate_target - prepare the target for data exchange
+ *
+ * @dev: The nfc device that found the target
+ * @target_idx: index of the target that must be activated
+ * @protocol: nfc protocol that will be used for data exchange
+ */
+int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol)
+{
+ int rc;
+ struct nfc_target *target;
+
+ pr_debug("dev_name=%s target_idx=%u protocol=%u\n",
+ dev_name(&dev->dev), target_idx, protocol);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->active_target) {
+ rc = -EBUSY;
+ goto error;
+ }
+
+ target = nfc_find_target(dev, target_idx);
+ if (target == NULL) {
+ rc = -ENOTCONN;
+ goto error;
+ }
+
+ rc = dev->ops->activate_target(dev, target, protocol);
+ if (!rc) {
+ dev->active_target = target;
+ dev->rf_mode = NFC_RF_INITIATOR;
+
+ if (dev->ops->check_presence && !dev->shutting_down)
+ mod_timer(&dev->check_pres_timer, jiffies +
+ msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+ }
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+/**
+ * nfc_deactivate_target - deactivate a nfc target
+ *
+ * @dev: The nfc device that found the target
+ * @target_idx: index of the target that must be deactivated
+ */
+int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx, u8 mode)
+{
+ int rc = 0;
+
+ pr_debug("dev_name=%s target_idx=%u\n",
+ dev_name(&dev->dev), target_idx);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->active_target == NULL) {
+ rc = -ENOTCONN;
+ goto error;
+ }
+
+ if (dev->active_target->idx != target_idx) {
+ rc = -ENOTCONN;
+ goto error;
+ }
+
+ if (dev->ops->check_presence)
+ del_timer_sync(&dev->check_pres_timer);
+
+ dev->ops->deactivate_target(dev, dev->active_target, mode);
+ dev->active_target = NULL;
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+/**
+ * nfc_data_exchange - transceive data
+ *
+ * @dev: The nfc device that found the target
+ * @target_idx: index of the target
+ * @skb: data to be sent
+ * @cb: callback called when the response is received
+ * @cb_context: parameter for the callback function
+ *
+ * The user must wait for the callback before calling this function again.
+ */
+int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
+ data_exchange_cb_t cb, void *cb_context)
+{
+ int rc;
+
+ pr_debug("dev_name=%s target_idx=%u skb->len=%u\n",
+ dev_name(&dev->dev), target_idx, skb->len);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ kfree_skb(skb);
+ goto error;
+ }
+
+ if (dev->rf_mode == NFC_RF_INITIATOR && dev->active_target != NULL) {
+ if (dev->active_target->idx != target_idx) {
+ rc = -EADDRNOTAVAIL;
+ kfree_skb(skb);
+ goto error;
+ }
+
+ if (dev->ops->check_presence)
+ del_timer_sync(&dev->check_pres_timer);
+
+ rc = dev->ops->im_transceive(dev, dev->active_target, skb, cb,
+ cb_context);
+
+ if (!rc && dev->ops->check_presence && !dev->shutting_down)
+ mod_timer(&dev->check_pres_timer, jiffies +
+ msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+ } else if (dev->rf_mode == NFC_RF_TARGET && dev->ops->tm_send != NULL) {
+ rc = dev->ops->tm_send(dev, skb);
+ } else {
+ rc = -ENOTCONN;
+ kfree_skb(skb);
+ goto error;
+ }
+
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+struct nfc_se *nfc_find_se(struct nfc_dev *dev, u32 se_idx)
+{
+ struct nfc_se *se;
+
+ list_for_each_entry(se, &dev->secure_elements, list)
+ if (se->idx == se_idx)
+ return se;
+
+ return NULL;
+}
+EXPORT_SYMBOL(nfc_find_se);
+
+int nfc_enable_se(struct nfc_dev *dev, u32 se_idx)
+{
+ struct nfc_se *se;
+ int rc;
+
+ pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->dev_up) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (dev->polling) {
+ rc = -EBUSY;
+ goto error;
+ }
+
+ if (!dev->ops->enable_se || !dev->ops->disable_se) {
+ rc = -EOPNOTSUPP;
+ goto error;
+ }
+
+ se = nfc_find_se(dev, se_idx);
+ if (!se) {
+ rc = -EINVAL;
+ goto error;
+ }
+
+ if (se->state == NFC_SE_ENABLED) {
+ rc = -EALREADY;
+ goto error;
+ }
+
+ rc = dev->ops->enable_se(dev, se_idx);
+ if (rc >= 0)
+ se->state = NFC_SE_ENABLED;
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+int nfc_disable_se(struct nfc_dev *dev, u32 se_idx)
+{
+ struct nfc_se *se;
+ int rc;
+
+ pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->dev_up) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->ops->enable_se || !dev->ops->disable_se) {
+ rc = -EOPNOTSUPP;
+ goto error;
+ }
+
+ se = nfc_find_se(dev, se_idx);
+ if (!se) {
+ rc = -EINVAL;
+ goto error;
+ }
+
+ if (se->state == NFC_SE_DISABLED) {
+ rc = -EALREADY;
+ goto error;
+ }
+
+ rc = dev->ops->disable_se(dev, se_idx);
+ if (rc >= 0)
+ se->state = NFC_SE_DISABLED;
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+int nfc_set_remote_general_bytes(struct nfc_dev *dev, u8 *gb, u8 gb_len)
+{
+ pr_debug("dev_name=%s gb_len=%d\n", dev_name(&dev->dev), gb_len);
+
+ return nfc_llcp_set_remote_gb(dev, gb, gb_len);
+}
+EXPORT_SYMBOL(nfc_set_remote_general_bytes);
+
+u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, size_t *gb_len)
+{
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ return nfc_llcp_general_bytes(dev, gb_len);
+}
+EXPORT_SYMBOL(nfc_get_local_general_bytes);
+
+int nfc_tm_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+ /* Only LLCP target mode for now */
+ if (dev->dep_link_up == false) {
+ kfree_skb(skb);
+ return -ENOLINK;
+ }
+
+ return nfc_llcp_data_received(dev, skb);
+}
+EXPORT_SYMBOL(nfc_tm_data_received);
+
+int nfc_tm_activated(struct nfc_dev *dev, u32 protocol, u8 comm_mode,
+ u8 *gb, size_t gb_len)
+{
+ int rc;
+
+ device_lock(&dev->dev);
+
+ dev->polling = false;
+
+ if (gb != NULL) {
+ rc = nfc_set_remote_general_bytes(dev, gb, gb_len);
+ if (rc < 0)
+ goto out;
+ }
+
+ dev->rf_mode = NFC_RF_TARGET;
+
+ if (protocol == NFC_PROTO_NFC_DEP_MASK)
+ nfc_dep_link_is_up(dev, 0, comm_mode, NFC_RF_TARGET);
+
+ rc = nfc_genl_tm_activated(dev, protocol);
+
+out:
+ device_unlock(&dev->dev);
+
+ return rc;
+}
+EXPORT_SYMBOL(nfc_tm_activated);
+
+int nfc_tm_deactivated(struct nfc_dev *dev)
+{
+ dev->dep_link_up = false;
+ dev->rf_mode = NFC_RF_NONE;
+
+ return nfc_genl_tm_deactivated(dev);
+}
+EXPORT_SYMBOL(nfc_tm_deactivated);
+
+/**
+ * nfc_alloc_send_skb - allocate a skb for data exchange responses
+ *
+ * @size: size to allocate
+ * @gfp: gfp flags
+ */
+struct sk_buff *nfc_alloc_send_skb(struct nfc_dev *dev, struct sock *sk,
+ unsigned int flags, unsigned int size,
+ unsigned int *err)
+{
+ struct sk_buff *skb;
+ unsigned int total_size;
+
+ total_size = size +
+ dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+ skb = sock_alloc_send_skb(sk, total_size, flags & MSG_DONTWAIT, err);
+ if (skb)
+ skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+ return skb;
+}
+
+/**
+ * nfc_alloc_recv_skb - allocate a skb for data exchange responses
+ *
+ * @size: size to allocate
+ * @gfp: gfp flags
+ */
+struct sk_buff *nfc_alloc_recv_skb(unsigned int size, gfp_t gfp)
+{
+ struct sk_buff *skb;
+ unsigned int total_size;
+
+ total_size = size + 1;
+ skb = alloc_skb(total_size, gfp);
+
+ if (skb)
+ skb_reserve(skb, 1);
+
+ return skb;
+}
+EXPORT_SYMBOL(nfc_alloc_recv_skb);
+
+/**
+ * nfc_targets_found - inform that targets were found
+ *
+ * @dev: The nfc device that found the targets
+ * @targets: array of nfc targets found
+ * @ntargets: targets array size
+ *
+ * The device driver must call this function when one or many nfc targets
+ * are found. After calling this function, the device driver must stop
+ * polling for targets.
+ * NOTE: This function can be called with targets=NULL and n_targets=0 to
+ * notify a driver error, meaning that the polling operation cannot complete.
+ * IMPORTANT: this function must not be called from an atomic context.
+ * In addition, it must also not be called from a context that would prevent
+ * the NFC Core to call other nfc ops entry point concurrently.
+ */
+int nfc_targets_found(struct nfc_dev *dev,
+ struct nfc_target *targets, int n_targets)
+{
+ int i;
+
+ pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets);
+
+ for (i = 0; i < n_targets; i++)
+ targets[i].idx = dev->target_next_idx++;
+
+ device_lock(&dev->dev);
+
+ if (dev->polling == false) {
+ device_unlock(&dev->dev);
+ return 0;
+ }
+
+ dev->polling = false;
+
+ dev->targets_generation++;
+
+ kfree(dev->targets);
+ dev->targets = NULL;
+
+ if (targets) {
+ dev->targets = kmemdup(targets,
+ n_targets * sizeof(struct nfc_target),
+ GFP_ATOMIC);
+
+ if (!dev->targets) {
+ dev->n_targets = 0;
+ device_unlock(&dev->dev);
+ return -ENOMEM;
+ }
+ }
+
+ dev->n_targets = n_targets;
+ device_unlock(&dev->dev);
+
+ nfc_genl_targets_found(dev);
+
+ return 0;
+}
+EXPORT_SYMBOL(nfc_targets_found);
+
+/**
+ * nfc_target_lost - inform that an activated target went out of field
+ *
+ * @dev: The nfc device that had the activated target in field
+ * @target_idx: the nfc index of the target
+ *
+ * The device driver must call this function when the activated target
+ * goes out of the field.
+ * IMPORTANT: this function must not be called from an atomic context.
+ * In addition, it must also not be called from a context that would prevent
+ * the NFC Core to call other nfc ops entry point concurrently.
+ */
+int nfc_target_lost(struct nfc_dev *dev, u32 target_idx)
+{
+ struct nfc_target *tg;
+ int i;
+
+ pr_debug("dev_name %s n_target %d\n", dev_name(&dev->dev), target_idx);
+
+ device_lock(&dev->dev);
+
+ for (i = 0; i < dev->n_targets; i++) {
+ tg = &dev->targets[i];
+ if (tg->idx == target_idx)
+ break;
+ }
+
+ if (i == dev->n_targets) {
+ device_unlock(&dev->dev);
+ return -EINVAL;
+ }
+
+ dev->targets_generation++;
+ dev->n_targets--;
+ dev->active_target = NULL;
+
+ if (dev->n_targets) {
+ memcpy(&dev->targets[i], &dev->targets[i + 1],
+ (dev->n_targets - i) * sizeof(struct nfc_target));
+ } else {
+ kfree(dev->targets);
+ dev->targets = NULL;
+ }
+
+ device_unlock(&dev->dev);
+
+ nfc_genl_target_lost(dev, target_idx);
+
+ return 0;
+}
+EXPORT_SYMBOL(nfc_target_lost);
+
+inline void nfc_driver_failure(struct nfc_dev *dev, int err)
+{
+ nfc_targets_found(dev, NULL, 0);
+}
+EXPORT_SYMBOL(nfc_driver_failure);
+
+int nfc_add_se(struct nfc_dev *dev, u32 se_idx, u16 type)
+{
+ struct nfc_se *se;
+ int rc;
+
+ pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
+
+ se = nfc_find_se(dev, se_idx);
+ if (se)
+ return -EALREADY;
+
+ se = kzalloc(sizeof(struct nfc_se), GFP_KERNEL);
+ if (!se)
+ return -ENOMEM;
+
+ se->idx = se_idx;
+ se->type = type;
+ se->state = NFC_SE_DISABLED;
+ INIT_LIST_HEAD(&se->list);
+
+ list_add(&se->list, &dev->secure_elements);
+
+ rc = nfc_genl_se_added(dev, se_idx, type);
+ if (rc < 0) {
+ list_del(&se->list);
+ kfree(se);
+
+ return rc;
+ }
+
+ return 0;
+}
+EXPORT_SYMBOL(nfc_add_se);
+
+int nfc_remove_se(struct nfc_dev *dev, u32 se_idx)
+{
+ struct nfc_se *se, *n;
+ int rc;
+
+ pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
+
+ list_for_each_entry_safe(se, n, &dev->secure_elements, list)
+ if (se->idx == se_idx) {
+ rc = nfc_genl_se_removed(dev, se_idx);
+ if (rc < 0)
+ return rc;
+
+ list_del(&se->list);
+ kfree(se);
+
+ return 0;
+ }
+
+ return -EINVAL;
+}
+EXPORT_SYMBOL(nfc_remove_se);
+
+int nfc_se_transaction(struct nfc_dev *dev, u8 se_idx,
+ struct nfc_evt_transaction *evt_transaction)
+{
+ int rc;
+
+ pr_debug("transaction: %x\n", se_idx);
+
+ device_lock(&dev->dev);
+
+ if (!evt_transaction) {
+ rc = -EPROTO;
+ goto out;
+ }
+
+ rc = nfc_genl_se_transaction(dev, se_idx, evt_transaction);
+out:
+ device_unlock(&dev->dev);
+ return rc;
+}
+EXPORT_SYMBOL(nfc_se_transaction);
+
+int nfc_se_connectivity(struct nfc_dev *dev, u8 se_idx)
+{
+ int rc;
+
+ pr_debug("connectivity: %x\n", se_idx);
+
+ device_lock(&dev->dev);
+ rc = nfc_genl_se_connectivity(dev, se_idx);
+ device_unlock(&dev->dev);
+ return rc;
+}
+EXPORT_SYMBOL(nfc_se_connectivity);
+
+static void nfc_release(struct device *d)
+{
+ struct nfc_dev *dev = to_nfc_dev(d);
+ struct nfc_se *se, *n;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ nfc_genl_data_exit(&dev->genl_data);
+ kfree(dev->targets);
+
+ list_for_each_entry_safe(se, n, &dev->secure_elements, list) {
+ nfc_genl_se_removed(dev, se->idx);
+ list_del(&se->list);
+ kfree(se);
+ }
+
+ ida_simple_remove(&nfc_index_ida, dev->idx);
+
+ kfree(dev);
+}
+
+static void nfc_check_pres_work(struct work_struct *work)
+{
+ struct nfc_dev *dev = container_of(work, struct nfc_dev,
+ check_pres_work);
+ int rc;
+
+ device_lock(&dev->dev);
+
+ if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) {
+ rc = dev->ops->check_presence(dev, dev->active_target);
+ if (rc == -EOPNOTSUPP)
+ goto exit;
+ if (rc) {
+ u32 active_target_idx = dev->active_target->idx;
+ device_unlock(&dev->dev);
+ nfc_target_lost(dev, active_target_idx);
+ return;
+ }
+
+ if (!dev->shutting_down)
+ mod_timer(&dev->check_pres_timer, jiffies +
+ msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
+ }
+
+exit:
+ device_unlock(&dev->dev);
+}
+
+static void nfc_check_pres_timeout(struct timer_list *t)
+{
+ struct nfc_dev *dev = from_timer(dev, t, check_pres_timer);
+
+ schedule_work(&dev->check_pres_work);
+}
+
+struct class nfc_class = {
+ .name = "nfc",
+ .dev_release = nfc_release,
+};
+EXPORT_SYMBOL(nfc_class);
+
+static int match_idx(struct device *d, const void *data)
+{
+ struct nfc_dev *dev = to_nfc_dev(d);
+ const unsigned int *idx = data;
+
+ return dev->idx == *idx;
+}
+
+struct nfc_dev *nfc_get_device(unsigned int idx)
+{
+ struct device *d;
+
+ d = class_find_device(&nfc_class, NULL, &idx, match_idx);
+ if (!d)
+ return NULL;
+
+ return to_nfc_dev(d);
+}
+
+/**
+ * nfc_allocate_device - allocate a new nfc device
+ *
+ * @ops: device operations
+ * @supported_protocols: NFC protocols supported by the device
+ */
+struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
+ u32 supported_protocols,
+ int tx_headroom, int tx_tailroom)
+{
+ struct nfc_dev *dev;
+ int rc;
+
+ if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
+ !ops->deactivate_target || !ops->im_transceive)
+ return NULL;
+
+ if (!supported_protocols)
+ return NULL;
+
+ dev = kzalloc(sizeof(struct nfc_dev), GFP_KERNEL);
+ if (!dev)
+ return NULL;
+
+ rc = ida_simple_get(&nfc_index_ida, 0, 0, GFP_KERNEL);
+ if (rc < 0)
+ goto err_free_dev;
+ dev->idx = rc;
+
+ dev->dev.class = &nfc_class;
+ dev_set_name(&dev->dev, "nfc%d", dev->idx);
+ device_initialize(&dev->dev);
+
+ dev->ops = ops;
+ dev->supported_protocols = supported_protocols;
+ dev->tx_headroom = tx_headroom;
+ dev->tx_tailroom = tx_tailroom;
+ INIT_LIST_HEAD(&dev->secure_elements);
+
+ nfc_genl_data_init(&dev->genl_data);
+
+ dev->rf_mode = NFC_RF_NONE;
+
+ /* first generation must not be 0 */
+ dev->targets_generation = 1;
+
+ if (ops->check_presence) {
+ timer_setup(&dev->check_pres_timer, nfc_check_pres_timeout, 0);
+ INIT_WORK(&dev->check_pres_work, nfc_check_pres_work);
+ }
+
+ return dev;
+
+err_free_dev:
+ kfree(dev);
+
+ return NULL;
+}
+EXPORT_SYMBOL(nfc_allocate_device);
+
+/**
+ * nfc_register_device - register a nfc device in the nfc subsystem
+ *
+ * @dev: The nfc device to register
+ */
+int nfc_register_device(struct nfc_dev *dev)
+{
+ int rc;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ mutex_lock(&nfc_devlist_mutex);
+ nfc_devlist_generation++;
+ rc = device_add(&dev->dev);
+ mutex_unlock(&nfc_devlist_mutex);
+
+ if (rc < 0)
+ return rc;
+
+ rc = nfc_llcp_register_device(dev);
+ if (rc)
+ pr_err("Could not register llcp device\n");
+
+ rc = nfc_genl_device_added(dev);
+ if (rc)
+ pr_debug("The userspace won't be notified that the device %s was added\n",
+ dev_name(&dev->dev));
+
+ dev->rfkill = rfkill_alloc(dev_name(&dev->dev), &dev->dev,
+ RFKILL_TYPE_NFC, &nfc_rfkill_ops, dev);
+ if (dev->rfkill) {
+ if (rfkill_register(dev->rfkill) < 0) {
+ rfkill_destroy(dev->rfkill);
+ dev->rfkill = NULL;
+ }
+ }
+
+ return 0;
+}
+EXPORT_SYMBOL(nfc_register_device);
+
+/**
+ * nfc_unregister_device - unregister a nfc device in the nfc subsystem
+ *
+ * @dev: The nfc device to unregister
+ */
+void nfc_unregister_device(struct nfc_dev *dev)
+{
+ int rc;
+
+ pr_debug("dev_name=%s\n", dev_name(&dev->dev));
+
+ if (dev->rfkill) {
+ rfkill_unregister(dev->rfkill);
+ rfkill_destroy(dev->rfkill);
+ }
+
+ if (dev->ops->check_presence) {
+ device_lock(&dev->dev);
+ dev->shutting_down = true;
+ device_unlock(&dev->dev);
+ del_timer_sync(&dev->check_pres_timer);
+ cancel_work_sync(&dev->check_pres_work);
+ }
+
+ rc = nfc_genl_device_removed(dev);
+ if (rc)
+ pr_debug("The userspace won't be notified that the device %s "
+ "was removed\n", dev_name(&dev->dev));
+
+ nfc_llcp_unregister_device(dev);
+
+ mutex_lock(&nfc_devlist_mutex);
+ nfc_devlist_generation++;
+ device_del(&dev->dev);
+ mutex_unlock(&nfc_devlist_mutex);
+}
+EXPORT_SYMBOL(nfc_unregister_device);
+
+static int __init nfc_init(void)
+{
+ int rc;
+
+ pr_info("NFC Core ver %s\n", VERSION);
+
+ rc = class_register(&nfc_class);
+ if (rc)
+ return rc;
+
+ rc = nfc_genl_init();
+ if (rc)
+ goto err_genl;
+
+ /* the first generation must not be 0 */
+ nfc_devlist_generation = 1;
+
+ rc = rawsock_init();
+ if (rc)
+ goto err_rawsock;
+
+ rc = nfc_llcp_init();
+ if (rc)
+ goto err_llcp_sock;
+
+ rc = af_nfc_init();
+ if (rc)
+ goto err_af_nfc;
+
+ return 0;
+
+err_af_nfc:
+ nfc_llcp_exit();
+err_llcp_sock:
+ rawsock_exit();
+err_rawsock:
+ nfc_genl_exit();
+err_genl:
+ class_unregister(&nfc_class);
+ return rc;
+}
+
+static void __exit nfc_exit(void)
+{
+ af_nfc_exit();
+ nfc_llcp_exit();
+ rawsock_exit();
+ nfc_genl_exit();
+ class_unregister(&nfc_class);
+}
+
+subsys_initcall(nfc_init);
+module_exit(nfc_exit);
+
+MODULE_AUTHOR("Lauro Ramos Venancio <lauro.venancio@openbossa.org>");
+MODULE_DESCRIPTION("NFC Core ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_NETPROTO(PF_NFC);
+MODULE_ALIAS_GENL_FAMILY(NFC_GENL_NAME);
diff --git a/net/nfc/digital.h b/net/nfc/digital.h
new file mode 100644
index 0000000..3c39c72
--- /dev/null
+++ b/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 */
diff --git a/net/nfc/digital_core.c b/net/nfc/digital_core.c
new file mode 100644
index 0000000..ec0a899
--- /dev/null
+++ b/net/nfc/digital_core.c
@@ -0,0 +1,862 @@
+/*
+ * 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 | NFC_PROTO_ISO14443_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
+
+/* Delay between each poll frame (ms) */
+#define DIGITAL_POLL_INTERVAL 10
+
+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_u8(skb, crc & 0xFF);
+ skb_put_u8(skb, (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;
+ }
+
+ cmd->pending = 1;
+
+ 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(*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(*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_delayed_work(&ddev->poll_work,
+ msecs_to_jiffies(DIGITAL_POLL_INTERVAL));
+}
+
+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.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_delayed_work(&ddev->poll_work, 0);
+
+ 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_delayed_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);
+
+ digital_abort_cmd(ddev);
+
+ 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,
+ u8 mode)
+{
+ struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+ if (!ddev->curr_protocol) {
+ pr_err("No active target\n");
+ return;
+ }
+
+ digital_abort_cmd(ddev);
+ 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(*data_exch), GFP_KERNEL);
+ if (!data_exch)
+ 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(*ddev), 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_DELAYED_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_delayed_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);
+
+ /* Call the command callback if any and pass it a ENODEV error.
+ * This gives a chance to the command issuer to free any
+ * allocated buffer.
+ */
+ if (cmd->cmd_cb)
+ cmd->cmd_cb(ddev, cmd->cb_context, ERR_PTR(-ENODEV));
+
+ kfree(cmd->mdaa_params);
+ kfree(cmd);
+ }
+}
+EXPORT_SYMBOL(nfc_digital_unregister_device);
+
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/digital_dep.c b/net/nfc/digital_dep.c
new file mode 100644
index 0000000..4f9a973
--- /dev/null
+++ b/net/nfc/digital_dep.c
@@ -0,0 +1,1643 @@
+/*
+ * 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_N_RETRY_NACK 2
+#define DIGITAL_NFC_DEP_N_RETRY_ATN 2
+
+#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_ATR_RES_TO_WT(s) ((s) & 0xF)
+
+#define DIGITAL_DID_MAX 14
+
+#define DIGITAL_PAYLOAD_SIZE_MAX 254
+#define DIGITAL_PAYLOAD_BITS_TO_PP(s) (((s) & 0x3) << 4)
+#define DIGITAL_PAYLOAD_PP_TO_BITS(s) (((s) >> 4) & 0x3)
+#define DIGITAL_PAYLOAD_BITS_TO_FSL(s) ((s) & 0x3)
+#define DIGITAL_PAYLOAD_FSL_TO_BITS(s) ((s) & 0x3)
+
+#define DIGITAL_GB_BIT 0x02
+
+#define DIGITAL_NFC_DEP_REQ_RES_HEADROOM 2 /* SoD: [SB (NFC-A)] + LEN */
+#define DIGITAL_NFC_DEP_REQ_RES_TAILROOM 2 /* EoD: 2-byte CRC */
+
+#define DIGITAL_NFC_DEP_PFB_TYPE(pfb) ((pfb) & 0xE0)
+
+#define DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT 0x10
+#define DIGITAL_NFC_DEP_PFB_MI_BIT 0x10
+#define DIGITAL_NFC_DEP_PFB_NACK_BIT 0x10
+#define DIGITAL_NFC_DEP_PFB_DID_BIT 0x04
+
+#define DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb) \
+ ((pfb) & DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT)
+#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_MI_BIT)
+#define DIGITAL_NFC_DEP_NACK_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_NACK_BIT)
+#define DIGITAL_NFC_DEP_NAD_BIT_SET(pfb) ((pfb) & 0x08)
+#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & DIGITAL_NFC_DEP_PFB_DID_BIT)
+#define DIGITAL_NFC_DEP_PFB_PNI(pfb) ((pfb) & 0x03)
+
+#define DIGITAL_NFC_DEP_RTOX_VALUE(data) ((data) & 0x3F)
+#define DIGITAL_NFC_DEP_RTOX_MAX 59
+
+#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_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp);
+
+static const u8 digital_payload_bits_map[4] = {
+ [0] = 64,
+ [1] = 128,
+ [2] = 192,
+ [3] = 254
+};
+
+/* Response Waiting Time for ATR_RES PDU in ms
+ *
+ * RWT(ATR_RES) = RWT(nfcdep,activation) + dRWT(nfcdep) + dT(nfcdep,initiator)
+ *
+ * with:
+ * RWT(nfcdep,activation) = 4096 * 2^12 / f(c) s
+ * dRWT(nfcdep) = 16 / f(c) s
+ * dT(nfcdep,initiator) = 100 ms
+ * f(c) = 13560000 Hz
+ */
+#define DIGITAL_ATR_RES_RWT 1337
+
+/* Response Waiting Time for other DEP PDUs in ms
+ *
+ * max_rwt = rwt + dRWT(nfcdep) + dT(nfcdep,initiator)
+ *
+ * with:
+ * rwt = (256 * 16 / f(c)) * 2^wt s
+ * dRWT(nfcdep) = 16 / f(c) s
+ * dT(nfcdep,initiator) = 100 ms
+ * f(c) = 13560000 Hz
+ * 0 <= wt <= 14 (given by the target by the TO field of ATR_RES response)
+ */
+#define DIGITAL_NFC_DEP_IN_MAX_WT 14
+#define DIGITAL_NFC_DEP_TG_MAX_WT 14
+static const u16 digital_rwt_map[DIGITAL_NFC_DEP_IN_MAX_WT + 1] = {
+ 100, 101, 101, 102, 105,
+ 110, 119, 139, 177, 255,
+ 409, 719, 1337, 2575, 5049,
+};
+
+static u8 digital_payload_bits_to_size(u8 payload_bits)
+{
+ if (payload_bits >= ARRAY_SIZE(digital_payload_bits_map))
+ return 0;
+
+ return digital_payload_bits_map[payload_bits];
+}
+
+static u8 digital_payload_size_to_bits(u8 payload_size)
+{
+ int i;
+
+ for (i = 0; i < ARRAY_SIZE(digital_payload_bits_map); i++)
+ if (digital_payload_bits_map[i] == payload_size)
+ return i;
+
+ return 0xff;
+}
+
+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)
+ *(u8 *)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 struct sk_buff *
+digital_send_dep_data_prep(struct nfc_digital_dev *ddev, struct sk_buff *skb,
+ struct digital_dep_req_res *dep_req_res,
+ struct digital_data_exch *data_exch)
+{
+ struct sk_buff *new_skb;
+
+ if (skb->len > ddev->remote_payload_max) {
+ dep_req_res->pfb |= DIGITAL_NFC_DEP_PFB_MI_BIT;
+
+ new_skb = digital_skb_alloc(ddev, ddev->remote_payload_max);
+ if (!new_skb) {
+ kfree_skb(ddev->chaining_skb);
+ ddev->chaining_skb = NULL;
+
+ return ERR_PTR(-ENOMEM);
+ }
+
+ skb_put_data(new_skb, skb->data, ddev->remote_payload_max);
+ skb_pull(skb, ddev->remote_payload_max);
+
+ ddev->chaining_skb = skb;
+ ddev->data_exch = data_exch;
+ } else {
+ ddev->chaining_skb = NULL;
+ new_skb = skb;
+ }
+
+ return new_skb;
+}
+
+static struct sk_buff *
+digital_recv_dep_data_gather(struct nfc_digital_dev *ddev, u8 pfb,
+ struct sk_buff *resp,
+ int (*send_ack)(struct nfc_digital_dev *ddev,
+ struct digital_data_exch
+ *data_exch),
+ struct digital_data_exch *data_exch)
+{
+ struct sk_buff *new_skb;
+ int rc;
+
+ if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb) && (!ddev->chaining_skb)) {
+ ddev->chaining_skb =
+ nfc_alloc_recv_skb(8 * ddev->local_payload_max,
+ GFP_KERNEL);
+ if (!ddev->chaining_skb) {
+ rc = -ENOMEM;
+ goto error;
+ }
+ }
+
+ if (ddev->chaining_skb) {
+ if (resp->len > skb_tailroom(ddev->chaining_skb)) {
+ new_skb = skb_copy_expand(ddev->chaining_skb,
+ skb_headroom(
+ ddev->chaining_skb),
+ 8 * ddev->local_payload_max,
+ GFP_KERNEL);
+ if (!new_skb) {
+ rc = -ENOMEM;
+ goto error;
+ }
+
+ kfree_skb(ddev->chaining_skb);
+ ddev->chaining_skb = new_skb;
+ }
+
+ skb_put_data(ddev->chaining_skb, resp->data, resp->len);
+
+ kfree_skb(resp);
+ resp = NULL;
+
+ if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) {
+ rc = send_ack(ddev, data_exch);
+ if (rc)
+ goto error;
+
+ return NULL;
+ }
+
+ resp = ddev->chaining_skb;
+ ddev->chaining_skb = NULL;
+ }
+
+ return resp;
+
+error:
+ kfree_skb(resp);
+
+ kfree_skb(ddev->chaining_skb);
+ ddev->chaining_skb = NULL;
+
+ return ERR_PTR(rc);
+}
+
+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;
+ int rc;
+ u8 payload_size, payload_bits;
+
+ 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 */
+
+ payload_size = min(ddev->local_payload_max, ddev->remote_payload_max);
+ payload_bits = digital_payload_size_to_bits(payload_size);
+ psl_req->fsl = DIGITAL_PAYLOAD_BITS_TO_FSL(payload_bits);
+
+ ddev->local_payload_max = payload_size;
+ ddev->remote_payload_max = payload_size;
+
+ digital_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ rc = digital_in_send_cmd(ddev, skb, ddev->dep_rwt,
+ digital_in_recv_psl_res, target);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+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, payload_bits;
+ u8 wt;
+ 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;
+
+ wt = DIGITAL_ATR_RES_TO_WT(atr_res->to);
+ if (wt > DIGITAL_NFC_DEP_IN_MAX_WT)
+ wt = DIGITAL_NFC_DEP_IN_MAX_WT;
+ ddev->dep_rwt = digital_rwt_map[wt];
+
+ payload_bits = DIGITAL_PAYLOAD_PP_TO_BITS(atr_res->pp);
+ ddev->remote_payload_max = digital_payload_bits_to_size(payload_bits);
+
+ if (!ddev->remote_payload_max) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ 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;
+ int rc;
+ u8 payload_bits;
+
+ 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;
+
+ ddev->local_payload_max = DIGITAL_PAYLOAD_SIZE_MAX;
+ payload_bits = digital_payload_size_to_bits(ddev->local_payload_max);
+ atr_req->pp = DIGITAL_PAYLOAD_BITS_TO_PP(payload_bits);
+
+ if (gb_len) {
+ atr_req->pp |= DIGITAL_GB_BIT;
+ skb_put_data(skb, gb, gb_len);
+ }
+
+ digital_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ rc = digital_in_send_cmd(ddev, skb, DIGITAL_ATR_RES_RWT,
+ digital_in_recv_atr_res, target);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static int digital_in_send_ack(struct nfc_digital_dev *ddev,
+ struct digital_data_exch *data_exch)
+{
+ struct digital_dep_req_res *dep_req;
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ 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_ACK_NACK_PDU |
+ ddev->curr_nfc_dep_pni;
+
+ digital_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ ddev->saved_skb = pskb_copy(skb, GFP_KERNEL);
+
+ rc = digital_in_send_cmd(ddev, skb, ddev->dep_rwt,
+ digital_in_recv_dep_res, data_exch);
+ if (rc) {
+ kfree_skb(skb);
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+ }
+
+ return rc;
+}
+
+static int digital_in_send_nack(struct nfc_digital_dev *ddev,
+ struct digital_data_exch *data_exch)
+{
+ struct digital_dep_req_res *dep_req;
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ 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_ACK_NACK_PDU |
+ DIGITAL_NFC_DEP_PFB_NACK_BIT | ddev->curr_nfc_dep_pni;
+
+ digital_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ rc = digital_in_send_cmd(ddev, skb, ddev->dep_rwt,
+ digital_in_recv_dep_res, data_exch);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static int digital_in_send_atn(struct nfc_digital_dev *ddev,
+ struct digital_data_exch *data_exch)
+{
+ struct digital_dep_req_res *dep_req;
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ 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_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ rc = digital_in_send_cmd(ddev, skb, ddev->dep_rwt,
+ digital_in_recv_dep_res, data_exch);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+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;
+ u16 rwt_int;
+
+ rwt_int = ddev->dep_rwt * rtox;
+ if (rwt_int > digital_rwt_map[DIGITAL_NFC_DEP_IN_MAX_WT])
+ rwt_int = digital_rwt_map[DIGITAL_NFC_DEP_IN_MAX_WT];
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put_u8(skb, 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, rwt_int,
+ digital_in_recv_dep_res, data_exch);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static int digital_in_send_saved_skb(struct nfc_digital_dev *ddev,
+ struct digital_data_exch *data_exch)
+{
+ int rc;
+
+ if (!ddev->saved_skb)
+ return -EINVAL;
+
+ skb_get(ddev->saved_skb);
+
+ rc = digital_in_send_cmd(ddev, ddev->saved_skb, ddev->dep_rwt,
+ digital_in_recv_dep_res, data_exch);
+ if (rc)
+ kfree_skb(ddev->saved_skb);
+
+ 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;
+ u8 rtox;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+
+ if ((rc == -EIO || (rc == -ETIMEDOUT && ddev->nack_count)) &&
+ (ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) {
+ ddev->atn_count = 0;
+
+ rc = digital_in_send_nack(ddev, data_exch);
+ if (rc)
+ goto error;
+
+ return;
+ } else if ((rc == -ETIMEDOUT) &&
+ (ddev->atn_count++ < DIGITAL_NFC_DEP_N_RETRY_ATN)) {
+ ddev->nack_count = 0;
+
+ rc = digital_in_send_atn(ddev, data_exch);
+ if (rc)
+ goto error;
+
+ return;
+ }
+
+ goto exit;
+ }
+
+ rc = digital_skb_pull_dep_sod(ddev, resp);
+ if (rc) {
+ PROTOCOL_ERR("14.4.1.2");
+ goto exit;
+ }
+
+ rc = ddev->skb_check_crc(resp);
+ if (rc) {
+ if ((resp->len >= 4) &&
+ (ddev->nack_count++ < DIGITAL_NFC_DEP_N_RETRY_NACK)) {
+ ddev->atn_count = 0;
+
+ rc = digital_in_send_nack(ddev, data_exch);
+ if (rc)
+ goto error;
+
+ kfree_skb(resp);
+
+ return;
+ }
+
+ PROTOCOL_ERR("14.4.1.6");
+ goto error;
+ }
+
+ ddev->atn_count = 0;
+ ddev->nack_count = 0;
+
+ if (resp->len > ddev->local_payload_max) {
+ rc = -EMSGSIZE;
+ goto exit;
+ }
+
+ size = sizeof(struct digital_dep_req_res);
+ dep_res = (struct digital_dep_req_res *)resp->data;
+
+ if (resp->len < size || dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN ||
+ dep_res->cmd != DIGITAL_CMD_DEP_RES) {
+ rc = -EIO;
+ goto error;
+ }
+
+ pfb = dep_res->pfb;
+
+ if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) {
+ PROTOCOL_ERR("14.8.2.1");
+ rc = -EIO;
+ goto error;
+ }
+
+ if (DIGITAL_NFC_DEP_NAD_BIT_SET(pfb)) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (size > resp->len) {
+ rc = -EIO;
+ goto error;
+ }
+
+ skb_pull(resp, size);
+
+ 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);
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+
+ resp = digital_recv_dep_data_gather(ddev, pfb, resp,
+ digital_in_send_ack,
+ data_exch);
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto error;
+ }
+
+ /* If resp is NULL then we're still chaining so return and
+ * wait for the next part of the PDU. Else, the PDU is
+ * complete so pass it up.
+ */
+ if (!resp)
+ return;
+
+ rc = 0;
+ break;
+
+ case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
+ if (DIGITAL_NFC_DEP_NACK_BIT_SET(pfb)) {
+ PROTOCOL_ERR("14.12.4.5");
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
+ PROTOCOL_ERR("14.12.3.3");
+ rc = -EIO;
+ goto exit;
+ }
+
+ ddev->curr_nfc_dep_pni =
+ DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
+
+ if (!ddev->chaining_skb) {
+ PROTOCOL_ERR("14.12.4.3");
+ rc = -EIO;
+ goto exit;
+ }
+
+ /* The initiator has received a valid ACK. Free the last sent
+ * PDU and keep on sending chained skb.
+ */
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+
+ rc = digital_in_send_dep_req(ddev, NULL,
+ ddev->chaining_skb,
+ ddev->data_exch);
+ if (rc)
+ goto error;
+
+ goto free_resp;
+
+ case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
+ if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) { /* ATN */
+ rc = digital_in_send_saved_skb(ddev, data_exch);
+ if (rc)
+ goto error;
+
+ goto free_resp;
+ }
+
+ if (ddev->atn_count || ddev->nack_count) {
+ PROTOCOL_ERR("14.12.4.4");
+ rc = -EIO;
+ goto error;
+ }
+
+ rtox = DIGITAL_NFC_DEP_RTOX_VALUE(resp->data[0]);
+ if (!rtox || rtox > DIGITAL_NFC_DEP_RTOX_MAX) {
+ PROTOCOL_ERR("14.8.4.1");
+ rc = -EIO;
+ goto error;
+ }
+
+ rc = digital_in_send_rtox(ddev, data_exch, rtox);
+ if (rc)
+ goto error;
+
+ goto free_resp;
+ }
+
+exit:
+ data_exch->cb(data_exch->cb_context, resp, rc);
+
+error:
+ kfree(data_exch);
+
+ kfree_skb(ddev->chaining_skb);
+ ddev->chaining_skb = NULL;
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+
+ if (rc)
+ kfree_skb(resp);
+
+ return;
+
+free_resp:
+ dev_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;
+ struct sk_buff *chaining_skb, *tmp_skb;
+ int rc;
+
+ 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;
+
+ ddev->atn_count = 0;
+ ddev->nack_count = 0;
+
+ chaining_skb = ddev->chaining_skb;
+
+ tmp_skb = digital_send_dep_data_prep(ddev, skb, dep_req, data_exch);
+ if (IS_ERR(tmp_skb))
+ return PTR_ERR(tmp_skb);
+
+ digital_skb_push_dep_sod(ddev, tmp_skb);
+
+ ddev->skb_add_crc(tmp_skb);
+
+ ddev->saved_skb = pskb_copy(tmp_skb, GFP_KERNEL);
+
+ rc = digital_in_send_cmd(ddev, tmp_skb, ddev->dep_rwt,
+ digital_in_recv_dep_res, data_exch);
+ if (rc) {
+ if (tmp_skb != skb)
+ kfree_skb(tmp_skb);
+
+ kfree_skb(chaining_skb);
+ ddev->chaining_skb = NULL;
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+ }
+
+ return rc;
+}
+
+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 int digital_tg_send_ack(struct nfc_digital_dev *ddev,
+ struct digital_data_exch *data_exch)
+{
+ struct digital_dep_req_res *dep_res;
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ 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 = DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU |
+ ddev->curr_nfc_dep_pni;
+
+ if (ddev->did) {
+ dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT;
+
+ skb_put_data(skb, &ddev->did, sizeof(ddev->did));
+ }
+
+ ddev->curr_nfc_dep_pni =
+ DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
+
+ digital_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ ddev->saved_skb = pskb_copy(skb, GFP_KERNEL);
+
+ rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
+ data_exch);
+ if (rc) {
+ kfree_skb(skb);
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+ }
+
+ return rc;
+}
+
+static int digital_tg_send_atn(struct nfc_digital_dev *ddev)
+{
+ struct digital_dep_req_res *dep_res;
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ 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 = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU;
+
+ if (ddev->did) {
+ dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT;
+
+ skb_put_data(skb, &ddev->did, sizeof(ddev->did));
+ }
+
+ digital_skb_push_dep_sod(ddev, skb);
+
+ ddev->skb_add_crc(skb);
+
+ rc = digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
+ NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static int digital_tg_send_saved_skb(struct nfc_digital_dev *ddev)
+{
+ int rc;
+
+ if (!ddev->saved_skb)
+ return -EINVAL;
+
+ skb_get(ddev->saved_skb);
+
+ rc = digital_tg_send_cmd(ddev, ddev->saved_skb, 1500,
+ digital_tg_recv_dep_req, NULL);
+ if (rc)
+ kfree_skb(ddev->saved_skb);
+
+ return rc;
+}
+
+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;
+ u8 pfb;
+ 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;
+ }
+
+ if (resp->len > ddev->local_payload_max) {
+ rc = -EMSGSIZE;
+ 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;
+ }
+
+ pfb = dep_req->pfb;
+
+ if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb)) {
+ if (ddev->did && (ddev->did == resp->data[3])) {
+ size++;
+ } else {
+ rc = -EIO;
+ goto exit;
+ }
+ } else if (ddev->did) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (DIGITAL_NFC_DEP_NAD_BIT_SET(pfb)) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (size > resp->len) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ skb_pull(resp, size);
+
+ switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) {
+ case DIGITAL_NFC_DEP_PFB_I_PDU:
+ pr_debug("DIGITAL_NFC_DEP_PFB_I_PDU\n");
+
+ if (ddev->atn_count) {
+ /* The target has received (and replied to) at least one
+ * ATN DEP_REQ.
+ */
+ ddev->atn_count = 0;
+
+ /* pni of resp PDU equal to the target current pni - 1
+ * means resp is the previous DEP_REQ PDU received from
+ * the initiator so the target replies with saved_skb
+ * which is the previous DEP_RES saved in
+ * digital_tg_send_dep_res().
+ */
+ if (DIGITAL_NFC_DEP_PFB_PNI(pfb) ==
+ DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni - 1)) {
+ rc = digital_tg_send_saved_skb(ddev);
+ if (rc)
+ goto exit;
+
+ goto free_resp;
+ }
+
+ /* atn_count > 0 and PDU pni != curr_nfc_dep_pni - 1
+ * means the target probably did not received the last
+ * DEP_REQ PDU sent by the initiator. The target
+ * fallbacks to normal processing then.
+ */
+ }
+
+ if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
+ PROTOCOL_ERR("14.12.3.4");
+ rc = -EIO;
+ goto exit;
+ }
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+
+ resp = digital_recv_dep_data_gather(ddev, pfb, resp,
+ digital_tg_send_ack, NULL);
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ /* If resp is NULL then we're still chaining so return and
+ * wait for the next part of the PDU. Else, the PDU is
+ * complete so pass it up.
+ */
+ if (!resp)
+ return;
+
+ rc = 0;
+ break;
+ case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
+ if (DIGITAL_NFC_DEP_NACK_BIT_SET(pfb)) { /* NACK */
+ if (DIGITAL_NFC_DEP_PFB_PNI(pfb + 1) !=
+ ddev->curr_nfc_dep_pni) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ ddev->atn_count = 0;
+
+ rc = digital_tg_send_saved_skb(ddev);
+ if (rc)
+ goto exit;
+
+ goto free_resp;
+ }
+
+ /* ACK */
+ if (ddev->atn_count) {
+ /* The target has previously recevied one or more ATN
+ * PDUs.
+ */
+ ddev->atn_count = 0;
+
+ /* If the ACK PNI is equal to the target PNI - 1 means
+ * that the initiator did not receive the previous PDU
+ * sent by the target so re-send it.
+ */
+ if (DIGITAL_NFC_DEP_PFB_PNI(pfb + 1) ==
+ ddev->curr_nfc_dep_pni) {
+ rc = digital_tg_send_saved_skb(ddev);
+ if (rc)
+ goto exit;
+
+ goto free_resp;
+ }
+
+ /* Otherwise, the target did not receive the previous
+ * ACK PDU from the initiator. Fallback to normal
+ * processing of chained PDU then.
+ */
+ }
+
+ /* Keep on sending chained PDU */
+ if (!ddev->chaining_skb ||
+ DIGITAL_NFC_DEP_PFB_PNI(pfb) !=
+ ddev->curr_nfc_dep_pni) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+
+ rc = digital_tg_send_dep_res(ddev, ddev->chaining_skb);
+ if (rc)
+ goto exit;
+
+ goto free_resp;
+ case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
+ if (DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ rc = digital_tg_send_atn(ddev);
+ if (rc)
+ goto exit;
+
+ ddev->atn_count++;
+
+ goto free_resp;
+ }
+
+ rc = nfc_tm_data_received(ddev->nfc_dev, resp);
+
+exit:
+ kfree_skb(ddev->chaining_skb);
+ ddev->chaining_skb = NULL;
+
+ ddev->atn_count = 0;
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+
+ if (rc)
+ kfree_skb(resp);
+
+ return;
+
+free_resp:
+ dev_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;
+ struct sk_buff *chaining_skb, *tmp_skb;
+ int rc;
+
+ 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;
+
+ if (ddev->did) {
+ dep_res->pfb |= DIGITAL_NFC_DEP_PFB_DID_BIT;
+
+ skb_put_data(skb, &ddev->did, sizeof(ddev->did));
+ }
+
+ ddev->curr_nfc_dep_pni =
+ DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
+
+ chaining_skb = ddev->chaining_skb;
+
+ tmp_skb = digital_send_dep_data_prep(ddev, skb, dep_res, NULL);
+ if (IS_ERR(tmp_skb))
+ return PTR_ERR(tmp_skb);
+
+ digital_skb_push_dep_sod(ddev, tmp_skb);
+
+ ddev->skb_add_crc(tmp_skb);
+
+ ddev->saved_skb = pskb_copy(tmp_skb, GFP_KERNEL);
+
+ rc = digital_tg_send_cmd(ddev, tmp_skb, 1500, digital_tg_recv_dep_req,
+ NULL);
+ if (rc) {
+ if (tmp_skb != skb)
+ kfree_skb(tmp_skb);
+
+ kfree_skb(chaining_skb);
+ ddev->chaining_skb = NULL;
+
+ kfree_skb(ddev->saved_skb);
+ ddev->saved_skb = NULL;
+ }
+
+ return rc;
+}
+
+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);
+
+ ddev->curr_nfc_dep_pni = 0;
+
+ 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, payload_size, payload_bits;
+
+ 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;
+ }
+
+ payload_bits = DIGITAL_PAYLOAD_FSL_TO_BITS(psl_req->fsl);
+ payload_size = digital_payload_bits_to_size(payload_bits);
+
+ if (!payload_size || (payload_size > min(ddev->local_payload_max,
+ ddev->remote_payload_max))) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ ddev->local_payload_max = payload_size;
+ ddev->remote_payload_max = payload_size;
+
+ 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++;
+
+ ddev->atn_count = 0;
+
+ 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, payload_bits;
+ 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 = DIGITAL_NFC_DEP_TG_MAX_WT;
+
+ ddev->local_payload_max = DIGITAL_PAYLOAD_SIZE_MAX;
+ payload_bits = digital_payload_size_to_bits(ddev->local_payload_max);
+ atr_res->pp = DIGITAL_PAYLOAD_BITS_TO_PP(payload_bits);
+
+ 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);
+
+ ddev->curr_nfc_dep_pni = 0;
+
+ rc = digital_tg_send_cmd(ddev, skb, 999,
+ digital_tg_send_atr_res_complete, NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ 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, payload_bits;
+
+ 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 ||
+ atr_req->did > DIGITAL_DID_MAX) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ payload_bits = DIGITAL_PAYLOAD_PP_TO_BITS(atr_req->pp);
+ ddev->remote_payload_max = digital_payload_bits_to_size(payload_bits);
+
+ if (!ddev->remote_payload_max) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ ddev->did = atr_req->did;
+
+ 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);
+}
diff --git a/net/nfc/digital_technology.c b/net/nfc/digital_technology.c
new file mode 100644
index 0000000..2021d1d
--- /dev/null
+++ b/net/nfc/digital_technology.c
@@ -0,0 +1,1305 @@
+/*
+ * 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_CMD_SENS_REQ 0x26
+#define DIGITAL_CMD_ALL_REQ 0x52
+#define DIGITAL_CMD_SEL_REQ_CL1 0x93
+#define DIGITAL_CMD_SEL_REQ_CL2 0x95
+#define DIGITAL_CMD_SEL_REQ_CL3 0x97
+
+#define DIGITAL_SDD_REQ_SEL_PAR 0x20
+
+#define DIGITAL_SDD_RES_CT 0x88
+#define DIGITAL_SDD_RES_LEN 5
+#define DIGITAL_SEL_RES_LEN 1
+
+#define DIGITAL_SEL_RES_NFCID1_COMPLETE(sel_res) (!((sel_res) & 0x04))
+#define DIGITAL_SEL_RES_IS_T2T(sel_res) (!((sel_res) & 0x60))
+#define DIGITAL_SEL_RES_IS_T4T(sel_res) ((sel_res) & 0x20)
+#define DIGITAL_SEL_RES_IS_NFC_DEP(sel_res) ((sel_res) & 0x40)
+
+#define DIGITAL_SENS_RES_IS_T1T(sens_res) (((sens_res) & 0x0C00) == 0x0C00)
+#define DIGITAL_SENS_RES_IS_VALID(sens_res) \
+ ((!((sens_res) & 0x001F) && (((sens_res) & 0x0C00) == 0x0C00)) || \
+ (((sens_res) & 0x001F) && ((sens_res) & 0x0C00) != 0x0C00))
+
+#define DIGITAL_MIFARE_READ_RES_LEN 16
+#define DIGITAL_MIFARE_ACK_RES 0x0A
+
+#define DIGITAL_CMD_SENSB_REQ 0x05
+#define DIGITAL_SENSB_ADVANCED BIT(5)
+#define DIGITAL_SENSB_EXTENDED BIT(4)
+#define DIGITAL_SENSB_ALLB_REQ BIT(3)
+#define DIGITAL_SENSB_N(n) ((n) & 0x7)
+
+#define DIGITAL_CMD_SENSB_RES 0x50
+
+#define DIGITAL_CMD_ATTRIB_REQ 0x1D
+#define DIGITAL_ATTRIB_P1_TR0_DEFAULT (0x0 << 6)
+#define DIGITAL_ATTRIB_P1_TR1_DEFAULT (0x0 << 4)
+#define DIGITAL_ATTRIB_P1_SUPRESS_EOS BIT(3)
+#define DIGITAL_ATTRIB_P1_SUPRESS_SOS BIT(2)
+#define DIGITAL_ATTRIB_P2_LISTEN_POLL_1 (0x0 << 6)
+#define DIGITAL_ATTRIB_P2_POLL_LISTEN_1 (0x0 << 4)
+#define DIGITAL_ATTRIB_P2_MAX_FRAME_256 0x8
+#define DIGITAL_ATTRIB_P4_DID(n) ((n) & 0xf)
+
+#define DIGITAL_CMD_SENSF_REQ 0x00
+#define DIGITAL_CMD_SENSF_RES 0x01
+
+#define DIGITAL_SENSF_RES_MIN_LENGTH 17
+#define DIGITAL_SENSF_RES_RD_AP_B1 0x00
+#define DIGITAL_SENSF_RES_RD_AP_B2 0x8F
+
+#define DIGITAL_SENSF_REQ_RC_NONE 0
+#define DIGITAL_SENSF_REQ_RC_SC 1
+#define DIGITAL_SENSF_REQ_RC_AP 2
+
+#define DIGITAL_CMD_ISO15693_INVENTORY_REQ 0x01
+
+#define DIGITAL_ISO15693_REQ_FLAG_DATA_RATE BIT(1)
+#define DIGITAL_ISO15693_REQ_FLAG_INVENTORY BIT(2)
+#define DIGITAL_ISO15693_REQ_FLAG_NB_SLOTS BIT(5)
+#define DIGITAL_ISO15693_RES_FLAG_ERROR BIT(0)
+#define DIGITAL_ISO15693_RES_IS_VALID(flags) \
+ (!((flags) & DIGITAL_ISO15693_RES_FLAG_ERROR))
+
+#define DIGITAL_ISO_DEP_I_PCB 0x02
+#define DIGITAL_ISO_DEP_PNI(pni) ((pni) & 0x01)
+
+#define DIGITAL_ISO_DEP_PCB_TYPE(pcb) ((pcb) & 0xC0)
+
+#define DIGITAL_ISO_DEP_I_BLOCK 0x00
+
+#define DIGITAL_ISO_DEP_BLOCK_HAS_DID(pcb) ((pcb) & 0x08)
+
+static const u8 digital_ats_fsc[] = {
+ 16, 24, 32, 40, 48, 64, 96, 128,
+};
+
+#define DIGITAL_ATS_FSCI(t0) ((t0) & 0x0F)
+#define DIGITAL_SENSB_FSCI(pi2) (((pi2) & 0xF0) >> 4)
+#define DIGITAL_ATS_MAX_FSC 256
+
+#define DIGITAL_RATS_BYTE1 0xE0
+#define DIGITAL_RATS_PARAM 0x80
+
+struct digital_sdd_res {
+ u8 nfcid1[4];
+ u8 bcc;
+} __packed;
+
+struct digital_sel_req {
+ u8 sel_cmd;
+ u8 b2;
+ u8 nfcid1[4];
+ u8 bcc;
+} __packed;
+
+struct digital_sensb_req {
+ u8 cmd;
+ u8 afi;
+ u8 param;
+} __packed;
+
+struct digital_sensb_res {
+ u8 cmd;
+ u8 nfcid0[4];
+ u8 app_data[4];
+ u8 proto_info[3];
+} __packed;
+
+struct digital_attrib_req {
+ u8 cmd;
+ u8 nfcid0[4];
+ u8 param1;
+ u8 param2;
+ u8 param3;
+ u8 param4;
+} __packed;
+
+struct digital_attrib_res {
+ u8 mbli_did;
+} __packed;
+
+struct digital_sensf_req {
+ u8 cmd;
+ u8 sc1;
+ u8 sc2;
+ u8 rc;
+ u8 tsn;
+} __packed;
+
+struct digital_sensf_res {
+ u8 cmd;
+ u8 nfcid2[8];
+ u8 pad0[2];
+ u8 pad1[3];
+ u8 mrti_check;
+ u8 mrti_update;
+ u8 pad2;
+ u8 rd[2];
+} __packed;
+
+struct digital_iso15693_inv_req {
+ u8 flags;
+ u8 cmd;
+ u8 mask_len;
+ u64 mask;
+} __packed;
+
+struct digital_iso15693_inv_res {
+ u8 flags;
+ u8 dsfid;
+ u64 uid;
+} __packed;
+
+static int digital_in_send_sdd_req(struct nfc_digital_dev *ddev,
+ struct nfc_target *target);
+
+int digital_in_iso_dep_pull_sod(struct nfc_digital_dev *ddev,
+ struct sk_buff *skb)
+{
+ u8 pcb;
+ u8 block_type;
+
+ if (skb->len < 1)
+ return -EIO;
+
+ pcb = *skb->data;
+ block_type = DIGITAL_ISO_DEP_PCB_TYPE(pcb);
+
+ /* No support fo R-block nor S-block */
+ if (block_type != DIGITAL_ISO_DEP_I_BLOCK) {
+ pr_err("ISO_DEP R-block and S-block not supported\n");
+ return -EIO;
+ }
+
+ if (DIGITAL_ISO_DEP_BLOCK_HAS_DID(pcb)) {
+ pr_err("DID field in ISO_DEP PCB not supported\n");
+ return -EIO;
+ }
+
+ skb_pull(skb, 1);
+
+ return 0;
+}
+
+int digital_in_iso_dep_push_sod(struct nfc_digital_dev *ddev,
+ struct sk_buff *skb)
+{
+ /*
+ * Chaining not supported so skb->len + 1 PCB byte + 2 CRC bytes must
+ * not be greater than remote FSC
+ */
+ if (skb->len + 3 > ddev->target_fsc)
+ return -EIO;
+
+ skb_push(skb, 1);
+
+ *skb->data = DIGITAL_ISO_DEP_I_PCB | ddev->curr_nfc_dep_pni;
+
+ ddev->curr_nfc_dep_pni =
+ DIGITAL_ISO_DEP_PNI(ddev->curr_nfc_dep_pni + 1);
+
+ return 0;
+}
+
+static void digital_in_recv_ats(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct nfc_target *target = arg;
+ u8 fsdi;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (resp->len < 2) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ fsdi = DIGITAL_ATS_FSCI(resp->data[1]);
+ if (fsdi >= 8)
+ ddev->target_fsc = DIGITAL_ATS_MAX_FSC;
+ else
+ ddev->target_fsc = digital_ats_fsc[fsdi];
+
+ ddev->curr_nfc_dep_pni = 0;
+
+ rc = digital_target_found(ddev, target, NFC_PROTO_ISO14443);
+
+exit:
+ dev_kfree_skb(resp);
+ kfree(target);
+
+ if (rc)
+ digital_poll_next_tech(ddev);
+}
+
+static int digital_in_send_rats(struct nfc_digital_dev *ddev,
+ struct nfc_target *target)
+{
+ int rc;
+ struct sk_buff *skb;
+
+ skb = digital_skb_alloc(ddev, 2);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put_u8(skb, DIGITAL_RATS_BYTE1);
+ skb_put_u8(skb, DIGITAL_RATS_PARAM);
+
+ rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_ats,
+ target);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_in_recv_sel_res(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct nfc_target *target = arg;
+ int rc;
+ u8 sel_res;
+ u8 nfc_proto;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (!DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+ rc = digital_skb_check_crc_a(resp);
+ if (rc) {
+ PROTOCOL_ERR("4.4.1.3");
+ goto exit;
+ }
+ }
+
+ if (resp->len != DIGITAL_SEL_RES_LEN) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ sel_res = resp->data[0];
+
+ if (!DIGITAL_SEL_RES_NFCID1_COMPLETE(sel_res)) {
+ rc = digital_in_send_sdd_req(ddev, target);
+ if (rc)
+ goto exit;
+
+ goto exit_free_skb;
+ }
+
+ target->sel_res = sel_res;
+
+ if (DIGITAL_SEL_RES_IS_T2T(sel_res)) {
+ nfc_proto = NFC_PROTO_MIFARE;
+ } else if (DIGITAL_SEL_RES_IS_NFC_DEP(sel_res)) {
+ nfc_proto = NFC_PROTO_NFC_DEP;
+ } else if (DIGITAL_SEL_RES_IS_T4T(sel_res)) {
+ rc = digital_in_send_rats(ddev, target);
+ if (rc)
+ goto exit;
+ /*
+ * Skip target_found and don't free it for now. This will be
+ * done when receiving the ATS
+ */
+ goto exit_free_skb;
+ } else {
+ rc = -EOPNOTSUPP;
+ goto exit;
+ }
+
+ rc = digital_target_found(ddev, target, nfc_proto);
+
+exit:
+ kfree(target);
+
+exit_free_skb:
+ dev_kfree_skb(resp);
+
+ if (rc)
+ digital_poll_next_tech(ddev);
+}
+
+static int digital_in_send_sel_req(struct nfc_digital_dev *ddev,
+ struct nfc_target *target,
+ struct digital_sdd_res *sdd_res)
+{
+ struct sk_buff *skb;
+ struct digital_sel_req *sel_req;
+ u8 sel_cmd;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, sizeof(struct digital_sel_req));
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put(skb, sizeof(struct digital_sel_req));
+ sel_req = (struct digital_sel_req *)skb->data;
+
+ if (target->nfcid1_len <= 4)
+ sel_cmd = DIGITAL_CMD_SEL_REQ_CL1;
+ else if (target->nfcid1_len < 10)
+ sel_cmd = DIGITAL_CMD_SEL_REQ_CL2;
+ else
+ sel_cmd = DIGITAL_CMD_SEL_REQ_CL3;
+
+ sel_req->sel_cmd = sel_cmd;
+ sel_req->b2 = 0x70;
+ memcpy(sel_req->nfcid1, sdd_res->nfcid1, 4);
+ sel_req->bcc = sdd_res->bcc;
+
+ if (DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A);
+ if (rc)
+ goto exit;
+ } else {
+ digital_skb_add_crc_a(skb);
+ }
+
+ rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sel_res,
+ target);
+exit:
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_in_recv_sdd_res(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct nfc_target *target = arg;
+ struct digital_sdd_res *sdd_res;
+ int rc;
+ u8 offset, size;
+ u8 i, bcc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (resp->len < DIGITAL_SDD_RES_LEN) {
+ PROTOCOL_ERR("4.7.2.8");
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ sdd_res = (struct digital_sdd_res *)resp->data;
+
+ for (i = 0, bcc = 0; i < 4; i++)
+ bcc ^= sdd_res->nfcid1[i];
+
+ if (bcc != sdd_res->bcc) {
+ PROTOCOL_ERR("4.7.2.6");
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ if (sdd_res->nfcid1[0] == DIGITAL_SDD_RES_CT) {
+ offset = 1;
+ size = 3;
+ } else {
+ offset = 0;
+ size = 4;
+ }
+
+ memcpy(target->nfcid1 + target->nfcid1_len, sdd_res->nfcid1 + offset,
+ size);
+ target->nfcid1_len += size;
+
+ rc = digital_in_send_sel_req(ddev, target, sdd_res);
+
+exit:
+ dev_kfree_skb(resp);
+
+ if (rc) {
+ kfree(target);
+ digital_poll_next_tech(ddev);
+ }
+}
+
+static int digital_in_send_sdd_req(struct nfc_digital_dev *ddev,
+ struct nfc_target *target)
+{
+ int rc;
+ struct sk_buff *skb;
+ u8 sel_cmd;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_STANDARD);
+ if (rc)
+ return rc;
+
+ skb = digital_skb_alloc(ddev, 2);
+ if (!skb)
+ return -ENOMEM;
+
+ if (target->nfcid1_len == 0)
+ sel_cmd = DIGITAL_CMD_SEL_REQ_CL1;
+ else if (target->nfcid1_len == 3)
+ sel_cmd = DIGITAL_CMD_SEL_REQ_CL2;
+ else
+ sel_cmd = DIGITAL_CMD_SEL_REQ_CL3;
+
+ skb_put_u8(skb, sel_cmd);
+ skb_put_u8(skb, DIGITAL_SDD_REQ_SEL_PAR);
+
+ return digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sdd_res,
+ target);
+}
+
+static void digital_in_recv_sens_res(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct nfc_target *target = NULL;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (resp->len < sizeof(u16)) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ target = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
+ if (!target) {
+ rc = -ENOMEM;
+ goto exit;
+ }
+
+ target->sens_res = __le16_to_cpu(*(__le16 *)resp->data);
+
+ if (!DIGITAL_SENS_RES_IS_VALID(target->sens_res)) {
+ PROTOCOL_ERR("4.6.3.3");
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ if (DIGITAL_SENS_RES_IS_T1T(target->sens_res))
+ rc = digital_target_found(ddev, target, NFC_PROTO_JEWEL);
+ else
+ rc = digital_in_send_sdd_req(ddev, target);
+
+exit:
+ dev_kfree_skb(resp);
+
+ if (rc) {
+ kfree(target);
+ digital_poll_next_tech(ddev);
+ }
+}
+
+int digital_in_send_sens_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ struct sk_buff *skb;
+ int rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+ NFC_DIGITAL_RF_TECH_106A);
+ if (rc)
+ return rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_SHORT);
+ if (rc)
+ return rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put_u8(skb, DIGITAL_CMD_SENS_REQ);
+
+ rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sens_res, NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+int digital_in_recv_mifare_res(struct sk_buff *resp)
+{
+ /* Successful READ command response is 16 data bytes + 2 CRC bytes long.
+ * Since the driver can't differentiate a ACK/NACK response from a valid
+ * READ response, the CRC calculation must be handled at digital level
+ * even if the driver supports it for this technology.
+ */
+ if (resp->len == DIGITAL_MIFARE_READ_RES_LEN + DIGITAL_CRC_LEN) {
+ if (digital_skb_check_crc_a(resp)) {
+ PROTOCOL_ERR("9.4.1.2");
+ return -EIO;
+ }
+
+ return 0;
+ }
+
+ /* ACK response (i.e. successful WRITE). */
+ if (resp->len == 1 && resp->data[0] == DIGITAL_MIFARE_ACK_RES) {
+ resp->data[0] = 0;
+ return 0;
+ }
+
+ /* NACK and any other responses are treated as error. */
+ return -EIO;
+}
+
+static void digital_in_recv_attrib_res(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct nfc_target *target = arg;
+ struct digital_attrib_res *attrib_res;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (resp->len < sizeof(*attrib_res)) {
+ PROTOCOL_ERR("12.6.2");
+ rc = -EIO;
+ goto exit;
+ }
+
+ attrib_res = (struct digital_attrib_res *)resp->data;
+
+ if (attrib_res->mbli_did & 0x0f) {
+ PROTOCOL_ERR("12.6.2.1");
+ rc = -EIO;
+ goto exit;
+ }
+
+ rc = digital_target_found(ddev, target, NFC_PROTO_ISO14443_B);
+
+exit:
+ dev_kfree_skb(resp);
+ kfree(target);
+
+ if (rc)
+ digital_poll_next_tech(ddev);
+}
+
+static int digital_in_send_attrib_req(struct nfc_digital_dev *ddev,
+ struct nfc_target *target,
+ struct digital_sensb_res *sensb_res)
+{
+ struct digital_attrib_req *attrib_req;
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, sizeof(*attrib_req));
+ if (!skb)
+ return -ENOMEM;
+
+ attrib_req = skb_put(skb, sizeof(*attrib_req));
+
+ attrib_req->cmd = DIGITAL_CMD_ATTRIB_REQ;
+ memcpy(attrib_req->nfcid0, sensb_res->nfcid0,
+ sizeof(attrib_req->nfcid0));
+ attrib_req->param1 = DIGITAL_ATTRIB_P1_TR0_DEFAULT |
+ DIGITAL_ATTRIB_P1_TR1_DEFAULT;
+ attrib_req->param2 = DIGITAL_ATTRIB_P2_LISTEN_POLL_1 |
+ DIGITAL_ATTRIB_P2_POLL_LISTEN_1 |
+ DIGITAL_ATTRIB_P2_MAX_FRAME_256;
+ attrib_req->param3 = sensb_res->proto_info[1] & 0x07;
+ attrib_req->param4 = DIGITAL_ATTRIB_P4_DID(0);
+
+ rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_attrib_res,
+ target);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_in_recv_sensb_res(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct nfc_target *target = NULL;
+ struct digital_sensb_res *sensb_res;
+ u8 fsci;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (resp->len != sizeof(*sensb_res)) {
+ PROTOCOL_ERR("5.6.2.1");
+ rc = -EIO;
+ goto exit;
+ }
+
+ sensb_res = (struct digital_sensb_res *)resp->data;
+
+ if (sensb_res->cmd != DIGITAL_CMD_SENSB_RES) {
+ PROTOCOL_ERR("5.6.2");
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (!(sensb_res->proto_info[1] & BIT(0))) {
+ PROTOCOL_ERR("5.6.2.12");
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (sensb_res->proto_info[1] & BIT(3)) {
+ PROTOCOL_ERR("5.6.2.16");
+ rc = -EIO;
+ goto exit;
+ }
+
+ fsci = DIGITAL_SENSB_FSCI(sensb_res->proto_info[1]);
+ if (fsci >= 8)
+ ddev->target_fsc = DIGITAL_ATS_MAX_FSC;
+ else
+ ddev->target_fsc = digital_ats_fsc[fsci];
+
+ target = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
+ if (!target) {
+ rc = -ENOMEM;
+ goto exit;
+ }
+
+ rc = digital_in_send_attrib_req(ddev, target, sensb_res);
+
+exit:
+ dev_kfree_skb(resp);
+
+ if (rc) {
+ kfree(target);
+ digital_poll_next_tech(ddev);
+ }
+}
+
+int digital_in_send_sensb_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ struct digital_sensb_req *sensb_req;
+ struct sk_buff *skb;
+ int rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+ NFC_DIGITAL_RF_TECH_106B);
+ if (rc)
+ return rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCB);
+ if (rc)
+ return rc;
+
+ skb = digital_skb_alloc(ddev, sizeof(*sensb_req));
+ if (!skb)
+ return -ENOMEM;
+
+ sensb_req = skb_put(skb, sizeof(*sensb_req));
+
+ sensb_req->cmd = DIGITAL_CMD_SENSB_REQ;
+ sensb_req->afi = 0x00; /* All families and sub-families */
+ sensb_req->param = DIGITAL_SENSB_N(0);
+
+ rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sensb_res,
+ NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_in_recv_sensf_res(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ int rc;
+ u8 proto;
+ struct nfc_target target;
+ struct digital_sensf_res *sensf_res;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (resp->len < DIGITAL_SENSF_RES_MIN_LENGTH) {
+ rc = -EIO;
+ goto exit;
+ }
+
+ if (!DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+ rc = digital_skb_check_crc_f(resp);
+ if (rc) {
+ PROTOCOL_ERR("6.4.1.8");
+ goto exit;
+ }
+ }
+
+ skb_pull(resp, 1);
+
+ memset(&target, 0, sizeof(struct nfc_target));
+
+ sensf_res = (struct digital_sensf_res *)resp->data;
+
+ memcpy(target.sensf_res, sensf_res, resp->len);
+ target.sensf_res_len = resp->len;
+
+ memcpy(target.nfcid2, sensf_res->nfcid2, NFC_NFCID2_MAXSIZE);
+ target.nfcid2_len = NFC_NFCID2_MAXSIZE;
+
+ if (target.nfcid2[0] == DIGITAL_SENSF_NFCID2_NFC_DEP_B1 &&
+ target.nfcid2[1] == DIGITAL_SENSF_NFCID2_NFC_DEP_B2)
+ proto = NFC_PROTO_NFC_DEP;
+ else
+ proto = NFC_PROTO_FELICA;
+
+ rc = digital_target_found(ddev, &target, proto);
+
+exit:
+ dev_kfree_skb(resp);
+
+ if (rc)
+ digital_poll_next_tech(ddev);
+}
+
+int digital_in_send_sensf_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ struct digital_sensf_req *sensf_req;
+ struct sk_buff *skb;
+ int rc;
+ u8 size;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+ if (rc)
+ return rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCF);
+ if (rc)
+ return rc;
+
+ size = sizeof(struct digital_sensf_req);
+
+ skb = digital_skb_alloc(ddev, size);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put(skb, size);
+
+ sensf_req = (struct digital_sensf_req *)skb->data;
+ sensf_req->cmd = DIGITAL_CMD_SENSF_REQ;
+ sensf_req->sc1 = 0xFF;
+ sensf_req->sc2 = 0xFF;
+ sensf_req->rc = 0;
+ sensf_req->tsn = 0;
+
+ *(u8 *)skb_push(skb, 1) = size + 1;
+
+ if (!DIGITAL_DRV_CAPS_IN_CRC(ddev))
+ digital_skb_add_crc_f(skb);
+
+ rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sensf_res,
+ NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_in_recv_iso15693_inv_res(struct nfc_digital_dev *ddev,
+ void *arg, struct sk_buff *resp)
+{
+ struct digital_iso15693_inv_res *res;
+ struct nfc_target *target = NULL;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto out_free_skb;
+ }
+
+ if (resp->len != sizeof(*res)) {
+ rc = -EIO;
+ goto out_free_skb;
+ }
+
+ res = (struct digital_iso15693_inv_res *)resp->data;
+
+ if (!DIGITAL_ISO15693_RES_IS_VALID(res->flags)) {
+ PROTOCOL_ERR("ISO15693 - 10.3.1");
+ rc = -EINVAL;
+ goto out_free_skb;
+ }
+
+ target = kzalloc(sizeof(*target), GFP_KERNEL);
+ if (!target) {
+ rc = -ENOMEM;
+ goto out_free_skb;
+ }
+
+ target->is_iso15693 = 1;
+ target->iso15693_dsfid = res->dsfid;
+ memcpy(target->iso15693_uid, &res->uid, sizeof(target->iso15693_uid));
+
+ rc = digital_target_found(ddev, target, NFC_PROTO_ISO15693);
+
+ kfree(target);
+
+out_free_skb:
+ dev_kfree_skb(resp);
+
+ if (rc)
+ digital_poll_next_tech(ddev);
+}
+
+int digital_in_send_iso15693_inv_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ struct digital_iso15693_inv_req *req;
+ struct sk_buff *skb;
+ int rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+ NFC_DIGITAL_RF_TECH_ISO15693);
+ if (rc)
+ return rc;
+
+ rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_ISO15693_INVENTORY);
+ if (rc)
+ return rc;
+
+ skb = digital_skb_alloc(ddev, sizeof(*req));
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put(skb, sizeof(*req) - sizeof(req->mask)); /* No mask */
+ req = (struct digital_iso15693_inv_req *)skb->data;
+
+ /* Single sub-carrier, high data rate, no AFI, single slot
+ * Inventory command
+ */
+ req->flags = DIGITAL_ISO15693_REQ_FLAG_DATA_RATE |
+ DIGITAL_ISO15693_REQ_FLAG_INVENTORY |
+ DIGITAL_ISO15693_REQ_FLAG_NB_SLOTS;
+ req->cmd = DIGITAL_CMD_ISO15693_INVENTORY_REQ;
+ req->mask_len = 0;
+
+ rc = digital_in_send_cmd(ddev, skb, 30,
+ digital_in_recv_iso15693_inv_res, NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static int digital_tg_send_sel_res(struct nfc_digital_dev *ddev)
+{
+ struct sk_buff *skb;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 1);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put_u8(skb, DIGITAL_SEL_RES_NFC_DEP);
+
+ if (!DIGITAL_DRV_CAPS_TG_CRC(ddev))
+ digital_skb_add_crc_a(skb);
+
+ rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_ANTICOL_COMPLETE);
+ if (rc) {
+ kfree_skb(skb);
+ return rc;
+ }
+
+ rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_atr_req,
+ NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_tg_recv_sel_req(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (!DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
+ rc = digital_skb_check_crc_a(resp);
+ if (rc) {
+ PROTOCOL_ERR("4.4.1.3");
+ goto exit;
+ }
+ }
+
+ /* Silently ignore SEL_REQ content and send a SEL_RES for NFC-DEP */
+
+ rc = digital_tg_send_sel_res(ddev);
+
+exit:
+ if (rc)
+ digital_poll_next_tech(ddev);
+
+ dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_sdd_res(struct nfc_digital_dev *ddev)
+{
+ struct sk_buff *skb;
+ struct digital_sdd_res *sdd_res;
+ int rc, i;
+
+ skb = digital_skb_alloc(ddev, sizeof(struct digital_sdd_res));
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put(skb, sizeof(struct digital_sdd_res));
+ sdd_res = (struct digital_sdd_res *)skb->data;
+
+ sdd_res->nfcid1[0] = 0x08;
+ get_random_bytes(sdd_res->nfcid1 + 1, 3);
+
+ sdd_res->bcc = 0;
+ for (i = 0; i < 4; i++)
+ sdd_res->bcc ^= sdd_res->nfcid1[i];
+
+ rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A);
+ if (rc) {
+ kfree_skb(skb);
+ return rc;
+ }
+
+ rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_sel_req,
+ NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+static void digital_tg_recv_sdd_req(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ u8 *sdd_req;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ sdd_req = resp->data;
+
+ if (resp->len < 2 || sdd_req[0] != DIGITAL_CMD_SEL_REQ_CL1 ||
+ sdd_req[1] != DIGITAL_SDD_REQ_SEL_PAR) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ rc = digital_tg_send_sdd_res(ddev);
+
+exit:
+ if (rc)
+ digital_poll_next_tech(ddev);
+
+ dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_sens_res(struct nfc_digital_dev *ddev)
+{
+ struct sk_buff *skb;
+ u8 *sens_res;
+ int rc;
+
+ skb = digital_skb_alloc(ddev, 2);
+ if (!skb)
+ return -ENOMEM;
+
+ sens_res = skb_put(skb, 2);
+
+ sens_res[0] = (DIGITAL_SENS_RES_NFC_DEP >> 8) & 0xFF;
+ sens_res[1] = DIGITAL_SENS_RES_NFC_DEP & 0xFF;
+
+ rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_STANDARD);
+ if (rc) {
+ kfree_skb(skb);
+ return rc;
+ }
+
+ rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_sdd_req,
+ NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+void digital_tg_recv_sens_req(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ u8 sens_req;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ sens_req = resp->data[0];
+
+ if (!resp->len || (sens_req != DIGITAL_CMD_SENS_REQ &&
+ sens_req != DIGITAL_CMD_ALL_REQ)) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ rc = digital_tg_send_sens_res(ddev);
+
+exit:
+ if (rc)
+ digital_poll_next_tech(ddev);
+
+ dev_kfree_skb(resp);
+}
+
+static void digital_tg_recv_atr_or_sensf_req(struct nfc_digital_dev *ddev,
+ void *arg, struct sk_buff *resp)
+{
+ if (!IS_ERR(resp) && (resp->len >= 2) &&
+ (resp->data[1] == DIGITAL_CMD_SENSF_REQ))
+ digital_tg_recv_sensf_req(ddev, arg, resp);
+ else
+ digital_tg_recv_atr_req(ddev, arg, resp);
+
+ return;
+}
+
+static int digital_tg_send_sensf_res(struct nfc_digital_dev *ddev,
+ struct digital_sensf_req *sensf_req)
+{
+ struct sk_buff *skb;
+ u8 size;
+ int rc;
+ struct digital_sensf_res *sensf_res;
+
+ size = sizeof(struct digital_sensf_res);
+
+ if (sensf_req->rc == DIGITAL_SENSF_REQ_RC_NONE)
+ size -= sizeof(sensf_res->rd);
+
+ skb = digital_skb_alloc(ddev, size);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_put(skb, size);
+
+ sensf_res = (struct digital_sensf_res *)skb->data;
+
+ memset(sensf_res, 0, size);
+
+ sensf_res->cmd = DIGITAL_CMD_SENSF_RES;
+ sensf_res->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
+ sensf_res->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
+ get_random_bytes(&sensf_res->nfcid2[2], 6);
+
+ switch (sensf_req->rc) {
+ case DIGITAL_SENSF_REQ_RC_SC:
+ sensf_res->rd[0] = sensf_req->sc1;
+ sensf_res->rd[1] = sensf_req->sc2;
+ break;
+ case DIGITAL_SENSF_REQ_RC_AP:
+ sensf_res->rd[0] = DIGITAL_SENSF_RES_RD_AP_B1;
+ sensf_res->rd[1] = DIGITAL_SENSF_RES_RD_AP_B2;
+ break;
+ }
+
+ *(u8 *)skb_push(skb, sizeof(u8)) = size + 1;
+
+ if (!DIGITAL_DRV_CAPS_TG_CRC(ddev))
+ digital_skb_add_crc_f(skb);
+
+ rc = digital_tg_send_cmd(ddev, skb, 300,
+ digital_tg_recv_atr_or_sensf_req, NULL);
+ if (rc)
+ kfree_skb(skb);
+
+ return rc;
+}
+
+void digital_tg_recv_sensf_req(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ struct digital_sensf_req *sensf_req;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ rc = PTR_ERR(resp);
+ resp = NULL;
+ goto exit;
+ }
+
+ if (!DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
+ rc = digital_skb_check_crc_f(resp);
+ if (rc) {
+ PROTOCOL_ERR("6.4.1.8");
+ goto exit;
+ }
+ }
+
+ if (resp->len != sizeof(struct digital_sensf_req) + 1) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ skb_pull(resp, 1);
+ sensf_req = (struct digital_sensf_req *)resp->data;
+
+ if (sensf_req->cmd != DIGITAL_CMD_SENSF_REQ) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ rc = digital_tg_send_sensf_res(ddev, sensf_req);
+
+exit:
+ if (rc)
+ digital_poll_next_tech(ddev);
+
+ dev_kfree_skb(resp);
+}
+
+static int digital_tg_config_nfca(struct nfc_digital_dev *ddev)
+{
+ int rc;
+
+ rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+ NFC_DIGITAL_RF_TECH_106A);
+ if (rc)
+ return rc;
+
+ return digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+}
+
+int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ int rc;
+
+ rc = digital_tg_config_nfca(ddev);
+ if (rc)
+ return rc;
+
+ return digital_tg_listen(ddev, 300, digital_tg_recv_sens_req, NULL);
+}
+
+static int digital_tg_config_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ int rc;
+
+ rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+ if (rc)
+ return rc;
+
+ return digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+ NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
+}
+
+int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+ int rc;
+
+ rc = digital_tg_config_nfcf(ddev, rf_tech);
+ if (rc)
+ return rc;
+
+ return digital_tg_listen(ddev, 300, digital_tg_recv_sensf_req, NULL);
+}
+
+void digital_tg_recv_md_req(struct nfc_digital_dev *ddev, void *arg,
+ struct sk_buff *resp)
+{
+ u8 rf_tech;
+ int rc;
+
+ if (IS_ERR(resp)) {
+ resp = NULL;
+ goto exit_free_skb;
+ }
+
+ rc = ddev->ops->tg_get_rf_tech(ddev, &rf_tech);
+ if (rc)
+ goto exit_free_skb;
+
+ switch (rf_tech) {
+ case NFC_DIGITAL_RF_TECH_106A:
+ rc = digital_tg_config_nfca(ddev);
+ if (rc)
+ goto exit_free_skb;
+ digital_tg_recv_sens_req(ddev, arg, resp);
+ break;
+ case NFC_DIGITAL_RF_TECH_212F:
+ case NFC_DIGITAL_RF_TECH_424F:
+ rc = digital_tg_config_nfcf(ddev, rf_tech);
+ if (rc)
+ goto exit_free_skb;
+ digital_tg_recv_sensf_req(ddev, arg, resp);
+ break;
+ default:
+ goto exit_free_skb;
+ }
+
+ return;
+
+exit_free_skb:
+ digital_poll_next_tech(ddev);
+ dev_kfree_skb(resp);
+}
diff --git a/net/nfc/hci/Kconfig b/net/nfc/hci/Kconfig
new file mode 100644
index 0000000..fd67f51
--- /dev/null
+++ b/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.
diff --git a/net/nfc/hci/Makefile b/net/nfc/hci/Makefile
new file mode 100644
index 0000000..c5dbb68
--- /dev/null
+++ b/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
diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c
new file mode 100644
index 0000000..844673c
--- /dev/null
+++ b/net/nfc/hci/command.c
@@ -0,0 +1,372 @@
+/*
+ * 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);
+
+/*
+ * 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 *) ¶ms, 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;
+
+ nfc_hci_reset_pipes(hdev);
+
+ 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 (pipe == NFC_HCI_DO_NOT_CREATE_PIPE)
+ return 0;
+
+ 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->pipes[pipe].gate = dest_gate;
+ hdev->pipes[pipe].dest_host = dest_host;
+ hdev->gate2pipe[dest_gate] = pipe;
+
+ return 0;
+}
+EXPORT_SYMBOL(nfc_hci_connect_gate);
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
new file mode 100644
index 0000000..19cb2e4
--- /dev/null
+++ b/net/nfc/hci/core.c
@@ -0,0 +1,1104 @@
+/*
+ * 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);
+
+void nfc_hci_reset_pipes(struct nfc_hci_dev *hdev)
+{
+ int i = 0;
+
+ for (i = 0; i < NFC_HCI_MAX_PIPES; i++) {
+ hdev->pipes[i].gate = NFC_HCI_INVALID_GATE;
+ hdev->pipes[i].dest_host = NFC_HCI_INVALID_HOST;
+ }
+ memset(hdev->gate2pipe, NFC_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+}
+EXPORT_SYMBOL(nfc_hci_reset_pipes);
+
+void nfc_hci_reset_pipes_per_host(struct nfc_hci_dev *hdev, u8 host)
+{
+ int i = 0;
+
+ for (i = 0; i < NFC_HCI_MAX_PIPES; i++) {
+ if (hdev->pipes[i].dest_host != host)
+ continue;
+
+ hdev->pipes[i].gate = NFC_HCI_INVALID_GATE;
+ hdev->pipes[i].dest_host = NFC_HCI_INVALID_HOST;
+ }
+}
+EXPORT_SYMBOL(nfc_hci_reset_pipes_per_host);
+
+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)
+{
+ u8 gate = hdev->pipes[pipe].gate;
+ u8 status = NFC_HCI_ANY_OK;
+ struct hci_create_pipe_resp *create_info;
+ struct hci_delete_pipe_noti *delete_info;
+ struct hci_all_pipe_cleared_noti *cleared_info;
+
+ pr_debug("from gate %x pipe %x cmd %x\n", gate, pipe, cmd);
+
+ switch (cmd) {
+ case NFC_HCI_ADM_NOTIFY_PIPE_CREATED:
+ if (skb->len != 5) {
+ status = NFC_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ create_info = (struct hci_create_pipe_resp *)skb->data;
+
+ if (create_info->pipe >= NFC_HCI_MAX_PIPES) {
+ status = NFC_HCI_ANY_E_NOK;
+ goto exit;
+ }
+
+ /* Save the new created pipe and bind with local gate,
+ * the description for skb->data[3] is destination gate id
+ * but since we received this cmd from host controller, we
+ * are the destination and it is our local gate
+ */
+ hdev->gate2pipe[create_info->dest_gate] = create_info->pipe;
+ hdev->pipes[create_info->pipe].gate = create_info->dest_gate;
+ hdev->pipes[create_info->pipe].dest_host =
+ create_info->src_host;
+ break;
+ case NFC_HCI_ANY_OPEN_PIPE:
+ if (gate == NFC_HCI_INVALID_GATE) {
+ status = NFC_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ break;
+ case NFC_HCI_ADM_NOTIFY_PIPE_DELETED:
+ if (skb->len != 1) {
+ status = NFC_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ delete_info = (struct hci_delete_pipe_noti *)skb->data;
+
+ if (delete_info->pipe >= NFC_HCI_MAX_PIPES) {
+ status = NFC_HCI_ANY_E_NOK;
+ goto exit;
+ }
+
+ hdev->pipes[delete_info->pipe].gate = NFC_HCI_INVALID_GATE;
+ hdev->pipes[delete_info->pipe].dest_host = NFC_HCI_INVALID_HOST;
+ break;
+ case NFC_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED:
+ if (skb->len != 1) {
+ status = NFC_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ cleared_info = (struct hci_all_pipe_cleared_noti *)skb->data;
+
+ nfc_hci_reset_pipes_per_host(hdev, cleared_info->host);
+ break;
+ default:
+ pr_info("Discarded unknown cmd %x to gate %x\n", cmd, gate);
+ break;
+ }
+
+ if (hdev->ops->cmd_received)
+ hdev->ops->cmd_received(hdev, pipe, cmd, skb);
+
+exit:
+ nfc_hci_hcp_message_tx(hdev, pipe, NFC_HCI_HCP_RESPONSE,
+ status, NULL, 0, NULL, NULL, 0);
+
+ 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 = hdev->pipes[pipe].gate;
+
+ if (gate == NFC_HCI_INVALID_GATE) {
+ 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, pipe, 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(struct timer_list *t)
+{
+ struct nfc_hci_dev *hdev = from_timer(hdev, t, cmd_timer);
+
+ 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);
+
+ nfc_hci_reset_pipes(hdev);
+
+ 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,
+ u8 mode)
+{
+}
+
+#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;
+ }
+
+ *(u8 *)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 int hci_se_io(struct nfc_dev *nfc_dev, u32 se_idx,
+ u8 *apdu, size_t apdu_length,
+ se_io_cb_t cb, void *cb_context)
+{
+ struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+ if (hdev->ops->se_io)
+ return hdev->ops->se_io(hdev, se_idx, apdu,
+ apdu_length, cb, cb_context);
+
+ 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_u8(hcp_skb, pipe);
+
+ skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) {
+ msg_len = frag_skb->len - NFC_HCI_HCP_PACKET_HEADER_LEN;
+ skb_put_data(hcp_skb,
+ 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,
+ .se_io = hci_se_io,
+};
+
+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);
+
+ nfc_hci_reset_pipes(hdev);
+
+ 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);
+
+ timer_setup(&hdev->cmd_timer, nfc_hci_cmd_timeout, 0);
+
+ skb_queue_head_init(&hdev->rx_hcp_frags);
+
+ INIT_WORK(&hdev->msg_rx_work, nfc_hci_msg_rx_work);
+
+ skb_queue_head_init(&hdev->msg_rx_queue);
+
+ return nfc_register_device(hdev->ndev);
+}
+EXPORT_SYMBOL(nfc_hci_register_device);
+
+void nfc_hci_unregister_device(struct nfc_hci_dev *hdev)
+{
+ struct hci_msg *msg, *n;
+
+ mutex_lock(&hdev->msg_tx_mutex);
+
+ if (hdev->cmd_pending_msg) {
+ if (hdev->cmd_pending_msg->cb)
+ hdev->cmd_pending_msg->cb(
+ hdev->cmd_pending_msg->cb_context,
+ NULL, -ESHUTDOWN);
+ kfree(hdev->cmd_pending_msg);
+ hdev->cmd_pending_msg = NULL;
+ }
+
+ hdev->shutting_down = true;
+
+ mutex_unlock(&hdev->msg_tx_mutex);
+
+ del_timer_sync(&hdev->cmd_timer);
+ cancel_work_sync(&hdev->msg_tx_work);
+
+ cancel_work_sync(&hdev->msg_rx_work);
+
+ nfc_unregister_device(hdev->ndev);
+
+ skb_queue_purge(&hdev->rx_hcp_frags);
+ skb_queue_purge(&hdev->msg_rx_queue);
+
+ list_for_each_entry_safe(msg, n, &hdev->msg_tx_queue, msg_l) {
+ list_del(&msg->msg_l);
+ skb_queue_purge(&msg->msg_frags);
+ kfree(msg);
+ }
+}
+EXPORT_SYMBOL(nfc_hci_unregister_device);
+
+void nfc_hci_set_clientdata(struct nfc_hci_dev *hdev, void *clientdata)
+{
+ hdev->clientdata = clientdata;
+}
+EXPORT_SYMBOL(nfc_hci_set_clientdata);
+
+void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev)
+{
+ return hdev->clientdata;
+}
+EXPORT_SYMBOL(nfc_hci_get_clientdata);
+
+void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err)
+{
+ nfc_hci_failure(hdev, err);
+}
+EXPORT_SYMBOL(nfc_hci_driver_failure);
+
+void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+{
+ nfc_llc_rcv_from_drv(hdev->llc, skb);
+}
+EXPORT_SYMBOL(nfc_hci_recv_frame);
+
+static int __init nfc_hci_init(void)
+{
+ return nfc_llc_init();
+}
+
+static void __exit nfc_hci_exit(void)
+{
+ nfc_llc_exit();
+}
+
+subsys_initcall(nfc_hci_init);
+module_exit(nfc_hci_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NFC HCI Core");
diff --git a/net/nfc/hci/hci.h b/net/nfc/hci/hci.h
new file mode 100644
index 0000000..ab4c8e8
--- /dev/null
+++ b/net/nfc/hci/hci.h
@@ -0,0 +1,132 @@
+/*
+ * 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;
+
+struct hci_delete_pipe_noti {
+ u8 pipe;
+} __packed;
+
+struct hci_all_pipe_cleared_noti {
+ u8 host;
+} __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);
+
+void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type,
+ u8 instruction, struct sk_buff *skb);
+
+/* HCP headers */
+#define NFC_HCI_HCP_PACKET_HEADER_LEN 1
+#define NFC_HCI_HCP_MESSAGE_HEADER_LEN 1
+#define NFC_HCI_HCP_HEADER_LEN 2
+
+/* HCP types */
+#define NFC_HCI_HCP_COMMAND 0x00
+#define NFC_HCI_HCP_EVENT 0x01
+#define NFC_HCI_HCP_RESPONSE 0x02
+
+/* Generic commands */
+#define NFC_HCI_ANY_SET_PARAMETER 0x01
+#define NFC_HCI_ANY_GET_PARAMETER 0x02
+#define NFC_HCI_ANY_OPEN_PIPE 0x03
+#define NFC_HCI_ANY_CLOSE_PIPE 0x04
+
+/* Reader RF commands */
+#define NFC_HCI_WR_XCHG_DATA 0x10
+
+/* Admin commands */
+#define NFC_HCI_ADM_CREATE_PIPE 0x10
+#define NFC_HCI_ADM_DELETE_PIPE 0x11
+#define NFC_HCI_ADM_NOTIFY_PIPE_CREATED 0x12
+#define NFC_HCI_ADM_NOTIFY_PIPE_DELETED 0x13
+#define NFC_HCI_ADM_CLEAR_ALL_PIPE 0x14
+#define NFC_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED 0x15
+
+/* Generic responses */
+#define NFC_HCI_ANY_OK 0x00
+#define NFC_HCI_ANY_E_NOT_CONNECTED 0x01
+#define NFC_HCI_ANY_E_CMD_PAR_UNKNOWN 0x02
+#define NFC_HCI_ANY_E_NOK 0x03
+#define NFC_HCI_ANY_E_PIPES_FULL 0x04
+#define NFC_HCI_ANY_E_REG_PAR_UNKNOWN 0x05
+#define NFC_HCI_ANY_E_PIPE_NOT_OPENED 0x06
+#define NFC_HCI_ANY_E_CMD_NOT_SUPPORTED 0x07
+#define NFC_HCI_ANY_E_INHIBITED 0x08
+#define NFC_HCI_ANY_E_TIMEOUT 0x09
+#define NFC_HCI_ANY_E_REG_ACCESS_DENIED 0x0a
+#define NFC_HCI_ANY_E_PIPE_ACCESS_DENIED 0x0b
+
+#endif /* __LOCAL_HCI_H */
diff --git a/net/nfc/hci/hcp.c b/net/nfc/hci/hcp.c
new file mode 100644
index 0000000..1fe725d
--- /dev/null
+++ b/net/nfc/hci/hcp.c
@@ -0,0 +1,150 @@
+/*
+ * 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;
+}
+
+/*
+ * Receive hcp message for pipe, with type and cmd.
+ * skb contains optional message data only.
+ */
+void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type,
+ u8 instruction, struct sk_buff *skb)
+{
+ switch (type) {
+ case NFC_HCI_HCP_RESPONSE:
+ nfc_hci_resp_received(hdev, instruction, skb);
+ break;
+ case NFC_HCI_HCP_COMMAND:
+ nfc_hci_cmd_received(hdev, pipe, instruction, skb);
+ break;
+ case NFC_HCI_HCP_EVENT:
+ nfc_hci_event_received(hdev, pipe, instruction, skb);
+ break;
+ default:
+ pr_err("UNKNOWN MSG Type %d, instruction=%d\n",
+ type, instruction);
+ kfree_skb(skb);
+ break;
+ }
+}
diff --git a/net/nfc/hci/llc.c b/net/nfc/hci/llc.c
new file mode 100644
index 0000000..3d699cb
--- /dev/null
+++ b/net/nfc/hci/llc.c
@@ -0,0 +1,161 @@
+/*
+ * 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);
+}
+
+int nfc_llc_start(struct nfc_llc *llc)
+{
+ return llc->ops->start(llc);
+}
+EXPORT_SYMBOL(nfc_llc_start);
+
+int nfc_llc_stop(struct nfc_llc *llc)
+{
+ return llc->ops->stop(llc);
+}
+EXPORT_SYMBOL(nfc_llc_stop);
+
+void nfc_llc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+ llc->ops->rcv_from_drv(llc, skb);
+}
+
+int nfc_llc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+ return llc->ops->xmit_from_hci(llc, skb);
+}
+
+void *nfc_llc_get_data(struct nfc_llc *llc)
+{
+ return llc->data;
+}
diff --git a/net/nfc/hci/llc.h b/net/nfc/hci/llc.h
new file mode 100644
index 0000000..5dad4c5
--- /dev/null
+++ b/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_ */
diff --git a/net/nfc/hci/llc_nop.c b/net/nfc/hci/llc_nop.c
new file mode 100644
index 0000000..d0435d5
--- /dev/null
+++ b/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);
+}
diff --git a/net/nfc/hci/llc_shdlc.c b/net/nfc/hci/llc_shdlc.c
new file mode 100644
index 0000000..fe98893
--- /dev/null
+++ b/net/nfc/hci/llc_shdlc.c
@@ -0,0 +1,846 @@
+/*
+ * 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;
+
+ *(u8 *)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);
+
+ *(u8 *)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_u8(skb, SHDLC_MAX_WINDOW);
+ skb_put_u8(skb, 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);
+
+ *(u8 *)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(struct timer_list *t)
+{
+ struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);
+
+ pr_debug("\n");
+
+ schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_t1_timeout(struct timer_list *t)
+{
+ struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);
+
+ pr_debug("SoftIRQ: need to send ack\n");
+
+ schedule_work(&shdlc->sm_work);
+}
+
+static void llc_shdlc_t2_timeout(struct timer_list *t)
+{
+ struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);
+
+ 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;
+
+ timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0);
+ timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0);
+ timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0);
+
+ shdlc->w = SHDLC_MAX_WINDOW;
+ shdlc->srej_support = SHDLC_SREJ_SUPPORT;
+
+ skb_queue_head_init(&shdlc->rcv_q);
+ skb_queue_head_init(&shdlc->send_q);
+ skb_queue_head_init(&shdlc->ack_pending_q);
+
+ INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
+
+ shdlc->hdev = hdev;
+ shdlc->xmit_to_drv = xmit_to_drv;
+ shdlc->rcv_to_hci = rcv_to_hci;
+ shdlc->tx_headroom = tx_headroom;
+ shdlc->tx_tailroom = tx_tailroom;
+ shdlc->llc_failure = llc_failure;
+
+ return shdlc;
+}
+
+static void llc_shdlc_deinit(struct nfc_llc *llc)
+{
+ struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+ skb_queue_purge(&shdlc->rcv_q);
+ skb_queue_purge(&shdlc->send_q);
+ skb_queue_purge(&shdlc->ack_pending_q);
+
+ kfree(shdlc);
+}
+
+static int llc_shdlc_start(struct nfc_llc *llc)
+{
+ struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+ return llc_shdlc_connect(shdlc);
+}
+
+static int llc_shdlc_stop(struct nfc_llc *llc)
+{
+ struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+ llc_shdlc_disconnect(shdlc);
+
+ return 0;
+}
+
+static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
+{
+ struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+ llc_shdlc_recv_frame(shdlc, skb);
+}
+
+static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
+{
+ struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
+
+ skb_queue_tail(&shdlc->send_q, skb);
+
+ schedule_work(&shdlc->sm_work);
+
+ return 0;
+}
+
+static struct nfc_llc_ops llc_shdlc_ops = {
+ .init = llc_shdlc_init,
+ .deinit = llc_shdlc_deinit,
+ .start = llc_shdlc_start,
+ .stop = llc_shdlc_stop,
+ .rcv_from_drv = llc_shdlc_rcv_from_drv,
+ .xmit_from_hci = llc_shdlc_xmit_from_hci,
+};
+
+int nfc_llc_shdlc_register(void)
+{
+ return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
+}
diff --git a/net/nfc/llcp.h b/net/nfc/llcp.h
new file mode 100644
index 0000000..1f68724
--- /dev/null
+++ b/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, int kern);
+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);
diff --git a/net/nfc/llcp_commands.c b/net/nfc/llcp_commands.c
new file mode 100644
index 0000000..6a196e4
--- /dev/null
+++ b/net/nfc/llcp_commands.c
@@ -0,0 +1,804 @@
+/*
+ * 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->tlv_len is u8, takes uri_len, + 3 for header, + 1 for NULL */
+ if (WARN_ON_ONCE(uri_len > U8_MAX - 4))
+ return NULL;
+
+ 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]);
+
+ skb_put_data(pdu, 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;
+
+ skb_put_data(pdu, 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;
+ __be16 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;
+ }
+
+ llcp_add_tlv(skb, service_name_tlv, service_name_tlv_length);
+ llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+ llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+ skb_queue_tail(&local->tx_queue, skb);
+
+ err = 0;
+
+error_tlv:
+ if (err)
+ 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;
+ __be16 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;
+ }
+
+ llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+ llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+ skb_queue_tail(&local->tx_queue, skb);
+
+ err = 0;
+
+error_tlv:
+ if (err)
+ 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) {
+ skb_put_data(skb, 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);
+
+ skb_put_data(skb, 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);
+
+ skb_put_data(skb, &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 = kmalloc(len, GFP_USER | __GFP_NOWARN);
+ if (msg_data == NULL)
+ return -ENOMEM;
+
+ if (memcpy_from_msg(msg_data, msg, 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))
+ skb_put_data(pdu, 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 = kmalloc(len, GFP_USER | __GFP_NOWARN);
+ if (msg_data == NULL)
+ return -ENOMEM;
+
+ if (memcpy_from_msg(msg_data, msg, 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, 0,
+ frag_len + LLCP_HEADER_SIZE, &err);
+ if (pdu == NULL) {
+ pr_err("Could not allocate PDU (error=%d)\n", err);
+ len -= remaining_len;
+ if (len == 0)
+ len = err;
+ break;
+ }
+
+ pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+ if (likely(frag_len > 0))
+ skb_put_data(pdu, msg_ptr, frag_len);
+
+ /* No need to check for the peer RW for UI frames */
+ skb_queue_tail(&local->tx_queue, pdu);
+
+ remaining_len -= frag_len;
+ msg_ptr += frag_len;
+ } while (remaining_len > 0);
+
+ kfree(msg_data);
+
+ return len;
+}
+
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
+{
+ struct sk_buff *skb;
+ struct nfc_llcp_local *local;
+
+ pr_debug("Send rr nr %d\n", sock->recv_n);
+
+ local = sock->local;
+ if (local == NULL)
+ return -ENODEV;
+
+ skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
+ if (skb == NULL)
+ return -ENOMEM;
+
+ skb_put(skb, LLCP_SEQUENCE_SIZE);
+
+ skb->data[2] = sock->recv_n;
+
+ skb_queue_head(&local->tx_queue, skb);
+
+ return 0;
+}
diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c
new file mode 100644
index 0000000..ef4026a
--- /dev/null
+++ b/net/nfc/llcp_core.c
@@ -0,0 +1,1632 @@
+/*
+ * Copyright (C) 2011 Intel Corporation. All rights reserved.
+ * Copyright (C) 2014 Marvell International Ltd.
+ *
+ * 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/list.h>
+#include <linux/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
+
+static LIST_HEAD(llcp_devices);
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
+
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
+{
+ write_lock(&l->lock);
+ sk_add_node(sk, &l->head);
+ write_unlock(&l->lock);
+}
+
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
+{
+ write_lock(&l->lock);
+ sk_del_node_init(sk);
+ write_unlock(&l->lock);
+}
+
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
+{
+ sock->remote_rw = LLCP_DEFAULT_RW;
+ sock->remote_miu = LLCP_MAX_MIU + 1;
+}
+
+static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
+{
+ struct nfc_llcp_local *local = sock->local;
+ struct sk_buff *s, *tmp;
+
+ pr_debug("%p\n", &sock->sk);
+
+ skb_queue_purge(&sock->tx_queue);
+ skb_queue_purge(&sock->tx_pending_queue);
+
+ if (local == NULL)
+ return;
+
+ /* Search for local pending SKBs that are related to this socket */
+ skb_queue_walk_safe(&local->tx_queue, s, tmp) {
+ if (s->sk != &sock->sk)
+ continue;
+
+ skb_unlink(s, &local->tx_queue);
+ kfree_skb(s);
+ }
+}
+
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
+ int err)
+{
+ struct sock *sk;
+ struct hlist_node *tmp;
+ struct nfc_llcp_sock *llcp_sock;
+
+ skb_queue_purge(&local->tx_queue);
+
+ write_lock(&local->sockets.lock);
+
+ sk_for_each_safe(sk, tmp, &local->sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
+
+ bh_lock_sock(sk);
+
+ nfc_llcp_socket_purge(llcp_sock);
+
+ if (sk->sk_state == LLCP_CONNECTED)
+ nfc_put_device(llcp_sock->dev);
+
+ if (sk->sk_state == LLCP_LISTEN) {
+ struct nfc_llcp_sock *lsk, *n;
+ struct sock *accept_sk;
+
+ list_for_each_entry_safe(lsk, n,
+ &llcp_sock->accept_queue,
+ accept_queue) {
+ accept_sk = &lsk->sk;
+ bh_lock_sock(accept_sk);
+
+ nfc_llcp_accept_unlink(accept_sk);
+
+ if (err)
+ accept_sk->sk_err = err;
+ accept_sk->sk_state = LLCP_CLOSED;
+ accept_sk->sk_state_change(sk);
+
+ bh_unlock_sock(accept_sk);
+ }
+ }
+
+ if (err)
+ sk->sk_err = err;
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+
+ bh_unlock_sock(sk);
+
+ sk_del_node_init(sk);
+ }
+
+ write_unlock(&local->sockets.lock);
+
+ /* If we still have a device, we keep the RAW sockets alive */
+ if (device == true)
+ return;
+
+ write_lock(&local->raw_sockets.lock);
+
+ sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
+
+ bh_lock_sock(sk);
+
+ nfc_llcp_socket_purge(llcp_sock);
+
+ if (err)
+ sk->sk_err = err;
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+
+ bh_unlock_sock(sk);
+
+ sk_del_node_init(sk);
+ }
+
+ write_unlock(&local->raw_sockets.lock);
+}
+
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
+{
+ kref_get(&local->ref);
+
+ return local;
+}
+
+static void local_cleanup(struct nfc_llcp_local *local)
+{
+ nfc_llcp_socket_release(local, false, ENXIO);
+ del_timer_sync(&local->link_timer);
+ skb_queue_purge(&local->tx_queue);
+ cancel_work_sync(&local->tx_work);
+ cancel_work_sync(&local->rx_work);
+ cancel_work_sync(&local->timeout_work);
+ kfree_skb(local->rx_pending);
+ del_timer_sync(&local->sdreq_timer);
+ cancel_work_sync(&local->sdreq_timeout_work);
+ nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
+}
+
+static void local_release(struct kref *ref)
+{
+ struct nfc_llcp_local *local;
+
+ local = container_of(ref, struct nfc_llcp_local, ref);
+
+ list_del(&local->list);
+ local_cleanup(local);
+ kfree(local);
+}
+
+int nfc_llcp_local_put(struct nfc_llcp_local *local)
+{
+ if (local == NULL)
+ return 0;
+
+ return kref_put(&local->ref, local_release);
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
+ u8 ssap, u8 dsap)
+{
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+ pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
+ if (ssap == 0 && dsap == 0)
+ return NULL;
+
+ read_lock(&local->sockets.lock);
+
+ llcp_sock = NULL;
+
+ sk_for_each(sk, &local->sockets.head) {
+ tmp_sock = nfc_llcp_sock(sk);
+
+ if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
+ llcp_sock = tmp_sock;
+ break;
+ }
+ }
+
+ read_unlock(&local->sockets.lock);
+
+ if (llcp_sock == NULL)
+ return NULL;
+
+ sock_hold(&llcp_sock->sk);
+
+ return llcp_sock;
+}
+
+static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+{
+ sock_put(&sock->sk);
+}
+
+static void nfc_llcp_timeout_work(struct work_struct *work)
+{
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ timeout_work);
+
+ nfc_dep_link_down(local->dev);
+}
+
+static void nfc_llcp_symm_timer(struct timer_list *t)
+{
+ struct nfc_llcp_local *local = from_timer(local, t, link_timer);
+
+ pr_err("SYMM timeout\n");
+
+ schedule_work(&local->timeout_work);
+}
+
+static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
+{
+ unsigned long time;
+ HLIST_HEAD(nl_sdres_list);
+ struct hlist_node *n;
+ struct nfc_llcp_sdp_tlv *sdp;
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ sdreq_timeout_work);
+
+ mutex_lock(&local->sdreq_lock);
+
+ time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
+
+ hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
+ if (time_after(sdp->time, time))
+ continue;
+
+ sdp->sap = LLCP_SDP_UNBOUND;
+
+ hlist_del(&sdp->node);
+
+ hlist_add_head(&sdp->node, &nl_sdres_list);
+ }
+
+ if (!hlist_empty(&local->pending_sdreqs))
+ mod_timer(&local->sdreq_timer,
+ jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+ mutex_unlock(&local->sdreq_lock);
+
+ if (!hlist_empty(&nl_sdres_list))
+ nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+}
+
+static void nfc_llcp_sdreq_timer(struct timer_list *t)
+{
+ struct nfc_llcp_local *local = from_timer(local, t, sdreq_timer);
+
+ schedule_work(&local->sdreq_timeout_work);
+}
+
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
+{
+ struct nfc_llcp_local *local;
+
+ list_for_each_entry(local, &llcp_devices, list)
+ if (local->dev == dev)
+ return local;
+
+ pr_debug("No device found\n");
+
+ return NULL;
+}
+
+static char *wks[] = {
+ NULL,
+ NULL, /* SDP */
+ "urn:nfc:sn:ip",
+ "urn:nfc:sn:obex",
+ "urn:nfc:sn:snep",
+};
+
+static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
+{
+ int sap, num_wks;
+
+ pr_debug("%s\n", service_name);
+
+ if (service_name == NULL)
+ return -EINVAL;
+
+ num_wks = ARRAY_SIZE(wks);
+
+ for (sap = 0; sap < num_wks; sap++) {
+ if (wks[sap] == NULL)
+ continue;
+
+ if (strncmp(wks[sap], service_name, service_name_len) == 0)
+ return sap;
+ }
+
+ return -EINVAL;
+}
+
+static
+struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
+ u8 *sn, size_t sn_len)
+{
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+ pr_debug("sn %zd %p\n", sn_len, sn);
+
+ if (sn == NULL || sn_len == 0)
+ return NULL;
+
+ read_lock(&local->sockets.lock);
+
+ llcp_sock = NULL;
+
+ sk_for_each(sk, &local->sockets.head) {
+ tmp_sock = nfc_llcp_sock(sk);
+
+ pr_debug("llcp sock %p\n", tmp_sock);
+
+ if (tmp_sock->sk.sk_type == SOCK_STREAM &&
+ tmp_sock->sk.sk_state != LLCP_LISTEN)
+ continue;
+
+ if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
+ tmp_sock->sk.sk_state != LLCP_BOUND)
+ continue;
+
+ if (tmp_sock->service_name == NULL ||
+ tmp_sock->service_name_len == 0)
+ continue;
+
+ if (tmp_sock->service_name_len != sn_len)
+ continue;
+
+ if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
+ llcp_sock = tmp_sock;
+ break;
+ }
+ }
+
+ read_unlock(&local->sockets.lock);
+
+ pr_debug("Found llcp sock %p\n", llcp_sock);
+
+ return llcp_sock;
+}
+
+u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
+ struct nfc_llcp_sock *sock)
+{
+ mutex_lock(&local->sdp_lock);
+
+ if (sock->service_name != NULL && sock->service_name_len > 0) {
+ int ssap = nfc_llcp_wks_sap(sock->service_name,
+ sock->service_name_len);
+
+ if (ssap > 0) {
+ pr_debug("WKS %d\n", ssap);
+
+ /* This is a WKS, let's check if it's free */
+ if (local->local_wks & BIT(ssap)) {
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_SAP_MAX;
+ }
+
+ set_bit(ssap, &local->local_wks);
+ mutex_unlock(&local->sdp_lock);
+
+ return ssap;
+ }
+
+ /*
+ * Check if there already is a non WKS socket bound
+ * to this service name.
+ */
+ if (nfc_llcp_sock_from_sn(local, sock->service_name,
+ sock->service_name_len) != NULL) {
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_SAP_MAX;
+ }
+
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_SDP_UNBOUND;
+
+ } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
+ if (!test_bit(sock->ssap, &local->local_wks)) {
+ set_bit(sock->ssap, &local->local_wks);
+ mutex_unlock(&local->sdp_lock);
+
+ return sock->ssap;
+ }
+ }
+
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_SAP_MAX;
+}
+
+u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
+{
+ u8 local_ssap;
+
+ mutex_lock(&local->sdp_lock);
+
+ local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
+ if (local_ssap == LLCP_LOCAL_NUM_SAP) {
+ mutex_unlock(&local->sdp_lock);
+ return LLCP_SAP_MAX;
+ }
+
+ set_bit(local_ssap, &local->local_sap);
+
+ mutex_unlock(&local->sdp_lock);
+
+ return local_ssap + LLCP_LOCAL_SAP_OFFSET;
+}
+
+void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
+{
+ u8 local_ssap;
+ unsigned long *sdp;
+
+ if (ssap < LLCP_WKS_NUM_SAP) {
+ local_ssap = ssap;
+ sdp = &local->local_wks;
+ } else if (ssap < LLCP_LOCAL_NUM_SAP) {
+ atomic_t *client_cnt;
+
+ local_ssap = ssap - LLCP_WKS_NUM_SAP;
+ sdp = &local->local_sdp;
+ client_cnt = &local->local_sdp_cnt[local_ssap];
+
+ pr_debug("%d clients\n", atomic_read(client_cnt));
+
+ mutex_lock(&local->sdp_lock);
+
+ if (atomic_dec_and_test(client_cnt)) {
+ struct nfc_llcp_sock *l_sock;
+
+ pr_debug("No more clients for SAP %d\n", ssap);
+
+ clear_bit(local_ssap, sdp);
+
+ /* Find the listening sock and set it back to UNBOUND */
+ l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
+ if (l_sock) {
+ l_sock->ssap = LLCP_SDP_UNBOUND;
+ nfc_llcp_sock_put(l_sock);
+ }
+ }
+
+ mutex_unlock(&local->sdp_lock);
+
+ return;
+ } else if (ssap < LLCP_MAX_SAP) {
+ local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
+ sdp = &local->local_sap;
+ } else {
+ return;
+ }
+
+ mutex_lock(&local->sdp_lock);
+
+ clear_bit(local_ssap, sdp);
+
+ mutex_unlock(&local->sdp_lock);
+}
+
+static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
+{
+ u8 ssap;
+
+ mutex_lock(&local->sdp_lock);
+
+ ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
+ if (ssap == LLCP_SDP_NUM_SAP) {
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_SAP_MAX;
+ }
+
+ pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
+
+ set_bit(ssap, &local->local_sdp);
+
+ mutex_unlock(&local->sdp_lock);
+
+ return LLCP_WKS_NUM_SAP + ssap;
+}
+
+static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
+{
+ u8 *gb_cur, *version_tlv, version, version_length;
+ u8 *lto_tlv, lto_length;
+ u8 *wks_tlv, wks_length;
+ u8 *miux_tlv, miux_length;
+ __be16 wks = cpu_to_be16(local->local_wks);
+ u8 gb_len = 0;
+ int ret = 0;
+
+ version = LLCP_VERSION_11;
+ version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
+ 1, &version_length);
+ gb_len += version_length;
+
+ lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, <o_length);
+ gb_len += lto_length;
+
+ pr_debug("Local wks 0x%lx\n", local->local_wks);
+ wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&wks, 2, &wks_length);
+ gb_len += wks_length;
+
+ miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
+ &miux_length);
+ gb_len += miux_length;
+
+ gb_len += ARRAY_SIZE(llcp_magic);
+
+ if (gb_len > NFC_MAX_GT_LEN) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ gb_cur = local->gb;
+
+ memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
+ gb_cur += ARRAY_SIZE(llcp_magic);
+
+ memcpy(gb_cur, version_tlv, version_length);
+ gb_cur += version_length;
+
+ memcpy(gb_cur, lto_tlv, lto_length);
+ gb_cur += lto_length;
+
+ memcpy(gb_cur, wks_tlv, wks_length);
+ gb_cur += wks_length;
+
+ memcpy(gb_cur, miux_tlv, miux_length);
+ gb_cur += miux_length;
+
+ local->gb_len = gb_len;
+
+out:
+ kfree(version_tlv);
+ kfree(lto_tlv);
+ kfree(wks_tlv);
+ kfree(miux_tlv);
+
+ return ret;
+}
+
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ *general_bytes_len = 0;
+ return NULL;
+ }
+
+ nfc_llcp_build_gb(local);
+
+ *general_bytes_len = local->gb_len;
+
+ return local->gb;
+}
+
+int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
+{
+ struct nfc_llcp_local *local;
+
+ if (gb_len < 3 || gb_len > NFC_MAX_GT_LEN)
+ return -EINVAL;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ pr_err("No LLCP device\n");
+ return -ENODEV;
+ }
+
+ memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
+ memcpy(local->remote_gb, gb, gb_len);
+ local->remote_gb_len = gb_len;
+
+ if (memcmp(local->remote_gb, llcp_magic, 3)) {
+ pr_err("MAC does not support LLCP\n");
+ return -EINVAL;
+ }
+
+ return nfc_llcp_parse_gb_tlv(local,
+ &local->remote_gb[3],
+ local->remote_gb_len - 3);
+}
+
+static u8 nfc_llcp_dsap(struct sk_buff *pdu)
+{
+ return (pdu->data[0] & 0xfc) >> 2;
+}
+
+static u8 nfc_llcp_ptype(struct sk_buff *pdu)
+{
+ return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
+}
+
+static u8 nfc_llcp_ssap(struct sk_buff *pdu)
+{
+ return pdu->data[1] & 0x3f;
+}
+
+static u8 nfc_llcp_ns(struct sk_buff *pdu)
+{
+ return pdu->data[2] >> 4;
+}
+
+static u8 nfc_llcp_nr(struct sk_buff *pdu)
+{
+ return pdu->data[2] & 0xf;
+}
+
+static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
+{
+ pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
+ sock->send_n = (sock->send_n + 1) % 16;
+ sock->recv_ack_n = (sock->recv_n - 1) % 16;
+}
+
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+ struct sk_buff *skb, u8 direction)
+{
+ struct sk_buff *skb_copy = NULL, *nskb;
+ struct sock *sk;
+ u8 *data;
+
+ read_lock(&local->raw_sockets.lock);
+
+ sk_for_each(sk, &local->raw_sockets.head) {
+ if (sk->sk_state != LLCP_BOUND)
+ continue;
+
+ if (skb_copy == NULL) {
+ skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
+ GFP_ATOMIC, true);
+
+ if (skb_copy == NULL)
+ continue;
+
+ data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
+
+ data[0] = local->dev ? local->dev->idx : 0xFF;
+ data[1] = direction & 0x01;
+ data[1] |= (RAW_PAYLOAD_LLCP << 1);
+ }
+
+ nskb = skb_clone(skb_copy, GFP_ATOMIC);
+ if (!nskb)
+ continue;
+
+ if (sock_queue_rcv_skb(sk, nskb))
+ kfree_skb(nskb);
+ }
+
+ read_unlock(&local->raw_sockets.lock);
+
+ kfree_skb(skb_copy);
+}
+
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ tx_work);
+ struct sk_buff *skb;
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
+
+ skb = skb_dequeue(&local->tx_queue);
+ if (skb != NULL) {
+ sk = skb->sk;
+ llcp_sock = nfc_llcp_sock(sk);
+
+ if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+ kfree_skb(skb);
+ nfc_llcp_send_symm(local->dev);
+ } else if (llcp_sock && !llcp_sock->remote_ready) {
+ skb_queue_head(&local->tx_queue, skb);
+ nfc_llcp_send_symm(local->dev);
+ } else {
+ struct sk_buff *copy_skb = NULL;
+ u8 ptype = nfc_llcp_ptype(skb);
+ int ret;
+
+ pr_debug("Sending pending skb\n");
+ print_hex_dump_debug("LLCP Tx: ", DUMP_PREFIX_OFFSET,
+ 16, 1, skb->data, skb->len, true);
+
+ if (ptype == LLCP_PDU_DISC && sk != NULL &&
+ sk->sk_state == LLCP_DISCONNECTING) {
+ nfc_llcp_sock_unlink(&local->sockets, sk);
+ sock_orphan(sk);
+ sock_put(sk);
+ }
+
+ if (ptype == LLCP_PDU_I)
+ copy_skb = skb_copy(skb, GFP_ATOMIC);
+
+ __net_timestamp(skb);
+
+ nfc_llcp_send_to_raw_sock(local, skb,
+ NFC_DIRECTION_TX);
+
+ ret = nfc_data_exchange(local->dev, local->target_idx,
+ skb, nfc_llcp_recv, local);
+
+ if (ret) {
+ kfree_skb(copy_skb);
+ goto out;
+ }
+
+ if (ptype == LLCP_PDU_I && copy_skb)
+ skb_queue_tail(&llcp_sock->tx_pending_queue,
+ copy_skb);
+ }
+ } else {
+ nfc_llcp_send_symm(local->dev);
+ }
+
+out:
+ mod_timer(&local->link_timer,
+ jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
+
+static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
+ u8 ssap)
+{
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
+
+ read_lock(&local->connecting_sockets.lock);
+
+ sk_for_each(sk, &local->connecting_sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
+
+ if (llcp_sock->ssap == ssap) {
+ sock_hold(&llcp_sock->sk);
+ goto out;
+ }
+ }
+
+ llcp_sock = NULL;
+
+out:
+ read_unlock(&local->connecting_sockets.lock);
+
+ return llcp_sock;
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
+ u8 *sn, size_t sn_len)
+{
+ struct nfc_llcp_sock *llcp_sock;
+
+ llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
+
+ if (llcp_sock == NULL)
+ return NULL;
+
+ sock_hold(&llcp_sock->sk);
+
+ return llcp_sock;
+}
+
+static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
+{
+ u8 *tlv = &skb->data[2], type, length;
+ size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
+
+ while (offset < tlv_array_len) {
+ type = tlv[0];
+ length = tlv[1];
+
+ pr_debug("type 0x%x length %d\n", type, length);
+
+ if (type == LLCP_TLV_SN) {
+ *sn_len = length;
+ return &tlv[2];
+ }
+
+ offset += length + 2;
+ tlv += length + 2;
+ }
+
+ return NULL;
+}
+
+static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct nfc_llcp_ui_cb *ui_cb;
+ u8 dsap, ssap;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ ui_cb = nfc_llcp_ui_skb_cb(skb);
+ ui_cb->dsap = dsap;
+ ui_cb->ssap = ssap;
+
+ pr_debug("%d %d\n", dsap, ssap);
+
+ /* We're looking for a bound socket, not a client one */
+ llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+ if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+ return;
+
+ /* There is no sequence with UI frames */
+ skb_pull(skb, LLCP_HEADER_SIZE);
+ if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+ /*
+ * UI frames will be freed from the socket layer, so we
+ * need to keep them alive until someone receives them.
+ */
+ skb_get(skb);
+ } else {
+ pr_err("Receive queue is full\n");
+ }
+
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct sock *new_sk, *parent;
+ struct nfc_llcp_sock *sock, *new_sock;
+ u8 dsap, ssap, reason;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ pr_debug("%d %d\n", dsap, ssap);
+
+ if (dsap != LLCP_SAP_SDP) {
+ sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+ if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
+ }
+ } else {
+ u8 *sn;
+ size_t sn_len;
+
+ sn = nfc_llcp_connect_sn(skb, &sn_len);
+ if (sn == NULL) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
+ }
+
+ pr_debug("Service name length %zu\n", sn_len);
+
+ sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
+ if (sock == NULL) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
+ }
+ }
+
+ lock_sock(&sock->sk);
+
+ parent = &sock->sk;
+
+ if (sk_acceptq_is_full(parent)) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ if (sock->ssap == LLCP_SDP_UNBOUND) {
+ u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
+
+ pr_debug("First client, reserving %d\n", ssap);
+
+ if (ssap == LLCP_SAP_MAX) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ sock->ssap = ssap;
+ }
+
+ new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC, 0);
+ if (new_sk == NULL) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ new_sock = nfc_llcp_sock(new_sk);
+ new_sock->dev = local->dev;
+ new_sock->local = nfc_llcp_local_get(local);
+ new_sock->rw = sock->rw;
+ new_sock->miux = sock->miux;
+ new_sock->nfc_protocol = sock->nfc_protocol;
+ new_sock->dsap = ssap;
+ new_sock->target_idx = local->target_idx;
+ new_sock->parent = parent;
+ new_sock->ssap = sock->ssap;
+ if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
+ atomic_t *client_count;
+
+ pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
+
+ client_count =
+ &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
+
+ atomic_inc(client_count);
+ new_sock->reserved_ssap = sock->ssap;
+ }
+
+ nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
+ skb->len - LLCP_HEADER_SIZE);
+
+ pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
+
+ nfc_llcp_sock_link(&local->sockets, new_sk);
+
+ nfc_llcp_accept_enqueue(&sock->sk, new_sk);
+
+ nfc_get_device(local->dev->idx);
+
+ new_sk->sk_state = LLCP_CONNECTED;
+
+ /* Wake the listening processes */
+ parent->sk_data_ready(parent);
+
+ /* Send CC */
+ nfc_llcp_send_cc(new_sock);
+
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+
+ return;
+
+fail:
+ /* Send DM */
+ nfc_llcp_send_dm(local, dsap, ssap, reason);
+}
+
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
+{
+ int nr_frames = 0;
+ struct nfc_llcp_local *local = sock->local;
+
+ pr_debug("Remote ready %d tx queue len %d remote rw %d",
+ sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
+ sock->remote_rw);
+
+ /* Try to queue some I frames for transmission */
+ while (sock->remote_ready &&
+ skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
+ struct sk_buff *pdu;
+
+ pdu = skb_dequeue(&sock->tx_queue);
+ if (pdu == NULL)
+ break;
+
+ /* Update N(S)/N(R) */
+ nfc_llcp_set_nrns(sock, pdu);
+
+ skb_queue_tail(&local->tx_queue, pdu);
+ nr_frames++;
+ }
+
+ return nr_frames;
+}
+
+static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap, ptype, ns, nr;
+
+ ptype = nfc_llcp_ptype(skb);
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+ ns = nfc_llcp_ns(skb);
+ nr = nfc_llcp_nr(skb);
+
+ pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
+
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ if (llcp_sock == NULL) {
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+ lock_sock(sk);
+ if (sk->sk_state == LLCP_CLOSED) {
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+ }
+
+ /* Pass the payload upstream */
+ if (ptype == LLCP_PDU_I) {
+ pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
+
+ if (ns == llcp_sock->recv_n)
+ llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
+ else
+ pr_err("Received out of sequence I PDU\n");
+
+ skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
+ if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+ /*
+ * I frames will be freed from the socket layer, so we
+ * need to keep them alive until someone receives them.
+ */
+ skb_get(skb);
+ } else {
+ pr_err("Receive queue is full\n");
+ }
+ }
+
+ /* Remove skbs from the pending queue */
+ if (llcp_sock->send_ack_n != nr) {
+ struct sk_buff *s, *tmp;
+ u8 n;
+
+ llcp_sock->send_ack_n = nr;
+
+ /* Remove and free all skbs until ns == nr */
+ skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+ n = nfc_llcp_ns(s);
+
+ skb_unlink(s, &llcp_sock->tx_pending_queue);
+ kfree_skb(s);
+
+ if (n == nr)
+ break;
+ }
+
+ /* Re-queue the remaining skbs for transmission */
+ skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+ s, tmp) {
+ skb_unlink(s, &llcp_sock->tx_pending_queue);
+ skb_queue_head(&local->tx_queue, s);
+ }
+ }
+
+ if (ptype == LLCP_PDU_RR)
+ llcp_sock->remote_ready = true;
+ else if (ptype == LLCP_PDU_RNR)
+ llcp_sock->remote_ready = false;
+
+ if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
+ nfc_llcp_send_rr(llcp_sock);
+
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ if ((dsap == 0) && (ssap == 0)) {
+ pr_debug("Connection termination");
+ nfc_dep_link_down(local->dev);
+ return;
+ }
+
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ if (llcp_sock == NULL) {
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+ lock_sock(sk);
+
+ nfc_llcp_socket_purge(llcp_sock);
+
+ if (sk->sk_state == LLCP_CLOSED) {
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+ }
+
+ if (sk->sk_state == LLCP_CONNECTED) {
+ nfc_put_device(local->dev);
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+ }
+
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
+
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+ if (llcp_sock == NULL) {
+ pr_err("Invalid CC\n");
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+
+ /* Unlink from connecting and link to the client array */
+ nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+ nfc_llcp_sock_link(&local->sockets, sk);
+ llcp_sock->dsap = ssap;
+
+ nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
+ skb->len - LLCP_HEADER_SIZE);
+
+ sk->sk_state = LLCP_CONNECTED;
+ sk->sk_state_change(sk);
+
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap, reason;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+ reason = skb->data[2];
+
+ pr_debug("%d %d reason %d\n", ssap, dsap, reason);
+
+ switch (reason) {
+ case LLCP_DM_NOBOUND:
+ case LLCP_DM_REJ:
+ llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+ break;
+
+ default:
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ break;
+ }
+
+ if (llcp_sock == NULL) {
+ pr_debug("Already closed\n");
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+
+ sk->sk_err = ENXIO;
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ u8 dsap, ssap, *tlv, type, length, tid, sap;
+ u16 tlv_len, offset;
+ char *service_name;
+ size_t service_name_len;
+ struct nfc_llcp_sdp_tlv *sdp;
+ HLIST_HEAD(llc_sdres_list);
+ size_t sdres_tlvs_len;
+ HLIST_HEAD(nl_sdres_list);
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ pr_debug("%d %d\n", dsap, ssap);
+
+ if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+ pr_err("Wrong SNL SAP\n");
+ return;
+ }
+
+ tlv = &skb->data[LLCP_HEADER_SIZE];
+ tlv_len = skb->len - LLCP_HEADER_SIZE;
+ offset = 0;
+ sdres_tlvs_len = 0;
+
+ while (offset < tlv_len) {
+ type = tlv[0];
+ length = tlv[1];
+
+ switch (type) {
+ case LLCP_TLV_SDREQ:
+ tid = tlv[2];
+ service_name = (char *) &tlv[3];
+ service_name_len = length - 1;
+
+ pr_debug("Looking for %.16s\n", service_name);
+
+ if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+ !strncmp(service_name, "urn:nfc:sn:sdp",
+ service_name_len)) {
+ sap = 1;
+ goto add_snl;
+ }
+
+ llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+ service_name_len);
+ if (!llcp_sock) {
+ sap = 0;
+ goto add_snl;
+ }
+
+ /*
+ * We found a socket but its ssap has not been reserved
+ * yet. We need to assign it for good and send a reply.
+ * The ssap will be freed when the socket is closed.
+ */
+ if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+ atomic_t *client_count;
+
+ sap = nfc_llcp_reserve_sdp_ssap(local);
+
+ pr_debug("Reserving %d\n", sap);
+
+ if (sap == LLCP_SAP_MAX) {
+ sap = 0;
+ goto add_snl;
+ }
+
+ client_count =
+ &local->local_sdp_cnt[sap -
+ LLCP_WKS_NUM_SAP];
+
+ atomic_inc(client_count);
+
+ llcp_sock->ssap = sap;
+ llcp_sock->reserved_ssap = sap;
+ } else {
+ sap = llcp_sock->ssap;
+ }
+
+ pr_debug("%p %d\n", llcp_sock, sap);
+
+add_snl:
+ sdp = nfc_llcp_build_sdres_tlv(tid, sap);
+ if (sdp == NULL)
+ goto exit;
+
+ sdres_tlvs_len += sdp->tlv_len;
+ hlist_add_head(&sdp->node, &llc_sdres_list);
+ break;
+
+ case LLCP_TLV_SDRES:
+ mutex_lock(&local->sdreq_lock);
+
+ pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
+
+ hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
+ if (sdp->tid != tlv[2])
+ continue;
+
+ sdp->sap = tlv[3];
+
+ pr_debug("Found: uri=%s, sap=%d\n",
+ sdp->uri, sdp->sap);
+
+ hlist_del(&sdp->node);
+
+ hlist_add_head(&sdp->node, &nl_sdres_list);
+
+ break;
+ }
+
+ mutex_unlock(&local->sdreq_lock);
+ break;
+
+ default:
+ pr_err("Invalid SNL tlv value 0x%x\n", type);
+ break;
+ }
+
+ offset += length + 2;
+ tlv += length + 2;
+ }
+
+exit:
+ if (!hlist_empty(&nl_sdres_list))
+ nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+
+ if (!hlist_empty(&llc_sdres_list))
+ nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
+}
+
+static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ u8 ptype;
+ u16 pdu_len;
+ struct sk_buff *new_skb;
+
+ if (skb->len <= LLCP_HEADER_SIZE) {
+ pr_err("Malformed AGF PDU\n");
+ return;
+ }
+
+ skb_pull(skb, LLCP_HEADER_SIZE);
+
+ while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
+ pdu_len = skb->data[0] << 8 | skb->data[1];
+
+ skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
+
+ if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
+ pr_err("Malformed AGF PDU\n");
+ return;
+ }
+
+ ptype = nfc_llcp_ptype(skb);
+
+ if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
+ goto next;
+
+ new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
+ if (new_skb == NULL) {
+ pr_err("Could not allocate PDU\n");
+ return;
+ }
+
+ skb_put_data(new_skb, skb->data, pdu_len);
+
+ nfc_llcp_rx_skb(local, new_skb);
+
+ kfree_skb(new_skb);
+next:
+ skb_pull(skb, pdu_len);
+ }
+}
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ u8 dsap, ssap, ptype;
+
+ ptype = nfc_llcp_ptype(skb);
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+ if (ptype != LLCP_PDU_SYMM)
+ print_hex_dump_debug("LLCP Rx: ", DUMP_PREFIX_OFFSET, 16, 1,
+ skb->data, skb->len, true);
+
+ switch (ptype) {
+ case LLCP_PDU_SYMM:
+ pr_debug("SYMM\n");
+ break;
+
+ case LLCP_PDU_UI:
+ pr_debug("UI\n");
+ nfc_llcp_recv_ui(local, skb);
+ break;
+
+ case LLCP_PDU_CONNECT:
+ pr_debug("CONNECT\n");
+ nfc_llcp_recv_connect(local, skb);
+ break;
+
+ case LLCP_PDU_DISC:
+ pr_debug("DISC\n");
+ nfc_llcp_recv_disc(local, skb);
+ break;
+
+ case LLCP_PDU_CC:
+ pr_debug("CC\n");
+ nfc_llcp_recv_cc(local, skb);
+ break;
+
+ case LLCP_PDU_DM:
+ pr_debug("DM\n");
+ nfc_llcp_recv_dm(local, skb);
+ break;
+
+ case LLCP_PDU_SNL:
+ pr_debug("SNL\n");
+ nfc_llcp_recv_snl(local, skb);
+ break;
+
+ case LLCP_PDU_I:
+ case LLCP_PDU_RR:
+ case LLCP_PDU_RNR:
+ pr_debug("I frame\n");
+ nfc_llcp_recv_hdlc(local, skb);
+ break;
+
+ case LLCP_PDU_AGF:
+ pr_debug("AGF frame\n");
+ nfc_llcp_recv_agf(local, skb);
+ break;
+ }
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ rx_work);
+ struct sk_buff *skb;
+
+ skb = local->rx_pending;
+ if (skb == NULL) {
+ pr_debug("No pending SKB\n");
+ return;
+ }
+
+ __net_timestamp(skb);
+
+ nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_RX);
+
+ nfc_llcp_rx_skb(local, skb);
+
+ schedule_work(&local->tx_work);
+ kfree_skb(local->rx_pending);
+ local->rx_pending = NULL;
+}
+
+static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ local->rx_pending = skb;
+ del_timer(&local->link_timer);
+ schedule_work(&local->rx_work);
+}
+
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
+{
+ struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+ pr_debug("Received an LLCP PDU\n");
+ if (err < 0) {
+ pr_err("err %d\n", err);
+ return;
+ }
+
+ __nfc_llcp_recv(local, skb);
+}
+
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ kfree_skb(skb);
+ return -ENODEV;
+ }
+
+ __nfc_llcp_recv(local, skb);
+
+ return 0;
+}
+
+void nfc_llcp_mac_is_down(struct nfc_dev *dev)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL)
+ return;
+
+ local->remote_miu = LLCP_DEFAULT_MIU;
+ local->remote_lto = LLCP_DEFAULT_LTO;
+
+ /* Close and purge all existing sockets */
+ nfc_llcp_socket_release(local, true, 0);
+}
+
+void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+ u8 comm_mode, u8 rf_mode)
+{
+ struct nfc_llcp_local *local;
+
+ pr_debug("rf mode %d\n", rf_mode);
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL)
+ return;
+
+ local->target_idx = target_idx;
+ local->comm_mode = comm_mode;
+ local->rf_mode = rf_mode;
+
+ if (rf_mode == NFC_RF_INITIATOR) {
+ pr_debug("Queueing Tx work\n");
+
+ schedule_work(&local->tx_work);
+ } else {
+ mod_timer(&local->link_timer,
+ jiffies + msecs_to_jiffies(local->remote_lto));
+ }
+}
+
+int nfc_llcp_register_device(struct nfc_dev *ndev)
+{
+ struct nfc_llcp_local *local;
+
+ local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
+ if (local == NULL)
+ return -ENOMEM;
+
+ local->dev = ndev;
+ INIT_LIST_HEAD(&local->list);
+ kref_init(&local->ref);
+ mutex_init(&local->sdp_lock);
+ timer_setup(&local->link_timer, nfc_llcp_symm_timer, 0);
+
+ skb_queue_head_init(&local->tx_queue);
+ INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
+
+ local->rx_pending = NULL;
+ INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
+
+ INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
+
+ rwlock_init(&local->sockets.lock);
+ rwlock_init(&local->connecting_sockets.lock);
+ rwlock_init(&local->raw_sockets.lock);
+
+ local->lto = 150; /* 1500 ms */
+ local->rw = LLCP_MAX_RW;
+ local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+ local->local_wks = 0x1; /* LLC Link Management */
+
+ nfc_llcp_build_gb(local);
+
+ local->remote_miu = LLCP_DEFAULT_MIU;
+ local->remote_lto = LLCP_DEFAULT_LTO;
+
+ mutex_init(&local->sdreq_lock);
+ INIT_HLIST_HEAD(&local->pending_sdreqs);
+ timer_setup(&local->sdreq_timer, nfc_llcp_sdreq_timer, 0);
+ INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
+
+ list_add(&local->list, &llcp_devices);
+
+ return 0;
+}
+
+void nfc_llcp_unregister_device(struct nfc_dev *dev)
+{
+ struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+ if (local == NULL) {
+ pr_debug("No such device\n");
+ return;
+ }
+
+ local_cleanup(local);
+
+ nfc_llcp_local_put(local);
+}
+
+int __init nfc_llcp_init(void)
+{
+ return nfc_llcp_sock_init();
+}
+
+void nfc_llcp_exit(void)
+{
+ nfc_llcp_sock_exit();
+}
diff --git a/net/nfc/llcp_sock.c b/net/nfc/llcp_sock.c
new file mode 100644
index 0000000..ae29627
--- /dev/null
+++ b/net/nfc/llcp_sock.c
@@ -0,0 +1,1041 @@
+/*
+ * 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 <linux/sched/signal.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
+{
+ DECLARE_WAITQUEUE(wait, current);
+ int err = 0;
+
+ pr_debug("sk %p", sk);
+
+ add_wait_queue(sk_sleep(sk), &wait);
+ set_current_state(TASK_INTERRUPTIBLE);
+
+ while (sk->sk_state != state) {
+ if (!timeo) {
+ err = -EINPROGRESS;
+ break;
+ }
+
+ if (signal_pending(current)) {
+ err = sock_intr_errno(timeo);
+ break;
+ }
+
+ release_sock(sk);
+ timeo = schedule_timeout(timeo);
+ lock_sock(sk);
+ set_current_state(TASK_INTERRUPTIBLE);
+
+ err = sock_error(sk);
+ if (err)
+ break;
+ }
+
+ __set_current_state(TASK_RUNNING);
+ remove_wait_queue(sk_sleep(sk), &wait);
+ return err;
+}
+
+static struct proto llcp_sock_proto = {
+ .name = "NFC_LLCP",
+ .owner = THIS_MODULE,
+ .obj_size = sizeof(struct nfc_llcp_sock),
+};
+
+static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ struct nfc_llcp_local *local;
+ struct nfc_dev *dev;
+ struct sockaddr_nfc_llcp llcp_addr;
+ int len, ret = 0;
+
+ if (!addr || alen < offsetofend(struct sockaddr, sa_family) ||
+ addr->sa_family != AF_NFC)
+ return -EINVAL;
+
+ pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+ memset(&llcp_addr, 0, sizeof(llcp_addr));
+ len = min_t(unsigned int, sizeof(llcp_addr), alen);
+ memcpy(&llcp_addr, addr, len);
+
+ /* This is going to be a listening socket, dsap must be 0 */
+ if (llcp_addr.dsap != 0)
+ return -EINVAL;
+
+ lock_sock(sk);
+
+ if (sk->sk_state != LLCP_CLOSED) {
+ ret = -EBADFD;
+ goto error;
+ }
+
+ dev = nfc_get_device(llcp_addr.dev_idx);
+ if (dev == NULL) {
+ ret = -ENODEV;
+ goto error;
+ }
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ ret = -ENODEV;
+ goto put_dev;
+ }
+
+ llcp_sock->dev = dev;
+ llcp_sock->local = nfc_llcp_local_get(local);
+ llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+ llcp_sock->service_name_len = min_t(unsigned int,
+ llcp_addr.service_name_len,
+ NFC_LLCP_MAX_SERVICE_NAME);
+ llcp_sock->service_name = kmemdup(llcp_addr.service_name,
+ llcp_sock->service_name_len,
+ GFP_KERNEL);
+
+ llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
+ if (llcp_sock->ssap == LLCP_SAP_MAX) {
+ ret = -EADDRINUSE;
+ goto put_dev;
+ }
+
+ llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+ nfc_llcp_sock_link(&local->sockets, sk);
+
+ pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
+
+ sk->sk_state = LLCP_BOUND;
+
+put_dev:
+ nfc_put_device(dev);
+
+error:
+ release_sock(sk);
+ return ret;
+}
+
+static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
+ int alen)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ struct nfc_llcp_local *local;
+ struct nfc_dev *dev;
+ struct sockaddr_nfc_llcp llcp_addr;
+ int len, ret = 0;
+
+ if (!addr || alen < offsetofend(struct sockaddr, sa_family) ||
+ addr->sa_family != AF_NFC)
+ return -EINVAL;
+
+ pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+ memset(&llcp_addr, 0, sizeof(llcp_addr));
+ len = min_t(unsigned int, sizeof(llcp_addr), alen);
+ memcpy(&llcp_addr, addr, len);
+
+ lock_sock(sk);
+
+ if (sk->sk_state != LLCP_CLOSED) {
+ ret = -EBADFD;
+ goto error;
+ }
+
+ dev = nfc_get_device(llcp_addr.dev_idx);
+ if (dev == NULL) {
+ ret = -ENODEV;
+ goto error;
+ }
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ ret = -ENODEV;
+ goto put_dev;
+ }
+
+ llcp_sock->dev = dev;
+ llcp_sock->local = nfc_llcp_local_get(local);
+ llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+
+ nfc_llcp_sock_link(&local->raw_sockets, sk);
+
+ sk->sk_state = LLCP_BOUND;
+
+put_dev:
+ nfc_put_device(dev);
+
+error:
+ release_sock(sk);
+ return ret;
+}
+
+static int llcp_sock_listen(struct socket *sock, int backlog)
+{
+ struct sock *sk = sock->sk;
+ int ret = 0;
+
+ pr_debug("sk %p backlog %d\n", sk, backlog);
+
+ lock_sock(sk);
+
+ if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
+ sk->sk_state != LLCP_BOUND) {
+ ret = -EBADFD;
+ goto error;
+ }
+
+ sk->sk_max_ack_backlog = backlog;
+ sk->sk_ack_backlog = 0;
+
+ pr_debug("Socket listening\n");
+ sk->sk_state = LLCP_LISTEN;
+
+error:
+ release_sock(sk);
+
+ return ret;
+}
+
+static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
+ char __user *optval, unsigned int optlen)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ u32 opt;
+ int err = 0;
+
+ pr_debug("%p optname %d\n", sk, optname);
+
+ if (level != SOL_NFC)
+ return -ENOPROTOOPT;
+
+ lock_sock(sk);
+
+ switch (optname) {
+ case NFC_LLCP_RW:
+ if (sk->sk_state == LLCP_CONNECTED ||
+ sk->sk_state == LLCP_BOUND ||
+ sk->sk_state == LLCP_LISTEN) {
+ err = -EINVAL;
+ break;
+ }
+
+ if (get_user(opt, (u32 __user *) optval)) {
+ err = -EFAULT;
+ break;
+ }
+
+ if (opt > LLCP_MAX_RW) {
+ err = -EINVAL;
+ break;
+ }
+
+ llcp_sock->rw = (u8) opt;
+
+ break;
+
+ case NFC_LLCP_MIUX:
+ if (sk->sk_state == LLCP_CONNECTED ||
+ sk->sk_state == LLCP_BOUND ||
+ sk->sk_state == LLCP_LISTEN) {
+ err = -EINVAL;
+ break;
+ }
+
+ if (get_user(opt, (u32 __user *) optval)) {
+ err = -EFAULT;
+ break;
+ }
+
+ if (opt > LLCP_MAX_MIUX) {
+ err = -EINVAL;
+ break;
+ }
+
+ llcp_sock->miux = cpu_to_be16((u16) opt);
+
+ break;
+
+ default:
+ err = -ENOPROTOOPT;
+ break;
+ }
+
+ release_sock(sk);
+
+ pr_debug("%p rw %d miux %d\n", llcp_sock,
+ llcp_sock->rw, llcp_sock->miux);
+
+ return err;
+}
+
+static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
+ char __user *optval, int __user *optlen)
+{
+ struct nfc_llcp_local *local;
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ int len, err = 0;
+ u16 miux, remote_miu;
+ u8 rw;
+
+ pr_debug("%p optname %d\n", sk, optname);
+
+ if (level != SOL_NFC)
+ return -ENOPROTOOPT;
+
+ if (get_user(len, optlen))
+ return -EFAULT;
+
+ local = llcp_sock->local;
+ if (!local)
+ return -ENODEV;
+
+ len = min_t(u32, len, sizeof(u32));
+
+ lock_sock(sk);
+
+ switch (optname) {
+ case NFC_LLCP_RW:
+ rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
+ if (put_user(rw, (u32 __user *) optval))
+ err = -EFAULT;
+
+ break;
+
+ case NFC_LLCP_MIUX:
+ miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
+ be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
+
+ if (put_user(miux, (u32 __user *) optval))
+ err = -EFAULT;
+
+ break;
+
+ case NFC_LLCP_REMOTE_MIU:
+ remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
+ local->remote_miu : llcp_sock->remote_miu;
+
+ if (put_user(remote_miu, (u32 __user *) optval))
+ err = -EFAULT;
+
+ break;
+
+ case NFC_LLCP_REMOTE_LTO:
+ if (put_user(local->remote_lto / 10, (u32 __user *) optval))
+ err = -EFAULT;
+
+ break;
+
+ case NFC_LLCP_REMOTE_RW:
+ if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
+ err = -EFAULT;
+
+ break;
+
+ default:
+ err = -ENOPROTOOPT;
+ break;
+ }
+
+ release_sock(sk);
+
+ if (put_user(len, optlen))
+ return -EFAULT;
+
+ return err;
+}
+
+void nfc_llcp_accept_unlink(struct sock *sk)
+{
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+ pr_debug("state %d\n", sk->sk_state);
+
+ list_del_init(&llcp_sock->accept_queue);
+ sk_acceptq_removed(llcp_sock->parent);
+ llcp_sock->parent = NULL;
+
+ sock_put(sk);
+}
+
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
+{
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
+
+ /* Lock will be free from unlink */
+ sock_hold(sk);
+
+ list_add_tail(&llcp_sock->accept_queue,
+ &llcp_sock_parent->accept_queue);
+ llcp_sock->parent = parent;
+ sk_acceptq_added(parent);
+}
+
+struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
+ struct socket *newsock)
+{
+ struct nfc_llcp_sock *lsk, *n, *llcp_parent;
+ struct sock *sk;
+
+ llcp_parent = nfc_llcp_sock(parent);
+
+ list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
+ accept_queue) {
+ sk = &lsk->sk;
+ lock_sock(sk);
+
+ if (sk->sk_state == LLCP_CLOSED) {
+ release_sock(sk);
+ nfc_llcp_accept_unlink(sk);
+ continue;
+ }
+
+ if (sk->sk_state == LLCP_CONNECTED || !newsock) {
+ list_del_init(&lsk->accept_queue);
+ sock_put(sk);
+
+ if (newsock)
+ sock_graft(sk, newsock);
+
+ release_sock(sk);
+
+ pr_debug("Returning sk state %d\n", sk->sk_state);
+
+ sk_acceptq_removed(parent);
+
+ return sk;
+ }
+
+ release_sock(sk);
+ }
+
+ return NULL;
+}
+
+static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
+ int flags, bool kern)
+{
+ DECLARE_WAITQUEUE(wait, current);
+ struct sock *sk = sock->sk, *new_sk;
+ long timeo;
+ int ret = 0;
+
+ pr_debug("parent %p\n", sk);
+
+ lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
+
+ if (sk->sk_state != LLCP_LISTEN) {
+ ret = -EBADFD;
+ goto error;
+ }
+
+ timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
+
+ /* Wait for an incoming connection. */
+ add_wait_queue_exclusive(sk_sleep(sk), &wait);
+ while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
+ set_current_state(TASK_INTERRUPTIBLE);
+
+ if (!timeo) {
+ ret = -EAGAIN;
+ break;
+ }
+
+ if (signal_pending(current)) {
+ ret = sock_intr_errno(timeo);
+ break;
+ }
+
+ release_sock(sk);
+ timeo = schedule_timeout(timeo);
+ lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
+ }
+ __set_current_state(TASK_RUNNING);
+ remove_wait_queue(sk_sleep(sk), &wait);
+
+ if (ret)
+ goto error;
+
+ newsock->state = SS_CONNECTED;
+
+ pr_debug("new socket %p\n", new_sk);
+
+error:
+ release_sock(sk);
+
+ return ret;
+}
+
+static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr,
+ int peer)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr);
+
+ if (llcp_sock == NULL || llcp_sock->dev == NULL)
+ return -EBADFD;
+
+ pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx,
+ llcp_sock->dsap, llcp_sock->ssap);
+
+ memset(llcp_addr, 0, sizeof(*llcp_addr));
+
+ lock_sock(sk);
+ if (!llcp_sock->dev) {
+ release_sock(sk);
+ return -EBADFD;
+ }
+ llcp_addr->sa_family = AF_NFC;
+ llcp_addr->dev_idx = llcp_sock->dev->idx;
+ llcp_addr->target_idx = llcp_sock->target_idx;
+ llcp_addr->nfc_protocol = llcp_sock->nfc_protocol;
+ llcp_addr->dsap = llcp_sock->dsap;
+ llcp_addr->ssap = llcp_sock->ssap;
+ llcp_addr->service_name_len = llcp_sock->service_name_len;
+ memcpy(llcp_addr->service_name, llcp_sock->service_name,
+ llcp_addr->service_name_len);
+ release_sock(sk);
+
+ return sizeof(struct sockaddr_nfc_llcp);
+}
+
+static inline __poll_t llcp_accept_poll(struct sock *parent)
+{
+ struct nfc_llcp_sock *llcp_sock, *parent_sock;
+ struct sock *sk;
+
+ parent_sock = nfc_llcp_sock(parent);
+
+ list_for_each_entry(llcp_sock, &parent_sock->accept_queue,
+ accept_queue) {
+ sk = &llcp_sock->sk;
+
+ if (sk->sk_state == LLCP_CONNECTED)
+ return EPOLLIN | EPOLLRDNORM;
+ }
+
+ return 0;
+}
+
+static __poll_t llcp_sock_poll(struct file *file, struct socket *sock,
+ poll_table *wait)
+{
+ struct sock *sk = sock->sk;
+ __poll_t mask = 0;
+
+ pr_debug("%p\n", sk);
+
+ sock_poll_wait(file, sock, wait);
+
+ if (sk->sk_state == LLCP_LISTEN)
+ return llcp_accept_poll(sk);
+
+ if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
+ mask |= EPOLLERR |
+ (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? EPOLLPRI : 0);
+
+ if (!skb_queue_empty(&sk->sk_receive_queue))
+ mask |= EPOLLIN | EPOLLRDNORM;
+
+ if (sk->sk_state == LLCP_CLOSED)
+ mask |= EPOLLHUP;
+
+ if (sk->sk_shutdown & RCV_SHUTDOWN)
+ mask |= EPOLLRDHUP | EPOLLIN | EPOLLRDNORM;
+
+ if (sk->sk_shutdown == SHUTDOWN_MASK)
+ mask |= EPOLLHUP;
+
+ if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED)
+ mask |= EPOLLOUT | EPOLLWRNORM | EPOLLWRBAND;
+ else
+ sk_set_bit(SOCKWQ_ASYNC_NOSPACE, sk);
+
+ pr_debug("mask 0x%x\n", mask);
+
+ return mask;
+}
+
+static int llcp_sock_release(struct socket *sock)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_local *local;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ int err = 0;
+
+ if (!sk)
+ return 0;
+
+ pr_debug("%p\n", sk);
+
+ local = llcp_sock->local;
+ if (local == NULL) {
+ err = -ENODEV;
+ goto out;
+ }
+
+ lock_sock(sk);
+
+ /* Send a DISC */
+ if (sk->sk_state == LLCP_CONNECTED)
+ nfc_llcp_send_disconnect(llcp_sock);
+
+ if (sk->sk_state == LLCP_LISTEN) {
+ struct nfc_llcp_sock *lsk, *n;
+ struct sock *accept_sk;
+
+ list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
+ accept_queue) {
+ accept_sk = &lsk->sk;
+ lock_sock(accept_sk);
+
+ nfc_llcp_send_disconnect(lsk);
+ nfc_llcp_accept_unlink(accept_sk);
+
+ release_sock(accept_sk);
+ }
+ }
+
+ if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
+ nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
+
+ release_sock(sk);
+
+ /* Keep this sock alive and therefore do not remove it from the sockets
+ * list until the DISC PDU has been actually sent. Otherwise we would
+ * reply with DM PDUs before sending the DISC one.
+ */
+ if (sk->sk_state == LLCP_DISCONNECTING)
+ return err;
+
+ if (sock->type == SOCK_RAW)
+ nfc_llcp_sock_unlink(&local->raw_sockets, sk);
+ else
+ nfc_llcp_sock_unlink(&local->sockets, sk);
+
+out:
+ sock_orphan(sk);
+ sock_put(sk);
+
+ return err;
+}
+
+static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
+ int len, int flags)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
+ struct nfc_dev *dev;
+ struct nfc_llcp_local *local;
+ int ret = 0;
+
+ pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
+
+ if (!addr || len < sizeof(*addr) || addr->sa_family != AF_NFC)
+ return -EINVAL;
+
+ if (addr->service_name_len == 0 && addr->dsap == 0)
+ return -EINVAL;
+
+ pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
+ addr->target_idx, addr->nfc_protocol);
+
+ lock_sock(sk);
+
+ if (sk->sk_state == LLCP_CONNECTED) {
+ ret = -EISCONN;
+ goto error;
+ }
+
+ dev = nfc_get_device(addr->dev_idx);
+ if (dev == NULL) {
+ ret = -ENODEV;
+ goto error;
+ }
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ ret = -ENODEV;
+ goto put_dev;
+ }
+
+ device_lock(&dev->dev);
+ if (dev->dep_link_up == false) {
+ ret = -ENOLINK;
+ device_unlock(&dev->dev);
+ goto put_dev;
+ }
+ device_unlock(&dev->dev);
+
+ if (local->rf_mode == NFC_RF_INITIATOR &&
+ addr->target_idx != local->target_idx) {
+ ret = -ENOLINK;
+ goto put_dev;
+ }
+
+ llcp_sock->dev = dev;
+ llcp_sock->local = nfc_llcp_local_get(local);
+ llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
+ if (llcp_sock->ssap == LLCP_SAP_MAX) {
+ ret = -ENOMEM;
+ goto put_dev;
+ }
+
+ llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+ if (addr->service_name_len == 0)
+ llcp_sock->dsap = addr->dsap;
+ else
+ llcp_sock->dsap = LLCP_SAP_SDP;
+ llcp_sock->nfc_protocol = addr->nfc_protocol;
+ llcp_sock->service_name_len = min_t(unsigned int,
+ addr->service_name_len,
+ NFC_LLCP_MAX_SERVICE_NAME);
+ llcp_sock->service_name = kmemdup(addr->service_name,
+ llcp_sock->service_name_len,
+ GFP_KERNEL);
+
+ nfc_llcp_sock_link(&local->connecting_sockets, sk);
+
+ ret = nfc_llcp_send_connect(llcp_sock);
+ if (ret)
+ goto sock_unlink;
+
+ sk->sk_state = LLCP_CONNECTING;
+
+ ret = sock_wait_state(sk, LLCP_CONNECTED,
+ sock_sndtimeo(sk, flags & O_NONBLOCK));
+ if (ret && ret != -EINPROGRESS)
+ goto sock_unlink;
+
+ release_sock(sk);
+
+ return ret;
+
+sock_unlink:
+ nfc_llcp_put_ssap(local, llcp_sock->ssap);
+
+ nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+
+put_dev:
+ nfc_put_device(dev);
+
+error:
+ release_sock(sk);
+ return ret;
+}
+
+static int llcp_sock_sendmsg(struct socket *sock, struct msghdr *msg,
+ size_t len)
+{
+ struct sock *sk = sock->sk;
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+ int ret;
+
+ pr_debug("sock %p sk %p", sock, sk);
+
+ ret = sock_error(sk);
+ if (ret)
+ return ret;
+
+ if (msg->msg_flags & MSG_OOB)
+ return -EOPNOTSUPP;
+
+ lock_sock(sk);
+
+ if (sk->sk_type == SOCK_DGRAM) {
+ DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, addr,
+ msg->msg_name);
+
+ if (msg->msg_namelen < sizeof(*addr)) {
+ release_sock(sk);
+ return -EINVAL;
+ }
+
+ release_sock(sk);
+
+ return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
+ msg, len);
+ }
+
+ if (sk->sk_state != LLCP_CONNECTED) {
+ release_sock(sk);
+ return -ENOTCONN;
+ }
+
+ release_sock(sk);
+
+ return nfc_llcp_send_i_frame(llcp_sock, msg, len);
+}
+
+static int llcp_sock_recvmsg(struct socket *sock, struct msghdr *msg,
+ size_t len, int flags)
+{
+ int noblock = flags & MSG_DONTWAIT;
+ struct sock *sk = sock->sk;
+ unsigned int copied, rlen;
+ struct sk_buff *skb, *cskb;
+ int err = 0;
+
+ pr_debug("%p %zu\n", sk, len);
+
+ lock_sock(sk);
+
+ if (sk->sk_state == LLCP_CLOSED &&
+ skb_queue_empty(&sk->sk_receive_queue)) {
+ release_sock(sk);
+ return 0;
+ }
+
+ release_sock(sk);
+
+ if (flags & (MSG_OOB))
+ return -EOPNOTSUPP;
+
+ skb = skb_recv_datagram(sk, flags, noblock, &err);
+ if (!skb) {
+ pr_err("Recv datagram failed state %d %d %d",
+ sk->sk_state, err, sock_error(sk));
+
+ if (sk->sk_shutdown & RCV_SHUTDOWN)
+ return 0;
+
+ return err;
+ }
+
+ rlen = skb->len; /* real length of skb */
+ copied = min_t(unsigned int, rlen, len);
+
+ cskb = skb;
+ if (skb_copy_datagram_msg(cskb, 0, msg, copied)) {
+ if (!(flags & MSG_PEEK))
+ skb_queue_head(&sk->sk_receive_queue, skb);
+ return -EFAULT;
+ }
+
+ sock_recv_timestamp(msg, sk, skb);
+
+ if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
+ struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
+ DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, sockaddr,
+ msg->msg_name);
+
+ msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
+
+ pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
+
+ memset(sockaddr, 0, sizeof(*sockaddr));
+ sockaddr->sa_family = AF_NFC;
+ sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
+ sockaddr->dsap = ui_cb->dsap;
+ sockaddr->ssap = ui_cb->ssap;
+ }
+
+ /* Mark read part of skb as used */
+ if (!(flags & MSG_PEEK)) {
+
+ /* SOCK_STREAM: re-queue skb if it contains unreceived data */
+ if (sk->sk_type == SOCK_STREAM ||
+ sk->sk_type == SOCK_DGRAM ||
+ sk->sk_type == SOCK_RAW) {
+ skb_pull(skb, copied);
+ if (skb->len) {
+ skb_queue_head(&sk->sk_receive_queue, skb);
+ goto done;
+ }
+ }
+
+ kfree_skb(skb);
+ }
+
+ /* XXX Queue backlogged skbs */
+
+done:
+ /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
+ if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
+ copied = rlen;
+
+ return copied;
+}
+
+static const struct proto_ops llcp_sock_ops = {
+ .family = PF_NFC,
+ .owner = THIS_MODULE,
+ .bind = llcp_sock_bind,
+ .connect = llcp_sock_connect,
+ .release = llcp_sock_release,
+ .socketpair = sock_no_socketpair,
+ .accept = llcp_sock_accept,
+ .getname = llcp_sock_getname,
+ .poll = llcp_sock_poll,
+ .ioctl = sock_no_ioctl,
+ .listen = llcp_sock_listen,
+ .shutdown = sock_no_shutdown,
+ .setsockopt = nfc_llcp_setsockopt,
+ .getsockopt = nfc_llcp_getsockopt,
+ .sendmsg = llcp_sock_sendmsg,
+ .recvmsg = llcp_sock_recvmsg,
+ .mmap = sock_no_mmap,
+};
+
+static const struct proto_ops llcp_rawsock_ops = {
+ .family = PF_NFC,
+ .owner = THIS_MODULE,
+ .bind = llcp_raw_sock_bind,
+ .connect = sock_no_connect,
+ .release = llcp_sock_release,
+ .socketpair = sock_no_socketpair,
+ .accept = sock_no_accept,
+ .getname = llcp_sock_getname,
+ .poll = llcp_sock_poll,
+ .ioctl = sock_no_ioctl,
+ .listen = sock_no_listen,
+ .shutdown = sock_no_shutdown,
+ .setsockopt = sock_no_setsockopt,
+ .getsockopt = sock_no_getsockopt,
+ .sendmsg = sock_no_sendmsg,
+ .recvmsg = llcp_sock_recvmsg,
+ .mmap = sock_no_mmap,
+};
+
+static void llcp_sock_destruct(struct sock *sk)
+{
+ struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+ pr_debug("%p\n", sk);
+
+ if (sk->sk_state == LLCP_CONNECTED)
+ nfc_put_device(llcp_sock->dev);
+
+ skb_queue_purge(&sk->sk_receive_queue);
+
+ nfc_llcp_sock_free(llcp_sock);
+
+ if (!sock_flag(sk, SOCK_DEAD)) {
+ pr_err("Freeing alive NFC LLCP socket %p\n", sk);
+ return;
+ }
+}
+
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp, int kern)
+{
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
+
+ sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto, kern);
+ if (!sk)
+ return NULL;
+
+ llcp_sock = nfc_llcp_sock(sk);
+
+ sock_init_data(sock, sk);
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_protocol = NFC_SOCKPROTO_LLCP;
+ sk->sk_type = type;
+ sk->sk_destruct = llcp_sock_destruct;
+
+ llcp_sock->ssap = 0;
+ llcp_sock->dsap = LLCP_SAP_SDP;
+ llcp_sock->rw = LLCP_MAX_RW + 1;
+ llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
+ llcp_sock->send_n = llcp_sock->send_ack_n = 0;
+ llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
+ llcp_sock->remote_ready = 1;
+ llcp_sock->reserved_ssap = LLCP_SAP_MAX;
+ nfc_llcp_socket_remote_param_init(llcp_sock);
+ skb_queue_head_init(&llcp_sock->tx_queue);
+ skb_queue_head_init(&llcp_sock->tx_pending_queue);
+ INIT_LIST_HEAD(&llcp_sock->accept_queue);
+
+ if (sock != NULL)
+ sock->state = SS_UNCONNECTED;
+
+ return sk;
+}
+
+void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
+{
+ kfree(sock->service_name);
+
+ skb_queue_purge(&sock->tx_queue);
+ skb_queue_purge(&sock->tx_pending_queue);
+
+ list_del_init(&sock->accept_queue);
+
+ sock->parent = NULL;
+
+ nfc_llcp_local_put(sock->local);
+}
+
+static int llcp_sock_create(struct net *net, struct socket *sock,
+ const struct nfc_protocol *nfc_proto, int kern)
+{
+ struct sock *sk;
+
+ pr_debug("%p\n", sock);
+
+ if (sock->type != SOCK_STREAM &&
+ sock->type != SOCK_DGRAM &&
+ sock->type != SOCK_RAW)
+ return -ESOCKTNOSUPPORT;
+
+ if (sock->type == SOCK_RAW)
+ sock->ops = &llcp_rawsock_ops;
+ else
+ sock->ops = &llcp_sock_ops;
+
+ sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC, kern);
+ if (sk == NULL)
+ return -ENOMEM;
+
+ return 0;
+}
+
+static const struct nfc_protocol llcp_nfc_proto = {
+ .id = NFC_SOCKPROTO_LLCP,
+ .proto = &llcp_sock_proto,
+ .owner = THIS_MODULE,
+ .create = llcp_sock_create
+};
+
+int __init nfc_llcp_sock_init(void)
+{
+ return nfc_proto_register(&llcp_nfc_proto);
+}
+
+void nfc_llcp_sock_exit(void)
+{
+ nfc_proto_unregister(&llcp_nfc_proto);
+}
diff --git a/net/nfc/nci/Kconfig b/net/nfc/nci/Kconfig
new file mode 100644
index 0000000..85d4819
--- /dev/null
+++ b/net/nfc/nci/Kconfig
@@ -0,0 +1,28 @@
+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
+ tristate "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.
+
+config NFC_NCI_UART
+ depends on NFC_NCI && TTY
+ tristate "NCI over UART protocol support"
+ default n
+ help
+ Say yes if you use an NCI driver that requires UART link layer.
diff --git a/net/nfc/nci/Makefile b/net/nfc/nci/Makefile
new file mode 100644
index 0000000..c3362c4
--- /dev/null
+++ b/net/nfc/nci/Makefile
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# 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 hci.o
+
+nci_spi-y += spi.o
+obj-$(CONFIG_NFC_NCI_SPI) += nci_spi.o
+
+nci_uart-y += uart.o
+obj-$(CONFIG_NFC_NCI_UART) += nci_uart.o
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
new file mode 100644
index 0000000..0749601
--- /dev/null
+++ b/net/nfc/nci/core.c
@@ -0,0 +1,1545 @@
+/*
+ * The NFC Controller Interface is the communication protocol between an
+ * NFC Controller (NFCC) and a Device Host (DH).
+ *
+ * Copyright (C) 2011 Texas Instruments, Inc.
+ * Copyright (C) 2014 Marvell International Ltd.
+ *
+ * Written by Ilan Elias <ilane@ti.com>
+ *
+ * Acknowledgements:
+ * This file is based on hci_core.c, which was written
+ * by Maxim Krasnyansky.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+#include <linux/completion.h>
+#include <linux/export.h>
+#include <linux/sched.h>
+#include <linux/bitops.h>
+#include <linux/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+struct core_conn_create_data {
+ int length;
+ struct nci_core_conn_create_cmd *cmd;
+};
+
+static void nci_cmd_work(struct work_struct *work);
+static void nci_rx_work(struct work_struct *work);
+static void nci_tx_work(struct work_struct *work);
+
+struct nci_conn_info *nci_get_conn_info_by_conn_id(struct nci_dev *ndev,
+ int conn_id)
+{
+ struct nci_conn_info *conn_info;
+
+ list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
+ if (conn_info->conn_id == conn_id)
+ return conn_info;
+ }
+
+ return NULL;
+}
+
+int nci_get_conn_info_by_dest_type_params(struct nci_dev *ndev, u8 dest_type,
+ struct dest_spec_params *params)
+{
+ struct nci_conn_info *conn_info;
+
+ list_for_each_entry(conn_info, &ndev->conn_info_list, list) {
+ if (conn_info->dest_type == dest_type) {
+ if (!params)
+ return conn_info->conn_id;
+
+ if (params->id == conn_info->dest_params->id &&
+ params->protocol == conn_info->dest_params->protocol)
+ return conn_info->conn_id;
+ }
+ }
+
+ return -EINVAL;
+}
+EXPORT_SYMBOL(nci_get_conn_info_by_dest_type_params);
+
+/* ---- NCI requests ---- */
+
+void nci_req_complete(struct nci_dev *ndev, int result)
+{
+ if (ndev->req_status == NCI_REQ_PEND) {
+ ndev->req_result = result;
+ ndev->req_status = NCI_REQ_DONE;
+ complete(&ndev->req_completion);
+ }
+}
+EXPORT_SYMBOL(nci_req_complete);
+
+static void nci_req_cancel(struct nci_dev *ndev, int err)
+{
+ if (ndev->req_status == NCI_REQ_PEND) {
+ ndev->req_result = err;
+ ndev->req_status = NCI_REQ_CANCELED;
+ complete(&ndev->req_completion);
+ }
+}
+
+/* Execute request and wait for completion. */
+static int __nci_request(struct nci_dev *ndev,
+ void (*req)(struct nci_dev *ndev, unsigned long opt),
+ unsigned long opt, __u32 timeout)
+{
+ int rc = 0;
+ long completion_rc;
+
+ ndev->req_status = NCI_REQ_PEND;
+
+ reinit_completion(&ndev->req_completion);
+ req(ndev, opt);
+ completion_rc =
+ wait_for_completion_interruptible_timeout(&ndev->req_completion,
+ timeout);
+
+ pr_debug("wait_for_completion return %ld\n", completion_rc);
+
+ if (completion_rc > 0) {
+ switch (ndev->req_status) {
+ case NCI_REQ_DONE:
+ rc = nci_to_errno(ndev->req_result);
+ break;
+
+ case NCI_REQ_CANCELED:
+ rc = -ndev->req_result;
+ break;
+
+ default:
+ rc = -ETIMEDOUT;
+ break;
+ }
+ } else {
+ pr_err("wait_for_completion_interruptible_timeout failed %ld\n",
+ completion_rc);
+
+ rc = ((completion_rc == 0) ? (-ETIMEDOUT) : (completion_rc));
+ }
+
+ ndev->req_status = ndev->req_result = 0;
+
+ return rc;
+}
+
+inline int nci_request(struct nci_dev *ndev,
+ void (*req)(struct nci_dev *ndev,
+ unsigned long opt),
+ unsigned long opt, __u32 timeout)
+{
+ int rc;
+
+ if (!test_bit(NCI_UP, &ndev->flags))
+ return -ENETDOWN;
+
+ /* Serialize all requests */
+ mutex_lock(&ndev->req_lock);
+ rc = __nci_request(ndev, req, opt, timeout);
+ mutex_unlock(&ndev->req_lock);
+
+ return rc;
+}
+
+static void nci_reset_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_core_reset_cmd cmd;
+
+ cmd.reset_type = NCI_RESET_TYPE_RESET_CONFIG;
+ nci_send_cmd(ndev, NCI_OP_CORE_RESET_CMD, 1, &cmd);
+}
+
+static void nci_init_req(struct nci_dev *ndev, unsigned long opt)
+{
+ nci_send_cmd(ndev, NCI_OP_CORE_INIT_CMD, 0, NULL);
+}
+
+static void nci_init_complete_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_rf_disc_map_cmd cmd;
+ struct disc_map_config *cfg = cmd.mapping_configs;
+ __u8 *num = &cmd.num_mapping_configs;
+ int i;
+
+ /* set rf mapping configurations */
+ *num = 0;
+
+ /* by default mapping is set to NCI_RF_INTERFACE_FRAME */
+ for (i = 0; i < ndev->num_supported_rf_interfaces; i++) {
+ if (ndev->supported_rf_interfaces[i] ==
+ NCI_RF_INTERFACE_ISO_DEP) {
+ cfg[*num].rf_protocol = NCI_RF_PROTOCOL_ISO_DEP;
+ cfg[*num].mode = NCI_DISC_MAP_MODE_POLL |
+ NCI_DISC_MAP_MODE_LISTEN;
+ cfg[*num].rf_interface = NCI_RF_INTERFACE_ISO_DEP;
+ (*num)++;
+ } else if (ndev->supported_rf_interfaces[i] ==
+ NCI_RF_INTERFACE_NFC_DEP) {
+ cfg[*num].rf_protocol = NCI_RF_PROTOCOL_NFC_DEP;
+ cfg[*num].mode = NCI_DISC_MAP_MODE_POLL |
+ NCI_DISC_MAP_MODE_LISTEN;
+ cfg[*num].rf_interface = NCI_RF_INTERFACE_NFC_DEP;
+ (*num)++;
+ }
+
+ if (*num == NCI_MAX_NUM_MAPPING_CONFIGS)
+ break;
+ }
+
+ nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_MAP_CMD,
+ (1 + ((*num) * sizeof(struct disc_map_config))), &cmd);
+}
+
+struct nci_set_config_param {
+ __u8 id;
+ size_t len;
+ __u8 *val;
+};
+
+static void nci_set_config_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_set_config_param *param = (struct nci_set_config_param *)opt;
+ struct nci_core_set_config_cmd cmd;
+
+ BUG_ON(param->len > NCI_MAX_PARAM_LEN);
+
+ cmd.num_params = 1;
+ cmd.param.id = param->id;
+ cmd.param.len = param->len;
+ memcpy(cmd.param.val, param->val, param->len);
+
+ nci_send_cmd(ndev, NCI_OP_CORE_SET_CONFIG_CMD, (3 + param->len), &cmd);
+}
+
+struct nci_rf_discover_param {
+ __u32 im_protocols;
+ __u32 tm_protocols;
+};
+
+static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_rf_discover_param *param =
+ (struct nci_rf_discover_param *)opt;
+ struct nci_rf_disc_cmd cmd;
+
+ cmd.num_disc_configs = 0;
+
+ if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+ (param->im_protocols & NFC_PROTO_JEWEL_MASK ||
+ param->im_protocols & NFC_PROTO_MIFARE_MASK ||
+ param->im_protocols & NFC_PROTO_ISO14443_MASK ||
+ param->im_protocols & NFC_PROTO_NFC_DEP_MASK)) {
+ cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+ NCI_NFC_A_PASSIVE_POLL_MODE;
+ cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+ cmd.num_disc_configs++;
+ }
+
+ if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+ (param->im_protocols & NFC_PROTO_ISO14443_B_MASK)) {
+ cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+ NCI_NFC_B_PASSIVE_POLL_MODE;
+ cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+ cmd.num_disc_configs++;
+ }
+
+ if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+ (param->im_protocols & NFC_PROTO_FELICA_MASK ||
+ param->im_protocols & NFC_PROTO_NFC_DEP_MASK)) {
+ cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+ NCI_NFC_F_PASSIVE_POLL_MODE;
+ cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+ cmd.num_disc_configs++;
+ }
+
+ if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
+ (param->im_protocols & NFC_PROTO_ISO15693_MASK)) {
+ cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+ NCI_NFC_V_PASSIVE_POLL_MODE;
+ cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+ cmd.num_disc_configs++;
+ }
+
+ if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS - 1) &&
+ (param->tm_protocols & NFC_PROTO_NFC_DEP_MASK)) {
+ cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+ NCI_NFC_A_PASSIVE_LISTEN_MODE;
+ cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+ cmd.num_disc_configs++;
+ cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
+ NCI_NFC_F_PASSIVE_LISTEN_MODE;
+ cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
+ cmd.num_disc_configs++;
+ }
+
+ nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_CMD,
+ (1 + (cmd.num_disc_configs * sizeof(struct disc_config))),
+ &cmd);
+}
+
+struct nci_rf_discover_select_param {
+ __u8 rf_discovery_id;
+ __u8 rf_protocol;
+};
+
+static void nci_rf_discover_select_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_rf_discover_select_param *param =
+ (struct nci_rf_discover_select_param *)opt;
+ struct nci_rf_discover_select_cmd cmd;
+
+ cmd.rf_discovery_id = param->rf_discovery_id;
+ cmd.rf_protocol = param->rf_protocol;
+
+ switch (cmd.rf_protocol) {
+ case NCI_RF_PROTOCOL_ISO_DEP:
+ cmd.rf_interface = NCI_RF_INTERFACE_ISO_DEP;
+ break;
+
+ case NCI_RF_PROTOCOL_NFC_DEP:
+ cmd.rf_interface = NCI_RF_INTERFACE_NFC_DEP;
+ break;
+
+ default:
+ cmd.rf_interface = NCI_RF_INTERFACE_FRAME;
+ break;
+ }
+
+ nci_send_cmd(ndev, NCI_OP_RF_DISCOVER_SELECT_CMD,
+ sizeof(struct nci_rf_discover_select_cmd), &cmd);
+}
+
+static void nci_rf_deactivate_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_rf_deactivate_cmd cmd;
+
+ cmd.type = opt;
+
+ nci_send_cmd(ndev, NCI_OP_RF_DEACTIVATE_CMD,
+ sizeof(struct nci_rf_deactivate_cmd), &cmd);
+}
+
+struct nci_cmd_param {
+ __u16 opcode;
+ size_t len;
+ __u8 *payload;
+};
+
+static void nci_generic_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_cmd_param *param =
+ (struct nci_cmd_param *)opt;
+
+ nci_send_cmd(ndev, param->opcode, param->len, param->payload);
+}
+
+int nci_prop_cmd(struct nci_dev *ndev, __u8 oid, size_t len, __u8 *payload)
+{
+ struct nci_cmd_param param;
+
+ param.opcode = nci_opcode_pack(NCI_GID_PROPRIETARY, oid);
+ param.len = len;
+ param.payload = payload;
+
+ return __nci_request(ndev, nci_generic_req, (unsigned long)¶m,
+ msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_prop_cmd);
+
+int nci_core_cmd(struct nci_dev *ndev, __u16 opcode, size_t len, __u8 *payload)
+{
+ struct nci_cmd_param param;
+
+ param.opcode = opcode;
+ param.len = len;
+ param.payload = payload;
+
+ return __nci_request(ndev, nci_generic_req, (unsigned long)¶m,
+ msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_cmd);
+
+int nci_core_reset(struct nci_dev *ndev)
+{
+ return __nci_request(ndev, nci_reset_req, 0,
+ msecs_to_jiffies(NCI_RESET_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_reset);
+
+int nci_core_init(struct nci_dev *ndev)
+{
+ return __nci_request(ndev, nci_init_req, 0,
+ msecs_to_jiffies(NCI_INIT_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_init);
+
+struct nci_loopback_data {
+ u8 conn_id;
+ struct sk_buff *data;
+};
+
+static void nci_send_data_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_loopback_data *data = (struct nci_loopback_data *)opt;
+
+ nci_send_data(ndev, data->conn_id, data->data);
+}
+
+static void nci_nfcc_loopback_cb(void *context, struct sk_buff *skb, int err)
+{
+ struct nci_dev *ndev = (struct nci_dev *)context;
+ struct nci_conn_info *conn_info;
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, ndev->cur_conn_id);
+ if (!conn_info) {
+ nci_req_complete(ndev, NCI_STATUS_REJECTED);
+ return;
+ }
+
+ conn_info->rx_skb = skb;
+
+ nci_req_complete(ndev, NCI_STATUS_OK);
+}
+
+int nci_nfcc_loopback(struct nci_dev *ndev, void *data, size_t data_len,
+ struct sk_buff **resp)
+{
+ int r;
+ struct nci_loopback_data loopback_data;
+ struct nci_conn_info *conn_info;
+ struct sk_buff *skb;
+ int conn_id = nci_get_conn_info_by_dest_type_params(ndev,
+ NCI_DESTINATION_NFCC_LOOPBACK, NULL);
+
+ if (conn_id < 0) {
+ r = nci_core_conn_create(ndev, NCI_DESTINATION_NFCC_LOOPBACK,
+ 0, 0, NULL);
+ if (r != NCI_STATUS_OK)
+ return r;
+
+ conn_id = nci_get_conn_info_by_dest_type_params(ndev,
+ NCI_DESTINATION_NFCC_LOOPBACK,
+ NULL);
+ }
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+ if (!conn_info)
+ return -EPROTO;
+
+ /* store cb and context to be used on receiving data */
+ conn_info->data_exchange_cb = nci_nfcc_loopback_cb;
+ conn_info->data_exchange_cb_context = ndev;
+
+ skb = nci_skb_alloc(ndev, NCI_DATA_HDR_SIZE + data_len, GFP_KERNEL);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_reserve(skb, NCI_DATA_HDR_SIZE);
+ skb_put_data(skb, data, data_len);
+
+ loopback_data.conn_id = conn_id;
+ loopback_data.data = skb;
+
+ ndev->cur_conn_id = conn_id;
+ r = nci_request(ndev, nci_send_data_req, (unsigned long)&loopback_data,
+ msecs_to_jiffies(NCI_DATA_TIMEOUT));
+ if (r == NCI_STATUS_OK && resp)
+ *resp = conn_info->rx_skb;
+
+ return r;
+}
+EXPORT_SYMBOL(nci_nfcc_loopback);
+
+static int nci_open_device(struct nci_dev *ndev)
+{
+ int rc = 0;
+
+ mutex_lock(&ndev->req_lock);
+
+ if (test_bit(NCI_UP, &ndev->flags)) {
+ rc = -EALREADY;
+ goto done;
+ }
+
+ if (ndev->ops->open(ndev)) {
+ rc = -EIO;
+ goto done;
+ }
+
+ atomic_set(&ndev->cmd_cnt, 1);
+
+ set_bit(NCI_INIT, &ndev->flags);
+
+ if (ndev->ops->init)
+ rc = ndev->ops->init(ndev);
+
+ if (!rc) {
+ rc = __nci_request(ndev, nci_reset_req, 0,
+ msecs_to_jiffies(NCI_RESET_TIMEOUT));
+ }
+
+ if (!rc && ndev->ops->setup) {
+ rc = ndev->ops->setup(ndev);
+ }
+
+ if (!rc) {
+ rc = __nci_request(ndev, nci_init_req, 0,
+ msecs_to_jiffies(NCI_INIT_TIMEOUT));
+ }
+
+ if (!rc && ndev->ops->post_setup)
+ rc = ndev->ops->post_setup(ndev);
+
+ if (!rc) {
+ rc = __nci_request(ndev, nci_init_complete_req, 0,
+ msecs_to_jiffies(NCI_INIT_TIMEOUT));
+ }
+
+ clear_bit(NCI_INIT, &ndev->flags);
+
+ if (!rc) {
+ set_bit(NCI_UP, &ndev->flags);
+ nci_clear_target_list(ndev);
+ atomic_set(&ndev->state, NCI_IDLE);
+ } else {
+ /* Init failed, cleanup */
+ skb_queue_purge(&ndev->cmd_q);
+ skb_queue_purge(&ndev->rx_q);
+ skb_queue_purge(&ndev->tx_q);
+
+ ndev->ops->close(ndev);
+ ndev->flags = 0;
+ }
+
+done:
+ mutex_unlock(&ndev->req_lock);
+ return rc;
+}
+
+static int nci_close_device(struct nci_dev *ndev)
+{
+ nci_req_cancel(ndev, ENODEV);
+ mutex_lock(&ndev->req_lock);
+
+ if (!test_and_clear_bit(NCI_UP, &ndev->flags)) {
+ del_timer_sync(&ndev->cmd_timer);
+ del_timer_sync(&ndev->data_timer);
+ mutex_unlock(&ndev->req_lock);
+ return 0;
+ }
+
+ /* Drop RX and TX queues */
+ skb_queue_purge(&ndev->rx_q);
+ skb_queue_purge(&ndev->tx_q);
+
+ /* Flush RX and TX wq */
+ flush_workqueue(ndev->rx_wq);
+ flush_workqueue(ndev->tx_wq);
+
+ /* Reset device */
+ skb_queue_purge(&ndev->cmd_q);
+ atomic_set(&ndev->cmd_cnt, 1);
+
+ set_bit(NCI_INIT, &ndev->flags);
+ __nci_request(ndev, nci_reset_req, 0,
+ msecs_to_jiffies(NCI_RESET_TIMEOUT));
+
+ /* After this point our queues are empty
+ * and no works are scheduled.
+ */
+ ndev->ops->close(ndev);
+
+ clear_bit(NCI_INIT, &ndev->flags);
+
+ del_timer_sync(&ndev->cmd_timer);
+
+ /* Flush cmd wq */
+ flush_workqueue(ndev->cmd_wq);
+
+ /* Clear flags */
+ ndev->flags = 0;
+
+ mutex_unlock(&ndev->req_lock);
+
+ return 0;
+}
+
+/* NCI command timer function */
+static void nci_cmd_timer(struct timer_list *t)
+{
+ struct nci_dev *ndev = from_timer(ndev, t, cmd_timer);
+
+ atomic_set(&ndev->cmd_cnt, 1);
+ queue_work(ndev->cmd_wq, &ndev->cmd_work);
+}
+
+/* NCI data exchange timer function */
+static void nci_data_timer(struct timer_list *t)
+{
+ struct nci_dev *ndev = from_timer(ndev, t, data_timer);
+
+ set_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
+ queue_work(ndev->rx_wq, &ndev->rx_work);
+}
+
+static int nci_dev_up(struct nfc_dev *nfc_dev)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ return nci_open_device(ndev);
+}
+
+static int nci_dev_down(struct nfc_dev *nfc_dev)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ return nci_close_device(ndev);
+}
+
+int nci_set_config(struct nci_dev *ndev, __u8 id, size_t len, __u8 *val)
+{
+ struct nci_set_config_param param;
+
+ if (!val || !len)
+ return 0;
+
+ param.id = id;
+ param.len = len;
+ param.val = val;
+
+ return __nci_request(ndev, nci_set_config_req, (unsigned long)¶m,
+ msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_set_config);
+
+static void nci_nfcee_discover_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_nfcee_discover_cmd cmd;
+ __u8 action = opt;
+
+ cmd.discovery_action = action;
+
+ nci_send_cmd(ndev, NCI_OP_NFCEE_DISCOVER_CMD, 1, &cmd);
+}
+
+int nci_nfcee_discover(struct nci_dev *ndev, u8 action)
+{
+ return __nci_request(ndev, nci_nfcee_discover_req, action,
+ msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_nfcee_discover);
+
+static void nci_nfcee_mode_set_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_nfcee_mode_set_cmd *cmd =
+ (struct nci_nfcee_mode_set_cmd *)opt;
+
+ nci_send_cmd(ndev, NCI_OP_NFCEE_MODE_SET_CMD,
+ sizeof(struct nci_nfcee_mode_set_cmd), cmd);
+}
+
+int nci_nfcee_mode_set(struct nci_dev *ndev, u8 nfcee_id, u8 nfcee_mode)
+{
+ struct nci_nfcee_mode_set_cmd cmd;
+
+ cmd.nfcee_id = nfcee_id;
+ cmd.nfcee_mode = nfcee_mode;
+
+ return __nci_request(ndev, nci_nfcee_mode_set_req,
+ (unsigned long)&cmd,
+ msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_nfcee_mode_set);
+
+static void nci_core_conn_create_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct core_conn_create_data *data =
+ (struct core_conn_create_data *)opt;
+
+ nci_send_cmd(ndev, NCI_OP_CORE_CONN_CREATE_CMD, data->length, data->cmd);
+}
+
+int nci_core_conn_create(struct nci_dev *ndev, u8 destination_type,
+ u8 number_destination_params,
+ size_t params_len,
+ struct core_conn_create_dest_spec_params *params)
+{
+ int r;
+ struct nci_core_conn_create_cmd *cmd;
+ struct core_conn_create_data data;
+
+ data.length = params_len + sizeof(struct nci_core_conn_create_cmd);
+ cmd = kzalloc(data.length, GFP_KERNEL);
+ if (!cmd)
+ return -ENOMEM;
+
+ cmd->destination_type = destination_type;
+ cmd->number_destination_params = number_destination_params;
+
+ data.cmd = cmd;
+
+ if (params) {
+ memcpy(cmd->params, params, params_len);
+ if (params->length > 0)
+ memcpy(&ndev->cur_params,
+ ¶ms->value[DEST_SPEC_PARAMS_ID_INDEX],
+ sizeof(struct dest_spec_params));
+ else
+ ndev->cur_params.id = 0;
+ } else {
+ ndev->cur_params.id = 0;
+ }
+ ndev->cur_dest_type = destination_type;
+
+ r = __nci_request(ndev, nci_core_conn_create_req, (unsigned long)&data,
+ msecs_to_jiffies(NCI_CMD_TIMEOUT));
+ kfree(cmd);
+ return r;
+}
+EXPORT_SYMBOL(nci_core_conn_create);
+
+static void nci_core_conn_close_req(struct nci_dev *ndev, unsigned long opt)
+{
+ __u8 conn_id = opt;
+
+ nci_send_cmd(ndev, NCI_OP_CORE_CONN_CLOSE_CMD, 1, &conn_id);
+}
+
+int nci_core_conn_close(struct nci_dev *ndev, u8 conn_id)
+{
+ ndev->cur_conn_id = conn_id;
+ return __nci_request(ndev, nci_core_conn_close_req, conn_id,
+ msecs_to_jiffies(NCI_CMD_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_core_conn_close);
+
+static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ struct nci_set_config_param param;
+ int rc;
+
+ param.val = nfc_get_local_general_bytes(nfc_dev, ¶m.len);
+ if ((param.val == NULL) || (param.len == 0))
+ return 0;
+
+ if (param.len > NFC_MAX_GT_LEN)
+ return -EINVAL;
+
+ param.id = NCI_PN_ATR_REQ_GEN_BYTES;
+
+ rc = nci_request(ndev, nci_set_config_req, (unsigned long)¶m,
+ msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
+ if (rc)
+ return rc;
+
+ param.id = NCI_LN_ATR_RES_GEN_BYTES;
+
+ return nci_request(ndev, nci_set_config_req, (unsigned long)¶m,
+ msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
+}
+
+static int nci_set_listen_parameters(struct nfc_dev *nfc_dev)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ int rc;
+ __u8 val;
+
+ val = NCI_LA_SEL_INFO_NFC_DEP_MASK;
+
+ rc = nci_set_config(ndev, NCI_LA_SEL_INFO, 1, &val);
+ if (rc)
+ return rc;
+
+ val = NCI_LF_PROTOCOL_TYPE_NFC_DEP_MASK;
+
+ rc = nci_set_config(ndev, NCI_LF_PROTOCOL_TYPE, 1, &val);
+ if (rc)
+ return rc;
+
+ val = NCI_LF_CON_BITR_F_212 | NCI_LF_CON_BITR_F_424;
+
+ return nci_set_config(ndev, NCI_LF_CON_BITR_F, 1, &val);
+}
+
+static int nci_start_poll(struct nfc_dev *nfc_dev,
+ __u32 im_protocols, __u32 tm_protocols)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ struct nci_rf_discover_param param;
+ int rc;
+
+ if ((atomic_read(&ndev->state) == NCI_DISCOVERY) ||
+ (atomic_read(&ndev->state) == NCI_W4_ALL_DISCOVERIES)) {
+ pr_err("unable to start poll, since poll is already active\n");
+ return -EBUSY;
+ }
+
+ if (ndev->target_active_prot) {
+ pr_err("there is an active target\n");
+ return -EBUSY;
+ }
+
+ if ((atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) ||
+ (atomic_read(&ndev->state) == NCI_POLL_ACTIVE)) {
+ pr_debug("target active or w4 select, implicitly deactivate\n");
+
+ rc = nci_request(ndev, nci_rf_deactivate_req,
+ NCI_DEACTIVATE_TYPE_IDLE_MODE,
+ msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+ if (rc)
+ return -EBUSY;
+ }
+
+ if ((im_protocols | tm_protocols) & NFC_PROTO_NFC_DEP_MASK) {
+ rc = nci_set_local_general_bytes(nfc_dev);
+ if (rc) {
+ pr_err("failed to set local general bytes\n");
+ return rc;
+ }
+ }
+
+ if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) {
+ rc = nci_set_listen_parameters(nfc_dev);
+ if (rc)
+ pr_err("failed to set listen parameters\n");
+ }
+
+ param.im_protocols = im_protocols;
+ param.tm_protocols = tm_protocols;
+ rc = nci_request(ndev, nci_rf_discover_req, (unsigned long)¶m,
+ msecs_to_jiffies(NCI_RF_DISC_TIMEOUT));
+
+ if (!rc)
+ ndev->poll_prots = im_protocols;
+
+ return rc;
+}
+
+static void nci_stop_poll(struct nfc_dev *nfc_dev)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ if ((atomic_read(&ndev->state) != NCI_DISCOVERY) &&
+ (atomic_read(&ndev->state) != NCI_W4_ALL_DISCOVERIES)) {
+ pr_err("unable to stop poll, since poll is not active\n");
+ return;
+ }
+
+ nci_request(ndev, nci_rf_deactivate_req, NCI_DEACTIVATE_TYPE_IDLE_MODE,
+ msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+}
+
+static int nci_activate_target(struct nfc_dev *nfc_dev,
+ struct nfc_target *target, __u32 protocol)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ struct nci_rf_discover_select_param param;
+ struct nfc_target *nci_target = NULL;
+ int i;
+ int rc = 0;
+
+ pr_debug("target_idx %d, protocol 0x%x\n", target->idx, protocol);
+
+ if ((atomic_read(&ndev->state) != NCI_W4_HOST_SELECT) &&
+ (atomic_read(&ndev->state) != NCI_POLL_ACTIVE)) {
+ pr_err("there is no available target to activate\n");
+ return -EINVAL;
+ }
+
+ if (ndev->target_active_prot) {
+ pr_err("there is already an active target\n");
+ return -EBUSY;
+ }
+
+ for (i = 0; i < ndev->n_targets; i++) {
+ if (ndev->targets[i].idx == target->idx) {
+ nci_target = &ndev->targets[i];
+ break;
+ }
+ }
+
+ if (!nci_target) {
+ pr_err("unable to find the selected target\n");
+ return -EINVAL;
+ }
+
+ if (!(nci_target->supported_protocols & (1 << protocol))) {
+ pr_err("target does not support the requested protocol 0x%x\n",
+ protocol);
+ return -EINVAL;
+ }
+
+ if (atomic_read(&ndev->state) == NCI_W4_HOST_SELECT) {
+ param.rf_discovery_id = nci_target->logical_idx;
+
+ if (protocol == NFC_PROTO_JEWEL)
+ param.rf_protocol = NCI_RF_PROTOCOL_T1T;
+ else if (protocol == NFC_PROTO_MIFARE)
+ param.rf_protocol = NCI_RF_PROTOCOL_T2T;
+ else if (protocol == NFC_PROTO_FELICA)
+ param.rf_protocol = NCI_RF_PROTOCOL_T3T;
+ else if (protocol == NFC_PROTO_ISO14443 ||
+ protocol == NFC_PROTO_ISO14443_B)
+ param.rf_protocol = NCI_RF_PROTOCOL_ISO_DEP;
+ else
+ param.rf_protocol = NCI_RF_PROTOCOL_NFC_DEP;
+
+ rc = nci_request(ndev, nci_rf_discover_select_req,
+ (unsigned long)¶m,
+ msecs_to_jiffies(NCI_RF_DISC_SELECT_TIMEOUT));
+ }
+
+ if (!rc)
+ ndev->target_active_prot = protocol;
+
+ return rc;
+}
+
+static void nci_deactivate_target(struct nfc_dev *nfc_dev,
+ struct nfc_target *target,
+ __u8 mode)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ u8 nci_mode = NCI_DEACTIVATE_TYPE_IDLE_MODE;
+
+ pr_debug("entry\n");
+
+ if (!ndev->target_active_prot) {
+ pr_err("unable to deactivate target, no active target\n");
+ return;
+ }
+
+ ndev->target_active_prot = 0;
+
+ switch (mode) {
+ case NFC_TARGET_MODE_SLEEP:
+ nci_mode = NCI_DEACTIVATE_TYPE_SLEEP_MODE;
+ break;
+ }
+
+ if (atomic_read(&ndev->state) == NCI_POLL_ACTIVE) {
+ nci_request(ndev, nci_rf_deactivate_req, nci_mode,
+ msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+ }
+}
+
+static int nci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+ __u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ int rc;
+
+ pr_debug("target_idx %d, comm_mode %d\n", target->idx, comm_mode);
+
+ rc = nci_activate_target(nfc_dev, target, NFC_PROTO_NFC_DEP);
+ if (rc)
+ return rc;
+
+ rc = nfc_set_remote_general_bytes(nfc_dev, ndev->remote_gb,
+ ndev->remote_gb_len);
+ if (!rc)
+ rc = nfc_dep_link_is_up(nfc_dev, target->idx, NFC_COMM_PASSIVE,
+ NFC_RF_INITIATOR);
+
+ return rc;
+}
+
+static int nci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ int rc;
+
+ pr_debug("entry\n");
+
+ if (nfc_dev->rf_mode == NFC_RF_INITIATOR) {
+ nci_deactivate_target(nfc_dev, NULL, NCI_DEACTIVATE_TYPE_IDLE_MODE);
+ } else {
+ if (atomic_read(&ndev->state) == NCI_LISTEN_ACTIVE ||
+ atomic_read(&ndev->state) == NCI_DISCOVERY) {
+ nci_request(ndev, nci_rf_deactivate_req, 0,
+ msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT));
+ }
+
+ rc = nfc_tm_deactivated(nfc_dev);
+ if (rc)
+ pr_err("error when signaling tm deactivation\n");
+ }
+
+ return 0;
+}
+
+
+static int nci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
+ struct sk_buff *skb,
+ data_exchange_cb_t cb, void *cb_context)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ int rc;
+ struct nci_conn_info *conn_info;
+
+ conn_info = ndev->rf_conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ pr_debug("target_idx %d, len %d\n", target->idx, skb->len);
+
+ if (!ndev->target_active_prot) {
+ pr_err("unable to exchange data, no active target\n");
+ return -EINVAL;
+ }
+
+ if (test_and_set_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+ return -EBUSY;
+
+ /* store cb and context to be used on receiving data */
+ conn_info->data_exchange_cb = cb;
+ conn_info->data_exchange_cb_context = cb_context;
+
+ rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb);
+ if (rc)
+ clear_bit(NCI_DATA_EXCHANGE, &ndev->flags);
+
+ return rc;
+}
+
+static int nci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+ int rc;
+
+ rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb);
+ if (rc)
+ pr_err("unable to send data\n");
+
+ return rc;
+}
+
+static int nci_enable_se(struct nfc_dev *nfc_dev, u32 se_idx)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ if (ndev->ops->enable_se)
+ return ndev->ops->enable_se(ndev, se_idx);
+
+ return 0;
+}
+
+static int nci_disable_se(struct nfc_dev *nfc_dev, u32 se_idx)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ if (ndev->ops->disable_se)
+ return ndev->ops->disable_se(ndev, se_idx);
+
+ return 0;
+}
+
+static int nci_discover_se(struct nfc_dev *nfc_dev)
+{
+ int r;
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ if (ndev->ops->discover_se) {
+ r = nci_nfcee_discover(ndev, NCI_NFCEE_DISCOVERY_ACTION_ENABLE);
+ if (r != NCI_STATUS_OK)
+ return -EPROTO;
+
+ return ndev->ops->discover_se(ndev);
+ }
+
+ return 0;
+}
+
+static int nci_se_io(struct nfc_dev *nfc_dev, u32 se_idx,
+ u8 *apdu, size_t apdu_length,
+ se_io_cb_t cb, void *cb_context)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ if (ndev->ops->se_io)
+ return ndev->ops->se_io(ndev, se_idx, apdu,
+ apdu_length, cb, cb_context);
+
+ return 0;
+}
+
+static int nci_fw_download(struct nfc_dev *nfc_dev, const char *firmware_name)
+{
+ struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
+
+ if (!ndev->ops->fw_download)
+ return -ENOTSUPP;
+
+ return ndev->ops->fw_download(ndev, firmware_name);
+}
+
+static struct nfc_ops nci_nfc_ops = {
+ .dev_up = nci_dev_up,
+ .dev_down = nci_dev_down,
+ .start_poll = nci_start_poll,
+ .stop_poll = nci_stop_poll,
+ .dep_link_up = nci_dep_link_up,
+ .dep_link_down = nci_dep_link_down,
+ .activate_target = nci_activate_target,
+ .deactivate_target = nci_deactivate_target,
+ .im_transceive = nci_transceive,
+ .tm_send = nci_tm_send,
+ .enable_se = nci_enable_se,
+ .disable_se = nci_disable_se,
+ .discover_se = nci_discover_se,
+ .se_io = nci_se_io,
+ .fw_download = nci_fw_download,
+};
+
+/* ---- Interface to NCI drivers ---- */
+/**
+ * nci_allocate_device - allocate a new nci device
+ *
+ * @ops: device operations
+ * @supported_protocols: NFC protocols supported by the device
+ */
+struct nci_dev *nci_allocate_device(struct nci_ops *ops,
+ __u32 supported_protocols,
+ int tx_headroom, int tx_tailroom)
+{
+ struct nci_dev *ndev;
+
+ pr_debug("supported_protocols 0x%x\n", supported_protocols);
+
+ if (!ops->open || !ops->close || !ops->send)
+ return NULL;
+
+ if (!supported_protocols)
+ return NULL;
+
+ ndev = kzalloc(sizeof(struct nci_dev), GFP_KERNEL);
+ if (!ndev)
+ return NULL;
+
+ ndev->ops = ops;
+
+ if (ops->n_prop_ops > NCI_MAX_PROPRIETARY_CMD) {
+ pr_err("Too many proprietary commands: %zd\n",
+ ops->n_prop_ops);
+ ops->prop_ops = NULL;
+ ops->n_prop_ops = 0;
+ }
+
+ ndev->tx_headroom = tx_headroom;
+ ndev->tx_tailroom = tx_tailroom;
+ init_completion(&ndev->req_completion);
+
+ ndev->nfc_dev = nfc_allocate_device(&nci_nfc_ops,
+ supported_protocols,
+ tx_headroom + NCI_DATA_HDR_SIZE,
+ tx_tailroom);
+ if (!ndev->nfc_dev)
+ goto free_nci;
+
+ ndev->hci_dev = nci_hci_allocate(ndev);
+ if (!ndev->hci_dev)
+ goto free_nfc;
+
+ nfc_set_drvdata(ndev->nfc_dev, ndev);
+
+ return ndev;
+
+free_nfc:
+ nfc_free_device(ndev->nfc_dev);
+free_nci:
+ kfree(ndev);
+ return NULL;
+}
+EXPORT_SYMBOL(nci_allocate_device);
+
+/**
+ * nci_free_device - deallocate nci device
+ *
+ * @ndev: The nci device to deallocate
+ */
+void nci_free_device(struct nci_dev *ndev)
+{
+ nfc_free_device(ndev->nfc_dev);
+ kfree(ndev);
+}
+EXPORT_SYMBOL(nci_free_device);
+
+/**
+ * nci_register_device - register a nci device in the nfc subsystem
+ *
+ * @dev: The nci device to register
+ */
+int nci_register_device(struct nci_dev *ndev)
+{
+ int rc;
+ struct device *dev = &ndev->nfc_dev->dev;
+ char name[32];
+
+ ndev->flags = 0;
+
+ INIT_WORK(&ndev->cmd_work, nci_cmd_work);
+ snprintf(name, sizeof(name), "%s_nci_cmd_wq", dev_name(dev));
+ ndev->cmd_wq = create_singlethread_workqueue(name);
+ if (!ndev->cmd_wq) {
+ rc = -ENOMEM;
+ goto exit;
+ }
+
+ INIT_WORK(&ndev->rx_work, nci_rx_work);
+ snprintf(name, sizeof(name), "%s_nci_rx_wq", dev_name(dev));
+ ndev->rx_wq = create_singlethread_workqueue(name);
+ if (!ndev->rx_wq) {
+ rc = -ENOMEM;
+ goto destroy_cmd_wq_exit;
+ }
+
+ INIT_WORK(&ndev->tx_work, nci_tx_work);
+ snprintf(name, sizeof(name), "%s_nci_tx_wq", dev_name(dev));
+ ndev->tx_wq = create_singlethread_workqueue(name);
+ if (!ndev->tx_wq) {
+ rc = -ENOMEM;
+ goto destroy_rx_wq_exit;
+ }
+
+ skb_queue_head_init(&ndev->cmd_q);
+ skb_queue_head_init(&ndev->rx_q);
+ skb_queue_head_init(&ndev->tx_q);
+
+ timer_setup(&ndev->cmd_timer, nci_cmd_timer, 0);
+ timer_setup(&ndev->data_timer, nci_data_timer, 0);
+
+ mutex_init(&ndev->req_lock);
+ INIT_LIST_HEAD(&ndev->conn_info_list);
+
+ rc = nfc_register_device(ndev->nfc_dev);
+ if (rc)
+ goto destroy_rx_wq_exit;
+
+ goto exit;
+
+destroy_rx_wq_exit:
+ destroy_workqueue(ndev->rx_wq);
+
+destroy_cmd_wq_exit:
+ destroy_workqueue(ndev->cmd_wq);
+
+exit:
+ return rc;
+}
+EXPORT_SYMBOL(nci_register_device);
+
+/**
+ * nci_unregister_device - unregister a nci device in the nfc subsystem
+ *
+ * @dev: The nci device to unregister
+ */
+void nci_unregister_device(struct nci_dev *ndev)
+{
+ struct nci_conn_info *conn_info, *n;
+
+ nci_close_device(ndev);
+
+ destroy_workqueue(ndev->cmd_wq);
+ destroy_workqueue(ndev->rx_wq);
+ destroy_workqueue(ndev->tx_wq);
+
+ list_for_each_entry_safe(conn_info, n, &ndev->conn_info_list, list) {
+ list_del(&conn_info->list);
+ /* conn_info is allocated with devm_kzalloc */
+ }
+
+ nfc_unregister_device(ndev->nfc_dev);
+}
+EXPORT_SYMBOL(nci_unregister_device);
+
+/**
+ * nci_recv_frame - receive frame from NCI drivers
+ *
+ * @ndev: The nci device
+ * @skb: The sk_buff to receive
+ */
+int nci_recv_frame(struct nci_dev *ndev, struct sk_buff *skb)
+{
+ pr_debug("len %d\n", skb->len);
+
+ if (!ndev || (!test_bit(NCI_UP, &ndev->flags) &&
+ !test_bit(NCI_INIT, &ndev->flags))) {
+ kfree_skb(skb);
+ return -ENXIO;
+ }
+
+ /* Queue frame for rx worker thread */
+ skb_queue_tail(&ndev->rx_q, skb);
+ queue_work(ndev->rx_wq, &ndev->rx_work);
+
+ return 0;
+}
+EXPORT_SYMBOL(nci_recv_frame);
+
+int nci_send_frame(struct nci_dev *ndev, struct sk_buff *skb)
+{
+ pr_debug("len %d\n", skb->len);
+
+ if (!ndev) {
+ kfree_skb(skb);
+ return -ENODEV;
+ }
+
+ /* Get rid of skb owner, prior to sending to the driver. */
+ skb_orphan(skb);
+
+ /* Send copy to sniffer */
+ nfc_send_to_raw_sock(ndev->nfc_dev, skb,
+ RAW_PAYLOAD_NCI, NFC_DIRECTION_TX);
+
+ return ndev->ops->send(ndev, skb);
+}
+EXPORT_SYMBOL(nci_send_frame);
+
+/* Send NCI command */
+int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload)
+{
+ struct nci_ctrl_hdr *hdr;
+ struct sk_buff *skb;
+
+ pr_debug("opcode 0x%x, plen %d\n", opcode, plen);
+
+ skb = nci_skb_alloc(ndev, (NCI_CTRL_HDR_SIZE + plen), GFP_KERNEL);
+ if (!skb) {
+ pr_err("no memory for command\n");
+ return -ENOMEM;
+ }
+
+ hdr = skb_put(skb, NCI_CTRL_HDR_SIZE);
+ hdr->gid = nci_opcode_gid(opcode);
+ hdr->oid = nci_opcode_oid(opcode);
+ hdr->plen = plen;
+
+ nci_mt_set((__u8 *)hdr, NCI_MT_CMD_PKT);
+ nci_pbf_set((__u8 *)hdr, NCI_PBF_LAST);
+
+ if (plen)
+ skb_put_data(skb, payload, plen);
+
+ skb_queue_tail(&ndev->cmd_q, skb);
+ queue_work(ndev->cmd_wq, &ndev->cmd_work);
+
+ return 0;
+}
+EXPORT_SYMBOL(nci_send_cmd);
+
+/* Proprietary commands API */
+static struct nci_driver_ops *ops_cmd_lookup(struct nci_driver_ops *ops,
+ size_t n_ops,
+ __u16 opcode)
+{
+ size_t i;
+ struct nci_driver_ops *op;
+
+ if (!ops || !n_ops)
+ return NULL;
+
+ for (i = 0; i < n_ops; i++) {
+ op = &ops[i];
+ if (op->opcode == opcode)
+ return op;
+ }
+
+ return NULL;
+}
+
+static int nci_op_rsp_packet(struct nci_dev *ndev, __u16 rsp_opcode,
+ struct sk_buff *skb, struct nci_driver_ops *ops,
+ size_t n_ops)
+{
+ struct nci_driver_ops *op;
+
+ op = ops_cmd_lookup(ops, n_ops, rsp_opcode);
+ if (!op || !op->rsp)
+ return -ENOTSUPP;
+
+ return op->rsp(ndev, skb);
+}
+
+static int nci_op_ntf_packet(struct nci_dev *ndev, __u16 ntf_opcode,
+ struct sk_buff *skb, struct nci_driver_ops *ops,
+ size_t n_ops)
+{
+ struct nci_driver_ops *op;
+
+ op = ops_cmd_lookup(ops, n_ops, ntf_opcode);
+ if (!op || !op->ntf)
+ return -ENOTSUPP;
+
+ return op->ntf(ndev, skb);
+}
+
+int nci_prop_rsp_packet(struct nci_dev *ndev, __u16 opcode,
+ struct sk_buff *skb)
+{
+ return nci_op_rsp_packet(ndev, opcode, skb, ndev->ops->prop_ops,
+ ndev->ops->n_prop_ops);
+}
+
+int nci_prop_ntf_packet(struct nci_dev *ndev, __u16 opcode,
+ struct sk_buff *skb)
+{
+ return nci_op_ntf_packet(ndev, opcode, skb, ndev->ops->prop_ops,
+ ndev->ops->n_prop_ops);
+}
+
+int nci_core_rsp_packet(struct nci_dev *ndev, __u16 opcode,
+ struct sk_buff *skb)
+{
+ return nci_op_rsp_packet(ndev, opcode, skb, ndev->ops->core_ops,
+ ndev->ops->n_core_ops);
+}
+
+int nci_core_ntf_packet(struct nci_dev *ndev, __u16 opcode,
+ struct sk_buff *skb)
+{
+ return nci_op_ntf_packet(ndev, opcode, skb, ndev->ops->core_ops,
+ ndev->ops->n_core_ops);
+}
+
+/* ---- NCI TX Data worker thread ---- */
+
+static void nci_tx_work(struct work_struct *work)
+{
+ struct nci_dev *ndev = container_of(work, struct nci_dev, tx_work);
+ struct nci_conn_info *conn_info;
+ struct sk_buff *skb;
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, ndev->cur_conn_id);
+ if (!conn_info)
+ return;
+
+ pr_debug("credits_cnt %d\n", atomic_read(&conn_info->credits_cnt));
+
+ /* Send queued tx data */
+ while (atomic_read(&conn_info->credits_cnt)) {
+ skb = skb_dequeue(&ndev->tx_q);
+ if (!skb)
+ return;
+
+ /* Check if data flow control is used */
+ if (atomic_read(&conn_info->credits_cnt) !=
+ NCI_DATA_FLOW_CONTROL_NOT_USED)
+ atomic_dec(&conn_info->credits_cnt);
+
+ pr_debug("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d\n",
+ nci_pbf(skb->data),
+ nci_conn_id(skb->data),
+ nci_plen(skb->data));
+
+ nci_send_frame(ndev, skb);
+
+ mod_timer(&ndev->data_timer,
+ jiffies + msecs_to_jiffies(NCI_DATA_TIMEOUT));
+ }
+}
+
+/* ----- NCI RX worker thread (data & control) ----- */
+
+static void nci_rx_work(struct work_struct *work)
+{
+ struct nci_dev *ndev = container_of(work, struct nci_dev, rx_work);
+ struct sk_buff *skb;
+
+ while ((skb = skb_dequeue(&ndev->rx_q))) {
+
+ /* Send copy to sniffer */
+ nfc_send_to_raw_sock(ndev->nfc_dev, skb,
+ RAW_PAYLOAD_NCI, NFC_DIRECTION_RX);
+
+ /* Process frame */
+ switch (nci_mt(skb->data)) {
+ case NCI_MT_RSP_PKT:
+ nci_rsp_packet(ndev, skb);
+ break;
+
+ case NCI_MT_NTF_PKT:
+ nci_ntf_packet(ndev, skb);
+ break;
+
+ case NCI_MT_DATA_PKT:
+ nci_rx_data_packet(ndev, skb);
+ break;
+
+ default:
+ pr_err("unknown MT 0x%x\n", nci_mt(skb->data));
+ kfree_skb(skb);
+ break;
+ }
+ }
+
+ /* check if a data exchange timout has occurred */
+ if (test_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags)) {
+ /* complete the data exchange transaction, if exists */
+ if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags))
+ nci_data_exchange_complete(ndev, NULL,
+ ndev->cur_conn_id,
+ -ETIMEDOUT);
+
+ clear_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags);
+ }
+}
+
+/* ----- NCI TX CMD worker thread ----- */
+
+static void nci_cmd_work(struct work_struct *work)
+{
+ struct nci_dev *ndev = container_of(work, struct nci_dev, cmd_work);
+ struct sk_buff *skb;
+
+ pr_debug("cmd_cnt %d\n", atomic_read(&ndev->cmd_cnt));
+
+ /* Send queued command */
+ if (atomic_read(&ndev->cmd_cnt)) {
+ skb = skb_dequeue(&ndev->cmd_q);
+ if (!skb)
+ return;
+
+ atomic_dec(&ndev->cmd_cnt);
+
+ pr_debug("NCI TX: MT=cmd, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n",
+ nci_pbf(skb->data),
+ nci_opcode_gid(nci_opcode(skb->data)),
+ nci_opcode_oid(nci_opcode(skb->data)),
+ nci_plen(skb->data));
+
+ nci_send_frame(ndev, skb);
+
+ mod_timer(&ndev->cmd_timer,
+ jiffies + msecs_to_jiffies(NCI_CMD_TIMEOUT));
+ }
+}
+
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c
new file mode 100644
index 0000000..908f25e
--- /dev/null
+++ b/net/nfc/nci/data.c
@@ -0,0 +1,311 @@
+/*
+ * The NFC Controller Interface is the communication protocol between an
+ * NFC Controller (NFCC) and a Device Host (DH).
+ *
+ * Copyright (C) 2011 Texas Instruments, Inc.
+ * Copyright (C) 2014 Marvell International Ltd.
+ *
+ * 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,
+ __u8 conn_id, int err)
+{
+ struct nci_conn_info *conn_info;
+ data_exchange_cb_t cb;
+ void *cb_context;
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+ if (!conn_info) {
+ kfree_skb(skb);
+ goto exit;
+ }
+
+ cb = conn_info->data_exchange_cb;
+ cb_context = conn_info->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) {
+ /* 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);
+ }
+
+exit:
+ 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 = 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);
+}
+
+int nci_conn_max_data_pkt_payload_size(struct nci_dev *ndev, __u8 conn_id)
+{
+ struct nci_conn_info *conn_info;
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+ if (!conn_info)
+ return -EPROTO;
+
+ return conn_info->max_pkt_payload_len;
+}
+EXPORT_SYMBOL(nci_conn_max_data_pkt_payload_size);
+
+static int nci_queue_tx_data_frags(struct nci_dev *ndev,
+ __u8 conn_id,
+ struct sk_buff *skb) {
+ struct nci_conn_info *conn_info;
+ 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);
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+ if (!conn_info) {
+ rc = -EPROTO;
+ goto free_exit;
+ }
+
+ __skb_queue_head_init(&frags_q);
+
+ while (total_len) {
+ frag_len =
+ min_t(int, total_len, conn_info->max_pkt_payload_len);
+
+ 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 */
+ skb_put_data(skb_frag, 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)
+{
+ struct nci_conn_info *conn_info;
+ int rc = 0;
+
+ pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len);
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, conn_id);
+ if (!conn_info) {
+ rc = -EPROTO;
+ goto free_exit;
+ }
+
+ /* check if the packet need to be fragmented */
+ if (skb->len <= conn_info->max_pkt_payload_len) {
+ /* 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;
+ }
+ }
+
+ ndev->cur_conn_id = conn_id;
+ queue_work(ndev->tx_wq, &ndev->tx_work);
+
+ goto exit;
+
+free_exit:
+ kfree_skb(skb);
+
+exit:
+ return rc;
+}
+EXPORT_SYMBOL(nci_send_data);
+
+/* ----------------- NCI RX Data ----------------- */
+
+static void nci_add_rx_data_frag(struct nci_dev *ndev,
+ struct sk_buff *skb,
+ __u8 pbf, __u8 conn_id, __u8 status)
+{
+ int reassembly_len;
+ int err = 0;
+
+ if (status) {
+ err = status;
+ goto exit;
+ }
+
+ 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:
+ if (ndev->nfc_dev->rf_mode == NFC_RF_TARGET) {
+ /* Data received in Target mode, forward to nfc core */
+ err = nfc_tm_data_received(ndev->nfc_dev, skb);
+ if (err)
+ pr_err("unable to handle received data\n");
+ } else {
+ nci_data_exchange_complete(ndev, skb, conn_id, err);
+ }
+}
+
+/* Rx Data packet */
+void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb)
+{
+ __u8 pbf = nci_pbf(skb->data);
+ __u8 status = 0;
+ __u8 conn_id = nci_conn_id(skb->data);
+ struct nci_conn_info *conn_info;
+
+ 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));
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev, nci_conn_id(skb->data));
+ if (!conn_info)
+ return;
+
+ /* 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");
+ status = skb->data[skb->len - 1];
+ skb_trim(skb, (skb->len - 1));
+ }
+
+ nci_add_rx_data_frag(ndev, skb, pbf, conn_id, nci_to_errno(status));
+}
diff --git a/net/nfc/nci/hci.c b/net/nfc/nci/hci.c
new file mode 100644
index 0000000..ddfc52a
--- /dev/null
+++ b/net/nfc/nci/hci.c
@@ -0,0 +1,801 @@
+/*
+ * The NFC Controller Interface is the communication protocol between an
+ * NFC Controller (NFCC) and a Device Host (DH).
+ * This is the HCI over NCI implementation, as specified in the 10.2
+ * section of the NCI 1.1 specification.
+ *
+ * Copyright (C) 2014 STMicroelectronics SAS. 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 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/skbuff.h>
+
+#include "../nfc.h"
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+#include <linux/nfc.h>
+
+struct nci_data {
+ u8 conn_id;
+ u8 pipe;
+ u8 cmd;
+ const u8 *data;
+ u32 data_len;
+} __packed;
+
+struct nci_hci_create_pipe_params {
+ u8 src_gate;
+ u8 dest_host;
+ u8 dest_gate;
+} __packed;
+
+struct nci_hci_create_pipe_resp {
+ u8 src_host;
+ u8 src_gate;
+ u8 dest_host;
+ u8 dest_gate;
+ u8 pipe;
+} __packed;
+
+struct nci_hci_delete_pipe_noti {
+ u8 pipe;
+} __packed;
+
+struct nci_hci_all_pipe_cleared_noti {
+ u8 host;
+} __packed;
+
+struct nci_hcp_message {
+ u8 header; /* type -cmd,evt,rsp- + instruction */
+ u8 data[];
+} __packed;
+
+struct nci_hcp_packet {
+ u8 header; /* cbit+pipe */
+ struct nci_hcp_message message;
+} __packed;
+
+#define NCI_HCI_ANY_SET_PARAMETER 0x01
+#define NCI_HCI_ANY_GET_PARAMETER 0x02
+#define NCI_HCI_ANY_CLOSE_PIPE 0x04
+#define NCI_HCI_ADM_CLEAR_ALL_PIPE 0x14
+
+#define NCI_HFP_NO_CHAINING 0x80
+
+#define NCI_NFCEE_ID_HCI 0x80
+
+#define NCI_EVT_HOT_PLUG 0x03
+
+#define NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY 0x01
+#define NCI_HCI_ADM_CREATE_PIPE 0x10
+#define NCI_HCI_ADM_DELETE_PIPE 0x11
+
+/* HCP headers */
+#define NCI_HCI_HCP_PACKET_HEADER_LEN 1
+#define NCI_HCI_HCP_MESSAGE_HEADER_LEN 1
+#define NCI_HCI_HCP_HEADER_LEN 2
+
+/* HCP types */
+#define NCI_HCI_HCP_COMMAND 0x00
+#define NCI_HCI_HCP_EVENT 0x01
+#define NCI_HCI_HCP_RESPONSE 0x02
+
+#define NCI_HCI_ADM_NOTIFY_PIPE_CREATED 0x12
+#define NCI_HCI_ADM_NOTIFY_PIPE_DELETED 0x13
+#define NCI_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED 0x15
+
+#define NCI_HCI_FRAGMENT 0x7f
+#define NCI_HCP_HEADER(type, instr) ((((type) & 0x03) << 6) |\
+ ((instr) & 0x3f))
+
+#define NCI_HCP_MSG_GET_TYPE(header) ((header & 0xc0) >> 6)
+#define NCI_HCP_MSG_GET_CMD(header) (header & 0x3f)
+#define NCI_HCP_MSG_GET_PIPE(header) (header & 0x7f)
+
+static int nci_hci_result_to_errno(u8 result)
+{
+ switch (result) {
+ case NCI_HCI_ANY_OK:
+ return 0;
+ case NCI_HCI_ANY_E_REG_PAR_UNKNOWN:
+ return -EOPNOTSUPP;
+ case NCI_HCI_ANY_E_TIMEOUT:
+ return -ETIME;
+ default:
+ return -1;
+ }
+}
+
+/* HCI core */
+static void nci_hci_reset_pipes(struct nci_hci_dev *hdev)
+{
+ int i;
+
+ for (i = 0; i < NCI_HCI_MAX_PIPES; i++) {
+ hdev->pipes[i].gate = NCI_HCI_INVALID_GATE;
+ hdev->pipes[i].host = NCI_HCI_INVALID_HOST;
+ }
+ memset(hdev->gate2pipe, NCI_HCI_INVALID_PIPE, sizeof(hdev->gate2pipe));
+}
+
+static void nci_hci_reset_pipes_per_host(struct nci_dev *ndev, u8 host)
+{
+ int i;
+
+ for (i = 0; i < NCI_HCI_MAX_PIPES; i++) {
+ if (ndev->hci_dev->pipes[i].host == host) {
+ ndev->hci_dev->pipes[i].gate = NCI_HCI_INVALID_GATE;
+ ndev->hci_dev->pipes[i].host = NCI_HCI_INVALID_HOST;
+ }
+ }
+}
+
+/* Fragment HCI data over NCI packet.
+ * NFC Forum NCI 10.2.2 Data Exchange:
+ * The payload of the Data Packets sent on the Logical Connection SHALL be
+ * valid HCP packets, as defined within [ETSI_102622]. Each Data Packet SHALL
+ * contain a single HCP packet. NCI Segmentation and Reassembly SHALL NOT be
+ * applied to Data Messages in either direction. The HCI fragmentation mechanism
+ * is used if required.
+ */
+static int nci_hci_send_data(struct nci_dev *ndev, u8 pipe,
+ const u8 data_type, const u8 *data,
+ size_t data_len)
+{
+ struct nci_conn_info *conn_info;
+ struct sk_buff *skb;
+ int len, i, r;
+ u8 cb = pipe;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ i = 0;
+ skb = nci_skb_alloc(ndev, conn_info->max_pkt_payload_len +
+ NCI_DATA_HDR_SIZE, GFP_KERNEL);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_reserve(skb, NCI_DATA_HDR_SIZE + 2);
+ *(u8 *)skb_push(skb, 1) = data_type;
+
+ do {
+ len = conn_info->max_pkt_payload_len;
+
+ /* If last packet add NCI_HFP_NO_CHAINING */
+ if (i + conn_info->max_pkt_payload_len -
+ (skb->len + 1) >= data_len) {
+ cb |= NCI_HFP_NO_CHAINING;
+ len = data_len - i;
+ } else {
+ len = conn_info->max_pkt_payload_len - skb->len - 1;
+ }
+
+ *(u8 *)skb_push(skb, 1) = cb;
+
+ if (len > 0)
+ skb_put_data(skb, data + i, len);
+
+ r = nci_send_data(ndev, conn_info->conn_id, skb);
+ if (r < 0)
+ return r;
+
+ i += len;
+
+ if (i < data_len) {
+ skb = nci_skb_alloc(ndev,
+ conn_info->max_pkt_payload_len +
+ NCI_DATA_HDR_SIZE, GFP_KERNEL);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_reserve(skb, NCI_DATA_HDR_SIZE + 1);
+ }
+ } while (i < data_len);
+
+ return i;
+}
+
+static void nci_hci_send_data_req(struct nci_dev *ndev, unsigned long opt)
+{
+ struct nci_data *data = (struct nci_data *)opt;
+
+ nci_hci_send_data(ndev, data->pipe, data->cmd,
+ data->data, data->data_len);
+}
+
+int nci_hci_send_event(struct nci_dev *ndev, u8 gate, u8 event,
+ const u8 *param, size_t param_len)
+{
+ u8 pipe = ndev->hci_dev->gate2pipe[gate];
+
+ if (pipe == NCI_HCI_INVALID_PIPE)
+ return -EADDRNOTAVAIL;
+
+ return nci_hci_send_data(ndev, pipe,
+ NCI_HCP_HEADER(NCI_HCI_HCP_EVENT, event),
+ param, param_len);
+}
+EXPORT_SYMBOL(nci_hci_send_event);
+
+int nci_hci_send_cmd(struct nci_dev *ndev, u8 gate, u8 cmd,
+ const u8 *param, size_t param_len,
+ struct sk_buff **skb)
+{
+ struct nci_hcp_message *message;
+ struct nci_conn_info *conn_info;
+ struct nci_data data;
+ int r;
+ u8 pipe = ndev->hci_dev->gate2pipe[gate];
+
+ if (pipe == NCI_HCI_INVALID_PIPE)
+ return -EADDRNOTAVAIL;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ data.conn_id = conn_info->conn_id;
+ data.pipe = pipe;
+ data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND, cmd);
+ data.data = param;
+ data.data_len = param_len;
+
+ r = nci_request(ndev, nci_hci_send_data_req, (unsigned long)&data,
+ msecs_to_jiffies(NCI_DATA_TIMEOUT));
+ if (r == NCI_STATUS_OK) {
+ message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+ r = nci_hci_result_to_errno(
+ NCI_HCP_MSG_GET_CMD(message->header));
+ skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+ if (!r && skb)
+ *skb = conn_info->rx_skb;
+ }
+
+ return r;
+}
+EXPORT_SYMBOL(nci_hci_send_cmd);
+
+int nci_hci_clear_all_pipes(struct nci_dev *ndev)
+{
+ int r;
+
+ r = nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE,
+ NCI_HCI_ADM_CLEAR_ALL_PIPE, NULL, 0, NULL);
+ if (r < 0)
+ return r;
+
+ nci_hci_reset_pipes(ndev->hci_dev);
+ return r;
+}
+EXPORT_SYMBOL(nci_hci_clear_all_pipes);
+
+static void nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
+ u8 event, struct sk_buff *skb)
+{
+ if (ndev->ops->hci_event_received)
+ ndev->ops->hci_event_received(ndev, pipe, event, skb);
+}
+
+static void nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe,
+ u8 cmd, struct sk_buff *skb)
+{
+ u8 gate = ndev->hci_dev->pipes[pipe].gate;
+ u8 status = NCI_HCI_ANY_OK | ~NCI_HCI_FRAGMENT;
+ u8 dest_gate, new_pipe;
+ struct nci_hci_create_pipe_resp *create_info;
+ struct nci_hci_delete_pipe_noti *delete_info;
+ struct nci_hci_all_pipe_cleared_noti *cleared_info;
+
+ pr_debug("from gate %x pipe %x cmd %x\n", gate, pipe, cmd);
+
+ switch (cmd) {
+ case NCI_HCI_ADM_NOTIFY_PIPE_CREATED:
+ if (skb->len != 5) {
+ status = NCI_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ create_info = (struct nci_hci_create_pipe_resp *)skb->data;
+ dest_gate = create_info->dest_gate;
+ new_pipe = create_info->pipe;
+
+ /* Save the new created pipe and bind with local gate,
+ * the description for skb->data[3] is destination gate id
+ * but since we received this cmd from host controller, we
+ * are the destination and it is our local gate
+ */
+ ndev->hci_dev->gate2pipe[dest_gate] = new_pipe;
+ ndev->hci_dev->pipes[new_pipe].gate = dest_gate;
+ ndev->hci_dev->pipes[new_pipe].host =
+ create_info->src_host;
+ break;
+ case NCI_HCI_ANY_OPEN_PIPE:
+ /* If the pipe is not created report an error */
+ if (gate == NCI_HCI_INVALID_GATE) {
+ status = NCI_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ break;
+ case NCI_HCI_ADM_NOTIFY_PIPE_DELETED:
+ if (skb->len != 1) {
+ status = NCI_HCI_ANY_E_NOK;
+ goto exit;
+ }
+ delete_info = (struct nci_hci_delete_pipe_noti *)skb->data;
+
+ ndev->hci_dev->pipes[delete_info->pipe].gate =
+ NCI_HCI_INVALID_GATE;
+ ndev->hci_dev->pipes[delete_info->pipe].host =
+ NCI_HCI_INVALID_HOST;
+ break;
+ case NCI_HCI_ADM_NOTIFY_ALL_PIPE_CLEARED:
+ if (skb->len != 1) {
+ status = NCI_HCI_ANY_E_NOK;
+ goto exit;
+ }
+
+ cleared_info =
+ (struct nci_hci_all_pipe_cleared_noti *)skb->data;
+ nci_hci_reset_pipes_per_host(ndev, cleared_info->host);
+ break;
+ default:
+ pr_debug("Discarded unknown cmd %x to gate %x\n", cmd, gate);
+ break;
+ }
+
+ if (ndev->ops->hci_cmd_received)
+ ndev->ops->hci_cmd_received(ndev, pipe, cmd, skb);
+
+exit:
+ nci_hci_send_data(ndev, pipe, status, NULL, 0);
+
+ kfree_skb(skb);
+}
+
+static void nci_hci_resp_received(struct nci_dev *ndev, u8 pipe,
+ u8 result, struct sk_buff *skb)
+{
+ struct nci_conn_info *conn_info;
+ u8 status = result;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info) {
+ status = NCI_STATUS_REJECTED;
+ goto exit;
+ }
+
+ conn_info->rx_skb = skb;
+
+exit:
+ nci_req_complete(ndev, NCI_STATUS_OK);
+}
+
+/* Receive hcp message for pipe, with type and cmd.
+ * skb contains optional message data only.
+ */
+static void nci_hci_hcp_message_rx(struct nci_dev *ndev, u8 pipe,
+ u8 type, u8 instruction, struct sk_buff *skb)
+{
+ switch (type) {
+ case NCI_HCI_HCP_RESPONSE:
+ nci_hci_resp_received(ndev, pipe, instruction, skb);
+ break;
+ case NCI_HCI_HCP_COMMAND:
+ nci_hci_cmd_received(ndev, pipe, instruction, skb);
+ break;
+ case NCI_HCI_HCP_EVENT:
+ nci_hci_event_received(ndev, pipe, instruction, skb);
+ break;
+ default:
+ pr_err("UNKNOWN MSG Type %d, instruction=%d\n",
+ type, instruction);
+ kfree_skb(skb);
+ break;
+ }
+
+ nci_req_complete(ndev, NCI_STATUS_OK);
+}
+
+static void nci_hci_msg_rx_work(struct work_struct *work)
+{
+ struct nci_hci_dev *hdev =
+ container_of(work, struct nci_hci_dev, msg_rx_work);
+ struct sk_buff *skb;
+ struct nci_hcp_message *message;
+ u8 pipe, type, instruction;
+
+ while ((skb = skb_dequeue(&hdev->msg_rx_queue)) != NULL) {
+ pipe = NCI_HCP_MSG_GET_PIPE(skb->data[0]);
+ skb_pull(skb, NCI_HCI_HCP_PACKET_HEADER_LEN);
+ message = (struct nci_hcp_message *)skb->data;
+ type = NCI_HCP_MSG_GET_TYPE(message->header);
+ instruction = NCI_HCP_MSG_GET_CMD(message->header);
+ skb_pull(skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+ nci_hci_hcp_message_rx(hdev->ndev, pipe,
+ type, instruction, skb);
+ }
+}
+
+void nci_hci_data_received_cb(void *context,
+ struct sk_buff *skb, int err)
+{
+ struct nci_dev *ndev = (struct nci_dev *)context;
+ struct nci_hcp_packet *packet;
+ u8 pipe, type;
+ struct sk_buff *hcp_skb;
+ struct sk_buff *frag_skb;
+ int msg_len;
+
+ pr_debug("\n");
+
+ if (err) {
+ nci_req_complete(ndev, err);
+ return;
+ }
+
+ packet = (struct nci_hcp_packet *)skb->data;
+ if ((packet->header & ~NCI_HCI_FRAGMENT) == 0) {
+ skb_queue_tail(&ndev->hci_dev->rx_hcp_frags, skb);
+ return;
+ }
+
+ /* it's the last fragment. Does it need re-aggregation? */
+ if (skb_queue_len(&ndev->hci_dev->rx_hcp_frags)) {
+ pipe = NCI_HCP_MSG_GET_PIPE(packet->header);
+ skb_queue_tail(&ndev->hci_dev->rx_hcp_frags, skb);
+
+ msg_len = 0;
+ skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) {
+ msg_len += (frag_skb->len -
+ NCI_HCI_HCP_PACKET_HEADER_LEN);
+ }
+
+ hcp_skb = nfc_alloc_recv_skb(NCI_HCI_HCP_PACKET_HEADER_LEN +
+ msg_len, GFP_KERNEL);
+ if (!hcp_skb) {
+ nci_req_complete(ndev, -ENOMEM);
+ return;
+ }
+
+ skb_put_u8(hcp_skb, pipe);
+
+ skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) {
+ msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN;
+ skb_put_data(hcp_skb,
+ frag_skb->data + NCI_HCI_HCP_PACKET_HEADER_LEN,
+ msg_len);
+ }
+
+ skb_queue_purge(&ndev->hci_dev->rx_hcp_frags);
+ } else {
+ packet->header &= NCI_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 nci_hcp_packet *)hcp_skb->data;
+ type = NCI_HCP_MSG_GET_TYPE(packet->message.header);
+ if (type == NCI_HCI_HCP_RESPONSE) {
+ pipe = NCI_HCP_MSG_GET_PIPE(packet->header);
+ skb_pull(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN);
+ nci_hci_hcp_message_rx(ndev, pipe, type,
+ NCI_STATUS_OK, hcp_skb);
+ } else {
+ skb_queue_tail(&ndev->hci_dev->msg_rx_queue, hcp_skb);
+ schedule_work(&ndev->hci_dev->msg_rx_work);
+ }
+}
+
+int nci_hci_open_pipe(struct nci_dev *ndev, u8 pipe)
+{
+ struct nci_data data;
+ struct nci_conn_info *conn_info;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ data.conn_id = conn_info->conn_id;
+ data.pipe = pipe;
+ data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND,
+ NCI_HCI_ANY_OPEN_PIPE);
+ data.data = NULL;
+ data.data_len = 0;
+
+ return nci_request(ndev, nci_hci_send_data_req,
+ (unsigned long)&data,
+ msecs_to_jiffies(NCI_DATA_TIMEOUT));
+}
+EXPORT_SYMBOL(nci_hci_open_pipe);
+
+static u8 nci_hci_create_pipe(struct nci_dev *ndev, u8 dest_host,
+ u8 dest_gate, int *result)
+{
+ u8 pipe;
+ struct sk_buff *skb;
+ struct nci_hci_create_pipe_params params;
+ struct nci_hci_create_pipe_resp *resp;
+
+ pr_debug("gate=%d\n", dest_gate);
+
+ params.src_gate = NCI_HCI_ADMIN_GATE;
+ params.dest_host = dest_host;
+ params.dest_gate = dest_gate;
+
+ *result = nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE,
+ NCI_HCI_ADM_CREATE_PIPE,
+ (u8 *)¶ms, sizeof(params), &skb);
+ if (*result < 0)
+ return NCI_HCI_INVALID_PIPE;
+
+ resp = (struct nci_hci_create_pipe_resp *)skb->data;
+ pipe = resp->pipe;
+ kfree_skb(skb);
+
+ pr_debug("pipe created=%d\n", pipe);
+
+ return pipe;
+}
+
+static int nci_hci_delete_pipe(struct nci_dev *ndev, u8 pipe)
+{
+ pr_debug("\n");
+
+ return nci_hci_send_cmd(ndev, NCI_HCI_ADMIN_GATE,
+ NCI_HCI_ADM_DELETE_PIPE, &pipe, 1, NULL);
+}
+
+int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx,
+ const u8 *param, size_t param_len)
+{
+ struct nci_hcp_message *message;
+ struct nci_conn_info *conn_info;
+ struct nci_data data;
+ int r;
+ u8 *tmp;
+ u8 pipe = ndev->hci_dev->gate2pipe[gate];
+
+ pr_debug("idx=%d to gate %d\n", idx, gate);
+
+ if (pipe == NCI_HCI_INVALID_PIPE)
+ return -EADDRNOTAVAIL;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ tmp = kmalloc(1 + param_len, GFP_KERNEL);
+ if (!tmp)
+ return -ENOMEM;
+
+ *tmp = idx;
+ memcpy(tmp + 1, param, param_len);
+
+ data.conn_id = conn_info->conn_id;
+ data.pipe = pipe;
+ data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND,
+ NCI_HCI_ANY_SET_PARAMETER);
+ data.data = tmp;
+ data.data_len = param_len + 1;
+
+ r = nci_request(ndev, nci_hci_send_data_req,
+ (unsigned long)&data,
+ msecs_to_jiffies(NCI_DATA_TIMEOUT));
+ if (r == NCI_STATUS_OK) {
+ message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+ r = nci_hci_result_to_errno(
+ NCI_HCP_MSG_GET_CMD(message->header));
+ skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+ }
+
+ kfree(tmp);
+ return r;
+}
+EXPORT_SYMBOL(nci_hci_set_param);
+
+int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx,
+ struct sk_buff **skb)
+{
+ struct nci_hcp_message *message;
+ struct nci_conn_info *conn_info;
+ struct nci_data data;
+ int r;
+ u8 pipe = ndev->hci_dev->gate2pipe[gate];
+
+ pr_debug("idx=%d to gate %d\n", idx, gate);
+
+ if (pipe == NCI_HCI_INVALID_PIPE)
+ return -EADDRNOTAVAIL;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ data.conn_id = conn_info->conn_id;
+ data.pipe = pipe;
+ data.cmd = NCI_HCP_HEADER(NCI_HCI_HCP_COMMAND,
+ NCI_HCI_ANY_GET_PARAMETER);
+ data.data = &idx;
+ data.data_len = 1;
+
+ r = nci_request(ndev, nci_hci_send_data_req, (unsigned long)&data,
+ msecs_to_jiffies(NCI_DATA_TIMEOUT));
+
+ if (r == NCI_STATUS_OK) {
+ message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+ r = nci_hci_result_to_errno(
+ NCI_HCP_MSG_GET_CMD(message->header));
+ skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+ if (!r && skb)
+ *skb = conn_info->rx_skb;
+ }
+
+ return r;
+}
+EXPORT_SYMBOL(nci_hci_get_param);
+
+int nci_hci_connect_gate(struct nci_dev *ndev,
+ u8 dest_host, u8 dest_gate, u8 pipe)
+{
+ bool pipe_created = false;
+ int r;
+
+ if (pipe == NCI_HCI_DO_NOT_OPEN_PIPE)
+ return 0;
+
+ if (ndev->hci_dev->gate2pipe[dest_gate] != NCI_HCI_INVALID_PIPE)
+ return -EADDRINUSE;
+
+ if (pipe != NCI_HCI_INVALID_PIPE)
+ goto open_pipe;
+
+ switch (dest_gate) {
+ case NCI_HCI_LINK_MGMT_GATE:
+ pipe = NCI_HCI_LINK_MGMT_PIPE;
+ break;
+ case NCI_HCI_ADMIN_GATE:
+ pipe = NCI_HCI_ADMIN_PIPE;
+ break;
+ default:
+ pipe = nci_hci_create_pipe(ndev, dest_host, dest_gate, &r);
+ if (pipe == NCI_HCI_INVALID_PIPE)
+ return r;
+ pipe_created = true;
+ break;
+ }
+
+open_pipe:
+ r = nci_hci_open_pipe(ndev, pipe);
+ if (r < 0) {
+ if (pipe_created) {
+ if (nci_hci_delete_pipe(ndev, pipe) < 0) {
+ /* TODO: Cannot clean by deleting pipe...
+ * -> inconsistent state
+ */
+ }
+ }
+ return r;
+ }
+
+ ndev->hci_dev->pipes[pipe].gate = dest_gate;
+ ndev->hci_dev->pipes[pipe].host = dest_host;
+ ndev->hci_dev->gate2pipe[dest_gate] = pipe;
+
+ return 0;
+}
+EXPORT_SYMBOL(nci_hci_connect_gate);
+
+static int nci_hci_dev_connect_gates(struct nci_dev *ndev,
+ u8 gate_count,
+ struct nci_hci_gate *gates)
+{
+ int r;
+
+ while (gate_count--) {
+ r = nci_hci_connect_gate(ndev, gates->dest_host,
+ gates->gate, gates->pipe);
+ if (r < 0)
+ return r;
+ gates++;
+ }
+
+ return 0;
+}
+
+int nci_hci_dev_session_init(struct nci_dev *ndev)
+{
+ struct nci_conn_info *conn_info;
+ struct sk_buff *skb;
+ int r;
+
+ ndev->hci_dev->count_pipes = 0;
+ ndev->hci_dev->expected_pipes = 0;
+
+ conn_info = ndev->hci_dev->conn_info;
+ if (!conn_info)
+ return -EPROTO;
+
+ conn_info->data_exchange_cb = nci_hci_data_received_cb;
+ conn_info->data_exchange_cb_context = ndev;
+
+ nci_hci_reset_pipes(ndev->hci_dev);
+
+ if (ndev->hci_dev->init_data.gates[0].gate != NCI_HCI_ADMIN_GATE)
+ return -EPROTO;
+
+ r = nci_hci_connect_gate(ndev,
+ ndev->hci_dev->init_data.gates[0].dest_host,
+ ndev->hci_dev->init_data.gates[0].gate,
+ ndev->hci_dev->init_data.gates[0].pipe);
+ if (r < 0)
+ return r;
+
+ r = nci_hci_get_param(ndev, NCI_HCI_ADMIN_GATE,
+ NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY, &skb);
+ if (r < 0)
+ return r;
+
+ if (skb->len &&
+ skb->len == strlen(ndev->hci_dev->init_data.session_id) &&
+ !memcmp(ndev->hci_dev->init_data.session_id, skb->data, skb->len) &&
+ ndev->ops->hci_load_session) {
+ /* Restore gate<->pipe table from some proprietary location. */
+ r = ndev->ops->hci_load_session(ndev);
+ } else {
+ r = nci_hci_clear_all_pipes(ndev);
+ if (r < 0)
+ goto exit;
+
+ r = nci_hci_dev_connect_gates(ndev,
+ ndev->hci_dev->init_data.gate_count,
+ ndev->hci_dev->init_data.gates);
+ if (r < 0)
+ goto exit;
+
+ r = nci_hci_set_param(ndev, NCI_HCI_ADMIN_GATE,
+ NCI_HCI_ADMIN_PARAM_SESSION_IDENTITY,
+ ndev->hci_dev->init_data.session_id,
+ strlen(ndev->hci_dev->init_data.session_id));
+ }
+
+exit:
+ kfree_skb(skb);
+
+ return r;
+}
+EXPORT_SYMBOL(nci_hci_dev_session_init);
+
+struct nci_hci_dev *nci_hci_allocate(struct nci_dev *ndev)
+{
+ struct nci_hci_dev *hdev;
+
+ hdev = kzalloc(sizeof(*hdev), GFP_KERNEL);
+ if (!hdev)
+ return NULL;
+
+ skb_queue_head_init(&hdev->rx_hcp_frags);
+ INIT_WORK(&hdev->msg_rx_work, nci_hci_msg_rx_work);
+ skb_queue_head_init(&hdev->msg_rx_queue);
+ hdev->ndev = ndev;
+
+ return hdev;
+}
diff --git a/net/nfc/nci/lib.c b/net/nfc/nci/lib.c
new file mode 100644
index 0000000..ed774a2
--- /dev/null
+++ b/net/nfc/nci/lib.c
@@ -0,0 +1,85 @@
+/*
+ * The NFC Controller Interface is the communication protocol between an
+ * NFC Controller (NFCC) and a Device Host (DH).
+ *
+ * Copyright (C) 2011 Texas Instruments, Inc.
+ *
+ * Written by Ilan Elias <ilane@ti.com>
+ *
+ * Acknowledgements:
+ * This file is based on lib.c, which was written
+ * by Maxim Krasnyansky.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, 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);
diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c
new file mode 100644
index 0000000..1e8c1a1
--- /dev/null
+++ b/net/nfc/nci/ntf.c
@@ -0,0 +1,811 @@
+/*
+ * 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;
+ struct nci_conn_info *conn_info;
+ 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);
+
+ conn_info = nci_get_conn_info_by_conn_id(ndev,
+ ntf->conn_entries[i].conn_id);
+ if (!conn_info)
+ return;
+
+ atomic_add(ntf->conn_entries[i].credits,
+ &conn_info->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, ntf->conn_id, -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(*((__le16 *)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;
+}
+
+static __u8 *nci_extract_rf_params_nfcf_passive_listen(struct nci_dev *ndev,
+ struct rf_tech_specific_params_nfcf_listen *nfcf_listen,
+ __u8 *data)
+{
+ nfcf_listen->local_nfcid2_len = min_t(__u8, *data++,
+ NFC_NFCID2_MAXSIZE);
+ memcpy(nfcf_listen->local_nfcid2, data, nfcf_listen->local_nfcid2_len);
+ data += nfcf_listen->local_nfcid2_len;
+
+ return data;
+}
+
+static __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;
+ struct activation_params_listen_nfc_dep *listen;
+
+ 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++,
+ NFC_ATR_RES_MAXSIZE - 2);
+ 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;
+
+ case NCI_NFC_A_PASSIVE_LISTEN_MODE:
+ case NCI_NFC_F_PASSIVE_LISTEN_MODE:
+ listen = &ntf->activation_params.listen_nfc_dep;
+ listen->atr_req_len = min_t(__u8, *data++,
+ NFC_ATR_REQ_MAXSIZE - 2);
+ pr_debug("atr_req_len %d\n", listen->atr_req_len);
+ if (listen->atr_req_len > 0)
+ memcpy(listen->atr_req, data, listen->atr_req_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 int nci_store_general_bytes_nfc_dep(struct nci_dev *ndev,
+ struct nci_rf_intf_activated_ntf *ntf)
+{
+ ndev->remote_gb_len = 0;
+
+ if (ntf->activation_params_len <= 0)
+ return NCI_STATUS_OK;
+
+ switch (ntf->activation_rf_tech_and_mode) {
+ case NCI_NFC_A_PASSIVE_POLL_MODE:
+ case NCI_NFC_F_PASSIVE_POLL_MODE:
+ ndev->remote_gb_len = min_t(__u8,
+ (ntf->activation_params.poll_nfc_dep.atr_res_len
+ - NFC_ATR_RES_GT_OFFSET),
+ NFC_ATR_RES_GB_MAXSIZE);
+ memcpy(ndev->remote_gb,
+ (ntf->activation_params.poll_nfc_dep.atr_res
+ + NFC_ATR_RES_GT_OFFSET),
+ ndev->remote_gb_len);
+ break;
+
+ case NCI_NFC_A_PASSIVE_LISTEN_MODE:
+ case NCI_NFC_F_PASSIVE_LISTEN_MODE:
+ ndev->remote_gb_len = min_t(__u8,
+ (ntf->activation_params.listen_nfc_dep.atr_req_len
+ - NFC_ATR_REQ_GT_OFFSET),
+ NFC_ATR_REQ_GB_MAXSIZE);
+ memcpy(ndev->remote_gb,
+ (ntf->activation_params.listen_nfc_dep.atr_req
+ + NFC_ATR_REQ_GT_OFFSET),
+ ndev->remote_gb_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_rf_intf_activated_ntf_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ struct nci_conn_info *conn_info;
+ 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 this contains a value of 0x00 (NFCEE Direct RF
+ * Interface) then all following parameters SHALL contain a
+ * value of 0 and SHALL be ignored.
+ */
+ if (ntf.rf_interface == NCI_RF_INTERFACE_NFCEE_DIRECT)
+ goto listen;
+
+ 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;
+
+ case NCI_NFC_A_PASSIVE_LISTEN_MODE:
+ /* no RF technology specific parameters */
+ break;
+
+ case NCI_NFC_F_PASSIVE_LISTEN_MODE:
+ data = nci_extract_rf_params_nfcf_passive_listen(ndev,
+ &(ntf.rf_tech_specific_params.nfcf_listen),
+ 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) {
+ conn_info = ndev->rf_conn_info;
+ if (!conn_info)
+ return;
+
+ conn_info->max_pkt_payload_len = ntf.max_data_pkt_payload_size;
+ conn_info->initial_num_credits = ntf.initial_num_credits;
+
+ /* set the available credits to initial value */
+ atomic_set(&conn_info->credits_cnt,
+ conn_info->initial_num_credits);
+
+ /* store general bytes to be reported later in dep_link_up */
+ if (ntf.rf_interface == NCI_RF_INTERFACE_NFC_DEP) {
+ err = nci_store_general_bytes_nfc_dep(ndev, &ntf);
+ if (err != NCI_STATUS_OK)
+ pr_err("unable to store general bytes\n");
+ }
+ }
+
+ if (!(ntf.activation_rf_tech_and_mode & NCI_RF_TECH_MODE_LISTEN_MASK)) {
+ /* Poll mode */
+ 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);
+ }
+ } else {
+listen:
+ /* Listen mode */
+ atomic_set(&ndev->state, NCI_LISTEN_ACTIVE);
+ if (err == NCI_STATUS_OK &&
+ ntf.rf_protocol == NCI_RF_PROTOCOL_NFC_DEP) {
+ err = nfc_tm_activated(ndev->nfc_dev,
+ NFC_PROTO_NFC_DEP_MASK,
+ NFC_COMM_PASSIVE,
+ ndev->remote_gb,
+ ndev->remote_gb_len);
+ if (err != NCI_STATUS_OK)
+ pr_err("error when signaling tm activation\n");
+ }
+ }
+}
+
+static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ struct nci_conn_info *conn_info;
+ struct nci_rf_deactivate_ntf *ntf = (void *) skb->data;
+
+ pr_debug("entry, type 0x%x, reason 0x%x\n", ntf->type, ntf->reason);
+
+ conn_info = ndev->rf_conn_info;
+ if (!conn_info)
+ return;
+
+ /* 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, NCI_STATIC_RF_CONN_ID,
+ -EIO);
+
+ switch (ntf->type) {
+ case NCI_DEACTIVATE_TYPE_IDLE_MODE:
+ nci_clear_target_list(ndev);
+ atomic_set(&ndev->state, NCI_IDLE);
+ break;
+ case NCI_DEACTIVATE_TYPE_SLEEP_MODE:
+ case NCI_DEACTIVATE_TYPE_SLEEP_AF_MODE:
+ atomic_set(&ndev->state, NCI_W4_HOST_SELECT);
+ break;
+ case NCI_DEACTIVATE_TYPE_DISCOVERY:
+ nci_clear_target_list(ndev);
+ atomic_set(&ndev->state, NCI_DISCOVERY);
+ break;
+ }
+
+ nci_req_complete(ndev, NCI_STATUS_OK);
+}
+
+static void nci_nfcee_discover_ntf_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ u8 status = NCI_STATUS_OK;
+ struct nci_nfcee_discover_ntf *nfcee_ntf =
+ (struct nci_nfcee_discover_ntf *)skb->data;
+
+ pr_debug("\n");
+
+ /* NFCForum NCI 9.2.1 HCI Network Specific Handling
+ * If the NFCC supports the HCI Network, it SHALL return one,
+ * and only one, NFCEE_DISCOVER_NTF with a Protocol type of
+ * “HCI Access”, even if the HCI Network contains multiple NFCEEs.
+ */
+ ndev->hci_dev->nfcee_id = nfcee_ntf->nfcee_id;
+ ndev->cur_params.id = nfcee_ntf->nfcee_id;
+
+ nci_req_complete(ndev, status);
+}
+
+static void nci_nfcee_action_ntf_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ pr_debug("\n");
+}
+
+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);
+
+ if (nci_opcode_gid(ntf_opcode) == NCI_GID_PROPRIETARY) {
+ if (nci_prop_ntf_packet(ndev, ntf_opcode, skb) == -ENOTSUPP) {
+ pr_err("unsupported ntf opcode 0x%x\n",
+ ntf_opcode);
+ }
+
+ goto end;
+ }
+
+ 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;
+
+ case NCI_OP_NFCEE_DISCOVER_NTF:
+ nci_nfcee_discover_ntf_packet(ndev, skb);
+ break;
+
+ case NCI_OP_RF_NFCEE_ACTION_NTF:
+ nci_nfcee_action_ntf_packet(ndev, skb);
+ break;
+
+ default:
+ pr_err("unknown ntf opcode 0x%x\n", ntf_opcode);
+ break;
+ }
+
+ nci_core_ntf_packet(ndev, ntf_opcode, skb);
+end:
+ kfree_skb(skb);
+}
diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c
new file mode 100644
index 0000000..e3bbf19
--- /dev/null
+++ b/net/nfc/nci/rsp.c
@@ -0,0 +1,381 @@
+/*
+ * 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)
+{
+ struct nci_conn_info *conn_info;
+ __u8 status = skb->data[0];
+
+ pr_debug("status 0x%x\n", status);
+
+ if (status == NCI_STATUS_OK) {
+ atomic_set(&ndev->state, NCI_DISCOVERY);
+
+ conn_info = ndev->rf_conn_info;
+ if (!conn_info) {
+ conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
+ sizeof(struct nci_conn_info),
+ GFP_KERNEL);
+ if (!conn_info) {
+ status = NCI_STATUS_REJECTED;
+ goto exit;
+ }
+ conn_info->conn_id = NCI_STATIC_RF_CONN_ID;
+ INIT_LIST_HEAD(&conn_info->list);
+ list_add(&conn_info->list, &ndev->conn_info_list);
+ ndev->rf_conn_info = conn_info;
+ }
+ }
+
+exit:
+ 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);
+ }
+}
+
+static void nci_nfcee_discover_rsp_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ struct nci_nfcee_discover_rsp *discover_rsp;
+
+ if (skb->len != 2) {
+ nci_req_complete(ndev, NCI_STATUS_NFCEE_PROTOCOL_ERROR);
+ return;
+ }
+
+ discover_rsp = (struct nci_nfcee_discover_rsp *)skb->data;
+
+ if (discover_rsp->status != NCI_STATUS_OK ||
+ discover_rsp->num_nfcee == 0)
+ nci_req_complete(ndev, discover_rsp->status);
+}
+
+static void nci_nfcee_mode_set_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_core_conn_create_rsp_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ __u8 status = skb->data[0];
+ struct nci_conn_info *conn_info = NULL;
+ struct nci_core_conn_create_rsp *rsp;
+
+ pr_debug("status 0x%x\n", status);
+
+ if (status == NCI_STATUS_OK) {
+ rsp = (struct nci_core_conn_create_rsp *)skb->data;
+
+ conn_info = devm_kzalloc(&ndev->nfc_dev->dev,
+ sizeof(*conn_info), GFP_KERNEL);
+ if (!conn_info) {
+ status = NCI_STATUS_REJECTED;
+ goto exit;
+ }
+
+ conn_info->dest_params = devm_kzalloc(&ndev->nfc_dev->dev,
+ sizeof(struct dest_spec_params),
+ GFP_KERNEL);
+ if (!conn_info->dest_params) {
+ status = NCI_STATUS_REJECTED;
+ goto free_conn_info;
+ }
+
+ conn_info->dest_type = ndev->cur_dest_type;
+ conn_info->dest_params->id = ndev->cur_params.id;
+ conn_info->dest_params->protocol = ndev->cur_params.protocol;
+ conn_info->conn_id = rsp->conn_id;
+
+ /* Note: data_exchange_cb and data_exchange_cb_context need to
+ * be specify out of nci_core_conn_create_rsp_packet
+ */
+
+ INIT_LIST_HEAD(&conn_info->list);
+ list_add(&conn_info->list, &ndev->conn_info_list);
+
+ if (ndev->cur_params.id == ndev->hci_dev->nfcee_id)
+ ndev->hci_dev->conn_info = conn_info;
+
+ conn_info->conn_id = rsp->conn_id;
+ conn_info->max_pkt_payload_len = rsp->max_ctrl_pkt_payload_len;
+ atomic_set(&conn_info->credits_cnt, rsp->credits_cnt);
+ }
+
+free_conn_info:
+ if (status == NCI_STATUS_REJECTED)
+ devm_kfree(&ndev->nfc_dev->dev, conn_info);
+exit:
+
+ nci_req_complete(ndev, status);
+}
+
+static void nci_core_conn_close_rsp_packet(struct nci_dev *ndev,
+ struct sk_buff *skb)
+{
+ struct nci_conn_info *conn_info;
+ __u8 status = skb->data[0];
+
+ pr_debug("status 0x%x\n", status);
+ if (status == NCI_STATUS_OK) {
+ conn_info = nci_get_conn_info_by_conn_id(ndev,
+ ndev->cur_conn_id);
+ if (conn_info) {
+ list_del(&conn_info->list);
+ devm_kfree(&ndev->nfc_dev->dev, conn_info);
+ }
+ }
+ 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);
+
+ if (nci_opcode_gid(rsp_opcode) == NCI_GID_PROPRIETARY) {
+ if (nci_prop_rsp_packet(ndev, rsp_opcode, skb) == -ENOTSUPP) {
+ pr_err("unsupported rsp opcode 0x%x\n",
+ rsp_opcode);
+ }
+
+ goto end;
+ }
+
+ 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_CORE_CONN_CREATE_RSP:
+ nci_core_conn_create_rsp_packet(ndev, skb);
+ break;
+
+ case NCI_OP_CORE_CONN_CLOSE_RSP:
+ nci_core_conn_close_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;
+
+ case NCI_OP_NFCEE_DISCOVER_RSP:
+ nci_nfcee_discover_rsp_packet(ndev, skb);
+ break;
+
+ case NCI_OP_NFCEE_MODE_SET_RSP:
+ nci_nfcee_mode_set_rsp_packet(ndev, skb);
+ break;
+
+ default:
+ pr_err("unknown rsp opcode 0x%x\n", rsp_opcode);
+ break;
+ }
+
+ nci_core_rsp_packet(ndev, rsp_opcode, skb);
+end:
+ kfree_skb(skb);
+
+ /* trigger the next cmd */
+ atomic_set(&ndev->cmd_cnt, 1);
+ if (!skb_queue_empty(&ndev->cmd_q))
+ queue_work(ndev->cmd_wq, &ndev->cmd_work);
+}
diff --git a/net/nfc/nci/spi.c b/net/nfc/nci/spi.c
new file mode 100644
index 0000000..452f4c1
--- /dev/null
+++ b/net/nfc/nci/spi.c
@@ -0,0 +1,331 @@
+/*
+ * 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/module.h>
+
+#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;
+ t.speed_hz = nspi->xfer_speed_hz;
+
+ 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_u8(skb, crc >> 8);
+ skb_put_u8(skb, 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;
+ /* Use controller max SPI speed by default */
+ nspi->xfer_speed_hz = 0;
+ 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_u8(skb, crc >> 8);
+ skb_put_u8(skb, 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;
+ tx.speed_hz = nspi->xfer_speed_hz;
+ spi_message_add_tail(&tx, &m);
+
+ memset(&rx, 0, sizeof(struct spi_transfer));
+ rx.rx_buf = resp_hdr;
+ rx.len = 2;
+ rx.cs_change = 1;
+ rx.speed_hz = nspi->xfer_speed_hz;
+ spi_message_add_tail(&rx, &m);
+
+ ret = spi_sync(nspi->spi, &m);
+ 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;
+ rx.speed_hz = nspi->xfer_speed_hz;
+ 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) {
+ *(u8 *)skb_push(skb, 1) = resp_hdr[1];
+ *(u8 *)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);
+
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/nci/uart.c b/net/nfc/nci/uart.c
new file mode 100644
index 0000000..a66f102
--- /dev/null
+++ b/net/nfc/nci/uart.c
@@ -0,0 +1,487 @@
+/*
+ * Copyright (C) 2015, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License"). You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available on the worldwide web at
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED. The License provides additional details about
+ * this warranty disclaimer.
+ */
+
+/* Inspired (hugely) by HCI LDISC implementation in Bluetooth.
+ *
+ * Copyright (C) 2000-2001 Qualcomm Incorporated
+ * Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com>
+ * Copyright (C) 2004-2005 Marcel Holtmann <marcel@holtmann.org>
+ */
+
+#include <linux/module.h>
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fcntl.h>
+#include <linux/interrupt.h>
+#include <linux/ptrace.h>
+#include <linux/poll.h>
+
+#include <linux/slab.h>
+#include <linux/tty.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/signal.h>
+#include <linux/ioctl.h>
+#include <linux/skbuff.h>
+
+#include <net/nfc/nci.h>
+#include <net/nfc/nci_core.h>
+
+/* TX states */
+#define NCI_UART_SENDING 1
+#define NCI_UART_TX_WAKEUP 2
+
+static struct nci_uart *nci_uart_drivers[NCI_UART_DRIVER_MAX];
+
+static inline struct sk_buff *nci_uart_dequeue(struct nci_uart *nu)
+{
+ struct sk_buff *skb = nu->tx_skb;
+
+ if (!skb)
+ skb = skb_dequeue(&nu->tx_q);
+ else
+ nu->tx_skb = NULL;
+
+ return skb;
+}
+
+static inline int nci_uart_queue_empty(struct nci_uart *nu)
+{
+ if (nu->tx_skb)
+ return 0;
+
+ return skb_queue_empty(&nu->tx_q);
+}
+
+static int nci_uart_tx_wakeup(struct nci_uart *nu)
+{
+ if (test_and_set_bit(NCI_UART_SENDING, &nu->tx_state)) {
+ set_bit(NCI_UART_TX_WAKEUP, &nu->tx_state);
+ return 0;
+ }
+
+ schedule_work(&nu->write_work);
+
+ return 0;
+}
+
+static void nci_uart_write_work(struct work_struct *work)
+{
+ struct nci_uart *nu = container_of(work, struct nci_uart, write_work);
+ struct tty_struct *tty = nu->tty;
+ struct sk_buff *skb;
+
+restart:
+ clear_bit(NCI_UART_TX_WAKEUP, &nu->tx_state);
+
+ if (nu->ops.tx_start)
+ nu->ops.tx_start(nu);
+
+ while ((skb = nci_uart_dequeue(nu))) {
+ int len;
+
+ set_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+ len = tty->ops->write(tty, skb->data, skb->len);
+ skb_pull(skb, len);
+ if (skb->len) {
+ nu->tx_skb = skb;
+ break;
+ }
+ kfree_skb(skb);
+ }
+
+ if (test_bit(NCI_UART_TX_WAKEUP, &nu->tx_state))
+ goto restart;
+
+ if (nu->ops.tx_done && nci_uart_queue_empty(nu))
+ nu->ops.tx_done(nu);
+
+ clear_bit(NCI_UART_SENDING, &nu->tx_state);
+}
+
+static int nci_uart_set_driver(struct tty_struct *tty, unsigned int driver)
+{
+ struct nci_uart *nu = NULL;
+ int ret;
+
+ if (driver >= NCI_UART_DRIVER_MAX)
+ return -EINVAL;
+
+ if (!nci_uart_drivers[driver])
+ return -ENOENT;
+
+ nu = kzalloc(sizeof(*nu), GFP_KERNEL);
+ if (!nu)
+ return -ENOMEM;
+
+ memcpy(nu, nci_uart_drivers[driver], sizeof(struct nci_uart));
+ nu->tty = tty;
+ tty->disc_data = nu;
+ skb_queue_head_init(&nu->tx_q);
+ INIT_WORK(&nu->write_work, nci_uart_write_work);
+ spin_lock_init(&nu->rx_lock);
+
+ ret = nu->ops.open(nu);
+ if (ret) {
+ tty->disc_data = NULL;
+ kfree(nu);
+ } else if (!try_module_get(nu->owner)) {
+ nu->ops.close(nu);
+ tty->disc_data = NULL;
+ kfree(nu);
+ return -ENOENT;
+ }
+ return ret;
+}
+
+/* ------ LDISC part ------ */
+
+/* nci_uart_tty_open
+ *
+ * Called when line discipline changed to NCI_UART.
+ *
+ * Arguments:
+ * tty pointer to tty info structure
+ * Return Value:
+ * 0 if success, otherwise error code
+ */
+static int nci_uart_tty_open(struct tty_struct *tty)
+{
+ /* Error if the tty has no write op instead of leaving an exploitable
+ * hole
+ */
+ if (!tty->ops->write)
+ return -EOPNOTSUPP;
+
+ tty->disc_data = NULL;
+ tty->receive_room = 65536;
+
+ /* Flush any pending characters in the driver */
+ tty_driver_flush_buffer(tty);
+
+ return 0;
+}
+
+/* nci_uart_tty_close()
+ *
+ * Called when the line discipline is changed to something
+ * else, the tty is closed, or the tty detects a hangup.
+ */
+static void nci_uart_tty_close(struct tty_struct *tty)
+{
+ struct nci_uart *nu = (void *)tty->disc_data;
+
+ /* Detach from the tty */
+ tty->disc_data = NULL;
+
+ if (!nu)
+ return;
+
+ if (nu->tx_skb)
+ kfree_skb(nu->tx_skb);
+ if (nu->rx_skb)
+ kfree_skb(nu->rx_skb);
+
+ skb_queue_purge(&nu->tx_q);
+
+ nu->ops.close(nu);
+ nu->tty = NULL;
+ module_put(nu->owner);
+
+ cancel_work_sync(&nu->write_work);
+
+ kfree(nu);
+}
+
+/* nci_uart_tty_wakeup()
+ *
+ * Callback for transmit wakeup. Called when low level
+ * device driver can accept more send data.
+ *
+ * Arguments: tty pointer to associated tty instance data
+ * Return Value: None
+ */
+static void nci_uart_tty_wakeup(struct tty_struct *tty)
+{
+ struct nci_uart *nu = (void *)tty->disc_data;
+
+ if (!nu)
+ return;
+
+ clear_bit(TTY_DO_WRITE_WAKEUP, &tty->flags);
+
+ if (tty != nu->tty)
+ return;
+
+ nci_uart_tx_wakeup(nu);
+}
+
+/* nci_uart_tty_receive()
+ *
+ * Called by tty low level driver when receive data is
+ * available.
+ *
+ * Arguments: tty pointer to tty isntance data
+ * data pointer to received data
+ * flags pointer to flags for data
+ * count count of received data in bytes
+ *
+ * Return Value: None
+ */
+static void nci_uart_tty_receive(struct tty_struct *tty, const u8 *data,
+ char *flags, int count)
+{
+ struct nci_uart *nu = (void *)tty->disc_data;
+
+ if (!nu || tty != nu->tty)
+ return;
+
+ spin_lock(&nu->rx_lock);
+ nu->ops.recv_buf(nu, (void *)data, flags, count);
+ spin_unlock(&nu->rx_lock);
+
+ tty_unthrottle(tty);
+}
+
+/* nci_uart_tty_ioctl()
+ *
+ * Process IOCTL system call for the tty device.
+ *
+ * Arguments:
+ *
+ * tty pointer to tty instance data
+ * file pointer to open file object for device
+ * cmd IOCTL command code
+ * arg argument for IOCTL call (cmd dependent)
+ *
+ * Return Value: Command dependent
+ */
+static int nci_uart_tty_ioctl(struct tty_struct *tty, struct file *file,
+ unsigned int cmd, unsigned long arg)
+{
+ struct nci_uart *nu = (void *)tty->disc_data;
+ int err = 0;
+
+ switch (cmd) {
+ case NCIUARTSETDRIVER:
+ if (!nu)
+ return nci_uart_set_driver(tty, (unsigned int)arg);
+ else
+ return -EBUSY;
+ break;
+ default:
+ err = n_tty_ioctl_helper(tty, file, cmd, arg);
+ break;
+ }
+
+ return err;
+}
+
+/* We don't provide read/write/poll interface for user space. */
+static ssize_t nci_uart_tty_read(struct tty_struct *tty, struct file *file,
+ unsigned char __user *buf, size_t nr)
+{
+ return 0;
+}
+
+static ssize_t nci_uart_tty_write(struct tty_struct *tty, struct file *file,
+ const unsigned char *data, size_t count)
+{
+ return 0;
+}
+
+static __poll_t nci_uart_tty_poll(struct tty_struct *tty,
+ struct file *filp, poll_table *wait)
+{
+ return 0;
+}
+
+static int nci_uart_send(struct nci_uart *nu, struct sk_buff *skb)
+{
+ /* Queue TX packet */
+ skb_queue_tail(&nu->tx_q, skb);
+
+ /* Try to start TX (if possible) */
+ nci_uart_tx_wakeup(nu);
+
+ return 0;
+}
+
+/* -- Default recv_buf handler --
+ *
+ * This handler supposes that NCI frames are sent over UART link without any
+ * framing. It reads NCI header, retrieve the packet size and once all packet
+ * bytes are received it passes it to nci_uart driver for processing.
+ */
+static int nci_uart_default_recv_buf(struct nci_uart *nu, const u8 *data,
+ char *flags, int count)
+{
+ int chunk_len;
+
+ if (!nu->ndev) {
+ nfc_err(nu->tty->dev,
+ "receive data from tty but no NCI dev is attached yet, drop buffer\n");
+ return 0;
+ }
+
+ /* Decode all incoming data in packets
+ * and enqueue then for processing.
+ */
+ while (count > 0) {
+ /* If this is the first data of a packet, allocate a buffer */
+ if (!nu->rx_skb) {
+ nu->rx_packet_len = -1;
+ nu->rx_skb = nci_skb_alloc(nu->ndev,
+ NCI_MAX_PACKET_SIZE,
+ GFP_KERNEL);
+ if (!nu->rx_skb)
+ return -ENOMEM;
+ }
+
+ /* Eat byte after byte till full packet header is received */
+ if (nu->rx_skb->len < NCI_CTRL_HDR_SIZE) {
+ skb_put_u8(nu->rx_skb, *data++);
+ --count;
+ continue;
+ }
+
+ /* Header was received but packet len was not read */
+ if (nu->rx_packet_len < 0)
+ nu->rx_packet_len = NCI_CTRL_HDR_SIZE +
+ nci_plen(nu->rx_skb->data);
+
+ /* Compute how many bytes are missing and how many bytes can
+ * be consumed.
+ */
+ chunk_len = nu->rx_packet_len - nu->rx_skb->len;
+ if (count < chunk_len)
+ chunk_len = count;
+ skb_put_data(nu->rx_skb, data, chunk_len);
+ data += chunk_len;
+ count -= chunk_len;
+
+ /* Chcek if packet is fully received */
+ if (nu->rx_packet_len == nu->rx_skb->len) {
+ /* Pass RX packet to driver */
+ if (nu->ops.recv(nu, nu->rx_skb) != 0)
+ nfc_err(nu->tty->dev, "corrupted RX packet\n");
+ /* Next packet will be a new one */
+ nu->rx_skb = NULL;
+ }
+ }
+
+ return 0;
+}
+
+/* -- Default recv handler -- */
+static int nci_uart_default_recv(struct nci_uart *nu, struct sk_buff *skb)
+{
+ return nci_recv_frame(nu->ndev, skb);
+}
+
+int nci_uart_register(struct nci_uart *nu)
+{
+ if (!nu || !nu->ops.open ||
+ !nu->ops.recv || !nu->ops.close)
+ return -EINVAL;
+
+ /* Set the send callback */
+ nu->ops.send = nci_uart_send;
+
+ /* Install default handlers if not overridden */
+ if (!nu->ops.recv_buf)
+ nu->ops.recv_buf = nci_uart_default_recv_buf;
+ if (!nu->ops.recv)
+ nu->ops.recv = nci_uart_default_recv;
+
+ /* Add this driver in the driver list */
+ if (nci_uart_drivers[nu->driver]) {
+ pr_err("driver %d is already registered\n", nu->driver);
+ return -EBUSY;
+ }
+ nci_uart_drivers[nu->driver] = nu;
+
+ pr_info("NCI uart driver '%s [%d]' registered\n", nu->name, nu->driver);
+
+ return 0;
+}
+EXPORT_SYMBOL_GPL(nci_uart_register);
+
+void nci_uart_unregister(struct nci_uart *nu)
+{
+ pr_info("NCI uart driver '%s [%d]' unregistered\n", nu->name,
+ nu->driver);
+
+ /* Remove this driver from the driver list */
+ nci_uart_drivers[nu->driver] = NULL;
+}
+EXPORT_SYMBOL_GPL(nci_uart_unregister);
+
+void nci_uart_set_config(struct nci_uart *nu, int baudrate, int flow_ctrl)
+{
+ struct ktermios new_termios;
+
+ if (!nu->tty)
+ return;
+
+ down_read(&nu->tty->termios_rwsem);
+ new_termios = nu->tty->termios;
+ up_read(&nu->tty->termios_rwsem);
+ tty_termios_encode_baud_rate(&new_termios, baudrate, baudrate);
+
+ if (flow_ctrl)
+ new_termios.c_cflag |= CRTSCTS;
+ else
+ new_termios.c_cflag &= ~CRTSCTS;
+
+ tty_set_termios(nu->tty, &new_termios);
+}
+EXPORT_SYMBOL_GPL(nci_uart_set_config);
+
+static struct tty_ldisc_ops nci_uart_ldisc = {
+ .magic = TTY_LDISC_MAGIC,
+ .owner = THIS_MODULE,
+ .name = "n_nci",
+ .open = nci_uart_tty_open,
+ .close = nci_uart_tty_close,
+ .read = nci_uart_tty_read,
+ .write = nci_uart_tty_write,
+ .poll = nci_uart_tty_poll,
+ .receive_buf = nci_uart_tty_receive,
+ .write_wakeup = nci_uart_tty_wakeup,
+ .ioctl = nci_uart_tty_ioctl,
+};
+
+static int __init nci_uart_init(void)
+{
+ memset(nci_uart_drivers, 0, sizeof(nci_uart_drivers));
+ return tty_register_ldisc(N_NCI, &nci_uart_ldisc);
+}
+
+static void __exit nci_uart_exit(void)
+{
+ tty_unregister_ldisc(N_NCI);
+}
+
+module_init(nci_uart_init);
+module_exit(nci_uart_exit);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("NFC NCI UART driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS_LDISC(N_NCI);
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
new file mode 100644
index 0000000..376181c
--- /dev/null
+++ b/net/nfc/netlink.c
@@ -0,0 +1,1884 @@
+/*
+ * Copyright (C) 2011 Instituto Nokia de Tecnologia
+ *
+ * Authors:
+ * Lauro Ramos Venancio <lauro.venancio@openbossa.org>
+ * Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
+ *
+ * Vendor commands implementation based on net/wireless/nl80211.c
+ * which is:
+ *
+ * Copyright 2006-2010 Johannes Berg <johannes@sipsolutions.net>
+ * Copyright 2013-2014 Intel Mobile Communications GmbH
+ *
+ * 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/genetlink.h>
+#include <linux/nfc.h>
+#include <linux/slab.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static const struct genl_multicast_group nfc_genl_mcgrps[] = {
+ { .name = NFC_GENL_MCAST_EVENT_NAME, },
+};
+
+static struct genl_family nfc_genl_family;
+static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = {
+ [NFC_ATTR_DEVICE_INDEX] = { .type = NLA_U32 },
+ [NFC_ATTR_DEVICE_NAME] = { .type = NLA_STRING,
+ .len = NFC_DEVICE_NAME_MAXSIZE },
+ [NFC_ATTR_PROTOCOLS] = { .type = NLA_U32 },
+ [NFC_ATTR_COMM_MODE] = { .type = NLA_U8 },
+ [NFC_ATTR_RF_MODE] = { .type = NLA_U8 },
+ [NFC_ATTR_DEVICE_POWERED] = { .type = NLA_U8 },
+ [NFC_ATTR_IM_PROTOCOLS] = { .type = NLA_U32 },
+ [NFC_ATTR_TM_PROTOCOLS] = { .type = NLA_U32 },
+ [NFC_ATTR_LLC_PARAM_LTO] = { .type = NLA_U8 },
+ [NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 },
+ [NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 },
+ [NFC_ATTR_LLC_SDP] = { .type = NLA_NESTED },
+ [NFC_ATTR_FIRMWARE_NAME] = { .type = NLA_STRING,
+ .len = NFC_FIRMWARE_NAME_MAXSIZE },
+ [NFC_ATTR_SE_APDU] = { .type = NLA_BINARY },
+ [NFC_ATTR_VENDOR_DATA] = { .type = NLA_BINARY },
+
+};
+
+static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = {
+ [NFC_SDP_ATTR_URI] = { .type = NLA_STRING,
+ .len = U8_MAX - 4 },
+ [NFC_SDP_ATTR_SAP] = { .type = NLA_U8 },
+};
+
+static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target,
+ struct netlink_callback *cb, int flags)
+{
+ void *hdr;
+
+ hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).portid, cb->nlh->nlmsg_seq,
+ &nfc_genl_family, flags, NFC_CMD_GET_TARGET);
+ if (!hdr)
+ return -EMSGSIZE;
+
+ genl_dump_check_consistent(cb, hdr);
+
+ if (nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target->idx) ||
+ nla_put_u32(msg, NFC_ATTR_PROTOCOLS, target->supported_protocols) ||
+ nla_put_u16(msg, NFC_ATTR_TARGET_SENS_RES, target->sens_res) ||
+ nla_put_u8(msg, NFC_ATTR_TARGET_SEL_RES, target->sel_res))
+ goto nla_put_failure;
+ if (target->nfcid1_len > 0 &&
+ nla_put(msg, NFC_ATTR_TARGET_NFCID1, target->nfcid1_len,
+ target->nfcid1))
+ goto nla_put_failure;
+ if (target->sensb_res_len > 0 &&
+ nla_put(msg, NFC_ATTR_TARGET_SENSB_RES, target->sensb_res_len,
+ target->sensb_res))
+ goto nla_put_failure;
+ if (target->sensf_res_len > 0 &&
+ nla_put(msg, NFC_ATTR_TARGET_SENSF_RES, target->sensf_res_len,
+ target->sensf_res))
+ goto nla_put_failure;
+
+ if (target->is_iso15693) {
+ if (nla_put_u8(msg, NFC_ATTR_TARGET_ISO15693_DSFID,
+ target->iso15693_dsfid) ||
+ nla_put(msg, NFC_ATTR_TARGET_ISO15693_UID,
+ sizeof(target->iso15693_uid), target->iso15693_uid))
+ goto nla_put_failure;
+ }
+
+ genlmsg_end(msg, hdr);
+ return 0;
+
+nla_put_failure:
+ genlmsg_cancel(msg, hdr);
+ return -EMSGSIZE;
+}
+
+static struct nfc_dev *__get_device_from_cb(struct netlink_callback *cb)
+{
+ struct nlattr **attrbuf = genl_family_attrbuf(&nfc_genl_family);
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+
+ rc = nlmsg_parse(cb->nlh, GENL_HDRLEN + nfc_genl_family.hdrsize,
+ attrbuf, nfc_genl_family.maxattr, nfc_genl_policy,
+ NULL);
+ if (rc < 0)
+ return ERR_PTR(rc);
+
+ if (!attrbuf[NFC_ATTR_DEVICE_INDEX])
+ return ERR_PTR(-EINVAL);
+
+ idx = nla_get_u32(attrbuf[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return ERR_PTR(-ENODEV);
+
+ return dev;
+}
+
+static int nfc_genl_dump_targets(struct sk_buff *skb,
+ struct netlink_callback *cb)
+{
+ int i = cb->args[0];
+ struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+ int rc;
+
+ if (!dev) {
+ dev = __get_device_from_cb(cb);
+ if (IS_ERR(dev))
+ return PTR_ERR(dev);
+
+ cb->args[1] = (long) dev;
+ }
+
+ device_lock(&dev->dev);
+
+ cb->seq = dev->targets_generation;
+
+ while (i < dev->n_targets) {
+ rc = nfc_genl_send_target(skb, &dev->targets[i], cb,
+ NLM_F_MULTI);
+ if (rc < 0)
+ break;
+
+ i++;
+ }
+
+ device_unlock(&dev->dev);
+
+ cb->args[0] = i;
+
+ return skb->len;
+}
+
+static int nfc_genl_dump_targets_done(struct netlink_callback *cb)
+{
+ struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+
+ if (dev)
+ nfc_put_device(dev);
+
+ return 0;
+}
+
+int nfc_genl_targets_found(struct nfc_dev *dev)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ dev->genl_data.poll_req_portid = 0;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_TARGETS_FOUND);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ return genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_target_lost(struct nfc_dev *dev, u32 target_idx)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_TARGET_LOST);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
+ nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_TM_ACTIVATED);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+ if (nla_put_u32(msg, NFC_ATTR_TM_PROTOCOLS, protocol))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_tm_deactivated(struct nfc_dev *dev)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_TM_DEACTIVATED);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_setup_device_added(struct nfc_dev *dev, struct sk_buff *msg)
+{
+ if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
+ nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+ nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) ||
+ nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) ||
+ nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode))
+ return -1;
+ return 0;
+}
+
+int nfc_genl_device_added(struct nfc_dev *dev)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_DEVICE_ADDED);
+ if (!hdr)
+ goto free_msg;
+
+ if (nfc_genl_setup_device_added(dev, msg))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_device_removed(struct nfc_dev *dev)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_DEVICE_REMOVED);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list)
+{
+ struct sk_buff *msg;
+ struct nlattr *sdp_attr, *uri_attr;
+ struct nfc_llcp_sdp_tlv *sdres;
+ struct hlist_node *n;
+ void *hdr;
+ int rc = -EMSGSIZE;
+ int i;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_LLC_SDRES);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+
+ sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP);
+ if (sdp_attr == NULL) {
+ rc = -ENOMEM;
+ goto nla_put_failure;
+ }
+
+ i = 1;
+ hlist_for_each_entry_safe(sdres, n, sdres_list, node) {
+ pr_debug("uri: %s, sap: %d\n", sdres->uri, sdres->sap);
+
+ uri_attr = nla_nest_start(msg, i++);
+ if (uri_attr == NULL) {
+ rc = -ENOMEM;
+ goto nla_put_failure;
+ }
+
+ if (nla_put_u8(msg, NFC_SDP_ATTR_SAP, sdres->sap))
+ goto nla_put_failure;
+
+ if (nla_put_string(msg, NFC_SDP_ATTR_URI, sdres->uri))
+ goto nla_put_failure;
+
+ nla_nest_end(msg, uri_attr);
+
+ hlist_del(&sdres->node);
+
+ nfc_llcp_free_sdp_tlv(sdres);
+ }
+
+ nla_nest_end(msg, sdp_attr);
+
+ genlmsg_end(msg, hdr);
+
+ return genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+
+ nfc_llcp_free_sdp_tlv_list(sdres_list);
+
+ return rc;
+}
+
+int nfc_genl_se_added(struct nfc_dev *dev, u32 se_idx, u16 type)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_SE_ADDED);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+ nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
+ nla_put_u8(msg, NFC_ATTR_SE_TYPE, type))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_se_removed(struct nfc_dev *dev, u32 se_idx)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_SE_REMOVED);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+ nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_se_transaction(struct nfc_dev *dev, u8 se_idx,
+ struct nfc_evt_transaction *evt_transaction)
+{
+ struct nfc_se *se;
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_SE_TRANSACTION);
+ if (!hdr)
+ goto free_msg;
+
+ se = nfc_find_se(dev, se_idx);
+ if (!se)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+ nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
+ nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type) ||
+ nla_put(msg, NFC_ATTR_SE_AID, evt_transaction->aid_len,
+ evt_transaction->aid) ||
+ nla_put(msg, NFC_ATTR_SE_PARAMS, evt_transaction->params_len,
+ evt_transaction->params))
+ goto nla_put_failure;
+
+ /* evt_transaction is no more used */
+ devm_kfree(&dev->dev, evt_transaction);
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ /* evt_transaction is no more used */
+ devm_kfree(&dev->dev, evt_transaction);
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_se_connectivity(struct nfc_dev *dev, u8 se_idx)
+{
+ struct nfc_se *se;
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_EVENT_SE_CONNECTIVITY);
+ if (!hdr)
+ goto free_msg;
+
+ se = nfc_find_se(dev, se_idx);
+ if (!se)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+ nla_put_u32(msg, NFC_ATTR_SE_INDEX, se_idx) ||
+ nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
+ u32 portid, u32 seq,
+ struct netlink_callback *cb,
+ int flags)
+{
+ void *hdr;
+
+ hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, flags,
+ NFC_CMD_GET_DEVICE);
+ if (!hdr)
+ return -EMSGSIZE;
+
+ if (cb)
+ genl_dump_check_consistent(cb, hdr);
+
+ if (nfc_genl_setup_device_added(dev, msg))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+ return 0;
+
+nla_put_failure:
+ genlmsg_cancel(msg, hdr);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_dump_devices(struct sk_buff *skb,
+ struct netlink_callback *cb)
+{
+ struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
+ struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+ bool first_call = false;
+
+ if (!iter) {
+ first_call = true;
+ iter = kmalloc(sizeof(struct class_dev_iter), GFP_KERNEL);
+ if (!iter)
+ return -ENOMEM;
+ cb->args[0] = (long) iter;
+ }
+
+ mutex_lock(&nfc_devlist_mutex);
+
+ cb->seq = nfc_devlist_generation;
+
+ if (first_call) {
+ nfc_device_iter_init(iter);
+ dev = nfc_device_iter_next(iter);
+ }
+
+ while (dev) {
+ int rc;
+
+ rc = nfc_genl_send_device(skb, dev, NETLINK_CB(cb->skb).portid,
+ cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
+ if (rc < 0)
+ break;
+
+ dev = nfc_device_iter_next(iter);
+ }
+
+ mutex_unlock(&nfc_devlist_mutex);
+
+ cb->args[1] = (long) dev;
+
+ return skb->len;
+}
+
+static int nfc_genl_dump_devices_done(struct netlink_callback *cb)
+{
+ struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
+
+ nfc_device_iter_exit(iter);
+ kfree(iter);
+
+ return 0;
+}
+
+int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx,
+ u8 comm_mode, u8 rf_mode)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ pr_debug("DEP link is up\n");
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, NFC_CMD_DEP_LINK_UP);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+ if (rf_mode == NFC_RF_INITIATOR &&
+ nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx))
+ goto nla_put_failure;
+ if (nla_put_u8(msg, NFC_ATTR_COMM_MODE, comm_mode) ||
+ nla_put_u8(msg, NFC_ATTR_RF_MODE, rf_mode))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ dev->dep_link_up = true;
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+int nfc_genl_dep_link_down_event(struct nfc_dev *dev)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ pr_debug("DEP link is down\n");
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_CMD_DEP_LINK_DOWN);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info)
+{
+ struct sk_buff *msg;
+ struct nfc_dev *dev;
+ u32 idx;
+ int rc = -ENOBUFS;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg) {
+ rc = -ENOMEM;
+ goto out_putdev;
+ }
+
+ rc = nfc_genl_send_device(msg, dev, info->snd_portid, info->snd_seq,
+ NULL, 0);
+ if (rc < 0)
+ goto out_free;
+
+ nfc_put_device(dev);
+
+ return genlmsg_reply(msg, info);
+
+out_free:
+ nlmsg_free(msg);
+out_putdev:
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_dev_up(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ rc = nfc_dev_up(dev);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_dev_down(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ rc = nfc_dev_down(dev);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+ u32 im_protocols = 0, tm_protocols = 0;
+
+ pr_debug("Poll start\n");
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
+ !info->attrs[NFC_ATTR_PROTOCOLS]) &&
+ !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ if (info->attrs[NFC_ATTR_TM_PROTOCOLS])
+ tm_protocols = nla_get_u32(info->attrs[NFC_ATTR_TM_PROTOCOLS]);
+
+ if (info->attrs[NFC_ATTR_IM_PROTOCOLS])
+ im_protocols = nla_get_u32(info->attrs[NFC_ATTR_IM_PROTOCOLS]);
+ else if (info->attrs[NFC_ATTR_PROTOCOLS])
+ im_protocols = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ mutex_lock(&dev->genl_data.genl_data_mutex);
+
+ rc = nfc_start_poll(dev, im_protocols, tm_protocols);
+ if (!rc)
+ dev->genl_data.poll_req_portid = info->snd_portid;
+
+ mutex_unlock(&dev->genl_data.genl_data_mutex);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_stop_poll(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ device_lock(&dev->dev);
+
+ if (!dev->polling) {
+ device_unlock(&dev->dev);
+ return -EINVAL;
+ }
+
+ device_unlock(&dev->dev);
+
+ mutex_lock(&dev->genl_data.genl_data_mutex);
+
+ if (dev->genl_data.poll_req_portid != info->snd_portid) {
+ rc = -EBUSY;
+ goto out;
+ }
+
+ rc = nfc_stop_poll(dev);
+ dev->genl_data.poll_req_portid = 0;
+
+out:
+ mutex_unlock(&dev->genl_data.genl_data_mutex);
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_activate_target(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ u32 device_idx, target_idx, protocol;
+ int rc;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_TARGET_INDEX] ||
+ !info->attrs[NFC_ATTR_PROTOCOLS])
+ return -EINVAL;
+
+ device_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(device_idx);
+ if (!dev)
+ return -ENODEV;
+
+ target_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
+ protocol = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]);
+
+ nfc_deactivate_target(dev, target_idx, NFC_TARGET_MODE_SLEEP);
+ rc = nfc_activate_target(dev, target_idx, protocol);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_deactivate_target(struct sk_buff *skb,
+ struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ u32 device_idx, target_idx;
+ int rc;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ device_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(device_idx);
+ if (!dev)
+ return -ENODEV;
+
+ target_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
+
+ rc = nfc_deactivate_target(dev, target_idx, NFC_TARGET_MODE_SLEEP);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_dep_link_up(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc, tgt_idx;
+ u32 idx;
+ u8 comm;
+
+ pr_debug("DEP link up\n");
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_COMM_MODE])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+ if (!info->attrs[NFC_ATTR_TARGET_INDEX])
+ tgt_idx = NFC_TARGET_IDX_ANY;
+ else
+ tgt_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]);
+
+ comm = nla_get_u8(info->attrs[NFC_ATTR_COMM_MODE]);
+
+ if (comm != NFC_COMM_ACTIVE && comm != NFC_COMM_PASSIVE)
+ return -EINVAL;
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ rc = nfc_dep_link_up(dev, tgt_idx, comm);
+
+ nfc_put_device(dev);
+
+ return rc;
+}
+
+static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ rc = nfc_dep_link_down(dev);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_send_params(struct sk_buff *msg,
+ struct nfc_llcp_local *local,
+ u32 portid, u32 seq)
+{
+ void *hdr;
+
+ hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, 0,
+ NFC_CMD_LLC_GET_PARAMS);
+ if (!hdr)
+ return -EMSGSIZE;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, local->dev->idx) ||
+ nla_put_u8(msg, NFC_ATTR_LLC_PARAM_LTO, local->lto) ||
+ nla_put_u8(msg, NFC_ATTR_LLC_PARAM_RW, local->rw) ||
+ nla_put_u16(msg, NFC_ATTR_LLC_PARAM_MIUX, be16_to_cpu(local->miux)))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+ return 0;
+
+nla_put_failure:
+ genlmsg_cancel(msg, hdr);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_llc_get_params(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ struct nfc_llcp_local *local;
+ int rc = 0;
+ struct sk_buff *msg = NULL;
+ u32 idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ device_lock(&dev->dev);
+
+ local = nfc_llcp_find_local(dev);
+ if (!local) {
+ rc = -ENODEV;
+ goto exit;
+ }
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg) {
+ rc = -ENOMEM;
+ goto exit;
+ }
+
+ rc = nfc_genl_send_params(msg, local, info->snd_portid, info->snd_seq);
+
+exit:
+ device_unlock(&dev->dev);
+
+ nfc_put_device(dev);
+
+ if (rc < 0) {
+ if (msg)
+ nlmsg_free(msg);
+
+ return rc;
+ }
+
+ return genlmsg_reply(msg, info);
+}
+
+static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ struct nfc_llcp_local *local;
+ u8 rw = 0;
+ u16 miux = 0;
+ u32 idx;
+ int rc = 0;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] &&
+ !info->attrs[NFC_ATTR_LLC_PARAM_RW] &&
+ !info->attrs[NFC_ATTR_LLC_PARAM_MIUX]))
+ return -EINVAL;
+
+ if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) {
+ rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]);
+
+ if (rw > LLCP_MAX_RW)
+ return -EINVAL;
+ }
+
+ if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) {
+ miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]);
+
+ if (miux > LLCP_MAX_MIUX)
+ return -EINVAL;
+ }
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ device_lock(&dev->dev);
+
+ local = nfc_llcp_find_local(dev);
+ if (!local) {
+ nfc_put_device(dev);
+ rc = -ENODEV;
+ goto exit;
+ }
+
+ if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) {
+ if (dev->dep_link_up) {
+ rc = -EINPROGRESS;
+ goto exit;
+ }
+
+ local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]);
+ }
+
+ if (info->attrs[NFC_ATTR_LLC_PARAM_RW])
+ local->rw = rw;
+
+ if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX])
+ local->miux = cpu_to_be16(miux);
+
+exit:
+ device_unlock(&dev->dev);
+
+ nfc_put_device(dev);
+
+ return rc;
+}
+
+static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ struct nfc_llcp_local *local;
+ struct nlattr *attr, *sdp_attrs[NFC_SDP_ATTR_MAX+1];
+ u32 idx;
+ u8 tid;
+ char *uri;
+ int rc = 0, rem;
+ size_t uri_len, tlvs_len;
+ struct hlist_head sdreq_list;
+ struct nfc_llcp_sdp_tlv *sdreq;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_LLC_SDP])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ device_lock(&dev->dev);
+
+ if (dev->dep_link_up == false) {
+ rc = -ENOLINK;
+ goto exit;
+ }
+
+ local = nfc_llcp_find_local(dev);
+ if (!local) {
+ nfc_put_device(dev);
+ rc = -ENODEV;
+ goto exit;
+ }
+
+ INIT_HLIST_HEAD(&sdreq_list);
+
+ tlvs_len = 0;
+
+ nla_for_each_nested(attr, info->attrs[NFC_ATTR_LLC_SDP], rem) {
+ rc = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX, attr,
+ nfc_sdp_genl_policy, info->extack);
+
+ if (rc != 0) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ if (!sdp_attrs[NFC_SDP_ATTR_URI])
+ continue;
+
+ uri_len = nla_len(sdp_attrs[NFC_SDP_ATTR_URI]);
+ if (uri_len == 0)
+ continue;
+
+ uri = nla_data(sdp_attrs[NFC_SDP_ATTR_URI]);
+ if (uri == NULL || *uri == 0)
+ continue;
+
+ tid = local->sdreq_next_tid++;
+
+ sdreq = nfc_llcp_build_sdreq_tlv(tid, uri, uri_len);
+ if (sdreq == NULL) {
+ rc = -ENOMEM;
+ goto exit;
+ }
+
+ tlvs_len += sdreq->tlv_len;
+
+ hlist_add_head(&sdreq->node, &sdreq_list);
+ }
+
+ if (hlist_empty(&sdreq_list)) {
+ rc = -EINVAL;
+ goto exit;
+ }
+
+ rc = nfc_llcp_send_snl_sdreq(local, &sdreq_list, tlvs_len);
+exit:
+ device_unlock(&dev->dev);
+
+ nfc_put_device(dev);
+
+ return rc;
+}
+
+static int nfc_genl_fw_download(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx;
+ char firmware_name[NFC_FIRMWARE_NAME_MAXSIZE + 1];
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ nla_strlcpy(firmware_name, info->attrs[NFC_ATTR_FIRMWARE_NAME],
+ sizeof(firmware_name));
+
+ rc = nfc_fw_download(dev, firmware_name);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+int nfc_genl_fw_download_done(struct nfc_dev *dev, const char *firmware_name,
+ u32 result)
+{
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg)
+ return -ENOMEM;
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_CMD_FW_DOWNLOAD);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_string(msg, NFC_ATTR_FIRMWARE_NAME, firmware_name) ||
+ nla_put_u32(msg, NFC_ATTR_FIRMWARE_DOWNLOAD_STATUS, result) ||
+ nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ return 0;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_enable_se(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx, se_idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_SE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+ se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ rc = nfc_enable_se(dev, se_idx);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_disable_se(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ int rc;
+ u32 idx, se_idx;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_SE_INDEX])
+ return -EINVAL;
+
+ idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+ se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
+
+ dev = nfc_get_device(idx);
+ if (!dev)
+ return -ENODEV;
+
+ rc = nfc_disable_se(dev, se_idx);
+
+ nfc_put_device(dev);
+ return rc;
+}
+
+static int nfc_genl_send_se(struct sk_buff *msg, struct nfc_dev *dev,
+ u32 portid, u32 seq,
+ struct netlink_callback *cb,
+ int flags)
+{
+ void *hdr;
+ struct nfc_se *se, *n;
+
+ list_for_each_entry_safe(se, n, &dev->secure_elements, list) {
+ hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, flags,
+ NFC_CMD_GET_SE);
+ if (!hdr)
+ goto nla_put_failure;
+
+ if (cb)
+ genl_dump_check_consistent(cb, hdr);
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
+ nla_put_u32(msg, NFC_ATTR_SE_INDEX, se->idx) ||
+ nla_put_u8(msg, NFC_ATTR_SE_TYPE, se->type))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+ }
+
+ return 0;
+
+nla_put_failure:
+ genlmsg_cancel(msg, hdr);
+ return -EMSGSIZE;
+}
+
+static int nfc_genl_dump_ses(struct sk_buff *skb,
+ struct netlink_callback *cb)
+{
+ struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
+ struct nfc_dev *dev = (struct nfc_dev *) cb->args[1];
+ bool first_call = false;
+
+ if (!iter) {
+ first_call = true;
+ iter = kmalloc(sizeof(struct class_dev_iter), GFP_KERNEL);
+ if (!iter)
+ return -ENOMEM;
+ cb->args[0] = (long) iter;
+ }
+
+ mutex_lock(&nfc_devlist_mutex);
+
+ cb->seq = nfc_devlist_generation;
+
+ if (first_call) {
+ nfc_device_iter_init(iter);
+ dev = nfc_device_iter_next(iter);
+ }
+
+ while (dev) {
+ int rc;
+
+ rc = nfc_genl_send_se(skb, dev, NETLINK_CB(cb->skb).portid,
+ cb->nlh->nlmsg_seq, cb, NLM_F_MULTI);
+ if (rc < 0)
+ break;
+
+ dev = nfc_device_iter_next(iter);
+ }
+
+ mutex_unlock(&nfc_devlist_mutex);
+
+ cb->args[1] = (long) dev;
+
+ return skb->len;
+}
+
+static int nfc_genl_dump_ses_done(struct netlink_callback *cb)
+{
+ struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0];
+
+ nfc_device_iter_exit(iter);
+ kfree(iter);
+
+ return 0;
+}
+
+static int nfc_se_io(struct nfc_dev *dev, u32 se_idx,
+ u8 *apdu, size_t apdu_length,
+ se_io_cb_t cb, void *cb_context)
+{
+ struct nfc_se *se;
+ int rc;
+
+ pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
+
+ device_lock(&dev->dev);
+
+ if (!device_is_registered(&dev->dev)) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->dev_up) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ if (!dev->ops->se_io) {
+ rc = -EOPNOTSUPP;
+ goto error;
+ }
+
+ se = nfc_find_se(dev, se_idx);
+ if (!se) {
+ rc = -EINVAL;
+ goto error;
+ }
+
+ if (se->state != NFC_SE_ENABLED) {
+ rc = -ENODEV;
+ goto error;
+ }
+
+ rc = dev->ops->se_io(dev, se_idx, apdu,
+ apdu_length, cb, cb_context);
+
+error:
+ device_unlock(&dev->dev);
+ return rc;
+}
+
+struct se_io_ctx {
+ u32 dev_idx;
+ u32 se_idx;
+};
+
+static void se_io_cb(void *context, u8 *apdu, size_t apdu_len, int err)
+{
+ struct se_io_ctx *ctx = context;
+ struct sk_buff *msg;
+ void *hdr;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+ if (!msg) {
+ kfree(ctx);
+ return;
+ }
+
+ hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+ NFC_CMD_SE_IO);
+ if (!hdr)
+ goto free_msg;
+
+ if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, ctx->dev_idx) ||
+ nla_put_u32(msg, NFC_ATTR_SE_INDEX, ctx->se_idx) ||
+ nla_put(msg, NFC_ATTR_SE_APDU, apdu_len, apdu))
+ goto nla_put_failure;
+
+ genlmsg_end(msg, hdr);
+
+ genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL);
+
+ kfree(ctx);
+
+ return;
+
+nla_put_failure:
+free_msg:
+ nlmsg_free(msg);
+ kfree(ctx);
+
+ return;
+}
+
+static int nfc_genl_se_io(struct sk_buff *skb, struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ struct se_io_ctx *ctx;
+ u32 dev_idx, se_idx;
+ u8 *apdu;
+ size_t apdu_len;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_SE_INDEX] ||
+ !info->attrs[NFC_ATTR_SE_APDU])
+ return -EINVAL;
+
+ dev_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+ se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
+
+ dev = nfc_get_device(dev_idx);
+ if (!dev)
+ return -ENODEV;
+
+ if (!dev->ops || !dev->ops->se_io)
+ return -ENOTSUPP;
+
+ apdu_len = nla_len(info->attrs[NFC_ATTR_SE_APDU]);
+ if (apdu_len == 0)
+ return -EINVAL;
+
+ apdu = nla_data(info->attrs[NFC_ATTR_SE_APDU]);
+ if (!apdu)
+ return -EINVAL;
+
+ ctx = kzalloc(sizeof(struct se_io_ctx), GFP_KERNEL);
+ if (!ctx)
+ return -ENOMEM;
+
+ ctx->dev_idx = dev_idx;
+ ctx->se_idx = se_idx;
+
+ return nfc_se_io(dev, se_idx, apdu, apdu_len, se_io_cb, ctx);
+}
+
+static int nfc_genl_vendor_cmd(struct sk_buff *skb,
+ struct genl_info *info)
+{
+ struct nfc_dev *dev;
+ struct nfc_vendor_cmd *cmd;
+ u32 dev_idx, vid, subcmd;
+ u8 *data;
+ size_t data_len;
+ int i, err;
+
+ if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+ !info->attrs[NFC_ATTR_VENDOR_ID] ||
+ !info->attrs[NFC_ATTR_VENDOR_SUBCMD])
+ return -EINVAL;
+
+ dev_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+ vid = nla_get_u32(info->attrs[NFC_ATTR_VENDOR_ID]);
+ subcmd = nla_get_u32(info->attrs[NFC_ATTR_VENDOR_SUBCMD]);
+
+ dev = nfc_get_device(dev_idx);
+ if (!dev || !dev->vendor_cmds || !dev->n_vendor_cmds)
+ return -ENODEV;
+
+ if (info->attrs[NFC_ATTR_VENDOR_DATA]) {
+ data = nla_data(info->attrs[NFC_ATTR_VENDOR_DATA]);
+ data_len = nla_len(info->attrs[NFC_ATTR_VENDOR_DATA]);
+ if (data_len == 0)
+ return -EINVAL;
+ } else {
+ data = NULL;
+ data_len = 0;
+ }
+
+ for (i = 0; i < dev->n_vendor_cmds; i++) {
+ cmd = &dev->vendor_cmds[i];
+
+ if (cmd->vendor_id != vid || cmd->subcmd != subcmd)
+ continue;
+
+ dev->cur_cmd_info = info;
+ err = cmd->doit(dev, data, data_len);
+ dev->cur_cmd_info = NULL;
+ return err;
+ }
+
+ return -EOPNOTSUPP;
+}
+
+/* message building helper */
+static inline void *nfc_hdr_put(struct sk_buff *skb, u32 portid, u32 seq,
+ int flags, u8 cmd)
+{
+ /* since there is no private header just add the generic one */
+ return genlmsg_put(skb, portid, seq, &nfc_genl_family, flags, cmd);
+}
+
+static struct sk_buff *
+__nfc_alloc_vendor_cmd_skb(struct nfc_dev *dev, int approxlen,
+ u32 portid, u32 seq,
+ enum nfc_attrs attr,
+ u32 oui, u32 subcmd, gfp_t gfp)
+{
+ struct sk_buff *skb;
+ void *hdr;
+
+ skb = nlmsg_new(approxlen + 100, gfp);
+ if (!skb)
+ return NULL;
+
+ hdr = nfc_hdr_put(skb, portid, seq, 0, NFC_CMD_VENDOR);
+ if (!hdr) {
+ kfree_skb(skb);
+ return NULL;
+ }
+
+ if (nla_put_u32(skb, NFC_ATTR_DEVICE_INDEX, dev->idx))
+ goto nla_put_failure;
+ if (nla_put_u32(skb, NFC_ATTR_VENDOR_ID, oui))
+ goto nla_put_failure;
+ if (nla_put_u32(skb, NFC_ATTR_VENDOR_SUBCMD, subcmd))
+ goto nla_put_failure;
+
+ ((void **)skb->cb)[0] = dev;
+ ((void **)skb->cb)[1] = hdr;
+
+ return skb;
+
+nla_put_failure:
+ kfree_skb(skb);
+ return NULL;
+}
+
+struct sk_buff *__nfc_alloc_vendor_cmd_reply_skb(struct nfc_dev *dev,
+ enum nfc_attrs attr,
+ u32 oui, u32 subcmd,
+ int approxlen)
+{
+ if (WARN_ON(!dev->cur_cmd_info))
+ return NULL;
+
+ return __nfc_alloc_vendor_cmd_skb(dev, approxlen,
+ dev->cur_cmd_info->snd_portid,
+ dev->cur_cmd_info->snd_seq, attr,
+ oui, subcmd, GFP_KERNEL);
+}
+EXPORT_SYMBOL(__nfc_alloc_vendor_cmd_reply_skb);
+
+int nfc_vendor_cmd_reply(struct sk_buff *skb)
+{
+ struct nfc_dev *dev = ((void **)skb->cb)[0];
+ void *hdr = ((void **)skb->cb)[1];
+
+ /* clear CB data for netlink core to own from now on */
+ memset(skb->cb, 0, sizeof(skb->cb));
+
+ if (WARN_ON(!dev->cur_cmd_info)) {
+ kfree_skb(skb);
+ return -EINVAL;
+ }
+
+ genlmsg_end(skb, hdr);
+ return genlmsg_reply(skb, dev->cur_cmd_info);
+}
+EXPORT_SYMBOL(nfc_vendor_cmd_reply);
+
+static const struct genl_ops nfc_genl_ops[] = {
+ {
+ .cmd = NFC_CMD_GET_DEVICE,
+ .doit = nfc_genl_get_device,
+ .dumpit = nfc_genl_dump_devices,
+ .done = nfc_genl_dump_devices_done,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_DEV_UP,
+ .doit = nfc_genl_dev_up,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_DEV_DOWN,
+ .doit = nfc_genl_dev_down,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_START_POLL,
+ .doit = nfc_genl_start_poll,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_STOP_POLL,
+ .doit = nfc_genl_stop_poll,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_DEP_LINK_UP,
+ .doit = nfc_genl_dep_link_up,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_DEP_LINK_DOWN,
+ .doit = nfc_genl_dep_link_down,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_GET_TARGET,
+ .dumpit = nfc_genl_dump_targets,
+ .done = nfc_genl_dump_targets_done,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_LLC_GET_PARAMS,
+ .doit = nfc_genl_llc_get_params,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_LLC_SET_PARAMS,
+ .doit = nfc_genl_llc_set_params,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_LLC_SDREQ,
+ .doit = nfc_genl_llc_sdreq,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_FW_DOWNLOAD,
+ .doit = nfc_genl_fw_download,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_ENABLE_SE,
+ .doit = nfc_genl_enable_se,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_DISABLE_SE,
+ .doit = nfc_genl_disable_se,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_GET_SE,
+ .dumpit = nfc_genl_dump_ses,
+ .done = nfc_genl_dump_ses_done,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_SE_IO,
+ .doit = nfc_genl_se_io,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_ACTIVATE_TARGET,
+ .doit = nfc_genl_activate_target,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_VENDOR,
+ .doit = nfc_genl_vendor_cmd,
+ .policy = nfc_genl_policy,
+ },
+ {
+ .cmd = NFC_CMD_DEACTIVATE_TARGET,
+ .doit = nfc_genl_deactivate_target,
+ .policy = nfc_genl_policy,
+ },
+};
+
+static struct genl_family nfc_genl_family __ro_after_init = {
+ .hdrsize = 0,
+ .name = NFC_GENL_NAME,
+ .version = NFC_GENL_VERSION,
+ .maxattr = NFC_ATTR_MAX,
+ .module = THIS_MODULE,
+ .ops = nfc_genl_ops,
+ .n_ops = ARRAY_SIZE(nfc_genl_ops),
+ .mcgrps = nfc_genl_mcgrps,
+ .n_mcgrps = ARRAY_SIZE(nfc_genl_mcgrps),
+};
+
+
+struct urelease_work {
+ struct work_struct w;
+ u32 portid;
+};
+
+static void nfc_urelease_event_work(struct work_struct *work)
+{
+ struct urelease_work *w = container_of(work, struct urelease_work, w);
+ struct class_dev_iter iter;
+ struct nfc_dev *dev;
+
+ pr_debug("portid %d\n", w->portid);
+
+ mutex_lock(&nfc_devlist_mutex);
+
+ nfc_device_iter_init(&iter);
+ dev = nfc_device_iter_next(&iter);
+
+ while (dev) {
+ mutex_lock(&dev->genl_data.genl_data_mutex);
+
+ if (dev->genl_data.poll_req_portid == w->portid) {
+ nfc_stop_poll(dev);
+ dev->genl_data.poll_req_portid = 0;
+ }
+
+ mutex_unlock(&dev->genl_data.genl_data_mutex);
+
+ dev = nfc_device_iter_next(&iter);
+ }
+
+ nfc_device_iter_exit(&iter);
+
+ mutex_unlock(&nfc_devlist_mutex);
+
+ kfree(w);
+}
+
+static int nfc_genl_rcv_nl_event(struct notifier_block *this,
+ unsigned long event, void *ptr)
+{
+ struct netlink_notify *n = ptr;
+ struct urelease_work *w;
+
+ if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC)
+ goto out;
+
+ pr_debug("NETLINK_URELEASE event from id %d\n", n->portid);
+
+ w = kmalloc(sizeof(*w), GFP_ATOMIC);
+ if (w) {
+ INIT_WORK((struct work_struct *) w, nfc_urelease_event_work);
+ w->portid = n->portid;
+ schedule_work((struct work_struct *) w);
+ }
+
+out:
+ return NOTIFY_DONE;
+}
+
+void nfc_genl_data_init(struct nfc_genl_data *genl_data)
+{
+ genl_data->poll_req_portid = 0;
+ mutex_init(&genl_data->genl_data_mutex);
+}
+
+void nfc_genl_data_exit(struct nfc_genl_data *genl_data)
+{
+ mutex_destroy(&genl_data->genl_data_mutex);
+}
+
+static struct notifier_block nl_notifier = {
+ .notifier_call = nfc_genl_rcv_nl_event,
+};
+
+/**
+ * nfc_genl_init() - Initialize netlink interface
+ *
+ * This initialization function registers the nfc netlink family.
+ */
+int __init nfc_genl_init(void)
+{
+ int rc;
+
+ rc = genl_register_family(&nfc_genl_family);
+ if (rc)
+ return rc;
+
+ netlink_register_notifier(&nl_notifier);
+
+ return 0;
+}
+
+/**
+ * nfc_genl_exit() - Deinitialize netlink interface
+ *
+ * This exit function unregisters the nfc netlink family.
+ */
+void nfc_genl_exit(void)
+{
+ netlink_unregister_notifier(&nl_notifier);
+ genl_unregister_family(&nfc_genl_family);
+}
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
new file mode 100644
index 0000000..6c6f76b
--- /dev/null
+++ b/net/nfc/nfc.h
@@ -0,0 +1,162 @@
+/*
+ * 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>
+
+#define NFC_TARGET_MODE_IDLE 0
+#define NFC_TARGET_MODE_SLEEP 1
+
+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, int kern);
+};
+
+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);
+int nfc_genl_se_transaction(struct nfc_dev *dev, u8 se_idx,
+ struct nfc_evt_transaction *evt_transaction);
+int nfc_genl_se_connectivity(struct nfc_dev *dev, u8 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, u8 mode);
+
+int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, struct sk_buff *skb,
+ data_exchange_cb_t cb, void *cb_context);
+
+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 */
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
new file mode 100644
index 0000000..e2188de
--- /dev/null
+++ b/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)
+{
+ *(u8 *)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 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_from_msg(skb_put(skb, len), msg, 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 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_msg(skb, 0, msg, 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_TARGET_MODE_IDLE);
+ 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, int kern)
+{
+ 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, kern);
+ 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);
+}