From 63cd353c34a08af2d1935f8d0c2b6b091714ff79 Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Wed, 13 Feb 2013 10:45:46 +0100 Subject: [PATCH 1/9] NFC: microread: Fix MEI build failure The mei_device field should be called device, not mei_device. Signed-off-by: Samuel Ortiz --- drivers/nfc/microread/mei.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/nfc/microread/mei.c b/drivers/nfc/microread/mei.c index eef38cfd812e..13bde92b1e29 100644 --- a/drivers/nfc/microread/mei.c +++ b/drivers/nfc/microread/mei.c @@ -48,7 +48,7 @@ struct mei_nfc_hdr { #define MEI_NFC_MAX_READ (MEI_NFC_HEADER_SIZE + MEI_NFC_MAX_HCI_PAYLOAD) struct microread_mei_phy { - struct mei_device *mei_device; + struct mei_device *device; struct nfc_hci_dev *hdev; int powered; From e4306bec47fc02178c612879c848d3a6544424dd Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Fri, 22 Feb 2013 01:12:28 +0100 Subject: [PATCH 2/9] NFC: llcp: Rename socket rw and miu fields They really are remote peer parameters, and we need to distinguish them from the local ones as we'll modify the latter with socket options. Signed-off-by: Samuel Ortiz --- net/nfc/llcp/commands.c | 17 +++++++++-------- net/nfc/llcp/llcp.c | 6 +++--- net/nfc/llcp/llcp.h | 6 ++++-- net/nfc/llcp/sock.c | 6 +++--- 4 files changed, 19 insertions(+), 16 deletions(-) diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index c6bc3bd95052..c8a209665792 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c @@ -184,10 +184,10 @@ int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, switch (type) { case LLCP_TLV_MIUX: - sock->miu = llcp_tlv_miux(tlv) + 128; + sock->remote_miu = llcp_tlv_miux(tlv) + 128; break; case LLCP_TLV_RW: - sock->rw = llcp_tlv_rw(tlv); + sock->remote_rw = llcp_tlv_rw(tlv); break; case LLCP_TLV_SN: break; @@ -200,7 +200,8 @@ int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, tlv += length + 2; } - pr_debug("sock %p rw %d miu %d\n", sock, sock->rw, sock->miu); + pr_debug("sock %p rw %d miu %d\n", sock, + sock->remote_rw, sock->remote_miu); return 0; } @@ -532,8 +533,8 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, /* Remote is ready but has not acknowledged our frames */ if((sock->remote_ready && - skb_queue_len(&sock->tx_pending_queue) >= sock->rw && - skb_queue_len(&sock->tx_queue) >= 2 * sock->rw)) { + 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; @@ -541,7 +542,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, /* Remote is not ready and we've been queueing enough frames */ if ((!sock->remote_ready && - skb_queue_len(&sock->tx_queue) >= 2 * sock->rw)) { + 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; @@ -561,7 +562,7 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock, while (remaining_len > 0) { - frag_len = min_t(size_t, sock->miu, remaining_len); + frag_len = min_t(size_t, sock->remote_miu, remaining_len); pr_debug("Fragment %zd bytes remaining %zd", frag_len, remaining_len); @@ -621,7 +622,7 @@ int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap, while (remaining_len > 0) { - frag_len = min_t(size_t, sock->miu, remaining_len); + frag_len = min_t(size_t, sock->remote_miu, remaining_len); pr_debug("Fragment %zd bytes remaining %zd", frag_len, remaining_len); diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 7f8266dd14cb..c0048b2395dd 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -865,7 +865,7 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, new_sock = nfc_llcp_sock(new_sk); new_sock->dev = local->dev; new_sock->local = nfc_llcp_local_get(local); - new_sock->miu = local->remote_miu; + 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; @@ -919,11 +919,11 @@ int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock) pr_debug("Remote ready %d tx queue len %d remote rw %d", sock->remote_ready, skb_queue_len(&sock->tx_pending_queue), - sock->rw); + sock->remote_rw); /* Try to queue some I frames for transmission */ while (sock->remote_ready && - skb_queue_len(&sock->tx_pending_queue) < sock->rw) { + skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) { struct sk_buff *pdu; pdu = skb_dequeue(&sock->tx_queue); diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index 0eae5c509504..32cec81939e6 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -104,8 +104,10 @@ struct nfc_llcp_sock { u8 dsap; char *service_name; size_t service_name_len; - u8 rw; - u16 miu; + + /* Remote link parameters */ + u8 remote_rw; + u16 remote_miu; /* Link variables */ u8 send_n; diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c index 5332751943a9..cc564992ba95 100644 --- a/net/nfc/llcp/sock.c +++ b/net/nfc/llcp/sock.c @@ -541,7 +541,7 @@ static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr, llcp_sock->dev = dev; llcp_sock->local = nfc_llcp_local_get(local); - llcp_sock->miu = llcp_sock->local->remote_miu; + 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; @@ -800,8 +800,8 @@ struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp) llcp_sock->ssap = 0; llcp_sock->dsap = LLCP_SAP_SDP; - llcp_sock->rw = LLCP_DEFAULT_RW; - llcp_sock->miu = LLCP_DEFAULT_MIU; + llcp_sock->remote_rw = LLCP_DEFAULT_RW; + llcp_sock->remote_miu = LLCP_DEFAULT_MIU; 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; From 26fd76cab2e61cedc5c25f7151fb31b57ddc53c7 Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Fri, 22 Feb 2013 10:53:25 +0100 Subject: [PATCH 3/9] NFC: llcp: Implement socket options Some LLCP services (e.g. the validation ones) require some control over the LLCP link parameters like the receive window (RW) or the MIU extension (MIUX). This can only be done through socket options. Signed-off-by: Samuel Ortiz --- include/linux/socket.h | 1 + include/uapi/linux/nfc.h | 4 ++ net/nfc/llcp/llcp.h | 3 + net/nfc/llcp/sock.c | 119 ++++++++++++++++++++++++++++++++++++++- 4 files changed, 125 insertions(+), 2 deletions(-) diff --git a/include/linux/socket.h b/include/linux/socket.h index 2b9f74b0ffea..428c37a1f95c 100644 --- a/include/linux/socket.h +++ b/include/linux/socket.h @@ -298,6 +298,7 @@ struct ucred { #define SOL_IUCV 277 #define SOL_CAIF 278 #define SOL_ALG 279 +#define SOL_NFC 280 /* IPX options */ #define IPX_TYPE 1 diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h index 7969f46f1bb3..855630fe731d 100644 --- a/include/uapi/linux/nfc.h +++ b/include/uapi/linux/nfc.h @@ -220,4 +220,8 @@ struct sockaddr_nfc_llcp { #define NFC_LLCP_DIRECTION_RX 0x00 #define NFC_LLCP_DIRECTION_TX 0x01 +/* socket option names */ +#define NFC_LLCP_RW 0 +#define NFC_LLCP_MIUX 1 + #endif /*__LINUX_NFC_H */ diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index 32cec81939e6..5f117adac2e5 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -104,6 +104,9 @@ struct nfc_llcp_sock { u8 dsap; char *service_name; size_t service_name_len; + u8 rw; + u16 miux; + /* Remote link parameters */ u8 remote_rw; diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c index cc564992ba95..9357a756f7a9 100644 --- a/net/nfc/llcp/sock.c +++ b/net/nfc/llcp/sock.c @@ -223,6 +223,121 @@ static int llcp_sock_listen(struct socket *sock, int backlog) 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 = (u16) opt; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + + return err; +} + +static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + int len, err = 0; + + pr_debug("%p optname %d\n", sk, optname); + + if (level != SOL_NFC) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + len = min_t(u32, len, sizeof(u32)); + + lock_sock(sk); + + switch (optname) { + case NFC_LLCP_RW: + if (put_user(llcp_sock->rw, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case NFC_LLCP_MIUX: + if (put_user(llcp_sock->miux, (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); @@ -735,8 +850,8 @@ static const struct proto_ops llcp_sock_ops = { .ioctl = sock_no_ioctl, .listen = llcp_sock_listen, .shutdown = sock_no_shutdown, - .setsockopt = sock_no_setsockopt, - .getsockopt = sock_no_getsockopt, + .setsockopt = nfc_llcp_setsockopt, + .getsockopt = nfc_llcp_getsockopt, .sendmsg = llcp_sock_sendmsg, .recvmsg = llcp_sock_recvmsg, .mmap = sock_no_mmap, From 06d44f806aafdafefec789583aba5f8bef301c0c Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Fri, 22 Feb 2013 11:38:05 +0100 Subject: [PATCH 4/9] NFC: llcp: Use socket specific link parameters before the local ones If the socket link options are set, use them before the local one. Signed-off-by: Samuel Ortiz --- net/nfc/llcp/commands.c | 24 ++++++++++++++++-------- net/nfc/llcp/llcp.c | 2 ++ net/nfc/llcp/sock.c | 5 +++++ 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index c8a209665792..5f61830a0e86 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c @@ -319,9 +319,9 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) 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; + u8 *rw_tlv = NULL, rw_tlv_length, rw; int err; - u16 size = 0; + u16 size = 0, miux; pr_debug("Sending CONNECT\n"); @@ -337,11 +337,15 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock) size += service_name_tlv_length; } - miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0, + /* If the socket parameters are not set, use the local ones */ + miux = 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, &local->rw, 0, &rw_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); @@ -378,9 +382,9 @@ 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; + u8 *rw_tlv = NULL, rw_tlv_length, rw; int err; - u16 size = 0; + u16 size = 0, miux; pr_debug("Sending CC\n"); @@ -388,11 +392,15 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) if (local == NULL) return -ENODEV; - miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0, + /* If the socket parameters are not set, use the local ones */ + miux = 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, &local->rw, 0, &rw_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); diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index c0048b2395dd..8d547ae9c85b 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -865,6 +865,8 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, 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; diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c index 9357a756f7a9..827d7d755d09 100644 --- a/net/nfc/llcp/sock.c +++ b/net/nfc/llcp/sock.c @@ -290,6 +290,9 @@ static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname, release_sock(sk); + pr_debug("%p rw %d miux %d\n", llcp_sock, + llcp_sock->rw, llcp_sock->miux); + return err; } @@ -915,6 +918,8 @@ struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp) llcp_sock->ssap = 0; llcp_sock->dsap = LLCP_SAP_SDP; + llcp_sock->rw = LLCP_MAX_RW + 1; + llcp_sock->miux = LLCP_MAX_MIUX + 1; llcp_sock->remote_rw = LLCP_DEFAULT_RW; llcp_sock->remote_miu = LLCP_DEFAULT_MIU; llcp_sock->send_n = llcp_sock->send_ack_n = 0; From 8808edb1ec8fd976acecf9c96fc2098eda7d315b Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Fri, 22 Feb 2013 11:39:53 +0100 Subject: [PATCH 5/9] NFC: llcp: Remove redundant printk We already have a pr_debug for that. Signed-off-by: Samuel Ortiz --- net/nfc/llcp/llcp.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 8d547ae9c85b..15e7e0024285 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -766,8 +766,6 @@ static void nfc_llcp_recv_ui(struct nfc_llcp_local *local, ui_cb->dsap = dsap; ui_cb->ssap = ssap; - printk("%s %d %d\n", __func__, dsap, ssap); - pr_debug("%d %d\n", dsap, ssap); /* We're looking for a bound socket, not a client one */ From 8af362d124ce250cafb50cb488b4beb69fee3373 Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Fri, 15 Feb 2013 10:42:52 +0100 Subject: [PATCH 6/9] NFC: Add missing type policies for netlink attributes Signed-off-by: Thierry Escande Signed-off-by: Samuel Ortiz --- net/nfc/netlink.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index 504b883439f1..63975c039b85 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -53,6 +53,9 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = { [NFC_ATTR_DEVICE_POWERED] = { .type = NLA_U8 }, [NFC_ATTR_IM_PROTOCOLS] = { .type = NLA_U32 }, [NFC_ATTR_TM_PROTOCOLS] = { .type = NLA_U32 }, + [NFC_ATTR_LLC_PARAM_LTO] = { .type = NLA_U8 }, + [NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 }, + [NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 }, }; static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, From e0ae7bac06ccb90bb0cf7a3362730b48c7d7f1a8 Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Fri, 15 Feb 2013 10:43:05 +0100 Subject: [PATCH 7/9] NFC: llcp: Service Name Lookup SDRES aggregation This modifies the way SDRES PDUs are sent back. If multiple SDREQs are received within a single SNL PDU, all SDRES replies are sent packed in one SNL PDU too. Signed-off-by: Thierry Escande Signed-off-by: Samuel Ortiz --- net/nfc/llcp/commands.c | 82 ++++++++++++++++++++++++++++++----------- net/nfc/llcp/llcp.c | 23 +++++++++--- net/nfc/llcp/llcp.h | 16 +++++++- 3 files changed, 94 insertions(+), 27 deletions(-) diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index 5f61830a0e86..59f7ffca783e 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c @@ -117,6 +117,39 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_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; +} + +void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp) +{ + kfree(sdp->tlv); + kfree(sdp); +} + int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local, u8 *tlv_array, u16 tlv_array_len) { @@ -425,48 +458,55 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) return err; } -int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap) +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; - u8 *sdres_tlv = NULL, sdres_tlv_length, sdres[2]; u16 size = 0; - pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid, sap); - if (local == NULL) - return -ENODEV; + return ERR_PTR(-ENODEV); dev = local->dev; if (dev == NULL) - return -ENODEV; - - sdres[0] = tid; - sdres[1] = sap; - sdres_tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, sdres, 0, - &sdres_tlv_length); - if (sdres_tlv == NULL) - return -ENOMEM; + return ERR_PTR(-ENODEV); size += LLCP_HEADER_SIZE; size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE; - size += sdres_tlv_length; + size += tlv_length; skb = alloc_skb(size, GFP_KERNEL); - if (skb == NULL) { - kfree(sdres_tlv); - return -ENOMEM; - } + 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); - memcpy(skb_put(skb, sdres_tlv_length), sdres_tlv, sdres_tlv_length); + return skb; +} - skb_queue_tail(&local->tx_queue, 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); - kfree(sdres_tlv); + hlist_del(&sdp->node); + + nfc_llcp_free_sdp_tlv(sdp); + } + + skb_queue_tail(&local->tx_queue, skb); return 0; } diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 15e7e0024285..30b61c1aa984 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -1144,6 +1144,9 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 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; dsap = nfc_llcp_dsap(skb); ssap = nfc_llcp_ssap(skb); @@ -1158,6 +1161,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 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]; @@ -1175,14 +1179,14 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, !strncmp(service_name, "urn:nfc:sn:sdp", service_name_len)) { sap = 1; - goto send_snl; + goto add_snl; } llcp_sock = nfc_llcp_sock_from_sn(local, service_name, service_name_len); if (!llcp_sock) { sap = 0; - goto send_snl; + goto add_snl; } /* @@ -1199,7 +1203,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, if (sap == LLCP_SAP_MAX) { sap = 0; - goto send_snl; + goto add_snl; } client_count = @@ -1216,8 +1220,13 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, pr_debug("%p %d\n", llcp_sock, sap); -send_snl: - nfc_llcp_send_snl(local, tid, 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; default: @@ -1228,6 +1237,10 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, offset += length + 2; tlv += length + 2; } + +exit: + if (!hlist_empty(&llc_sdres_list)) + nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len); } static void nfc_llcp_rx_work(struct work_struct *work) diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index 5f117adac2e5..465f5953e2d1 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -46,6 +46,17 @@ struct llcp_sock_list { rwlock_t lock; }; +struct nfc_llcp_sdp_tlv { + u8 *tlv; + u8 tlv_len; + + char *uri; + u8 tid; + u8 sap; + + struct hlist_node node; +}; + struct nfc_llcp_local { struct list_head list; struct nfc_dev *dev; @@ -218,12 +229,15 @@ int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, /* 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); +void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp); 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(struct nfc_llcp_local *local, u8 tid, u8 sap); +int nfc_llcp_send_snl_sdres(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, From d9b8d8e19b073096d3609bbd60f82148d128b555 Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Fri, 15 Feb 2013 10:43:06 +0100 Subject: [PATCH 8/9] NFC: llcp: Service Name Lookup netlink interface This adds a netlink interface for service name lookup support. Multiple URIs can be passed nested into the NFC_ATTR_LLC_SDP attribute using the NFC_CMD_LLC_SDREQ netlink command. When the SNL reply is received, a NFC_EVENT_LLC_SDRES event is sent to the user space. URI and SAP tuples are passed back, nested into NFC_ATTR_LLC_SDP attribute. Signed-off-by: Thierry Escande Signed-off-by: Samuel Ortiz --- include/uapi/linux/nfc.h | 12 +++ net/nfc/llcp/commands.c | 78 ++++++++++++++++++ net/nfc/llcp/llcp.c | 32 ++++++++ net/nfc/llcp/llcp.h | 9 +++ net/nfc/netlink.c | 169 +++++++++++++++++++++++++++++++++++++++ net/nfc/nfc.h | 14 ++++ 6 files changed, 314 insertions(+) diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h index 855630fe731d..7440bc81a04b 100644 --- a/include/uapi/linux/nfc.h +++ b/include/uapi/linux/nfc.h @@ -90,6 +90,8 @@ enum nfc_commands { NFC_CMD_LLC_SET_PARAMS, NFC_CMD_ENABLE_SE, NFC_CMD_DISABLE_SE, + NFC_CMD_LLC_SDREQ, + NFC_EVENT_LLC_SDRES, /* private: internal use only */ __NFC_CMD_AFTER_LAST }; @@ -140,11 +142,21 @@ enum nfc_attrs { NFC_ATTR_LLC_PARAM_RW, NFC_ATTR_LLC_PARAM_MIUX, NFC_ATTR_SE, + NFC_ATTR_LLC_SDP, /* private: internal use only */ __NFC_ATTR_AFTER_LAST }; #define NFC_ATTR_MAX (__NFC_ATTR_AFTER_LAST - 1) +enum nfc_sdp_attr { + NFC_SDP_ATTR_UNSPEC, + NFC_SDP_ATTR_URI, + NFC_SDP_ATTR_SAP, +/* private: internal use only */ + __NFC_SDP_ATTR_AFTER_LAST +}; +#define NFC_SDP_ATTR_MAX (__NFC_SDP_ATTR_AFTER_LAST - 1) + #define NFC_DEVICE_NAME_MAXSIZE 8 #define NFC_NFCID1_MAXSIZE 10 #define NFC_SENSB_RES_MAXSIZE 12 diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index 59f7ffca783e..c943edb07b72 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c @@ -144,12 +144,59 @@ struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap) 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); + + 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) { @@ -511,6 +558,37 @@ int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local, 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); + + 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; diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 30b61c1aa984..99e911060422 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -156,6 +156,7 @@ static void local_release(struct kref *ref) cancel_work_sync(&local->rx_work); cancel_work_sync(&local->timeout_work); kfree_skb(local->rx_pending); + nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs); kfree(local); } @@ -1147,6 +1148,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 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); @@ -1229,6 +1231,30 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 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; @@ -1239,6 +1265,9 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, } 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); } @@ -1426,6 +1455,9 @@ int nfc_llcp_register_device(struct nfc_dev *ndev) local->remote_miu = LLCP_DEFAULT_MIU; local->remote_lto = LLCP_DEFAULT_LTO; + mutex_init(&local->sdreq_lock); + INIT_HLIST_HEAD(&local->pending_sdreqs); + list_add(&local->list, &llcp_devices); return 0; diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index 465f5953e2d1..ca8c6d94ab85 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -97,6 +97,10 @@ struct nfc_llcp_local { u8 remote_opt; u16 remote_wks; + struct mutex sdreq_lock; + struct hlist_head pending_sdreqs; + u8 sdreq_next_tid; + /* sockets array */ struct llcp_sock_list sockets; struct llcp_sock_list connecting_sockets; @@ -230,7 +234,10 @@ int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, 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); @@ -238,6 +245,8 @@ 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, diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index 63975c039b85..73fd51098f4d 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -56,6 +56,12 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = { [NFC_ATTR_LLC_PARAM_LTO] = { .type = NLA_U8 }, [NFC_ATTR_LLC_PARAM_RW] = { .type = NLA_U8 }, [NFC_ATTR_LLC_PARAM_MIUX] = { .type = NLA_U16 }, + [NFC_ATTR_LLC_SDP] = { .type = NLA_NESTED }, +}; + +static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = { + [NFC_SDP_ATTR_URI] = { .type = NLA_STRING }, + [NFC_SDP_ATTR_SAP] = { .type = NLA_U8 }, }; static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, @@ -351,6 +357,74 @@ int nfc_genl_device_removed(struct nfc_dev *dev) return -EMSGSIZE; } +int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list) +{ + struct sk_buff *msg; + struct nlattr *sdp_attr, *uri_attr; + struct nfc_llcp_sdp_tlv *sdres; + struct hlist_node *n; + void *hdr; + int rc = -EMSGSIZE; + int i; + + msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_EVENT_LLC_SDRES); + if (!hdr) + goto free_msg; + + if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx)) + goto nla_put_failure; + + sdp_attr = nla_nest_start(msg, NFC_ATTR_LLC_SDP); + if (sdp_attr == NULL) { + rc = -ENOMEM; + goto nla_put_failure; + } + + i = 1; + hlist_for_each_entry_safe(sdres, n, sdres_list, node) { + pr_debug("uri: %s, sap: %d\n", sdres->uri, sdres->sap); + + uri_attr = nla_nest_start(msg, i++); + if (uri_attr == NULL) { + rc = -ENOMEM; + goto nla_put_failure; + } + + if (nla_put_u8(msg, NFC_SDP_ATTR_SAP, sdres->sap)) + goto nla_put_failure; + + if (nla_put_string(msg, NFC_SDP_ATTR_URI, sdres->uri)) + goto nla_put_failure; + + nla_nest_end(msg, uri_attr); + + hlist_del(&sdres->node); + + nfc_llcp_free_sdp_tlv(sdres); + } + + nla_nest_end(msg, sdp_attr); + + genlmsg_end(msg, hdr); + + return genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC); + +nla_put_failure: + genlmsg_cancel(msg, hdr); + +free_msg: + nlmsg_free(msg); + + nfc_llcp_free_sdp_tlv_list(sdres_list); + + return rc; +} + static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev, u32 portid, u32 seq, struct netlink_callback *cb, @@ -862,6 +936,96 @@ static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info) return rc; } +static int nfc_genl_llc_sdreq(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + struct nfc_llcp_local *local; + struct nlattr *attr, *sdp_attrs[NFC_SDP_ATTR_MAX+1]; + u32 idx; + u8 tid; + char *uri; + int rc = 0, rem; + size_t uri_len, tlvs_len; + struct hlist_head sdreq_list; + struct nfc_llcp_sdp_tlv *sdreq; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || + !info->attrs[NFC_ATTR_LLC_SDP]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) { + rc = -ENODEV; + goto exit; + } + + device_lock(&dev->dev); + + if (dev->dep_link_up == false) { + rc = -ENOLINK; + goto exit; + } + + local = nfc_llcp_find_local(dev); + if (!local) { + nfc_put_device(dev); + rc = -ENODEV; + goto exit; + } + + INIT_HLIST_HEAD(&sdreq_list); + + tlvs_len = 0; + + nla_for_each_nested(attr, info->attrs[NFC_ATTR_LLC_SDP], rem) { + rc = nla_parse_nested(sdp_attrs, NFC_SDP_ATTR_MAX, attr, + nfc_sdp_genl_policy); + + if (rc != 0) { + rc = -EINVAL; + goto exit; + } + + if (!sdp_attrs[NFC_SDP_ATTR_URI]) + continue; + + uri_len = nla_len(sdp_attrs[NFC_SDP_ATTR_URI]); + if (uri_len == 0) + continue; + + uri = nla_data(sdp_attrs[NFC_SDP_ATTR_URI]); + if (uri == NULL || *uri == 0) + continue; + + tid = local->sdreq_next_tid++; + + sdreq = nfc_llcp_build_sdreq_tlv(tid, uri, uri_len); + if (sdreq == NULL) { + rc = -ENOMEM; + goto exit; + } + + tlvs_len += sdreq->tlv_len; + + hlist_add_head(&sdreq->node, &sdreq_list); + } + + if (hlist_empty(&sdreq_list)) { + rc = -EINVAL; + goto exit; + } + + rc = nfc_llcp_send_snl_sdreq(local, &sdreq_list, tlvs_len); +exit: + device_unlock(&dev->dev); + + nfc_put_device(dev); + + return rc; +} + static struct genl_ops nfc_genl_ops[] = { { .cmd = NFC_CMD_GET_DEVICE, @@ -916,6 +1080,11 @@ static struct genl_ops nfc_genl_ops[] = { .doit = nfc_genl_llc_set_params, .policy = nfc_genl_policy, }, + { + .cmd = NFC_CMD_LLC_SDREQ, + .doit = nfc_genl_llc_sdreq, + .policy = nfc_genl_policy, + }, }; diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h index 87d914d2876a..94bfe19ba678 100644 --- a/net/nfc/nfc.h +++ b/net/nfc/nfc.h @@ -46,6 +46,8 @@ struct nfc_rawsock { #define to_rawsock_sk(_tx_work) \ ((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work)) +struct nfc_llcp_sdp_tlv; + #ifdef CONFIG_NFC_LLCP void nfc_llcp_mac_is_down(struct nfc_dev *dev); @@ -59,6 +61,8 @@ int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb); struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev); int __init nfc_llcp_init(void); void nfc_llcp_exit(void); +void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp); +void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head); #else @@ -112,6 +116,14 @@ 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); @@ -144,6 +156,8 @@ int nfc_genl_dep_link_down_event(struct nfc_dev *dev); int nfc_genl_tm_activated(struct nfc_dev *dev, u32 protocol); int nfc_genl_tm_deactivated(struct nfc_dev *dev); +int nfc_genl_llc_send_sdres(struct nfc_dev *dev, struct hlist_head *sdres_list); + struct nfc_dev *nfc_get_device(unsigned int idx); static inline void nfc_put_device(struct nfc_dev *dev) From 40213fa8513c2a92e7390f25571f7c17c7955e2b Mon Sep 17 00:00:00 2001 From: Thierry Escande Date: Mon, 4 Mar 2013 15:43:32 +0100 Subject: [PATCH 9/9] NFC: llcp: Add cleanup support for unreplied SNL requests If the remote LLC doesn't reply in time to our SNL requests we remove them from the list of pending requests. The timeout is fixed to an arbitrary value of 3 times remote_lto. When not replied, the local LLC broadcasts NFC_EVENT_LLC_SDRES nl events for the concerned uris with sap values set to LLCP_SDP_UNBOUND (which is 65). Signed-off-by: Thierry Escande Signed-off-by: Samuel Ortiz --- net/nfc/llcp/commands.c | 6 ++++++ net/nfc/llcp/llcp.c | 47 +++++++++++++++++++++++++++++++++++++++++ net/nfc/llcp/llcp.h | 4 ++++ 3 files changed, 57 insertions(+) diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c index c943edb07b72..b75a9b3f9e89 100644 --- a/net/nfc/llcp/commands.c +++ b/net/nfc/llcp/commands.c @@ -174,6 +174,8 @@ struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri, sdreq->uri = sdreq->tlv + 3; memcpy(sdreq->uri, uri, uri_len); + sdreq->time = jiffies; + INIT_HLIST_NODE(&sdreq->node); return sdreq; @@ -571,6 +573,10 @@ int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local, 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); diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 99e911060422..3361170cb262 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -156,6 +156,8 @@ static void local_release(struct kref *ref) 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); kfree(local); } @@ -224,6 +226,47 @@ static void nfc_llcp_symm_timer(unsigned long data) 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; @@ -1457,6 +1500,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev) 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); diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index ca8c6d94ab85..7e87a66b02ec 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -54,6 +54,8 @@ struct nfc_llcp_sdp_tlv { u8 tid; u8 sap; + unsigned long time; + struct hlist_node node; }; @@ -99,6 +101,8 @@ struct nfc_llcp_local { 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 */