From 30cc4587659e1c9b1bfade8b2a757d91e04758ab Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Fri, 26 Apr 2013 11:49:40 +0200 Subject: NFC: Move LLCP code to the NFC top level diirectory And stop making it optional. LLCP is a fundamental part of the NFC specifications and making it optional does not make much sense. Signed-off-by: Samuel Ortiz --- net/nfc/Kconfig | 1 - net/nfc/Makefile | 6 +- net/nfc/llcp.h | 267 ++++++++ net/nfc/llcp/Kconfig | 7 - net/nfc/llcp/commands.c | 817 ------------------------ net/nfc/llcp/llcp.c | 1624 ----------------------------------------------- net/nfc/llcp/llcp.h | 267 -------- net/nfc/llcp/sock.c | 1025 ------------------------------ net/nfc/llcp_commands.c | 817 ++++++++++++++++++++++++ net/nfc/llcp_core.c | 1624 +++++++++++++++++++++++++++++++++++++++++++++++ net/nfc/llcp_sock.c | 1025 ++++++++++++++++++++++++++++++ net/nfc/netlink.c | 3 +- net/nfc/nfc.h | 64 -- 13 files changed, 3738 insertions(+), 3809 deletions(-) create mode 100644 net/nfc/llcp.h delete mode 100644 net/nfc/llcp/Kconfig delete mode 100644 net/nfc/llcp/commands.c delete mode 100644 net/nfc/llcp/llcp.c delete mode 100644 net/nfc/llcp/llcp.h delete mode 100644 net/nfc/llcp/sock.c create mode 100644 net/nfc/llcp_commands.c create mode 100644 net/nfc/llcp_core.c create mode 100644 net/nfc/llcp_sock.c diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig index 60c3bbb63e8e..91b70865623f 100644 --- a/net/nfc/Kconfig +++ b/net/nfc/Kconfig @@ -15,6 +15,5 @@ menuconfig NFC source "net/nfc/nci/Kconfig" source "net/nfc/hci/Kconfig" -source "net/nfc/llcp/Kconfig" source "drivers/nfc/Kconfig" diff --git a/net/nfc/Makefile b/net/nfc/Makefile index d1a117c2c401..fb799deaed4f 100644 --- a/net/nfc/Makefile +++ b/net/nfc/Makefile @@ -5,6 +5,8 @@ obj-$(CONFIG_NFC) += nfc.o obj-$(CONFIG_NFC_NCI) += nci/ obj-$(CONFIG_NFC_HCI) += hci/ +#obj-$(CONFIG_NFC_LLCP) += llcp/ + +nfc-objs := core.o netlink.o af_nfc.o rawsock.o llcp_core.o llcp_commands.o \ + llcp_sock.o -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/llcp.h b/net/nfc/llcp.h new file mode 100644 index 000000000000..ff8c434f7df8 --- /dev/null +++ b/net/nfc/llcp.h @@ -0,0 +1,267 @@ +/* + * 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_MAX_LTO 0xff +#define LLCP_MAX_RW 15 +#define LLCP_MAX_MIUX 0x7ff +#define LLCP_MAX_MIU (LLCP_MAX_MIUX + 128) + +#define LLCP_WKS_NUM_SAP 16 +#define LLCP_SDP_NUM_SAP 16 +#define LLCP_LOCAL_NUM_SAP 32 +#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP) +#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP) +#define LLCP_SDP_UNBOUND (LLCP_MAX_SAP + 1) + +struct nfc_llcp_sock; + +struct llcp_sock_list { + struct hlist_head head; + rwlock_t lock; +}; + +struct nfc_llcp_sdp_tlv { + u8 *tlv; + u8 tlv_len; + + char *uri; + u8 tid; + u8 sap; + + unsigned long time; + + struct hlist_node node; +}; + +struct nfc_llcp_local { + struct list_head list; + struct nfc_dev *dev; + + struct kref ref; + + struct mutex sdp_lock; + + struct timer_list link_timer; + struct sk_buff_head tx_queue; + struct work_struct tx_work; + struct work_struct rx_work; + struct sk_buff *rx_pending; + struct work_struct timeout_work; + + u32 target_idx; + u8 rf_mode; + u8 comm_mode; + u8 lto; + u8 rw; + __be16 miux; + unsigned long local_wks; /* Well known services */ + unsigned long local_sdp; /* Local services */ + unsigned long local_sap; /* Local SAPs, not available for discovery */ + atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP]; + + /* local */ + u8 gb[NFC_MAX_GT_LEN]; + u8 gb_len; + + /* remote */ + u8 remote_gb[NFC_MAX_GT_LEN]; + u8 remote_gb_len; + + u8 remote_version; + u16 remote_miu; + u16 remote_lto; + u8 remote_opt; + u16 remote_wks; + + struct mutex sdreq_lock; + struct hlist_head pending_sdreqs; + struct timer_list sdreq_timer; + struct work_struct sdreq_timeout_work; + u8 sdreq_next_tid; + + /* sockets array */ + struct llcp_sock_list sockets; + struct llcp_sock_list connecting_sockets; + struct llcp_sock_list raw_sockets; +}; + +struct nfc_llcp_sock { + struct sock sk; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + u32 target_idx; + u32 nfc_protocol; + + /* Link parameters */ + u8 ssap; + u8 dsap; + char *service_name; + size_t service_name_len; + u8 rw; + __be16 miux; + + + /* Remote link parameters */ + u8 remote_rw; + u16 remote_miu; + + /* Link variables */ + u8 send_n; + u8 send_ack_n; + u8 recv_n; + u8 recv_ack_n; + + /* Is the remote peer ready to receive */ + u8 remote_ready; + + /* Reserved source SAP */ + u8 reserved_ssap; + + struct sk_buff_head tx_queue; + struct sk_buff_head tx_pending_queue; + + struct list_head accept_queue; + struct sock *parent; +}; + +struct nfc_llcp_ui_cb { + __u8 dsap; + __u8 ssap; +}; + +#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0])) + +#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk)) +#define nfc_llcp_dev(sk) (nfc_llcp_sock((sk))->dev) + +#define LLCP_HEADER_SIZE 2 +#define LLCP_SEQUENCE_SIZE 1 +#define LLCP_AGF_PDU_HEADER_SIZE 2 + +/* LLCP versions: 1.1 is 1.0 plus SDP */ +#define LLCP_VERSION_10 0x10 +#define LLCP_VERSION_11 0x11 + +/* LLCP PDU types */ +#define LLCP_PDU_SYMM 0x0 +#define LLCP_PDU_PAX 0x1 +#define LLCP_PDU_AGF 0x2 +#define LLCP_PDU_UI 0x3 +#define LLCP_PDU_CONNECT 0x4 +#define LLCP_PDU_DISC 0x5 +#define LLCP_PDU_CC 0x6 +#define LLCP_PDU_DM 0x7 +#define LLCP_PDU_FRMR 0x8 +#define LLCP_PDU_SNL 0x9 +#define LLCP_PDU_I 0xc +#define LLCP_PDU_RR 0xd +#define LLCP_PDU_RNR 0xe + +/* Parameters TLV types */ +#define LLCP_TLV_VERSION 0x1 +#define LLCP_TLV_MIUX 0x2 +#define LLCP_TLV_WKS 0x3 +#define LLCP_TLV_LTO 0x4 +#define LLCP_TLV_RW 0x5 +#define LLCP_TLV_SN 0x6 +#define LLCP_TLV_OPT 0x7 +#define LLCP_TLV_SDREQ 0x8 +#define LLCP_TLV_SDRES 0x9 +#define LLCP_TLV_MAX 0xa + +/* Well known LLCP SAP */ +#define LLCP_SAP_SDP 0x1 +#define LLCP_SAP_IP 0x2 +#define LLCP_SAP_OBEX 0x3 +#define LLCP_SAP_SNEP 0x4 +#define LLCP_SAP_MAX 0xff + +/* Disconnection reason code */ +#define LLCP_DM_DISC 0x00 +#define LLCP_DM_NOCONN 0x01 +#define LLCP_DM_NOBOUND 0x02 +#define LLCP_DM_REJ 0x03 + + +void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s); +void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s); +void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock); +struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev); +struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local); +int nfc_llcp_local_put(struct nfc_llcp_local *local); +u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, + struct nfc_llcp_sock *sock); +u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local); +void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap); +int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock); +void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local, + struct sk_buff *skb, u8 direction); + +/* Sock API */ +struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp); +void nfc_llcp_sock_free(struct nfc_llcp_sock *sock); +void nfc_llcp_accept_unlink(struct sock *sk); +void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk); +struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock); + +/* TLV API */ +int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, + u8 *tlv_array, u16 tlv_array_len); +int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, + u8 *tlv_array, u16 tlv_array_len); + +/* Commands API */ +void nfc_llcp_recv(void *data, struct sk_buff *skb, int err); +u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length); +struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap); +struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, + size_t uri_len); +void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp); +void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head); +void nfc_llcp_recv(void *data, struct sk_buff *skb, int err); +int nfc_llcp_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_snl_sdres(struct nfc_llcp_local *local, + struct hlist_head *tlv_list, size_t tlvs_len); +int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, + struct hlist_head *tlv_list, size_t tlvs_len); +int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason); +int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock); +int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, + struct msghdr *msg, size_t len); +int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, + struct msghdr *msg, size_t len); +int nfc_llcp_send_rr(struct nfc_llcp_sock *sock); + +/* Socket API */ +int __init nfc_llcp_sock_init(void); +void nfc_llcp_sock_exit(void); diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig deleted file mode 100644 index a1a41cd68255..000000000000 --- a/net/nfc/llcp/Kconfig +++ /dev/null @@ -1,7 +0,0 @@ -config NFC_LLCP - depends on NFC - bool "NFC LLCP support" - 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 deleted file mode 100644 index 094f7e27e910..000000000000 --- a/net/nfc/llcp/commands.c +++ /dev/null @@ -1,817 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include - -#include "../nfc.h" -#include "llcp.h" - -static u8 llcp_tlv_length[LLCP_TLV_MAX] = { - 0, - 1, /* VERSION */ - 2, /* MIUX */ - 2, /* WKS */ - 1, /* LTO */ - 1, /* RW */ - 0, /* SN */ - 1, /* OPT */ - 0, /* SDREQ */ - 2, /* SDRES */ - -}; - -static u8 llcp_tlv8(u8 *tlv, u8 type) -{ - if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) - return 0; - - return tlv[2]; -} - -static u16 llcp_tlv16(u8 *tlv, u8 type) -{ - if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) - return 0; - - return be16_to_cpu(*((__be16 *)(tlv + 2))); -} - - -static u8 llcp_tlv_version(u8 *tlv) -{ - return llcp_tlv8(tlv, LLCP_TLV_VERSION); -} - -static u16 llcp_tlv_miux(u8 *tlv) -{ - return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff; -} - -static u16 llcp_tlv_wks(u8 *tlv) -{ - return llcp_tlv16(tlv, LLCP_TLV_WKS); -} - -static u16 llcp_tlv_lto(u8 *tlv) -{ - return llcp_tlv8(tlv, LLCP_TLV_LTO); -} - -static u8 llcp_tlv_opt(u8 *tlv) -{ - return llcp_tlv8(tlv, LLCP_TLV_OPT); -} - -static u8 llcp_tlv_rw(u8 *tlv) -{ - return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf; -} - -u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) -{ - u8 *tlv, length; - - pr_debug("type %d\n", type); - - if (type >= LLCP_TLV_MAX) - return NULL; - - length = llcp_tlv_length[type]; - if (length == 0 && value_length == 0) - return NULL; - else if (length == 0) - length = value_length; - - *tlv_length = 2 + length; - tlv = kzalloc(2 + length, GFP_KERNEL); - if (tlv == NULL) - return tlv; - - tlv[0] = type; - tlv[1] = length; - memcpy(tlv + 2, value, length); - - return tlv; -} - -struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap) -{ - struct nfc_llcp_sdp_tlv *sdres; - u8 value[2]; - - sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); - if (sdres == NULL) - return NULL; - - value[0] = tid; - value[1] = sap; - - sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2, - &sdres->tlv_len); - if (sdres->tlv == NULL) { - kfree(sdres); - return NULL; - } - - sdres->tid = tid; - sdres->sap = sap; - - INIT_HLIST_NODE(&sdres->node); - - return sdres; -} - -struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, - size_t uri_len) -{ - struct nfc_llcp_sdp_tlv *sdreq; - - pr_debug("uri: %s, len: %zu\n", uri, uri_len); - - sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); - if (sdreq == NULL) - return NULL; - - sdreq->tlv_len = uri_len + 3; - - if (uri[uri_len - 1] == 0) - sdreq->tlv_len--; - - sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL); - if (sdreq->tlv == NULL) { - kfree(sdreq); - return NULL; - } - - sdreq->tlv[0] = LLCP_TLV_SDREQ; - sdreq->tlv[1] = sdreq->tlv_len - 2; - sdreq->tlv[2] = tid; - - sdreq->tid = tid; - sdreq->uri = sdreq->tlv + 3; - memcpy(sdreq->uri, uri, uri_len); - - sdreq->time = jiffies; - - INIT_HLIST_NODE(&sdreq->node); - - return sdreq; -} - -void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) -{ - kfree(sdp->tlv); - kfree(sdp); -} - -void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head) -{ - struct nfc_llcp_sdp_tlv *sdp; - struct hlist_node *n; - - hlist_for_each_entry_safe(sdp, n, head, node) { - hlist_del(&sdp->node); - - nfc_llcp_free_sdp_tlv(sdp); - } -} - -int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, - u8 *tlv_array, u16 tlv_array_len) -{ - u8 *tlv = tlv_array, type, length, offset = 0; - - pr_debug("TLV array length %d\n", tlv_array_len); - - if (local == NULL) - return -ENODEV; - - while (offset < tlv_array_len) { - type = tlv[0]; - length = tlv[1]; - - pr_debug("type 0x%x length %d\n", type, length); - - switch (type) { - case LLCP_TLV_VERSION: - local->remote_version = llcp_tlv_version(tlv); - break; - case LLCP_TLV_MIUX: - local->remote_miu = llcp_tlv_miux(tlv) + 128; - break; - case LLCP_TLV_WKS: - local->remote_wks = llcp_tlv_wks(tlv); - break; - case LLCP_TLV_LTO: - local->remote_lto = llcp_tlv_lto(tlv) * 10; - break; - case LLCP_TLV_OPT: - local->remote_opt = llcp_tlv_opt(tlv); - break; - default: - pr_err("Invalid gt tlv value 0x%x\n", type); - break; - } - - offset += length + 2; - tlv += length + 2; - } - - pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n", - local->remote_version, local->remote_miu, - local->remote_lto, local->remote_opt, - local->remote_wks); - - return 0; -} - -int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, - u8 *tlv_array, u16 tlv_array_len) -{ - u8 *tlv = tlv_array, type, length, offset = 0; - - pr_debug("TLV array length %d\n", tlv_array_len); - - if (sock == NULL) - return -ENOTCONN; - - while (offset < tlv_array_len) { - type = tlv[0]; - length = tlv[1]; - - pr_debug("type 0x%x length %d\n", type, length); - - switch (type) { - case LLCP_TLV_MIUX: - sock->remote_miu = llcp_tlv_miux(tlv) + 128; - break; - case LLCP_TLV_RW: - sock->remote_rw = llcp_tlv_rw(tlv); - break; - case LLCP_TLV_SN: - break; - default: - pr_err("Invalid gt tlv value 0x%x\n", type); - break; - } - - offset += length + 2; - tlv += length + 2; - } - - pr_debug("sock %p rw %d miu %d\n", sock, - sock->remote_rw, sock->remote_miu); - - return 0; -} - -static struct sk_buff *llcp_add_header(struct sk_buff *pdu, - u8 dsap, u8 ssap, u8 ptype) -{ - u8 header[2]; - - pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); - - header[0] = (u8)((dsap << 2) | (ptype >> 2)); - header[1] = (u8)((ptype << 6) | ssap); - - pr_debug("header 0x%x 0x%x\n", header[0], header[1]); - - memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE); - - return pdu; -} - -static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, - u8 tlv_length) -{ - /* XXX Add an skb length check */ - - if (tlv == NULL) - return NULL; - - memcpy(skb_put(pdu, tlv_length), tlv, tlv_length); - - return pdu; -} - -static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, - u8 cmd, u16 size) -{ - struct sk_buff *skb; - int err; - - if (sock->ssap == 0) - return NULL; - - skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, - size + LLCP_HEADER_SIZE, &err); - if (skb == NULL) { - pr_err("Could not allocate PDU\n"); - return NULL; - } - - skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd); - - return skb; -} - -int nfc_llcp_disconnect(struct nfc_llcp_sock *sock) -{ - struct sk_buff *skb; - struct nfc_dev *dev; - struct nfc_llcp_local *local; - - pr_debug("Sending DISC\n"); - - local = sock->local; - if (local == NULL) - return -ENODEV; - - dev = sock->dev; - if (dev == NULL) - return -ENODEV; - - skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0); - if (skb == NULL) - return -ENOMEM; - - skb_queue_tail(&local->tx_queue, skb); - - return 0; -} - -int nfc_llcp_send_symm(struct nfc_dev *dev) -{ - struct sk_buff *skb; - struct nfc_llcp_local *local; - u16 size = 0; - - pr_debug("Sending SYMM\n"); - - local = nfc_llcp_find_local(dev); - if (local == NULL) - return -ENODEV; - - size += LLCP_HEADER_SIZE; - size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; - - skb = alloc_skb(size, GFP_KERNEL); - if (skb == NULL) - return -ENOMEM; - - skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); - - skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); - - __net_timestamp(skb); - - nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX); - - return nfc_data_exchange(dev, local->target_idx, skb, - nfc_llcp_recv, local); -} - -int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) -{ - struct nfc_llcp_local *local; - struct sk_buff *skb; - u8 *service_name_tlv = NULL, service_name_tlv_length; - u8 *miux_tlv = NULL, miux_tlv_length; - u8 *rw_tlv = NULL, rw_tlv_length, rw; - int err; - u16 size = 0, miux; - - pr_debug("Sending CONNECT\n"); - - local = sock->local; - if (local == NULL) - return -ENODEV; - - if (sock->service_name != NULL) { - service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN, - sock->service_name, - sock->service_name_len, - &service_name_tlv_length); - size += service_name_tlv_length; - } - - /* If the socket parameters are not set, use the local ones */ - miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? - local->miux : sock->miux; - rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; - - miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, - &miux_tlv_length); - size += miux_tlv_length; - - rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); - size += rw_tlv_length; - - pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); - - skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); - if (skb == NULL) { - err = -ENOMEM; - goto error_tlv; - } - - if (service_name_tlv != NULL) - skb = llcp_add_tlv(skb, service_name_tlv, - service_name_tlv_length); - - skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); - skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); - - skb_queue_tail(&local->tx_queue, skb); - - return 0; - -error_tlv: - pr_err("error %d\n", err); - - kfree(service_name_tlv); - kfree(miux_tlv); - kfree(rw_tlv); - - return err; -} - -int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) -{ - struct nfc_llcp_local *local; - struct sk_buff *skb; - u8 *miux_tlv = NULL, miux_tlv_length; - u8 *rw_tlv = NULL, rw_tlv_length, rw; - int err; - u16 size = 0, miux; - - pr_debug("Sending CC\n"); - - local = sock->local; - if (local == NULL) - return -ENODEV; - - /* If the socket parameters are not set, use the local ones */ - miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? - local->miux : sock->miux; - rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; - - miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, - &miux_tlv_length); - size += miux_tlv_length; - - rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); - size += rw_tlv_length; - - skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size); - if (skb == NULL) { - err = -ENOMEM; - goto error_tlv; - } - - skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); - skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); - - skb_queue_tail(&local->tx_queue, skb); - - return 0; - -error_tlv: - pr_err("error %d\n", err); - - kfree(miux_tlv); - kfree(rw_tlv); - - return err; -} - -static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local, - size_t tlv_length) -{ - struct sk_buff *skb; - struct nfc_dev *dev; - u16 size = 0; - - if (local == NULL) - return ERR_PTR(-ENODEV); - - dev = local->dev; - if (dev == NULL) - return ERR_PTR(-ENODEV); - - size += LLCP_HEADER_SIZE; - size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; - size += tlv_length; - - skb = alloc_skb(size, GFP_KERNEL); - if (skb == NULL) - return ERR_PTR(-ENOMEM); - - skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); - - skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL); - - return skb; -} - -int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local, - struct hlist_head *tlv_list, size_t tlvs_len) -{ - struct nfc_llcp_sdp_tlv *sdp; - struct hlist_node *n; - struct sk_buff *skb; - - skb = nfc_llcp_allocate_snl(local, tlvs_len); - if (IS_ERR(skb)) - return PTR_ERR(skb); - - hlist_for_each_entry_safe(sdp, n, tlv_list, node) { - memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len); - - hlist_del(&sdp->node); - - nfc_llcp_free_sdp_tlv(sdp); - } - - skb_queue_tail(&local->tx_queue, skb); - - return 0; -} - -int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, - struct hlist_head *tlv_list, size_t tlvs_len) -{ - struct nfc_llcp_sdp_tlv *sdreq; - struct hlist_node *n; - struct sk_buff *skb; - - skb = nfc_llcp_allocate_snl(local, tlvs_len); - if (IS_ERR(skb)) - return PTR_ERR(skb); - - mutex_lock(&local->sdreq_lock); - - if (hlist_empty(&local->pending_sdreqs)) - mod_timer(&local->sdreq_timer, - jiffies + msecs_to_jiffies(3 * local->remote_lto)); - - hlist_for_each_entry_safe(sdreq, n, tlv_list, node) { - pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri); - - memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv, - sdreq->tlv_len); - - hlist_del(&sdreq->node); - - hlist_add_head(&sdreq->node, &local->pending_sdreqs); - } - - mutex_unlock(&local->sdreq_lock); - - skb_queue_tail(&local->tx_queue, skb); - - return 0; -} - -int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) -{ - struct sk_buff *skb; - struct nfc_dev *dev; - u16 size = 1; /* Reason code */ - - pr_debug("Sending DM reason 0x%x\n", reason); - - if (local == NULL) - return -ENODEV; - - dev = local->dev; - if (dev == NULL) - return -ENODEV; - - size += LLCP_HEADER_SIZE; - size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; - - skb = alloc_skb(size, GFP_KERNEL); - if (skb == NULL) - return -ENOMEM; - - skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); - - skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM); - - memcpy(skb_put(skb, 1), &reason, 1); - - skb_queue_head(&local->tx_queue, skb); - - return 0; -} - -int nfc_llcp_send_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; -} - -int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, - struct msghdr *msg, size_t len) -{ - struct sk_buff *pdu; - struct sock *sk = &sock->sk; - struct nfc_llcp_local *local; - size_t frag_len = 0, remaining_len; - u8 *msg_data, *msg_ptr; - u16 remote_miu; - - pr_debug("Send I frame len %zd\n", len); - - local = sock->local; - if (local == NULL) - return -ENODEV; - - /* Remote is ready but has not acknowledged our frames */ - if((sock->remote_ready && - skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw && - skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { - pr_err("Pending queue is full %d frames\n", - skb_queue_len(&sock->tx_pending_queue)); - return -ENOBUFS; - } - - /* Remote is not ready and we've been queueing enough frames */ - if ((!sock->remote_ready && - skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { - pr_err("Tx queue is full %d frames\n", - skb_queue_len(&sock->tx_queue)); - return -ENOBUFS; - } - - msg_data = kzalloc(len, GFP_KERNEL); - if (msg_data == NULL) - return -ENOMEM; - - if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { - kfree(msg_data); - return -EFAULT; - } - - remaining_len = len; - msg_ptr = msg_data; - - do { - remote_miu = sock->remote_miu > LLCP_MAX_MIU ? - local->remote_miu : sock->remote_miu; - - frag_len = min_t(size_t, remote_miu, remaining_len); - - pr_debug("Fragment %zd bytes remaining %zd", - frag_len, remaining_len); - - pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, - frag_len + LLCP_SEQUENCE_SIZE); - if (pdu == NULL) - return -ENOMEM; - - skb_put(pdu, LLCP_SEQUENCE_SIZE); - - if (likely(frag_len > 0)) - memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); - - skb_queue_tail(&sock->tx_queue, pdu); - - lock_sock(sk); - - nfc_llcp_queue_i_frames(sock); - - release_sock(sk); - - remaining_len -= frag_len; - msg_ptr += frag_len; - } while (remaining_len > 0); - - kfree(msg_data); - - return len; -} - -int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, - struct msghdr *msg, size_t len) -{ - struct sk_buff *pdu; - struct nfc_llcp_local *local; - size_t frag_len = 0, remaining_len; - u8 *msg_ptr, *msg_data; - u16 remote_miu; - int err; - - pr_debug("Send UI frame len %zd\n", len); - - local = sock->local; - if (local == NULL) - return -ENODEV; - - msg_data = kzalloc(len, GFP_KERNEL); - if (msg_data == NULL) - return -ENOMEM; - - if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { - kfree(msg_data); - return -EFAULT; - } - - remaining_len = len; - msg_ptr = msg_data; - - do { - remote_miu = sock->remote_miu > LLCP_MAX_MIU ? - local->remote_miu : sock->remote_miu; - - frag_len = min_t(size_t, remote_miu, remaining_len); - - pr_debug("Fragment %zd bytes remaining %zd", - frag_len, remaining_len); - - pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, - frag_len + LLCP_HEADER_SIZE, &err); - if (pdu == NULL) { - pr_err("Could not allocate PDU\n"); - continue; - } - - pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI); - - if (likely(frag_len > 0)) - memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); - - /* No need to check for the peer RW for UI frames */ - skb_queue_tail(&local->tx_queue, pdu); - - remaining_len -= frag_len; - msg_ptr += frag_len; - } while (remaining_len > 0); - - kfree(msg_data); - - return len; -} - -int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) -{ - struct sk_buff *skb; - struct nfc_llcp_local *local; - - pr_debug("Send rr nr %d\n", sock->recv_n); - - local = sock->local; - if (local == NULL) - return -ENODEV; - - skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); - if (skb == NULL) - return -ENOMEM; - - skb_put(skb, LLCP_SEQUENCE_SIZE); - - skb->data[2] = sock->recv_n; - - skb_queue_head(&local->tx_queue, skb); - - return 0; -} diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c deleted file mode 100644 index 9e483c8e52f8..000000000000 --- a/net/nfc/llcp/llcp.c +++ /dev/null @@ -1,1624 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include "../nfc.h" -#include "llcp.h" - -static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; - -static struct list_head llcp_devices; - -static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb); - -void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk) -{ - write_lock(&l->lock); - sk_add_node(sk, &l->head); - write_unlock(&l->lock); -} - -void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk) -{ - write_lock(&l->lock); - sk_del_node_init(sk); - write_unlock(&l->lock); -} - -void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock) -{ - sock->remote_rw = LLCP_DEFAULT_RW; - sock->remote_miu = LLCP_MAX_MIU + 1; -} - -static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock) -{ - struct nfc_llcp_local *local = sock->local; - struct sk_buff *s, *tmp; - - pr_debug("%p\n", &sock->sk); - - skb_queue_purge(&sock->tx_queue); - skb_queue_purge(&sock->tx_pending_queue); - - if (local == NULL) - return; - - /* Search for local pending SKBs that are related to this socket */ - skb_queue_walk_safe(&local->tx_queue, s, tmp) { - if (s->sk != &sock->sk) - continue; - - skb_unlink(s, &local->tx_queue); - kfree_skb(s); - } -} - -static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device, - int err) -{ - struct sock *sk; - struct hlist_node *tmp; - struct nfc_llcp_sock *llcp_sock; - - skb_queue_purge(&local->tx_queue); - - write_lock(&local->sockets.lock); - - sk_for_each_safe(sk, tmp, &local->sockets.head) { - llcp_sock = nfc_llcp_sock(sk); - - bh_lock_sock(sk); - - nfc_llcp_socket_purge(llcp_sock); - - if (sk->sk_state == LLCP_CONNECTED) - nfc_put_device(llcp_sock->dev); - - if (sk->sk_state == LLCP_LISTEN) { - struct nfc_llcp_sock *lsk, *n; - struct sock *accept_sk; - - list_for_each_entry_safe(lsk, n, - &llcp_sock->accept_queue, - accept_queue) { - accept_sk = &lsk->sk; - bh_lock_sock(accept_sk); - - nfc_llcp_accept_unlink(accept_sk); - - if (err) - accept_sk->sk_err = err; - accept_sk->sk_state = LLCP_CLOSED; - accept_sk->sk_state_change(sk); - - bh_unlock_sock(accept_sk); - } - } - - if (err) - sk->sk_err = err; - sk->sk_state = LLCP_CLOSED; - sk->sk_state_change(sk); - - bh_unlock_sock(sk); - - sk_del_node_init(sk); - } - - write_unlock(&local->sockets.lock); - - /* If we still have a device, we keep the RAW sockets alive */ - if (device == true) - return; - - write_lock(&local->raw_sockets.lock); - - sk_for_each_safe(sk, tmp, &local->raw_sockets.head) { - llcp_sock = nfc_llcp_sock(sk); - - bh_lock_sock(sk); - - nfc_llcp_socket_purge(llcp_sock); - - if (err) - sk->sk_err = err; - sk->sk_state = LLCP_CLOSED; - sk->sk_state_change(sk); - - bh_unlock_sock(sk); - - sk_del_node_init(sk); - } - - write_unlock(&local->raw_sockets.lock); -} - -struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) -{ - kref_get(&local->ref); - - return local; -} - -static void local_cleanup(struct nfc_llcp_local *local) -{ - nfc_llcp_socket_release(local, false, ENXIO); - del_timer_sync(&local->link_timer); - skb_queue_purge(&local->tx_queue); - cancel_work_sync(&local->tx_work); - cancel_work_sync(&local->rx_work); - cancel_work_sync(&local->timeout_work); - kfree_skb(local->rx_pending); - del_timer_sync(&local->sdreq_timer); - cancel_work_sync(&local->sdreq_timeout_work); - nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs); -} - -static void local_release(struct kref *ref) -{ - struct nfc_llcp_local *local; - - local = container_of(ref, struct nfc_llcp_local, ref); - - list_del(&local->list); - local_cleanup(local); - kfree(local); -} - -int nfc_llcp_local_put(struct nfc_llcp_local *local) -{ - if (local == NULL) - return 0; - - return kref_put(&local->ref, local_release); -} - -static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, - u8 ssap, u8 dsap) -{ - struct sock *sk; - struct nfc_llcp_sock *llcp_sock, *tmp_sock; - - pr_debug("ssap dsap %d %d\n", ssap, dsap); - - if (ssap == 0 && dsap == 0) - return NULL; - - read_lock(&local->sockets.lock); - - llcp_sock = NULL; - - sk_for_each(sk, &local->sockets.head) { - tmp_sock = nfc_llcp_sock(sk); - - if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) { - llcp_sock = tmp_sock; - break; - } - } - - read_unlock(&local->sockets.lock); - - if (llcp_sock == NULL) - return NULL; - - sock_hold(&llcp_sock->sk); - - return llcp_sock; -} - -static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) -{ - sock_put(&sock->sk); -} - -static void nfc_llcp_timeout_work(struct work_struct *work) -{ - struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, - timeout_work); - - nfc_dep_link_down(local->dev); -} - -static void nfc_llcp_symm_timer(unsigned long data) -{ - struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; - - pr_err("SYMM timeout\n"); - - schedule_work(&local->timeout_work); -} - -static void nfc_llcp_sdreq_timeout_work(struct work_struct *work) -{ - unsigned long time; - HLIST_HEAD(nl_sdres_list); - struct hlist_node *n; - struct nfc_llcp_sdp_tlv *sdp; - struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, - sdreq_timeout_work); - - mutex_lock(&local->sdreq_lock); - - time = jiffies - msecs_to_jiffies(3 * local->remote_lto); - - hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) { - if (time_after(sdp->time, time)) - continue; - - sdp->sap = LLCP_SDP_UNBOUND; - - hlist_del(&sdp->node); - - hlist_add_head(&sdp->node, &nl_sdres_list); - } - - if (!hlist_empty(&local->pending_sdreqs)) - mod_timer(&local->sdreq_timer, - jiffies + msecs_to_jiffies(3 * local->remote_lto)); - - mutex_unlock(&local->sdreq_lock); - - if (!hlist_empty(&nl_sdres_list)) - nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); -} - -static void nfc_llcp_sdreq_timer(unsigned long data) -{ - struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; - - schedule_work(&local->sdreq_timeout_work); -} - -struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) -{ - struct nfc_llcp_local *local, *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; -} - -static -struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local, - u8 *sn, size_t sn_len) -{ - struct sock *sk; - struct nfc_llcp_sock *llcp_sock, *tmp_sock; - - pr_debug("sn %zd %p\n", sn_len, sn); - - if (sn == NULL || sn_len == 0) - return NULL; - - read_lock(&local->sockets.lock); - - llcp_sock = NULL; - - sk_for_each(sk, &local->sockets.head) { - tmp_sock = nfc_llcp_sock(sk); - - pr_debug("llcp sock %p\n", tmp_sock); - - if (tmp_sock->sk.sk_type == SOCK_STREAM && - tmp_sock->sk.sk_state != LLCP_LISTEN) - continue; - - if (tmp_sock->sk.sk_type == SOCK_DGRAM && - tmp_sock->sk.sk_state != LLCP_BOUND) - continue; - - if (tmp_sock->service_name == NULL || - tmp_sock->service_name_len == 0) - continue; - - if (tmp_sock->service_name_len != sn_len) - continue; - - if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) { - llcp_sock = tmp_sock; - break; - } - } - - read_unlock(&local->sockets.lock); - - pr_debug("Found llcp sock %p\n", llcp_sock); - - return llcp_sock; -} - -u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, - struct nfc_llcp_sock *sock) -{ - mutex_lock(&local->sdp_lock); - - if (sock->service_name != NULL && sock->service_name_len > 0) { - int ssap = nfc_llcp_wks_sap(sock->service_name, - sock->service_name_len); - - if (ssap > 0) { - pr_debug("WKS %d\n", ssap); - - /* This is a WKS, let's check if it's free */ - if (local->local_wks & BIT(ssap)) { - mutex_unlock(&local->sdp_lock); - - return LLCP_SAP_MAX; - } - - set_bit(ssap, &local->local_wks); - mutex_unlock(&local->sdp_lock); - - return ssap; - } - - /* - * Check if there already is a non WKS socket bound - * to this service name. - */ - if (nfc_llcp_sock_from_sn(local, sock->service_name, - sock->service_name_len) != NULL) { - mutex_unlock(&local->sdp_lock); - - return LLCP_SAP_MAX; - } - - mutex_unlock(&local->sdp_lock); - - return LLCP_SDP_UNBOUND; - - } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) { - if (!test_bit(sock->ssap, &local->local_wks)) { - set_bit(sock->ssap, &local->local_wks); - mutex_unlock(&local->sdp_lock); - - return sock->ssap; - } - } - - mutex_unlock(&local->sdp_lock); - - return LLCP_SAP_MAX; -} - -u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) -{ - u8 local_ssap; - - mutex_lock(&local->sdp_lock); - - local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); - if (local_ssap == LLCP_LOCAL_NUM_SAP) { - mutex_unlock(&local->sdp_lock); - return LLCP_SAP_MAX; - } - - set_bit(local_ssap, &local->local_sap); - - mutex_unlock(&local->sdp_lock); - - return local_ssap + LLCP_LOCAL_SAP_OFFSET; -} - -void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) -{ - u8 local_ssap; - unsigned long *sdp; - - if (ssap < LLCP_WKS_NUM_SAP) { - local_ssap = ssap; - sdp = &local->local_wks; - } else if (ssap < LLCP_LOCAL_NUM_SAP) { - atomic_t *client_cnt; - - local_ssap = ssap - LLCP_WKS_NUM_SAP; - sdp = &local->local_sdp; - client_cnt = &local->local_sdp_cnt[local_ssap]; - - pr_debug("%d clients\n", atomic_read(client_cnt)); - - mutex_lock(&local->sdp_lock); - - if (atomic_dec_and_test(client_cnt)) { - struct nfc_llcp_sock *l_sock; - - pr_debug("No more clients for SAP %d\n", ssap); - - clear_bit(local_ssap, sdp); - - /* Find the listening sock and set it back to UNBOUND */ - l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP); - if (l_sock) { - l_sock->ssap = LLCP_SDP_UNBOUND; - nfc_llcp_sock_put(l_sock); - } - } - - mutex_unlock(&local->sdp_lock); - - return; - } else if (ssap < LLCP_MAX_SAP) { - local_ssap = ssap - LLCP_LOCAL_NUM_SAP; - sdp = &local->local_sap; - } else { - return; - } - - mutex_lock(&local->sdp_lock); - - clear_bit(local_ssap, sdp); - - mutex_unlock(&local->sdp_lock); -} - -static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local) -{ - u8 ssap; - - mutex_lock(&local->sdp_lock); - - ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); - if (ssap == LLCP_SDP_NUM_SAP) { - mutex_unlock(&local->sdp_lock); - - return LLCP_SAP_MAX; - } - - pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); - - set_bit(ssap, &local->local_sdp); - - mutex_unlock(&local->sdp_lock); - - return LLCP_WKS_NUM_SAP + ssap; -} - -static int nfc_llcp_build_gb(struct nfc_llcp_local *local) -{ - u8 *gb_cur, *version_tlv, version, version_length; - u8 *lto_tlv, lto_length; - u8 *wks_tlv, wks_length; - u8 *miux_tlv, miux_length; - u8 gb_len = 0; - int ret = 0; - - version = LLCP_VERSION_11; - version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, - 1, &version_length); - gb_len += version_length; - - lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, <o_length); - gb_len += lto_length; - - pr_debug("Local wks 0x%lx\n", local->local_wks); - wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, - &wks_length); - gb_len += wks_length; - - miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0, - &miux_length); - gb_len += miux_length; - - gb_len += ARRAY_SIZE(llcp_magic); - - if (gb_len > NFC_MAX_GT_LEN) { - ret = -EINVAL; - goto out; - } - - gb_cur = local->gb; - - memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); - gb_cur += ARRAY_SIZE(llcp_magic); - - memcpy(gb_cur, version_tlv, version_length); - gb_cur += version_length; - - memcpy(gb_cur, lto_tlv, lto_length); - gb_cur += lto_length; - - memcpy(gb_cur, wks_tlv, wks_length); - gb_cur += wks_length; - - memcpy(gb_cur, miux_tlv, miux_length); - gb_cur += miux_length; - - local->gb_len = gb_len; - -out: - kfree(version_tlv); - kfree(lto_tlv); - kfree(wks_tlv); - kfree(miux_tlv); - - return ret; -} - -u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) -{ - struct nfc_llcp_local *local; - - local = nfc_llcp_find_local(dev); - if (local == NULL) { - *general_bytes_len = 0; - return NULL; - } - - nfc_llcp_build_gb(local); - - *general_bytes_len = local->gb_len; - - return local->gb; -} - -int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) -{ - struct nfc_llcp_local *local = nfc_llcp_find_local(dev); - - if (local == NULL) { - pr_err("No LLCP device\n"); - return -ENODEV; - } - if (gb_len < 3) - return -EINVAL; - - memset(local->remote_gb, 0, NFC_MAX_GT_LEN); - memcpy(local->remote_gb, gb, gb_len); - local->remote_gb_len = gb_len; - - if (memcmp(local->remote_gb, llcp_magic, 3)) { - pr_err("MAC does not support LLCP\n"); - return -EINVAL; - } - - return nfc_llcp_parse_gb_tlv(local, - &local->remote_gb[3], - local->remote_gb_len - 3); -} - -static u8 nfc_llcp_dsap(struct sk_buff *pdu) -{ - return (pdu->data[0] & 0xfc) >> 2; -} - -static u8 nfc_llcp_ptype(struct sk_buff *pdu) -{ - return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); -} - -static u8 nfc_llcp_ssap(struct sk_buff *pdu) -{ - return pdu->data[1] & 0x3f; -} - -static u8 nfc_llcp_ns(struct sk_buff *pdu) -{ - return pdu->data[2] >> 4; -} - -static u8 nfc_llcp_nr(struct sk_buff *pdu) -{ - return pdu->data[2] & 0xf; -} - -static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) -{ - pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); - sock->send_n = (sock->send_n + 1) % 16; - sock->recv_ack_n = (sock->recv_n - 1) % 16; -} - -void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local, - struct sk_buff *skb, u8 direction) -{ - struct sk_buff *skb_copy = NULL, *nskb; - struct sock *sk; - u8 *data; - - read_lock(&local->raw_sockets.lock); - - sk_for_each(sk, &local->raw_sockets.head) { - if (sk->sk_state != LLCP_BOUND) - continue; - - if (skb_copy == NULL) { - skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE, - GFP_ATOMIC); - - if (skb_copy == NULL) - continue; - - data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE); - - data[0] = local->dev ? local->dev->idx : 0xFF; - data[1] = direction; - } - - nskb = skb_clone(skb_copy, GFP_ATOMIC); - if (!nskb) - continue; - - if (sock_queue_rcv_skb(sk, nskb)) - kfree_skb(nskb); - } - - read_unlock(&local->raw_sockets.lock); - - kfree_skb(skb_copy); -} - -static void nfc_llcp_tx_work(struct work_struct *work) -{ - struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, - tx_work); - struct sk_buff *skb; - struct sock *sk; - struct nfc_llcp_sock *llcp_sock; - - skb = skb_dequeue(&local->tx_queue); - if (skb != NULL) { - sk = skb->sk; - llcp_sock = nfc_llcp_sock(sk); - - if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) { - nfc_llcp_send_symm(local->dev); - } else { - struct sk_buff *copy_skb = NULL; - u8 ptype = nfc_llcp_ptype(skb); - int ret; - - pr_debug("Sending pending skb\n"); - print_hex_dump(KERN_DEBUG, "LLCP Tx: ", - DUMP_PREFIX_OFFSET, 16, 1, - skb->data, skb->len, true); - - if (ptype == LLCP_PDU_I) - copy_skb = skb_copy(skb, GFP_ATOMIC); - - __net_timestamp(skb); - - nfc_llcp_send_to_raw_sock(local, skb, - NFC_LLCP_DIRECTION_TX); - - ret = nfc_data_exchange(local->dev, local->target_idx, - skb, nfc_llcp_recv, local); - - if (ret) { - kfree_skb(copy_skb); - goto out; - } - - if (ptype == LLCP_PDU_I && copy_skb) - skb_queue_tail(&llcp_sock->tx_pending_queue, - copy_skb); - } - } else { - nfc_llcp_send_symm(local->dev); - } - -out: - mod_timer(&local->link_timer, - jiffies + msecs_to_jiffies(2 * local->remote_lto)); -} - -static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local, - u8 ssap) -{ - struct sock *sk; - struct nfc_llcp_sock *llcp_sock; - - read_lock(&local->connecting_sockets.lock); - - sk_for_each(sk, &local->connecting_sockets.head) { - llcp_sock = nfc_llcp_sock(sk); - - if (llcp_sock->ssap == ssap) { - sock_hold(&llcp_sock->sk); - goto out; - } - } - - llcp_sock = NULL; - -out: - read_unlock(&local->connecting_sockets.lock); - - return llcp_sock; -} - -static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local, - u8 *sn, size_t sn_len) -{ - struct nfc_llcp_sock *llcp_sock; - - llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len); - - if (llcp_sock == NULL) - return NULL; - - sock_hold(&llcp_sock->sk); - - return llcp_sock; -} - -static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) -{ - u8 *tlv = &skb->data[2], type, length; - size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; - - while (offset < tlv_array_len) { - type = tlv[0]; - length = tlv[1]; - - pr_debug("type 0x%x length %d\n", type, length); - - if (type == LLCP_TLV_SN) { - *sn_len = length; - return &tlv[2]; - } - - offset += length + 2; - tlv += length + 2; - } - - return NULL; -} - -static void nfc_llcp_recv_ui(struct nfc_llcp_local *local, - struct sk_buff *skb) -{ - struct nfc_llcp_sock *llcp_sock; - struct nfc_llcp_ui_cb *ui_cb; - u8 dsap, ssap; - - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - - ui_cb = nfc_llcp_ui_skb_cb(skb); - ui_cb->dsap = dsap; - ui_cb->ssap = ssap; - - pr_debug("%d %d\n", dsap, ssap); - - /* We're looking for a bound socket, not a client one */ - llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); - if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM) - return; - - /* There is no sequence with UI frames */ - skb_pull(skb, LLCP_HEADER_SIZE); - if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { - /* - * UI frames will be freed from the socket layer, so we - * need to keep them alive until someone receives them. - */ - skb_get(skb); - } else { - pr_err("Receive queue is full\n"); - } - - nfc_llcp_sock_put(llcp_sock); -} - -static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, - struct sk_buff *skb) -{ - struct sock *new_sk, *parent; - struct nfc_llcp_sock *sock, *new_sock; - u8 dsap, ssap, reason; - - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - - pr_debug("%d %d\n", dsap, ssap); - - if (dsap != LLCP_SAP_SDP) { - sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); - if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) { - reason = LLCP_DM_NOBOUND; - goto fail; - } - } else { - u8 *sn; - size_t sn_len; - - sn = nfc_llcp_connect_sn(skb, &sn_len); - if (sn == NULL) { - reason = LLCP_DM_NOBOUND; - goto fail; - } - - pr_debug("Service name length %zu\n", sn_len); - - sock = nfc_llcp_sock_get_sn(local, sn, sn_len); - if (sock == NULL) { - reason = LLCP_DM_NOBOUND; - goto fail; - } - } - - lock_sock(&sock->sk); - - parent = &sock->sk; - - if (sk_acceptq_is_full(parent)) { - reason = LLCP_DM_REJ; - release_sock(&sock->sk); - sock_put(&sock->sk); - goto fail; - } - - if (sock->ssap == LLCP_SDP_UNBOUND) { - u8 ssap = nfc_llcp_reserve_sdp_ssap(local); - - pr_debug("First client, reserving %d\n", ssap); - - if (ssap == LLCP_SAP_MAX) { - reason = LLCP_DM_REJ; - release_sock(&sock->sk); - sock_put(&sock->sk); - goto fail; - } - - sock->ssap = ssap; - } - - new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC); - if (new_sk == NULL) { - reason = LLCP_DM_REJ; - release_sock(&sock->sk); - sock_put(&sock->sk); - goto fail; - } - - new_sock = nfc_llcp_sock(new_sk); - new_sock->dev = local->dev; - new_sock->local = nfc_llcp_local_get(local); - new_sock->rw = sock->rw; - new_sock->miux = sock->miux; - new_sock->remote_miu = local->remote_miu; - new_sock->nfc_protocol = sock->nfc_protocol; - new_sock->dsap = ssap; - new_sock->target_idx = local->target_idx; - new_sock->parent = parent; - new_sock->ssap = sock->ssap; - if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) { - atomic_t *client_count; - - pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock); - - client_count = - &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP]; - - atomic_inc(client_count); - new_sock->reserved_ssap = sock->ssap; - } - - nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE], - skb->len - LLCP_HEADER_SIZE); - - pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); - - nfc_llcp_sock_link(&local->sockets, new_sk); - - nfc_llcp_accept_enqueue(&sock->sk, new_sk); - - nfc_get_device(local->dev->idx); - - new_sk->sk_state = LLCP_CONNECTED; - - /* Wake the listening processes */ - parent->sk_data_ready(parent, 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); -} - -int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) -{ - int nr_frames = 0; - struct nfc_llcp_local *local = sock->local; - - pr_debug("Remote ready %d tx queue len %d remote rw %d", - sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), - sock->remote_rw); - - /* Try to queue some I frames for transmission */ - while (sock->remote_ready && - skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) { - struct sk_buff *pdu; - - pdu = skb_dequeue(&sock->tx_queue); - if (pdu == NULL) - break; - - /* Update N(S)/N(R) */ - nfc_llcp_set_nrns(sock, pdu); - - skb_queue_tail(&local->tx_queue, pdu); - nr_frames++; - } - - return nr_frames; -} - -static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, - struct sk_buff *skb) -{ - struct nfc_llcp_sock *llcp_sock; - struct sock *sk; - u8 dsap, ssap, ptype, ns, nr; - - ptype = nfc_llcp_ptype(skb); - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - ns = nfc_llcp_ns(skb); - nr = nfc_llcp_nr(skb); - - pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); - - llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); - if (llcp_sock == NULL) { - nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); - return; - } - - sk = &llcp_sock->sk; - lock_sock(sk); - if (sk->sk_state == LLCP_CLOSED) { - release_sock(sk); - nfc_llcp_sock_put(llcp_sock); - } - - /* Pass the payload upstream */ - if (ptype == LLCP_PDU_I) { - pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); - - if (ns == llcp_sock->recv_n) - llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; - else - pr_err("Received out of sequence I PDU\n"); - - skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); - if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { - /* - * I frames will be freed from the socket layer, so we - * need to keep them alive until someone receives them. - */ - skb_get(skb); - } else { - pr_err("Receive queue is full\n"); - } - } - - /* Remove skbs from the pending queue */ - if (llcp_sock->send_ack_n != nr) { - struct sk_buff *s, *tmp; - u8 n; - - llcp_sock->send_ack_n = nr; - - /* Remove and free all skbs until ns == nr */ - skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) { - n = nfc_llcp_ns(s); - - skb_unlink(s, &llcp_sock->tx_pending_queue); - kfree_skb(s); - - if (n == nr) - break; - } - - /* Re-queue the remaining skbs for transmission */ - skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue, - s, tmp) { - skb_unlink(s, &llcp_sock->tx_pending_queue); - skb_queue_head(&local->tx_queue, s); - } - } - - if (ptype == LLCP_PDU_RR) - llcp_sock->remote_ready = true; - else if (ptype == LLCP_PDU_RNR) - llcp_sock->remote_ready = false; - - if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I) - nfc_llcp_send_rr(llcp_sock); - - release_sock(sk); - nfc_llcp_sock_put(llcp_sock); -} - -static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, - struct sk_buff *skb) -{ - struct nfc_llcp_sock *llcp_sock; - struct sock *sk; - u8 dsap, ssap; - - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - - if ((dsap == 0) && (ssap == 0)) { - pr_debug("Connection termination"); - nfc_dep_link_down(local->dev); - return; - } - - llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); - if (llcp_sock == NULL) { - nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); - return; - } - - sk = &llcp_sock->sk; - lock_sock(sk); - - nfc_llcp_socket_purge(llcp_sock); - - if (sk->sk_state == LLCP_CLOSED) { - release_sock(sk); - nfc_llcp_sock_put(llcp_sock); - } - - if (sk->sk_state == LLCP_CONNECTED) { - nfc_put_device(local->dev); - sk->sk_state = LLCP_CLOSED; - sk->sk_state_change(sk); - } - - nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); - - release_sock(sk); - nfc_llcp_sock_put(llcp_sock); -} - -static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) -{ - struct nfc_llcp_sock *llcp_sock; - struct sock *sk; - u8 dsap, ssap; - - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - - llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); - if (llcp_sock == NULL) { - pr_err("Invalid CC\n"); - nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); - - return; - } - - sk = &llcp_sock->sk; - - /* Unlink from connecting and link to the client array */ - nfc_llcp_sock_unlink(&local->connecting_sockets, sk); - nfc_llcp_sock_link(&local->sockets, sk); - llcp_sock->dsap = ssap; - - nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], - skb->len - LLCP_HEADER_SIZE); - - sk->sk_state = LLCP_CONNECTED; - sk->sk_state_change(sk); - - nfc_llcp_sock_put(llcp_sock); -} - -static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb) -{ - struct nfc_llcp_sock *llcp_sock; - struct sock *sk; - u8 dsap, ssap, reason; - - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - reason = skb->data[2]; - - pr_debug("%d %d reason %d\n", ssap, dsap, reason); - - switch (reason) { - case LLCP_DM_NOBOUND: - case LLCP_DM_REJ: - llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); - break; - - default: - llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); - break; - } - - if (llcp_sock == NULL) { - pr_debug("Already closed\n"); - return; - } - - sk = &llcp_sock->sk; - - sk->sk_err = ENXIO; - sk->sk_state = LLCP_CLOSED; - sk->sk_state_change(sk); - - nfc_llcp_sock_put(llcp_sock); -} - -static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, - struct sk_buff *skb) -{ - struct nfc_llcp_sock *llcp_sock; - u8 dsap, ssap, *tlv, type, length, tid, sap; - u16 tlv_len, offset; - char *service_name; - size_t service_name_len; - struct nfc_llcp_sdp_tlv *sdp; - HLIST_HEAD(llc_sdres_list); - size_t sdres_tlvs_len; - HLIST_HEAD(nl_sdres_list); - - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - - pr_debug("%d %d\n", dsap, ssap); - - if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) { - pr_err("Wrong SNL SAP\n"); - return; - } - - tlv = &skb->data[LLCP_HEADER_SIZE]; - tlv_len = skb->len - LLCP_HEADER_SIZE; - offset = 0; - sdres_tlvs_len = 0; - - while (offset < tlv_len) { - type = tlv[0]; - length = tlv[1]; - - switch (type) { - case LLCP_TLV_SDREQ: - tid = tlv[2]; - service_name = (char *) &tlv[3]; - service_name_len = length - 1; - - pr_debug("Looking for %.16s\n", service_name); - - if (service_name_len == strlen("urn:nfc:sn:sdp") && - !strncmp(service_name, "urn:nfc:sn:sdp", - service_name_len)) { - sap = 1; - goto add_snl; - } - - llcp_sock = nfc_llcp_sock_from_sn(local, service_name, - service_name_len); - if (!llcp_sock) { - sap = 0; - goto add_snl; - } - - /* - * We found a socket but its ssap has not been reserved - * yet. We need to assign it for good and send a reply. - * The ssap will be freed when the socket is closed. - */ - if (llcp_sock->ssap == LLCP_SDP_UNBOUND) { - atomic_t *client_count; - - sap = nfc_llcp_reserve_sdp_ssap(local); - - pr_debug("Reserving %d\n", sap); - - if (sap == LLCP_SAP_MAX) { - sap = 0; - goto add_snl; - } - - client_count = - &local->local_sdp_cnt[sap - - LLCP_WKS_NUM_SAP]; - - atomic_inc(client_count); - - llcp_sock->ssap = sap; - llcp_sock->reserved_ssap = sap; - } else { - sap = llcp_sock->ssap; - } - - pr_debug("%p %d\n", llcp_sock, sap); - -add_snl: - sdp = nfc_llcp_build_sdres_tlv(tid, sap); - if (sdp == NULL) - goto exit; - - sdres_tlvs_len += sdp->tlv_len; - hlist_add_head(&sdp->node, &llc_sdres_list); - break; - - case LLCP_TLV_SDRES: - mutex_lock(&local->sdreq_lock); - - pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]); - - hlist_for_each_entry(sdp, &local->pending_sdreqs, node) { - if (sdp->tid != tlv[2]) - continue; - - sdp->sap = tlv[3]; - - pr_debug("Found: uri=%s, sap=%d\n", - sdp->uri, sdp->sap); - - hlist_del(&sdp->node); - - hlist_add_head(&sdp->node, &nl_sdres_list); - - break; - } - - mutex_unlock(&local->sdreq_lock); - break; - - default: - pr_err("Invalid SNL tlv value 0x%x\n", type); - break; - } - - offset += length + 2; - tlv += length + 2; - } - -exit: - if (!hlist_empty(&nl_sdres_list)) - nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); - - if (!hlist_empty(&llc_sdres_list)) - nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); -} - -static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb) -{ - u8 ptype; - u16 pdu_len; - struct sk_buff *new_skb; - - if (skb->len <= LLCP_HEADER_SIZE) { - pr_err("Malformed AGF PDU\n"); - return; - } - - skb_pull(skb, LLCP_HEADER_SIZE); - - while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) { - pdu_len = skb->data[0] << 8 | skb->data[1]; - - skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE); - - if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) { - pr_err("Malformed AGF PDU\n"); - return; - } - - ptype = nfc_llcp_ptype(skb); - - if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) - goto next; - - new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL); - if (new_skb == NULL) { - pr_err("Could not allocate PDU\n"); - return; - } - - memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len); - - nfc_llcp_rx_skb(local, new_skb); - - kfree_skb(new_skb); -next: - skb_pull(skb, pdu_len); - } -} - -static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb) -{ - u8 dsap, ssap, ptype; - - ptype = nfc_llcp_ptype(skb); - dsap = nfc_llcp_dsap(skb); - ssap = nfc_llcp_ssap(skb); - - pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); - - if (ptype != LLCP_PDU_SYMM) - print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, - 16, 1, skb->data, skb->len, true); - - switch (ptype) { - case LLCP_PDU_SYMM: - pr_debug("SYMM\n"); - break; - - case LLCP_PDU_UI: - pr_debug("UI\n"); - nfc_llcp_recv_ui(local, skb); - break; - - case LLCP_PDU_CONNECT: - pr_debug("CONNECT\n"); - nfc_llcp_recv_connect(local, skb); - break; - - case LLCP_PDU_DISC: - pr_debug("DISC\n"); - nfc_llcp_recv_disc(local, skb); - break; - - case LLCP_PDU_CC: - pr_debug("CC\n"); - nfc_llcp_recv_cc(local, skb); - break; - - case LLCP_PDU_DM: - pr_debug("DM\n"); - nfc_llcp_recv_dm(local, skb); - break; - - case LLCP_PDU_SNL: - pr_debug("SNL\n"); - nfc_llcp_recv_snl(local, skb); - break; - - case LLCP_PDU_I: - case LLCP_PDU_RR: - case LLCP_PDU_RNR: - pr_debug("I frame\n"); - nfc_llcp_recv_hdlc(local, skb); - break; - - case LLCP_PDU_AGF: - pr_debug("AGF frame\n"); - nfc_llcp_recv_agf(local, skb); - break; - } -} - -static void nfc_llcp_rx_work(struct work_struct *work) -{ - struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, - rx_work); - struct sk_buff *skb; - - skb = local->rx_pending; - if (skb == NULL) { - pr_debug("No pending SKB\n"); - return; - } - - __net_timestamp(skb); - - nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX); - - nfc_llcp_rx_skb(local, skb); - - schedule_work(&local->tx_work); - kfree_skb(local->rx_pending); - local->rx_pending = NULL; -} - -static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb) -{ - local->rx_pending = skb; - del_timer(&local->link_timer); - schedule_work(&local->rx_work); -} - -void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) -{ - struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; - - pr_debug("Received an LLCP PDU\n"); - if (err < 0) { - pr_err("err %d\n", err); - return; - } - - __nfc_llcp_recv(local, skb); -} - -int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb) -{ - struct nfc_llcp_local *local; - - local = nfc_llcp_find_local(dev); - if (local == NULL) - return -ENODEV; - - __nfc_llcp_recv(local, skb); - - return 0; -} - -void nfc_llcp_mac_is_down(struct nfc_dev *dev) -{ - struct nfc_llcp_local *local; - - local = nfc_llcp_find_local(dev); - if (local == NULL) - return; - - local->remote_miu = LLCP_DEFAULT_MIU; - local->remote_lto = LLCP_DEFAULT_LTO; - - /* Close and purge all existing sockets */ - nfc_llcp_socket_release(local, true, 0); -} - -void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, - u8 comm_mode, u8 rf_mode) -{ - struct nfc_llcp_local *local; - - pr_debug("rf mode %d\n", rf_mode); - - local = nfc_llcp_find_local(dev); - if (local == NULL) - return; - - local->target_idx = target_idx; - local->comm_mode = comm_mode; - local->rf_mode = rf_mode; - - if (rf_mode == NFC_RF_INITIATOR) { - pr_debug("Queueing Tx work\n"); - - schedule_work(&local->tx_work); - } else { - mod_timer(&local->link_timer, - jiffies + msecs_to_jiffies(local->remote_lto)); - } -} - -int nfc_llcp_register_device(struct nfc_dev *ndev) -{ - struct nfc_llcp_local *local; - - local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); - if (local == NULL) - return -ENOMEM; - - local->dev = ndev; - INIT_LIST_HEAD(&local->list); - kref_init(&local->ref); - mutex_init(&local->sdp_lock); - init_timer(&local->link_timer); - local->link_timer.data = (unsigned long) local; - local->link_timer.function = nfc_llcp_symm_timer; - - skb_queue_head_init(&local->tx_queue); - INIT_WORK(&local->tx_work, nfc_llcp_tx_work); - - local->rx_pending = NULL; - INIT_WORK(&local->rx_work, nfc_llcp_rx_work); - - INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); - - rwlock_init(&local->sockets.lock); - rwlock_init(&local->connecting_sockets.lock); - rwlock_init(&local->raw_sockets.lock); - - local->lto = 150; /* 1500 ms */ - local->rw = LLCP_MAX_RW; - local->miux = cpu_to_be16(LLCP_MAX_MIUX); - - nfc_llcp_build_gb(local); - - local->remote_miu = LLCP_DEFAULT_MIU; - local->remote_lto = LLCP_DEFAULT_LTO; - - mutex_init(&local->sdreq_lock); - INIT_HLIST_HEAD(&local->pending_sdreqs); - init_timer(&local->sdreq_timer); - local->sdreq_timer.data = (unsigned long) local; - local->sdreq_timer.function = nfc_llcp_sdreq_timer; - INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work); - - list_add(&local->list, &llcp_devices); - - return 0; -} - -void nfc_llcp_unregister_device(struct nfc_dev *dev) -{ - struct nfc_llcp_local *local = nfc_llcp_find_local(dev); - - if (local == NULL) { - pr_debug("No such device\n"); - return; - } - - local_cleanup(local); - - nfc_llcp_local_put(local); -} - -int __init nfc_llcp_init(void) -{ - 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 deleted file mode 100644 index ff8c434f7df8..000000000000 --- a/net/nfc/llcp/llcp.h +++ /dev/null @@ -1,267 +0,0 @@ -/* - * 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_MAX_LTO 0xff -#define LLCP_MAX_RW 15 -#define LLCP_MAX_MIUX 0x7ff -#define LLCP_MAX_MIU (LLCP_MAX_MIUX + 128) - -#define LLCP_WKS_NUM_SAP 16 -#define LLCP_SDP_NUM_SAP 16 -#define LLCP_LOCAL_NUM_SAP 32 -#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP) -#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP) -#define LLCP_SDP_UNBOUND (LLCP_MAX_SAP + 1) - -struct nfc_llcp_sock; - -struct llcp_sock_list { - struct hlist_head head; - rwlock_t lock; -}; - -struct nfc_llcp_sdp_tlv { - u8 *tlv; - u8 tlv_len; - - char *uri; - u8 tid; - u8 sap; - - unsigned long time; - - struct hlist_node node; -}; - -struct nfc_llcp_local { - struct list_head list; - struct nfc_dev *dev; - - struct kref ref; - - struct mutex sdp_lock; - - struct timer_list link_timer; - struct sk_buff_head tx_queue; - struct work_struct tx_work; - struct work_struct rx_work; - struct sk_buff *rx_pending; - struct work_struct timeout_work; - - u32 target_idx; - u8 rf_mode; - u8 comm_mode; - u8 lto; - u8 rw; - __be16 miux; - unsigned long local_wks; /* Well known services */ - unsigned long local_sdp; /* Local services */ - unsigned long local_sap; /* Local SAPs, not available for discovery */ - atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP]; - - /* local */ - u8 gb[NFC_MAX_GT_LEN]; - u8 gb_len; - - /* remote */ - u8 remote_gb[NFC_MAX_GT_LEN]; - u8 remote_gb_len; - - u8 remote_version; - u16 remote_miu; - u16 remote_lto; - u8 remote_opt; - u16 remote_wks; - - struct mutex sdreq_lock; - struct hlist_head pending_sdreqs; - struct timer_list sdreq_timer; - struct work_struct sdreq_timeout_work; - u8 sdreq_next_tid; - - /* sockets array */ - struct llcp_sock_list sockets; - struct llcp_sock_list connecting_sockets; - struct llcp_sock_list raw_sockets; -}; - -struct nfc_llcp_sock { - struct sock sk; - struct nfc_dev *dev; - struct nfc_llcp_local *local; - u32 target_idx; - u32 nfc_protocol; - - /* Link parameters */ - u8 ssap; - u8 dsap; - char *service_name; - size_t service_name_len; - u8 rw; - __be16 miux; - - - /* Remote link parameters */ - u8 remote_rw; - u16 remote_miu; - - /* Link variables */ - u8 send_n; - u8 send_ack_n; - u8 recv_n; - u8 recv_ack_n; - - /* Is the remote peer ready to receive */ - u8 remote_ready; - - /* Reserved source SAP */ - u8 reserved_ssap; - - struct sk_buff_head tx_queue; - struct sk_buff_head tx_pending_queue; - - struct list_head accept_queue; - struct sock *parent; -}; - -struct nfc_llcp_ui_cb { - __u8 dsap; - __u8 ssap; -}; - -#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0])) - -#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk)) -#define nfc_llcp_dev(sk) (nfc_llcp_sock((sk))->dev) - -#define LLCP_HEADER_SIZE 2 -#define LLCP_SEQUENCE_SIZE 1 -#define LLCP_AGF_PDU_HEADER_SIZE 2 - -/* LLCP versions: 1.1 is 1.0 plus SDP */ -#define LLCP_VERSION_10 0x10 -#define LLCP_VERSION_11 0x11 - -/* LLCP PDU types */ -#define LLCP_PDU_SYMM 0x0 -#define LLCP_PDU_PAX 0x1 -#define LLCP_PDU_AGF 0x2 -#define LLCP_PDU_UI 0x3 -#define LLCP_PDU_CONNECT 0x4 -#define LLCP_PDU_DISC 0x5 -#define LLCP_PDU_CC 0x6 -#define LLCP_PDU_DM 0x7 -#define LLCP_PDU_FRMR 0x8 -#define LLCP_PDU_SNL 0x9 -#define LLCP_PDU_I 0xc -#define LLCP_PDU_RR 0xd -#define LLCP_PDU_RNR 0xe - -/* Parameters TLV types */ -#define LLCP_TLV_VERSION 0x1 -#define LLCP_TLV_MIUX 0x2 -#define LLCP_TLV_WKS 0x3 -#define LLCP_TLV_LTO 0x4 -#define LLCP_TLV_RW 0x5 -#define LLCP_TLV_SN 0x6 -#define LLCP_TLV_OPT 0x7 -#define LLCP_TLV_SDREQ 0x8 -#define LLCP_TLV_SDRES 0x9 -#define LLCP_TLV_MAX 0xa - -/* Well known LLCP SAP */ -#define LLCP_SAP_SDP 0x1 -#define LLCP_SAP_IP 0x2 -#define LLCP_SAP_OBEX 0x3 -#define LLCP_SAP_SNEP 0x4 -#define LLCP_SAP_MAX 0xff - -/* Disconnection reason code */ -#define LLCP_DM_DISC 0x00 -#define LLCP_DM_NOCONN 0x01 -#define LLCP_DM_NOBOUND 0x02 -#define LLCP_DM_REJ 0x03 - - -void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s); -void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s); -void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock); -struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev); -struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local); -int nfc_llcp_local_put(struct nfc_llcp_local *local); -u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, - struct nfc_llcp_sock *sock); -u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local); -void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap); -int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock); -void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local, - struct sk_buff *skb, u8 direction); - -/* Sock API */ -struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp); -void nfc_llcp_sock_free(struct nfc_llcp_sock *sock); -void nfc_llcp_accept_unlink(struct sock *sk); -void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk); -struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock); - -/* TLV API */ -int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, - u8 *tlv_array, u16 tlv_array_len); -int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, - u8 *tlv_array, u16 tlv_array_len); - -/* Commands API */ -void nfc_llcp_recv(void *data, struct sk_buff *skb, int err); -u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length); -struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap); -struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, - size_t uri_len); -void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp); -void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head); -void nfc_llcp_recv(void *data, struct sk_buff *skb, int err); -int nfc_llcp_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_snl_sdres(struct nfc_llcp_local *local, - struct hlist_head *tlv_list, size_t tlvs_len); -int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, - struct hlist_head *tlv_list, size_t tlvs_len); -int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason); -int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock); -int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, - struct msghdr *msg, size_t len); -int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, - struct msghdr *msg, size_t len); -int nfc_llcp_send_rr(struct nfc_llcp_sock *sock); - -/* Socket API */ -int __init nfc_llcp_sock_init(void); -void nfc_llcp_sock_exit(void); diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c deleted file mode 100644 index fd01ac6e0bf4..000000000000 --- a/net/nfc/llcp/sock.c +++ /dev/null @@ -1,1025 +0,0 @@ -/* - * 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 -#include -#include -#include - -#include "../nfc.h" -#include "llcp.h" - -static int sock_wait_state(struct sock *sk, int state, unsigned long timeo) -{ - DECLARE_WAITQUEUE(wait, current); - int err = 0; - - pr_debug("sk %p", sk); - - add_wait_queue(sk_sleep(sk), &wait); - set_current_state(TASK_INTERRUPTIBLE); - - while (sk->sk_state != state) { - if (!timeo) { - err = -EINPROGRESS; - break; - } - - if (signal_pending(current)) { - err = sock_intr_errno(timeo); - break; - } - - release_sock(sk); - timeo = schedule_timeout(timeo); - lock_sock(sk); - set_current_state(TASK_INTERRUPTIBLE); - - err = sock_error(sk); - if (err) - break; - } - - __set_current_state(TASK_RUNNING); - remove_wait_queue(sk_sleep(sk), &wait); - return err; -} - -static struct proto llcp_sock_proto = { - .name = "NFC_LLCP", - .owner = THIS_MODULE, - .obj_size = sizeof(struct nfc_llcp_sock), -}; - -static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - struct nfc_llcp_local *local; - struct nfc_dev *dev; - struct sockaddr_nfc_llcp llcp_addr; - int len, ret = 0; - - if (!addr || addr->sa_family != AF_NFC) - return -EINVAL; - - pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); - - memset(&llcp_addr, 0, sizeof(llcp_addr)); - len = min_t(unsigned int, sizeof(llcp_addr), alen); - memcpy(&llcp_addr, addr, len); - - /* This is going to be a listening socket, dsap must be 0 */ - if (llcp_addr.dsap != 0) - return -EINVAL; - - lock_sock(sk); - - if (sk->sk_state != LLCP_CLOSED) { - ret = -EBADFD; - goto error; - } - - dev = nfc_get_device(llcp_addr.dev_idx); - if (dev == NULL) { - ret = -ENODEV; - goto error; - } - - local = nfc_llcp_find_local(dev); - if (local == NULL) { - ret = -ENODEV; - goto put_dev; - } - - llcp_sock->dev = dev; - llcp_sock->local = nfc_llcp_local_get(local); - llcp_sock->nfc_protocol = llcp_addr.nfc_protocol; - llcp_sock->service_name_len = min_t(unsigned int, - llcp_addr.service_name_len, - NFC_LLCP_MAX_SERVICE_NAME); - llcp_sock->service_name = kmemdup(llcp_addr.service_name, - llcp_sock->service_name_len, - GFP_KERNEL); - - llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock); - if (llcp_sock->ssap == LLCP_SAP_MAX) { - ret = -EADDRINUSE; - goto put_dev; - } - - llcp_sock->reserved_ssap = llcp_sock->ssap; - - nfc_llcp_sock_link(&local->sockets, sk); - - pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap); - - sk->sk_state = LLCP_BOUND; - -put_dev: - nfc_put_device(dev); - -error: - release_sock(sk); - return ret; -} - -static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr, - int alen) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - struct nfc_llcp_local *local; - struct nfc_dev *dev; - struct sockaddr_nfc_llcp llcp_addr; - int len, ret = 0; - - if (!addr || addr->sa_family != AF_NFC) - return -EINVAL; - - pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); - - memset(&llcp_addr, 0, sizeof(llcp_addr)); - len = min_t(unsigned int, sizeof(llcp_addr), alen); - memcpy(&llcp_addr, addr, len); - - lock_sock(sk); - - if (sk->sk_state != LLCP_CLOSED) { - ret = -EBADFD; - goto error; - } - - dev = nfc_get_device(llcp_addr.dev_idx); - if (dev == NULL) { - ret = -ENODEV; - goto error; - } - - local = nfc_llcp_find_local(dev); - if (local == NULL) { - ret = -ENODEV; - goto put_dev; - } - - llcp_sock->dev = dev; - llcp_sock->local = nfc_llcp_local_get(local); - llcp_sock->nfc_protocol = llcp_addr.nfc_protocol; - - nfc_llcp_sock_link(&local->raw_sockets, sk); - - sk->sk_state = LLCP_BOUND; - -put_dev: - nfc_put_device(dev); - -error: - release_sock(sk); - return ret; -} - -static int llcp_sock_listen(struct socket *sock, int backlog) -{ - struct sock *sk = sock->sk; - int ret = 0; - - pr_debug("sk %p backlog %d\n", sk, backlog); - - lock_sock(sk); - - if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) || - sk->sk_state != LLCP_BOUND) { - ret = -EBADFD; - goto error; - } - - sk->sk_max_ack_backlog = backlog; - sk->sk_ack_backlog = 0; - - pr_debug("Socket listening\n"); - sk->sk_state = LLCP_LISTEN; - -error: - release_sock(sk); - - return ret; -} - -static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname, - char __user *optval, unsigned int optlen) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - u32 opt; - int err = 0; - - pr_debug("%p optname %d\n", sk, optname); - - if (level != SOL_NFC) - return -ENOPROTOOPT; - - lock_sock(sk); - - switch (optname) { - case NFC_LLCP_RW: - if (sk->sk_state == LLCP_CONNECTED || - sk->sk_state == LLCP_BOUND || - sk->sk_state == LLCP_LISTEN) { - err = -EINVAL; - break; - } - - if (get_user(opt, (u32 __user *) optval)) { - err = -EFAULT; - break; - } - - if (opt > LLCP_MAX_RW) { - err = -EINVAL; - break; - } - - llcp_sock->rw = (u8) opt; - - break; - - case NFC_LLCP_MIUX: - if (sk->sk_state == LLCP_CONNECTED || - sk->sk_state == LLCP_BOUND || - sk->sk_state == LLCP_LISTEN) { - err = -EINVAL; - break; - } - - if (get_user(opt, (u32 __user *) optval)) { - err = -EFAULT; - break; - } - - if (opt > LLCP_MAX_MIUX) { - err = -EINVAL; - break; - } - - llcp_sock->miux = cpu_to_be16((u16) opt); - - break; - - default: - err = -ENOPROTOOPT; - break; - } - - release_sock(sk); - - pr_debug("%p rw %d miux %d\n", llcp_sock, - llcp_sock->rw, llcp_sock->miux); - - return err; -} - -static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname, - char __user *optval, int __user *optlen) -{ - struct nfc_llcp_local *local; - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - int len, err = 0; - u16 miux, remote_miu; - u8 rw; - - pr_debug("%p optname %d\n", sk, optname); - - if (level != SOL_NFC) - return -ENOPROTOOPT; - - if (get_user(len, optlen)) - return -EFAULT; - - local = llcp_sock->local; - if (!local) - return -ENODEV; - - len = min_t(u32, len, sizeof(u32)); - - lock_sock(sk); - - switch (optname) { - case NFC_LLCP_RW: - rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw; - if (put_user(rw, (u32 __user *) optval)) - err = -EFAULT; - - break; - - case NFC_LLCP_MIUX: - miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ? - be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux); - - if (put_user(miux, (u32 __user *) optval)) - err = -EFAULT; - - break; - - case NFC_LLCP_REMOTE_MIU: - remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ? - local->remote_miu : llcp_sock->remote_miu; - - if (put_user(remote_miu, (u32 __user *) optval)) - err = -EFAULT; - - break; - - case NFC_LLCP_REMOTE_LTO: - if (put_user(local->remote_lto / 10, (u32 __user *) optval)) - err = -EFAULT; - - break; - - case NFC_LLCP_REMOTE_RW: - if (put_user(llcp_sock->remote_rw, (u32 __user *) optval)) - err = -EFAULT; - - break; - - default: - err = -ENOPROTOOPT; - break; - } - - release_sock(sk); - - if (put_user(len, optlen)) - return -EFAULT; - - return err; -} - -void nfc_llcp_accept_unlink(struct sock *sk) -{ - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - - pr_debug("state %d\n", sk->sk_state); - - list_del_init(&llcp_sock->accept_queue); - sk_acceptq_removed(llcp_sock->parent); - llcp_sock->parent = NULL; - - sock_put(sk); -} - -void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk) -{ - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent); - - /* Lock will be free from unlink */ - sock_hold(sk); - - list_add_tail(&llcp_sock->accept_queue, - &llcp_sock_parent->accept_queue); - llcp_sock->parent = parent; - sk_acceptq_added(parent); -} - -struct sock *nfc_llcp_accept_dequeue(struct sock *parent, - struct socket *newsock) -{ - struct nfc_llcp_sock *lsk, *n, *llcp_parent; - struct sock *sk; - - llcp_parent = nfc_llcp_sock(parent); - - list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue, - accept_queue) { - sk = &lsk->sk; - lock_sock(sk); - - if (sk->sk_state == LLCP_CLOSED) { - release_sock(sk); - nfc_llcp_accept_unlink(sk); - continue; - } - - if (sk->sk_state == LLCP_CONNECTED || !newsock) { - list_del_init(&lsk->accept_queue); - sock_put(sk); - - if (newsock) - sock_graft(sk, newsock); - - release_sock(sk); - - pr_debug("Returning sk state %d\n", sk->sk_state); - - sk_acceptq_removed(parent); - - return sk; - } - - release_sock(sk); - } - - return NULL; -} - -static int llcp_sock_accept(struct socket *sock, struct socket *newsock, - int flags) -{ - DECLARE_WAITQUEUE(wait, current); - struct sock *sk = sock->sk, *new_sk; - long timeo; - int ret = 0; - - pr_debug("parent %p\n", sk); - - lock_sock_nested(sk, SINGLE_DEPTH_NESTING); - - if (sk->sk_state != LLCP_LISTEN) { - ret = -EBADFD; - goto error; - } - - timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); - - /* Wait for an incoming connection. */ - add_wait_queue_exclusive(sk_sleep(sk), &wait); - while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) { - set_current_state(TASK_INTERRUPTIBLE); - - if (!timeo) { - ret = -EAGAIN; - break; - } - - if (signal_pending(current)) { - ret = sock_intr_errno(timeo); - break; - } - - release_sock(sk); - timeo = schedule_timeout(timeo); - lock_sock_nested(sk, SINGLE_DEPTH_NESTING); - } - __set_current_state(TASK_RUNNING); - remove_wait_queue(sk_sleep(sk), &wait); - - if (ret) - goto error; - - newsock->state = SS_CONNECTED; - - pr_debug("new socket %p\n", new_sk); - -error: - release_sock(sk); - - return ret; -} - -static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr, - int *len, int peer) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr); - - if (llcp_sock == NULL || llcp_sock->dev == NULL) - return -EBADFD; - - pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx, - llcp_sock->dsap, llcp_sock->ssap); - - uaddr->sa_family = AF_NFC; - - *len = sizeof(struct sockaddr_nfc_llcp); - - llcp_addr->dev_idx = llcp_sock->dev->idx; - llcp_addr->target_idx = llcp_sock->target_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 | POLLRDNORM; - - if (sk->sk_state == LLCP_CLOSED) - mask |= POLLHUP; - - if (sk->sk_shutdown & RCV_SHUTDOWN) - mask |= POLLRDHUP | POLLIN | POLLRDNORM; - - if (sk->sk_shutdown == SHUTDOWN_MASK) - mask |= POLLHUP; - - if (sock_writeable(sk)) - mask |= POLLOUT | POLLWRNORM | POLLWRBAND; - else - set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags); - - pr_debug("mask 0x%x\n", mask); - - return mask; -} - -static int llcp_sock_release(struct socket *sock) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_local *local; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - int err = 0; - - if (!sk) - return 0; - - pr_debug("%p\n", sk); - - local = llcp_sock->local; - if (local == NULL) { - err = -ENODEV; - goto out; - } - - lock_sock(sk); - - /* Send a DISC */ - if (sk->sk_state == LLCP_CONNECTED) - nfc_llcp_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); - } - } - - if (llcp_sock->reserved_ssap < LLCP_SAP_MAX) - nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); - - release_sock(sk); - - if (sock->type == SOCK_RAW) - nfc_llcp_sock_unlink(&local->raw_sockets, sk); - else - nfc_llcp_sock_unlink(&local->sockets, sk); - -out: - sock_orphan(sk); - sock_put(sk); - - return err; -} - -static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr, - int len, int flags) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr; - struct nfc_dev *dev; - struct nfc_llcp_local *local; - int ret = 0; - - pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags); - - if (!addr || len < sizeof(struct sockaddr_nfc) || - addr->sa_family != AF_NFC) - return -EINVAL; - - if (addr->service_name_len == 0 && addr->dsap == 0) - return -EINVAL; - - pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx, - addr->target_idx, addr->nfc_protocol); - - lock_sock(sk); - - if (sk->sk_state == LLCP_CONNECTED) { - ret = -EISCONN; - goto error; - } - - dev = nfc_get_device(addr->dev_idx); - if (dev == NULL) { - ret = -ENODEV; - goto error; - } - - local = nfc_llcp_find_local(dev); - if (local == NULL) { - ret = -ENODEV; - goto put_dev; - } - - device_lock(&dev->dev); - if (dev->dep_link_up == false) { - ret = -ENOLINK; - device_unlock(&dev->dev); - goto put_dev; - } - device_unlock(&dev->dev); - - if (local->rf_mode == NFC_RF_INITIATOR && - addr->target_idx != local->target_idx) { - ret = -ENOLINK; - goto put_dev; - } - - llcp_sock->dev = dev; - llcp_sock->local = nfc_llcp_local_get(local); - llcp_sock->remote_miu = llcp_sock->local->remote_miu; - llcp_sock->ssap = nfc_llcp_get_local_ssap(local); - if (llcp_sock->ssap == LLCP_SAP_MAX) { - ret = -ENOMEM; - goto put_dev; - } - - llcp_sock->reserved_ssap = llcp_sock->ssap; - - if (addr->service_name_len == 0) - llcp_sock->dsap = addr->dsap; - else - llcp_sock->dsap = LLCP_SAP_SDP; - llcp_sock->nfc_protocol = addr->nfc_protocol; - llcp_sock->service_name_len = min_t(unsigned int, - addr->service_name_len, - NFC_LLCP_MAX_SERVICE_NAME); - llcp_sock->service_name = kmemdup(addr->service_name, - llcp_sock->service_name_len, - GFP_KERNEL); - - nfc_llcp_sock_link(&local->connecting_sockets, sk); - - ret = nfc_llcp_send_connect(llcp_sock); - if (ret) - goto sock_unlink; - - ret = sock_wait_state(sk, LLCP_CONNECTED, - sock_sndtimeo(sk, flags & O_NONBLOCK)); - if (ret) - goto sock_unlink; - - release_sock(sk); - - return 0; - -sock_unlink: - nfc_llcp_put_ssap(local, llcp_sock->ssap); - - nfc_llcp_sock_unlink(&local->connecting_sockets, sk); - -put_dev: - nfc_put_device(dev); - -error: - release_sock(sk); - return ret; -} - -static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock, - struct msghdr *msg, size_t len) -{ - struct sock *sk = sock->sk; - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - int ret; - - pr_debug("sock %p sk %p", sock, sk); - - ret = sock_error(sk); - if (ret) - return ret; - - if (msg->msg_flags & MSG_OOB) - return -EOPNOTSUPP; - - lock_sock(sk); - - if (sk->sk_type == SOCK_DGRAM) { - struct sockaddr_nfc_llcp *addr = - (struct sockaddr_nfc_llcp *)msg->msg_name; - - if (msg->msg_namelen < sizeof(*addr)) { - release_sock(sk); - return -EINVAL; - } - - release_sock(sk); - - return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap, - msg, len); - } - - if (sk->sk_state != LLCP_CONNECTED) { - release_sock(sk); - return -ENOTCONN; - } - - release_sock(sk); - - return nfc_llcp_send_i_frame(llcp_sock, msg, len); -} - -static int llcp_sock_recvmsg(struct 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 (skb_copy_datagram_iovec(cskb, 0, msg->msg_iov, copied)) { - if (!(flags & MSG_PEEK)) - skb_queue_head(&sk->sk_receive_queue, skb); - return -EFAULT; - } - - sock_recv_timestamp(msg, sk, skb); - - if (sk->sk_type == SOCK_DGRAM && msg->msg_name) { - struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb); - struct sockaddr_nfc_llcp *sockaddr = - (struct sockaddr_nfc_llcp *) msg->msg_name; - - msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp); - - pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap); - - sockaddr->sa_family = AF_NFC; - sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP; - sockaddr->dsap = ui_cb->dsap; - sockaddr->ssap = ui_cb->ssap; - } - - /* Mark read part of skb as used */ - if (!(flags & MSG_PEEK)) { - - /* SOCK_STREAM: re-queue skb if it contains unreceived data */ - if (sk->sk_type == SOCK_STREAM || - sk->sk_type == SOCK_DGRAM || - sk->sk_type == SOCK_RAW) { - skb_pull(skb, copied); - if (skb->len) { - skb_queue_head(&sk->sk_receive_queue, skb); - goto done; - } - } - - kfree_skb(skb); - } - - /* XXX Queue backlogged skbs */ - -done: - /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */ - if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC)) - copied = rlen; - - return copied; -} - -static const struct proto_ops llcp_sock_ops = { - .family = PF_NFC, - .owner = THIS_MODULE, - .bind = llcp_sock_bind, - .connect = llcp_sock_connect, - .release = llcp_sock_release, - .socketpair = sock_no_socketpair, - .accept = llcp_sock_accept, - .getname = llcp_sock_getname, - .poll = llcp_sock_poll, - .ioctl = sock_no_ioctl, - .listen = llcp_sock_listen, - .shutdown = sock_no_shutdown, - .setsockopt = nfc_llcp_setsockopt, - .getsockopt = nfc_llcp_getsockopt, - .sendmsg = llcp_sock_sendmsg, - .recvmsg = llcp_sock_recvmsg, - .mmap = sock_no_mmap, -}; - -static const struct proto_ops llcp_rawsock_ops = { - .family = PF_NFC, - .owner = THIS_MODULE, - .bind = llcp_raw_sock_bind, - .connect = sock_no_connect, - .release = llcp_sock_release, - .socketpair = sock_no_socketpair, - .accept = sock_no_accept, - .getname = llcp_sock_getname, - .poll = llcp_sock_poll, - .ioctl = sock_no_ioctl, - .listen = sock_no_listen, - .shutdown = sock_no_shutdown, - .setsockopt = sock_no_setsockopt, - .getsockopt = sock_no_getsockopt, - .sendmsg = sock_no_sendmsg, - .recvmsg = llcp_sock_recvmsg, - .mmap = sock_no_mmap, -}; - -static void llcp_sock_destruct(struct sock *sk) -{ - struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); - - pr_debug("%p\n", sk); - - if (sk->sk_state == LLCP_CONNECTED) - nfc_put_device(llcp_sock->dev); - - skb_queue_purge(&sk->sk_receive_queue); - - nfc_llcp_sock_free(llcp_sock); - - if (!sock_flag(sk, SOCK_DEAD)) { - pr_err("Freeing alive NFC LLCP socket %p\n", sk); - return; - } -} - -struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp) -{ - 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->rw = LLCP_MAX_RW + 1; - llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1); - llcp_sock->send_n = llcp_sock->send_ack_n = 0; - llcp_sock->recv_n = llcp_sock->recv_ack_n = 0; - llcp_sock->remote_ready = 1; - llcp_sock->reserved_ssap = LLCP_SAP_MAX; - nfc_llcp_socket_remote_param_init(llcp_sock); - skb_queue_head_init(&llcp_sock->tx_queue); - skb_queue_head_init(&llcp_sock->tx_pending_queue); - INIT_LIST_HEAD(&llcp_sock->accept_queue); - - if (sock != NULL) - sock->state = SS_UNCONNECTED; - - return sk; -} - -void nfc_llcp_sock_free(struct nfc_llcp_sock *sock) -{ - kfree(sock->service_name); - - skb_queue_purge(&sock->tx_queue); - skb_queue_purge(&sock->tx_pending_queue); - - list_del_init(&sock->accept_queue); - - sock->parent = NULL; - - nfc_llcp_local_put(sock->local); -} - -static int llcp_sock_create(struct net *net, struct socket *sock, - const struct nfc_protocol *nfc_proto) -{ - struct sock *sk; - - pr_debug("%p\n", sock); - - if (sock->type != SOCK_STREAM && - sock->type != SOCK_DGRAM && - sock->type != SOCK_RAW) - return -ESOCKTNOSUPPORT; - - if (sock->type == SOCK_RAW) - sock->ops = &llcp_rawsock_ops; - else - sock->ops = &llcp_sock_ops; - - sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC); - 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/llcp_commands.c b/net/nfc/llcp_commands.c new file mode 100644 index 000000000000..c1b23eef83ca --- /dev/null +++ b/net/nfc/llcp_commands.c @@ -0,0 +1,817 @@ +/* + * 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 +#include +#include +#include + +#include + +#include "nfc.h" +#include "llcp.h" + +static u8 llcp_tlv_length[LLCP_TLV_MAX] = { + 0, + 1, /* VERSION */ + 2, /* MIUX */ + 2, /* WKS */ + 1, /* LTO */ + 1, /* RW */ + 0, /* SN */ + 1, /* OPT */ + 0, /* SDREQ */ + 2, /* SDRES */ + +}; + +static u8 llcp_tlv8(u8 *tlv, u8 type) +{ + if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) + return 0; + + return tlv[2]; +} + +static u16 llcp_tlv16(u8 *tlv, u8 type) +{ + if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]]) + return 0; + + return be16_to_cpu(*((__be16 *)(tlv + 2))); +} + + +static u8 llcp_tlv_version(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_VERSION); +} + +static u16 llcp_tlv_miux(u8 *tlv) +{ + return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff; +} + +static u16 llcp_tlv_wks(u8 *tlv) +{ + return llcp_tlv16(tlv, LLCP_TLV_WKS); +} + +static u16 llcp_tlv_lto(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_LTO); +} + +static u8 llcp_tlv_opt(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_OPT); +} + +static u8 llcp_tlv_rw(u8 *tlv) +{ + return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf; +} + +u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) +{ + u8 *tlv, length; + + pr_debug("type %d\n", type); + + if (type >= LLCP_TLV_MAX) + return NULL; + + length = llcp_tlv_length[type]; + if (length == 0 && value_length == 0) + return NULL; + else if (length == 0) + length = value_length; + + *tlv_length = 2 + length; + tlv = kzalloc(2 + length, GFP_KERNEL); + if (tlv == NULL) + return tlv; + + tlv[0] = type; + tlv[1] = length; + memcpy(tlv + 2, value, length); + + return tlv; +} + +struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap) +{ + struct nfc_llcp_sdp_tlv *sdres; + u8 value[2]; + + sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); + if (sdres == NULL) + return NULL; + + value[0] = tid; + value[1] = sap; + + sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2, + &sdres->tlv_len); + if (sdres->tlv == NULL) { + kfree(sdres); + return NULL; + } + + sdres->tid = tid; + sdres->sap = sap; + + INIT_HLIST_NODE(&sdres->node); + + return sdres; +} + +struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, + size_t uri_len) +{ + struct nfc_llcp_sdp_tlv *sdreq; + + pr_debug("uri: %s, len: %zu\n", uri, uri_len); + + sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL); + if (sdreq == NULL) + return NULL; + + sdreq->tlv_len = uri_len + 3; + + if (uri[uri_len - 1] == 0) + sdreq->tlv_len--; + + sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL); + if (sdreq->tlv == NULL) { + kfree(sdreq); + return NULL; + } + + sdreq->tlv[0] = LLCP_TLV_SDREQ; + sdreq->tlv[1] = sdreq->tlv_len - 2; + sdreq->tlv[2] = tid; + + sdreq->tid = tid; + sdreq->uri = sdreq->tlv + 3; + memcpy(sdreq->uri, uri, uri_len); + + sdreq->time = jiffies; + + INIT_HLIST_NODE(&sdreq->node); + + return sdreq; +} + +void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) +{ + kfree(sdp->tlv); + kfree(sdp); +} + +void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head) +{ + struct nfc_llcp_sdp_tlv *sdp; + struct hlist_node *n; + + hlist_for_each_entry_safe(sdp, n, head, node) { + hlist_del(&sdp->node); + + nfc_llcp_free_sdp_tlv(sdp); + } +} + +int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, + u8 *tlv_array, u16 tlv_array_len) +{ + u8 *tlv = tlv_array, type, length, offset = 0; + + pr_debug("TLV array length %d\n", tlv_array_len); + + if (local == NULL) + return -ENODEV; + + while (offset < tlv_array_len) { + type = tlv[0]; + length = tlv[1]; + + pr_debug("type 0x%x length %d\n", type, length); + + switch (type) { + case LLCP_TLV_VERSION: + local->remote_version = llcp_tlv_version(tlv); + break; + case LLCP_TLV_MIUX: + local->remote_miu = llcp_tlv_miux(tlv) + 128; + break; + case LLCP_TLV_WKS: + local->remote_wks = llcp_tlv_wks(tlv); + break; + case LLCP_TLV_LTO: + local->remote_lto = llcp_tlv_lto(tlv) * 10; + break; + case LLCP_TLV_OPT: + local->remote_opt = llcp_tlv_opt(tlv); + break; + default: + pr_err("Invalid gt tlv value 0x%x\n", type); + break; + } + + offset += length + 2; + tlv += length + 2; + } + + pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n", + local->remote_version, local->remote_miu, + local->remote_lto, local->remote_opt, + local->remote_wks); + + return 0; +} + +int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, + u8 *tlv_array, u16 tlv_array_len) +{ + u8 *tlv = tlv_array, type, length, offset = 0; + + pr_debug("TLV array length %d\n", tlv_array_len); + + if (sock == NULL) + return -ENOTCONN; + + while (offset < tlv_array_len) { + type = tlv[0]; + length = tlv[1]; + + pr_debug("type 0x%x length %d\n", type, length); + + switch (type) { + case LLCP_TLV_MIUX: + sock->remote_miu = llcp_tlv_miux(tlv) + 128; + break; + case LLCP_TLV_RW: + sock->remote_rw = llcp_tlv_rw(tlv); + break; + case LLCP_TLV_SN: + break; + default: + pr_err("Invalid gt tlv value 0x%x\n", type); + break; + } + + offset += length + 2; + tlv += length + 2; + } + + pr_debug("sock %p rw %d miu %d\n", sock, + sock->remote_rw, sock->remote_miu); + + return 0; +} + +static struct sk_buff *llcp_add_header(struct sk_buff *pdu, + u8 dsap, u8 ssap, u8 ptype) +{ + u8 header[2]; + + pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); + + header[0] = (u8)((dsap << 2) | (ptype >> 2)); + header[1] = (u8)((ptype << 6) | ssap); + + pr_debug("header 0x%x 0x%x\n", header[0], header[1]); + + memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE); + + return pdu; +} + +static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv, + u8 tlv_length) +{ + /* XXX Add an skb length check */ + + if (tlv == NULL) + return NULL; + + memcpy(skb_put(pdu, tlv_length), tlv, tlv_length); + + return pdu; +} + +static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock, + u8 cmd, u16 size) +{ + struct sk_buff *skb; + int err; + + if (sock->ssap == 0) + return NULL; + + skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, + size + LLCP_HEADER_SIZE, &err); + if (skb == NULL) { + pr_err("Could not allocate PDU\n"); + return NULL; + } + + skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd); + + return skb; +} + +int nfc_llcp_disconnect(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + + pr_debug("Sending DISC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + dev = sock->dev; + if (dev == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0); + if (skb == NULL) + return -ENOMEM; + + skb_queue_tail(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_symm(struct nfc_dev *dev) +{ + struct sk_buff *skb; + struct nfc_llcp_local *local; + u16 size = 0; + + pr_debug("Sending SYMM\n"); + + local = nfc_llcp_find_local(dev); + if (local == NULL) + return -ENODEV; + + size += LLCP_HEADER_SIZE; + size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; + + skb = alloc_skb(size, GFP_KERNEL); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM); + + __net_timestamp(skb); + + nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX); + + return nfc_data_exchange(dev, local->target_idx, skb, + nfc_llcp_recv, local); +} + +int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) +{ + struct nfc_llcp_local *local; + struct sk_buff *skb; + u8 *service_name_tlv = NULL, service_name_tlv_length; + u8 *miux_tlv = NULL, miux_tlv_length; + u8 *rw_tlv = NULL, rw_tlv_length, rw; + int err; + u16 size = 0, miux; + + pr_debug("Sending CONNECT\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + if (sock->service_name != NULL) { + service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN, + sock->service_name, + sock->service_name_len, + &service_name_tlv_length); + size += service_name_tlv_length; + } + + /* If the socket parameters are not set, use the local ones */ + miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? + local->miux : sock->miux; + rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; + + miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, + &miux_tlv_length); + size += miux_tlv_length; + + rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); + size += rw_tlv_length; + + pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len); + + skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size); + if (skb == NULL) { + err = -ENOMEM; + goto error_tlv; + } + + if (service_name_tlv != NULL) + skb = llcp_add_tlv(skb, service_name_tlv, + service_name_tlv_length); + + skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); + skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); + + skb_queue_tail(&local->tx_queue, skb); + + return 0; + +error_tlv: + pr_err("error %d\n", err); + + kfree(service_name_tlv); + kfree(miux_tlv); + kfree(rw_tlv); + + return err; +} + +int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) +{ + struct nfc_llcp_local *local; + struct sk_buff *skb; + u8 *miux_tlv = NULL, miux_tlv_length; + u8 *rw_tlv = NULL, rw_tlv_length, rw; + int err; + u16 size = 0, miux; + + pr_debug("Sending CC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + /* If the socket parameters are not set, use the local ones */ + miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ? + local->miux : sock->miux; + rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw; + + miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0, + &miux_tlv_length); + size += miux_tlv_length; + + rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length); + size += rw_tlv_length; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size); + if (skb == NULL) { + err = -ENOMEM; + goto error_tlv; + } + + skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length); + skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length); + + skb_queue_tail(&local->tx_queue, skb); + + return 0; + +error_tlv: + pr_err("error %d\n", err); + + kfree(miux_tlv); + kfree(rw_tlv); + + return err; +} + +static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local, + size_t tlv_length) +{ + struct sk_buff *skb; + struct nfc_dev *dev; + u16 size = 0; + + if (local == NULL) + return ERR_PTR(-ENODEV); + + dev = local->dev; + if (dev == NULL) + return ERR_PTR(-ENODEV); + + size += LLCP_HEADER_SIZE; + size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; + size += tlv_length; + + skb = alloc_skb(size, GFP_KERNEL); + if (skb == NULL) + return ERR_PTR(-ENOMEM); + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL); + + return skb; +} + +int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local, + struct hlist_head *tlv_list, size_t tlvs_len) +{ + struct nfc_llcp_sdp_tlv *sdp; + struct hlist_node *n; + struct sk_buff *skb; + + skb = nfc_llcp_allocate_snl(local, tlvs_len); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + hlist_for_each_entry_safe(sdp, n, tlv_list, node) { + memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len); + + hlist_del(&sdp->node); + + nfc_llcp_free_sdp_tlv(sdp); + } + + skb_queue_tail(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, + struct hlist_head *tlv_list, size_t tlvs_len) +{ + struct nfc_llcp_sdp_tlv *sdreq; + struct hlist_node *n; + struct sk_buff *skb; + + skb = nfc_llcp_allocate_snl(local, tlvs_len); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + mutex_lock(&local->sdreq_lock); + + if (hlist_empty(&local->pending_sdreqs)) + mod_timer(&local->sdreq_timer, + jiffies + msecs_to_jiffies(3 * local->remote_lto)); + + hlist_for_each_entry_safe(sdreq, n, tlv_list, node) { + pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri); + + memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv, + sdreq->tlv_len); + + hlist_del(&sdreq->node); + + hlist_add_head(&sdreq->node, &local->pending_sdreqs); + } + + mutex_unlock(&local->sdreq_lock); + + skb_queue_tail(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason) +{ + struct sk_buff *skb; + struct nfc_dev *dev; + u16 size = 1; /* Reason code */ + + pr_debug("Sending DM reason 0x%x\n", reason); + + if (local == NULL) + return -ENODEV; + + dev = local->dev; + if (dev == NULL) + return -ENODEV; + + size += LLCP_HEADER_SIZE; + size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; + + skb = alloc_skb(size, GFP_KERNEL); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM); + + memcpy(skb_put(skb, 1), &reason, 1); + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} + +int nfc_llcp_send_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; +} + +int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, + struct msghdr *msg, size_t len) +{ + struct sk_buff *pdu; + struct sock *sk = &sock->sk; + struct nfc_llcp_local *local; + size_t frag_len = 0, remaining_len; + u8 *msg_data, *msg_ptr; + u16 remote_miu; + + pr_debug("Send I frame len %zd\n", len); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + /* Remote is ready but has not acknowledged our frames */ + if((sock->remote_ready && + skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw && + skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { + pr_err("Pending queue is full %d frames\n", + skb_queue_len(&sock->tx_pending_queue)); + return -ENOBUFS; + } + + /* Remote is not ready and we've been queueing enough frames */ + if ((!sock->remote_ready && + skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) { + pr_err("Tx queue is full %d frames\n", + skb_queue_len(&sock->tx_queue)); + return -ENOBUFS; + } + + msg_data = kzalloc(len, GFP_KERNEL); + if (msg_data == NULL) + return -ENOMEM; + + if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { + kfree(msg_data); + return -EFAULT; + } + + remaining_len = len; + msg_ptr = msg_data; + + do { + remote_miu = sock->remote_miu > LLCP_MAX_MIU ? + local->remote_miu : sock->remote_miu; + + frag_len = min_t(size_t, remote_miu, remaining_len); + + pr_debug("Fragment %zd bytes remaining %zd", + frag_len, remaining_len); + + pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, + frag_len + LLCP_SEQUENCE_SIZE); + if (pdu == NULL) + return -ENOMEM; + + skb_put(pdu, LLCP_SEQUENCE_SIZE); + + if (likely(frag_len > 0)) + memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); + + skb_queue_tail(&sock->tx_queue, pdu); + + lock_sock(sk); + + nfc_llcp_queue_i_frames(sock); + + release_sock(sk); + + remaining_len -= frag_len; + msg_ptr += frag_len; + } while (remaining_len > 0); + + kfree(msg_data); + + return len; +} + +int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, + struct msghdr *msg, size_t len) +{ + struct sk_buff *pdu; + struct nfc_llcp_local *local; + size_t frag_len = 0, remaining_len; + u8 *msg_ptr, *msg_data; + u16 remote_miu; + int err; + + pr_debug("Send UI frame len %zd\n", len); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + msg_data = kzalloc(len, GFP_KERNEL); + if (msg_data == NULL) + return -ENOMEM; + + if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) { + kfree(msg_data); + return -EFAULT; + } + + remaining_len = len; + msg_ptr = msg_data; + + do { + remote_miu = sock->remote_miu > LLCP_MAX_MIU ? + local->remote_miu : sock->remote_miu; + + frag_len = min_t(size_t, remote_miu, remaining_len); + + pr_debug("Fragment %zd bytes remaining %zd", + frag_len, remaining_len); + + pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT, + frag_len + LLCP_HEADER_SIZE, &err); + if (pdu == NULL) { + pr_err("Could not allocate PDU\n"); + continue; + } + + pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI); + + if (likely(frag_len > 0)) + memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len); + + /* No need to check for the peer RW for UI frames */ + skb_queue_tail(&local->tx_queue, pdu); + + remaining_len -= frag_len; + msg_ptr += frag_len; + } while (remaining_len > 0); + + kfree(msg_data); + + return len; +} + +int nfc_llcp_send_rr(struct nfc_llcp_sock *sock) +{ + struct sk_buff *skb; + struct nfc_llcp_local *local; + + pr_debug("Send rr nr %d\n", sock->recv_n); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE); + if (skb == NULL) + return -ENOMEM; + + skb_put(skb, LLCP_SEQUENCE_SIZE); + + skb->data[2] = sock->recv_n; + + skb_queue_head(&local->tx_queue, skb); + + return 0; +} diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c new file mode 100644 index 000000000000..158bdbf668cc --- /dev/null +++ b/net/nfc/llcp_core.c @@ -0,0 +1,1624 @@ +/* + * 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 +#include +#include +#include + +#include "nfc.h" +#include "llcp.h" + +static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; + +static struct list_head llcp_devices; + +static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb); + +void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_add_node(sk, &l->head); + write_unlock(&l->lock); +} + +void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_del_node_init(sk); + write_unlock(&l->lock); +} + +void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock) +{ + sock->remote_rw = LLCP_DEFAULT_RW; + sock->remote_miu = LLCP_MAX_MIU + 1; +} + +static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock) +{ + struct nfc_llcp_local *local = sock->local; + struct sk_buff *s, *tmp; + + pr_debug("%p\n", &sock->sk); + + skb_queue_purge(&sock->tx_queue); + skb_queue_purge(&sock->tx_pending_queue); + + if (local == NULL) + return; + + /* Search for local pending SKBs that are related to this socket */ + skb_queue_walk_safe(&local->tx_queue, s, tmp) { + if (s->sk != &sock->sk) + continue; + + skb_unlink(s, &local->tx_queue); + kfree_skb(s); + } +} + +static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device, + int err) +{ + struct sock *sk; + struct hlist_node *tmp; + struct nfc_llcp_sock *llcp_sock; + + skb_queue_purge(&local->tx_queue); + + write_lock(&local->sockets.lock); + + sk_for_each_safe(sk, tmp, &local->sockets.head) { + llcp_sock = nfc_llcp_sock(sk); + + bh_lock_sock(sk); + + nfc_llcp_socket_purge(llcp_sock); + + if (sk->sk_state == LLCP_CONNECTED) + nfc_put_device(llcp_sock->dev); + + if (sk->sk_state == LLCP_LISTEN) { + struct nfc_llcp_sock *lsk, *n; + struct sock *accept_sk; + + list_for_each_entry_safe(lsk, n, + &llcp_sock->accept_queue, + accept_queue) { + accept_sk = &lsk->sk; + bh_lock_sock(accept_sk); + + nfc_llcp_accept_unlink(accept_sk); + + if (err) + accept_sk->sk_err = err; + accept_sk->sk_state = LLCP_CLOSED; + accept_sk->sk_state_change(sk); + + bh_unlock_sock(accept_sk); + } + } + + if (err) + sk->sk_err = err; + sk->sk_state = LLCP_CLOSED; + sk->sk_state_change(sk); + + bh_unlock_sock(sk); + + sk_del_node_init(sk); + } + + write_unlock(&local->sockets.lock); + + /* If we still have a device, we keep the RAW sockets alive */ + if (device == true) + return; + + write_lock(&local->raw_sockets.lock); + + sk_for_each_safe(sk, tmp, &local->raw_sockets.head) { + llcp_sock = nfc_llcp_sock(sk); + + bh_lock_sock(sk); + + nfc_llcp_socket_purge(llcp_sock); + + if (err) + sk->sk_err = err; + sk->sk_state = LLCP_CLOSED; + sk->sk_state_change(sk); + + bh_unlock_sock(sk); + + sk_del_node_init(sk); + } + + write_unlock(&local->raw_sockets.lock); +} + +struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local) +{ + kref_get(&local->ref); + + return local; +} + +static void local_cleanup(struct nfc_llcp_local *local) +{ + nfc_llcp_socket_release(local, false, ENXIO); + del_timer_sync(&local->link_timer); + skb_queue_purge(&local->tx_queue); + cancel_work_sync(&local->tx_work); + cancel_work_sync(&local->rx_work); + cancel_work_sync(&local->timeout_work); + kfree_skb(local->rx_pending); + del_timer_sync(&local->sdreq_timer); + cancel_work_sync(&local->sdreq_timeout_work); + nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs); +} + +static void local_release(struct kref *ref) +{ + struct nfc_llcp_local *local; + + local = container_of(ref, struct nfc_llcp_local, ref); + + list_del(&local->list); + local_cleanup(local); + kfree(local); +} + +int nfc_llcp_local_put(struct nfc_llcp_local *local) +{ + if (local == NULL) + return 0; + + return kref_put(&local->ref, local_release); +} + +static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, + u8 ssap, u8 dsap) +{ + struct sock *sk; + struct nfc_llcp_sock *llcp_sock, *tmp_sock; + + pr_debug("ssap dsap %d %d\n", ssap, dsap); + + if (ssap == 0 && dsap == 0) + return NULL; + + read_lock(&local->sockets.lock); + + llcp_sock = NULL; + + sk_for_each(sk, &local->sockets.head) { + tmp_sock = nfc_llcp_sock(sk); + + if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) { + llcp_sock = tmp_sock; + break; + } + } + + read_unlock(&local->sockets.lock); + + if (llcp_sock == NULL) + return NULL; + + sock_hold(&llcp_sock->sk); + + return llcp_sock; +} + +static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) +{ + sock_put(&sock->sk); +} + +static void nfc_llcp_timeout_work(struct work_struct *work) +{ + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + timeout_work); + + nfc_dep_link_down(local->dev); +} + +static void nfc_llcp_symm_timer(unsigned long data) +{ + struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; + + pr_err("SYMM timeout\n"); + + schedule_work(&local->timeout_work); +} + +static void nfc_llcp_sdreq_timeout_work(struct work_struct *work) +{ + unsigned long time; + HLIST_HEAD(nl_sdres_list); + struct hlist_node *n; + struct nfc_llcp_sdp_tlv *sdp; + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + sdreq_timeout_work); + + mutex_lock(&local->sdreq_lock); + + time = jiffies - msecs_to_jiffies(3 * local->remote_lto); + + hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) { + if (time_after(sdp->time, time)) + continue; + + sdp->sap = LLCP_SDP_UNBOUND; + + hlist_del(&sdp->node); + + hlist_add_head(&sdp->node, &nl_sdres_list); + } + + if (!hlist_empty(&local->pending_sdreqs)) + mod_timer(&local->sdreq_timer, + jiffies + msecs_to_jiffies(3 * local->remote_lto)); + + mutex_unlock(&local->sdreq_lock); + + if (!hlist_empty(&nl_sdres_list)) + nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); +} + +static void nfc_llcp_sdreq_timer(unsigned long data) +{ + struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; + + schedule_work(&local->sdreq_timeout_work); +} + +struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) +{ + struct nfc_llcp_local *local, *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; +} + +static +struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local, + u8 *sn, size_t sn_len) +{ + struct sock *sk; + struct nfc_llcp_sock *llcp_sock, *tmp_sock; + + pr_debug("sn %zd %p\n", sn_len, sn); + + if (sn == NULL || sn_len == 0) + return NULL; + + read_lock(&local->sockets.lock); + + llcp_sock = NULL; + + sk_for_each(sk, &local->sockets.head) { + tmp_sock = nfc_llcp_sock(sk); + + pr_debug("llcp sock %p\n", tmp_sock); + + if (tmp_sock->sk.sk_type == SOCK_STREAM && + tmp_sock->sk.sk_state != LLCP_LISTEN) + continue; + + if (tmp_sock->sk.sk_type == SOCK_DGRAM && + tmp_sock->sk.sk_state != LLCP_BOUND) + continue; + + if (tmp_sock->service_name == NULL || + tmp_sock->service_name_len == 0) + continue; + + if (tmp_sock->service_name_len != sn_len) + continue; + + if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) { + llcp_sock = tmp_sock; + break; + } + } + + read_unlock(&local->sockets.lock); + + pr_debug("Found llcp sock %p\n", llcp_sock); + + return llcp_sock; +} + +u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, + struct nfc_llcp_sock *sock) +{ + mutex_lock(&local->sdp_lock); + + if (sock->service_name != NULL && sock->service_name_len > 0) { + int ssap = nfc_llcp_wks_sap(sock->service_name, + sock->service_name_len); + + if (ssap > 0) { + pr_debug("WKS %d\n", ssap); + + /* This is a WKS, let's check if it's free */ + if (local->local_wks & BIT(ssap)) { + mutex_unlock(&local->sdp_lock); + + return LLCP_SAP_MAX; + } + + set_bit(ssap, &local->local_wks); + mutex_unlock(&local->sdp_lock); + + return ssap; + } + + /* + * Check if there already is a non WKS socket bound + * to this service name. + */ + if (nfc_llcp_sock_from_sn(local, sock->service_name, + sock->service_name_len) != NULL) { + mutex_unlock(&local->sdp_lock); + + return LLCP_SAP_MAX; + } + + mutex_unlock(&local->sdp_lock); + + return LLCP_SDP_UNBOUND; + + } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) { + if (!test_bit(sock->ssap, &local->local_wks)) { + set_bit(sock->ssap, &local->local_wks); + mutex_unlock(&local->sdp_lock); + + return sock->ssap; + } + } + + mutex_unlock(&local->sdp_lock); + + return LLCP_SAP_MAX; +} + +u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local) +{ + u8 local_ssap; + + mutex_lock(&local->sdp_lock); + + local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP); + if (local_ssap == LLCP_LOCAL_NUM_SAP) { + mutex_unlock(&local->sdp_lock); + return LLCP_SAP_MAX; + } + + set_bit(local_ssap, &local->local_sap); + + mutex_unlock(&local->sdp_lock); + + return local_ssap + LLCP_LOCAL_SAP_OFFSET; +} + +void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) +{ + u8 local_ssap; + unsigned long *sdp; + + if (ssap < LLCP_WKS_NUM_SAP) { + local_ssap = ssap; + sdp = &local->local_wks; + } else if (ssap < LLCP_LOCAL_NUM_SAP) { + atomic_t *client_cnt; + + local_ssap = ssap - LLCP_WKS_NUM_SAP; + sdp = &local->local_sdp; + client_cnt = &local->local_sdp_cnt[local_ssap]; + + pr_debug("%d clients\n", atomic_read(client_cnt)); + + mutex_lock(&local->sdp_lock); + + if (atomic_dec_and_test(client_cnt)) { + struct nfc_llcp_sock *l_sock; + + pr_debug("No more clients for SAP %d\n", ssap); + + clear_bit(local_ssap, sdp); + + /* Find the listening sock and set it back to UNBOUND */ + l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP); + if (l_sock) { + l_sock->ssap = LLCP_SDP_UNBOUND; + nfc_llcp_sock_put(l_sock); + } + } + + mutex_unlock(&local->sdp_lock); + + return; + } else if (ssap < LLCP_MAX_SAP) { + local_ssap = ssap - LLCP_LOCAL_NUM_SAP; + sdp = &local->local_sap; + } else { + return; + } + + mutex_lock(&local->sdp_lock); + + clear_bit(local_ssap, sdp); + + mutex_unlock(&local->sdp_lock); +} + +static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local) +{ + u8 ssap; + + mutex_lock(&local->sdp_lock); + + ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); + if (ssap == LLCP_SDP_NUM_SAP) { + mutex_unlock(&local->sdp_lock); + + return LLCP_SAP_MAX; + } + + pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); + + set_bit(ssap, &local->local_sdp); + + mutex_unlock(&local->sdp_lock); + + return LLCP_WKS_NUM_SAP + ssap; +} + +static int nfc_llcp_build_gb(struct nfc_llcp_local *local) +{ + u8 *gb_cur, *version_tlv, version, version_length; + u8 *lto_tlv, lto_length; + u8 *wks_tlv, wks_length; + u8 *miux_tlv, miux_length; + u8 gb_len = 0; + int ret = 0; + + version = LLCP_VERSION_11; + version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, + 1, &version_length); + gb_len += version_length; + + lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, <o_length); + gb_len += lto_length; + + pr_debug("Local wks 0x%lx\n", local->local_wks); + wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2, + &wks_length); + gb_len += wks_length; + + miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0, + &miux_length); + gb_len += miux_length; + + gb_len += ARRAY_SIZE(llcp_magic); + + if (gb_len > NFC_MAX_GT_LEN) { + ret = -EINVAL; + goto out; + } + + gb_cur = local->gb; + + memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic)); + gb_cur += ARRAY_SIZE(llcp_magic); + + memcpy(gb_cur, version_tlv, version_length); + gb_cur += version_length; + + memcpy(gb_cur, lto_tlv, lto_length); + gb_cur += lto_length; + + memcpy(gb_cur, wks_tlv, wks_length); + gb_cur += wks_length; + + memcpy(gb_cur, miux_tlv, miux_length); + gb_cur += miux_length; + + local->gb_len = gb_len; + +out: + kfree(version_tlv); + kfree(lto_tlv); + kfree(wks_tlv); + kfree(miux_tlv); + + return ret; +} + +u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) +{ + struct nfc_llcp_local *local; + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + *general_bytes_len = 0; + return NULL; + } + + nfc_llcp_build_gb(local); + + *general_bytes_len = local->gb_len; + + return local->gb; +} + +int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len) +{ + struct nfc_llcp_local *local = nfc_llcp_find_local(dev); + + if (local == NULL) { + pr_err("No LLCP device\n"); + return -ENODEV; + } + if (gb_len < 3) + return -EINVAL; + + memset(local->remote_gb, 0, NFC_MAX_GT_LEN); + memcpy(local->remote_gb, gb, gb_len); + local->remote_gb_len = gb_len; + + if (memcmp(local->remote_gb, llcp_magic, 3)) { + pr_err("MAC does not support LLCP\n"); + return -EINVAL; + } + + return nfc_llcp_parse_gb_tlv(local, + &local->remote_gb[3], + local->remote_gb_len - 3); +} + +static u8 nfc_llcp_dsap(struct sk_buff *pdu) +{ + return (pdu->data[0] & 0xfc) >> 2; +} + +static u8 nfc_llcp_ptype(struct sk_buff *pdu) +{ + return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6); +} + +static u8 nfc_llcp_ssap(struct sk_buff *pdu) +{ + return pdu->data[1] & 0x3f; +} + +static u8 nfc_llcp_ns(struct sk_buff *pdu) +{ + return pdu->data[2] >> 4; +} + +static u8 nfc_llcp_nr(struct sk_buff *pdu) +{ + return pdu->data[2] & 0xf; +} + +static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu) +{ + pdu->data[2] = (sock->send_n << 4) | (sock->recv_n); + sock->send_n = (sock->send_n + 1) % 16; + sock->recv_ack_n = (sock->recv_n - 1) % 16; +} + +void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local, + struct sk_buff *skb, u8 direction) +{ + struct sk_buff *skb_copy = NULL, *nskb; + struct sock *sk; + u8 *data; + + read_lock(&local->raw_sockets.lock); + + sk_for_each(sk, &local->raw_sockets.head) { + if (sk->sk_state != LLCP_BOUND) + continue; + + if (skb_copy == NULL) { + skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE, + GFP_ATOMIC); + + if (skb_copy == NULL) + continue; + + data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE); + + data[0] = local->dev ? local->dev->idx : 0xFF; + data[1] = direction; + } + + nskb = skb_clone(skb_copy, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&local->raw_sockets.lock); + + kfree_skb(skb_copy); +} + +static void nfc_llcp_tx_work(struct work_struct *work) +{ + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + tx_work); + struct sk_buff *skb; + struct sock *sk; + struct nfc_llcp_sock *llcp_sock; + + skb = skb_dequeue(&local->tx_queue); + if (skb != NULL) { + sk = skb->sk; + llcp_sock = nfc_llcp_sock(sk); + + if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) { + nfc_llcp_send_symm(local->dev); + } else { + struct sk_buff *copy_skb = NULL; + u8 ptype = nfc_llcp_ptype(skb); + int ret; + + pr_debug("Sending pending skb\n"); + print_hex_dump(KERN_DEBUG, "LLCP Tx: ", + DUMP_PREFIX_OFFSET, 16, 1, + skb->data, skb->len, true); + + if (ptype == LLCP_PDU_I) + copy_skb = skb_copy(skb, GFP_ATOMIC); + + __net_timestamp(skb); + + nfc_llcp_send_to_raw_sock(local, skb, + NFC_LLCP_DIRECTION_TX); + + ret = nfc_data_exchange(local->dev, local->target_idx, + skb, nfc_llcp_recv, local); + + if (ret) { + kfree_skb(copy_skb); + goto out; + } + + if (ptype == LLCP_PDU_I && copy_skb) + skb_queue_tail(&llcp_sock->tx_pending_queue, + copy_skb); + } + } else { + nfc_llcp_send_symm(local->dev); + } + +out: + mod_timer(&local->link_timer, + jiffies + msecs_to_jiffies(2 * local->remote_lto)); +} + +static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local, + u8 ssap) +{ + struct sock *sk; + struct nfc_llcp_sock *llcp_sock; + + read_lock(&local->connecting_sockets.lock); + + sk_for_each(sk, &local->connecting_sockets.head) { + llcp_sock = nfc_llcp_sock(sk); + + if (llcp_sock->ssap == ssap) { + sock_hold(&llcp_sock->sk); + goto out; + } + } + + llcp_sock = NULL; + +out: + read_unlock(&local->connecting_sockets.lock); + + return llcp_sock; +} + +static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local, + u8 *sn, size_t sn_len) +{ + struct nfc_llcp_sock *llcp_sock; + + llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len); + + if (llcp_sock == NULL) + return NULL; + + sock_hold(&llcp_sock->sk); + + return llcp_sock; +} + +static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) +{ + u8 *tlv = &skb->data[2], type, length; + size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0; + + while (offset < tlv_array_len) { + type = tlv[0]; + length = tlv[1]; + + pr_debug("type 0x%x length %d\n", type, length); + + if (type == LLCP_TLV_SN) { + *sn_len = length; + return &tlv[2]; + } + + offset += length + 2; + tlv += length + 2; + } + + return NULL; +} + +static void nfc_llcp_recv_ui(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct nfc_llcp_ui_cb *ui_cb; + u8 dsap, ssap; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + ui_cb = nfc_llcp_ui_skb_cb(skb); + ui_cb->dsap = dsap; + ui_cb->ssap = ssap; + + pr_debug("%d %d\n", dsap, ssap); + + /* We're looking for a bound socket, not a client one */ + llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); + if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM) + return; + + /* There is no sequence with UI frames */ + skb_pull(skb, LLCP_HEADER_SIZE); + if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { + /* + * UI frames will be freed from the socket layer, so we + * need to keep them alive until someone receives them. + */ + skb_get(skb); + } else { + pr_err("Receive queue is full\n"); + } + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct sock *new_sk, *parent; + struct nfc_llcp_sock *sock, *new_sock; + u8 dsap, ssap, reason; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("%d %d\n", dsap, ssap); + + if (dsap != LLCP_SAP_SDP) { + sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); + if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) { + reason = LLCP_DM_NOBOUND; + goto fail; + } + } else { + u8 *sn; + size_t sn_len; + + sn = nfc_llcp_connect_sn(skb, &sn_len); + if (sn == NULL) { + reason = LLCP_DM_NOBOUND; + goto fail; + } + + pr_debug("Service name length %zu\n", sn_len); + + sock = nfc_llcp_sock_get_sn(local, sn, sn_len); + if (sock == NULL) { + reason = LLCP_DM_NOBOUND; + goto fail; + } + } + + lock_sock(&sock->sk); + + parent = &sock->sk; + + if (sk_acceptq_is_full(parent)) { + reason = LLCP_DM_REJ; + release_sock(&sock->sk); + sock_put(&sock->sk); + goto fail; + } + + if (sock->ssap == LLCP_SDP_UNBOUND) { + u8 ssap = nfc_llcp_reserve_sdp_ssap(local); + + pr_debug("First client, reserving %d\n", ssap); + + if (ssap == LLCP_SAP_MAX) { + reason = LLCP_DM_REJ; + release_sock(&sock->sk); + sock_put(&sock->sk); + goto fail; + } + + sock->ssap = ssap; + } + + new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC); + if (new_sk == NULL) { + reason = LLCP_DM_REJ; + release_sock(&sock->sk); + sock_put(&sock->sk); + goto fail; + } + + new_sock = nfc_llcp_sock(new_sk); + new_sock->dev = local->dev; + new_sock->local = nfc_llcp_local_get(local); + new_sock->rw = sock->rw; + new_sock->miux = sock->miux; + new_sock->remote_miu = local->remote_miu; + new_sock->nfc_protocol = sock->nfc_protocol; + new_sock->dsap = ssap; + new_sock->target_idx = local->target_idx; + new_sock->parent = parent; + new_sock->ssap = sock->ssap; + if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) { + atomic_t *client_count; + + pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock); + + client_count = + &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP]; + + atomic_inc(client_count); + new_sock->reserved_ssap = sock->ssap; + } + + nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); + + nfc_llcp_sock_link(&local->sockets, new_sk); + + nfc_llcp_accept_enqueue(&sock->sk, new_sk); + + nfc_get_device(local->dev->idx); + + new_sk->sk_state = LLCP_CONNECTED; + + /* Wake the listening processes */ + parent->sk_data_ready(parent, 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); +} + +int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) +{ + int nr_frames = 0; + struct nfc_llcp_local *local = sock->local; + + pr_debug("Remote ready %d tx queue len %d remote rw %d", + sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), + sock->remote_rw); + + /* Try to queue some I frames for transmission */ + while (sock->remote_ready && + skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) { + struct sk_buff *pdu; + + pdu = skb_dequeue(&sock->tx_queue); + if (pdu == NULL) + break; + + /* Update N(S)/N(R) */ + nfc_llcp_set_nrns(sock, pdu); + + skb_queue_tail(&local->tx_queue, pdu); + nr_frames++; + } + + return nr_frames; +} + +static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap, ptype, ns, nr; + + ptype = nfc_llcp_ptype(skb); + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + ns = nfc_llcp_ns(skb); + nr = nfc_llcp_nr(skb); + + pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns); + + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + if (llcp_sock == NULL) { + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + return; + } + + sk = &llcp_sock->sk; + lock_sock(sk); + if (sk->sk_state == LLCP_CLOSED) { + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); + } + + /* Pass the payload upstream */ + if (ptype == LLCP_PDU_I) { + pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); + + if (ns == llcp_sock->recv_n) + llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16; + else + pr_err("Received out of sequence I PDU\n"); + + skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); + if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) { + /* + * I frames will be freed from the socket layer, so we + * need to keep them alive until someone receives them. + */ + skb_get(skb); + } else { + pr_err("Receive queue is full\n"); + } + } + + /* Remove skbs from the pending queue */ + if (llcp_sock->send_ack_n != nr) { + struct sk_buff *s, *tmp; + u8 n; + + llcp_sock->send_ack_n = nr; + + /* Remove and free all skbs until ns == nr */ + skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) { + n = nfc_llcp_ns(s); + + skb_unlink(s, &llcp_sock->tx_pending_queue); + kfree_skb(s); + + if (n == nr) + break; + } + + /* Re-queue the remaining skbs for transmission */ + skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue, + s, tmp) { + skb_unlink(s, &llcp_sock->tx_pending_queue); + skb_queue_head(&local->tx_queue, s); + } + } + + if (ptype == LLCP_PDU_RR) + llcp_sock->remote_ready = true; + else if (ptype == LLCP_PDU_RNR) + llcp_sock->remote_ready = false; + + if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I) + nfc_llcp_send_rr(llcp_sock); + + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_disc(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + if ((dsap == 0) && (ssap == 0)) { + pr_debug("Connection termination"); + nfc_dep_link_down(local->dev); + return; + } + + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + if (llcp_sock == NULL) { + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + return; + } + + sk = &llcp_sock->sk; + lock_sock(sk); + + nfc_llcp_socket_purge(llcp_sock); + + if (sk->sk_state == LLCP_CLOSED) { + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); + } + + if (sk->sk_state == LLCP_CONNECTED) { + nfc_put_device(local->dev); + sk->sk_state = LLCP_CLOSED; + sk->sk_state_change(sk); + } + + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC); + + release_sock(sk); + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); + if (llcp_sock == NULL) { + pr_err("Invalid CC\n"); + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + + return; + } + + sk = &llcp_sock->sk; + + /* Unlink from connecting and link to the client array */ + nfc_llcp_sock_unlink(&local->connecting_sockets, sk); + nfc_llcp_sock_link(&local->sockets, sk); + llcp_sock->dsap = ssap; + + nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + sk->sk_state = LLCP_CONNECTED; + sk->sk_state_change(sk); + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + struct sock *sk; + u8 dsap, ssap, reason; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + reason = skb->data[2]; + + pr_debug("%d %d reason %d\n", ssap, dsap, reason); + + switch (reason) { + case LLCP_DM_NOBOUND: + case LLCP_DM_REJ: + llcp_sock = nfc_llcp_connecting_sock_get(local, dsap); + break; + + default: + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + break; + } + + if (llcp_sock == NULL) { + pr_debug("Already closed\n"); + return; + } + + sk = &llcp_sock->sk; + + sk->sk_err = ENXIO; + sk->sk_state = LLCP_CLOSED; + sk->sk_state_change(sk); + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, + struct sk_buff *skb) +{ + struct nfc_llcp_sock *llcp_sock; + u8 dsap, ssap, *tlv, type, length, tid, sap; + u16 tlv_len, offset; + char *service_name; + size_t service_name_len; + struct nfc_llcp_sdp_tlv *sdp; + HLIST_HEAD(llc_sdres_list); + size_t sdres_tlvs_len; + HLIST_HEAD(nl_sdres_list); + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("%d %d\n", dsap, ssap); + + if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) { + pr_err("Wrong SNL SAP\n"); + return; + } + + tlv = &skb->data[LLCP_HEADER_SIZE]; + tlv_len = skb->len - LLCP_HEADER_SIZE; + offset = 0; + sdres_tlvs_len = 0; + + while (offset < tlv_len) { + type = tlv[0]; + length = tlv[1]; + + switch (type) { + case LLCP_TLV_SDREQ: + tid = tlv[2]; + service_name = (char *) &tlv[3]; + service_name_len = length - 1; + + pr_debug("Looking for %.16s\n", service_name); + + if (service_name_len == strlen("urn:nfc:sn:sdp") && + !strncmp(service_name, "urn:nfc:sn:sdp", + service_name_len)) { + sap = 1; + goto add_snl; + } + + llcp_sock = nfc_llcp_sock_from_sn(local, service_name, + service_name_len); + if (!llcp_sock) { + sap = 0; + goto add_snl; + } + + /* + * We found a socket but its ssap has not been reserved + * yet. We need to assign it for good and send a reply. + * The ssap will be freed when the socket is closed. + */ + if (llcp_sock->ssap == LLCP_SDP_UNBOUND) { + atomic_t *client_count; + + sap = nfc_llcp_reserve_sdp_ssap(local); + + pr_debug("Reserving %d\n", sap); + + if (sap == LLCP_SAP_MAX) { + sap = 0; + goto add_snl; + } + + client_count = + &local->local_sdp_cnt[sap - + LLCP_WKS_NUM_SAP]; + + atomic_inc(client_count); + + llcp_sock->ssap = sap; + llcp_sock->reserved_ssap = sap; + } else { + sap = llcp_sock->ssap; + } + + pr_debug("%p %d\n", llcp_sock, sap); + +add_snl: + sdp = nfc_llcp_build_sdres_tlv(tid, sap); + if (sdp == NULL) + goto exit; + + sdres_tlvs_len += sdp->tlv_len; + hlist_add_head(&sdp->node, &llc_sdres_list); + break; + + case LLCP_TLV_SDRES: + mutex_lock(&local->sdreq_lock); + + pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]); + + hlist_for_each_entry(sdp, &local->pending_sdreqs, node) { + if (sdp->tid != tlv[2]) + continue; + + sdp->sap = tlv[3]; + + pr_debug("Found: uri=%s, sap=%d\n", + sdp->uri, sdp->sap); + + hlist_del(&sdp->node); + + hlist_add_head(&sdp->node, &nl_sdres_list); + + break; + } + + mutex_unlock(&local->sdreq_lock); + break; + + default: + pr_err("Invalid SNL tlv value 0x%x\n", type); + break; + } + + offset += length + 2; + tlv += length + 2; + } + +exit: + if (!hlist_empty(&nl_sdres_list)) + nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list); + + if (!hlist_empty(&llc_sdres_list)) + nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); +} + +static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + u8 ptype; + u16 pdu_len; + struct sk_buff *new_skb; + + if (skb->len <= LLCP_HEADER_SIZE) { + pr_err("Malformed AGF PDU\n"); + return; + } + + skb_pull(skb, LLCP_HEADER_SIZE); + + while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) { + pdu_len = skb->data[0] << 8 | skb->data[1]; + + skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE); + + if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) { + pr_err("Malformed AGF PDU\n"); + return; + } + + ptype = nfc_llcp_ptype(skb); + + if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF) + goto next; + + new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL); + if (new_skb == NULL) { + pr_err("Could not allocate PDU\n"); + return; + } + + memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len); + + nfc_llcp_rx_skb(local, new_skb); + + kfree_skb(new_skb); +next: + skb_pull(skb, pdu_len); + } +} + +static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + u8 dsap, ssap, ptype; + + ptype = nfc_llcp_ptype(skb); + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap); + + if (ptype != LLCP_PDU_SYMM) + print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET, + 16, 1, skb->data, skb->len, true); + + switch (ptype) { + case LLCP_PDU_SYMM: + pr_debug("SYMM\n"); + break; + + case LLCP_PDU_UI: + pr_debug("UI\n"); + nfc_llcp_recv_ui(local, skb); + break; + + case LLCP_PDU_CONNECT: + pr_debug("CONNECT\n"); + nfc_llcp_recv_connect(local, skb); + break; + + case LLCP_PDU_DISC: + pr_debug("DISC\n"); + nfc_llcp_recv_disc(local, skb); + break; + + case LLCP_PDU_CC: + pr_debug("CC\n"); + nfc_llcp_recv_cc(local, skb); + break; + + case LLCP_PDU_DM: + pr_debug("DM\n"); + nfc_llcp_recv_dm(local, skb); + break; + + case LLCP_PDU_SNL: + pr_debug("SNL\n"); + nfc_llcp_recv_snl(local, skb); + break; + + case LLCP_PDU_I: + case LLCP_PDU_RR: + case LLCP_PDU_RNR: + pr_debug("I frame\n"); + nfc_llcp_recv_hdlc(local, skb); + break; + + case LLCP_PDU_AGF: + pr_debug("AGF frame\n"); + nfc_llcp_recv_agf(local, skb); + break; + } +} + +static void nfc_llcp_rx_work(struct work_struct *work) +{ + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + rx_work); + struct sk_buff *skb; + + skb = local->rx_pending; + if (skb == NULL) { + pr_debug("No pending SKB\n"); + return; + } + + __net_timestamp(skb); + + nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX); + + nfc_llcp_rx_skb(local, skb); + + schedule_work(&local->tx_work); + kfree_skb(local->rx_pending); + local->rx_pending = NULL; +} + +static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb) +{ + local->rx_pending = skb; + del_timer(&local->link_timer); + schedule_work(&local->rx_work); +} + +void nfc_llcp_recv(void *data, struct sk_buff *skb, int err) +{ + struct nfc_llcp_local *local = (struct nfc_llcp_local *) data; + + pr_debug("Received an LLCP PDU\n"); + if (err < 0) { + pr_err("err %d\n", err); + return; + } + + __nfc_llcp_recv(local, skb); +} + +int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb) +{ + struct nfc_llcp_local *local; + + local = nfc_llcp_find_local(dev); + if (local == NULL) + return -ENODEV; + + __nfc_llcp_recv(local, skb); + + return 0; +} + +void nfc_llcp_mac_is_down(struct nfc_dev *dev) +{ + struct nfc_llcp_local *local; + + local = nfc_llcp_find_local(dev); + if (local == NULL) + return; + + local->remote_miu = LLCP_DEFAULT_MIU; + local->remote_lto = LLCP_DEFAULT_LTO; + + /* Close and purge all existing sockets */ + nfc_llcp_socket_release(local, true, 0); +} + +void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode) +{ + struct nfc_llcp_local *local; + + pr_debug("rf mode %d\n", rf_mode); + + local = nfc_llcp_find_local(dev); + if (local == NULL) + return; + + local->target_idx = target_idx; + local->comm_mode = comm_mode; + local->rf_mode = rf_mode; + + if (rf_mode == NFC_RF_INITIATOR) { + pr_debug("Queueing Tx work\n"); + + schedule_work(&local->tx_work); + } else { + mod_timer(&local->link_timer, + jiffies + msecs_to_jiffies(local->remote_lto)); + } +} + +int nfc_llcp_register_device(struct nfc_dev *ndev) +{ + struct nfc_llcp_local *local; + + local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); + if (local == NULL) + return -ENOMEM; + + local->dev = ndev; + INIT_LIST_HEAD(&local->list); + kref_init(&local->ref); + mutex_init(&local->sdp_lock); + init_timer(&local->link_timer); + local->link_timer.data = (unsigned long) local; + local->link_timer.function = nfc_llcp_symm_timer; + + skb_queue_head_init(&local->tx_queue); + INIT_WORK(&local->tx_work, nfc_llcp_tx_work); + + local->rx_pending = NULL; + INIT_WORK(&local->rx_work, nfc_llcp_rx_work); + + INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); + + rwlock_init(&local->sockets.lock); + rwlock_init(&local->connecting_sockets.lock); + rwlock_init(&local->raw_sockets.lock); + + local->lto = 150; /* 1500 ms */ + local->rw = LLCP_MAX_RW; + local->miux = cpu_to_be16(LLCP_MAX_MIUX); + + nfc_llcp_build_gb(local); + + local->remote_miu = LLCP_DEFAULT_MIU; + local->remote_lto = LLCP_DEFAULT_LTO; + + mutex_init(&local->sdreq_lock); + INIT_HLIST_HEAD(&local->pending_sdreqs); + init_timer(&local->sdreq_timer); + local->sdreq_timer.data = (unsigned long) local; + local->sdreq_timer.function = nfc_llcp_sdreq_timer; + INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work); + + list_add(&local->list, &llcp_devices); + + return 0; +} + +void nfc_llcp_unregister_device(struct nfc_dev *dev) +{ + struct nfc_llcp_local *local = nfc_llcp_find_local(dev); + + if (local == NULL) { + pr_debug("No such device\n"); + return; + } + + local_cleanup(local); + + nfc_llcp_local_put(local); +} + +int __init nfc_llcp_init(void) +{ + 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_sock.c b/net/nfc/llcp_sock.c new file mode 100644 index 000000000000..4950c189705e --- /dev/null +++ b/net/nfc/llcp_sock.c @@ -0,0 +1,1025 @@ +/* + * 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 +#include +#include +#include + +#include "nfc.h" +#include "llcp.h" + +static int sock_wait_state(struct sock *sk, int state, unsigned long timeo) +{ + DECLARE_WAITQUEUE(wait, current); + int err = 0; + + pr_debug("sk %p", sk); + + add_wait_queue(sk_sleep(sk), &wait); + set_current_state(TASK_INTERRUPTIBLE); + + while (sk->sk_state != state) { + if (!timeo) { + err = -EINPROGRESS; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + set_current_state(TASK_INTERRUPTIBLE); + + err = sock_error(sk); + if (err) + break; + } + + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return err; +} + +static struct proto llcp_sock_proto = { + .name = "NFC_LLCP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct nfc_llcp_sock), +}; + +static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + struct nfc_llcp_local *local; + struct nfc_dev *dev; + struct sockaddr_nfc_llcp llcp_addr; + int len, ret = 0; + + if (!addr || addr->sa_family != AF_NFC) + return -EINVAL; + + pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); + + memset(&llcp_addr, 0, sizeof(llcp_addr)); + len = min_t(unsigned int, sizeof(llcp_addr), alen); + memcpy(&llcp_addr, addr, len); + + /* This is going to be a listening socket, dsap must be 0 */ + if (llcp_addr.dsap != 0) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != LLCP_CLOSED) { + ret = -EBADFD; + goto error; + } + + dev = nfc_get_device(llcp_addr.dev_idx); + if (dev == NULL) { + ret = -ENODEV; + goto error; + } + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + ret = -ENODEV; + goto put_dev; + } + + llcp_sock->dev = dev; + llcp_sock->local = nfc_llcp_local_get(local); + llcp_sock->nfc_protocol = llcp_addr.nfc_protocol; + llcp_sock->service_name_len = min_t(unsigned int, + llcp_addr.service_name_len, + NFC_LLCP_MAX_SERVICE_NAME); + llcp_sock->service_name = kmemdup(llcp_addr.service_name, + llcp_sock->service_name_len, + GFP_KERNEL); + + llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock); + if (llcp_sock->ssap == LLCP_SAP_MAX) { + ret = -EADDRINUSE; + goto put_dev; + } + + llcp_sock->reserved_ssap = llcp_sock->ssap; + + nfc_llcp_sock_link(&local->sockets, sk); + + pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap); + + sk->sk_state = LLCP_BOUND; + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr, + int alen) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + struct nfc_llcp_local *local; + struct nfc_dev *dev; + struct sockaddr_nfc_llcp llcp_addr; + int len, ret = 0; + + if (!addr || addr->sa_family != AF_NFC) + return -EINVAL; + + pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); + + memset(&llcp_addr, 0, sizeof(llcp_addr)); + len = min_t(unsigned int, sizeof(llcp_addr), alen); + memcpy(&llcp_addr, addr, len); + + lock_sock(sk); + + if (sk->sk_state != LLCP_CLOSED) { + ret = -EBADFD; + goto error; + } + + dev = nfc_get_device(llcp_addr.dev_idx); + if (dev == NULL) { + ret = -ENODEV; + goto error; + } + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + ret = -ENODEV; + goto put_dev; + } + + llcp_sock->dev = dev; + llcp_sock->local = nfc_llcp_local_get(local); + llcp_sock->nfc_protocol = llcp_addr.nfc_protocol; + + nfc_llcp_sock_link(&local->raw_sockets, sk); + + sk->sk_state = LLCP_BOUND; + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +static int llcp_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + int ret = 0; + + pr_debug("sk %p backlog %d\n", sk, backlog); + + lock_sock(sk); + + if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) || + sk->sk_state != LLCP_BOUND) { + ret = -EBADFD; + goto error; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + + pr_debug("Socket listening\n"); + sk->sk_state = LLCP_LISTEN; + +error: + release_sock(sk); + + return ret; +} + +static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname, + char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + u32 opt; + int err = 0; + + pr_debug("%p optname %d\n", sk, optname); + + if (level != SOL_NFC) + return -ENOPROTOOPT; + + lock_sock(sk); + + switch (optname) { + case NFC_LLCP_RW: + if (sk->sk_state == LLCP_CONNECTED || + sk->sk_state == LLCP_BOUND || + sk->sk_state == LLCP_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > LLCP_MAX_RW) { + err = -EINVAL; + break; + } + + llcp_sock->rw = (u8) opt; + + break; + + case NFC_LLCP_MIUX: + if (sk->sk_state == LLCP_CONNECTED || + sk->sk_state == LLCP_BOUND || + sk->sk_state == LLCP_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > LLCP_MAX_MIUX) { + err = -EINVAL; + break; + } + + llcp_sock->miux = cpu_to_be16((u16) opt); + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + + pr_debug("%p rw %d miux %d\n", llcp_sock, + llcp_sock->rw, llcp_sock->miux); + + return err; +} + +static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct nfc_llcp_local *local; + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + int len, err = 0; + u16 miux, remote_miu; + u8 rw; + + pr_debug("%p optname %d\n", sk, optname); + + if (level != SOL_NFC) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + local = llcp_sock->local; + if (!local) + return -ENODEV; + + len = min_t(u32, len, sizeof(u32)); + + lock_sock(sk); + + switch (optname) { + case NFC_LLCP_RW: + rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw; + if (put_user(rw, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_MIUX: + miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ? + be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux); + + if (put_user(miux, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_REMOTE_MIU: + remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ? + local->remote_miu : llcp_sock->remote_miu; + + if (put_user(remote_miu, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_REMOTE_LTO: + if (put_user(local->remote_lto / 10, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_REMOTE_RW: + if (put_user(llcp_sock->remote_rw, (u32 __user *) optval)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + + if (put_user(len, optlen)) + return -EFAULT; + + return err; +} + +void nfc_llcp_accept_unlink(struct sock *sk) +{ + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + + pr_debug("state %d\n", sk->sk_state); + + list_del_init(&llcp_sock->accept_queue); + sk_acceptq_removed(llcp_sock->parent); + llcp_sock->parent = NULL; + + sock_put(sk); +} + +void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk) +{ + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent); + + /* Lock will be free from unlink */ + sock_hold(sk); + + list_add_tail(&llcp_sock->accept_queue, + &llcp_sock_parent->accept_queue); + llcp_sock->parent = parent; + sk_acceptq_added(parent); +} + +struct sock *nfc_llcp_accept_dequeue(struct sock *parent, + struct socket *newsock) +{ + struct nfc_llcp_sock *lsk, *n, *llcp_parent; + struct sock *sk; + + llcp_parent = nfc_llcp_sock(parent); + + list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue, + accept_queue) { + sk = &lsk->sk; + lock_sock(sk); + + if (sk->sk_state == LLCP_CLOSED) { + release_sock(sk); + nfc_llcp_accept_unlink(sk); + continue; + } + + if (sk->sk_state == LLCP_CONNECTED || !newsock) { + list_del_init(&lsk->accept_queue); + sock_put(sk); + + if (newsock) + sock_graft(sk, newsock); + + release_sock(sk); + + pr_debug("Returning sk state %d\n", sk->sk_state); + + sk_acceptq_removed(parent); + + return sk; + } + + release_sock(sk); + } + + return NULL; +} + +static int llcp_sock_accept(struct socket *sock, struct socket *newsock, + int flags) +{ + DECLARE_WAITQUEUE(wait, current); + struct sock *sk = sock->sk, *new_sk; + long timeo; + int ret = 0; + + pr_debug("parent %p\n", sk); + + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + + if (sk->sk_state != LLCP_LISTEN) { + ret = -EBADFD; + goto error; + } + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + /* Wait for an incoming connection. */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) { + set_current_state(TASK_INTERRUPTIBLE); + + if (!timeo) { + ret = -EAGAIN; + break; + } + + if (signal_pending(current)) { + ret = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + if (ret) + goto error; + + newsock->state = SS_CONNECTED; + + pr_debug("new socket %p\n", new_sk); + +error: + release_sock(sk); + + return ret; +} + +static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr, + int *len, int peer) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr); + + if (llcp_sock == NULL || llcp_sock->dev == NULL) + return -EBADFD; + + pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx, + llcp_sock->dsap, llcp_sock->ssap); + + uaddr->sa_family = AF_NFC; + + *len = sizeof(struct sockaddr_nfc_llcp); + + llcp_addr->dev_idx = llcp_sock->dev->idx; + llcp_addr->target_idx = llcp_sock->target_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 | POLLRDNORM; + + if (sk->sk_state == LLCP_CLOSED) + mask |= POLLHUP; + + if (sk->sk_shutdown & RCV_SHUTDOWN) + mask |= POLLRDHUP | POLLIN | POLLRDNORM; + + if (sk->sk_shutdown == SHUTDOWN_MASK) + mask |= POLLHUP; + + if (sock_writeable(sk)) + mask |= POLLOUT | POLLWRNORM | POLLWRBAND; + else + set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags); + + pr_debug("mask 0x%x\n", mask); + + return mask; +} + +static int llcp_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_local *local; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + int err = 0; + + if (!sk) + return 0; + + pr_debug("%p\n", sk); + + local = llcp_sock->local; + if (local == NULL) { + err = -ENODEV; + goto out; + } + + lock_sock(sk); + + /* Send a DISC */ + if (sk->sk_state == LLCP_CONNECTED) + nfc_llcp_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); + } + } + + if (llcp_sock->reserved_ssap < LLCP_SAP_MAX) + nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); + + release_sock(sk); + + if (sock->type == SOCK_RAW) + nfc_llcp_sock_unlink(&local->raw_sockets, sk); + else + nfc_llcp_sock_unlink(&local->sockets, sk); + +out: + sock_orphan(sk); + sock_put(sk); + + return err; +} + +static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr, + int len, int flags) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + int ret = 0; + + pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags); + + if (!addr || len < sizeof(struct sockaddr_nfc) || + addr->sa_family != AF_NFC) + return -EINVAL; + + if (addr->service_name_len == 0 && addr->dsap == 0) + return -EINVAL; + + pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx, + addr->target_idx, addr->nfc_protocol); + + lock_sock(sk); + + if (sk->sk_state == LLCP_CONNECTED) { + ret = -EISCONN; + goto error; + } + + dev = nfc_get_device(addr->dev_idx); + if (dev == NULL) { + ret = -ENODEV; + goto error; + } + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + ret = -ENODEV; + goto put_dev; + } + + device_lock(&dev->dev); + if (dev->dep_link_up == false) { + ret = -ENOLINK; + device_unlock(&dev->dev); + goto put_dev; + } + device_unlock(&dev->dev); + + if (local->rf_mode == NFC_RF_INITIATOR && + addr->target_idx != local->target_idx) { + ret = -ENOLINK; + goto put_dev; + } + + llcp_sock->dev = dev; + llcp_sock->local = nfc_llcp_local_get(local); + llcp_sock->remote_miu = llcp_sock->local->remote_miu; + llcp_sock->ssap = nfc_llcp_get_local_ssap(local); + if (llcp_sock->ssap == LLCP_SAP_MAX) { + ret = -ENOMEM; + goto put_dev; + } + + llcp_sock->reserved_ssap = llcp_sock->ssap; + + if (addr->service_name_len == 0) + llcp_sock->dsap = addr->dsap; + else + llcp_sock->dsap = LLCP_SAP_SDP; + llcp_sock->nfc_protocol = addr->nfc_protocol; + llcp_sock->service_name_len = min_t(unsigned int, + addr->service_name_len, + NFC_LLCP_MAX_SERVICE_NAME); + llcp_sock->service_name = kmemdup(addr->service_name, + llcp_sock->service_name_len, + GFP_KERNEL); + + nfc_llcp_sock_link(&local->connecting_sockets, sk); + + ret = nfc_llcp_send_connect(llcp_sock); + if (ret) + goto sock_unlink; + + ret = sock_wait_state(sk, LLCP_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + if (ret) + goto sock_unlink; + + release_sock(sk); + + return 0; + +sock_unlink: + nfc_llcp_put_ssap(local, llcp_sock->ssap); + + nfc_llcp_sock_unlink(&local->connecting_sockets, sk); + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock, + struct msghdr *msg, size_t len) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + int ret; + + pr_debug("sock %p sk %p", sock, sk); + + ret = sock_error(sk); + if (ret) + return ret; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + lock_sock(sk); + + if (sk->sk_type == SOCK_DGRAM) { + struct sockaddr_nfc_llcp *addr = + (struct sockaddr_nfc_llcp *)msg->msg_name; + + if (msg->msg_namelen < sizeof(*addr)) { + release_sock(sk); + return -EINVAL; + } + + release_sock(sk); + + return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap, + msg, len); + } + + if (sk->sk_state != LLCP_CONNECTED) { + release_sock(sk); + return -ENOTCONN; + } + + release_sock(sk); + + return nfc_llcp_send_i_frame(llcp_sock, msg, len); +} + +static int llcp_sock_recvmsg(struct 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 (skb_copy_datagram_iovec(cskb, 0, msg->msg_iov, copied)) { + if (!(flags & MSG_PEEK)) + skb_queue_head(&sk->sk_receive_queue, skb); + return -EFAULT; + } + + sock_recv_timestamp(msg, sk, skb); + + if (sk->sk_type == SOCK_DGRAM && msg->msg_name) { + struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb); + struct sockaddr_nfc_llcp *sockaddr = + (struct sockaddr_nfc_llcp *) msg->msg_name; + + msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp); + + pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap); + + sockaddr->sa_family = AF_NFC; + sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP; + sockaddr->dsap = ui_cb->dsap; + sockaddr->ssap = ui_cb->ssap; + } + + /* Mark read part of skb as used */ + if (!(flags & MSG_PEEK)) { + + /* SOCK_STREAM: re-queue skb if it contains unreceived data */ + if (sk->sk_type == SOCK_STREAM || + sk->sk_type == SOCK_DGRAM || + sk->sk_type == SOCK_RAW) { + skb_pull(skb, copied); + if (skb->len) { + skb_queue_head(&sk->sk_receive_queue, skb); + goto done; + } + } + + kfree_skb(skb); + } + + /* XXX Queue backlogged skbs */ + +done: + /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */ + if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC)) + copied = rlen; + + return copied; +} + +static const struct proto_ops llcp_sock_ops = { + .family = PF_NFC, + .owner = THIS_MODULE, + .bind = llcp_sock_bind, + .connect = llcp_sock_connect, + .release = llcp_sock_release, + .socketpair = sock_no_socketpair, + .accept = llcp_sock_accept, + .getname = llcp_sock_getname, + .poll = llcp_sock_poll, + .ioctl = sock_no_ioctl, + .listen = llcp_sock_listen, + .shutdown = sock_no_shutdown, + .setsockopt = nfc_llcp_setsockopt, + .getsockopt = nfc_llcp_getsockopt, + .sendmsg = llcp_sock_sendmsg, + .recvmsg = llcp_sock_recvmsg, + .mmap = sock_no_mmap, +}; + +static const struct proto_ops llcp_rawsock_ops = { + .family = PF_NFC, + .owner = THIS_MODULE, + .bind = llcp_raw_sock_bind, + .connect = sock_no_connect, + .release = llcp_sock_release, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .getname = llcp_sock_getname, + .poll = llcp_sock_poll, + .ioctl = sock_no_ioctl, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .sendmsg = sock_no_sendmsg, + .recvmsg = llcp_sock_recvmsg, + .mmap = sock_no_mmap, +}; + +static void llcp_sock_destruct(struct sock *sk) +{ + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + + pr_debug("%p\n", sk); + + if (sk->sk_state == LLCP_CONNECTED) + nfc_put_device(llcp_sock->dev); + + skb_queue_purge(&sk->sk_receive_queue); + + nfc_llcp_sock_free(llcp_sock); + + if (!sock_flag(sk, SOCK_DEAD)) { + pr_err("Freeing alive NFC LLCP socket %p\n", sk); + return; + } +} + +struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp) +{ + 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->rw = LLCP_MAX_RW + 1; + llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1); + llcp_sock->send_n = llcp_sock->send_ack_n = 0; + llcp_sock->recv_n = llcp_sock->recv_ack_n = 0; + llcp_sock->remote_ready = 1; + llcp_sock->reserved_ssap = LLCP_SAP_MAX; + nfc_llcp_socket_remote_param_init(llcp_sock); + skb_queue_head_init(&llcp_sock->tx_queue); + skb_queue_head_init(&llcp_sock->tx_pending_queue); + INIT_LIST_HEAD(&llcp_sock->accept_queue); + + if (sock != NULL) + sock->state = SS_UNCONNECTED; + + return sk; +} + +void nfc_llcp_sock_free(struct nfc_llcp_sock *sock) +{ + kfree(sock->service_name); + + skb_queue_purge(&sock->tx_queue); + skb_queue_purge(&sock->tx_pending_queue); + + list_del_init(&sock->accept_queue); + + sock->parent = NULL; + + nfc_llcp_local_put(sock->local); +} + +static int llcp_sock_create(struct net *net, struct socket *sock, + const struct nfc_protocol *nfc_proto) +{ + struct sock *sk; + + pr_debug("%p\n", sock); + + if (sock->type != SOCK_STREAM && + sock->type != SOCK_DGRAM && + sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + if (sock->type == SOCK_RAW) + sock->ops = &llcp_rawsock_ops; + else + sock->ops = &llcp_sock_ops; + + sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC); + 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/netlink.c b/net/nfc/netlink.c index 73fd51098f4d..f0c4d61f37c0 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -28,8 +28,7 @@ #include #include "nfc.h" - -#include "llcp/llcp.h" +#include "llcp.h" static struct genl_multicast_group nfc_genl_event_mcgrp = { .name = NFC_GENL_MCAST_EVENT_NAME, diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h index 94bfe19ba678..afa1f84ba040 100644 --- a/net/nfc/nfc.h +++ b/net/nfc/nfc.h @@ -48,8 +48,6 @@ struct nfc_rawsock { struct nfc_llcp_sdp_tlv; -#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); @@ -64,68 +62,6 @@ void nfc_llcp_exit(void); void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp); void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head); -#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, size_t *gb_len) -{ - *gb_len = 0; - return NULL; -} - -static inline int nfc_llcp_data_received(struct nfc_dev *dev, - struct sk_buff *skb) -{ - return 0; -} - -static inline struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev) -{ - return NULL; -} - -static inline int nfc_llcp_init(void) -{ - return 0; -} - -static inline void nfc_llcp_exit(void) -{ -} - -static inline void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) -{ -} - -static inline void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head) -{ -} - -#endif - int __init rawsock_init(void); void rawsock_exit(void); -- cgit