source "net/nfc/nci/Kconfig"
source "net/nfc/hci/Kconfig"
-source "net/nfc/llcp/Kconfig"
source "drivers/nfc/Kconfig"
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
--- /dev/null
+/*
+ * 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);
+++ /dev/null
-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
+++ /dev/null
-/*
- * 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;
-}
+++ /dev/null
-/*
- * 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, <o_length);
- gb_len += lto_length;
-
- pr_debug("Local wks 0x%lx\n", local->local_wks);
- wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
- &wks_length);
- gb_len += wks_length;
-
- miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
- &miux_length);
- gb_len += miux_length;
-
- gb_len += ARRAY_SIZE(llcp_magic);
-
- if (gb_len > NFC_MAX_GT_LEN) {
- ret = -EINVAL;
- goto out;
- }
-
- gb_cur = local->gb;
-
- memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
- gb_cur += ARRAY_SIZE(llcp_magic);
-
- memcpy(gb_cur, version_tlv, version_length);
- gb_cur += version_length;
-
- memcpy(gb_cur, lto_tlv, lto_length);
- gb_cur += lto_length;
-
- memcpy(gb_cur, wks_tlv, wks_length);
- gb_cur += wks_length;
-
- memcpy(gb_cur, miux_tlv, miux_length);
- gb_cur += miux_length;
-
- local->gb_len = gb_len;
-
-out:
- kfree(version_tlv);
- kfree(lto_tlv);
- kfree(wks_tlv);
- kfree(miux_tlv);
-
- return ret;
-}
-
-u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
-{
- struct nfc_llcp_local *local;
-
- local = nfc_llcp_find_local(dev);
- if (local == NULL) {
- *general_bytes_len = 0;
- return NULL;
- }
-
- nfc_llcp_build_gb(local);
-
- *general_bytes_len = local->gb_len;
-
- return local->gb;
-}
-
-int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
-{
- struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
-
- if (local == NULL) {
- pr_err("No LLCP device\n");
- return -ENODEV;
- }
- if (gb_len < 3)
- return -EINVAL;
-
- memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
- memcpy(local->remote_gb, gb, gb_len);
- local->remote_gb_len = gb_len;
-
- if (memcmp(local->remote_gb, llcp_magic, 3)) {
- pr_err("MAC does not support LLCP\n");
- return -EINVAL;
- }
-
- return nfc_llcp_parse_gb_tlv(local,
- &local->remote_gb[3],
- local->remote_gb_len - 3);
-}
-
-static u8 nfc_llcp_dsap(struct sk_buff *pdu)
-{
- return (pdu->data[0] & 0xfc) >> 2;
-}
-
-static u8 nfc_llcp_ptype(struct sk_buff *pdu)
-{
- return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
-}
-
-static u8 nfc_llcp_ssap(struct sk_buff *pdu)
-{
- return pdu->data[1] & 0x3f;
-}
-
-static u8 nfc_llcp_ns(struct sk_buff *pdu)
-{
- return pdu->data[2] >> 4;
-}
-
-static u8 nfc_llcp_nr(struct sk_buff *pdu)
-{
- return pdu->data[2] & 0xf;
-}
-
-static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
-{
- pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
- sock->send_n = (sock->send_n + 1) % 16;
- sock->recv_ack_n = (sock->recv_n - 1) % 16;
-}
-
-void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
- struct sk_buff *skb, u8 direction)
-{
- struct sk_buff *skb_copy = NULL, *nskb;
- struct sock *sk;
- u8 *data;
-
- read_lock(&local->raw_sockets.lock);
-
- sk_for_each(sk, &local->raw_sockets.head) {
- if (sk->sk_state != LLCP_BOUND)
- continue;
-
- if (skb_copy == NULL) {
- skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
- GFP_ATOMIC);
-
- if (skb_copy == NULL)
- continue;
-
- data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
-
- data[0] = local->dev ? local->dev->idx : 0xFF;
- data[1] = direction;
- }
-
- nskb = skb_clone(skb_copy, GFP_ATOMIC);
- if (!nskb)
- continue;
-
- if (sock_queue_rcv_skb(sk, nskb))
- kfree_skb(nskb);
- }
-
- read_unlock(&local->raw_sockets.lock);
-
- kfree_skb(skb_copy);
-}
-
-static void nfc_llcp_tx_work(struct work_struct *work)
-{
- struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
- tx_work);
- struct sk_buff *skb;
- struct sock *sk;
- struct nfc_llcp_sock *llcp_sock;
-
- skb = skb_dequeue(&local->tx_queue);
- if (skb != NULL) {
- sk = skb->sk;
- llcp_sock = nfc_llcp_sock(sk);
-
- if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
- nfc_llcp_send_symm(local->dev);
- } else {
- struct sk_buff *copy_skb = NULL;
- u8 ptype = nfc_llcp_ptype(skb);
- int ret;
-
- pr_debug("Sending pending skb\n");
- print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
- DUMP_PREFIX_OFFSET, 16, 1,
- skb->data, skb->len, true);
-
- if (ptype == LLCP_PDU_I)
- copy_skb = skb_copy(skb, GFP_ATOMIC);
-
- __net_timestamp(skb);
-
- nfc_llcp_send_to_raw_sock(local, skb,
- NFC_LLCP_DIRECTION_TX);
-
- ret = nfc_data_exchange(local->dev, local->target_idx,
- skb, nfc_llcp_recv, local);
-
- if (ret) {
- kfree_skb(copy_skb);
- goto out;
- }
-
- if (ptype == LLCP_PDU_I && copy_skb)
- skb_queue_tail(&llcp_sock->tx_pending_queue,
- copy_skb);
- }
- } else {
- nfc_llcp_send_symm(local->dev);
- }
-
-out:
- mod_timer(&local->link_timer,
- jiffies + msecs_to_jiffies(2 * local->remote_lto));
-}
-
-static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
- u8 ssap)
-{
- struct sock *sk;
- struct nfc_llcp_sock *llcp_sock;
-
- read_lock(&local->connecting_sockets.lock);
-
- sk_for_each(sk, &local->connecting_sockets.head) {
- llcp_sock = nfc_llcp_sock(sk);
-
- if (llcp_sock->ssap == ssap) {
- sock_hold(&llcp_sock->sk);
- goto out;
- }
- }
-
- llcp_sock = NULL;
-
-out:
- read_unlock(&local->connecting_sockets.lock);
-
- return llcp_sock;
-}
-
-static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
- u8 *sn, size_t sn_len)
-{
- struct nfc_llcp_sock *llcp_sock;
-
- llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
-
- if (llcp_sock == NULL)
- return NULL;
-
- sock_hold(&llcp_sock->sk);
-
- return llcp_sock;
-}
-
-static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
-{
- u8 *tlv = &skb->data[2], type, length;
- size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
-
- while (offset < tlv_array_len) {
- type = tlv[0];
- length = tlv[1];
-
- pr_debug("type 0x%x length %d\n", type, length);
-
- if (type == LLCP_TLV_SN) {
- *sn_len = length;
- return &tlv[2];
- }
-
- offset += length + 2;
- tlv += length + 2;
- }
-
- return NULL;
-}
-
-static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
- struct sk_buff *skb)
-{
- struct nfc_llcp_sock *llcp_sock;
- struct nfc_llcp_ui_cb *ui_cb;
- u8 dsap, ssap;
-
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
-
- ui_cb = nfc_llcp_ui_skb_cb(skb);
- ui_cb->dsap = dsap;
- ui_cb->ssap = ssap;
-
- pr_debug("%d %d\n", dsap, ssap);
-
- /* We're looking for a bound socket, not a client one */
- llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
- if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
- return;
-
- /* There is no sequence with UI frames */
- skb_pull(skb, LLCP_HEADER_SIZE);
- if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
- /*
- * UI frames will be freed from the socket layer, so we
- * need to keep them alive until someone receives them.
- */
- skb_get(skb);
- } else {
- pr_err("Receive queue is full\n");
- }
-
- nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
- struct sk_buff *skb)
-{
- struct sock *new_sk, *parent;
- struct nfc_llcp_sock *sock, *new_sock;
- u8 dsap, ssap, reason;
-
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
-
- pr_debug("%d %d\n", dsap, ssap);
-
- if (dsap != LLCP_SAP_SDP) {
- sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
- if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
- reason = LLCP_DM_NOBOUND;
- goto fail;
- }
- } else {
- u8 *sn;
- size_t sn_len;
-
- sn = nfc_llcp_connect_sn(skb, &sn_len);
- if (sn == NULL) {
- reason = LLCP_DM_NOBOUND;
- goto fail;
- }
-
- pr_debug("Service name length %zu\n", sn_len);
-
- sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
- if (sock == NULL) {
- reason = LLCP_DM_NOBOUND;
- goto fail;
- }
- }
-
- lock_sock(&sock->sk);
-
- parent = &sock->sk;
-
- if (sk_acceptq_is_full(parent)) {
- reason = LLCP_DM_REJ;
- release_sock(&sock->sk);
- sock_put(&sock->sk);
- goto fail;
- }
-
- if (sock->ssap == LLCP_SDP_UNBOUND) {
- u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
-
- pr_debug("First client, reserving %d\n", ssap);
-
- if (ssap == LLCP_SAP_MAX) {
- reason = LLCP_DM_REJ;
- release_sock(&sock->sk);
- sock_put(&sock->sk);
- goto fail;
- }
-
- sock->ssap = ssap;
- }
-
- new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
- if (new_sk == NULL) {
- reason = LLCP_DM_REJ;
- release_sock(&sock->sk);
- sock_put(&sock->sk);
- goto fail;
- }
-
- new_sock = nfc_llcp_sock(new_sk);
- new_sock->dev = local->dev;
- new_sock->local = nfc_llcp_local_get(local);
- new_sock->rw = sock->rw;
- new_sock->miux = sock->miux;
- new_sock->remote_miu = local->remote_miu;
- new_sock->nfc_protocol = sock->nfc_protocol;
- new_sock->dsap = ssap;
- new_sock->target_idx = local->target_idx;
- new_sock->parent = parent;
- new_sock->ssap = sock->ssap;
- if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
- atomic_t *client_count;
-
- pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
-
- client_count =
- &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
-
- atomic_inc(client_count);
- new_sock->reserved_ssap = sock->ssap;
- }
-
- nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
- skb->len - LLCP_HEADER_SIZE);
-
- pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
-
- nfc_llcp_sock_link(&local->sockets, new_sk);
-
- nfc_llcp_accept_enqueue(&sock->sk, new_sk);
-
- nfc_get_device(local->dev->idx);
-
- new_sk->sk_state = LLCP_CONNECTED;
-
- /* Wake the listening processes */
- parent->sk_data_ready(parent, 0);
-
- /* Send CC */
- nfc_llcp_send_cc(new_sock);
-
- release_sock(&sock->sk);
- sock_put(&sock->sk);
-
- return;
-
-fail:
- /* Send DM */
- nfc_llcp_send_dm(local, dsap, ssap, reason);
-}
-
-int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
-{
- int nr_frames = 0;
- struct nfc_llcp_local *local = sock->local;
-
- pr_debug("Remote ready %d tx queue len %d remote rw %d",
- sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
- sock->remote_rw);
-
- /* Try to queue some I frames for transmission */
- while (sock->remote_ready &&
- skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
- struct sk_buff *pdu;
-
- pdu = skb_dequeue(&sock->tx_queue);
- if (pdu == NULL)
- break;
-
- /* Update N(S)/N(R) */
- nfc_llcp_set_nrns(sock, pdu);
-
- skb_queue_tail(&local->tx_queue, pdu);
- nr_frames++;
- }
-
- return nr_frames;
-}
-
-static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
- struct sk_buff *skb)
-{
- struct nfc_llcp_sock *llcp_sock;
- struct sock *sk;
- u8 dsap, ssap, ptype, ns, nr;
-
- ptype = nfc_llcp_ptype(skb);
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
- ns = nfc_llcp_ns(skb);
- nr = nfc_llcp_nr(skb);
-
- pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
-
- llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
- if (llcp_sock == NULL) {
- nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
- return;
- }
-
- sk = &llcp_sock->sk;
- lock_sock(sk);
- if (sk->sk_state == LLCP_CLOSED) {
- release_sock(sk);
- nfc_llcp_sock_put(llcp_sock);
- }
-
- /* Pass the payload upstream */
- if (ptype == LLCP_PDU_I) {
- pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
-
- if (ns == llcp_sock->recv_n)
- llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
- else
- pr_err("Received out of sequence I PDU\n");
-
- skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
- if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
- /*
- * I frames will be freed from the socket layer, so we
- * need to keep them alive until someone receives them.
- */
- skb_get(skb);
- } else {
- pr_err("Receive queue is full\n");
- }
- }
-
- /* Remove skbs from the pending queue */
- if (llcp_sock->send_ack_n != nr) {
- struct sk_buff *s, *tmp;
- u8 n;
-
- llcp_sock->send_ack_n = nr;
-
- /* Remove and free all skbs until ns == nr */
- skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
- n = nfc_llcp_ns(s);
-
- skb_unlink(s, &llcp_sock->tx_pending_queue);
- kfree_skb(s);
-
- if (n == nr)
- break;
- }
-
- /* Re-queue the remaining skbs for transmission */
- skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
- s, tmp) {
- skb_unlink(s, &llcp_sock->tx_pending_queue);
- skb_queue_head(&local->tx_queue, s);
- }
- }
-
- if (ptype == LLCP_PDU_RR)
- llcp_sock->remote_ready = true;
- else if (ptype == LLCP_PDU_RNR)
- llcp_sock->remote_ready = false;
-
- if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
- nfc_llcp_send_rr(llcp_sock);
-
- release_sock(sk);
- nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
- struct sk_buff *skb)
-{
- struct nfc_llcp_sock *llcp_sock;
- struct sock *sk;
- u8 dsap, ssap;
-
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
-
- if ((dsap == 0) && (ssap == 0)) {
- pr_debug("Connection termination");
- nfc_dep_link_down(local->dev);
- return;
- }
-
- llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
- if (llcp_sock == NULL) {
- nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
- return;
- }
-
- sk = &llcp_sock->sk;
- lock_sock(sk);
-
- nfc_llcp_socket_purge(llcp_sock);
-
- if (sk->sk_state == LLCP_CLOSED) {
- release_sock(sk);
- nfc_llcp_sock_put(llcp_sock);
- }
-
- if (sk->sk_state == LLCP_CONNECTED) {
- nfc_put_device(local->dev);
- sk->sk_state = LLCP_CLOSED;
- sk->sk_state_change(sk);
- }
-
- nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
-
- release_sock(sk);
- nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
- struct nfc_llcp_sock *llcp_sock;
- struct sock *sk;
- u8 dsap, ssap;
-
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
-
- llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
- if (llcp_sock == NULL) {
- pr_err("Invalid CC\n");
- nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
-
- return;
- }
-
- sk = &llcp_sock->sk;
-
- /* Unlink from connecting and link to the client array */
- nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
- nfc_llcp_sock_link(&local->sockets, sk);
- llcp_sock->dsap = ssap;
-
- nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
- skb->len - LLCP_HEADER_SIZE);
-
- sk->sk_state = LLCP_CONNECTED;
- sk->sk_state_change(sk);
-
- nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
- struct nfc_llcp_sock *llcp_sock;
- struct sock *sk;
- u8 dsap, ssap, reason;
-
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
- reason = skb->data[2];
-
- pr_debug("%d %d reason %d\n", ssap, dsap, reason);
-
- switch (reason) {
- case LLCP_DM_NOBOUND:
- case LLCP_DM_REJ:
- llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
- break;
-
- default:
- llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
- break;
- }
-
- if (llcp_sock == NULL) {
- pr_debug("Already closed\n");
- return;
- }
-
- sk = &llcp_sock->sk;
-
- sk->sk_err = ENXIO;
- sk->sk_state = LLCP_CLOSED;
- sk->sk_state_change(sk);
-
- nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
- struct sk_buff *skb)
-{
- struct nfc_llcp_sock *llcp_sock;
- u8 dsap, ssap, *tlv, type, length, tid, sap;
- u16 tlv_len, offset;
- char *service_name;
- size_t service_name_len;
- struct nfc_llcp_sdp_tlv *sdp;
- HLIST_HEAD(llc_sdres_list);
- size_t sdres_tlvs_len;
- HLIST_HEAD(nl_sdres_list);
-
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
-
- pr_debug("%d %d\n", dsap, ssap);
-
- if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
- pr_err("Wrong SNL SAP\n");
- return;
- }
-
- tlv = &skb->data[LLCP_HEADER_SIZE];
- tlv_len = skb->len - LLCP_HEADER_SIZE;
- offset = 0;
- sdres_tlvs_len = 0;
-
- while (offset < tlv_len) {
- type = tlv[0];
- length = tlv[1];
-
- switch (type) {
- case LLCP_TLV_SDREQ:
- tid = tlv[2];
- service_name = (char *) &tlv[3];
- service_name_len = length - 1;
-
- pr_debug("Looking for %.16s\n", service_name);
-
- if (service_name_len == strlen("urn:nfc:sn:sdp") &&
- !strncmp(service_name, "urn:nfc:sn:sdp",
- service_name_len)) {
- sap = 1;
- goto add_snl;
- }
-
- llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
- service_name_len);
- if (!llcp_sock) {
- sap = 0;
- goto add_snl;
- }
-
- /*
- * We found a socket but its ssap has not been reserved
- * yet. We need to assign it for good and send a reply.
- * The ssap will be freed when the socket is closed.
- */
- if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
- atomic_t *client_count;
-
- sap = nfc_llcp_reserve_sdp_ssap(local);
-
- pr_debug("Reserving %d\n", sap);
-
- if (sap == LLCP_SAP_MAX) {
- sap = 0;
- goto add_snl;
- }
-
- client_count =
- &local->local_sdp_cnt[sap -
- LLCP_WKS_NUM_SAP];
-
- atomic_inc(client_count);
-
- llcp_sock->ssap = sap;
- llcp_sock->reserved_ssap = sap;
- } else {
- sap = llcp_sock->ssap;
- }
-
- pr_debug("%p %d\n", llcp_sock, sap);
-
-add_snl:
- sdp = nfc_llcp_build_sdres_tlv(tid, sap);
- if (sdp == NULL)
- goto exit;
-
- sdres_tlvs_len += sdp->tlv_len;
- hlist_add_head(&sdp->node, &llc_sdres_list);
- break;
-
- case LLCP_TLV_SDRES:
- mutex_lock(&local->sdreq_lock);
-
- pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
-
- hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
- if (sdp->tid != tlv[2])
- continue;
-
- sdp->sap = tlv[3];
-
- pr_debug("Found: uri=%s, sap=%d\n",
- sdp->uri, sdp->sap);
-
- hlist_del(&sdp->node);
-
- hlist_add_head(&sdp->node, &nl_sdres_list);
-
- break;
- }
-
- mutex_unlock(&local->sdreq_lock);
- break;
-
- default:
- pr_err("Invalid SNL tlv value 0x%x\n", type);
- break;
- }
-
- offset += length + 2;
- tlv += length + 2;
- }
-
-exit:
- if (!hlist_empty(&nl_sdres_list))
- nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
-
- if (!hlist_empty(&llc_sdres_list))
- nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
-}
-
-static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
- u8 ptype;
- u16 pdu_len;
- struct sk_buff *new_skb;
-
- if (skb->len <= LLCP_HEADER_SIZE) {
- pr_err("Malformed AGF PDU\n");
- return;
- }
-
- skb_pull(skb, LLCP_HEADER_SIZE);
-
- while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
- pdu_len = skb->data[0] << 8 | skb->data[1];
-
- skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
-
- if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
- pr_err("Malformed AGF PDU\n");
- return;
- }
-
- ptype = nfc_llcp_ptype(skb);
-
- if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
- goto next;
-
- new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
- if (new_skb == NULL) {
- pr_err("Could not allocate PDU\n");
- return;
- }
-
- memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
-
- nfc_llcp_rx_skb(local, new_skb);
-
- kfree_skb(new_skb);
-next:
- skb_pull(skb, pdu_len);
- }
-}
-
-static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
- u8 dsap, ssap, ptype;
-
- ptype = nfc_llcp_ptype(skb);
- dsap = nfc_llcp_dsap(skb);
- ssap = nfc_llcp_ssap(skb);
-
- pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
-
- if (ptype != LLCP_PDU_SYMM)
- print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
- 16, 1, skb->data, skb->len, true);
-
- switch (ptype) {
- case LLCP_PDU_SYMM:
- pr_debug("SYMM\n");
- break;
-
- case LLCP_PDU_UI:
- pr_debug("UI\n");
- nfc_llcp_recv_ui(local, skb);
- break;
-
- case LLCP_PDU_CONNECT:
- pr_debug("CONNECT\n");
- nfc_llcp_recv_connect(local, skb);
- break;
-
- case LLCP_PDU_DISC:
- pr_debug("DISC\n");
- nfc_llcp_recv_disc(local, skb);
- break;
-
- case LLCP_PDU_CC:
- pr_debug("CC\n");
- nfc_llcp_recv_cc(local, skb);
- break;
-
- case LLCP_PDU_DM:
- pr_debug("DM\n");
- nfc_llcp_recv_dm(local, skb);
- break;
-
- case LLCP_PDU_SNL:
- pr_debug("SNL\n");
- nfc_llcp_recv_snl(local, skb);
- break;
-
- case LLCP_PDU_I:
- case LLCP_PDU_RR:
- case LLCP_PDU_RNR:
- pr_debug("I frame\n");
- nfc_llcp_recv_hdlc(local, skb);
- break;
-
- case LLCP_PDU_AGF:
- pr_debug("AGF frame\n");
- nfc_llcp_recv_agf(local, skb);
- break;
- }
-}
-
-static void nfc_llcp_rx_work(struct work_struct *work)
-{
- struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
- rx_work);
- struct sk_buff *skb;
-
- skb = local->rx_pending;
- if (skb == NULL) {
- pr_debug("No pending SKB\n");
- return;
- }
-
- __net_timestamp(skb);
-
- nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
-
- nfc_llcp_rx_skb(local, skb);
-
- schedule_work(&local->tx_work);
- kfree_skb(local->rx_pending);
- local->rx_pending = NULL;
-}
-
-static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
- local->rx_pending = skb;
- del_timer(&local->link_timer);
- schedule_work(&local->rx_work);
-}
-
-void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
-{
- struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
-
- pr_debug("Received an LLCP PDU\n");
- if (err < 0) {
- pr_err("err %d\n", err);
- return;
- }
-
- __nfc_llcp_recv(local, skb);
-}
-
-int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
-{
- struct nfc_llcp_local *local;
-
- local = nfc_llcp_find_local(dev);
- if (local == NULL)
- return -ENODEV;
-
- __nfc_llcp_recv(local, skb);
-
- return 0;
-}
-
-void nfc_llcp_mac_is_down(struct nfc_dev *dev)
-{
- struct nfc_llcp_local *local;
-
- local = nfc_llcp_find_local(dev);
- if (local == NULL)
- return;
-
- local->remote_miu = LLCP_DEFAULT_MIU;
- local->remote_lto = LLCP_DEFAULT_LTO;
-
- /* Close and purge all existing sockets */
- nfc_llcp_socket_release(local, true, 0);
-}
-
-void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
- u8 comm_mode, u8 rf_mode)
-{
- struct nfc_llcp_local *local;
-
- pr_debug("rf mode %d\n", rf_mode);
-
- local = nfc_llcp_find_local(dev);
- if (local == NULL)
- return;
-
- local->target_idx = target_idx;
- local->comm_mode = comm_mode;
- local->rf_mode = rf_mode;
-
- if (rf_mode == NFC_RF_INITIATOR) {
- pr_debug("Queueing Tx work\n");
-
- schedule_work(&local->tx_work);
- } else {
- mod_timer(&local->link_timer,
- jiffies + msecs_to_jiffies(local->remote_lto));
- }
-}
-
-int nfc_llcp_register_device(struct nfc_dev *ndev)
-{
- struct nfc_llcp_local *local;
-
- local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
- if (local == NULL)
- return -ENOMEM;
-
- local->dev = ndev;
- INIT_LIST_HEAD(&local->list);
- kref_init(&local->ref);
- mutex_init(&local->sdp_lock);
- init_timer(&local->link_timer);
- local->link_timer.data = (unsigned long) local;
- local->link_timer.function = nfc_llcp_symm_timer;
-
- skb_queue_head_init(&local->tx_queue);
- INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
-
- local->rx_pending = NULL;
- INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
-
- INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
-
- rwlock_init(&local->sockets.lock);
- rwlock_init(&local->connecting_sockets.lock);
- rwlock_init(&local->raw_sockets.lock);
-
- local->lto = 150; /* 1500 ms */
- local->rw = LLCP_MAX_RW;
- local->miux = cpu_to_be16(LLCP_MAX_MIUX);
-
- nfc_llcp_build_gb(local);
-
- local->remote_miu = LLCP_DEFAULT_MIU;
- local->remote_lto = LLCP_DEFAULT_LTO;
-
- mutex_init(&local->sdreq_lock);
- INIT_HLIST_HEAD(&local->pending_sdreqs);
- init_timer(&local->sdreq_timer);
- local->sdreq_timer.data = (unsigned long) local;
- local->sdreq_timer.function = nfc_llcp_sdreq_timer;
- INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
-
- list_add(&local->list, &llcp_devices);
-
- return 0;
-}
-
-void nfc_llcp_unregister_device(struct nfc_dev *dev)
-{
- struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
-
- if (local == NULL) {
- pr_debug("No such device\n");
- return;
- }
-
- local_cleanup(local);
-
- nfc_llcp_local_put(local);
-}
-
-int __init nfc_llcp_init(void)
-{
- INIT_LIST_HEAD(&llcp_devices);
-
- return nfc_llcp_sock_init();
-}
-
-void nfc_llcp_exit(void)
-{
- nfc_llcp_sock_exit();
-}
+++ /dev/null
-/*
- * 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);
+++ /dev/null
-/*
- * 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);
-}
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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, <o_length);
+ gb_len += lto_length;
+
+ pr_debug("Local wks 0x%lx\n", local->local_wks);
+ wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
+ &wks_length);
+ gb_len += wks_length;
+
+ miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
+ &miux_length);
+ gb_len += miux_length;
+
+ gb_len += ARRAY_SIZE(llcp_magic);
+
+ if (gb_len > NFC_MAX_GT_LEN) {
+ ret = -EINVAL;
+ goto out;
+ }
+
+ gb_cur = local->gb;
+
+ memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
+ gb_cur += ARRAY_SIZE(llcp_magic);
+
+ memcpy(gb_cur, version_tlv, version_length);
+ gb_cur += version_length;
+
+ memcpy(gb_cur, lto_tlv, lto_length);
+ gb_cur += lto_length;
+
+ memcpy(gb_cur, wks_tlv, wks_length);
+ gb_cur += wks_length;
+
+ memcpy(gb_cur, miux_tlv, miux_length);
+ gb_cur += miux_length;
+
+ local->gb_len = gb_len;
+
+out:
+ kfree(version_tlv);
+ kfree(lto_tlv);
+ kfree(wks_tlv);
+ kfree(miux_tlv);
+
+ return ret;
+}
+
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL) {
+ *general_bytes_len = 0;
+ return NULL;
+ }
+
+ nfc_llcp_build_gb(local);
+
+ *general_bytes_len = local->gb_len;
+
+ return local->gb;
+}
+
+int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
+{
+ struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+ if (local == NULL) {
+ pr_err("No LLCP device\n");
+ return -ENODEV;
+ }
+ if (gb_len < 3)
+ return -EINVAL;
+
+ memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
+ memcpy(local->remote_gb, gb, gb_len);
+ local->remote_gb_len = gb_len;
+
+ if (memcmp(local->remote_gb, llcp_magic, 3)) {
+ pr_err("MAC does not support LLCP\n");
+ return -EINVAL;
+ }
+
+ return nfc_llcp_parse_gb_tlv(local,
+ &local->remote_gb[3],
+ local->remote_gb_len - 3);
+}
+
+static u8 nfc_llcp_dsap(struct sk_buff *pdu)
+{
+ return (pdu->data[0] & 0xfc) >> 2;
+}
+
+static u8 nfc_llcp_ptype(struct sk_buff *pdu)
+{
+ return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
+}
+
+static u8 nfc_llcp_ssap(struct sk_buff *pdu)
+{
+ return pdu->data[1] & 0x3f;
+}
+
+static u8 nfc_llcp_ns(struct sk_buff *pdu)
+{
+ return pdu->data[2] >> 4;
+}
+
+static u8 nfc_llcp_nr(struct sk_buff *pdu)
+{
+ return pdu->data[2] & 0xf;
+}
+
+static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
+{
+ pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
+ sock->send_n = (sock->send_n + 1) % 16;
+ sock->recv_ack_n = (sock->recv_n - 1) % 16;
+}
+
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+ struct sk_buff *skb, u8 direction)
+{
+ struct sk_buff *skb_copy = NULL, *nskb;
+ struct sock *sk;
+ u8 *data;
+
+ read_lock(&local->raw_sockets.lock);
+
+ sk_for_each(sk, &local->raw_sockets.head) {
+ if (sk->sk_state != LLCP_BOUND)
+ continue;
+
+ if (skb_copy == NULL) {
+ skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
+ GFP_ATOMIC);
+
+ if (skb_copy == NULL)
+ continue;
+
+ data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
+
+ data[0] = local->dev ? local->dev->idx : 0xFF;
+ data[1] = direction;
+ }
+
+ nskb = skb_clone(skb_copy, GFP_ATOMIC);
+ if (!nskb)
+ continue;
+
+ if (sock_queue_rcv_skb(sk, nskb))
+ kfree_skb(nskb);
+ }
+
+ read_unlock(&local->raw_sockets.lock);
+
+ kfree_skb(skb_copy);
+}
+
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ tx_work);
+ struct sk_buff *skb;
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
+
+ skb = skb_dequeue(&local->tx_queue);
+ if (skb != NULL) {
+ sk = skb->sk;
+ llcp_sock = nfc_llcp_sock(sk);
+
+ if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+ nfc_llcp_send_symm(local->dev);
+ } else {
+ struct sk_buff *copy_skb = NULL;
+ u8 ptype = nfc_llcp_ptype(skb);
+ int ret;
+
+ pr_debug("Sending pending skb\n");
+ print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
+ DUMP_PREFIX_OFFSET, 16, 1,
+ skb->data, skb->len, true);
+
+ if (ptype == LLCP_PDU_I)
+ copy_skb = skb_copy(skb, GFP_ATOMIC);
+
+ __net_timestamp(skb);
+
+ nfc_llcp_send_to_raw_sock(local, skb,
+ NFC_LLCP_DIRECTION_TX);
+
+ ret = nfc_data_exchange(local->dev, local->target_idx,
+ skb, nfc_llcp_recv, local);
+
+ if (ret) {
+ kfree_skb(copy_skb);
+ goto out;
+ }
+
+ if (ptype == LLCP_PDU_I && copy_skb)
+ skb_queue_tail(&llcp_sock->tx_pending_queue,
+ copy_skb);
+ }
+ } else {
+ nfc_llcp_send_symm(local->dev);
+ }
+
+out:
+ mod_timer(&local->link_timer,
+ jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
+
+static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
+ u8 ssap)
+{
+ struct sock *sk;
+ struct nfc_llcp_sock *llcp_sock;
+
+ read_lock(&local->connecting_sockets.lock);
+
+ sk_for_each(sk, &local->connecting_sockets.head) {
+ llcp_sock = nfc_llcp_sock(sk);
+
+ if (llcp_sock->ssap == ssap) {
+ sock_hold(&llcp_sock->sk);
+ goto out;
+ }
+ }
+
+ llcp_sock = NULL;
+
+out:
+ read_unlock(&local->connecting_sockets.lock);
+
+ return llcp_sock;
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
+ u8 *sn, size_t sn_len)
+{
+ struct nfc_llcp_sock *llcp_sock;
+
+ llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
+
+ if (llcp_sock == NULL)
+ return NULL;
+
+ sock_hold(&llcp_sock->sk);
+
+ return llcp_sock;
+}
+
+static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
+{
+ u8 *tlv = &skb->data[2], type, length;
+ size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
+
+ while (offset < tlv_array_len) {
+ type = tlv[0];
+ length = tlv[1];
+
+ pr_debug("type 0x%x length %d\n", type, length);
+
+ if (type == LLCP_TLV_SN) {
+ *sn_len = length;
+ return &tlv[2];
+ }
+
+ offset += length + 2;
+ tlv += length + 2;
+ }
+
+ return NULL;
+}
+
+static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct nfc_llcp_ui_cb *ui_cb;
+ u8 dsap, ssap;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ ui_cb = nfc_llcp_ui_skb_cb(skb);
+ ui_cb->dsap = dsap;
+ ui_cb->ssap = ssap;
+
+ pr_debug("%d %d\n", dsap, ssap);
+
+ /* We're looking for a bound socket, not a client one */
+ llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+ if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+ return;
+
+ /* There is no sequence with UI frames */
+ skb_pull(skb, LLCP_HEADER_SIZE);
+ if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+ /*
+ * UI frames will be freed from the socket layer, so we
+ * need to keep them alive until someone receives them.
+ */
+ skb_get(skb);
+ } else {
+ pr_err("Receive queue is full\n");
+ }
+
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct sock *new_sk, *parent;
+ struct nfc_llcp_sock *sock, *new_sock;
+ u8 dsap, ssap, reason;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ pr_debug("%d %d\n", dsap, ssap);
+
+ if (dsap != LLCP_SAP_SDP) {
+ sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+ if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
+ }
+ } else {
+ u8 *sn;
+ size_t sn_len;
+
+ sn = nfc_llcp_connect_sn(skb, &sn_len);
+ if (sn == NULL) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
+ }
+
+ pr_debug("Service name length %zu\n", sn_len);
+
+ sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
+ if (sock == NULL) {
+ reason = LLCP_DM_NOBOUND;
+ goto fail;
+ }
+ }
+
+ lock_sock(&sock->sk);
+
+ parent = &sock->sk;
+
+ if (sk_acceptq_is_full(parent)) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ if (sock->ssap == LLCP_SDP_UNBOUND) {
+ u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
+
+ pr_debug("First client, reserving %d\n", ssap);
+
+ if (ssap == LLCP_SAP_MAX) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ sock->ssap = ssap;
+ }
+
+ new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
+ if (new_sk == NULL) {
+ reason = LLCP_DM_REJ;
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+ goto fail;
+ }
+
+ new_sock = nfc_llcp_sock(new_sk);
+ new_sock->dev = local->dev;
+ new_sock->local = nfc_llcp_local_get(local);
+ new_sock->rw = sock->rw;
+ new_sock->miux = sock->miux;
+ new_sock->remote_miu = local->remote_miu;
+ new_sock->nfc_protocol = sock->nfc_protocol;
+ new_sock->dsap = ssap;
+ new_sock->target_idx = local->target_idx;
+ new_sock->parent = parent;
+ new_sock->ssap = sock->ssap;
+ if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
+ atomic_t *client_count;
+
+ pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
+
+ client_count =
+ &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
+
+ atomic_inc(client_count);
+ new_sock->reserved_ssap = sock->ssap;
+ }
+
+ nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
+ skb->len - LLCP_HEADER_SIZE);
+
+ pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
+
+ nfc_llcp_sock_link(&local->sockets, new_sk);
+
+ nfc_llcp_accept_enqueue(&sock->sk, new_sk);
+
+ nfc_get_device(local->dev->idx);
+
+ new_sk->sk_state = LLCP_CONNECTED;
+
+ /* Wake the listening processes */
+ parent->sk_data_ready(parent, 0);
+
+ /* Send CC */
+ nfc_llcp_send_cc(new_sock);
+
+ release_sock(&sock->sk);
+ sock_put(&sock->sk);
+
+ return;
+
+fail:
+ /* Send DM */
+ nfc_llcp_send_dm(local, dsap, ssap, reason);
+}
+
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
+{
+ int nr_frames = 0;
+ struct nfc_llcp_local *local = sock->local;
+
+ pr_debug("Remote ready %d tx queue len %d remote rw %d",
+ sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
+ sock->remote_rw);
+
+ /* Try to queue some I frames for transmission */
+ while (sock->remote_ready &&
+ skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
+ struct sk_buff *pdu;
+
+ pdu = skb_dequeue(&sock->tx_queue);
+ if (pdu == NULL)
+ break;
+
+ /* Update N(S)/N(R) */
+ nfc_llcp_set_nrns(sock, pdu);
+
+ skb_queue_tail(&local->tx_queue, pdu);
+ nr_frames++;
+ }
+
+ return nr_frames;
+}
+
+static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap, ptype, ns, nr;
+
+ ptype = nfc_llcp_ptype(skb);
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+ ns = nfc_llcp_ns(skb);
+ nr = nfc_llcp_nr(skb);
+
+ pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
+
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ if (llcp_sock == NULL) {
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+ lock_sock(sk);
+ if (sk->sk_state == LLCP_CLOSED) {
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+ }
+
+ /* Pass the payload upstream */
+ if (ptype == LLCP_PDU_I) {
+ pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
+
+ if (ns == llcp_sock->recv_n)
+ llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
+ else
+ pr_err("Received out of sequence I PDU\n");
+
+ skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
+ if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+ /*
+ * I frames will be freed from the socket layer, so we
+ * need to keep them alive until someone receives them.
+ */
+ skb_get(skb);
+ } else {
+ pr_err("Receive queue is full\n");
+ }
+ }
+
+ /* Remove skbs from the pending queue */
+ if (llcp_sock->send_ack_n != nr) {
+ struct sk_buff *s, *tmp;
+ u8 n;
+
+ llcp_sock->send_ack_n = nr;
+
+ /* Remove and free all skbs until ns == nr */
+ skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+ n = nfc_llcp_ns(s);
+
+ skb_unlink(s, &llcp_sock->tx_pending_queue);
+ kfree_skb(s);
+
+ if (n == nr)
+ break;
+ }
+
+ /* Re-queue the remaining skbs for transmission */
+ skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+ s, tmp) {
+ skb_unlink(s, &llcp_sock->tx_pending_queue);
+ skb_queue_head(&local->tx_queue, s);
+ }
+ }
+
+ if (ptype == LLCP_PDU_RR)
+ llcp_sock->remote_ready = true;
+ else if (ptype == LLCP_PDU_RNR)
+ llcp_sock->remote_ready = false;
+
+ if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
+ nfc_llcp_send_rr(llcp_sock);
+
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ if ((dsap == 0) && (ssap == 0)) {
+ pr_debug("Connection termination");
+ nfc_dep_link_down(local->dev);
+ return;
+ }
+
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ if (llcp_sock == NULL) {
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+ lock_sock(sk);
+
+ nfc_llcp_socket_purge(llcp_sock);
+
+ if (sk->sk_state == LLCP_CLOSED) {
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+ }
+
+ if (sk->sk_state == LLCP_CONNECTED) {
+ nfc_put_device(local->dev);
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+ }
+
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
+
+ release_sock(sk);
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+ if (llcp_sock == NULL) {
+ pr_err("Invalid CC\n");
+ nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+
+ /* Unlink from connecting and link to the client array */
+ nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+ nfc_llcp_sock_link(&local->sockets, sk);
+ llcp_sock->dsap = ssap;
+
+ nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
+ skb->len - LLCP_HEADER_SIZE);
+
+ sk->sk_state = LLCP_CONNECTED;
+ sk->sk_state_change(sk);
+
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ struct sock *sk;
+ u8 dsap, ssap, reason;
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+ reason = skb->data[2];
+
+ pr_debug("%d %d reason %d\n", ssap, dsap, reason);
+
+ switch (reason) {
+ case LLCP_DM_NOBOUND:
+ case LLCP_DM_REJ:
+ llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+ break;
+
+ default:
+ llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+ break;
+ }
+
+ if (llcp_sock == NULL) {
+ pr_debug("Already closed\n");
+ return;
+ }
+
+ sk = &llcp_sock->sk;
+
+ sk->sk_err = ENXIO;
+ sk->sk_state = LLCP_CLOSED;
+ sk->sk_state_change(sk);
+
+ nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+ struct sk_buff *skb)
+{
+ struct nfc_llcp_sock *llcp_sock;
+ u8 dsap, ssap, *tlv, type, length, tid, sap;
+ u16 tlv_len, offset;
+ char *service_name;
+ size_t service_name_len;
+ struct nfc_llcp_sdp_tlv *sdp;
+ HLIST_HEAD(llc_sdres_list);
+ size_t sdres_tlvs_len;
+ HLIST_HEAD(nl_sdres_list);
+
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ pr_debug("%d %d\n", dsap, ssap);
+
+ if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+ pr_err("Wrong SNL SAP\n");
+ return;
+ }
+
+ tlv = &skb->data[LLCP_HEADER_SIZE];
+ tlv_len = skb->len - LLCP_HEADER_SIZE;
+ offset = 0;
+ sdres_tlvs_len = 0;
+
+ while (offset < tlv_len) {
+ type = tlv[0];
+ length = tlv[1];
+
+ switch (type) {
+ case LLCP_TLV_SDREQ:
+ tid = tlv[2];
+ service_name = (char *) &tlv[3];
+ service_name_len = length - 1;
+
+ pr_debug("Looking for %.16s\n", service_name);
+
+ if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+ !strncmp(service_name, "urn:nfc:sn:sdp",
+ service_name_len)) {
+ sap = 1;
+ goto add_snl;
+ }
+
+ llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+ service_name_len);
+ if (!llcp_sock) {
+ sap = 0;
+ goto add_snl;
+ }
+
+ /*
+ * We found a socket but its ssap has not been reserved
+ * yet. We need to assign it for good and send a reply.
+ * The ssap will be freed when the socket is closed.
+ */
+ if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+ atomic_t *client_count;
+
+ sap = nfc_llcp_reserve_sdp_ssap(local);
+
+ pr_debug("Reserving %d\n", sap);
+
+ if (sap == LLCP_SAP_MAX) {
+ sap = 0;
+ goto add_snl;
+ }
+
+ client_count =
+ &local->local_sdp_cnt[sap -
+ LLCP_WKS_NUM_SAP];
+
+ atomic_inc(client_count);
+
+ llcp_sock->ssap = sap;
+ llcp_sock->reserved_ssap = sap;
+ } else {
+ sap = llcp_sock->ssap;
+ }
+
+ pr_debug("%p %d\n", llcp_sock, sap);
+
+add_snl:
+ sdp = nfc_llcp_build_sdres_tlv(tid, sap);
+ if (sdp == NULL)
+ goto exit;
+
+ sdres_tlvs_len += sdp->tlv_len;
+ hlist_add_head(&sdp->node, &llc_sdres_list);
+ break;
+
+ case LLCP_TLV_SDRES:
+ mutex_lock(&local->sdreq_lock);
+
+ pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
+
+ hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
+ if (sdp->tid != tlv[2])
+ continue;
+
+ sdp->sap = tlv[3];
+
+ pr_debug("Found: uri=%s, sap=%d\n",
+ sdp->uri, sdp->sap);
+
+ hlist_del(&sdp->node);
+
+ hlist_add_head(&sdp->node, &nl_sdres_list);
+
+ break;
+ }
+
+ mutex_unlock(&local->sdreq_lock);
+ break;
+
+ default:
+ pr_err("Invalid SNL tlv value 0x%x\n", type);
+ break;
+ }
+
+ offset += length + 2;
+ tlv += length + 2;
+ }
+
+exit:
+ if (!hlist_empty(&nl_sdres_list))
+ nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+
+ if (!hlist_empty(&llc_sdres_list))
+ nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
+}
+
+static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ u8 ptype;
+ u16 pdu_len;
+ struct sk_buff *new_skb;
+
+ if (skb->len <= LLCP_HEADER_SIZE) {
+ pr_err("Malformed AGF PDU\n");
+ return;
+ }
+
+ skb_pull(skb, LLCP_HEADER_SIZE);
+
+ while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
+ pdu_len = skb->data[0] << 8 | skb->data[1];
+
+ skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
+
+ if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
+ pr_err("Malformed AGF PDU\n");
+ return;
+ }
+
+ ptype = nfc_llcp_ptype(skb);
+
+ if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
+ goto next;
+
+ new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
+ if (new_skb == NULL) {
+ pr_err("Could not allocate PDU\n");
+ return;
+ }
+
+ memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
+
+ nfc_llcp_rx_skb(local, new_skb);
+
+ kfree_skb(new_skb);
+next:
+ skb_pull(skb, pdu_len);
+ }
+}
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ u8 dsap, ssap, ptype;
+
+ ptype = nfc_llcp_ptype(skb);
+ dsap = nfc_llcp_dsap(skb);
+ ssap = nfc_llcp_ssap(skb);
+
+ pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+ if (ptype != LLCP_PDU_SYMM)
+ print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
+ 16, 1, skb->data, skb->len, true);
+
+ switch (ptype) {
+ case LLCP_PDU_SYMM:
+ pr_debug("SYMM\n");
+ break;
+
+ case LLCP_PDU_UI:
+ pr_debug("UI\n");
+ nfc_llcp_recv_ui(local, skb);
+ break;
+
+ case LLCP_PDU_CONNECT:
+ pr_debug("CONNECT\n");
+ nfc_llcp_recv_connect(local, skb);
+ break;
+
+ case LLCP_PDU_DISC:
+ pr_debug("DISC\n");
+ nfc_llcp_recv_disc(local, skb);
+ break;
+
+ case LLCP_PDU_CC:
+ pr_debug("CC\n");
+ nfc_llcp_recv_cc(local, skb);
+ break;
+
+ case LLCP_PDU_DM:
+ pr_debug("DM\n");
+ nfc_llcp_recv_dm(local, skb);
+ break;
+
+ case LLCP_PDU_SNL:
+ pr_debug("SNL\n");
+ nfc_llcp_recv_snl(local, skb);
+ break;
+
+ case LLCP_PDU_I:
+ case LLCP_PDU_RR:
+ case LLCP_PDU_RNR:
+ pr_debug("I frame\n");
+ nfc_llcp_recv_hdlc(local, skb);
+ break;
+
+ case LLCP_PDU_AGF:
+ pr_debug("AGF frame\n");
+ nfc_llcp_recv_agf(local, skb);
+ break;
+ }
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+ struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+ rx_work);
+ struct sk_buff *skb;
+
+ skb = local->rx_pending;
+ if (skb == NULL) {
+ pr_debug("No pending SKB\n");
+ return;
+ }
+
+ __net_timestamp(skb);
+
+ nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
+ nfc_llcp_rx_skb(local, skb);
+
+ schedule_work(&local->tx_work);
+ kfree_skb(local->rx_pending);
+ local->rx_pending = NULL;
+}
+
+static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+ local->rx_pending = skb;
+ del_timer(&local->link_timer);
+ schedule_work(&local->rx_work);
+}
+
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
+{
+ struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+ pr_debug("Received an LLCP PDU\n");
+ if (err < 0) {
+ pr_err("err %d\n", err);
+ return;
+ }
+
+ __nfc_llcp_recv(local, skb);
+}
+
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL)
+ return -ENODEV;
+
+ __nfc_llcp_recv(local, skb);
+
+ return 0;
+}
+
+void nfc_llcp_mac_is_down(struct nfc_dev *dev)
+{
+ struct nfc_llcp_local *local;
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL)
+ return;
+
+ local->remote_miu = LLCP_DEFAULT_MIU;
+ local->remote_lto = LLCP_DEFAULT_LTO;
+
+ /* Close and purge all existing sockets */
+ nfc_llcp_socket_release(local, true, 0);
+}
+
+void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+ u8 comm_mode, u8 rf_mode)
+{
+ struct nfc_llcp_local *local;
+
+ pr_debug("rf mode %d\n", rf_mode);
+
+ local = nfc_llcp_find_local(dev);
+ if (local == NULL)
+ return;
+
+ local->target_idx = target_idx;
+ local->comm_mode = comm_mode;
+ local->rf_mode = rf_mode;
+
+ if (rf_mode == NFC_RF_INITIATOR) {
+ pr_debug("Queueing Tx work\n");
+
+ schedule_work(&local->tx_work);
+ } else {
+ mod_timer(&local->link_timer,
+ jiffies + msecs_to_jiffies(local->remote_lto));
+ }
+}
+
+int nfc_llcp_register_device(struct nfc_dev *ndev)
+{
+ struct nfc_llcp_local *local;
+
+ local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
+ if (local == NULL)
+ return -ENOMEM;
+
+ local->dev = ndev;
+ INIT_LIST_HEAD(&local->list);
+ kref_init(&local->ref);
+ mutex_init(&local->sdp_lock);
+ init_timer(&local->link_timer);
+ local->link_timer.data = (unsigned long) local;
+ local->link_timer.function = nfc_llcp_symm_timer;
+
+ skb_queue_head_init(&local->tx_queue);
+ INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
+
+ local->rx_pending = NULL;
+ INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
+
+ INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
+
+ rwlock_init(&local->sockets.lock);
+ rwlock_init(&local->connecting_sockets.lock);
+ rwlock_init(&local->raw_sockets.lock);
+
+ local->lto = 150; /* 1500 ms */
+ local->rw = LLCP_MAX_RW;
+ local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+
+ nfc_llcp_build_gb(local);
+
+ local->remote_miu = LLCP_DEFAULT_MIU;
+ local->remote_lto = LLCP_DEFAULT_LTO;
+
+ mutex_init(&local->sdreq_lock);
+ INIT_HLIST_HEAD(&local->pending_sdreqs);
+ init_timer(&local->sdreq_timer);
+ local->sdreq_timer.data = (unsigned long) local;
+ local->sdreq_timer.function = nfc_llcp_sdreq_timer;
+ INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
+
+ list_add(&local->list, &llcp_devices);
+
+ return 0;
+}
+
+void nfc_llcp_unregister_device(struct nfc_dev *dev)
+{
+ struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+ if (local == NULL) {
+ pr_debug("No such device\n");
+ return;
+ }
+
+ local_cleanup(local);
+
+ nfc_llcp_local_put(local);
+}
+
+int __init nfc_llcp_init(void)
+{
+ INIT_LIST_HEAD(&llcp_devices);
+
+ return nfc_llcp_sock_init();
+}
+
+void nfc_llcp_exit(void)
+{
+ nfc_llcp_sock_exit();
+}
--- /dev/null
+/*
+ * 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);
+}
#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,
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);
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);
projects / GitHub / LineageOS / android_kernel_motorola_exynos9610.git / commitdiff