diff options
Diffstat (limited to 'net/nfc')
| -rw-r--r-- | net/nfc/Kconfig | 1 | ||||
| -rw-r--r-- | net/nfc/Makefile | 1 | ||||
| -rw-r--r-- | net/nfc/core.c | 201 | ||||
| -rw-r--r-- | net/nfc/llcp/Kconfig | 7 | ||||
| -rw-r--r-- | net/nfc/llcp/commands.c | 399 | ||||
| -rw-r--r-- | net/nfc/llcp/llcp.c | 971 | ||||
| -rw-r--r-- | net/nfc/llcp/llcp.h | 193 | ||||
| -rw-r--r-- | net/nfc/llcp/sock.c | 675 | ||||
| -rw-r--r-- | net/nfc/nci/core.c | 124 | ||||
| -rw-r--r-- | net/nfc/nci/data.c | 35 | ||||
| -rw-r--r-- | net/nfc/nci/lib.c | 11 | ||||
| -rw-r--r-- | net/nfc/nci/ntf.c | 209 | ||||
| -rw-r--r-- | net/nfc/nci/rsp.c | 131 | ||||
| -rw-r--r-- | net/nfc/netlink.c | 179 | ||||
| -rw-r--r-- | net/nfc/nfc.h | 70 | ||||
| -rw-r--r-- | net/nfc/rawsock.c | 37 |
16 files changed, 2904 insertions, 340 deletions
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig index 58cddad..44c865b 100644 --- a/net/nfc/Kconfig +++ b/net/nfc/Kconfig @@ -14,5 +14,6 @@ menuconfig NFC be called nfc. source "net/nfc/nci/Kconfig" +source "net/nfc/llcp/Kconfig" source "drivers/nfc/Kconfig" diff --git a/net/nfc/Makefile b/net/nfc/Makefile index fbb550f..7b4a6dc 100644 --- a/net/nfc/Makefile +++ b/net/nfc/Makefile @@ -6,3 +6,4 @@ obj-$(CONFIG_NFC) += nfc.o obj-$(CONFIG_NFC_NCI) += nci/ nfc-objs := core.o netlink.o af_nfc.o rawsock.o +nfc-$(CONFIG_NFC_LLCP) += llcp/llcp.o llcp/commands.o llcp/sock.o diff --git a/net/nfc/core.c b/net/nfc/core.c index 47e02c1..3ddf6e6 100644 --- a/net/nfc/core.c +++ b/net/nfc/core.c @@ -21,10 +21,13 @@ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <linux/init.h> #include <linux/kernel.h> #include <linux/module.h> #include <linux/slab.h> +#include <linux/nfc.h> #include "nfc.h" @@ -33,25 +36,6 @@ int nfc_devlist_generation; DEFINE_MUTEX(nfc_devlist_mutex); -int nfc_printk(const char *level, const char *format, ...) -{ - struct va_format vaf; - va_list args; - int r; - - va_start(args, format); - - vaf.fmt = format; - vaf.va = &args; - - r = printk("%sNFC: %pV\n", level, &vaf); - - va_end(args); - - return r; -} -EXPORT_SYMBOL(nfc_printk); - /** * nfc_dev_up - turn on the NFC device * @@ -63,7 +47,7 @@ int nfc_dev_up(struct nfc_dev *dev) { int rc = 0; - nfc_dbg("dev_name=%s", dev_name(&dev->dev)); + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); device_lock(&dev->dev); @@ -97,7 +81,7 @@ int nfc_dev_down(struct nfc_dev *dev) { int rc = 0; - nfc_dbg("dev_name=%s", dev_name(&dev->dev)); + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); device_lock(&dev->dev); @@ -139,7 +123,8 @@ int nfc_start_poll(struct nfc_dev *dev, u32 protocols) { int rc; - nfc_dbg("dev_name=%s protocols=0x%x", dev_name(&dev->dev), protocols); + pr_debug("dev_name=%s protocols=0x%x\n", + dev_name(&dev->dev), protocols); if (!protocols) return -EINVAL; @@ -174,7 +159,7 @@ int nfc_stop_poll(struct nfc_dev *dev) { int rc = 0; - nfc_dbg("dev_name=%s", dev_name(&dev->dev)); + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); device_lock(&dev->dev); @@ -196,6 +181,86 @@ error: return rc; } +int nfc_dep_link_up(struct nfc_dev *dev, int target_index, + u8 comm_mode, u8 rf_mode) +{ + int rc = 0; + + pr_debug("dev_name=%s comm:%d rf:%d\n", + dev_name(&dev->dev), comm_mode, rf_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; + } + + rc = dev->ops->dep_link_up(dev, target_index, comm_mode, rf_mode); + +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; + } + + if (dev->dep_rf_mode == NFC_RF_TARGET) { + rc = -EOPNOTSUPP; + goto error; + } + + rc = dev->ops->dep_link_down(dev); + if (!rc) { + dev->dep_link_up = false; + 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; + dev->dep_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 * @@ -207,8 +272,8 @@ int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol) { int rc; - nfc_dbg("dev_name=%s target_idx=%u protocol=%u", dev_name(&dev->dev), - target_idx, protocol); + pr_debug("dev_name=%s target_idx=%u protocol=%u\n", + dev_name(&dev->dev), target_idx, protocol); device_lock(&dev->dev); @@ -236,7 +301,8 @@ int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx) { int rc = 0; - nfc_dbg("dev_name=%s target_idx=%u", dev_name(&dev->dev), target_idx); + pr_debug("dev_name=%s target_idx=%u\n", + dev_name(&dev->dev), target_idx); device_lock(&dev->dev); @@ -271,8 +337,8 @@ int nfc_data_exchange(struct nfc_dev *dev, u32 target_idx, { int rc; - nfc_dbg("dev_name=%s target_idx=%u skb->len=%u", dev_name(&dev->dev), - target_idx, skb->len); + pr_debug("dev_name=%s target_idx=%u skb->len=%u\n", + dev_name(&dev->dev), target_idx, skb->len); device_lock(&dev->dev); @@ -289,13 +355,54 @@ error: return rc; } +int nfc_set_remote_general_bytes(struct nfc_dev *dev, u8 *gb, u8 gb_len) +{ + pr_debug("dev_name=%s gb_len=%d\n", + dev_name(&dev->dev), gb_len); + + if (gb_len > NFC_MAX_GT_LEN) + return -EINVAL; + + return nfc_llcp_set_remote_gb(dev, gb, gb_len); +} +EXPORT_SYMBOL(nfc_set_remote_general_bytes); + +u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, u8 *gt_len) +{ + return nfc_llcp_general_bytes(dev, gt_len); +} +EXPORT_SYMBOL(nfc_get_local_general_bytes); + /** - * nfc_alloc_skb - allocate a skb for data exchange responses + * nfc_alloc_send_skb - allocate a skb for data exchange responses * * @size: size to allocate * @gfp: gfp flags */ -struct sk_buff *nfc_alloc_skb(unsigned int size, gfp_t gfp) +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; @@ -308,7 +415,7 @@ struct sk_buff *nfc_alloc_skb(unsigned int size, gfp_t gfp) return skb; } -EXPORT_SYMBOL(nfc_alloc_skb); +EXPORT_SYMBOL(nfc_alloc_recv_skb); /** * nfc_targets_found - inform that targets were found @@ -326,7 +433,7 @@ int nfc_targets_found(struct nfc_dev *dev, struct nfc_target *targets, { int i; - nfc_dbg("dev_name=%s n_targets=%d", dev_name(&dev->dev), n_targets); + pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets); dev->polling = false; @@ -360,7 +467,7 @@ static void nfc_release(struct device *d) { struct nfc_dev *dev = to_nfc_dev(d); - nfc_dbg("dev_name=%s", dev_name(&dev->dev)); + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); nfc_genl_data_exit(&dev->genl_data); kfree(dev->targets); @@ -446,7 +553,7 @@ int nfc_register_device(struct nfc_dev *dev) { int rc; - nfc_dbg("dev_name=%s", dev_name(&dev->dev)); + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); mutex_lock(&nfc_devlist_mutex); nfc_devlist_generation++; @@ -456,11 +563,14 @@ int nfc_register_device(struct nfc_dev *dev) if (rc < 0) return rc; - rc = nfc_genl_device_added(dev); + rc = nfc_llcp_register_device(dev); if (rc) - nfc_dbg("The userspace won't be notified that the device %s was" - " added", dev_name(&dev->dev)); + 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)); return 0; } @@ -475,7 +585,7 @@ void nfc_unregister_device(struct nfc_dev *dev) { int rc; - nfc_dbg("dev_name=%s", dev_name(&dev->dev)); + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); mutex_lock(&nfc_devlist_mutex); nfc_devlist_generation++; @@ -488,10 +598,12 @@ void nfc_unregister_device(struct nfc_dev *dev) mutex_unlock(&nfc_devlist_mutex); + nfc_llcp_unregister_device(dev); + rc = nfc_genl_device_removed(dev); if (rc) - nfc_dbg("The userspace won't be notified that the device %s" - " was removed", dev_name(&dev->dev)); + pr_debug("The userspace won't be notified that the device %s was removed\n", + dev_name(&dev->dev)); } EXPORT_SYMBOL(nfc_unregister_device); @@ -500,7 +612,7 @@ static int __init nfc_init(void) { int rc; - nfc_info("NFC Core ver %s", VERSION); + pr_info("NFC Core ver %s\n", VERSION); rc = class_register(&nfc_class); if (rc) @@ -517,6 +629,10 @@ static int __init nfc_init(void) 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; @@ -524,6 +640,8 @@ static int __init nfc_init(void) return 0; err_af_nfc: + nfc_llcp_exit(); +err_llcp_sock: rawsock_exit(); err_rawsock: nfc_genl_exit(); @@ -535,6 +653,7 @@ err_genl: static void __exit nfc_exit(void) { af_nfc_exit(); + nfc_llcp_exit(); rawsock_exit(); nfc_genl_exit(); class_unregister(&nfc_class); diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig new file mode 100644 index 0000000..fbf5e81 --- /dev/null +++ b/net/nfc/llcp/Kconfig @@ -0,0 +1,7 @@ +config NFC_LLCP + depends on NFC && EXPERIMENTAL + bool "NFC LLCP support (EXPERIMENTAL)" + default n + help + Say Y here if you want to build support for a kernel NFC LLCP + implementation.
\ No newline at end of file diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c new file mode 100644 index 0000000..151f2ef --- /dev/null +++ b/net/nfc/llcp/commands.c @@ -0,0 +1,399 @@ +/* + * Copyright (C) 2011 Intel Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the + * Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +#define pr_fmt(fmt) "llcp: %s: " fmt, __func__ + +#include <linux/init.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/nfc.h> + +#include <net/nfc/nfc.h> + +#include "../nfc.h" +#include "llcp.h" + +static u8 llcp_tlv_length[LLCP_TLV_MAX] = { + 0, + 1, /* VERSION */ + 2, /* MIUX */ + 2, /* WKS */ + 1, /* LTO */ + 1, /* RW */ + 0, /* SN */ + 1, /* OPT */ + 0, /* SDREQ */ + 2, /* SDRES */ + +}; + +static u8 llcp_tlv8(u8 *tlv, u8 type) +{ + if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) + return 0; + + return tlv[2]; +} + +static u8 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) & 0x7f; +} + +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 + 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; +} + +int nfc_llcp_parse_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; + case LLCP_TLV_RW: + local->remote_rw = llcp_tlv_rw(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 rw %d\n", + local->remote_version, local->remote_miu, + local->remote_lto, local->remote_opt, + local->remote_wks, local->remote_rw); + + return 0; +} + +static struct sk_buff *llcp_add_header(struct sk_buff *pdu, + u8 dsap, u8 ssap, u8 ptype) +{ + u8 header[2]; + + pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); + + header[0] = (u8)((dsap << 2) | (ptype >> 2)); + header[1] = (u8)((ptype << 6) | ssap); + + pr_debug("header 0x%x 0x%x\n", header[0], header[1]); + + memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE); + + return pdu; +} + +static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, u8 tlv_length) +{ + /* XXX Add an skb length check */ + + if (tlv == NULL) + return NULL; + + memcpy(skb_put(pdu, tlv_length), tlv, tlv_length); + + return pdu; +} + +static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, + u8 cmd, u16 size) +{ + struct sk_buff *skb; + int err; + + if (sock->ssap == 0) + return NULL; + + skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, + size + LLCP_HEADER_SIZE, &err); + if (skb == NULL) { + pr_err("Could not allocate PDU\n"); + return NULL; + } + + skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd); + + return skb; +} + +int nfc_llcp_disconnect(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + u16 size = 0; + + pr_debug("Sending DISC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + dev = sock->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_ATOMIC); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, sock->ssap, sock->dsap, LLCP_PDU_DISC); + + 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); + + 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; + int err; + u16 size = 0; + + 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; + } + + pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); + + skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); + if (skb == NULL) { + err = -ENOMEM; + goto error_tlv; + } + + if (service_name_tlv != NULL) + skb = llcp_add_tlv(skb, service_name_tlv, + service_name_tlv_length); + + skb_queue_tail(&local->tx_queue, skb); + + return 0; + +error_tlv: + pr_err("error %d\n", err); + + kfree(service_name_tlv); + + return err; +} + +int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) +{ + struct nfc_llcp_local *local; + struct sk_buff *skb; + + pr_debug("Sending CC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, 0); + if (skb == NULL) + return -ENOMEM; + + 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, ssap, dsap, LLCP_PDU_DM); + + memcpy(skb_put(skb, 1), &reason, 1); + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_llcp_local *local; + + pr_debug("Send DISC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0); + if (skb == NULL) + return -ENOMEM; + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c new file mode 100644 index 0000000..1d32680 --- /dev/null +++ b/net/nfc/llcp/llcp.c @@ -0,0 +1,971 @@ +/* + * Copyright (C) 2011 Intel Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the + * Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +#define pr_fmt(fmt) "llcp: %s: " fmt, __func__ + +#include <linux/init.h> +#include <linux/kernel.h> +#include <linux/list.h> +#include <linux/nfc.h> + +#include "../nfc.h" +#include "llcp.h" + +static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; + +static struct list_head llcp_devices; + +static void nfc_llcp_socket_release(struct nfc_llcp_local *local) +{ + struct nfc_llcp_sock *parent, *s, *n; + struct sock *sk, *parent_sk; + int i; + + + mutex_lock(&local->socket_lock); + + for (i = 0; i < LLCP_MAX_SAP; i++) { + parent = local->sockets[i]; + if (parent == NULL) + continue; + + /* Release all child sockets */ + list_for_each_entry_safe(s, n, &parent->list, list) { + list_del(&s->list); + sk = &s->sk; + + lock_sock(sk); + + if (sk->sk_state == LLCP_CONNECTED) + nfc_put_device(s->dev); + + sk->sk_state = LLCP_CLOSED; + sock_set_flag(sk, SOCK_DEAD); + + release_sock(sk); + } + + parent_sk = &parent->sk; + + lock_sock(parent_sk); + + if (parent_sk->sk_state == LLCP_LISTEN) { + struct nfc_llcp_sock *lsk, *n; + struct sock *accept_sk; + + list_for_each_entry_safe(lsk, n, &parent->accept_queue, + accept_queue) { + accept_sk = &lsk->sk; + lock_sock(accept_sk); + + nfc_llcp_accept_unlink(accept_sk); + + accept_sk->sk_state = LLCP_CLOSED; + sock_set_flag(accept_sk, SOCK_DEAD); + + release_sock(accept_sk); + + sock_orphan(accept_sk); + } + } + + if (parent_sk->sk_state == LLCP_CONNECTED) + nfc_put_device(parent->dev); + + parent_sk->sk_state = LLCP_CLOSED; + sock_set_flag(parent_sk, SOCK_DEAD); + + release_sock(parent_sk); + } + + mutex_unlock(&local->socket_lock); +} + +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"); + + queue_work(local->timeout_wq, &local->timeout_work); +} + +struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) +{ + struct nfc_llcp_local *local, *n; + + list_for_each_entry_safe(local, n, &llcp_devices, list) + if (local->dev == dev) + return local; + + pr_debug("No device found\n"); + + return NULL; +} + +static char *wks[] = { + NULL, + NULL, /* SDP */ + "urn:nfc:sn:ip", + "urn:nfc:sn:obex", + "urn:nfc:sn:snep", +}; + +static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len) +{ + int sap, num_wks; + + pr_debug("%s\n", service_name); + + if (service_name == NULL) + return -EINVAL; + + num_wks = ARRAY_SIZE(wks); + + for (sap = 0 ; sap < num_wks; sap++) { + if (wks[sap] == NULL) + continue; + + if (strncmp(wks[sap], service_name, service_name_len) == 0) + return sap; + } + + return -EINVAL; +} + +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(BIT(ssap), &local->local_wks); + mutex_unlock(&local->sdp_lock); + + return ssap; + } + + /* + * This is not a well known service, + * we should try to find a local SDP free spot + */ + 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(BIT(ssap), &local->local_sdp); + mutex_unlock(&local->sdp_lock); + + return LLCP_WKS_NUM_SAP + ssap; + + } else if (sock->ssap != 0) { + if (sock->ssap < LLCP_WKS_NUM_SAP) { + if (!(local->local_wks & BIT(sock->ssap))) { + set_bit(BIT(sock->ssap), &local->local_wks); + mutex_unlock(&local->sdp_lock); + + return sock->ssap; + } + + } else if (sock->ssap < LLCP_SDP_NUM_SAP) { + if (!(local->local_sdp & + BIT(sock->ssap - LLCP_WKS_NUM_SAP))) { + set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP), + &local->local_sdp); + 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(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) { + local_ssap = ssap - LLCP_WKS_NUM_SAP; + sdp = &local->local_sdp; + } 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(1 << local_ssap, sdp); + + mutex_unlock(&local->sdp_lock); +} + +u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len) +{ + struct nfc_llcp_local *local; + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + *general_bytes_len = 0; + return NULL; + } + + *general_bytes_len = local->gb_len; + + return local->gb; +} + +static int nfc_llcp_build_gb(struct nfc_llcp_local *local) +{ + u8 *gb_cur, *version_tlv, version, version_length; + u8 *lto_tlv, lto, lto_length; + u8 *wks_tlv, wks_length; + u8 gb_len = 0; + + version = LLCP_VERSION_11; + version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, + 1, &version_length); + gb_len += version_length; + + /* 1500 ms */ + lto = 150; + lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, <o, 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 *)&local->local_wks, 2, + &wks_length); + gb_len += wks_length; + + gb_len += ARRAY_SIZE(llcp_magic); + + if (gb_len > NFC_MAX_GT_LEN) { + kfree(version_tlv); + return -EINVAL; + } + + 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; + + kfree(version_tlv); + kfree(lto_tlv); + + local->gb_len = gb_len; + + return 0; +} + +int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) +{ + struct nfc_llcp_local *local = nfc_llcp_find_local(dev); + + if (local == NULL) { + pr_err("No LLCP device\n"); + return -ENODEV; + } + + memset(local->remote_gb, 0, NFC_MAX_GT_LEN); + memcpy(local->remote_gb, gb, gb_len); + local->remote_gb_len = gb_len; + + if (local->remote_gb == NULL || + local->remote_gb_len == 0) + return -ENODEV; + + if (memcmp(local->remote_gb, llcp_magic, 3)) { + pr_err("MAC does not support LLCP\n"); + return -EINVAL; + } + + return nfc_llcp_parse_tlv(local, + &local->remote_gb[3], local->remote_gb_len - 3); +} + +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; + + skb = skb_dequeue(&local->tx_queue); + if (skb != NULL) { + pr_debug("Sending pending skb\n"); + nfc_data_exchange(local->dev, local->target_idx, + skb, nfc_llcp_recv, local); + } else { + nfc_llcp_send_symm(local->dev); + } + + mod_timer(&local->link_timer, + jiffies + msecs_to_jiffies(local->remote_lto)); +} + +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 - 1) % 16); + sock->send_n = (sock->send_n + 1) % 16; + sock->recv_ack_n = (sock->recv_n - 1) % 16; +} + +static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, + u8 ssap, u8 dsap) +{ + struct nfc_llcp_sock *sock, *llcp_sock, *n; + + if (ssap == 0 && dsap == 0) + return NULL; + + mutex_lock(&local->socket_lock); + sock = local->sockets[ssap]; + if (sock == NULL) { + mutex_unlock(&local->socket_lock); + return NULL; + } + + pr_debug("root dsap %d (%d)\n", sock->dsap, dsap); + + if (sock->dsap == dsap) { + sock_hold(&sock->sk); + mutex_unlock(&local->socket_lock); + return sock; + } + + list_for_each_entry_safe(llcp_sock, n, &sock->list, list) { + pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock, + &llcp_sock->sk, llcp_sock->dsap); + if (llcp_sock->dsap == dsap) { + sock_hold(&llcp_sock->sk); + mutex_unlock(&local->socket_lock); + return llcp_sock; + } + } + + pr_err("Could not find socket for %d %d\n", ssap, dsap); + + mutex_unlock(&local->socket_lock); + + return NULL; +} + +static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) +{ + sock_put(&sock->sk); +} + +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_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, bound_sap, reason; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("%d %d\n", dsap, ssap); + + nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + if (dsap != LLCP_SAP_SDP) { + bound_sap = dsap; + + mutex_lock(&local->socket_lock); + sock = local->sockets[dsap]; + if (sock == NULL) { + mutex_unlock(&local->socket_lock); + reason = LLCP_DM_NOBOUND; + goto fail; + } + + sock_hold(&sock->sk); + mutex_unlock(&local->socket_lock); + + lock_sock(&sock->sk); + + if (sock->dsap == LLCP_SAP_SDP && + sock->sk.sk_state == LLCP_LISTEN) + goto enqueue; + } 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); + + mutex_lock(&local->socket_lock); + for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET; + bound_sap++) { + sock = local->sockets[bound_sap]; + if (sock == NULL) + continue; + + if (sock->service_name == NULL || + sock->service_name_len == 0) + continue; + + if (sock->service_name_len != sn_len) + continue; + + if (sock->dsap == LLCP_SAP_SDP && + sock->sk.sk_state == LLCP_LISTEN && + !memcmp(sn, sock->service_name, sn_len)) { + pr_debug("Found service name at SAP %d\n", + bound_sap); + sock_hold(&sock->sk); + mutex_unlock(&local->socket_lock); + + lock_sock(&sock->sk); + + goto enqueue; + } + } + mutex_unlock(&local->socket_lock); + } + + reason = LLCP_DM_NOBOUND; + goto fail; + +enqueue: + parent = &sock->sk; + + if (sk_acceptq_is_full(parent)) { + reason = LLCP_DM_REJ; + release_sock(&sock->sk); + sock_put(&sock->sk); + goto fail; + } + + new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, + GFP_ATOMIC); + if (new_sk == NULL) { + reason = LLCP_DM_REJ; + release_sock(&sock->sk); + sock_put(&sock->sk); + goto fail; + } + + new_sock = nfc_llcp_sock(new_sk); + new_sock->dev = local->dev; + new_sock->local = local; + new_sock->nfc_protocol = sock->nfc_protocol; + new_sock->ssap = bound_sap; + new_sock->dsap = ssap; + new_sock->parent = parent; + + pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); + + list_add_tail(&new_sock->list, &sock->list); + + nfc_llcp_accept_enqueue(&sock->sk, new_sk); + + nfc_get_device(local->dev->idx); + + new_sk->sk_state = LLCP_CONNECTED; + + /* Wake the listening processes */ + parent->sk_data_ready(parent, 0); + + /* Send CC */ + nfc_llcp_send_cc(new_sock); + + release_sock(&sock->sk); + sock_put(&sock->sk); + + return; + +fail: + /* Send DM */ + nfc_llcp_send_dm(local, dsap, ssap, reason); + + return; + +} + +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); + } + + 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"); + + /* Pass the payload upstream */ + if (ptype == LLCP_PDU_I) { + pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); + + skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); + if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { + pr_err("receive queue is full\n"); + skb_queue_head(&llcp_sock->tx_backlog_queue, skb); + } + } + + /* Remove skbs from the pending queue */ + if (llcp_sock->send_ack_n != nr) { + struct sk_buff *s, *tmp; + + llcp_sock->send_ack_n = nr; + + skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) + if (nfc_llcp_ns(s) <= nr) { + skb_unlink(s, &llcp_sock->tx_pending_queue); + kfree_skb(s); + } + } + + /* Queue some I frames for transmission */ + while (llcp_sock->remote_ready && + skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) { + struct sk_buff *pdu, *pending_pdu; + + pdu = skb_dequeue(&llcp_sock->tx_queue); + if (pdu == NULL) + break; + + /* Update N(S)/N(R) */ + nfc_llcp_set_nrns(llcp_sock, pdu); + + pending_pdu = skb_clone(pdu, GFP_KERNEL); + + skb_queue_tail(&local->tx_queue, pdu); + skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu); + } + + 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); + + 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); + } + + + 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; + u8 dsap, ssap; + + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + + if (llcp_sock == NULL) + llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); + + if (llcp_sock == NULL) { + pr_err("Invalid CC\n"); + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + + return; + } + + llcp_sock->dsap = ssap; + + nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_rx_work(struct work_struct *work) +{ + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + rx_work); + u8 dsap, ssap, ptype; + struct sk_buff *skb; + + skb = local->rx_pending; + if (skb == NULL) { + pr_debug("No pending SKB\n"); + return; + } + + 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); + + switch (ptype) { + case LLCP_PDU_SYMM: + pr_debug("SYMM\n"); + 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_I: + case LLCP_PDU_RR: + pr_debug("I frame\n"); + nfc_llcp_recv_hdlc(local, skb); + break; + + } + + queue_work(local->tx_wq, &local->tx_work); + kfree_skb(local->rx_pending); + local->rx_pending = NULL; + + return; +} + +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", err); + return; + } + + local->rx_pending = skb_get(skb); + del_timer(&local->link_timer); + queue_work(local->rx_wq, &local->rx_work); + + return; +} + +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; + + /* Close and purge all existing sockets */ + nfc_llcp_socket_release(local); +} + +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"); + + queue_work(local->tx_wq, &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 device *dev = &ndev->dev; + struct nfc_llcp_local *local; + char name[32]; + int err; + + local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); + if (local == NULL) + return -ENOMEM; + + local->dev = ndev; + INIT_LIST_HEAD(&local->list); + mutex_init(&local->sdp_lock); + mutex_init(&local->socket_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); + snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); + local->tx_wq = alloc_workqueue(name, + WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); + if (local->tx_wq == NULL) { + err = -ENOMEM; + goto err_local; + } + + local->rx_pending = NULL; + INIT_WORK(&local->rx_work, nfc_llcp_rx_work); + snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); + local->rx_wq = alloc_workqueue(name, + WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); + if (local->rx_wq == NULL) { + err = -ENOMEM; + goto err_tx_wq; + } + + INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); + snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); + local->timeout_wq = alloc_workqueue(name, + WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); + if (local->timeout_wq == NULL) { + err = -ENOMEM; + goto err_rx_wq; + } + + nfc_llcp_build_gb(local); + + local->remote_miu = LLCP_DEFAULT_MIU; + local->remote_lto = LLCP_DEFAULT_LTO; + local->remote_rw = LLCP_DEFAULT_RW; + + list_add(&llcp_devices, &local->list); + + return 0; + +err_rx_wq: + destroy_workqueue(local->rx_wq); + +err_tx_wq: + destroy_workqueue(local->tx_wq); + +err_local: + kfree(local); + + 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; + } + + list_del(&local->list); + nfc_llcp_socket_release(local); + del_timer_sync(&local->link_timer); + skb_queue_purge(&local->tx_queue); + destroy_workqueue(local->tx_wq); + destroy_workqueue(local->rx_wq); + kfree_skb(local->rx_pending); + kfree(local); +} + +int __init nfc_llcp_init(void) +{ + INIT_LIST_HEAD(&llcp_devices); + + return nfc_llcp_sock_init(); +} + +void nfc_llcp_exit(void) +{ + nfc_llcp_sock_exit(); +} diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h new file mode 100644 index 0000000..0ad2e33 --- /dev/null +++ b/net/nfc/llcp/llcp.h @@ -0,0 +1,193 @@ +/* + * Copyright (C) 2011 Intel Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the + * Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +enum llcp_state { + LLCP_CONNECTED = 1, /* wait_for_packet() wants that */ + LLCP_CLOSED, + LLCP_BOUND, + LLCP_LISTEN, +}; + +#define LLCP_DEFAULT_LTO 100 +#define LLCP_DEFAULT_RW 1 +#define LLCP_DEFAULT_MIU 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) + +struct nfc_llcp_sock; + +struct nfc_llcp_local { + struct list_head list; + struct nfc_dev *dev; + + struct mutex sdp_lock; + struct mutex socket_lock; + + struct timer_list link_timer; + struct sk_buff_head tx_queue; + struct workqueue_struct *tx_wq; + struct work_struct tx_work; + struct workqueue_struct *rx_wq; + struct work_struct rx_work; + struct sk_buff *rx_pending; + struct workqueue_struct *timeout_wq; + struct work_struct timeout_work; + + u32 target_idx; + u8 rf_mode; + u8 comm_mode; + unsigned long local_wks; /* Well known services */ + unsigned long local_sdp; /* Local services */ + unsigned long local_sap; /* Local SAPs, not available for discovery */ + + /* 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; + u8 remote_rw; + + /* sockets array */ + struct nfc_llcp_sock *sockets[LLCP_MAX_SAP]; +}; + +struct nfc_llcp_sock { + struct sock sk; + struct list_head list; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + u32 target_idx; + u32 nfc_protocol; + + u8 ssap; + u8 dsap; + char *service_name; + size_t service_name_len; + + /* 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; + + struct sk_buff_head tx_queue; + struct sk_buff_head tx_pending_queue; + struct sk_buff_head tx_backlog_queue; + + struct list_head accept_queue; + struct sock *parent; +}; + +#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 + +/* 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 + + +struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev); +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); + +/* Sock API */ +struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp); +void nfc_llcp_sock_free(struct nfc_llcp_sock *sock); +void nfc_llcp_accept_unlink(struct sock *sk); +void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk); +struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock); + +/* TLV API */ +int nfc_llcp_parse_tlv(struct nfc_llcp_local *local, + 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); +void nfc_llcp_recv(void *data, struct sk_buff *skb, int err); +int nfc_llcp_disconnect(struct nfc_llcp_sock *sock); +int nfc_llcp_send_symm(struct nfc_dev *dev); +int nfc_llcp_send_connect(struct nfc_llcp_sock *sock); +int nfc_llcp_send_cc(struct nfc_llcp_sock *sock); +int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason); +int nfc_llcp_send_disconnect(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/sock.c b/net/nfc/llcp/sock.c new file mode 100644 index 0000000..f738ccd --- /dev/null +++ b/net/nfc/llcp/sock.c @@ -0,0 +1,675 @@ +/* + * Copyright (C) 2011 Intel Corporation. All rights reserved. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the + * Free Software Foundation, Inc., + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + */ + +#define pr_fmt(fmt) "llcp: %s: " fmt, __func__ + +#include <linux/init.h> +#include <linux/kernel.h> +#include <linux/module.h> +#include <linux/nfc.h> + +#include "../nfc.h" +#include "llcp.h" + +static 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; + + pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); + + if (!addr || addr->sa_family != AF_NFC) + return -EINVAL; + + 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 = 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_MAX_SAP) + goto put_dev; + + local->sockets[llcp_sock->ssap] = llcp_sock; + + 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_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; +} + +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) { + nfc_llcp_accept_unlink(sk); + if (newsock) + sock_graft(sk, newsock); + + release_sock(sk); + + pr_debug("Returning sk state %d\n", sk->sk_state); + + return sk; + } + + release_sock(sk); + } + + return NULL; +} + +static int llcp_sock_accept(struct socket *sock, struct socket *newsock, + int flags) +{ + DECLARE_WAITQUEUE(wait, current); + struct sock *sk = sock->sk, *new_sk; + long timeo; + int ret = 0; + + pr_debug("parent %p\n", sk); + + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + + if (sk->sk_state != LLCP_LISTEN) { + ret = -EBADFD; + goto error; + } + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + /* Wait for an incoming connection. */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) { + set_current_state(TASK_INTERRUPTIBLE); + + if (!timeo) { + ret = -EAGAIN; + break; + } + + if (signal_pending(current)) { + ret = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + if (ret) + goto error; + + newsock->state = SS_CONNECTED; + + pr_debug("new socket %p\n", new_sk); + +error: + release_sock(sk); + + return ret; +} + +static int llcp_sock_getname(struct socket *sock, struct sockaddr *addr, + int *len, int peer) +{ + struct sockaddr_nfc_llcp *llcp_addr = (struct sockaddr_nfc_llcp *) addr; + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + + pr_debug("%p\n", sk); + + addr->sa_family = AF_NFC; + *len = sizeof(struct sockaddr_nfc_llcp); + + llcp_addr->dev_idx = llcp_sock->dev->idx; + llcp_addr->dsap = llcp_sock->dsap; + llcp_addr->ssap = llcp_sock->ssap; + llcp_addr->service_name_len = llcp_sock->service_name_len; + memcpy(llcp_addr->service_name, llcp_sock->service_name, + llcp_addr->service_name_len); + + return 0; +} + +static inline unsigned int llcp_accept_poll(struct sock *parent) +{ + struct nfc_llcp_sock *llcp_sock, *n, *parent_sock; + struct sock *sk; + + parent_sock = nfc_llcp_sock(parent); + + list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue, + accept_queue) { + sk = &llcp_sock->sk; + + if (sk->sk_state == LLCP_CONNECTED) + return POLLIN | POLLRDNORM; + } + + return 0; +} + +static unsigned int llcp_sock_poll(struct file *file, struct socket *sock, + poll_table *wait) +{ + struct sock *sk = sock->sk; + unsigned int mask = 0; + + pr_debug("%p\n", sk); + + sock_poll_wait(file, sk_sleep(sk), wait); + + if (sk->sk_state == LLCP_LISTEN) + return llcp_accept_poll(sk); + + if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue)) + mask |= POLLERR; + + if (!skb_queue_empty(&sk->sk_receive_queue)) + mask |= POLLIN; + + if (sk->sk_state == LLCP_CLOSED) + mask |= POLLHUP; + + 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); + + if (!sk) + return 0; + + pr_debug("%p\n", sk); + + local = llcp_sock->local; + if (local == NULL) + return -ENODEV; + + mutex_lock(&local->socket_lock); + + if (llcp_sock == local->sockets[llcp_sock->ssap]) { + local->sockets[llcp_sock->ssap] = NULL; + } else { + struct nfc_llcp_sock *parent, *s, *n; + + parent = local->sockets[llcp_sock->ssap]; + + list_for_each_entry_safe(s, n, &parent->list, list) + if (llcp_sock == s) { + list_del(&s->list); + break; + } + + } + + mutex_unlock(&local->socket_lock); + + lock_sock(sk); + + /* Send a DISC */ + if (sk->sk_state == LLCP_CONNECTED) + nfc_llcp_disconnect(llcp_sock); + + if (sk->sk_state == LLCP_LISTEN) { + struct nfc_llcp_sock *lsk, *n; + struct sock *accept_sk; + + list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue, + accept_queue) { + accept_sk = &lsk->sk; + lock_sock(accept_sk); + + nfc_llcp_disconnect(lsk); + nfc_llcp_accept_unlink(accept_sk); + + release_sock(accept_sk); + + sock_set_flag(sk, SOCK_DEAD); + sock_orphan(accept_sk); + sock_put(accept_sk); + } + } + + /* Freeing the SAP */ + if ((sk->sk_state == LLCP_CONNECTED + && llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) || + sk->sk_state == LLCP_BOUND || + sk->sk_state == LLCP_LISTEN) + nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); + + sock_set_flag(sk, SOCK_DEAD); + + release_sock(sk); + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +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) { + pr_err("Invalid socket\n"); + return -EINVAL; + } + + if (addr->service_name_len == 0 && addr->dsap == 0) { + pr_err("Missing service name or dsap\n"); + 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 = local; + llcp_sock->ssap = nfc_llcp_get_local_ssap(local); + if (llcp_sock->ssap == LLCP_SAP_MAX) { + ret = -ENOMEM; + goto put_dev; + } + 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); + + local->sockets[llcp_sock->ssap] = llcp_sock; + + ret = nfc_llcp_send_connect(llcp_sock); + if (ret) + goto put_dev; + + sk->sk_state = LLCP_CONNECTED; + + release_sock(sk); + return 0; + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len, int flags) +{ + int noblock = flags & MSG_DONTWAIT; + struct sock *sk = sock->sk; + unsigned int copied, rlen; + struct sk_buff *skb, *cskb; + int err = 0; + + pr_debug("%p %zu\n", sk, len); + + 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 (memcpy_toiovec(msg->msg_iov, cskb->data, copied)) { + if (!(flags & MSG_PEEK)) + skb_queue_head(&sk->sk_receive_queue, skb); + return -EFAULT; + } + + /* 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) { + skb_pull(skb, copied); + if (skb->len) { + skb_queue_head(&sk->sk_receive_queue, skb); + goto done; + } + } + + kfree_skb(skb); + } + + /* XXX Queue backlogged skbs */ + +done: + /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */ + if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC)) + copied = rlen; + + return copied; +} + +static const struct proto_ops llcp_sock_ops = { + .family = PF_NFC, + .owner = THIS_MODULE, + .bind = llcp_sock_bind, + .connect = llcp_sock_connect, + .release = llcp_sock_release, + .socketpair = sock_no_socketpair, + .accept = llcp_sock_accept, + .getname = llcp_sock_getname, + .poll = llcp_sock_poll, + .ioctl = sock_no_ioctl, + .listen = llcp_sock_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .sendmsg = sock_no_sendmsg, + .recvmsg = llcp_sock_recvmsg, + .mmap = sock_no_mmap, +}; + +static void llcp_sock_destruct(struct sock *sk) +{ + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + + pr_debug("%p\n", sk); + + if (sk->sk_state == LLCP_CONNECTED) + nfc_put_device(llcp_sock->dev); + + skb_queue_purge(&sk->sk_receive_queue); + + nfc_llcp_sock_free(llcp_sock); + + if (!sock_flag(sk, SOCK_DEAD)) { + pr_err("Freeing alive NFC LLCP socket %p\n", sk); + return; + } +} + +struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp) +{ + struct sock *sk; + struct nfc_llcp_sock *llcp_sock; + + sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto); + if (!sk) + return NULL; + + llcp_sock = nfc_llcp_sock(sk); + + sock_init_data(sock, sk); + sk->sk_state = LLCP_CLOSED; + sk->sk_protocol = NFC_SOCKPROTO_LLCP; + sk->sk_type = type; + sk->sk_destruct = llcp_sock_destruct; + + llcp_sock->ssap = 0; + llcp_sock->dsap = LLCP_SAP_SDP; + llcp_sock->send_n = llcp_sock->send_ack_n = 0; + llcp_sock->recv_n = llcp_sock->recv_ack_n = 0; + llcp_sock->remote_ready = 1; + skb_queue_head_init(&llcp_sock->tx_queue); + skb_queue_head_init(&llcp_sock->tx_pending_queue); + skb_queue_head_init(&llcp_sock->tx_backlog_queue); + INIT_LIST_HEAD(&llcp_sock->list); + 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); + skb_queue_purge(&sock->tx_backlog_queue); + + list_del_init(&sock->accept_queue); + + sock->parent = NULL; +} + +static int llcp_sock_create(struct net *net, struct socket *sock, + const struct nfc_protocol *nfc_proto) +{ + struct sock *sk; + + pr_debug("%p\n", sock); + + if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM) + return -ESOCKTNOSUPPORT; + + sock->ops = &llcp_sock_ops; + + sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC); + if (sk == NULL) + return -ENOMEM; + + return 0; +} + +static const struct nfc_protocol llcp_nfc_proto = { + .id = NFC_SOCKPROTO_LLCP, + .proto = &llcp_sock_proto, + .owner = THIS_MODULE, + .create = llcp_sock_create +}; + +int __init nfc_llcp_sock_init(void) +{ + return nfc_proto_register(&llcp_nfc_proto); +} + +void nfc_llcp_sock_exit(void) +{ + nfc_proto_unregister(&llcp_nfc_proto); +} diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c index ea66034..7650139 100644 --- a/net/nfc/nci/core.c +++ b/net/nfc/nci/core.c @@ -25,6 +25,8 @@ * */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <linux/types.h> #include <linux/workqueue.h> #include <linux/completion.h> @@ -79,7 +81,7 @@ static int __nci_request(struct nci_dev *ndev, &ndev->req_completion, timeout); - nfc_dbg("wait_for_completion return %ld", completion_rc); + pr_debug("wait_for_completion return %ld\n", completion_rc); if (completion_rc > 0) { switch (ndev->req_status) { @@ -96,8 +98,8 @@ static int __nci_request(struct nci_dev *ndev, break; } } else { - nfc_err("wait_for_completion_interruptible_timeout failed %ld", - completion_rc); + pr_err("wait_for_completion_interruptible_timeout failed %ld\n", + completion_rc); rc = ((completion_rc == 0) ? (-ETIMEDOUT) : (completion_rc)); } @@ -126,7 +128,10 @@ static inline int nci_request(struct nci_dev *ndev, static void nci_reset_req(struct nci_dev *ndev, unsigned long opt) { - nci_send_cmd(ndev, NCI_OP_CORE_RESET_CMD, 0, NULL); + 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) @@ -136,17 +141,11 @@ static void nci_init_req(struct nci_dev *ndev, unsigned long opt) static void nci_init_complete_req(struct nci_dev *ndev, unsigned long opt) { - struct nci_core_conn_create_cmd conn_cmd; struct nci_rf_disc_map_cmd cmd; struct disc_map_config *cfg = cmd.mapping_configs; __u8 *num = &cmd.num_mapping_configs; int i; - /* create static rf connection */ - conn_cmd.target_handle = 0; - conn_cmd.num_target_specific_params = 0; - nci_send_cmd(ndev, NCI_OP_CORE_CONN_CREATE_CMD, 2, &conn_cmd); - /* set rf mapping configurations */ *num = 0; @@ -155,14 +154,16 @@ static void nci_init_complete_req(struct nci_dev *ndev, unsigned long opt) 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_BOTH; - cfg[*num].rf_interface_type = NCI_RF_INTERFACE_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_BOTH; - cfg[*num].rf_interface_type = NCI_RF_INTERFACE_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)++; } @@ -187,16 +188,16 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) || protocols & NFC_PROTO_MIFARE_MASK || protocols & NFC_PROTO_ISO14443_MASK || protocols & NFC_PROTO_NFC_DEP_MASK)) { - cmd.disc_configs[cmd.num_disc_configs].type = - NCI_DISCOVERY_TYPE_POLL_A_PASSIVE; + cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = + NCI_NFC_A_PASSIVE_POLL_MODE; cmd.disc_configs[cmd.num_disc_configs].frequency = 1; cmd.num_disc_configs++; } if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && (protocols & NFC_PROTO_ISO14443_MASK)) { - cmd.disc_configs[cmd.num_disc_configs].type = - NCI_DISCOVERY_TYPE_POLL_B_PASSIVE; + 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++; } @@ -204,8 +205,8 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && (protocols & NFC_PROTO_FELICA_MASK || protocols & NFC_PROTO_NFC_DEP_MASK)) { - cmd.disc_configs[cmd.num_disc_configs].type = - NCI_DISCOVERY_TYPE_POLL_F_PASSIVE; + 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++; } @@ -326,8 +327,6 @@ static void nci_cmd_timer(unsigned long arg) { struct nci_dev *ndev = (void *) arg; - nfc_dbg("entry"); - atomic_set(&ndev->cmd_cnt, 1); queue_work(ndev->cmd_wq, &ndev->cmd_work); } @@ -336,8 +335,6 @@ static int nci_dev_up(struct nfc_dev *nfc_dev) { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); - nfc_dbg("entry"); - return nci_open_device(ndev); } @@ -345,8 +342,6 @@ static int nci_dev_down(struct nfc_dev *nfc_dev) { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); - nfc_dbg("entry"); - return nci_close_device(ndev); } @@ -355,20 +350,18 @@ static int nci_start_poll(struct nfc_dev *nfc_dev, __u32 protocols) struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); int rc; - nfc_dbg("entry"); - if (test_bit(NCI_DISCOVERY, &ndev->flags)) { - nfc_err("unable to start poll, since poll is already active"); + pr_err("unable to start poll, since poll is already active\n"); return -EBUSY; } if (ndev->target_active_prot) { - nfc_err("there is an active target"); + pr_err("there is an active target\n"); return -EBUSY; } if (test_bit(NCI_POLL_ACTIVE, &ndev->flags)) { - nfc_dbg("target is active, implicitly deactivate..."); + pr_debug("target is active, implicitly deactivate...\n"); rc = nci_request(ndev, nci_rf_deactivate_req, 0, msecs_to_jiffies(NCI_RF_DEACTIVATE_TIMEOUT)); @@ -389,10 +382,8 @@ static void nci_stop_poll(struct nfc_dev *nfc_dev) { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); - nfc_dbg("entry"); - if (!test_bit(NCI_DISCOVERY, &ndev->flags)) { - nfc_err("unable to stop poll, since poll is not active"); + pr_err("unable to stop poll, since poll is not active\n"); return; } @@ -405,21 +396,21 @@ static int nci_activate_target(struct nfc_dev *nfc_dev, __u32 target_idx, { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); - nfc_dbg("entry, target_idx %d, protocol 0x%x", target_idx, protocol); + pr_debug("target_idx %d, protocol 0x%x\n", target_idx, protocol); if (!test_bit(NCI_POLL_ACTIVE, &ndev->flags)) { - nfc_err("there is no available target to activate"); + pr_err("there is no available target to activate\n"); return -EINVAL; } if (ndev->target_active_prot) { - nfc_err("there is already an active target"); + pr_err("there is already an active target\n"); return -EBUSY; } if (!(ndev->target_available_prots & (1 << protocol))) { - nfc_err("target does not support the requested protocol 0x%x", - protocol); + pr_err("target does not support the requested protocol 0x%x\n", + protocol); return -EINVAL; } @@ -433,10 +424,10 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev, __u32 target_idx) { struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); - nfc_dbg("entry, target_idx %d", target_idx); + pr_debug("target_idx %d\n", target_idx); if (!ndev->target_active_prot) { - nfc_err("unable to deactivate target, no active target"); + pr_err("unable to deactivate target, no active target\n"); return; } @@ -456,10 +447,10 @@ static int nci_data_exchange(struct nfc_dev *nfc_dev, __u32 target_idx, struct nci_dev *ndev = nfc_get_drvdata(nfc_dev); int rc; - nfc_dbg("entry, target_idx %d, len %d", target_idx, skb->len); + pr_debug("target_idx %d, len %d\n", target_idx, skb->len); if (!ndev->target_active_prot) { - nfc_err("unable to exchange data, no active target"); + pr_err("unable to exchange data, no active target\n"); return -EINVAL; } @@ -470,7 +461,7 @@ static int nci_data_exchange(struct nfc_dev *nfc_dev, __u32 target_idx, ndev->data_exchange_cb = cb; ndev->data_exchange_cb_context = cb_context; - rc = nci_send_data(ndev, ndev->conn_id, skb); + rc = nci_send_data(ndev, NCI_STATIC_RF_CONN_ID, skb); if (rc) clear_bit(NCI_DATA_EXCHANGE, &ndev->flags); @@ -502,7 +493,7 @@ struct nci_dev *nci_allocate_device(struct nci_ops *ops, { struct nci_dev *ndev; - nfc_dbg("entry, supported_protocols 0x%x", supported_protocols); + pr_debug("supported_protocols 0x%x\n", supported_protocols); if (!ops->open || !ops->close || !ops->send) return NULL; @@ -542,8 +533,6 @@ EXPORT_SYMBOL(nci_allocate_device); */ void nci_free_device(struct nci_dev *ndev) { - nfc_dbg("entry"); - nfc_free_device(ndev->nfc_dev); kfree(ndev); } @@ -560,8 +549,6 @@ int nci_register_device(struct nci_dev *ndev) struct device *dev = &ndev->nfc_dev->dev; char name[32]; - nfc_dbg("entry"); - rc = nfc_register_device(ndev->nfc_dev); if (rc) goto exit; @@ -624,8 +611,6 @@ EXPORT_SYMBOL(nci_register_device); */ void nci_unregister_device(struct nci_dev *ndev) { - nfc_dbg("entry"); - nci_close_device(ndev); destroy_workqueue(ndev->cmd_wq); @@ -645,7 +630,7 @@ int nci_recv_frame(struct sk_buff *skb) { struct nci_dev *ndev = (struct nci_dev *) skb->dev; - nfc_dbg("entry, len %d", skb->len); + pr_debug("len %d\n", skb->len); if (!ndev || (!test_bit(NCI_UP, &ndev->flags) && !test_bit(NCI_INIT, &ndev->flags))) { @@ -665,7 +650,7 @@ static int nci_send_frame(struct sk_buff *skb) { struct nci_dev *ndev = (struct nci_dev *) skb->dev; - nfc_dbg("entry, len %d", skb->len); + pr_debug("len %d\n", skb->len); if (!ndev) { kfree_skb(skb); @@ -684,11 +669,11 @@ int nci_send_cmd(struct nci_dev *ndev, __u16 opcode, __u8 plen, void *payload) struct nci_ctrl_hdr *hdr; struct sk_buff *skb; - nfc_dbg("entry, opcode 0x%x, plen %d", opcode, plen); + pr_debug("opcode 0x%x, plen %d\n", opcode, plen); skb = nci_skb_alloc(ndev, (NCI_CTRL_HDR_SIZE + plen), GFP_KERNEL); if (!skb) { - nfc_err("no memory for command"); + pr_err("no memory for command\n"); return -ENOMEM; } @@ -718,7 +703,7 @@ static void nci_tx_work(struct work_struct *work) struct nci_dev *ndev = container_of(work, struct nci_dev, tx_work); struct sk_buff *skb; - nfc_dbg("entry, credits_cnt %d", atomic_read(&ndev->credits_cnt)); + pr_debug("credits_cnt %d\n", atomic_read(&ndev->credits_cnt)); /* Send queued tx data */ while (atomic_read(&ndev->credits_cnt)) { @@ -726,12 +711,15 @@ static void nci_tx_work(struct work_struct *work) if (!skb) return; - atomic_dec(&ndev->credits_cnt); + /* Check if data flow control is used */ + if (atomic_read(&ndev->credits_cnt) != + NCI_DATA_FLOW_CONTROL_NOT_USED) + atomic_dec(&ndev->credits_cnt); - nfc_dbg("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d", - nci_pbf(skb->data), - nci_conn_id(skb->data), - nci_plen(skb->data)); + pr_debug("NCI TX: MT=data, PBF=%d, conn_id=%d, plen=%d\n", + nci_pbf(skb->data), + nci_conn_id(skb->data), + nci_plen(skb->data)); nci_send_frame(skb); } @@ -760,7 +748,7 @@ static void nci_rx_work(struct work_struct *work) break; default: - nfc_err("unknown MT 0x%x", nci_mt(skb->data)); + pr_err("unknown MT 0x%x\n", nci_mt(skb->data)); kfree_skb(skb); break; } @@ -774,7 +762,7 @@ 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; - nfc_dbg("entry, cmd_cnt %d", atomic_read(&ndev->cmd_cnt)); + pr_debug("cmd_cnt %d\n", atomic_read(&ndev->cmd_cnt)); /* Send queued command */ if (atomic_read(&ndev->cmd_cnt)) { @@ -784,11 +772,11 @@ static void nci_cmd_work(struct work_struct *work) atomic_dec(&ndev->cmd_cnt); - nfc_dbg("NCI TX: MT=cmd, PBF=%d, GID=0x%x, OID=0x%x, plen=%d", - nci_pbf(skb->data), - nci_opcode_gid(nci_opcode(skb->data)), - nci_opcode_oid(nci_opcode(skb->data)), - nci_plen(skb->data)); + pr_debug("NCI TX: MT=cmd, PBF=%d, GID=0x%x, OID=0x%x, plen=%d\n", + nci_pbf(skb->data), + nci_opcode_gid(nci_opcode(skb->data)), + nci_opcode_oid(nci_opcode(skb->data)), + nci_plen(skb->data)); nci_send_frame(skb); diff --git a/net/nfc/nci/data.c b/net/nfc/nci/data.c index e5ed90f..e5756b3 100644 --- a/net/nfc/nci/data.c +++ b/net/nfc/nci/data.c @@ -21,6 +21,8 @@ * */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <linux/types.h> #include <linux/interrupt.h> #include <linux/wait.h> @@ -40,7 +42,7 @@ void nci_data_exchange_complete(struct nci_dev *ndev, data_exchange_cb_t cb = ndev->data_exchange_cb; void *cb_context = ndev->data_exchange_cb_context; - nfc_dbg("entry, len %d, err %d", ((skb) ? (skb->len) : (0)), err); + pr_debug("len %d, err %d\n", skb ? skb->len : 0, err); if (cb) { ndev->data_exchange_cb = NULL; @@ -49,7 +51,7 @@ void nci_data_exchange_complete(struct nci_dev *ndev, /* forward skb to nfc core */ cb(cb_context, skb, err); } else if (skb) { - nfc_err("no rx callback, dropping rx data..."); + pr_err("no rx callback, dropping rx data...\n"); /* no waiting callback, free skb */ kfree_skb(skb); @@ -90,12 +92,13 @@ static int nci_queue_tx_data_frags(struct nci_dev *ndev, int frag_len; int rc = 0; - nfc_dbg("entry, conn_id 0x%x, total_len %d", conn_id, total_len); + pr_debug("conn_id 0x%x, total_len %d\n", conn_id, total_len); __skb_queue_head_init(&frags_q); while (total_len) { - frag_len = min_t(int, total_len, ndev->max_pkt_payload_size); + frag_len = + min_t(int, total_len, ndev->max_data_pkt_payload_size); skb_frag = nci_skb_alloc(ndev, (NCI_DATA_HDR_SIZE + frag_len), @@ -118,8 +121,8 @@ static int nci_queue_tx_data_frags(struct nci_dev *ndev, data += frag_len; total_len -= frag_len; - nfc_dbg("frag_len %d, remaining total_len %d", - frag_len, total_len); + pr_debug("frag_len %d, remaining total_len %d\n", + frag_len, total_len); } /* queue all fragments atomically */ @@ -148,10 +151,10 @@ int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb) { int rc = 0; - nfc_dbg("entry, conn_id 0x%x, plen %d", conn_id, skb->len); + pr_debug("conn_id 0x%x, plen %d\n", conn_id, skb->len); /* check if the packet need to be fragmented */ - if (skb->len <= ndev->max_pkt_payload_size) { + if (skb->len <= ndev->max_data_pkt_payload_size) { /* no need to fragment packet */ nci_push_data_hdr(ndev, conn_id, skb, NCI_PBF_LAST); @@ -160,7 +163,7 @@ int nci_send_data(struct nci_dev *ndev, __u8 conn_id, struct sk_buff *skb) /* fragment packet and queue the fragments */ rc = nci_queue_tx_data_frags(ndev, conn_id, skb); if (rc) { - nfc_err("failed to fragment tx data packet"); + pr_err("failed to fragment tx data packet\n"); goto free_exit; } } @@ -190,7 +193,7 @@ static void nci_add_rx_data_frag(struct nci_dev *ndev, /* first, make enough room for the already accumulated data */ if (skb_cow_head(skb, reassembly_len)) { - nfc_err("error adding room for accumulated rx data"); + pr_err("error adding room for accumulated rx data\n"); kfree_skb(skb); skb = 0; @@ -227,19 +230,19 @@ void nci_rx_data_packet(struct nci_dev *ndev, struct sk_buff *skb) { __u8 pbf = nci_pbf(skb->data); - nfc_dbg("entry, len %d", skb->len); + pr_debug("len %d\n", skb->len); - nfc_dbg("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d", - nci_pbf(skb->data), - nci_conn_id(skb->data), - nci_plen(skb->data)); + pr_debug("NCI RX: MT=data, PBF=%d, conn_id=%d, plen=%d\n", + nci_pbf(skb->data), + nci_conn_id(skb->data), + nci_plen(skb->data)); /* strip the nci data header */ skb_pull(skb, NCI_DATA_HDR_SIZE); if (ndev->target_active_prot == NFC_PROTO_MIFARE) { /* frame I/F => remove the status byte */ - nfc_dbg("NFC_PROTO_MIFARE => remove the status byte"); + pr_debug("NFC_PROTO_MIFARE => remove the status byte\n"); skb_trim(skb, (skb->len - 1)); } diff --git a/net/nfc/nci/lib.c b/net/nfc/nci/lib.c index b19dc2f..6a63e5e 100644 --- a/net/nfc/nci/lib.c +++ b/net/nfc/nci/lib.c @@ -42,12 +42,9 @@ int nci_to_errno(__u8 code) case NCI_STATUS_REJECTED: return -EBUSY; - case NCI_STATUS_MESSAGE_CORRUPTED: + case NCI_STATUS_RF_FRAME_CORRUPTED: return -EBADMSG; - case NCI_STATUS_BUFFER_FULL: - return -ENOBUFS; - case NCI_STATUS_NOT_INITIALIZED: return -EHOSTDOWN; @@ -80,12 +77,6 @@ int nci_to_errno(__u8 code) case NCI_STATUS_NFCEE_TIMEOUT_ERROR: return -ETIMEDOUT; - case NCI_STATUS_RF_LINK_LOSS_ERROR: - return -ENOLINK; - - case NCI_STATUS_MAX_ACTIVE_NFCEE_INTERFACES_REACHED: - return -EDQUOT; - case NCI_STATUS_FAILED: default: return -ENOSYS; diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c index 96633f5..b16a8dc 100644 --- a/net/nfc/nci/ntf.c +++ b/net/nfc/nci/ntf.c @@ -25,6 +25,8 @@ * */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <linux/types.h> #include <linux/interrupt.h> #include <linux/bitops.h> @@ -43,18 +45,21 @@ static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev, struct nci_core_conn_credit_ntf *ntf = (void *) skb->data; int i; - nfc_dbg("entry, num_entries %d", ntf->num_entries); + 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++) { - nfc_dbg("entry[%d]: conn_id %d, credits %d", i, - ntf->conn_entries[i].conn_id, - ntf->conn_entries[i].credits); + ntf->conn_entries[i].conn_id = + nci_conn_id(&ntf->conn_entries[i].conn_id); + + pr_debug("entry[%d]: conn_id %d, credits %d\n", + i, ntf->conn_entries[i].conn_id, + ntf->conn_entries[i].credits); - if (ntf->conn_entries[i].conn_id == ndev->conn_id) { + if (ntf->conn_entries[i].conn_id == NCI_STATIC_RF_CONN_ID) { /* found static rf connection */ atomic_add(ntf->conn_entries[i].credits, &ndev->credits_cnt); @@ -66,31 +71,34 @@ static void nci_core_conn_credits_ntf_packet(struct nci_dev *ndev, queue_work(ndev->tx_wq, &ndev->tx_work); } -static void nci_rf_field_info_ntf_packet(struct nci_dev *ndev, - struct sk_buff *skb) +static void nci_core_conn_intf_error_ntf_packet(struct nci_dev *ndev, + struct sk_buff *skb) { - struct nci_rf_field_info_ntf *ntf = (void *) skb->data; + struct nci_core_intf_error_ntf *ntf = (void *) skb->data; - nfc_dbg("entry, rf_field_status %d", ntf->rf_field_status); + ntf->conn_id = nci_conn_id(&ntf->conn_id); + + pr_debug("status 0x%x, conn_id %d\n", ntf->status, ntf->conn_id); + + /* complete the data exchange transaction, if exists */ + if (test_bit(NCI_DATA_EXCHANGE, &ndev->flags)) + nci_data_exchange_complete(ndev, NULL, -EIO); } -static int nci_rf_activate_nfca_passive_poll(struct nci_dev *ndev, - struct nci_rf_activate_ntf *ntf, __u8 *data) +static __u8 *nci_extract_rf_params_nfca_passive_poll(struct nci_dev *ndev, + struct nci_rf_intf_activated_ntf *ntf, __u8 *data) { struct rf_tech_specific_params_nfca_poll *nfca_poll; - struct activation_params_nfca_poll_iso_dep *nfca_poll_iso_dep; nfca_poll = &ntf->rf_tech_specific_params.nfca_poll; - nfca_poll_iso_dep = &ntf->activation_params.nfca_poll_iso_dep; nfca_poll->sens_res = __le16_to_cpu(*((__u16 *)data)); data += 2; nfca_poll->nfcid1_len = *data++; - nfc_dbg("sens_res 0x%x, nfcid1_len %d", - nfca_poll->sens_res, - nfca_poll->nfcid1_len); + 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; @@ -100,32 +108,32 @@ static int nci_rf_activate_nfca_passive_poll(struct nci_dev *ndev, if (nfca_poll->sel_res_len != 0) nfca_poll->sel_res = *data++; - ntf->rf_interface_type = *data++; - ntf->activation_params_len = *data++; + pr_debug("sel_res_len %d, sel_res 0x%x\n", + nfca_poll->sel_res_len, + nfca_poll->sel_res); - nfc_dbg("sel_res_len %d, sel_res 0x%x, rf_interface_type %d, activation_params_len %d", - nfca_poll->sel_res_len, - nfca_poll->sel_res, - ntf->rf_interface_type, - ntf->activation_params_len); + return data; +} - switch (ntf->rf_interface_type) { - case NCI_RF_INTERFACE_ISO_DEP: - nfca_poll_iso_dep->rats_res_len = *data++; - if (nfca_poll_iso_dep->rats_res_len > 0) { - memcpy(nfca_poll_iso_dep->rats_res, +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; + + 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 = *data++; + if (nfca_poll->rats_res_len > 0) { + memcpy(nfca_poll->rats_res, data, - nfca_poll_iso_dep->rats_res_len); + nfca_poll->rats_res_len); } break; - case NCI_RF_INTERFACE_FRAME: - /* no activation params */ - break; - default: - nfc_err("unsupported rf_interface_type 0x%x", - ntf->rf_interface_type); + pr_err("unsupported activation_rf_tech_and_mode 0x%x\n", + ntf->activation_rf_tech_and_mode); return -EPROTO; } @@ -133,7 +141,7 @@ static int nci_rf_activate_nfca_passive_poll(struct nci_dev *ndev, } static void nci_target_found(struct nci_dev *ndev, - struct nci_rf_activate_ntf *ntf) + struct nci_rf_intf_activated_ntf *ntf) { struct nfc_target nfc_tgt; @@ -141,66 +149,121 @@ static void nci_target_found(struct nci_dev *ndev, nfc_tgt.supported_protocols = NFC_PROTO_MIFARE_MASK; else if (ntf->rf_protocol == NCI_RF_PROTOCOL_ISO_DEP) /* 4A */ nfc_tgt.supported_protocols = NFC_PROTO_ISO14443_MASK; + else + nfc_tgt.supported_protocols = 0; nfc_tgt.sens_res = ntf->rf_tech_specific_params.nfca_poll.sens_res; nfc_tgt.sel_res = ntf->rf_tech_specific_params.nfca_poll.sel_res; + nfc_tgt.nfcid1_len = ntf->rf_tech_specific_params.nfca_poll.nfcid1_len; + if (nfc_tgt.nfcid1_len > 0) { + memcpy(nfc_tgt.nfcid1, + ntf->rf_tech_specific_params.nfca_poll.nfcid1, + nfc_tgt.nfcid1_len); + } if (!(nfc_tgt.supported_protocols & ndev->poll_prots)) { - nfc_dbg("the target found does not have the desired protocol"); + pr_debug("the target found does not have the desired protocol\n"); return; } - nfc_dbg("new target found, supported_protocols 0x%x", - nfc_tgt.supported_protocols); + pr_debug("new target found, supported_protocols 0x%x\n", + nfc_tgt.supported_protocols); ndev->target_available_prots = nfc_tgt.supported_protocols; + ndev->max_data_pkt_payload_size = ntf->max_data_pkt_payload_size; + ndev->initial_num_credits = ntf->initial_num_credits; + + /* set the available credits to initial value */ + atomic_set(&ndev->credits_cnt, ndev->initial_num_credits); nfc_targets_found(ndev->nfc_dev, &nfc_tgt, 1); } -static void nci_rf_activate_ntf_packet(struct nci_dev *ndev, - struct sk_buff *skb) +static void nci_rf_intf_activated_ntf_packet(struct nci_dev *ndev, + struct sk_buff *skb) { - struct nci_rf_activate_ntf ntf; + struct nci_rf_intf_activated_ntf ntf; __u8 *data = skb->data; - int rc = -1; + int err = 0; clear_bit(NCI_DISCOVERY, &ndev->flags); set_bit(NCI_POLL_ACTIVE, &ndev->flags); - ntf.target_handle = *data++; + ntf.rf_discovery_id = *data++; + ntf.rf_interface = *data++; ntf.rf_protocol = *data++; - ntf.rf_tech_and_mode = *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++; - nfc_dbg("target_handle %d, rf_protocol 0x%x, rf_tech_and_mode 0x%x, rf_tech_specific_params_len %d", - ntf.target_handle, - ntf.rf_protocol, - ntf.rf_tech_and_mode, - ntf.rf_tech_specific_params_len); - - switch (ntf.rf_tech_and_mode) { - case NCI_NFC_A_PASSIVE_POLL_MODE: - rc = nci_rf_activate_nfca_passive_poll(ndev, &ntf, - data); - break; + pr_debug("rf_discovery_id %d\n", ntf.rf_discovery_id); + pr_debug("rf_interface 0x%x\n", ntf.rf_interface); + pr_debug("rf_protocol 0x%x\n", ntf.rf_protocol); + pr_debug("activation_rf_tech_and_mode 0x%x\n", + ntf.activation_rf_tech_and_mode); + pr_debug("max_data_pkt_payload_size 0x%x\n", + ntf.max_data_pkt_payload_size); + pr_debug("initial_num_credits 0x%x\n", ntf.initial_num_credits); + pr_debug("rf_tech_specific_params_len %d\n", + ntf.rf_tech_specific_params_len); + + if (ntf.rf_tech_specific_params_len > 0) { + switch (ntf.activation_rf_tech_and_mode) { + case NCI_NFC_A_PASSIVE_POLL_MODE: + data = nci_extract_rf_params_nfca_passive_poll(ndev, + &ntf, data); + break; + + default: + pr_err("unsupported activation_rf_tech_and_mode 0x%x\n", + ntf.activation_rf_tech_and_mode); + return; + } + } - default: - nfc_err("unsupported rf_tech_and_mode 0x%x", - ntf.rf_tech_and_mode); - return; + 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_FRAME: + /* no activation params */ + break; + + default: + pr_err("unsupported rf_interface 0x%x\n", + ntf.rf_interface); + return; + } } - if (!rc) + if (!err) nci_target_found(ndev, &ntf); } static void nci_rf_deactivate_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb) { - __u8 type = skb->data[0]; + struct nci_rf_deactivate_ntf *ntf = (void *) skb->data; - nfc_dbg("entry, type 0x%x", type); + pr_debug("entry, type 0x%x, reason 0x%x\n", ntf->type, ntf->reason); clear_bit(NCI_POLL_ACTIVE, &ndev->flags); ndev->target_active_prot = 0; @@ -223,11 +286,11 @@ void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb) { __u16 ntf_opcode = nci_opcode(skb->data); - nfc_dbg("NCI RX: MT=ntf, PBF=%d, GID=0x%x, OID=0x%x, plen=%d", - nci_pbf(skb->data), - nci_opcode_gid(ntf_opcode), - nci_opcode_oid(ntf_opcode), - nci_plen(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); @@ -237,12 +300,12 @@ void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb) nci_core_conn_credits_ntf_packet(ndev, skb); break; - case NCI_OP_RF_FIELD_INFO_NTF: - nci_rf_field_info_ntf_packet(ndev, skb); + case NCI_OP_CORE_INTF_ERROR_NTF: + nci_core_conn_intf_error_ntf_packet(ndev, skb); break; - case NCI_OP_RF_ACTIVATE_NTF: - nci_rf_activate_ntf_packet(ndev, skb); + case NCI_OP_RF_INTF_ACTIVATED_NTF: + nci_rf_intf_activated_ntf_packet(ndev, skb); break; case NCI_OP_RF_DEACTIVATE_NTF: @@ -250,7 +313,7 @@ void nci_ntf_packet(struct nci_dev *ndev, struct sk_buff *skb) break; default: - nfc_err("unknown ntf opcode 0x%x", ntf_opcode); + pr_err("unknown ntf opcode 0x%x\n", ntf_opcode); break; } diff --git a/net/nfc/nci/rsp.c b/net/nfc/nci/rsp.c index 0403d4c..2840ae2 100644 --- a/net/nfc/nci/rsp.c +++ b/net/nfc/nci/rsp.c @@ -25,6 +25,8 @@ * */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <linux/types.h> #include <linux/interrupt.h> #include <linux/bitops.h> @@ -40,12 +42,13 @@ static void nci_core_reset_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) { struct nci_core_reset_rsp *rsp = (void *) skb->data; - nfc_dbg("entry, status 0x%x", rsp->status); + pr_debug("status 0x%x\n", rsp->status); - if (rsp->status == NCI_STATUS_OK) + if (rsp->status == NCI_STATUS_OK) { ndev->nci_ver = rsp->nci_ver; - - nfc_dbg("nci_ver 0x%x", ndev->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); } @@ -55,16 +58,16 @@ 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; - nfc_dbg("entry, status 0x%x", rsp_1->status); + pr_debug("status 0x%x\n", rsp_1->status); if (rsp_1->status != NCI_STATUS_OK) - return; + 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) { + NCI_MAX_SUPPORTED_RF_INTERFACES) { ndev->num_supported_rf_interfaces = NCI_MAX_SUPPORTED_RF_INTERFACES; } @@ -73,76 +76,56 @@ static void nci_core_init_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) rsp_1->supported_rf_interfaces, ndev->num_supported_rf_interfaces); - rsp_2 = (void *) (skb->data + 6 + 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_control_packet_payload_length = - rsp_2->max_control_packet_payload_length; - ndev->rf_sending_buffer_size = - __le16_to_cpu(rsp_2->rf_sending_buffer_size); - ndev->rf_receiving_buffer_size = - __le16_to_cpu(rsp_2->rf_receiving_buffer_size); - ndev->manufacturer_id = - __le16_to_cpu(rsp_2->manufacturer_id); - - nfc_dbg("nfcc_features 0x%x", - ndev->nfcc_features); - nfc_dbg("num_supported_rf_interfaces %d", - ndev->num_supported_rf_interfaces); - nfc_dbg("supported_rf_interfaces[0] 0x%x", - ndev->supported_rf_interfaces[0]); - nfc_dbg("supported_rf_interfaces[1] 0x%x", - ndev->supported_rf_interfaces[1]); - nfc_dbg("supported_rf_interfaces[2] 0x%x", - ndev->supported_rf_interfaces[2]); - nfc_dbg("supported_rf_interfaces[3] 0x%x", - ndev->supported_rf_interfaces[3]); - nfc_dbg("max_logical_connections %d", - ndev->max_logical_connections); - nfc_dbg("max_routing_table_size %d", - ndev->max_routing_table_size); - nfc_dbg("max_control_packet_payload_length %d", - ndev->max_control_packet_payload_length); - nfc_dbg("rf_sending_buffer_size %d", - ndev->rf_sending_buffer_size); - nfc_dbg("rf_receiving_buffer_size %d", - ndev->rf_receiving_buffer_size); - nfc_dbg("manufacturer_id 0x%x", - ndev->manufacturer_id); - + 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_conn_create_rsp_packet(struct nci_dev *ndev, - struct sk_buff *skb) -{ - struct nci_core_conn_create_rsp *rsp = (void *) skb->data; - - nfc_dbg("entry, status 0x%x", rsp->status); - - if (rsp->status != NCI_STATUS_OK) - return; - - ndev->max_pkt_payload_size = rsp->max_pkt_payload_size; - ndev->initial_num_credits = rsp->initial_num_credits; - ndev->conn_id = rsp->conn_id; - - atomic_set(&ndev->credits_cnt, ndev->initial_num_credits); - - nfc_dbg("max_pkt_payload_size %d", ndev->max_pkt_payload_size); - nfc_dbg("initial_num_credits %d", ndev->initial_num_credits); - nfc_dbg("conn_id %d", ndev->conn_id); -} - static void nci_rf_disc_map_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) { __u8 status = skb->data[0]; - nfc_dbg("entry, status 0x%x", status); + pr_debug("status 0x%x\n", status); nci_req_complete(ndev, status); } @@ -151,7 +134,7 @@ static void nci_rf_disc_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) { __u8 status = skb->data[0]; - nfc_dbg("entry, status 0x%x", status); + pr_debug("status 0x%x\n", status); if (status == NCI_STATUS_OK) set_bit(NCI_DISCOVERY, &ndev->flags); @@ -164,7 +147,7 @@ static void nci_rf_deactivate_rsp_packet(struct nci_dev *ndev, { __u8 status = skb->data[0]; - nfc_dbg("entry, status 0x%x", status); + pr_debug("status 0x%x\n", status); clear_bit(NCI_DISCOVERY, &ndev->flags); @@ -178,11 +161,11 @@ void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) /* we got a rsp, stop the cmd timer */ del_timer(&ndev->cmd_timer); - nfc_dbg("NCI RX: MT=rsp, PBF=%d, GID=0x%x, OID=0x%x, plen=%d", - nci_pbf(skb->data), - nci_opcode_gid(rsp_opcode), - nci_opcode_oid(rsp_opcode), - nci_plen(skb->data)); + 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); @@ -196,10 +179,6 @@ void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) nci_core_init_rsp_packet(ndev, skb); break; - case NCI_OP_CORE_CONN_CREATE_RSP: - nci_core_conn_create_rsp_packet(ndev, skb); - break; - case NCI_OP_RF_DISCOVER_MAP_RSP: nci_rf_disc_map_rsp_packet(ndev, skb); break; @@ -213,7 +192,7 @@ void nci_rsp_packet(struct nci_dev *ndev, struct sk_buff *skb) break; default: - nfc_err("unknown rsp opcode 0x%x", rsp_opcode); + pr_err("unknown rsp opcode 0x%x\n", rsp_opcode); break; } diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index 03f8818..6989dfa 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -21,6 +21,8 @@ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <net/genetlink.h> #include <linux/nfc.h> #include <linux/slab.h> @@ -44,6 +46,8 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = { [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 }, }; static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, @@ -51,8 +55,6 @@ static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, { void *hdr; - nfc_dbg("entry"); - hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, &nfc_genl_family, flags, NFC_CMD_GET_TARGET); if (!hdr) @@ -65,6 +67,9 @@ static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, 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); + if (target->nfcid1_len > 0) + NLA_PUT(msg, NFC_ATTR_TARGET_NFCID1, target->nfcid1_len, + target->nfcid1); return genlmsg_end(msg, hdr); @@ -105,8 +110,6 @@ static int nfc_genl_dump_targets(struct sk_buff *skb, struct nfc_dev *dev = (struct nfc_dev *) cb->args[1]; int rc; - nfc_dbg("entry"); - if (!dev) { dev = __get_device_from_cb(cb); if (IS_ERR(dev)) @@ -139,8 +142,6 @@ static int nfc_genl_dump_targets_done(struct netlink_callback *cb) { struct nfc_dev *dev = (struct nfc_dev *) cb->args[1]; - nfc_dbg("entry"); - if (dev) nfc_put_device(dev); @@ -152,8 +153,6 @@ int nfc_genl_targets_found(struct nfc_dev *dev) struct sk_buff *msg; void *hdr; - nfc_dbg("entry"); - dev->genl_data.poll_req_pid = 0; msg = nlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); @@ -183,8 +182,6 @@ int nfc_genl_device_added(struct nfc_dev *dev) struct sk_buff *msg; void *hdr; - nfc_dbg("entry"); - msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); if (!msg) return -ENOMEM; @@ -216,8 +213,6 @@ int nfc_genl_device_removed(struct nfc_dev *dev) struct sk_buff *msg; void *hdr; - nfc_dbg("entry"); - msg = nlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL); if (!msg) return -ENOMEM; @@ -249,8 +244,6 @@ static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev, { void *hdr; - nfc_dbg("entry"); - hdr = genlmsg_put(msg, pid, seq, &nfc_genl_family, flags, NFC_CMD_GET_DEVICE); if (!hdr) @@ -277,8 +270,6 @@ static int nfc_genl_dump_devices(struct sk_buff *skb, struct nfc_dev *dev = (struct nfc_dev *) cb->args[1]; bool first_call = false; - nfc_dbg("entry"); - if (!iter) { first_call = true; iter = kmalloc(sizeof(struct class_dev_iter), GFP_KERNEL); @@ -319,14 +310,81 @@ static int nfc_genl_dump_devices_done(struct netlink_callback *cb) { struct class_dev_iter *iter = (struct class_dev_iter *) cb->args[0]; - nfc_dbg("entry"); - 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_GOODSIZE, 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; + + NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx); + if (rf_mode == NFC_RF_INITIATOR) + NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx); + NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, comm_mode); + NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, rf_mode); + + genlmsg_end(msg, hdr); + + dev->dep_link_up = true; + + genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_dep_link_down_event(struct nfc_dev *dev) +{ + struct sk_buff *msg; + void *hdr; + + pr_debug("DEP link is down\n"); + + msg = nlmsg_new(NLMSG_GOODSIZE, 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; + + NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx); + + genlmsg_end(msg, hdr); + + genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info) { struct sk_buff *msg; @@ -334,8 +392,6 @@ static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info) u32 idx; int rc = -ENOBUFS; - nfc_dbg("entry"); - if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) return -EINVAL; @@ -373,8 +429,6 @@ static int nfc_genl_dev_up(struct sk_buff *skb, struct genl_info *info) int rc; u32 idx; - nfc_dbg("entry"); - if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) return -EINVAL; @@ -396,8 +450,6 @@ static int nfc_genl_dev_down(struct sk_buff *skb, struct genl_info *info) int rc; u32 idx; - nfc_dbg("entry"); - if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) return -EINVAL; @@ -420,7 +472,7 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info) u32 idx; u32 protocols; - nfc_dbg("entry"); + pr_debug("Poll start\n"); if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || !info->attrs[NFC_ATTR_PROTOCOLS]) @@ -451,8 +503,6 @@ static int nfc_genl_stop_poll(struct sk_buff *skb, struct genl_info *info) int rc; u32 idx; - nfc_dbg("entry"); - if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) return -EINVAL; @@ -478,6 +528,67 @@ out: return rc; } +static int nfc_genl_dep_link_up(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc, tgt_idx; + u32 idx; + u8 comm, rf; + + pr_debug("DEP link up\n"); + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || + !info->attrs[NFC_ATTR_COMM_MODE] || + !info->attrs[NFC_ATTR_RF_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]); + rf = nla_get_u8(info->attrs[NFC_ATTR_RF_MODE]); + + if (comm != NFC_COMM_ACTIVE && comm != NFC_COMM_PASSIVE) + return -EINVAL; + + if (rf != NFC_RF_INITIATOR && comm != NFC_RF_TARGET) + return -EINVAL; + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + rc = nfc_dep_link_up(dev, tgt_idx, comm, rf); + + 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 struct genl_ops nfc_genl_ops[] = { { .cmd = NFC_CMD_GET_DEVICE, @@ -507,6 +618,16 @@ static struct genl_ops nfc_genl_ops[] = { .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, @@ -524,18 +645,16 @@ static int nfc_genl_rcv_nl_event(struct notifier_block *this, if (event != NETLINK_URELEASE || n->protocol != NETLINK_GENERIC) goto out; - nfc_dbg("NETLINK_URELEASE event from id %d", n->pid); + pr_debug("NETLINK_URELEASE event from id %d\n", n->pid); 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_pid == n->pid) { nfc_stop_poll(dev); dev->genl_data.poll_req_pid = 0; } - mutex_unlock(&dev->genl_data.genl_data_mutex); dev = nfc_device_iter_next(&iter); } diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h index d86583f..6d28d75 100644 --- a/net/nfc/nfc.h +++ b/net/nfc/nfc.h @@ -27,13 +27,6 @@ #include <net/nfc/nfc.h> #include <net/sock.h> -__printf(2, 3) -int nfc_printk(const char *level, const char *fmt, ...); - -#define nfc_info(fmt, arg...) nfc_printk(KERN_INFO, fmt, ##arg) -#define nfc_err(fmt, arg...) nfc_printk(KERN_ERR, fmt, ##arg) -#define nfc_dbg(fmt, arg...) pr_debug(fmt "\n", ##arg) - struct nfc_protocol { int id; struct proto *proto; @@ -53,6 +46,60 @@ struct nfc_rawsock { #define to_rawsock_sk(_tx_work) \ ((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work)) +#ifdef CONFIG_NFC_LLCP + +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, u8 *general_bytes_len); +int __init nfc_llcp_init(void); +void nfc_llcp_exit(void); + +#else + +static inline void nfc_llcp_mac_is_down(struct nfc_dev *dev) +{ +} + +static inline void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode) +{ +} + +static inline int nfc_llcp_register_device(struct nfc_dev *dev) +{ + return 0; +} + +static inline void nfc_llcp_unregister_device(struct nfc_dev *dev) +{ +} + +static inline int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) +{ + return 0; +} + +static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *gb_len) +{ + *gb_len = 0; + return NULL; +} + +static inline int nfc_llcp_init(void) +{ + return 0; +} + +static inline void nfc_llcp_exit(void) +{ +} + +#endif + int __init rawsock_init(void); void rawsock_exit(void); @@ -75,6 +122,10 @@ int nfc_genl_targets_found(struct nfc_dev *dev); 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); + struct nfc_dev *nfc_get_device(unsigned idx); static inline void nfc_put_device(struct nfc_dev *dev) @@ -109,6 +160,11 @@ int nfc_start_poll(struct nfc_dev *dev, u32 protocols); int nfc_stop_poll(struct nfc_dev *dev); +int nfc_dep_link_up(struct nfc_dev *dev, int target_idx, + u8 comm_mode, u8 rf_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); diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c index ee7b2b3..2e2f8c6 100644 --- a/net/nfc/rawsock.c +++ b/net/nfc/rawsock.c @@ -21,6 +21,8 @@ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ +#define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__ + #include <net/tcp_states.h> #include <linux/nfc.h> #include <linux/export.h> @@ -29,7 +31,7 @@ static void rawsock_write_queue_purge(struct sock *sk) { - nfc_dbg("sk=%p", sk); + pr_debug("sk=%p\n", sk); spin_lock_bh(&sk->sk_write_queue.lock); __skb_queue_purge(&sk->sk_write_queue); @@ -39,7 +41,7 @@ static void rawsock_write_queue_purge(struct sock *sk) static void rawsock_report_error(struct sock *sk, int err) { - nfc_dbg("sk=%p err=%d", sk, err); + pr_debug("sk=%p err=%d\n", sk, err); sk->sk_shutdown = SHUTDOWN_MASK; sk->sk_err = -err; @@ -52,7 +54,7 @@ static int rawsock_release(struct socket *sock) { struct sock *sk = sock->sk; - nfc_dbg("sock=%p", sock); + pr_debug("sock=%p\n", sock); sock_orphan(sk); sock_put(sk); @@ -68,14 +70,14 @@ static int rawsock_connect(struct socket *sock, struct sockaddr *_addr, struct nfc_dev *dev; int rc = 0; - nfc_dbg("sock=%p sk=%p flags=%d", sock, sk, flags); + 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; - nfc_dbg("addr dev_idx=%u target_idx=%u protocol=%u", addr->dev_idx, - addr->target_idx, addr->nfc_protocol); + pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", + addr->dev_idx, addr->target_idx, addr->nfc_protocol); lock_sock(sk); @@ -136,7 +138,7 @@ static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb, BUG_ON(in_irq()); - nfc_dbg("sk=%p err=%d", sk, err); + pr_debug("sk=%p err=%d\n", sk, err); if (err) goto error; @@ -172,7 +174,7 @@ static void rawsock_tx_work(struct work_struct *work) struct sk_buff *skb; int rc; - nfc_dbg("sk=%p target_idx=%u", sk, target_idx); + pr_debug("sk=%p target_idx=%u\n", sk, target_idx); if (sk->sk_shutdown & SEND_SHUTDOWN) { rawsock_write_queue_purge(sk); @@ -198,7 +200,7 @@ static int rawsock_sendmsg(struct kiocb *iocb, struct socket *sock, struct sk_buff *skb; int rc; - nfc_dbg("sock=%p sk=%p len=%zu", sock, sk, len); + pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len); if (msg->msg_namelen) return -EOPNOTSUPP; @@ -206,13 +208,10 @@ static int rawsock_sendmsg(struct kiocb *iocb, struct socket *sock, if (sock->state != SS_CONNECTED) return -ENOTCONN; - skb = sock_alloc_send_skb(sk, len + dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE, - msg->msg_flags & MSG_DONTWAIT, &rc); - if (!skb) + skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc); + if (skb == NULL) return rc; - skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); - rc = memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len); if (rc < 0) { kfree_skb(skb); @@ -239,7 +238,7 @@ static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock, int copied; int rc; - nfc_dbg("sock=%p sk=%p len=%zu flags=%d", sock, sk, len, flags); + 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) @@ -283,7 +282,7 @@ static const struct proto_ops rawsock_ops = { static void rawsock_destruct(struct sock *sk) { - nfc_dbg("sk=%p", sk); + pr_debug("sk=%p\n", sk); if (sk->sk_state == TCP_ESTABLISHED) { nfc_deactivate_target(nfc_rawsock(sk)->dev, @@ -294,7 +293,7 @@ static void rawsock_destruct(struct sock *sk) skb_queue_purge(&sk->sk_receive_queue); if (!sock_flag(sk, SOCK_DEAD)) { - nfc_err("Freeing alive NFC raw socket %p", sk); + pr_err("Freeing alive NFC raw socket %p\n", sk); return; } } @@ -304,14 +303,14 @@ static int rawsock_create(struct net *net, struct socket *sock, { struct sock *sk; - nfc_dbg("sock=%p", sock); + pr_debug("sock=%p\n", sock); if (sock->type != SOCK_SEQPACKET) return -ESOCKTNOSUPPORT; sock->ops = &rawsock_ops; - sk = sk_alloc(net, PF_NFC, GFP_KERNEL, nfc_proto->proto); + sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto); if (!sk) return -ENOMEM; |