diff options
Diffstat (limited to 'net/nfc')
35 files changed, 19408 insertions, 0 deletions
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig new file mode 100644 index 0000000..5e54818 --- /dev/null +++ b/net/nfc/Kconfig @@ -0,0 +1,37 @@ +# +# NFC sybsystem configuration +# + +menuconfig NFC + depends on !KERNEL_3_1 + depends on NET + depends on RFKILL || !RFKILL + tristate "NFC subsystem support" + depends on m + default n + help + Say Y here if you want to build support for NFC (Near field + communication) devices. + + To compile this support as a module, choose M here: the module will + be called nfc. + +config NFC_DIGITAL + depends on NFC + depends on CRC_CCITT + depends on CRC_ITU_T + tristate "NFC Digital Protocol stack support" + depends on m + 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..babbbc3 --- /dev/null +++ b/net/nfc/Makefile @@ -0,0 +1,13 @@ +# +# Makefile for the Linux NFC subsystem. +# + +obj-$(CPTCFG_NFC) += nfc.o +obj-$(CPTCFG_NFC_NCI) += nci/ +obj-$(CPTCFG_NFC_HCI) += hci/ +obj-$(CPTCFG_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..54e40fa --- /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 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..122bb81 --- /dev/null +++ b/net/nfc/core.c @@ -0,0 +1,1246 @@ +/* + * 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); + } + + kfree(dev); +} + +static void nfc_check_pres_work(struct work_struct *work) +{ + struct nfc_dev *dev = container_of(work, struct nfc_dev, + check_pres_work); + int rc; + + device_lock(&dev->dev); + + if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) { + rc = dev->ops->check_presence(dev, dev->active_target); + if (rc == -EOPNOTSUPP) + goto exit; + if (rc) { + u32 active_target_idx = dev->active_target->idx; + device_unlock(&dev->dev); + nfc_target_lost(dev, active_target_idx); + return; + } + + if (!dev->shutting_down) + mod_timer(&dev->check_pres_timer, jiffies + + msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS)); + } + +exit: + device_unlock(&dev->dev); +} + +static void nfc_check_pres_timeout(unsigned long data) +{ + struct nfc_dev *dev = (struct nfc_dev *)data; + + schedule_work(&dev->check_pres_work); +} + +struct class nfc_class = { + .name = "nfc", + .dev_release = nfc_release, +}; +EXPORT_SYMBOL(nfc_class); + +static int match_idx(struct device *d, const void *data) +{ + struct nfc_dev *dev = to_nfc_dev(d); + const unsigned int *idx = data; + + return dev->idx == *idx; +} + +struct nfc_dev *nfc_get_device(unsigned int idx) +{ + struct device *d; + + d = class_find_device(&nfc_class, NULL, &idx, match_idx); + if (!d) + return NULL; + + return to_nfc_dev(d); +} + +/** + * nfc_allocate_device - allocate a new nfc device + * + * @ops: device operations + * @supported_protocols: NFC protocols supported by the device + */ +struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops, + u32 supported_protocols, + int tx_headroom, int tx_tailroom) +{ + struct nfc_dev *dev; + + if (!ops->start_poll || !ops->stop_poll || !ops->activate_target || + !ops->deactivate_target || !ops->im_transceive) + return NULL; + + if (!supported_protocols) + return NULL; + + dev = kzalloc(sizeof(struct nfc_dev), GFP_KERNEL); + if (!dev) + return NULL; + + dev->ops = ops; + dev->supported_protocols = supported_protocols; + dev->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) { + init_timer(&dev->check_pres_timer); + dev->check_pres_timer.data = (unsigned long)dev; + dev->check_pres_timer.function = nfc_check_pres_timeout; + + INIT_WORK(&dev->check_pres_work, nfc_check_pres_work); + } + + return dev; +} +EXPORT_SYMBOL(nfc_allocate_device); + +/** + * nfc_register_device - register a nfc device in the nfc subsystem + * + * @dev: The nfc device to register + */ +int nfc_register_device(struct nfc_dev *dev) +{ + int rc; + + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); + + dev->idx = ida_simple_get(&nfc_index_ida, 0, 0, GFP_KERNEL); + if (dev->idx < 0) + return dev->idx; + + dev->dev.class = &nfc_class; + dev_set_name(&dev->dev, "nfc%d", dev->idx); + device_initialize(&dev->dev); + + mutex_lock(&nfc_devlist_mutex); + nfc_devlist_generation++; + rc = device_add(&dev->dev); + mutex_unlock(&nfc_devlist_mutex); + + if (rc < 0) + return rc; + + rc = nfc_llcp_register_device(dev); + if (rc) + pr_err("Could not register llcp device\n"); + + rc = nfc_genl_device_added(dev); + if (rc) + pr_debug("The userspace won't be notified that the device %s was added\n", + dev_name(&dev->dev)); + + dev->rfkill = rfkill_alloc(dev_name(&dev->dev), &dev->dev, + RFKILL_TYPE_NFC, &nfc_rfkill_ops, dev); + if (dev->rfkill) { + if (rfkill_register(dev->rfkill) < 0) { + rfkill_destroy(dev->rfkill); + dev->rfkill = NULL; + } + } + + return 0; +} +EXPORT_SYMBOL(nfc_register_device); + +/** + * nfc_unregister_device - unregister a nfc device in the nfc subsystem + * + * @dev: The nfc device to unregister + */ +void nfc_unregister_device(struct nfc_dev *dev) +{ + int rc, id; + + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); + + id = dev->idx; + + if (dev->rfkill) { + rfkill_unregister(dev->rfkill); + rfkill_destroy(dev->rfkill); + } + + if (dev->ops->check_presence) { + device_lock(&dev->dev); + dev->shutting_down = true; + device_unlock(&dev->dev); + del_timer_sync(&dev->check_pres_timer); + cancel_work_sync(&dev->check_pres_work); + } + + rc = nfc_genl_device_removed(dev); + if (rc) + pr_debug("The userspace won't be notified that the device %s " + "was removed\n", dev_name(&dev->dev)); + + nfc_llcp_unregister_device(dev); + + mutex_lock(&nfc_devlist_mutex); + nfc_devlist_generation++; + device_del(&dev->dev); + mutex_unlock(&nfc_devlist_mutex); + + ida_simple_remove(&nfc_index_ida, id); +} +EXPORT_SYMBOL(nfc_unregister_device); + +static int __init nfc_init(void) +{ + int rc; + + pr_info("NFC Core ver %s\n", VERSION); + + rc = class_register(&nfc_class); + if (rc) + return rc; + + rc = nfc_genl_init(); + if (rc) + goto err_genl; + + /* the first generation must not be 0 */ + nfc_devlist_generation = 1; + + rc = rawsock_init(); + if (rc) + goto err_rawsock; + + rc = nfc_llcp_init(); + if (rc) + goto err_llcp_sock; + + rc = af_nfc_init(); + if (rc) + goto err_af_nfc; + + return 0; + +err_af_nfc: + nfc_llcp_exit(); +err_llcp_sock: + rawsock_exit(); +err_rawsock: + nfc_genl_exit(); +err_genl: + class_unregister(&nfc_class); + return rc; +} + +static void __exit nfc_exit(void) +{ + af_nfc_exit(); + nfc_llcp_exit(); + rawsock_exit(); + nfc_genl_exit(); + class_unregister(&nfc_class); +} + +subsys_initcall(nfc_init); +module_exit(nfc_exit); + +MODULE_AUTHOR("Lauro Ramos Venancio <lauro.venancio@openbossa.org>"); +MODULE_DESCRIPTION("NFC Core ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS_NETPROTO(PF_NFC); +MODULE_ALIAS_GENL_FAMILY(NFC_GENL_NAME); diff --git a/net/nfc/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..0fd5518 --- /dev/null +++ b/net/nfc/digital_core.c @@ -0,0 +1,863 @@ +/* + * 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(skb, 1) = crc & 0xFF; + *skb_put(skb, 1) = (crc >> 8) & 0xFF; +} + +int digital_skb_check_crc(struct sk_buff *skb, crc_func_t crc_func, + u16 crc_init, u8 bitwise_inv, u8 msb_first) +{ + int rc; + u16 crc; + + if (skb->len <= 2) + return -EIO; + + crc = crc_func(crc_init, skb->data, skb->len - 2); + + if (bitwise_inv) + crc = ~crc; + + if (msb_first) + crc = __swab16(crc); + + rc = (skb->data[skb->len - 2] - (crc & 0xFF)) + + (skb->data[skb->len - 1] - ((crc >> 8) & 0xFF)); + + if (rc) + return -EIO; + + skb_trim(skb, skb->len - 2); + + return 0; +} + +static inline void digital_switch_rf(struct nfc_digital_dev *ddev, bool on) +{ + ddev->ops->switch_rf(ddev, on); +} + +static inline void digital_abort_cmd(struct nfc_digital_dev *ddev) +{ + ddev->ops->abort_cmd(ddev); +} + +static void digital_wq_cmd_complete(struct work_struct *work) +{ + struct digital_cmd *cmd; + struct nfc_digital_dev *ddev = container_of(work, + struct nfc_digital_dev, + cmd_complete_work); + + mutex_lock(&ddev->cmd_lock); + + cmd = list_first_entry_or_null(&ddev->cmd_queue, struct digital_cmd, + queue); + if (!cmd) { + mutex_unlock(&ddev->cmd_lock); + return; + } + + list_del(&cmd->queue); + + mutex_unlock(&ddev->cmd_lock); + + if (!IS_ERR(cmd->resp)) + print_hex_dump_debug("DIGITAL RX: ", DUMP_PREFIX_NONE, 16, 1, + cmd->resp->data, cmd->resp->len, false); + + cmd->cmd_cb(ddev, cmd->cb_context, cmd->resp); + + kfree(cmd->mdaa_params); + kfree(cmd); + + schedule_work(&ddev->cmd_work); +} + +static void digital_send_cmd_complete(struct nfc_digital_dev *ddev, + void *arg, struct sk_buff *resp) +{ + struct digital_cmd *cmd = arg; + + cmd->resp = resp; + + schedule_work(&ddev->cmd_complete_work); +} + +static void digital_wq_cmd(struct work_struct *work) +{ + int rc; + struct digital_cmd *cmd; + struct digital_tg_mdaa_params *params; + struct nfc_digital_dev *ddev = container_of(work, + struct nfc_digital_dev, + cmd_work); + + mutex_lock(&ddev->cmd_lock); + + cmd = list_first_entry_or_null(&ddev->cmd_queue, struct digital_cmd, + queue); + if (!cmd || cmd->pending) { + mutex_unlock(&ddev->cmd_lock); + return; + } + + 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(struct digital_cmd), GFP_KERNEL); + if (!cmd) + return -ENOMEM; + + cmd->type = cmd_type; + cmd->timeout = timeout; + cmd->req = skb; + cmd->mdaa_params = params; + cmd->cmd_cb = cmd_cb; + cmd->cb_context = cb_context; + INIT_LIST_HEAD(&cmd->queue); + + mutex_lock(&ddev->cmd_lock); + list_add_tail(&cmd->queue, &ddev->cmd_queue); + mutex_unlock(&ddev->cmd_lock); + + schedule_work(&ddev->cmd_work); + + return 0; +} + +int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param) +{ + int rc; + + rc = ddev->ops->in_configure_hw(ddev, type, param); + if (rc) + pr_err("in_configure_hw failed: %d\n", rc); + + return rc; +} + +int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param) +{ + int rc; + + rc = ddev->ops->tg_configure_hw(ddev, type, param); + if (rc) + pr_err("tg_configure_hw failed: %d\n", rc); + + return rc; +} + +static int digital_tg_listen_mdaa(struct nfc_digital_dev *ddev, u8 rf_tech) +{ + struct digital_tg_mdaa_params *params; + + params = kzalloc(sizeof(struct digital_tg_mdaa_params), GFP_KERNEL); + if (!params) + return -ENOMEM; + + params->sens_res = DIGITAL_SENS_RES_NFC_DEP; + get_random_bytes(params->nfcid1, sizeof(params->nfcid1)); + params->sel_res = DIGITAL_SEL_RES_NFC_DEP; + + params->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1; + params->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2; + get_random_bytes(params->nfcid2 + 2, NFC_NFCID2_MAXSIZE - 2); + params->sc = DIGITAL_SENSF_FELICA_SC; + + return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MDAA, NULL, params, + 500, digital_tg_recv_atr_req, NULL); +} + +static int digital_tg_listen_md(struct nfc_digital_dev *ddev, u8 rf_tech) +{ + return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MD, NULL, NULL, 500, + digital_tg_recv_md_req, NULL); +} + +int digital_target_found(struct nfc_digital_dev *ddev, + struct nfc_target *target, u8 protocol) +{ + int rc; + u8 framing; + u8 rf_tech; + u8 poll_tech_count; + int (*check_crc)(struct sk_buff *skb); + void (*add_crc)(struct sk_buff *skb); + + rf_tech = ddev->poll_techs[ddev->poll_tech_index].rf_tech; + + switch (protocol) { + case NFC_PROTO_JEWEL: + framing = NFC_DIGITAL_FRAMING_NFCA_T1T; + check_crc = digital_skb_check_crc_b; + add_crc = digital_skb_add_crc_b; + break; + + case NFC_PROTO_MIFARE: + framing = NFC_DIGITAL_FRAMING_NFCA_T2T; + check_crc = digital_skb_check_crc_a; + add_crc = digital_skb_add_crc_a; + break; + + case NFC_PROTO_FELICA: + framing = NFC_DIGITAL_FRAMING_NFCF_T3T; + check_crc = digital_skb_check_crc_f; + add_crc = digital_skb_add_crc_f; + break; + + case NFC_PROTO_NFC_DEP: + if (rf_tech == NFC_DIGITAL_RF_TECH_106A) { + framing = NFC_DIGITAL_FRAMING_NFCA_NFC_DEP; + check_crc = digital_skb_check_crc_a; + add_crc = digital_skb_add_crc_a; + } else { + framing = NFC_DIGITAL_FRAMING_NFCF_NFC_DEP; + check_crc = digital_skb_check_crc_f; + add_crc = digital_skb_add_crc_f; + } + break; + + case NFC_PROTO_ISO15693: + framing = NFC_DIGITAL_FRAMING_ISO15693_T5T; + check_crc = digital_skb_check_crc_b; + add_crc = digital_skb_add_crc_b; + break; + + case NFC_PROTO_ISO14443: + framing = NFC_DIGITAL_FRAMING_NFCA_T4T; + check_crc = digital_skb_check_crc_a; + add_crc = digital_skb_add_crc_a; + break; + + case NFC_PROTO_ISO14443_B: + framing = NFC_DIGITAL_FRAMING_NFCB_T4T; + check_crc = digital_skb_check_crc_b; + add_crc = digital_skb_add_crc_b; + break; + + default: + pr_err("Invalid protocol %d\n", protocol); + return -EINVAL; + } + + pr_debug("rf_tech=%d, protocol=%d\n", rf_tech, protocol); + + ddev->curr_rf_tech = rf_tech; + + if (DIGITAL_DRV_CAPS_IN_CRC(ddev)) { + ddev->skb_add_crc = digital_skb_add_crc_none; + ddev->skb_check_crc = digital_skb_check_crc_none; + } else { + ddev->skb_add_crc = add_crc; + ddev->skb_check_crc = check_crc; + } + + rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING, framing); + if (rc) + return rc; + + target->supported_protocols = (1 << protocol); + + poll_tech_count = ddev->poll_tech_count; + ddev->poll_tech_count = 0; + + rc = nfc_targets_found(ddev->nfc_dev, target, 1); + if (rc) { + ddev->poll_tech_count = poll_tech_count; + return rc; + } + + return 0; +} + +void digital_poll_next_tech(struct nfc_digital_dev *ddev) +{ + u8 rand_mod; + + digital_switch_rf(ddev, 0); + + mutex_lock(&ddev->poll_lock); + + if (!ddev->poll_tech_count) { + mutex_unlock(&ddev->poll_lock); + return; + } + + get_random_bytes(&rand_mod, sizeof(rand_mod)); + ddev->poll_tech_index = rand_mod % ddev->poll_tech_count; + + mutex_unlock(&ddev->poll_lock); + + schedule_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; + } + + ddev->curr_protocol = 0; +} + +static int digital_tg_send(struct nfc_dev *dev, struct sk_buff *skb) +{ + struct nfc_digital_dev *ddev = nfc_get_drvdata(dev); + + return digital_tg_send_dep_res(ddev, skb); +} + +static void digital_in_send_complete(struct nfc_digital_dev *ddev, void *arg, + struct sk_buff *resp) +{ + struct digital_data_exch *data_exch = arg; + int rc; + + if (IS_ERR(resp)) { + rc = PTR_ERR(resp); + resp = NULL; + goto done; + } + + if (ddev->curr_protocol == NFC_PROTO_MIFARE) { + rc = digital_in_recv_mifare_res(resp); + /* crc check is done in digital_in_recv_mifare_res() */ + goto done; + } + + if ((ddev->curr_protocol == NFC_PROTO_ISO14443) || + (ddev->curr_protocol == NFC_PROTO_ISO14443_B)) { + rc = digital_in_iso_dep_pull_sod(ddev, resp); + if (rc) + goto done; + } + + rc = ddev->skb_check_crc(resp); + +done: + if (rc) { + kfree_skb(resp); + resp = NULL; + } + + data_exch->cb(data_exch->cb_context, resp, rc); + + kfree(data_exch); +} + +static int digital_in_send(struct nfc_dev *nfc_dev, struct nfc_target *target, + struct sk_buff *skb, data_exchange_cb_t cb, + void *cb_context) +{ + struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev); + struct digital_data_exch *data_exch; + int rc; + + data_exch = kzalloc(sizeof(struct digital_data_exch), GFP_KERNEL); + if (!data_exch) { + pr_err("Failed to allocate data_exch struct\n"); + return -ENOMEM; + } + + data_exch->cb = cb; + data_exch->cb_context = cb_context; + + if (ddev->curr_protocol == NFC_PROTO_NFC_DEP) { + rc = digital_in_send_dep_req(ddev, target, skb, data_exch); + goto exit; + } + + if ((ddev->curr_protocol == NFC_PROTO_ISO14443) || + (ddev->curr_protocol == NFC_PROTO_ISO14443_B)) { + rc = digital_in_iso_dep_push_sod(ddev, skb); + if (rc) + goto exit; + } + + ddev->skb_add_crc(skb); + + rc = digital_in_send_cmd(ddev, skb, 500, digital_in_send_complete, + data_exch); + +exit: + if (rc) + kfree(data_exch); + + return rc; +} + +static struct nfc_ops digital_nfc_ops = { + .dev_up = digital_dev_up, + .dev_down = digital_dev_down, + .start_poll = digital_start_poll, + .stop_poll = digital_stop_poll, + .dep_link_up = digital_dep_link_up, + .dep_link_down = digital_dep_link_down, + .activate_target = digital_activate_target, + .deactivate_target = digital_deactivate_target, + .tm_send = digital_tg_send, + .im_transceive = digital_in_send, +}; + +struct nfc_digital_dev *nfc_digital_allocate_device(struct nfc_digital_ops *ops, + __u32 supported_protocols, + __u32 driver_capabilities, + int tx_headroom, int tx_tailroom) +{ + struct nfc_digital_dev *ddev; + + if (!ops->in_configure_hw || !ops->in_send_cmd || !ops->tg_listen || + !ops->tg_configure_hw || !ops->tg_send_cmd || !ops->abort_cmd || + !ops->switch_rf || (ops->tg_listen_md && !ops->tg_get_rf_tech)) + return NULL; + + ddev = kzalloc(sizeof(struct nfc_digital_dev), GFP_KERNEL); + if (!ddev) + return NULL; + + ddev->driver_capabilities = driver_capabilities; + ddev->ops = ops; + + mutex_init(&ddev->cmd_lock); + INIT_LIST_HEAD(&ddev->cmd_queue); + + INIT_WORK(&ddev->cmd_work, digital_wq_cmd); + INIT_WORK(&ddev->cmd_complete_work, digital_wq_cmd_complete); + + mutex_init(&ddev->poll_lock); + INIT_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..f864ce1 --- /dev/null +++ b/net/nfc/digital_dep.c @@ -0,0 +1,1648 @@ +/* + * 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 8 +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) + *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); + } + + memcpy(skb_put(new_skb, ddev->remote_payload_max), 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; + } + + memcpy(skb_put(ddev->chaining_skb, resp->len), 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; + memcpy(skb_put(skb, gb_len), 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(skb, 1) = rtox; + + skb_push(skb, sizeof(struct digital_dep_req_res)); + + dep_req = (struct digital_dep_req_res *)skb->data; + + dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT; + dep_req->cmd = DIGITAL_CMD_DEP_REQ; + dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU | + DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT; + + digital_skb_push_dep_sod(ddev, skb); + + ddev->skb_add_crc(skb); + + rc = digital_in_send_cmd(ddev, skb, 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; + + memcpy(skb_put(skb, sizeof(ddev->did)), &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; + + memcpy(skb_put(skb, sizeof(ddev->did)), &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; + + memcpy(skb_put(skb, sizeof(ddev->did)), &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..d9080de --- /dev/null +++ b/net/nfc/digital_technology.c @@ -0,0 +1,1306 @@ +/* + * 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_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(skb, 1) = DIGITAL_RATS_BYTE1; + *skb_put(skb, 1) = 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) { + 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(skb, sizeof(u8)) = sel_cmd; + *skb_put(skb, sizeof(u8)) = 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(skb, sizeof(u8)) = 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 = (struct digital_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 = (struct digital_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; + + *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(skb, 1) = 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; + } + + *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..aabfc72 --- /dev/null +++ b/net/nfc/hci/Kconfig @@ -0,0 +1,18 @@ +config NFC_HCI + depends on NFC + tristate "NFC HCI implementation" + depends on m + default n + help + Say Y here if you want to build support for a kernel NFC HCI + implementation. This is mostly needed for devices that only process + HCI frames, like for example the NXP pn544. + +config NFC_SHDLC + depends on NFC_HCI + depends on CRC_CCITT + bool "SHDLC link layer for HCI based NFC drivers" + default n + ---help--- + Say yes if you use an NFC HCI driver that requires SHDLC link layer. + If unsure, say N here. diff --git a/net/nfc/hci/Makefile b/net/nfc/hci/Makefile new file mode 100644 index 0000000..0a13989 --- /dev/null +++ b/net/nfc/hci/Makefile @@ -0,0 +1,8 @@ +# +# Makefile for the Linux NFC HCI layer. +# + +obj-$(CPTCFG_NFC_HCI) += hci.o + +hci-y := core.o hcp.o command.o llc.o llc_nop.o +hci-$(CPTCFG_NFC_SHDLC) += llc_shdlc.o diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c new file mode 100644 index 0000000..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..2b0f0ac --- /dev/null +++ b/net/nfc/hci/core.c @@ -0,0 +1,1096 @@ +/* + * 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; + + /* 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; + + 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(unsigned long data) +{ + struct nfc_hci_dev *hdev = (struct nfc_hci_dev *)data; + + schedule_work(&hdev->msg_tx_work); +} + +static int hci_dev_connect_gates(struct nfc_hci_dev *hdev, u8 gate_count, + struct nfc_hci_gate *gates) +{ + int r; + while (gate_count--) { + r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, + gates->gate, gates->pipe); + if (r < 0) + return r; + gates++; + } + + return 0; +} + +static int hci_dev_session_init(struct nfc_hci_dev *hdev) +{ + struct sk_buff *skb = NULL; + int r; + + if (hdev->init_data.gates[0].gate != NFC_HCI_ADMIN_GATE) + return -EPROTO; + + r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, + hdev->init_data.gates[0].gate, + hdev->init_data.gates[0].pipe); + if (r < 0) + goto exit; + + r = nfc_hci_get_param(hdev, NFC_HCI_ADMIN_GATE, + NFC_HCI_ADMIN_SESSION_IDENTITY, &skb); + if (r < 0) + goto disconnect_all; + + if (skb->len && skb->len == strlen(hdev->init_data.session_id) && + (memcmp(hdev->init_data.session_id, skb->data, + skb->len) == 0) && hdev->ops->load_session) { + /* Restore gate<->pipe table from some proprietary location. */ + + r = hdev->ops->load_session(hdev); + + if (r < 0) + goto disconnect_all; + } else { + + r = nfc_hci_disconnect_all_gates(hdev); + if (r < 0) + goto exit; + + r = hci_dev_connect_gates(hdev, hdev->init_data.gate_count, + hdev->init_data.gates); + if (r < 0) + goto disconnect_all; + + r = nfc_hci_set_param(hdev, NFC_HCI_ADMIN_GATE, + NFC_HCI_ADMIN_SESSION_IDENTITY, + hdev->init_data.session_id, + strlen(hdev->init_data.session_id)); + } + if (r == 0) + goto exit; + +disconnect_all: + nfc_hci_disconnect_all_gates(hdev); + +exit: + kfree_skb(skb); + + return r; +} + +static int hci_dev_version(struct nfc_hci_dev *hdev) +{ + int r; + struct sk_buff *skb; + + r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE, + NFC_HCI_ID_MGMT_VERSION_SW, &skb); + if (r == -EOPNOTSUPP) { + pr_info("Software/Hardware info not available\n"); + return 0; + } + if (r < 0) + return r; + + if (skb->len != 3) { + kfree_skb(skb); + return -EINVAL; + } + + hdev->sw_romlib = (skb->data[0] & 0xf0) >> 4; + hdev->sw_patch = skb->data[0] & 0x0f; + hdev->sw_flashlib_major = skb->data[1]; + hdev->sw_flashlib_minor = skb->data[2]; + + kfree_skb(skb); + + r = nfc_hci_get_param(hdev, NFC_HCI_ID_MGMT_GATE, + NFC_HCI_ID_MGMT_VERSION_HW, &skb); + if (r < 0) + return r; + + if (skb->len != 3) { + kfree_skb(skb); + return -EINVAL; + } + + hdev->hw_derivative = (skb->data[0] & 0xe0) >> 5; + hdev->hw_version = skb->data[0] & 0x1f; + hdev->hw_mpw = (skb->data[1] & 0xc0) >> 6; + hdev->hw_software = skb->data[1] & 0x3f; + hdev->hw_bsid = skb->data[2]; + + kfree_skb(skb); + + pr_info("SOFTWARE INFO:\n"); + pr_info("RomLib : %d\n", hdev->sw_romlib); + pr_info("Patch : %d\n", hdev->sw_patch); + pr_info("FlashLib Major : %d\n", hdev->sw_flashlib_major); + pr_info("FlashLib Minor : %d\n", hdev->sw_flashlib_minor); + pr_info("HARDWARE INFO:\n"); + pr_info("Derivative : %d\n", hdev->hw_derivative); + pr_info("HW Version : %d\n", hdev->hw_version); + pr_info("#MPW : %d\n", hdev->hw_mpw); + pr_info("Software : %d\n", hdev->hw_software); + pr_info("BSID Version : %d\n", hdev->hw_bsid); + + return 0; +} + +static int hci_dev_up(struct nfc_dev *nfc_dev) +{ + struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); + int r = 0; + + if (hdev->ops->open) { + r = hdev->ops->open(hdev); + if (r < 0) + return r; + } + + r = nfc_llc_start(hdev->llc); + if (r < 0) + goto exit_close; + + r = hci_dev_session_init(hdev); + if (r < 0) + goto exit_llc; + + r = nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE, + NFC_HCI_EVT_END_OPERATION, NULL, 0); + if (r < 0) + goto exit_llc; + + if (hdev->ops->hci_ready) { + r = hdev->ops->hci_ready(hdev); + if (r < 0) + goto exit_llc; + } + + r = hci_dev_version(hdev); + if (r < 0) + goto exit_llc; + + return 0; + +exit_llc: + nfc_llc_stop(hdev->llc); + +exit_close: + if (hdev->ops->close) + hdev->ops->close(hdev); + + return r; +} + +static int hci_dev_down(struct nfc_dev *nfc_dev) +{ + struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); + + nfc_llc_stop(hdev->llc); + + if (hdev->ops->close) + hdev->ops->close(hdev); + + 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; + } + + *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(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN) = pipe; + + skb_queue_walk(&hdev->rx_hcp_frags, frag_skb) { + msg_len = frag_skb->len - NFC_HCI_HCP_PACKET_HEADER_LEN; + memcpy(skb_put(hcp_skb, msg_len), + frag_skb->data + NFC_HCI_HCP_PACKET_HEADER_LEN, + msg_len); + } + + skb_queue_purge(&hdev->rx_hcp_frags); + } else { + packet->header &= NFC_HCI_FRAGMENT; + hcp_skb = skb; + } + + /* if this is a response, dispatch immediately to + * unblock waiting cmd context. Otherwise, enqueue to dispatch + * in separate context where handler can also execute command. + */ + packet = (struct hcp_packet *)hcp_skb->data; + type = HCP_MSG_GET_TYPE(packet->message.header); + if (type == NFC_HCI_HCP_RESPONSE) { + pipe = packet->header; + instruction = HCP_MSG_GET_CMD(packet->message.header); + skb_pull(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN + + NFC_HCI_HCP_MESSAGE_HEADER_LEN); + nfc_hci_hcp_message_rx(hdev, pipe, type, instruction, hcp_skb); + } else { + skb_queue_tail(&hdev->msg_rx_queue, hcp_skb); + schedule_work(&hdev->msg_rx_work); + } +} + +static int hci_fw_download(struct nfc_dev *nfc_dev, const char *firmware_name) +{ + struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev); + + if (!hdev->ops->fw_download) + return -ENOTSUPP; + + return hdev->ops->fw_download(hdev, firmware_name); +} + +static struct nfc_ops hci_nfc_ops = { + .dev_up = hci_dev_up, + .dev_down = hci_dev_down, + .start_poll = hci_start_poll, + .stop_poll = hci_stop_poll, + .dep_link_up = hci_dep_link_up, + .dep_link_down = hci_dep_link_down, + .activate_target = hci_activate_target, + .deactivate_target = hci_deactivate_target, + .im_transceive = hci_transceive, + .tm_send = hci_tm_send, + .check_presence = hci_check_presence, + .fw_download = hci_fw_download, + .discover_se = hci_discover_se, + .enable_se = hci_enable_se, + .disable_se = hci_disable_se, + .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); + + init_timer(&hdev->cmd_timer); + hdev->cmd_timer.data = (unsigned long)hdev; + hdev->cmd_timer.function = nfc_hci_cmd_timeout; + + skb_queue_head_init(&hdev->rx_hcp_frags); + + INIT_WORK(&hdev->msg_rx_work, nfc_hci_msg_rx_work); + + skb_queue_head_init(&hdev->msg_rx_queue); + + return nfc_register_device(hdev->ndev); +} +EXPORT_SYMBOL(nfc_hci_register_device); + +void nfc_hci_unregister_device(struct nfc_hci_dev *hdev) +{ + struct hci_msg *msg, *n; + + mutex_lock(&hdev->msg_tx_mutex); + + if (hdev->cmd_pending_msg) { + if (hdev->cmd_pending_msg->cb) + hdev->cmd_pending_msg->cb( + hdev->cmd_pending_msg->cb_context, + NULL, -ESHUTDOWN); + kfree(hdev->cmd_pending_msg); + hdev->cmd_pending_msg = NULL; + } + + hdev->shutting_down = true; + + mutex_unlock(&hdev->msg_tx_mutex); + + del_timer_sync(&hdev->cmd_timer); + cancel_work_sync(&hdev->msg_tx_work); + + cancel_work_sync(&hdev->msg_rx_work); + + nfc_unregister_device(hdev->ndev); + + skb_queue_purge(&hdev->rx_hcp_frags); + skb_queue_purge(&hdev->msg_rx_queue); + + list_for_each_entry_safe(msg, n, &hdev->msg_tx_queue, msg_l) { + list_del(&msg->msg_l); + skb_queue_purge(&msg->msg_frags); + kfree(msg); + } +} +EXPORT_SYMBOL(nfc_hci_unregister_device); + +void nfc_hci_set_clientdata(struct nfc_hci_dev *hdev, void *clientdata) +{ + hdev->clientdata = clientdata; +} +EXPORT_SYMBOL(nfc_hci_set_clientdata); + +void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev) +{ + return hdev->clientdata; +} +EXPORT_SYMBOL(nfc_hci_get_clientdata); + +void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err) +{ + nfc_hci_failure(hdev, err); +} +EXPORT_SYMBOL(nfc_hci_driver_failure); + +void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb) +{ + nfc_llc_rcv_from_drv(hdev->llc, skb); +} +EXPORT_SYMBOL(nfc_hci_recv_frame); + +static int __init nfc_hci_init(void) +{ + return nfc_llc_init(); +} + +static void __exit nfc_hci_exit(void) +{ + nfc_llc_exit(); +} + +subsys_initcall(nfc_hci_init); +module_exit(nfc_hci_exit); + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("NFC HCI Core"); diff --git a/net/nfc/hci/hci.h b/net/nfc/hci/hci.h new file mode 100644 index 0000000..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..1f23124 --- /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(CPTCFG_NFC_SHDLC) +int nfc_llc_shdlc_register(void); +#else +static inline int nfc_llc_shdlc_register(void) +{ + return 0; +} +#endif + +#endif /* __LOCAL_LLC_H_ */ diff --git a/net/nfc/hci/llc_nop.c b/net/nfc/hci/llc_nop.c new file mode 100644 index 0000000..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..401c7e2 --- /dev/null +++ b/net/nfc/hci/llc_shdlc.c @@ -0,0 +1,854 @@ +/* + * shdlc Link Layer Control + * + * Copyright (C) 2012 Intel Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, see <http://www.gnu.org/licenses/>. + */ + +#define pr_fmt(fmt) "shdlc: %s: " fmt, __func__ + +#include <linux/types.h> +#include <linux/sched.h> +#include <linux/wait.h> +#include <linux/slab.h> +#include <linux/skbuff.h> + +#include "llc.h" + +enum shdlc_state { + SHDLC_DISCONNECTED = 0, + SHDLC_CONNECTING = 1, + SHDLC_NEGOTIATING = 2, + SHDLC_HALF_CONNECTED = 3, + SHDLC_CONNECTED = 4 +}; + +struct llc_shdlc { + struct nfc_hci_dev *hdev; + xmit_to_drv_t xmit_to_drv; + rcv_to_hci_t rcv_to_hci; + + struct mutex state_mutex; + enum shdlc_state state; + int hard_fault; + + wait_queue_head_t *connect_wq; + int connect_tries; + int connect_result; + struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */ + + u8 w; /* window size */ + bool srej_support; + + struct timer_list t1_timer; /* send ack timeout */ + bool t1_active; + + struct timer_list t2_timer; /* guard/retransmit timeout */ + bool t2_active; + + int ns; /* next seq num for send */ + int nr; /* next expected seq num for receive */ + int dnr; /* oldest sent unacked seq num */ + + struct sk_buff_head rcv_q; + + struct sk_buff_head send_q; + bool rnr; /* other side is not ready to receive */ + + struct sk_buff_head ack_pending_q; + + struct work_struct sm_work; + + int tx_headroom; + int tx_tailroom; + + llc_failure_t llc_failure; +}; + +#define SHDLC_LLC_HEAD_ROOM 2 + +#define SHDLC_MAX_WINDOW 4 +#define SHDLC_SREJ_SUPPORT false + +#define SHDLC_CONTROL_HEAD_MASK 0xe0 +#define SHDLC_CONTROL_HEAD_I 0x80 +#define SHDLC_CONTROL_HEAD_I2 0xa0 +#define SHDLC_CONTROL_HEAD_S 0xc0 +#define SHDLC_CONTROL_HEAD_U 0xe0 + +#define SHDLC_CONTROL_NS_MASK 0x38 +#define SHDLC_CONTROL_NR_MASK 0x07 +#define SHDLC_CONTROL_TYPE_MASK 0x18 + +#define SHDLC_CONTROL_M_MASK 0x1f + +enum sframe_type { + S_FRAME_RR = 0x00, + S_FRAME_REJ = 0x01, + S_FRAME_RNR = 0x02, + S_FRAME_SREJ = 0x03 +}; + +enum uframe_modifier { + U_FRAME_UA = 0x06, + U_FRAME_RSET = 0x19 +}; + +#define SHDLC_CONNECT_VALUE_MS 5 +#define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4) +#define SHDLC_T2_VALUE_MS 300 + +#define SHDLC_DUMP_SKB(info, skb) \ +do { \ + pr_debug("%s:\n", info); \ + print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \ + 16, 1, skb->data, skb->len, 0); \ +} while (0) + +/* checks x < y <= z modulo 8 */ +static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z) +{ + if (x < z) + return ((x < y) && (y <= z)) ? true : false; + else + return ((y > x) || (y <= z)) ? true : false; +} + +/* checks x <= y < z modulo 8 */ +static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z) +{ + if (x <= z) + return ((x <= y) && (y < z)) ? true : false; + else /* x > z -> z+8 > x */ + return ((y >= x) || (y < z)) ? true : false; +} + +static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc, + int payload_len) +{ + struct sk_buff *skb; + + skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM + + shdlc->tx_tailroom + payload_len, GFP_KERNEL); + if (skb) + skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM); + + return skb; +} + +/* immediately sends an S frame. */ +static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc, + enum sframe_type sframe_type, int nr) +{ + int r; + struct sk_buff *skb; + + pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr); + + skb = llc_shdlc_alloc_skb(shdlc, 0); + if (skb == NULL) + return -ENOMEM; + + *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr; + + r = shdlc->xmit_to_drv(shdlc->hdev, skb); + + kfree_skb(skb); + + return r; +} + +/* immediately sends an U frame. skb may contain optional payload */ +static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc, + struct sk_buff *skb, + enum uframe_modifier uframe_modifier) +{ + int r; + + pr_debug("uframe_modifier=%d\n", uframe_modifier); + + *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier; + + r = shdlc->xmit_to_drv(shdlc->hdev, skb); + + kfree_skb(skb); + + return r; +} + +/* + * Free ack_pending frames until y_nr - 1, and reset t2 according to + * the remaining oldest ack_pending frame sent time + */ +static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr) +{ + struct sk_buff *skb; + int dnr = shdlc->dnr; /* MUST initially be < y_nr */ + + pr_debug("release ack pending up to frame %d excluded\n", y_nr); + + while (dnr != y_nr) { + pr_debug("release ack pending frame %d\n", dnr); + + skb = skb_dequeue(&shdlc->ack_pending_q); + kfree_skb(skb); + + dnr = (dnr + 1) % 8; + } + + if (skb_queue_empty(&shdlc->ack_pending_q)) { + if (shdlc->t2_active) { + del_timer_sync(&shdlc->t2_timer); + shdlc->t2_active = false; + + pr_debug + ("All sent frames acked. Stopped T2(retransmit)\n"); + } + } else { + skb = skb_peek(&shdlc->ack_pending_q); + + mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb + + msecs_to_jiffies(SHDLC_T2_VALUE_MS)); + shdlc->t2_active = true; + + pr_debug + ("Start T2(retransmit) for remaining unacked sent frames\n"); + } +} + +/* + * Receive validated frames from lower layer. skb contains HCI payload only. + * Handle according to algorithm at spec:10.8.2 + */ +static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc, + struct sk_buff *skb, int ns, int nr) +{ + int x_ns = ns; + int y_nr = nr; + + pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr); + + if (shdlc->state != SHDLC_CONNECTED) + goto exit; + + if (x_ns != shdlc->nr) { + llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr); + goto exit; + } + + if (shdlc->t1_active == false) { + shdlc->t1_active = true; + mod_timer(&shdlc->t1_timer, jiffies + + msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w))); + pr_debug("(re)Start T1(send ack)\n"); + } + + if (skb->len) { + shdlc->rcv_to_hci(shdlc->hdev, skb); + skb = NULL; + } + + shdlc->nr = (shdlc->nr + 1) % 8; + + if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { + llc_shdlc_reset_t2(shdlc, y_nr); + + shdlc->dnr = y_nr; + } + +exit: + kfree_skb(skb); +} + +static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr) +{ + pr_debug("remote acked up to frame %d excluded\n", y_nr); + + if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { + llc_shdlc_reset_t2(shdlc, y_nr); + shdlc->dnr = y_nr; + } +} + +static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc) +{ + struct sk_buff *skb; + + pr_debug("ns reset to %d\n", shdlc->dnr); + + while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) { + skb_pull(skb, 1); /* remove control field */ + skb_queue_head(&shdlc->send_q, skb); + } + shdlc->ns = shdlc->dnr; +} + +static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr) +{ + struct sk_buff *skb; + + pr_debug("remote asks retransmission from frame %d\n", y_nr); + + if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) { + if (shdlc->t2_active) { + del_timer_sync(&shdlc->t2_timer); + shdlc->t2_active = false; + pr_debug("Stopped T2(retransmit)\n"); + } + + if (shdlc->dnr != y_nr) { + while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) { + skb = skb_dequeue(&shdlc->ack_pending_q); + kfree_skb(skb); + } + } + + llc_shdlc_requeue_ack_pending(shdlc); + } +} + +/* See spec RR:10.8.3 REJ:10.8.4 */ +static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc, + enum sframe_type s_frame_type, int nr) +{ + struct sk_buff *skb; + + if (shdlc->state != SHDLC_CONNECTED) + return; + + switch (s_frame_type) { + case S_FRAME_RR: + llc_shdlc_rcv_ack(shdlc, nr); + if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */ + shdlc->rnr = false; + if (shdlc->send_q.qlen == 0) { + skb = llc_shdlc_alloc_skb(shdlc, 0); + if (skb) + skb_queue_tail(&shdlc->send_q, skb); + } + } + break; + case S_FRAME_REJ: + llc_shdlc_rcv_rej(shdlc, nr); + break; + case S_FRAME_RNR: + llc_shdlc_rcv_ack(shdlc, nr); + shdlc->rnr = true; + break; + default: + break; + } +} + +static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r) +{ + pr_debug("result=%d\n", r); + + del_timer_sync(&shdlc->connect_timer); + + if (r == 0) { + shdlc->ns = 0; + shdlc->nr = 0; + shdlc->dnr = 0; + + shdlc->state = SHDLC_HALF_CONNECTED; + } else { + shdlc->state = SHDLC_DISCONNECTED; + } + + shdlc->connect_result = r; + + wake_up(shdlc->connect_wq); +} + +static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc) +{ + struct sk_buff *skb; + + pr_debug("\n"); + + skb = llc_shdlc_alloc_skb(shdlc, 2); + if (skb == NULL) + return -ENOMEM; + + *skb_put(skb, 1) = SHDLC_MAX_WINDOW; + *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0; + + return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET); +} + +static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc) +{ + struct sk_buff *skb; + + pr_debug("\n"); + + skb = llc_shdlc_alloc_skb(shdlc, 0); + if (skb == NULL) + return -ENOMEM; + + return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA); +} + +static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc, + struct sk_buff *skb, + enum uframe_modifier u_frame_modifier) +{ + u8 w = SHDLC_MAX_WINDOW; + bool srej_support = SHDLC_SREJ_SUPPORT; + int r; + + pr_debug("u_frame_modifier=%d\n", u_frame_modifier); + + switch (u_frame_modifier) { + case U_FRAME_RSET: + switch (shdlc->state) { + case SHDLC_NEGOTIATING: + case SHDLC_CONNECTING: + /* + * We sent RSET, but chip wants to negociate or we + * got RSET before we managed to send out our. + */ + if (skb->len > 0) + w = skb->data[0]; + + if (skb->len > 1) + srej_support = skb->data[1] & 0x01 ? true : + false; + + if ((w <= SHDLC_MAX_WINDOW) && + (SHDLC_SREJ_SUPPORT || (srej_support == false))) { + shdlc->w = w; + shdlc->srej_support = srej_support; + r = llc_shdlc_connect_send_ua(shdlc); + llc_shdlc_connect_complete(shdlc, r); + } + break; + case SHDLC_HALF_CONNECTED: + /* + * Chip resent RSET due to its timeout - Ignote it + * as we already sent UA. + */ + break; + case SHDLC_CONNECTED: + /* + * Chip wants to reset link. This is unexpected and + * unsupported. + */ + shdlc->hard_fault = -ECONNRESET; + break; + default: + break; + } + break; + case U_FRAME_UA: + if ((shdlc->state == SHDLC_CONNECTING && + shdlc->connect_tries > 0) || + (shdlc->state == SHDLC_NEGOTIATING)) { + llc_shdlc_connect_complete(shdlc, 0); + shdlc->state = SHDLC_CONNECTED; + } + break; + default: + break; + } + + kfree_skb(skb); +} + +static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc) +{ + struct sk_buff *skb; + u8 control; + int nr; + int ns; + enum sframe_type s_frame_type; + enum uframe_modifier u_frame_modifier; + + if (shdlc->rcv_q.qlen) + pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen); + + while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) { + control = skb->data[0]; + skb_pull(skb, 1); + switch (control & SHDLC_CONTROL_HEAD_MASK) { + case SHDLC_CONTROL_HEAD_I: + case SHDLC_CONTROL_HEAD_I2: + if (shdlc->state == SHDLC_HALF_CONNECTED) + shdlc->state = SHDLC_CONNECTED; + + ns = (control & SHDLC_CONTROL_NS_MASK) >> 3; + nr = control & SHDLC_CONTROL_NR_MASK; + llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr); + break; + case SHDLC_CONTROL_HEAD_S: + if (shdlc->state == SHDLC_HALF_CONNECTED) + shdlc->state = SHDLC_CONNECTED; + + s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3; + nr = control & SHDLC_CONTROL_NR_MASK; + llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr); + kfree_skb(skb); + break; + case SHDLC_CONTROL_HEAD_U: + u_frame_modifier = control & SHDLC_CONTROL_M_MASK; + llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier); + break; + default: + pr_err("UNKNOWN Control=%d\n", control); + kfree_skb(skb); + break; + } + } +} + +static int llc_shdlc_w_used(int ns, int dnr) +{ + int unack_count; + + if (dnr <= ns) + unack_count = ns - dnr; + else + unack_count = 8 - dnr + ns; + + return unack_count; +} + +/* Send frames according to algorithm at spec:10.8.1 */ +static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc) +{ + struct sk_buff *skb; + int r; + unsigned long time_sent; + + if (shdlc->send_q.qlen) + pr_debug + ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n", + shdlc->send_q.qlen, shdlc->ns, shdlc->dnr, + shdlc->rnr == false ? "false" : "true", + shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr), + shdlc->ack_pending_q.qlen); + + while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w && + (shdlc->rnr == false)) { + + if (shdlc->t1_active) { + del_timer_sync(&shdlc->t1_timer); + shdlc->t1_active = false; + pr_debug("Stopped T1(send ack)\n"); + } + + skb = skb_dequeue(&shdlc->send_q); + + *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) | + shdlc->nr; + + pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns, + shdlc->nr); + SHDLC_DUMP_SKB("shdlc frame written", skb); + + r = shdlc->xmit_to_drv(shdlc->hdev, skb); + if (r < 0) { + shdlc->hard_fault = r; + break; + } + + shdlc->ns = (shdlc->ns + 1) % 8; + + time_sent = jiffies; + *(unsigned long *)skb->cb = time_sent; + + skb_queue_tail(&shdlc->ack_pending_q, skb); + + if (shdlc->t2_active == false) { + shdlc->t2_active = true; + mod_timer(&shdlc->t2_timer, time_sent + + msecs_to_jiffies(SHDLC_T2_VALUE_MS)); + pr_debug("Started T2 (retransmit)\n"); + } + } +} + +static void llc_shdlc_connect_timeout(unsigned long data) +{ + struct llc_shdlc *shdlc = (struct llc_shdlc *)data; + + pr_debug("\n"); + + schedule_work(&shdlc->sm_work); +} + +static void llc_shdlc_t1_timeout(unsigned long data) +{ + struct llc_shdlc *shdlc = (struct llc_shdlc *)data; + + pr_debug("SoftIRQ: need to send ack\n"); + + schedule_work(&shdlc->sm_work); +} + +static void llc_shdlc_t2_timeout(unsigned long data) +{ + struct llc_shdlc *shdlc = (struct llc_shdlc *)data; + + pr_debug("SoftIRQ: need to retransmit\n"); + + schedule_work(&shdlc->sm_work); +} + +static void llc_shdlc_sm_work(struct work_struct *work) +{ + struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work); + int r; + + pr_debug("\n"); + + mutex_lock(&shdlc->state_mutex); + + switch (shdlc->state) { + case SHDLC_DISCONNECTED: + skb_queue_purge(&shdlc->rcv_q); + skb_queue_purge(&shdlc->send_q); + skb_queue_purge(&shdlc->ack_pending_q); + break; + case SHDLC_CONNECTING: + if (shdlc->hard_fault) { + llc_shdlc_connect_complete(shdlc, shdlc->hard_fault); + break; + } + + if (shdlc->connect_tries++ < 5) + r = llc_shdlc_connect_initiate(shdlc); + else + r = -ETIME; + if (r < 0) { + llc_shdlc_connect_complete(shdlc, r); + } else { + mod_timer(&shdlc->connect_timer, jiffies + + msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS)); + + shdlc->state = SHDLC_NEGOTIATING; + } + break; + case SHDLC_NEGOTIATING: + if (timer_pending(&shdlc->connect_timer) == 0) { + shdlc->state = SHDLC_CONNECTING; + schedule_work(&shdlc->sm_work); + } + + llc_shdlc_handle_rcv_queue(shdlc); + + if (shdlc->hard_fault) { + llc_shdlc_connect_complete(shdlc, shdlc->hard_fault); + break; + } + break; + case SHDLC_HALF_CONNECTED: + case SHDLC_CONNECTED: + llc_shdlc_handle_rcv_queue(shdlc); + llc_shdlc_handle_send_queue(shdlc); + + if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) { + pr_debug + ("Handle T1(send ack) elapsed (T1 now inactive)\n"); + + shdlc->t1_active = false; + r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR, + shdlc->nr); + if (r < 0) + shdlc->hard_fault = r; + } + + if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) { + pr_debug + ("Handle T2(retransmit) elapsed (T2 inactive)\n"); + + shdlc->t2_active = false; + + llc_shdlc_requeue_ack_pending(shdlc); + llc_shdlc_handle_send_queue(shdlc); + } + + if (shdlc->hard_fault) + shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault); + break; + default: + break; + } + mutex_unlock(&shdlc->state_mutex); +} + +/* + * Called from syscall context to establish shdlc link. Sleeps until + * link is ready or failure. + */ +static int llc_shdlc_connect(struct llc_shdlc *shdlc) +{ + DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq); + + pr_debug("\n"); + + mutex_lock(&shdlc->state_mutex); + + shdlc->state = SHDLC_CONNECTING; + shdlc->connect_wq = &connect_wq; + shdlc->connect_tries = 0; + shdlc->connect_result = 1; + + mutex_unlock(&shdlc->state_mutex); + + schedule_work(&shdlc->sm_work); + + wait_event(connect_wq, shdlc->connect_result != 1); + + return shdlc->connect_result; +} + +static void llc_shdlc_disconnect(struct llc_shdlc *shdlc) +{ + pr_debug("\n"); + + mutex_lock(&shdlc->state_mutex); + + shdlc->state = SHDLC_DISCONNECTED; + + mutex_unlock(&shdlc->state_mutex); + + schedule_work(&shdlc->sm_work); +} + +/* + * Receive an incoming shdlc frame. Frame has already been crc-validated. + * skb contains only LLC header and payload. + * If skb == NULL, it is a notification that the link below is dead. + */ +static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb) +{ + if (skb == NULL) { + pr_err("NULL Frame -> link is dead\n"); + shdlc->hard_fault = -EREMOTEIO; + } else { + SHDLC_DUMP_SKB("incoming frame", skb); + skb_queue_tail(&shdlc->rcv_q, skb); + } + + schedule_work(&shdlc->sm_work); +} + +static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv, + rcv_to_hci_t rcv_to_hci, int tx_headroom, + int tx_tailroom, int *rx_headroom, int *rx_tailroom, + llc_failure_t llc_failure) +{ + struct llc_shdlc *shdlc; + + *rx_headroom = SHDLC_LLC_HEAD_ROOM; + *rx_tailroom = 0; + + shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL); + if (shdlc == NULL) + return NULL; + + mutex_init(&shdlc->state_mutex); + shdlc->state = SHDLC_DISCONNECTED; + + init_timer(&shdlc->connect_timer); + shdlc->connect_timer.data = (unsigned long)shdlc; + shdlc->connect_timer.function = llc_shdlc_connect_timeout; + + init_timer(&shdlc->t1_timer); + shdlc->t1_timer.data = (unsigned long)shdlc; + shdlc->t1_timer.function = llc_shdlc_t1_timeout; + + init_timer(&shdlc->t2_timer); + shdlc->t2_timer.data = (unsigned long)shdlc; + shdlc->t2_timer.function = llc_shdlc_t2_timeout; + + shdlc->w = SHDLC_MAX_WINDOW; + shdlc->srej_support = SHDLC_SREJ_SUPPORT; + + skb_queue_head_init(&shdlc->rcv_q); + skb_queue_head_init(&shdlc->send_q); + skb_queue_head_init(&shdlc->ack_pending_q); + + INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work); + + shdlc->hdev = hdev; + shdlc->xmit_to_drv = xmit_to_drv; + shdlc->rcv_to_hci = rcv_to_hci; + shdlc->tx_headroom = tx_headroom; + shdlc->tx_tailroom = tx_tailroom; + shdlc->llc_failure = llc_failure; + + return shdlc; +} + +static void llc_shdlc_deinit(struct nfc_llc *llc) +{ + struct llc_shdlc *shdlc = nfc_llc_get_data(llc); + + skb_queue_purge(&shdlc->rcv_q); + skb_queue_purge(&shdlc->send_q); + skb_queue_purge(&shdlc->ack_pending_q); + + kfree(shdlc); +} + +static int llc_shdlc_start(struct nfc_llc *llc) +{ + struct llc_shdlc *shdlc = nfc_llc_get_data(llc); + + return llc_shdlc_connect(shdlc); +} + +static int llc_shdlc_stop(struct nfc_llc *llc) +{ + struct llc_shdlc *shdlc = nfc_llc_get_data(llc); + + llc_shdlc_disconnect(shdlc); + + return 0; +} + +static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb) +{ + struct llc_shdlc *shdlc = nfc_llc_get_data(llc); + + llc_shdlc_recv_frame(shdlc, skb); +} + +static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb) +{ + struct llc_shdlc *shdlc = nfc_llc_get_data(llc); + + skb_queue_tail(&shdlc->send_q, skb); + + schedule_work(&shdlc->sm_work); + + return 0; +} + +static struct nfc_llc_ops llc_shdlc_ops = { + .init = llc_shdlc_init, + .deinit = llc_shdlc_deinit, + .start = llc_shdlc_start, + .stop = llc_shdlc_stop, + .rcv_from_drv = llc_shdlc_rcv_from_drv, + .xmit_from_hci = llc_shdlc_xmit_from_hci, +}; + +int nfc_llc_shdlc_register(void) +{ + return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops); +} 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..c5959ce --- /dev/null +++ b/net/nfc/llcp_commands.c @@ -0,0 +1,798 @@ +/* + * Copyright (C) 2011 Intel Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, see <http://www.gnu.org/licenses/>. + */ + +#define pr_fmt(fmt) "llcp: %s: " fmt, __func__ + +#include <linux/init.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/nfc.h> + +#include <net/nfc/nfc.h> + +#include "nfc.h" +#include "llcp.h" + +static u8 llcp_tlv_length[LLCP_TLV_MAX] = { + 0, + 1, /* VERSION */ + 2, /* MIUX */ + 2, /* WKS */ + 1, /* LTO */ + 1, /* RW */ + 0, /* SN */ + 1, /* OPT */ + 0, /* SDREQ */ + 2, /* SDRES */ + +}; + +static u8 llcp_tlv8(u8 *tlv, u8 type) +{ + if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) + return 0; + + return tlv[2]; +} + +static u16 llcp_tlv16(u8 *tlv, u8 type) +{ + if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) + return 0; + + return be16_to_cpu(*((__be16 *)(tlv + 2))); +} + + +static u8 llcp_tlv_version(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_VERSION); +} + +static u16 llcp_tlv_miux(u8 *tlv) +{ + return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff; +} + +static u16 llcp_tlv_wks(u8 *tlv) +{ + return llcp_tlv16(tlv, LLCP_TLV_WKS); +} + +static u16 llcp_tlv_lto(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_LTO); +} + +static u8 llcp_tlv_opt(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_OPT); +} + +static u8 llcp_tlv_rw(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf; +} + +u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) +{ + u8 *tlv, length; + + pr_debug("type %d\n", type); + + if (type >= LLCP_TLV_MAX) + return NULL; + + length = llcp_tlv_length[type]; + if (length == 0 && value_length == 0) + return NULL; + else if (length == 0) + length = value_length; + + *tlv_length = 2 + length; + tlv = kzalloc(2 + length, GFP_KERNEL); + if (tlv == NULL) + return tlv; + + tlv[0] = type; + tlv[1] = length; + memcpy(tlv + 2, value, length); + + return tlv; +} + +struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap) +{ + struct nfc_llcp_sdp_tlv *sdres; + u8 value[2]; + + sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); + if (sdres == NULL) + return NULL; + + value[0] = tid; + value[1] = sap; + + sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2, + &sdres->tlv_len); + if (sdres->tlv == NULL) { + kfree(sdres); + return NULL; + } + + sdres->tid = tid; + sdres->sap = sap; + + INIT_HLIST_NODE(&sdres->node); + + return sdres; +} + +struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, + size_t uri_len) +{ + struct nfc_llcp_sdp_tlv *sdreq; + + pr_debug("uri: %s, len: %zu\n", uri, uri_len); + + sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); + if (sdreq == NULL) + return NULL; + + sdreq->tlv_len = uri_len + 3; + + if (uri[uri_len - 1] == 0) + sdreq->tlv_len--; + + sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL); + if (sdreq->tlv == NULL) { + kfree(sdreq); + return NULL; + } + + sdreq->tlv[0] = LLCP_TLV_SDREQ; + sdreq->tlv[1] = sdreq->tlv_len - 2; + sdreq->tlv[2] = tid; + + sdreq->tid = tid; + sdreq->uri = sdreq->tlv + 3; + memcpy(sdreq->uri, uri, uri_len); + + sdreq->time = jiffies; + + INIT_HLIST_NODE(&sdreq->node); + + return sdreq; +} + +void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) +{ + kfree(sdp->tlv); + kfree(sdp); +} + +void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head) +{ + struct nfc_llcp_sdp_tlv *sdp; + struct hlist_node *n; + + hlist_for_each_entry_safe(sdp, n, head, node) { + hlist_del(&sdp->node); + + nfc_llcp_free_sdp_tlv(sdp); + } +} + +int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, + u8 *tlv_array, u16 tlv_array_len) +{ + u8 *tlv = tlv_array, type, length, offset = 0; + + pr_debug("TLV array length %d\n", tlv_array_len); + + if (local == NULL) + return -ENODEV; + + while (offset < tlv_array_len) { + type = tlv[0]; + length = tlv[1]; + + pr_debug("type 0x%x length %d\n", type, length); + + switch (type) { + case LLCP_TLV_VERSION: + local->remote_version = llcp_tlv_version(tlv); + break; + case LLCP_TLV_MIUX: + local->remote_miu = llcp_tlv_miux(tlv) + 128; + break; + case LLCP_TLV_WKS: + local->remote_wks = llcp_tlv_wks(tlv); + break; + case LLCP_TLV_LTO: + local->remote_lto = llcp_tlv_lto(tlv) * 10; + break; + case LLCP_TLV_OPT: + local->remote_opt = llcp_tlv_opt(tlv); + break; + default: + pr_err("Invalid gt tlv value 0x%x\n", type); + break; + } + + offset += length + 2; + tlv += length + 2; + } + + pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n", + local->remote_version, local->remote_miu, + local->remote_lto, local->remote_opt, + local->remote_wks); + + return 0; +} + +int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, + u8 *tlv_array, u16 tlv_array_len) +{ + u8 *tlv = tlv_array, type, length, offset = 0; + + pr_debug("TLV array length %d\n", tlv_array_len); + + if (sock == NULL) + return -ENOTCONN; + + while (offset < tlv_array_len) { + type = tlv[0]; + length = tlv[1]; + + pr_debug("type 0x%x length %d\n", type, length); + + switch (type) { + case LLCP_TLV_MIUX: + sock->remote_miu = llcp_tlv_miux(tlv) + 128; + break; + case LLCP_TLV_RW: + sock->remote_rw = llcp_tlv_rw(tlv); + break; + case LLCP_TLV_SN: + break; + default: + pr_err("Invalid gt tlv value 0x%x\n", type); + break; + } + + offset += length + 2; + tlv += length + 2; + } + + pr_debug("sock %p rw %d miu %d\n", sock, + sock->remote_rw, sock->remote_miu); + + return 0; +} + +static struct sk_buff *llcp_add_header(struct sk_buff *pdu, + u8 dsap, u8 ssap, u8 ptype) +{ + u8 header[2]; + + pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); + + header[0] = (u8)((dsap << 2) | (ptype >> 2)); + header[1] = (u8)((ptype << 6) | ssap); + + pr_debug("header 0x%x 0x%x\n", header[0], header[1]); + + memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE); + + return pdu; +} + +static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, + u8 tlv_length) +{ + /* XXX Add an skb length check */ + + if (tlv == NULL) + return NULL; + + memcpy(skb_put(pdu, tlv_length), tlv, tlv_length); + + return pdu; +} + +static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, + u8 cmd, u16 size) +{ + struct sk_buff *skb; + int err; + + if (sock->ssap == 0) + return NULL; + + skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, + size + LLCP_HEADER_SIZE, &err); + if (skb == NULL) { + pr_err("Could not allocate PDU\n"); + return NULL; + } + + skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd); + + return skb; +} + +int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + + pr_debug("Sending DISC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + dev = sock->dev; + if (dev == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0); + if (skb == NULL) + return -ENOMEM; + + skb_queue_tail(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_symm(struct nfc_dev *dev) +{ + struct sk_buff *skb; + struct nfc_llcp_local *local; + u16 size = 0; + + pr_debug("Sending SYMM\n"); + + local = nfc_llcp_find_local(dev); + if (local == NULL) + return -ENODEV; + + size += LLCP_HEADER_SIZE; + size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; + + skb = alloc_skb(size, GFP_KERNEL); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); + + __net_timestamp(skb); + + nfc_llcp_send_to_raw_sock(local, skb, NFC_DIRECTION_TX); + + return nfc_data_exchange(dev, local->target_idx, skb, + nfc_llcp_recv, local); +} + +int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) +{ + struct nfc_llcp_local *local; + struct sk_buff *skb; + u8 *service_name_tlv = NULL, service_name_tlv_length; + u8 *miux_tlv = NULL, miux_tlv_length; + u8 *rw_tlv = NULL, rw_tlv_length, rw; + int err; + u16 size = 0; + __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) { + memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len); + + hlist_del(&sdp->node); + + nfc_llcp_free_sdp_tlv(sdp); + } + + skb_queue_tail(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, + struct hlist_head *tlv_list, size_t tlvs_len) +{ + struct nfc_llcp_sdp_tlv *sdreq; + struct hlist_node *n; + struct sk_buff *skb; + + skb = nfc_llcp_allocate_snl(local, tlvs_len); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + mutex_lock(&local->sdreq_lock); + + if (hlist_empty(&local->pending_sdreqs)) + mod_timer(&local->sdreq_timer, + jiffies + msecs_to_jiffies(3 * local->remote_lto)); + + hlist_for_each_entry_safe(sdreq, n, tlv_list, node) { + pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri); + + memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv, + sdreq->tlv_len); + + hlist_del(&sdreq->node); + + hlist_add_head(&sdreq->node, &local->pending_sdreqs); + } + + mutex_unlock(&local->sdreq_lock); + + skb_queue_tail(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) +{ + struct sk_buff *skb; + struct nfc_dev *dev; + u16 size = 1; /* Reason code */ + + pr_debug("Sending DM reason 0x%x\n", reason); + + if (local == NULL) + return -ENODEV; + + dev = local->dev; + if (dev == NULL) + return -ENODEV; + + size += LLCP_HEADER_SIZE; + size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; + + skb = alloc_skb(size, GFP_KERNEL); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM); + + memcpy(skb_put(skb, 1), &reason, 1); + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, + struct msghdr *msg, size_t len) +{ + struct sk_buff *pdu; + struct sock *sk = &sock->sk; + struct nfc_llcp_local *local; + size_t frag_len = 0, remaining_len; + u8 *msg_data, *msg_ptr; + u16 remote_miu; + + pr_debug("Send I frame len %zd\n", len); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + /* Remote is ready but has not acknowledged our frames */ + if((sock->remote_ready && + skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw && + skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { + pr_err("Pending queue is full %d frames\n", + skb_queue_len(&sock->tx_pending_queue)); + return -ENOBUFS; + } + + /* Remote is not ready and we've been queueing enough frames */ + if ((!sock->remote_ready && + skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { + pr_err("Tx queue is full %d frames\n", + skb_queue_len(&sock->tx_queue)); + return -ENOBUFS; + } + + msg_data = 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)) + memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); + + skb_queue_tail(&sock->tx_queue, pdu); + + lock_sock(sk); + + nfc_llcp_queue_i_frames(sock); + + release_sock(sk); + + remaining_len -= frag_len; + msg_ptr += frag_len; + } while (remaining_len > 0); + + kfree(msg_data); + + return len; +} + +int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, + struct msghdr *msg, size_t len) +{ + struct sk_buff *pdu; + struct nfc_llcp_local *local; + size_t frag_len = 0, remaining_len; + u8 *msg_ptr, *msg_data; + u16 remote_miu; + int err; + + pr_debug("Send UI frame len %zd\n", len); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + msg_data = 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, MSG_DONTWAIT, + frag_len + LLCP_HEADER_SIZE, &err); + if (pdu == NULL) { + pr_err("Could not allocate PDU\n"); + continue; + } + + pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI); + + if (likely(frag_len > 0)) + memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); + + /* No need to check for the peer RW for UI frames */ + skb_queue_tail(&local->tx_queue, pdu); + + remaining_len -= frag_len; + msg_ptr += frag_len; + } while (remaining_len > 0); + + kfree(msg_data); + + return len; +} + +int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_llcp_local *local; + + pr_debug("Send rr nr %d\n", sock->recv_n); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); + if (skb == NULL) + return -ENOMEM; + + skb_put(skb, LLCP_SEQUENCE_SIZE); + + skb->data[2] = sock->recv_n; + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c new file mode 100644 index 0000000..252dfa7 --- /dev/null +++ b/net/nfc/llcp_core.c @@ -0,0 +1,1640 @@ +/* + * 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(unsigned long data) +{ + struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; + + pr_err("SYMM timeout\n"); + + schedule_work(&local->timeout_work); +} + +static void nfc_llcp_sdreq_timeout_work(struct work_struct *work) +{ + unsigned long time; + HLIST_HEAD(nl_sdres_list); + struct hlist_node *n; + struct nfc_llcp_sdp_tlv *sdp; + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + sdreq_timeout_work); + + mutex_lock(&local->sdreq_lock); + + time = jiffies - msecs_to_jiffies(3 * local->remote_lto); + + hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) { + if (time_after(sdp->time, time)) + continue; + + sdp->sap = LLCP_SDP_UNBOUND; + + hlist_del(&sdp->node); + + hlist_add_head(&sdp->node, &nl_sdres_list); + } + + if (!hlist_empty(&local->pending_sdreqs)) + mod_timer(&local->sdreq_timer, + jiffies + msecs_to_jiffies(3 * local->remote_lto)); + + mutex_unlock(&local->sdreq_lock); + + if (!hlist_empty(&nl_sdres_list)) + nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); +} + +static void nfc_llcp_sdreq_timer(unsigned long data) +{ + struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; + + schedule_work(&local->sdreq_timeout_work); +} + +struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) +{ + struct nfc_llcp_local *local; + + 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 */ +#if LINUX_VERSION_IS_GEQ(3,15,0) + parent->sk_data_ready(parent); +#else + parent->sk_data_ready(parent, 0); +#endif + + /* Send CC */ + nfc_llcp_send_cc(new_sock); + + release_sock(&sock->sk); + sock_put(&sock->sk); + + return; + +fail: + /* Send DM */ + nfc_llcp_send_dm(local, dsap, ssap, reason); +} + +int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) +{ + int nr_frames = 0; + struct nfc_llcp_local *local = sock->local; + + pr_debug("Remote ready %d tx queue len %d remote rw %d", + sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), + sock->remote_rw); + + /* Try to queue some I frames for transmission */ + while (sock->remote_ready && + skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) { + struct sk_buff *pdu; + + pdu = skb_dequeue(&sock->tx_queue); + if (pdu == NULL) + break; + + /* Update N(S)/N(R) */ + nfc_llcp_set_nrns(sock, pdu); + + skb_queue_tail(&local->tx_queue, pdu); + nr_frames++; + } + + return nr_frames; +} + +static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap, ptype, ns, nr; + + ptype = nfc_llcp_ptype(skb); + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + ns = nfc_llcp_ns(skb); + nr = nfc_llcp_nr(skb); + + pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); + + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + if (llcp_sock == NULL) { + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + return; + } + + sk = &llcp_sock->sk; + lock_sock(sk); + if (sk->sk_state == LLCP_CLOSED) { + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); + } + + /* Pass the payload upstream */ + if (ptype == LLCP_PDU_I) { + pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); + + if (ns == llcp_sock->recv_n) + llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; + else + pr_err("Received out of sequence I PDU\n"); + + skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); + if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { + /* + * I frames will be freed from the socket layer, so we + * need to keep them alive until someone receives them. + */ + skb_get(skb); + } else { + pr_err("Receive queue is full\n"); + } + } + + /* Remove skbs from the pending queue */ + if (llcp_sock->send_ack_n != nr) { + struct sk_buff *s, *tmp; + u8 n; + + llcp_sock->send_ack_n = nr; + + /* Remove and free all skbs until ns == nr */ + skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) { + n = nfc_llcp_ns(s); + + skb_unlink(s, &llcp_sock->tx_pending_queue); + kfree_skb(s); + + if (n == nr) + break; + } + + /* Re-queue the remaining skbs for transmission */ + skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue, + s, tmp) { + skb_unlink(s, &llcp_sock->tx_pending_queue); + skb_queue_head(&local->tx_queue, s); + } + } + + if (ptype == LLCP_PDU_RR) + llcp_sock->remote_ready = true; + else if (ptype == LLCP_PDU_RNR) + llcp_sock->remote_ready = false; + + if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I) + nfc_llcp_send_rr(llcp_sock); + + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + if ((dsap == 0) && (ssap == 0)) { + pr_debug("Connection termination"); + nfc_dep_link_down(local->dev); + return; + } + + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + if (llcp_sock == NULL) { + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + return; + } + + sk = &llcp_sock->sk; + lock_sock(sk); + + nfc_llcp_socket_purge(llcp_sock); + + if (sk->sk_state == LLCP_CLOSED) { + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); + } + + if (sk->sk_state == LLCP_CONNECTED) { + nfc_put_device(local->dev); + sk->sk_state = LLCP_CLOSED; + sk->sk_state_change(sk); + } + + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); + + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); + if (llcp_sock == NULL) { + pr_err("Invalid CC\n"); + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + + return; + } + + sk = &llcp_sock->sk; + + /* Unlink from connecting and link to the client array */ + nfc_llcp_sock_unlink(&local->connecting_sockets, sk); + nfc_llcp_sock_link(&local->sockets, sk); + llcp_sock->dsap = ssap; + + nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + sk->sk_state = LLCP_CONNECTED; + sk->sk_state_change(sk); + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap, reason; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + reason = skb->data[2]; + + pr_debug("%d %d reason %d\n", ssap, dsap, reason); + + switch (reason) { + case LLCP_DM_NOBOUND: + case LLCP_DM_REJ: + llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); + break; + + default: + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + break; + } + + if (llcp_sock == NULL) { + pr_debug("Already closed\n"); + return; + } + + sk = &llcp_sock->sk; + + sk->sk_err = ENXIO; + sk->sk_state = LLCP_CLOSED; + sk->sk_state_change(sk); + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + u8 dsap, ssap, *tlv, type, length, tid, sap; + u16 tlv_len, offset; + char *service_name; + size_t service_name_len; + struct nfc_llcp_sdp_tlv *sdp; + HLIST_HEAD(llc_sdres_list); + size_t sdres_tlvs_len; + HLIST_HEAD(nl_sdres_list); + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("%d %d\n", dsap, ssap); + + if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) { + pr_err("Wrong SNL SAP\n"); + return; + } + + tlv = &skb->data[LLCP_HEADER_SIZE]; + tlv_len = skb->len - LLCP_HEADER_SIZE; + offset = 0; + sdres_tlvs_len = 0; + + while (offset < tlv_len) { + type = tlv[0]; + length = tlv[1]; + + switch (type) { + case LLCP_TLV_SDREQ: + tid = tlv[2]; + service_name = (char *) &tlv[3]; + service_name_len = length - 1; + + pr_debug("Looking for %.16s\n", service_name); + + if (service_name_len == strlen("urn:nfc:sn:sdp") && + !strncmp(service_name, "urn:nfc:sn:sdp", + service_name_len)) { + sap = 1; + goto add_snl; + } + + llcp_sock = nfc_llcp_sock_from_sn(local, service_name, + service_name_len); + if (!llcp_sock) { + sap = 0; + goto add_snl; + } + + /* + * We found a socket but its ssap has not been reserved + * yet. We need to assign it for good and send a reply. + * The ssap will be freed when the socket is closed. + */ + if (llcp_sock->ssap == LLCP_SDP_UNBOUND) { + atomic_t *client_count; + + sap = nfc_llcp_reserve_sdp_ssap(local); + + pr_debug("Reserving %d\n", sap); + + if (sap == LLCP_SAP_MAX) { + sap = 0; + goto add_snl; + } + + client_count = + &local->local_sdp_cnt[sap - + LLCP_WKS_NUM_SAP]; + + atomic_inc(client_count); + + llcp_sock->ssap = sap; + llcp_sock->reserved_ssap = sap; + } else { + sap = llcp_sock->ssap; + } + + pr_debug("%p %d\n", llcp_sock, sap); + +add_snl: + sdp = nfc_llcp_build_sdres_tlv(tid, sap); + if (sdp == NULL) + goto exit; + + sdres_tlvs_len += sdp->tlv_len; + hlist_add_head(&sdp->node, &llc_sdres_list); + break; + + case LLCP_TLV_SDRES: + mutex_lock(&local->sdreq_lock); + + pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]); + + hlist_for_each_entry(sdp, &local->pending_sdreqs, node) { + if (sdp->tid != tlv[2]) + continue; + + sdp->sap = tlv[3]; + + pr_debug("Found: uri=%s, sap=%d\n", + sdp->uri, sdp->sap); + + hlist_del(&sdp->node); + + hlist_add_head(&sdp->node, &nl_sdres_list); + + break; + } + + mutex_unlock(&local->sdreq_lock); + break; + + default: + pr_err("Invalid SNL tlv value 0x%x\n", type); + break; + } + + offset += length + 2; + tlv += length + 2; + } + +exit: + if (!hlist_empty(&nl_sdres_list)) + nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); + + if (!hlist_empty(&llc_sdres_list)) + nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); +} + +static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + u8 ptype; + u16 pdu_len; + struct sk_buff *new_skb; + + if (skb->len <= LLCP_HEADER_SIZE) { + pr_err("Malformed AGF PDU\n"); + return; + } + + skb_pull(skb, LLCP_HEADER_SIZE); + + while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) { + pdu_len = skb->data[0] << 8 | skb->data[1]; + + skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE); + + if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) { + pr_err("Malformed AGF PDU\n"); + return; + } + + ptype = nfc_llcp_ptype(skb); + + if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) + goto next; + + new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL); + if (new_skb == NULL) { + pr_err("Could not allocate PDU\n"); + return; + } + + memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len); + + nfc_llcp_rx_skb(local, new_skb); + + kfree_skb(new_skb); +next: + skb_pull(skb, pdu_len); + } +} + +static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + u8 dsap, ssap, ptype; + + ptype = nfc_llcp_ptype(skb); + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); + + if (ptype != LLCP_PDU_SYMM) + print_hex_dump_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); + init_timer(&local->link_timer); + local->link_timer.data = (unsigned long) local; + local->link_timer.function = nfc_llcp_symm_timer; + + skb_queue_head_init(&local->tx_queue); + INIT_WORK(&local->tx_work, nfc_llcp_tx_work); + + local->rx_pending = NULL; + INIT_WORK(&local->rx_work, nfc_llcp_rx_work); + + INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); + + rwlock_init(&local->sockets.lock); + rwlock_init(&local->connecting_sockets.lock); + rwlock_init(&local->raw_sockets.lock); + + local->lto = 150; /* 1500 ms */ + local->rw = LLCP_MAX_RW; + local->miux = cpu_to_be16(LLCP_MAX_MIUX); + 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); + init_timer(&local->sdreq_timer); + local->sdreq_timer.data = (unsigned long) local; + local->sdreq_timer.function = nfc_llcp_sdreq_timer; + INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work); + + list_add(&local->list, &llcp_devices); + + return 0; +} + +void nfc_llcp_unregister_device(struct nfc_dev *dev) +{ + struct nfc_llcp_local *local = nfc_llcp_find_local(dev); + + if (local == NULL) { + pr_debug("No such device\n"); + return; + } + + local_cleanup(local); + + nfc_llcp_local_put(local); +} + +int __init nfc_llcp_init(void) +{ + 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..1ae0c0c --- /dev/null +++ b/net/nfc/llcp_sock.c @@ -0,0 +1,1066 @@ +/* + * 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 || addr->sa_family != AF_NFC) + return -EINVAL; + + pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); + + memset(&llcp_addr, 0, sizeof(llcp_addr)); + len = min_t(unsigned int, sizeof(llcp_addr), alen); + memcpy(&llcp_addr, addr, len); + + /* This is going to be a listening socket, dsap must be 0 */ + if (llcp_addr.dsap != 0) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != LLCP_CLOSED) { + ret = -EBADFD; + goto error; + } + + dev = nfc_get_device(llcp_addr.dev_idx); + if (dev == NULL) { + ret = -ENODEV; + goto error; + } + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + ret = -ENODEV; + goto put_dev; + } + + llcp_sock->dev = dev; + llcp_sock->local = nfc_llcp_local_get(local); + llcp_sock->nfc_protocol = llcp_addr.nfc_protocol; + llcp_sock->service_name_len = min_t(unsigned int, + llcp_addr.service_name_len, + NFC_LLCP_MAX_SERVICE_NAME); + llcp_sock->service_name = kmemdup(llcp_addr.service_name, + llcp_sock->service_name_len, + GFP_KERNEL); + + llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock); + if (llcp_sock->ssap == LLCP_SAP_MAX) { + ret = -EADDRINUSE; + goto put_dev; + } + + llcp_sock->reserved_ssap = llcp_sock->ssap; + + nfc_llcp_sock_link(&local->sockets, sk); + + pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap); + + sk->sk_state = LLCP_BOUND; + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr, + int alen) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + struct nfc_llcp_local *local; + struct nfc_dev *dev; + struct sockaddr_nfc_llcp llcp_addr; + int len, ret = 0; + + if (!addr || addr->sa_family != AF_NFC) + return -EINVAL; + + pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); + + memset(&llcp_addr, 0, sizeof(llcp_addr)); + len = min_t(unsigned int, sizeof(llcp_addr), alen); + memcpy(&llcp_addr, addr, len); + + lock_sock(sk); + + if (sk->sk_state != LLCP_CLOSED) { + ret = -EBADFD; + goto error; + } + + dev = nfc_get_device(llcp_addr.dev_idx); + if (dev == NULL) { + ret = -ENODEV; + goto error; + } + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + ret = -ENODEV; + goto put_dev; + } + + llcp_sock->dev = dev; + llcp_sock->local = nfc_llcp_local_get(local); + llcp_sock->nfc_protocol = llcp_addr.nfc_protocol; + + nfc_llcp_sock_link(&local->raw_sockets, sk); + + sk->sk_state = LLCP_BOUND; + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +static int llcp_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + int ret = 0; + + pr_debug("sk %p backlog %d\n", sk, backlog); + + lock_sock(sk); + + if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) || + sk->sk_state != LLCP_BOUND) { + ret = -EBADFD; + goto error; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + + pr_debug("Socket listening\n"); + sk->sk_state = LLCP_LISTEN; + +error: + release_sock(sk); + + return ret; +} + +static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname, + char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + u32 opt; + int err = 0; + + pr_debug("%p optname %d\n", sk, optname); + + if (level != SOL_NFC) + return -ENOPROTOOPT; + + lock_sock(sk); + + switch (optname) { + case NFC_LLCP_RW: + if (sk->sk_state == LLCP_CONNECTED || + sk->sk_state == LLCP_BOUND || + sk->sk_state == LLCP_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > LLCP_MAX_RW) { + err = -EINVAL; + break; + } + + llcp_sock->rw = (u8) opt; + + break; + + case NFC_LLCP_MIUX: + if (sk->sk_state == LLCP_CONNECTED || + sk->sk_state == LLCP_BOUND || + sk->sk_state == LLCP_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > LLCP_MAX_MIUX) { + err = -EINVAL; + break; + } + + llcp_sock->miux = cpu_to_be16((u16) opt); + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + + pr_debug("%p rw %d miux %d\n", llcp_sock, + llcp_sock->rw, llcp_sock->miux); + + return err; +} + +static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct nfc_llcp_local *local; + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + int len, err = 0; + u16 miux, remote_miu; + u8 rw; + + pr_debug("%p optname %d\n", sk, optname); + + if (level != SOL_NFC) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + local = llcp_sock->local; + if (!local) + return -ENODEV; + + len = min_t(u32, len, sizeof(u32)); + + lock_sock(sk); + + switch (optname) { + case NFC_LLCP_RW: + rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw; + if (put_user(rw, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_MIUX: + miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ? + be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux); + + if (put_user(miux, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_REMOTE_MIU: + remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ? + local->remote_miu : llcp_sock->remote_miu; + + if (put_user(remote_miu, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_REMOTE_LTO: + if (put_user(local->remote_lto / 10, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_REMOTE_RW: + if (put_user(llcp_sock->remote_rw, (u32 __user *) optval)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + + if (put_user(len, optlen)) + return -EFAULT; + + return err; +} + +void nfc_llcp_accept_unlink(struct sock *sk) +{ + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + + pr_debug("state %d\n", sk->sk_state); + + list_del_init(&llcp_sock->accept_queue); + sk_acceptq_removed(llcp_sock->parent); + llcp_sock->parent = NULL; + + sock_put(sk); +} + +void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk) +{ + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent); + + /* Lock will be free from unlink */ + sock_hold(sk); + + list_add_tail(&llcp_sock->accept_queue, + &llcp_sock_parent->accept_queue); + llcp_sock->parent = parent; + sk_acceptq_added(parent); +} + +struct sock *nfc_llcp_accept_dequeue(struct sock *parent, + struct socket *newsock) +{ + struct nfc_llcp_sock *lsk, *n, *llcp_parent; + struct sock *sk; + + llcp_parent = nfc_llcp_sock(parent); + + list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue, + accept_queue) { + sk = &lsk->sk; + lock_sock(sk); + + if (sk->sk_state == LLCP_CLOSED) { + release_sock(sk); + nfc_llcp_accept_unlink(sk); + continue; + } + + if (sk->sk_state == LLCP_CONNECTED || !newsock) { + list_del_init(&lsk->accept_queue); + sock_put(sk); + + if (newsock) + sock_graft(sk, newsock); + + release_sock(sk); + + pr_debug("Returning sk state %d\n", sk->sk_state); + + sk_acceptq_removed(parent); + + return sk; + } + + release_sock(sk); + } + + return NULL; +} + +static int llcp_sock_accept(struct socket *sock, struct socket *newsock, + int flags, 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 *len, int peer) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr); + + if (llcp_sock == NULL || llcp_sock->dev == NULL) + return -EBADFD; + + pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx, + llcp_sock->dsap, llcp_sock->ssap); + + memset(llcp_addr, 0, sizeof(*llcp_addr)); + *len = sizeof(struct sockaddr_nfc_llcp); + + 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 0; +} + +static inline unsigned int 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 POLLIN | POLLRDNORM; + } + + return 0; +} + +static unsigned int llcp_sock_poll(struct file *file, struct socket *sock, + poll_table *wait) +{ + struct sock *sk = sock->sk; + unsigned int mask = 0; + + pr_debug("%p\n", sk); + + sock_poll_wait(file, sk_sleep(sk), wait); + + if (sk->sk_state == LLCP_LISTEN) + return llcp_accept_poll(sk); + + if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue)) + mask |= POLLERR | + (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0); + + if (!skb_queue_empty(&sk->sk_receive_queue)) + mask |= POLLIN | POLLRDNORM; + + if (sk->sk_state == LLCP_CLOSED) + mask |= POLLHUP; + + if (sk->sk_shutdown & RCV_SHUTDOWN) + mask |= POLLRDHUP | POLLIN | POLLRDNORM; + + if (sk->sk_shutdown == SHUTDOWN_MASK) + mask |= POLLHUP; + + if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED) + mask |= POLLOUT | POLLWRNORM | POLLWRBAND; + else + 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(struct sockaddr_nfc) || + addr->sa_family != AF_NFC) + return -EINVAL; + + if (addr->service_name_len == 0 && addr->dsap == 0) + return -EINVAL; + + pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx, + addr->target_idx, addr->nfc_protocol); + + lock_sock(sk); + + if (sk->sk_state == LLCP_CONNECTED) { + ret = -EISCONN; + goto error; + } + + dev = nfc_get_device(addr->dev_idx); + if (dev == NULL) { + ret = -ENODEV; + goto error; + } + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + ret = -ENODEV; + goto put_dev; + } + + device_lock(&dev->dev); + if (dev->dep_link_up == false) { + ret = -ENOLINK; + device_unlock(&dev->dev); + goto put_dev; + } + device_unlock(&dev->dev); + + if (local->rf_mode == NFC_RF_INITIATOR && + addr->target_idx != local->target_idx) { + ret = -ENOLINK; + goto put_dev; + } + + llcp_sock->dev = dev; + llcp_sock->local = nfc_llcp_local_get(local); + llcp_sock->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); +} +#if LINUX_VERSION_IS_LESS(4,1,0) +static int backport_llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len){ + return llcp_sock_sendmsg(sock, msg, len); +} +#endif /* LINUX_VERSION_IS_LESS(4,1,0) */ + +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; +} +#if LINUX_VERSION_IS_LESS(4,1,0) +static int backport_llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, + int flags){ + return llcp_sock_recvmsg(sock, msg, len, flags); +} +#endif /* LINUX_VERSION_IS_LESS(4,1,0) */ + +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, +#if LINUX_VERSION_IS_GEQ(4,1,0) + .sendmsg = llcp_sock_sendmsg, +#else + .sendmsg = backport_llcp_sock_sendmsg, +#endif /* LINUX_VERSION_IS_GEQ(4,1,0) */ +#if LINUX_VERSION_IS_GEQ(4,1,0) + .recvmsg = llcp_sock_recvmsg, +#else + .recvmsg = backport_llcp_sock_recvmsg, +#endif /* LINUX_VERSION_IS_GEQ(4,1,0) */ + .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, +#if LINUX_VERSION_IS_GEQ(4,1,0) + .recvmsg = llcp_sock_recvmsg, +#else + .recvmsg = backport_llcp_sock_recvmsg, +#endif /* LINUX_VERSION_IS_GEQ(4,1,0) */ + .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..a71bcc3 --- /dev/null +++ b/net/nfc/nci/Kconfig @@ -0,0 +1,31 @@ +config NFC_NCI + depends on NFC + tristate "NCI protocol support" + depends on m + default n + help + NCI (NFC Controller Interface) is a communication protocol between + an NFC Controller (NFCC) and a Device Host (DH). + + Say Y here to compile NCI support into the kernel or say M to + compile it as module (nci). + +config NFC_NCI_SPI + depends on NFC_NCI && SPI + depends on CRC_CCITT + tristate "NCI over SPI protocol support" + depends on m + default n + help + NCI (NFC Controller Interface) is a communication protocol between + an NFC Controller (NFCC) and a Device Host (DH). + + Say 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" + depends on m + 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..4a00ed5 --- /dev/null +++ b/net/nfc/nci/Makefile @@ -0,0 +1,13 @@ +# +# Makefile for the Linux NFC NCI layer. +# + +obj-$(CPTCFG_NFC_NCI) += nci.o + +nci-objs := core.o data.o lib.o ntf.o rsp.o hci.o + +nci_spi-y += spi.o +obj-$(CPTCFG_NFC_NCI_SPI) += nci_spi.o + +nci_uart-y += uart.o +obj-$(CPTCFG_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..61fff42 --- /dev/null +++ b/net/nfc/nci/core.c @@ -0,0 +1,1549 @@ +/* + * 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 (conn_info) { + 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); + memcpy(skb_put(skb, data_len), 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(unsigned long arg) +{ + struct nci_dev *ndev = (void *) arg; + + atomic_set(&ndev->cmd_cnt, 1); + queue_work(ndev->cmd_wq, &ndev->cmd_work); +} + +/* NCI data exchange timer function */ +static void nci_data_timer(unsigned long arg) +{ + struct nci_dev *ndev = (void *) arg; + + set_bit(NCI_DATA_EXCHANGE_TO, &ndev->flags); + queue_work(ndev->rx_wq, &ndev->rx_work); +} + +static int nci_dev_up(struct nfc_dev *nfc_dev) +{ + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + + return nci_open_device(ndev); +} + +static int nci_dev_down(struct nfc_dev *nfc_dev) +{ + struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); + + return nci_close_device(ndev); +} + +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: + kfree(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); + + setup_timer(&ndev->cmd_timer, nci_cmd_timer, + (unsigned long) ndev); + setup_timer(&ndev->data_timer, nci_data_timer, + (unsigned long) ndev); + + mutex_init(&ndev->req_lock); + 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 = (struct nci_ctrl_hdr *) skb_put(skb, NCI_CTRL_HDR_SIZE); + hdr->gid = nci_opcode_gid(opcode); + hdr->oid = nci_opcode_oid(opcode); + hdr->plen = plen; + + nci_mt_set((__u8 *)hdr, NCI_MT_CMD_PKT); + nci_pbf_set((__u8 *)hdr, NCI_PBF_LAST); + + if (plen) + memcpy(skb_put(skb, plen), payload, plen); + + skb_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..dbd2425 --- /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 = (struct nci_data_hdr *) skb_push(skb, NCI_DATA_HDR_SIZE); + hdr->conn_id = conn_id; + hdr->rfu = 0; + hdr->plen = plen; + + nci_mt_set((__u8 *)hdr, NCI_MT_DATA_PKT); + nci_pbf_set((__u8 *)hdr, pbf); +} + +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 */ + memcpy(skb_put(skb_frag, frag_len), data, frag_len); + + /* second, set the header */ + nci_push_data_hdr(ndev, conn_id, skb_frag, + ((total_len == frag_len) ? + (NCI_PBF_LAST) : (NCI_PBF_CONT))); + + __skb_queue_tail(&frags_q, skb_frag); + + data += frag_len; + total_len -= frag_len; + + pr_debug("frag_len %d, remaining total_len %d\n", + frag_len, total_len); + } + + /* queue all fragments atomically */ + spin_lock_irqsave(&ndev->tx_q.lock, flags); + + while ((skb_frag = __skb_dequeue(&frags_q)) != NULL) + __skb_queue_tail(&ndev->tx_q, skb_frag); + + spin_unlock_irqrestore(&ndev->tx_q.lock, flags); + + /* free the original skb */ + kfree_skb(skb); + + goto exit; + +free_exit: + while ((skb_frag = __skb_dequeue(&frags_q)) != NULL) + kfree_skb(skb_frag); + +exit: + return rc; +} + +/* Send NCI data */ +int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb) +{ + 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..a0ab26d --- /dev/null +++ b/net/nfc/nci/hci.c @@ -0,0 +1,800 @@ +/* + * 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); + *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; + } + + *skb_push(skb, 1) = cb; + + if (len > 0) + memcpy(skb_put(skb, len), 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(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN) = pipe; + + skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) { + msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN; + memcpy(skb_put(hcp_skb, msg_len), 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..d904cd2 --- /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(skb, 1) = crc >> 8; + *skb_put(skb, 1) = crc & 0xFF; + } + + if (write_handshake_completion) { + /* Trick SPI driver to raise chip select */ + ret = __nci_spi_send(nspi, NULL, 1); + if (ret) + goto done; + + /* wait for NFC chip hardware handshake to complete */ + if (wait_for_completion_timeout(write_handshake_completion, + msecs_to_jiffies(1000)) == 0) { + ret = -ETIME; + goto done; + } + } + + ret = __nci_spi_send(nspi, skb, 0); + if (ret != 0 || nspi->acknowledge_mode == NCI_SPI_CRC_DISABLED) + goto done; + + reinit_completion(&nspi->req_completion); + completion_rc = wait_for_completion_interruptible_timeout( + &nspi->req_completion, + NCI_SPI_SEND_TIMEOUT); + + if (completion_rc <= 0 || nspi->req_result == ACKNOWLEDGE_NACK) + ret = -EIO; + +done: + kfree_skb(skb); + + return ret; +} +EXPORT_SYMBOL_GPL(nci_spi_send); + +/* ---- Interface to NCI SPI drivers ---- */ + +/** + * nci_spi_allocate_spi - allocate a new nci spi + * + * @spi: SPI device + * @acknowledge_mode: Acknowledge mode used by the NFC device + * @delay: delay between transactions in us + * @ndev: nci dev to send incoming nci frames to + */ +struct nci_spi *nci_spi_allocate_spi(struct spi_device *spi, + u8 acknowledge_mode, unsigned int delay, + struct nci_dev *ndev) +{ + struct nci_spi *nspi; + + nspi = devm_kzalloc(&spi->dev, sizeof(struct nci_spi), GFP_KERNEL); + if (!nspi) + return NULL; + + nspi->acknowledge_mode = acknowledge_mode; + nspi->xfer_udelay = delay; + /* 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(skb, 1) = crc >> 8; + *skb_put(skb, 1) = crc & 0xFF; + + ret = __nci_spi_send(nspi, skb, 0); + + kfree_skb(skb); + + return ret; +} + +static struct sk_buff *__nci_spi_read(struct nci_spi *nspi) +{ + struct sk_buff *skb; + struct spi_message m; + unsigned char req[2], resp_hdr[2]; + struct spi_transfer tx, rx; + unsigned short rx_len = 0; + int ret; + + spi_message_init(&m); + + memset(&tx, 0, sizeof(struct spi_transfer)); + req[0] = NCI_SPI_DIRECT_READ; + req[1] = nspi->acknowledge_mode; + tx.tx_buf = req; + tx.len = 2; + tx.cs_change = 0; + 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) { + *skb_push(skb, 1) = resp_hdr[1]; + *skb_push(skb, 1) = resp_hdr[0]; + } + + return skb; + +receive_error: + kfree_skb(skb); + + return NULL; +} + +static int nci_spi_check_crc(struct sk_buff *skb) +{ + u16 crc_data = (skb->data[skb->len - 2] << 8) | + skb->data[skb->len - 1]; + int ret; + + ret = (crc_ccitt(CRC_INIT, skb->data, skb->len - NCI_SPI_CRC_LEN) + == crc_data); + + skb_trim(skb, skb->len - NCI_SPI_CRC_LEN); + + return ret; +} + +static u8 nci_spi_get_ack(struct sk_buff *skb) +{ + u8 ret; + + ret = skb->data[0] >> NCI_SPI_ACK_SHIFT; + + /* Remove NFCC part of the header: ACK, NACK and MSB payload len */ + skb_pull(skb, 2); + + return ret; +} + +/** + * nci_spi_read - read frame from NCI SPI drivers + * + * @nspi: The nci spi + * Context: can sleep + * + * This call may only be used from a context that may sleep. The sleep + * is non-interruptible, and has no timeout. + * + * It returns an allocated skb containing the frame on success, or NULL. + */ +struct sk_buff *nci_spi_read(struct nci_spi *nspi) +{ + struct sk_buff *skb; + + /* Retrieve frame from SPI */ + skb = __nci_spi_read(nspi); + if (!skb) + goto done; + + if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) { + if (!nci_spi_check_crc(skb)) { + send_acknowledge(nspi, ACKNOWLEDGE_NACK); + goto done; + } + + /* In case of acknowledged mode: if ACK or NACK received, + * unblock completion of latest frame sent. + */ + nspi->req_result = nci_spi_get_ack(skb); + if (nspi->req_result) + complete(&nspi->req_completion); + } + + /* If there is no payload (ACK/NACK only frame), + * free the socket buffer + */ + if (!skb->len) { + kfree_skb(skb); + skb = NULL; + goto done; + } + + if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) + send_acknowledge(nspi, ACKNOWLEDGE_ACK); + +done: + + return skb; +} +EXPORT_SYMBOL_GPL(nci_spi_read); + +MODULE_LICENSE("GPL"); diff --git a/net/nfc/nci/uart.c b/net/nfc/nci/uart.c new file mode 100644 index 0000000..cff92db --- /dev/null +++ b/net/nfc/nci/uart.c @@ -0,0 +1,495 @@ +/* + * 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 unsigned int 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(nu->rx_skb, 1) = *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; + memcpy(skb_put(nu->rx_skb, chunk_len), 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; + +#if LINUX_VERSION_IS_GEQ(3,12,0) + down_read(&nu->tty->termios_rwsem); +#else + mutex_lock(&nu->tty->termios_mutex); +#endif /* if LINUX_VERSION_IS_GEQ(3,12,0) */ + new_termios = nu->tty->termios; +#if LINUX_VERSION_IS_GEQ(3,12,0) + up_read(&nu->tty->termios_rwsem); +#else + mutex_unlock(&nu->tty->termios_mutex); +#endif /* if LINUX_VERSION_IS_GEQ(3,12,0) */ + 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..165ea67 --- /dev/null +++ b/net/nfc/netlink.c @@ -0,0 +1,1868 @@ +/* + * 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 }, + [NFC_SDP_ATTR_SAP] = { .type = NLA_U8 }, +}; + +static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, + struct netlink_callback *cb, int flags) +{ + void *hdr; + + hdr = genlmsg_put(msg, NETLINK_CB_PORTID(cb->skb), cb->nlh->nlmsg_seq, + &nfc_genl_family, flags, NFC_CMD_GET_TARGET); + if (!hdr) + return -EMSGSIZE; + + genl_dump_check_consistent(cb, hdr, &nfc_genl_family); + + if (nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target->idx) || + nla_put_u32(msg, NFC_ATTR_PROTOCOLS, target->supported_protocols) || + nla_put_u16(msg, NFC_ATTR_TARGET_SENS_RES, target->sens_res) || + nla_put_u8(msg, NFC_ATTR_TARGET_SEL_RES, target->sel_res)) + goto nla_put_failure; + if (target->nfcid1_len > 0 && + nla_put(msg, NFC_ATTR_TARGET_NFCID1, target->nfcid1_len, + target->nfcid1)) + goto nla_put_failure; + if (target->sensb_res_len > 0 && + nla_put(msg, NFC_ATTR_TARGET_SENSB_RES, target->sensb_res_len, + target->sensb_res)) + goto nla_put_failure; + if (target->sensf_res_len > 0 && + nla_put(msg, NFC_ATTR_TARGET_SENSF_RES, target->sensf_res_len, + target->sensf_res)) + goto nla_put_failure; + + 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); + 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: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_target_lost(struct nfc_dev *dev, u32 target_idx) +{ + struct sk_buff *msg; + void *hdr; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_TARGET_LOST); + if (!hdr) + goto free_msg; + + if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) || + nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol) +{ + struct sk_buff *msg; + void *hdr; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_TM_ACTIVATED); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + if (nla_put_u32(msg, NFC_ATTR_TM_PROTOCOLS, protocol)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_tm_deactivated(struct nfc_dev *dev) +{ + struct sk_buff *msg; + void *hdr; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_TM_DEACTIVATED); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_device_added(struct nfc_dev *dev) +{ + struct sk_buff *msg; + void *hdr; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_DEVICE_ADDED); + if (!hdr) + goto free_msg; + + if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) || + nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) || + nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) || + nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_device_removed(struct nfc_dev *dev) +{ + struct sk_buff *msg; + void *hdr; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_DEVICE_REMOVED); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_KERNEL); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list) +{ + struct sk_buff *msg; + struct nlattr *sdp_attr, *uri_attr; + struct nfc_llcp_sdp_tlv *sdres; + struct hlist_node *n; + void *hdr; + int rc = -EMSGSIZE; + int i; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_LLC_SDRES); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + + sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP); + if (sdp_attr == NULL) { + rc = -ENOMEM; + goto nla_put_failure; + } + + i = 1; + hlist_for_each_entry_safe(sdres, n, sdres_list, node) { + pr_debug("uri: %s, sap: %d\n", sdres->uri, sdres->sap); + + uri_attr = nla_nest_start(msg, i++); + if (uri_attr == NULL) { + rc = -ENOMEM; + goto nla_put_failure; + } + + if (nla_put_u8(msg, NFC_SDP_ATTR_SAP, sdres->sap)) + goto nla_put_failure; + + if (nla_put_string(msg, NFC_SDP_ATTR_URI, sdres->uri)) + goto nla_put_failure; + + nla_nest_end(msg, uri_attr); + + hlist_del(&sdres->node); + + nfc_llcp_free_sdp_tlv(sdres); + } + + nla_nest_end(msg, sdp_attr); + + genlmsg_end(msg, hdr); + + return genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC); + +nla_put_failure: + genlmsg_cancel(msg, hdr); + +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: + genlmsg_cancel(msg, hdr); +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: + genlmsg_cancel(msg, hdr); +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: + genlmsg_cancel(msg, hdr); +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: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev, + u32 portid, u32 seq, + struct netlink_callback *cb, + int flags) +{ + void *hdr; + + hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, flags, + NFC_CMD_GET_DEVICE); + if (!hdr) + return -EMSGSIZE; + + if (cb) + genl_dump_check_consistent(cb, hdr, &nfc_genl_family); + + if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) || + nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) || + nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) || + nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) || + nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode)) + 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_PORTID(cb->skb), + cb->nlh->nlmsg_seq, cb, NLM_F_MULTI); + if (rc < 0) + break; + + dev = nfc_device_iter_next(iter); + } + + mutex_unlock(&nfc_devlist_mutex); + + cb->args[1] = (long) dev; + + return skb->len; +} + +static int nfc_genl_dump_devices_done(struct netlink_callback *cb) +{ + struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0]; + + nfc_device_iter_exit(iter); + kfree(iter); + + return 0; +} + +int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode) +{ + struct sk_buff *msg; + void *hdr; + + pr_debug("DEP link is up\n"); + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, NFC_CMD_DEP_LINK_UP); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + if (rf_mode == NFC_RF_INITIATOR && + nla_put_u32(msg, NFC_ATTR_TARGET_INDEX, target_idx)) + goto nla_put_failure; + if (nla_put_u8(msg, NFC_ATTR_COMM_MODE, comm_mode) || + nla_put_u8(msg, NFC_ATTR_RF_MODE, rf_mode)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + dev->dep_link_up = true; + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_dep_link_down_event(struct nfc_dev *dev) +{ + struct sk_buff *msg; + void *hdr; + + pr_debug("DEP link is down\n"); + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_CMD_DEP_LINK_DOWN); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + + genlmsg_end(msg, hdr); + + genlmsg_multicast(&nfc_genl_family, msg, 0, 0, GFP_ATOMIC); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info) +{ + struct sk_buff *msg; + struct nfc_dev *dev; + u32 idx; + int rc = -ENOBUFS; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) { + rc = -ENOMEM; + goto out_putdev; + } + + rc = nfc_genl_send_device(msg, dev, genl_info_snd_portid(info), + info->snd_seq, + NULL, 0); + if (rc < 0) + goto out_free; + + nfc_put_device(dev); + + return genlmsg_reply(msg, info); + +out_free: + nlmsg_free(msg); +out_putdev: + nfc_put_device(dev); + return rc; +} + +static int nfc_genl_dev_up(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc; + u32 idx; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + rc = nfc_dev_up(dev); + + nfc_put_device(dev); + return rc; +} + +static int nfc_genl_dev_down(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc; + u32 idx; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + rc = nfc_dev_down(dev); + + nfc_put_device(dev); + return rc; +} + +static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc; + u32 idx; + u32 im_protocols = 0, tm_protocols = 0; + + pr_debug("Poll start\n"); + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || + ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] && + !info->attrs[NFC_ATTR_PROTOCOLS]) && + !info->attrs[NFC_ATTR_TM_PROTOCOLS])) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + if (info->attrs[NFC_ATTR_TM_PROTOCOLS]) + tm_protocols = nla_get_u32(info->attrs[NFC_ATTR_TM_PROTOCOLS]); + + if (info->attrs[NFC_ATTR_IM_PROTOCOLS]) + im_protocols = nla_get_u32(info->attrs[NFC_ATTR_IM_PROTOCOLS]); + else if (info->attrs[NFC_ATTR_PROTOCOLS]) + im_protocols = nla_get_u32(info->attrs[NFC_ATTR_PROTOCOLS]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + mutex_lock(&dev->genl_data.genl_data_mutex); + + rc = nfc_start_poll(dev, im_protocols, tm_protocols); + if (!rc) + dev->genl_data.poll_req_portid = genl_info_snd_portid(info); + + mutex_unlock(&dev->genl_data.genl_data_mutex); + + nfc_put_device(dev); + return rc; +} + +static int nfc_genl_stop_poll(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc; + u32 idx; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + device_lock(&dev->dev); + + if (!dev->polling) { + device_unlock(&dev->dev); + return -EINVAL; + } + + device_unlock(&dev->dev); + + mutex_lock(&dev->genl_data.genl_data_mutex); + + if (dev->genl_data.poll_req_portid != genl_info_snd_portid(info)) { + rc = -EBUSY; + goto out; + } + + rc = nfc_stop_poll(dev); + dev->genl_data.poll_req_portid = 0; + +out: + mutex_unlock(&dev->genl_data.genl_data_mutex); + nfc_put_device(dev); + return rc; +} + +static int nfc_genl_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]) + 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 0; +} + +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, genl_info_snd_portid(info), + info->snd_seq); + +exit: + device_unlock(&dev->dev); + + nfc_put_device(dev); + + if (rc < 0) { + if (msg) + nlmsg_free(msg); + + return rc; + } + + return genlmsg_reply(msg, info); +} + +static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + struct nfc_llcp_local *local; + u8 rw = 0; + u16 miux = 0; + u32 idx; + int rc = 0; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || + (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] && + !info->attrs[NFC_ATTR_LLC_PARAM_RW] && + !info->attrs[NFC_ATTR_LLC_PARAM_MIUX])) + return -EINVAL; + + if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) { + rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]); + + if (rw > LLCP_MAX_RW) + return -EINVAL; + } + + if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) { + miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]); + + if (miux > LLCP_MAX_MIUX) + return -EINVAL; + } + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + device_lock(&dev->dev); + + local = nfc_llcp_find_local(dev); + if (!local) { + nfc_put_device(dev); + rc = -ENODEV; + goto exit; + } + + if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) { + if (dev->dep_link_up) { + rc = -EINPROGRESS; + goto exit; + } + + local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]); + } + + if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) + local->rw = rw; + + if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) + local->miux = cpu_to_be16(miux); + +exit: + device_unlock(&dev->dev); + + nfc_put_device(dev); + + return rc; +} + +static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + struct nfc_llcp_local *local; + struct nlattr *attr, *sdp_attrs[NFC_SDP_ATTR_MAX+1]; + u32 idx; + u8 tid; + char *uri; + int rc = 0, rem; + size_t uri_len, tlvs_len; + struct hlist_head sdreq_list; + struct nfc_llcp_sdp_tlv *sdreq; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || + !info->attrs[NFC_ATTR_LLC_SDP]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + 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); + + 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: + genlmsg_cancel(msg, hdr); +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, &nfc_genl_family); + + 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_PORTID(cb->skb), + cb->nlh->nlmsg_seq, cb, NLM_F_MULTI); + if (rc < 0) + break; + + dev = nfc_device_iter_next(iter); + } + + mutex_unlock(&nfc_devlist_mutex); + + cb->args[1] = (long) dev; + + return skb->len; +} + +static int nfc_genl_dump_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: + genlmsg_cancel(msg, hdr); +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, + genl_info_snd_portid(dev->cur_cmd_info), + 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, + }, +}; + +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", + netlink_notify_portid(n)); + + w = kmalloc(sizeof(*w), GFP_ATOMIC); + if (w) { + INIT_WORK((struct work_struct *) w, nfc_urelease_event_work); + w->portid = netlink_notify_portid(n); + schedule_work((struct work_struct *) w); + } + +out: + return NOTIFY_DONE; +} + +void nfc_genl_data_init(struct nfc_genl_data *genl_data) +{ + genl_data->poll_req_portid = 0; + mutex_init(&genl_data->genl_data_mutex); +} + +void nfc_genl_data_exit(struct nfc_genl_data *genl_data) +{ + mutex_destroy(&genl_data->genl_data_mutex); +} + +static struct notifier_block nl_notifier = { + .notifier_call = nfc_genl_rcv_nl_event, +}; + +/** + * nfc_genl_init() - Initialize netlink interface + * + * This initialization function registers the nfc netlink family. + */ +int __init nfc_genl_init(void) +{ + int rc; + + rc = genl_register_family(&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..df85b13 --- /dev/null +++ b/net/nfc/rawsock.c @@ -0,0 +1,456 @@ +/* + * Copyright (C) 2011 Instituto Nokia de Tecnologia + * + * Authors: + * Aloisio Almeida Jr <aloisio.almeida@openbossa.org> + * Lauro Ramos Venancio <lauro.venancio@openbossa.org> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, see <http://www.gnu.org/licenses/>. + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + +#include <net/tcp_states.h> +#include <linux/nfc.h> +#include <linux/export.h> + +#include "nfc.h" + +static struct nfc_sock_list raw_sk_list = { + .lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock) +}; + +static void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_add_node(sk, &l->head); + write_unlock(&l->lock); +} + +static void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_del_node_init(sk); + write_unlock(&l->lock); +} + +static void rawsock_write_queue_purge(struct sock *sk) +{ + pr_debug("sk=%p\n", sk); + + spin_lock_bh(&sk->sk_write_queue.lock); + __skb_queue_purge(&sk->sk_write_queue); + nfc_rawsock(sk)->tx_work_scheduled = false; + spin_unlock_bh(&sk->sk_write_queue.lock); +} + +static void rawsock_report_error(struct sock *sk, int err) +{ + pr_debug("sk=%p err=%d\n", sk, err); + + sk->sk_shutdown = SHUTDOWN_MASK; + sk->sk_err = -err; + sk->sk_error_report(sk); + + rawsock_write_queue_purge(sk); +} + +static int rawsock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + pr_debug("sock=%p sk=%p\n", sock, sk); + + if (!sk) + return 0; + + if (sock->type == SOCK_RAW) + nfc_sock_unlink(&raw_sk_list, sk); + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +static int rawsock_connect(struct socket *sock, struct sockaddr *_addr, + int len, int flags) +{ + struct sock *sk = sock->sk; + struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr; + struct nfc_dev *dev; + int rc = 0; + + pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags); + + if (!addr || len < sizeof(struct sockaddr_nfc) || + addr->sa_family != AF_NFC) + return -EINVAL; + + pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", + addr->dev_idx, addr->target_idx, addr->nfc_protocol); + + lock_sock(sk); + + if (sock->state == SS_CONNECTED) { + rc = -EISCONN; + goto error; + } + + dev = nfc_get_device(addr->dev_idx); + if (!dev) { + rc = -ENODEV; + goto error; + } + + if (addr->target_idx > dev->target_next_idx - 1 || + addr->target_idx < dev->target_next_idx - dev->n_targets) { + rc = -EINVAL; + goto error; + } + + rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol); + if (rc) + goto put_dev; + + nfc_rawsock(sk)->dev = dev; + nfc_rawsock(sk)->target_idx = addr->target_idx; + sock->state = SS_CONNECTED; + sk->sk_state = TCP_ESTABLISHED; + sk->sk_state_change(sk); + + release_sock(sk); + return 0; + +put_dev: + nfc_put_device(dev); +error: + release_sock(sk); + return rc; +} + +static int rawsock_add_header(struct sk_buff *skb) +{ + *skb_push(skb, NFC_HEADER_SIZE) = 0; + + return 0; +} + +static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb, + int err) +{ + struct sock *sk = (struct sock *) context; + + BUG_ON(in_irq()); + + pr_debug("sk=%p err=%d\n", sk, err); + + if (err) + goto error; + + err = rawsock_add_header(skb); + if (err) + goto error_skb; + + err = sock_queue_rcv_skb(sk, skb); + if (err) + goto error_skb; + + spin_lock_bh(&sk->sk_write_queue.lock); + if (!skb_queue_empty(&sk->sk_write_queue)) + schedule_work(&nfc_rawsock(sk)->tx_work); + else + nfc_rawsock(sk)->tx_work_scheduled = false; + spin_unlock_bh(&sk->sk_write_queue.lock); + + sock_put(sk); + return; + +error_skb: + kfree_skb(skb); + +error: + rawsock_report_error(sk, err); + sock_put(sk); +} + +static void rawsock_tx_work(struct work_struct *work) +{ + struct sock *sk = to_rawsock_sk(work); + struct nfc_dev *dev = nfc_rawsock(sk)->dev; + u32 target_idx = nfc_rawsock(sk)->target_idx; + struct sk_buff *skb; + int rc; + + pr_debug("sk=%p target_idx=%u\n", sk, target_idx); + + if (sk->sk_shutdown & SEND_SHUTDOWN) { + rawsock_write_queue_purge(sk); + return; + } + + skb = skb_dequeue(&sk->sk_write_queue); + + sock_hold(sk); + rc = nfc_data_exchange(dev, target_idx, skb, + rawsock_data_exchange_complete, sk); + if (rc) { + rawsock_report_error(sk, rc); + sock_put(sk); + } +} + +static int rawsock_sendmsg(struct 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; +} +#if LINUX_VERSION_IS_LESS(4,1,0) +static int backport_rawsock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len){ + return rawsock_sendmsg(sock, msg, len); +} +#endif /* LINUX_VERSION_IS_LESS(4,1,0) */ + +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; +} +#if LINUX_VERSION_IS_LESS(4,1,0) +static int backport_rawsock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, int flags){ + return rawsock_recvmsg(sock, msg, len, flags); +} +#endif /* LINUX_VERSION_IS_LESS(4,1,0) */ + +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, +#if LINUX_VERSION_IS_GEQ(4,1,0) + .sendmsg = rawsock_sendmsg, +#else + .sendmsg = backport_rawsock_sendmsg, +#endif /* LINUX_VERSION_IS_GEQ(4,1,0) */ +#if LINUX_VERSION_IS_GEQ(4,1,0) + .recvmsg = rawsock_recvmsg, +#else + .recvmsg = backport_rawsock_recvmsg, +#endif /* LINUX_VERSION_IS_GEQ(4,1,0) */ + .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, +#if LINUX_VERSION_IS_GEQ(4,1,0) + .recvmsg = rawsock_recvmsg, +#else + .recvmsg = backport_rawsock_recvmsg, +#endif /* LINUX_VERSION_IS_GEQ(4,1,0) */ + .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); +} |