NFC: Move LLCP code to the NFC top level diirectory
authorSamuel Ortiz <sameo@linux.intel.com>
Fri, 26 Apr 2013 09:49:40 +0000 (11:49 +0200)
committerSamuel Ortiz <sameo@linux.intel.com>
Fri, 26 Apr 2013 10:37:28 +0000 (12:37 +0200)
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 <sameo@linux.intel.com>
13 files changed:
net/nfc/Kconfig
net/nfc/Makefile
net/nfc/llcp.h [new file with mode: 0644]
net/nfc/llcp/Kconfig [deleted file]
net/nfc/llcp/commands.c [deleted file]
net/nfc/llcp/llcp.c [deleted file]
net/nfc/llcp/llcp.h [deleted file]
net/nfc/llcp/sock.c [deleted file]
net/nfc/llcp_commands.c [new file with mode: 0644]
net/nfc/llcp_core.c [new file with mode: 0644]
net/nfc/llcp_sock.c [new file with mode: 0644]
net/nfc/netlink.c
net/nfc/nfc.h

index 60c3bbb63e8efaf1ea9de16e32641de804235829..91b70865623f822af55c3979cdea9df9428f1f27 100644 (file)
@@ -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"
index d1a117c2c401720864b0566bd4fca0ef82bf6fbf..fb799deaed4f9d5b130e88ed5402190b33fea1f6 100644 (file)
@@ -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 (file)
index 0000000..ff8c434
--- /dev/null
@@ -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 (file)
index a1a41cd..0000000
+++ /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 (file)
index 094f7e2..0000000
+++ /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 <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/nfc.h>
-
-#include <net/nfc/nfc.h>
-
-#include "../nfc.h"
-#include "llcp.h"
-
-static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
-       0,
-       1, /* VERSION */
-       2, /* MIUX */
-       2, /* WKS */
-       1, /* LTO */
-       1, /* RW */
-       0, /* SN */
-       1, /* OPT */
-       0, /* SDREQ */
-       2, /* SDRES */
-
-};
-
-static u8 llcp_tlv8(u8 *tlv, u8 type)
-{
-       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
-               return 0;
-
-       return tlv[2];
-}
-
-static u16 llcp_tlv16(u8 *tlv, u8 type)
-{
-       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
-               return 0;
-
-       return be16_to_cpu(*((__be16 *)(tlv + 2)));
-}
-
-
-static u8 llcp_tlv_version(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_VERSION);
-}
-
-static u16 llcp_tlv_miux(u8 *tlv)
-{
-       return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
-}
-
-static u16 llcp_tlv_wks(u8 *tlv)
-{
-       return llcp_tlv16(tlv, LLCP_TLV_WKS);
-}
-
-static u16 llcp_tlv_lto(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_LTO);
-}
-
-static u8 llcp_tlv_opt(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_OPT);
-}
-
-static u8 llcp_tlv_rw(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
-}
-
-u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
-{
-       u8 *tlv, length;
-
-       pr_debug("type %d\n", type);
-
-       if (type >= LLCP_TLV_MAX)
-               return NULL;
-
-       length = llcp_tlv_length[type];
-       if (length == 0 && value_length == 0)
-               return NULL;
-       else if (length == 0)
-               length = value_length;
-
-       *tlv_length = 2 + length;
-       tlv = kzalloc(2 + length, GFP_KERNEL);
-       if (tlv == NULL)
-               return tlv;
-
-       tlv[0] = type;
-       tlv[1] = length;
-       memcpy(tlv + 2, value, length);
-
-       return tlv;
-}
-
-struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
-{
-       struct nfc_llcp_sdp_tlv *sdres;
-       u8 value[2];
-
-       sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
-       if (sdres == NULL)
-               return NULL;
-
-       value[0] = tid;
-       value[1] = sap;
-
-       sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
-                                       &sdres->tlv_len);
-       if (sdres->tlv == NULL) {
-               kfree(sdres);
-               return NULL;
-       }
-
-       sdres->tid = tid;
-       sdres->sap = sap;
-
-       INIT_HLIST_NODE(&sdres->node);
-
-       return sdres;
-}
-
-struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
-                                                 size_t uri_len)
-{
-       struct nfc_llcp_sdp_tlv *sdreq;
-
-       pr_debug("uri: %s, len: %zu\n", uri, uri_len);
-
-       sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
-       if (sdreq == NULL)
-               return NULL;
-
-       sdreq->tlv_len = uri_len + 3;
-
-       if (uri[uri_len - 1] == 0)
-               sdreq->tlv_len--;
-
-       sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
-       if (sdreq->tlv == NULL) {
-               kfree(sdreq);
-               return NULL;
-       }
-
-       sdreq->tlv[0] = LLCP_TLV_SDREQ;
-       sdreq->tlv[1] = sdreq->tlv_len - 2;
-       sdreq->tlv[2] = tid;
-
-       sdreq->tid = tid;
-       sdreq->uri = sdreq->tlv + 3;
-       memcpy(sdreq->uri, uri, uri_len);
-
-       sdreq->time = jiffies;
-
-       INIT_HLIST_NODE(&sdreq->node);
-
-       return sdreq;
-}
-
-void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
-{
-       kfree(sdp->tlv);
-       kfree(sdp);
-}
-
-void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
-{
-       struct nfc_llcp_sdp_tlv *sdp;
-       struct hlist_node *n;
-
-       hlist_for_each_entry_safe(sdp, n, head, node) {
-               hlist_del(&sdp->node);
-
-               nfc_llcp_free_sdp_tlv(sdp);
-       }
-}
-
-int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
-                         u8 *tlv_array, u16 tlv_array_len)
-{
-       u8 *tlv = tlv_array, type, length, offset = 0;
-
-       pr_debug("TLV array length %d\n", tlv_array_len);
-
-       if (local == NULL)
-               return -ENODEV;
-
-       while (offset < tlv_array_len) {
-               type = tlv[0];
-               length = tlv[1];
-
-               pr_debug("type 0x%x length %d\n", type, length);
-
-               switch (type) {
-               case LLCP_TLV_VERSION:
-                       local->remote_version = llcp_tlv_version(tlv);
-                       break;
-               case LLCP_TLV_MIUX:
-                       local->remote_miu = llcp_tlv_miux(tlv) + 128;
-                       break;
-               case LLCP_TLV_WKS:
-                       local->remote_wks = llcp_tlv_wks(tlv);
-                       break;
-               case LLCP_TLV_LTO:
-                       local->remote_lto = llcp_tlv_lto(tlv) * 10;
-                       break;
-               case LLCP_TLV_OPT:
-                       local->remote_opt = llcp_tlv_opt(tlv);
-                       break;
-               default:
-                       pr_err("Invalid gt tlv value 0x%x\n", type);
-                       break;
-               }
-
-               offset += length + 2;
-               tlv += length + 2;
-       }
-
-       pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
-                local->remote_version, local->remote_miu,
-                local->remote_lto, local->remote_opt,
-                local->remote_wks);
-
-       return 0;
-}
-
-int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
-                                 u8 *tlv_array, u16 tlv_array_len)
-{
-       u8 *tlv = tlv_array, type, length, offset = 0;
-
-       pr_debug("TLV array length %d\n", tlv_array_len);
-
-       if (sock == NULL)
-               return -ENOTCONN;
-
-       while (offset < tlv_array_len) {
-               type = tlv[0];
-               length = tlv[1];
-
-               pr_debug("type 0x%x length %d\n", type, length);
-
-               switch (type) {
-               case LLCP_TLV_MIUX:
-                       sock->remote_miu = llcp_tlv_miux(tlv) + 128;
-                       break;
-               case LLCP_TLV_RW:
-                       sock->remote_rw = llcp_tlv_rw(tlv);
-                       break;
-               case LLCP_TLV_SN:
-                       break;
-               default:
-                       pr_err("Invalid gt tlv value 0x%x\n", type);
-                       break;
-               }
-
-               offset += length + 2;
-               tlv += length + 2;
-       }
-
-       pr_debug("sock %p rw %d miu %d\n", sock,
-                sock->remote_rw, sock->remote_miu);
-
-       return 0;
-}
-
-static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
-                                      u8 dsap, u8 ssap, u8 ptype)
-{
-       u8 header[2];
-
-       pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
-
-       header[0] = (u8)((dsap << 2) | (ptype >> 2));
-       header[1] = (u8)((ptype << 6) | ssap);
-
-       pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
-
-       memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
-
-       return pdu;
-}
-
-static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
-                                   u8 tlv_length)
-{
-       /* XXX Add an skb length check */
-
-       if (tlv == NULL)
-               return NULL;
-
-       memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
-
-       return pdu;
-}
-
-static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
-                                        u8 cmd, u16 size)
-{
-       struct sk_buff *skb;
-       int err;
-
-       if (sock->ssap == 0)
-               return NULL;
-
-       skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
-                                size + LLCP_HEADER_SIZE, &err);
-       if (skb == NULL) {
-               pr_err("Could not allocate PDU\n");
-               return NULL;
-       }
-
-       skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
-
-       return skb;
-}
-
-int nfc_llcp_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 (file)
index 9e483c8..0000000
+++ /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 <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/nfc.h>
-
-#include "../nfc.h"
-#include "llcp.h"
-
-static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
-
-static struct list_head llcp_devices;
-
-static void nfc_llcp_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, &lto_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 (file)
index ff8c434..0000000
+++ /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 (file)
index fd01ac6..0000000
+++ /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 <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/nfc.h>
-
-#include "../nfc.h"
-#include "llcp.h"
-
-static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
-{
-       DECLARE_WAITQUEUE(wait, current);
-       int err = 0;
-
-       pr_debug("sk %p", sk);
-
-       add_wait_queue(sk_sleep(sk), &wait);
-       set_current_state(TASK_INTERRUPTIBLE);
-
-       while (sk->sk_state != state) {
-               if (!timeo) {
-                       err = -EINPROGRESS;
-                       break;
-               }
-
-               if (signal_pending(current)) {
-                       err = sock_intr_errno(timeo);
-                       break;
-               }
-
-               release_sock(sk);
-               timeo = schedule_timeout(timeo);
-               lock_sock(sk);
-               set_current_state(TASK_INTERRUPTIBLE);
-
-               err = sock_error(sk);
-               if (err)
-                       break;
-       }
-
-       __set_current_state(TASK_RUNNING);
-       remove_wait_queue(sk_sleep(sk), &wait);
-       return err;
-}
-
-static struct proto llcp_sock_proto = {
-       .name     = "NFC_LLCP",
-       .owner    = THIS_MODULE,
-       .obj_size = sizeof(struct nfc_llcp_sock),
-};
-
-static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct nfc_llcp_local *local;
-       struct nfc_dev *dev;
-       struct sockaddr_nfc_llcp llcp_addr;
-       int len, ret = 0;
-
-       if (!addr || addr->sa_family != AF_NFC)
-               return -EINVAL;
-
-       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
-
-       memset(&llcp_addr, 0, sizeof(llcp_addr));
-       len = min_t(unsigned int, sizeof(llcp_addr), alen);
-       memcpy(&llcp_addr, addr, len);
-
-       /* This is going to be a listening socket, dsap must be 0 */
-       if (llcp_addr.dsap != 0)
-               return -EINVAL;
-
-       lock_sock(sk);
-
-       if (sk->sk_state != LLCP_CLOSED) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       dev = nfc_get_device(llcp_addr.dev_idx);
-       if (dev == NULL) {
-               ret = -ENODEV;
-               goto error;
-       }
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL) {
-               ret = -ENODEV;
-               goto put_dev;
-       }
-
-       llcp_sock->dev = dev;
-       llcp_sock->local = nfc_llcp_local_get(local);
-       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
-       llcp_sock->service_name_len = min_t(unsigned int,
-                                           llcp_addr.service_name_len,
-                                           NFC_LLCP_MAX_SERVICE_NAME);
-       llcp_sock->service_name = kmemdup(llcp_addr.service_name,
-                                         llcp_sock->service_name_len,
-                                         GFP_KERNEL);
-
-       llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
-       if (llcp_sock->ssap == LLCP_SAP_MAX) {
-               ret = -EADDRINUSE;
-               goto put_dev;
-       }
-
-       llcp_sock->reserved_ssap = llcp_sock->ssap;
-
-       nfc_llcp_sock_link(&local->sockets, sk);
-
-       pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
-
-       sk->sk_state = LLCP_BOUND;
-
-put_dev:
-       nfc_put_device(dev);
-
-error:
-       release_sock(sk);
-       return ret;
-}
-
-static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
-                             int alen)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct nfc_llcp_local *local;
-       struct nfc_dev *dev;
-       struct sockaddr_nfc_llcp llcp_addr;
-       int len, ret = 0;
-
-       if (!addr || addr->sa_family != AF_NFC)
-               return -EINVAL;
-
-       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
-
-       memset(&llcp_addr, 0, sizeof(llcp_addr));
-       len = min_t(unsigned int, sizeof(llcp_addr), alen);
-       memcpy(&llcp_addr, addr, len);
-
-       lock_sock(sk);
-
-       if (sk->sk_state != LLCP_CLOSED) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       dev = nfc_get_device(llcp_addr.dev_idx);
-       if (dev == NULL) {
-               ret = -ENODEV;
-               goto error;
-       }
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL) {
-               ret = -ENODEV;
-               goto put_dev;
-       }
-
-       llcp_sock->dev = dev;
-       llcp_sock->local = nfc_llcp_local_get(local);
-       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
-
-       nfc_llcp_sock_link(&local->raw_sockets, sk);
-
-       sk->sk_state = LLCP_BOUND;
-
-put_dev:
-       nfc_put_device(dev);
-
-error:
-       release_sock(sk);
-       return ret;
-}
-
-static int llcp_sock_listen(struct socket *sock, int backlog)
-{
-       struct sock *sk = sock->sk;
-       int ret = 0;
-
-       pr_debug("sk %p backlog %d\n", sk, backlog);
-
-       lock_sock(sk);
-
-       if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
-           sk->sk_state != LLCP_BOUND) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       sk->sk_max_ack_backlog = backlog;
-       sk->sk_ack_backlog = 0;
-
-       pr_debug("Socket listening\n");
-       sk->sk_state = LLCP_LISTEN;
-
-error:
-       release_sock(sk);
-
-       return ret;
-}
-
-static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
-                              char __user *optval, unsigned int optlen)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       u32 opt;
-       int err = 0;
-
-       pr_debug("%p optname %d\n", sk, optname);
-
-       if (level != SOL_NFC)
-               return -ENOPROTOOPT;
-
-       lock_sock(sk);
-
-       switch (optname) {
-       case NFC_LLCP_RW:
-               if (sk->sk_state == LLCP_CONNECTED ||
-                   sk->sk_state == LLCP_BOUND ||
-                   sk->sk_state == LLCP_LISTEN) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               if (get_user(opt, (u32 __user *) optval)) {
-                       err = -EFAULT;
-                       break;
-               }
-
-               if (opt > LLCP_MAX_RW) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               llcp_sock->rw = (u8) opt;
-
-               break;
-
-       case NFC_LLCP_MIUX:
-               if (sk->sk_state == LLCP_CONNECTED ||
-                   sk->sk_state == LLCP_BOUND ||
-                   sk->sk_state == LLCP_LISTEN) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               if (get_user(opt, (u32 __user *) optval)) {
-                       err = -EFAULT;
-                       break;
-               }
-
-               if (opt > LLCP_MAX_MIUX) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               llcp_sock->miux = cpu_to_be16((u16) opt);
-
-               break;
-
-       default:
-               err = -ENOPROTOOPT;
-               break;
-       }
-
-       release_sock(sk);
-
-       pr_debug("%p rw %d miux %d\n", llcp_sock,
-                llcp_sock->rw, llcp_sock->miux);
-
-       return err;
-}
-
-static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
-                              char __user *optval, int __user *optlen)
-{
-       struct nfc_llcp_local *local;
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       int len, err = 0;
-       u16 miux, remote_miu;
-       u8 rw;
-
-       pr_debug("%p optname %d\n", sk, optname);
-
-       if (level != SOL_NFC)
-               return -ENOPROTOOPT;
-
-       if (get_user(len, optlen))
-               return -EFAULT;
-
-       local = llcp_sock->local;
-       if (!local)
-               return -ENODEV;
-
-       len = min_t(u32, len, sizeof(u32));
-
-       lock_sock(sk);
-
-       switch (optname) {
-       case NFC_LLCP_RW:
-               rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
-               if (put_user(rw, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_MIUX:
-               miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
-                       be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
-
-               if (put_user(miux, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_REMOTE_MIU:
-               remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
-                               local->remote_miu : llcp_sock->remote_miu;
-
-               if (put_user(remote_miu, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_REMOTE_LTO:
-               if (put_user(local->remote_lto / 10, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_REMOTE_RW:
-               if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       default:
-               err = -ENOPROTOOPT;
-               break;
-       }
-
-       release_sock(sk);
-
-       if (put_user(len, optlen))
-               return -EFAULT;
-
-       return err;
-}
-
-void nfc_llcp_accept_unlink(struct sock *sk)
-{
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-
-       pr_debug("state %d\n", sk->sk_state);
-
-       list_del_init(&llcp_sock->accept_queue);
-       sk_acceptq_removed(llcp_sock->parent);
-       llcp_sock->parent = NULL;
-
-       sock_put(sk);
-}
-
-void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
-{
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
-
-       /* Lock will be free from unlink */
-       sock_hold(sk);
-
-       list_add_tail(&llcp_sock->accept_queue,
-                     &llcp_sock_parent->accept_queue);
-       llcp_sock->parent = parent;
-       sk_acceptq_added(parent);
-}
-
-struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
-                                    struct socket *newsock)
-{
-       struct nfc_llcp_sock *lsk, *n, *llcp_parent;
-       struct sock *sk;
-
-       llcp_parent = nfc_llcp_sock(parent);
-
-       list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
-                                accept_queue) {
-               sk = &lsk->sk;
-               lock_sock(sk);
-
-               if (sk->sk_state == LLCP_CLOSED) {
-                       release_sock(sk);
-                       nfc_llcp_accept_unlink(sk);
-                       continue;
-               }
-
-               if (sk->sk_state == LLCP_CONNECTED || !newsock) {
-                       list_del_init(&lsk->accept_queue);
-                       sock_put(sk);
-
-                       if (newsock)
-                               sock_graft(sk, newsock);
-
-                       release_sock(sk);
-
-                       pr_debug("Returning sk state %d\n", sk->sk_state);
-
-                       sk_acceptq_removed(parent);
-
-                       return sk;
-               }
-
-               release_sock(sk);
-       }
-
-       return NULL;
-}
-
-static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
-                           int flags)
-{
-       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 (file)
index 0000000..c1b23ee
--- /dev/null
@@ -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 <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include <net/nfc/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
+       0,
+       1, /* VERSION */
+       2, /* MIUX */
+       2, /* WKS */
+       1, /* LTO */
+       1, /* RW */
+       0, /* SN */
+       1, /* OPT */
+       0, /* SDREQ */
+       2, /* SDRES */
+
+};
+
+static u8 llcp_tlv8(u8 *tlv, u8 type)
+{
+       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+               return 0;
+
+       return tlv[2];
+}
+
+static u16 llcp_tlv16(u8 *tlv, u8 type)
+{
+       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+               return 0;
+
+       return be16_to_cpu(*((__be16 *)(tlv + 2)));
+}
+
+
+static u8 llcp_tlv_version(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_VERSION);
+}
+
+static u16 llcp_tlv_miux(u8 *tlv)
+{
+       return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
+}
+
+static u16 llcp_tlv_wks(u8 *tlv)
+{
+       return llcp_tlv16(tlv, LLCP_TLV_WKS);
+}
+
+static u16 llcp_tlv_lto(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_LTO);
+}
+
+static u8 llcp_tlv_opt(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_OPT);
+}
+
+static u8 llcp_tlv_rw(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
+}
+
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
+{
+       u8 *tlv, length;
+
+       pr_debug("type %d\n", type);
+
+       if (type >= LLCP_TLV_MAX)
+               return NULL;
+
+       length = llcp_tlv_length[type];
+       if (length == 0 && value_length == 0)
+               return NULL;
+       else if (length == 0)
+               length = value_length;
+
+       *tlv_length = 2 + length;
+       tlv = kzalloc(2 + length, GFP_KERNEL);
+       if (tlv == NULL)
+               return tlv;
+
+       tlv[0] = type;
+       tlv[1] = length;
+       memcpy(tlv + 2, value, length);
+
+       return tlv;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
+{
+       struct nfc_llcp_sdp_tlv *sdres;
+       u8 value[2];
+
+       sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+       if (sdres == NULL)
+               return NULL;
+
+       value[0] = tid;
+       value[1] = sap;
+
+       sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
+                                       &sdres->tlv_len);
+       if (sdres->tlv == NULL) {
+               kfree(sdres);
+               return NULL;
+       }
+
+       sdres->tid = tid;
+       sdres->sap = sap;
+
+       INIT_HLIST_NODE(&sdres->node);
+
+       return sdres;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+                                                 size_t uri_len)
+{
+       struct nfc_llcp_sdp_tlv *sdreq;
+
+       pr_debug("uri: %s, len: %zu\n", uri, uri_len);
+
+       sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+       if (sdreq == NULL)
+               return NULL;
+
+       sdreq->tlv_len = uri_len + 3;
+
+       if (uri[uri_len - 1] == 0)
+               sdreq->tlv_len--;
+
+       sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
+       if (sdreq->tlv == NULL) {
+               kfree(sdreq);
+               return NULL;
+       }
+
+       sdreq->tlv[0] = LLCP_TLV_SDREQ;
+       sdreq->tlv[1] = sdreq->tlv_len - 2;
+       sdreq->tlv[2] = tid;
+
+       sdreq->tid = tid;
+       sdreq->uri = sdreq->tlv + 3;
+       memcpy(sdreq->uri, uri, uri_len);
+
+       sdreq->time = jiffies;
+
+       INIT_HLIST_NODE(&sdreq->node);
+
+       return sdreq;
+}
+
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
+{
+       kfree(sdp->tlv);
+       kfree(sdp);
+}
+
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
+{
+       struct nfc_llcp_sdp_tlv *sdp;
+       struct hlist_node *n;
+
+       hlist_for_each_entry_safe(sdp, n, head, node) {
+               hlist_del(&sdp->node);
+
+               nfc_llcp_free_sdp_tlv(sdp);
+       }
+}
+
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+                         u8 *tlv_array, u16 tlv_array_len)
+{
+       u8 *tlv = tlv_array, type, length, offset = 0;
+
+       pr_debug("TLV array length %d\n", tlv_array_len);
+
+       if (local == NULL)
+               return -ENODEV;
+
+       while (offset < tlv_array_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               pr_debug("type 0x%x length %d\n", type, length);
+
+               switch (type) {
+               case LLCP_TLV_VERSION:
+                       local->remote_version = llcp_tlv_version(tlv);
+                       break;
+               case LLCP_TLV_MIUX:
+                       local->remote_miu = llcp_tlv_miux(tlv) + 128;
+                       break;
+               case LLCP_TLV_WKS:
+                       local->remote_wks = llcp_tlv_wks(tlv);
+                       break;
+               case LLCP_TLV_LTO:
+                       local->remote_lto = llcp_tlv_lto(tlv) * 10;
+                       break;
+               case LLCP_TLV_OPT:
+                       local->remote_opt = llcp_tlv_opt(tlv);
+                       break;
+               default:
+                       pr_err("Invalid gt tlv value 0x%x\n", type);
+                       break;
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+       pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
+                local->remote_version, local->remote_miu,
+                local->remote_lto, local->remote_opt,
+                local->remote_wks);
+
+       return 0;
+}
+
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+                                 u8 *tlv_array, u16 tlv_array_len)
+{
+       u8 *tlv = tlv_array, type, length, offset = 0;
+
+       pr_debug("TLV array length %d\n", tlv_array_len);
+
+       if (sock == NULL)
+               return -ENOTCONN;
+
+       while (offset < tlv_array_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               pr_debug("type 0x%x length %d\n", type, length);
+
+               switch (type) {
+               case LLCP_TLV_MIUX:
+                       sock->remote_miu = llcp_tlv_miux(tlv) + 128;
+                       break;
+               case LLCP_TLV_RW:
+                       sock->remote_rw = llcp_tlv_rw(tlv);
+                       break;
+               case LLCP_TLV_SN:
+                       break;
+               default:
+                       pr_err("Invalid gt tlv value 0x%x\n", type);
+                       break;
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+       pr_debug("sock %p rw %d miu %d\n", sock,
+                sock->remote_rw, sock->remote_miu);
+
+       return 0;
+}
+
+static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
+                                      u8 dsap, u8 ssap, u8 ptype)
+{
+       u8 header[2];
+
+       pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+       header[0] = (u8)((dsap << 2) | (ptype >> 2));
+       header[1] = (u8)((ptype << 6) | ssap);
+
+       pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
+
+       memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
+
+       return pdu;
+}
+
+static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
+                                   u8 tlv_length)
+{
+       /* XXX Add an skb length check */
+
+       if (tlv == NULL)
+               return NULL;
+
+       memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
+
+       return pdu;
+}
+
+static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
+                                        u8 cmd, u16 size)
+{
+       struct sk_buff *skb;
+       int err;
+
+       if (sock->ssap == 0)
+               return NULL;
+
+       skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+                                size + LLCP_HEADER_SIZE, &err);
+       if (skb == NULL) {
+               pr_err("Could not allocate PDU\n");
+               return NULL;
+       }
+
+       skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
+
+       return skb;
+}
+
+int nfc_llcp_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 (file)
index 0000000..158bdbf
--- /dev/null
@@ -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 <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
+
+static struct list_head llcp_devices;
+
+static void nfc_llcp_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, &lto_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 (file)
index 0000000..4950c18
--- /dev/null
@@ -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 <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
+{
+       DECLARE_WAITQUEUE(wait, current);
+       int err = 0;
+
+       pr_debug("sk %p", sk);
+
+       add_wait_queue(sk_sleep(sk), &wait);
+       set_current_state(TASK_INTERRUPTIBLE);
+
+       while (sk->sk_state != state) {
+               if (!timeo) {
+                       err = -EINPROGRESS;
+                       break;
+               }
+
+               if (signal_pending(current)) {
+                       err = sock_intr_errno(timeo);
+                       break;
+               }
+
+               release_sock(sk);
+               timeo = schedule_timeout(timeo);
+               lock_sock(sk);
+               set_current_state(TASK_INTERRUPTIBLE);
+
+               err = sock_error(sk);
+               if (err)
+                       break;
+       }
+
+       __set_current_state(TASK_RUNNING);
+       remove_wait_queue(sk_sleep(sk), &wait);
+       return err;
+}
+
+static struct proto llcp_sock_proto = {
+       .name     = "NFC_LLCP",
+       .owner    = THIS_MODULE,
+       .obj_size = sizeof(struct nfc_llcp_sock),
+};
+
+static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct nfc_llcp_local *local;
+       struct nfc_dev *dev;
+       struct sockaddr_nfc_llcp llcp_addr;
+       int len, ret = 0;
+
+       if (!addr || addr->sa_family != AF_NFC)
+               return -EINVAL;
+
+       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+       memset(&llcp_addr, 0, sizeof(llcp_addr));
+       len = min_t(unsigned int, sizeof(llcp_addr), alen);
+       memcpy(&llcp_addr, addr, len);
+
+       /* This is going to be a listening socket, dsap must be 0 */
+       if (llcp_addr.dsap != 0)
+               return -EINVAL;
+
+       lock_sock(sk);
+
+       if (sk->sk_state != LLCP_CLOSED) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       dev = nfc_get_device(llcp_addr.dev_idx);
+       if (dev == NULL) {
+               ret = -ENODEV;
+               goto error;
+       }
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL) {
+               ret = -ENODEV;
+               goto put_dev;
+       }
+
+       llcp_sock->dev = dev;
+       llcp_sock->local = nfc_llcp_local_get(local);
+       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+       llcp_sock->service_name_len = min_t(unsigned int,
+                                           llcp_addr.service_name_len,
+                                           NFC_LLCP_MAX_SERVICE_NAME);
+       llcp_sock->service_name = kmemdup(llcp_addr.service_name,
+                                         llcp_sock->service_name_len,
+                                         GFP_KERNEL);
+
+       llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
+       if (llcp_sock->ssap == LLCP_SAP_MAX) {
+               ret = -EADDRINUSE;
+               goto put_dev;
+       }
+
+       llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+       nfc_llcp_sock_link(&local->sockets, sk);
+
+       pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
+
+       sk->sk_state = LLCP_BOUND;
+
+put_dev:
+       nfc_put_device(dev);
+
+error:
+       release_sock(sk);
+       return ret;
+}
+
+static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
+                             int alen)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct nfc_llcp_local *local;
+       struct nfc_dev *dev;
+       struct sockaddr_nfc_llcp llcp_addr;
+       int len, ret = 0;
+
+       if (!addr || addr->sa_family != AF_NFC)
+               return -EINVAL;
+
+       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+       memset(&llcp_addr, 0, sizeof(llcp_addr));
+       len = min_t(unsigned int, sizeof(llcp_addr), alen);
+       memcpy(&llcp_addr, addr, len);
+
+       lock_sock(sk);
+
+       if (sk->sk_state != LLCP_CLOSED) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       dev = nfc_get_device(llcp_addr.dev_idx);
+       if (dev == NULL) {
+               ret = -ENODEV;
+               goto error;
+       }
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL) {
+               ret = -ENODEV;
+               goto put_dev;
+       }
+
+       llcp_sock->dev = dev;
+       llcp_sock->local = nfc_llcp_local_get(local);
+       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+
+       nfc_llcp_sock_link(&local->raw_sockets, sk);
+
+       sk->sk_state = LLCP_BOUND;
+
+put_dev:
+       nfc_put_device(dev);
+
+error:
+       release_sock(sk);
+       return ret;
+}
+
+static int llcp_sock_listen(struct socket *sock, int backlog)
+{
+       struct sock *sk = sock->sk;
+       int ret = 0;
+
+       pr_debug("sk %p backlog %d\n", sk, backlog);
+
+       lock_sock(sk);
+
+       if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
+           sk->sk_state != LLCP_BOUND) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       sk->sk_max_ack_backlog = backlog;
+       sk->sk_ack_backlog = 0;
+
+       pr_debug("Socket listening\n");
+       sk->sk_state = LLCP_LISTEN;
+
+error:
+       release_sock(sk);
+
+       return ret;
+}
+
+static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
+                              char __user *optval, unsigned int optlen)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       u32 opt;
+       int err = 0;
+
+       pr_debug("%p optname %d\n", sk, optname);
+
+       if (level != SOL_NFC)
+               return -ENOPROTOOPT;
+
+       lock_sock(sk);
+
+       switch (optname) {
+       case NFC_LLCP_RW:
+               if (sk->sk_state == LLCP_CONNECTED ||
+                   sk->sk_state == LLCP_BOUND ||
+                   sk->sk_state == LLCP_LISTEN) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               if (get_user(opt, (u32 __user *) optval)) {
+                       err = -EFAULT;
+                       break;
+               }
+
+               if (opt > LLCP_MAX_RW) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               llcp_sock->rw = (u8) opt;
+
+               break;
+
+       case NFC_LLCP_MIUX:
+               if (sk->sk_state == LLCP_CONNECTED ||
+                   sk->sk_state == LLCP_BOUND ||
+                   sk->sk_state == LLCP_LISTEN) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               if (get_user(opt, (u32 __user *) optval)) {
+                       err = -EFAULT;
+                       break;
+               }
+
+               if (opt > LLCP_MAX_MIUX) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               llcp_sock->miux = cpu_to_be16((u16) opt);
+
+               break;
+
+       default:
+               err = -ENOPROTOOPT;
+               break;
+       }
+
+       release_sock(sk);
+
+       pr_debug("%p rw %d miux %d\n", llcp_sock,
+                llcp_sock->rw, llcp_sock->miux);
+
+       return err;
+}
+
+static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
+                              char __user *optval, int __user *optlen)
+{
+       struct nfc_llcp_local *local;
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       int len, err = 0;
+       u16 miux, remote_miu;
+       u8 rw;
+
+       pr_debug("%p optname %d\n", sk, optname);
+
+       if (level != SOL_NFC)
+               return -ENOPROTOOPT;
+
+       if (get_user(len, optlen))
+               return -EFAULT;
+
+       local = llcp_sock->local;
+       if (!local)
+               return -ENODEV;
+
+       len = min_t(u32, len, sizeof(u32));
+
+       lock_sock(sk);
+
+       switch (optname) {
+       case NFC_LLCP_RW:
+               rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
+               if (put_user(rw, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_MIUX:
+               miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
+                       be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
+
+               if (put_user(miux, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_REMOTE_MIU:
+               remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
+                               local->remote_miu : llcp_sock->remote_miu;
+
+               if (put_user(remote_miu, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_REMOTE_LTO:
+               if (put_user(local->remote_lto / 10, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_REMOTE_RW:
+               if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       default:
+               err = -ENOPROTOOPT;
+               break;
+       }
+
+       release_sock(sk);
+
+       if (put_user(len, optlen))
+               return -EFAULT;
+
+       return err;
+}
+
+void nfc_llcp_accept_unlink(struct sock *sk)
+{
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+       pr_debug("state %d\n", sk->sk_state);
+
+       list_del_init(&llcp_sock->accept_queue);
+       sk_acceptq_removed(llcp_sock->parent);
+       llcp_sock->parent = NULL;
+
+       sock_put(sk);
+}
+
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
+{
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
+
+       /* Lock will be free from unlink */
+       sock_hold(sk);
+
+       list_add_tail(&llcp_sock->accept_queue,
+                     &llcp_sock_parent->accept_queue);
+       llcp_sock->parent = parent;
+       sk_acceptq_added(parent);
+}
+
+struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
+                                    struct socket *newsock)
+{
+       struct nfc_llcp_sock *lsk, *n, *llcp_parent;
+       struct sock *sk;
+
+       llcp_parent = nfc_llcp_sock(parent);
+
+       list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
+                                accept_queue) {
+               sk = &lsk->sk;
+               lock_sock(sk);
+
+               if (sk->sk_state == LLCP_CLOSED) {
+                       release_sock(sk);
+                       nfc_llcp_accept_unlink(sk);
+                       continue;
+               }
+
+               if (sk->sk_state == LLCP_CONNECTED || !newsock) {
+                       list_del_init(&lsk->accept_queue);
+                       sock_put(sk);
+
+                       if (newsock)
+                               sock_graft(sk, newsock);
+
+                       release_sock(sk);
+
+                       pr_debug("Returning sk state %d\n", sk->sk_state);
+
+                       sk_acceptq_removed(parent);
+
+                       return sk;
+               }
+
+               release_sock(sk);
+       }
+
+       return NULL;
+}
+
+static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
+                           int flags)
+{
+       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);
+}
index 73fd51098f4dc1250ecc89fe160f56055b635987..f0c4d61f37c0f623df3eb40a1222a87f9c875f69 100644 (file)
@@ -28,8 +28,7 @@
 #include <linux/slab.h>
 
 #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,
index 94bfe19ba678186cb7467ae03213f07e638046dd..afa1f84ba0406c6bb7622043302a2a6d9bb04703 100644 (file)
@@ -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);