diff options
Diffstat (limited to 'net/nfc')
| -rw-r--r-- | net/nfc/core.c | 38 | ||||
| -rw-r--r-- | net/nfc/hci/command.c | 26 | ||||
| -rw-r--r-- | net/nfc/hci/core.c | 104 | ||||
| -rw-r--r-- | net/nfc/hci/hci.h | 12 | ||||
| -rw-r--r-- | net/nfc/hci/shdlc.c | 38 | ||||
| -rw-r--r-- | net/nfc/llcp/llcp.c | 342 | ||||
| -rw-r--r-- | net/nfc/llcp/llcp.h | 5 | ||||
| -rw-r--r-- | net/nfc/llcp/sock.c | 31 | ||||
| -rw-r--r-- | net/nfc/nci/core.c | 5 | ||||
| -rw-r--r-- | net/nfc/nci/ntf.c | 5 | ||||
| -rw-r--r-- | net/nfc/netlink.c | 9 |
11 files changed, 392 insertions, 223 deletions
diff --git a/net/nfc/core.c b/net/nfc/core.c index 4177bb5104b9..ff749794bc5b 100644 --- a/net/nfc/core.c +++ b/net/nfc/core.c @@ -29,6 +29,8 @@ #include <linux/slab.h> #include <linux/nfc.h> +#include <net/genetlink.h> + #include "nfc.h" #define VERSION "0.1" @@ -560,6 +562,8 @@ EXPORT_SYMBOL(nfc_alloc_recv_skb); * The device driver must call this function when one or many nfc targets * are found. After calling this function, the device driver must stop * polling for targets. + * NOTE: This function can be called with targets=NULL and n_targets=0 to + * notify a driver error, meaning that the polling operation cannot complete. * IMPORTANT: this function must not be called from an atomic context. * In addition, it must also not be called from a context that would prevent * the NFC Core to call other nfc ops entry point concurrently. @@ -571,23 +575,33 @@ int nfc_targets_found(struct nfc_dev *dev, pr_debug("dev_name=%s n_targets=%d\n", dev_name(&dev->dev), n_targets); - dev->polling = false; - for (i = 0; i < n_targets; i++) targets[i].idx = dev->target_next_idx++; device_lock(&dev->dev); + if (dev->polling == false) { + device_unlock(&dev->dev); + return 0; + } + + dev->polling = false; + dev->targets_generation++; kfree(dev->targets); - dev->targets = kmemdup(targets, n_targets * sizeof(struct nfc_target), - GFP_ATOMIC); + dev->targets = NULL; - if (!dev->targets) { - dev->n_targets = 0; - device_unlock(&dev->dev); - return -ENOMEM; + if (targets) { + dev->targets = kmemdup(targets, + n_targets * sizeof(struct nfc_target), + GFP_ATOMIC); + + if (!dev->targets) { + dev->n_targets = 0; + device_unlock(&dev->dev); + return -ENOMEM; + } } dev->n_targets = n_targets; @@ -651,6 +665,12 @@ int nfc_target_lost(struct nfc_dev *dev, u32 target_idx) } EXPORT_SYMBOL(nfc_target_lost); +inline void nfc_driver_failure(struct nfc_dev *dev, int err) +{ + nfc_targets_found(dev, NULL, 0); +} +EXPORT_SYMBOL(nfc_driver_failure); + static void nfc_release(struct device *d) { struct nfc_dev *dev = to_nfc_dev(d); @@ -906,3 +926,5 @@ MODULE_AUTHOR("Lauro Ramos Venancio <lauro.venancio@openbossa.org>"); MODULE_DESCRIPTION("NFC Core ver " VERSION); MODULE_VERSION(VERSION); MODULE_LICENSE("GPL"); +MODULE_ALIAS_NETPROTO(PF_NFC); +MODULE_ALIAS_GENL_FAMILY(NFC_GENL_NAME); diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c index 8729abf5f18b..46362ef979db 100644 --- a/net/nfc/hci/command.c +++ b/net/nfc/hci/command.c @@ -28,26 +28,14 @@ #include "hci.h" -static int nfc_hci_result_to_errno(u8 result) -{ - switch (result) { - case NFC_HCI_ANY_OK: - return 0; - case NFC_HCI_ANY_E_TIMEOUT: - return -ETIMEDOUT; - default: - return -1; - } -} - -static void nfc_hci_execute_cb(struct nfc_hci_dev *hdev, u8 result, +static void nfc_hci_execute_cb(struct nfc_hci_dev *hdev, int err, struct sk_buff *skb, void *cb_data) { struct hcp_exec_waiter *hcp_ew = (struct hcp_exec_waiter *)cb_data; - pr_debug("HCI Cmd completed with HCI result=%d\n", result); + pr_debug("HCI Cmd completed with result=%d\n", err); - hcp_ew->exec_result = nfc_hci_result_to_errno(result); + hcp_ew->exec_result = err; if (hcp_ew->exec_result == 0) hcp_ew->result_skb = skb; else @@ -311,9 +299,9 @@ int nfc_hci_disconnect_all_gates(struct nfc_hci_dev *hdev) } EXPORT_SYMBOL(nfc_hci_disconnect_all_gates); -int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate) +int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate, + u8 pipe) { - u8 pipe = NFC_HCI_INVALID_PIPE; bool pipe_created = false; int r; @@ -322,6 +310,9 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate) if (hdev->gate2pipe[dest_gate] != NFC_HCI_INVALID_PIPE) return -EADDRINUSE; + if (pipe != NFC_HCI_INVALID_PIPE) + goto pipe_is_open; + switch (dest_gate) { case NFC_HCI_LINK_MGMT_GATE: pipe = NFC_HCI_LINK_MGMT_PIPE; @@ -347,6 +338,7 @@ int nfc_hci_connect_gate(struct nfc_hci_dev *hdev, u8 dest_host, u8 dest_gate) return r; } +pipe_is_open: hdev->gate2pipe[dest_gate] = pipe; return 0; diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c index a8b0b71e8f86..36717cebfbb6 100644 --- a/net/nfc/hci/core.c +++ b/net/nfc/hci/core.c @@ -32,6 +32,18 @@ /* Largest headroom needed for outgoing HCI commands */ #define HCI_CMDS_HEADROOM 1 +static int nfc_hci_result_to_errno(u8 result) +{ + switch (result) { + case NFC_HCI_ANY_OK: + return 0; + case NFC_HCI_ANY_E_TIMEOUT: + return -ETIME; + default: + return -1; + } +} + static void nfc_hci_msg_tx_work(struct work_struct *work) { struct nfc_hci_dev *hdev = container_of(work, struct nfc_hci_dev, @@ -46,7 +58,7 @@ static void nfc_hci_msg_tx_work(struct work_struct *work) if (timer_pending(&hdev->cmd_timer) == 0) { if (hdev->cmd_pending_msg->cb) hdev->cmd_pending_msg->cb(hdev, - NFC_HCI_ANY_E_TIMEOUT, + -ETIME, NULL, hdev-> cmd_pending_msg-> @@ -71,8 +83,7 @@ next_msg: kfree_skb(skb); skb_queue_purge(&msg->msg_frags); if (msg->cb) - msg->cb(hdev, NFC_HCI_ANY_E_NOK, NULL, - msg->cb_context); + msg->cb(hdev, r, NULL, msg->cb_context); kfree(msg); break; } @@ -116,20 +127,13 @@ static void nfc_hci_msg_rx_work(struct work_struct *work) } } -void nfc_hci_resp_received(struct nfc_hci_dev *hdev, u8 result, - struct sk_buff *skb) +static void __nfc_hci_cmd_completion(struct nfc_hci_dev *hdev, int err, + struct sk_buff *skb) { - mutex_lock(&hdev->msg_tx_mutex); - - if (hdev->cmd_pending_msg == NULL) { - kfree_skb(skb); - goto exit; - } - del_timer_sync(&hdev->cmd_timer); if (hdev->cmd_pending_msg->cb) - hdev->cmd_pending_msg->cb(hdev, result, skb, + hdev->cmd_pending_msg->cb(hdev, err, skb, hdev->cmd_pending_msg->cb_context); else kfree_skb(skb); @@ -138,6 +142,19 @@ void nfc_hci_resp_received(struct nfc_hci_dev *hdev, u8 result, hdev->cmd_pending_msg = NULL; queue_work(hdev->msg_tx_wq, &hdev->msg_tx_work); +} + +void nfc_hci_resp_received(struct nfc_hci_dev *hdev, u8 result, + struct sk_buff *skb) +{ + mutex_lock(&hdev->msg_tx_mutex); + + if (hdev->cmd_pending_msg == NULL) { + kfree_skb(skb); + goto exit; + } + + __nfc_hci_cmd_completion(hdev, nfc_hci_result_to_errno(result), skb); exit: mutex_unlock(&hdev->msg_tx_mutex); @@ -213,7 +230,7 @@ static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate) } break; case NFC_HCI_RF_READER_B_GATE: - targets->supported_protocols = NFC_PROTO_ISO14443_MASK; + targets->supported_protocols = NFC_PROTO_ISO14443_B_MASK; break; default: if (hdev->ops->target_from_gate) @@ -298,15 +315,15 @@ static void nfc_hci_cmd_timeout(unsigned long data) } static int hci_dev_connect_gates(struct nfc_hci_dev *hdev, u8 gate_count, - u8 gates[]) + struct nfc_hci_gate *gates) { int r; - u8 *p = gates; while (gate_count--) { - r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, *p); + r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, + gates->gate, gates->pipe); if (r < 0) return r; - p++; + gates++; } return 0; @@ -316,14 +333,13 @@ static int hci_dev_session_init(struct nfc_hci_dev *hdev) { struct sk_buff *skb = NULL; int r; - u8 hci_gates[] = { /* NFC_HCI_ADMIN_GATE MUST be first */ - NFC_HCI_ADMIN_GATE, NFC_HCI_LOOPBACK_GATE, - NFC_HCI_ID_MGMT_GATE, NFC_HCI_LINK_MGMT_GATE, - NFC_HCI_RF_READER_B_GATE, NFC_HCI_RF_READER_A_GATE - }; + + if (hdev->init_data.gates[0].gate != NFC_HCI_ADMIN_GATE) + return -EPROTO; r = nfc_hci_connect_gate(hdev, NFC_HCI_HOST_CONTROLLER_ID, - NFC_HCI_ADMIN_GATE); + hdev->init_data.gates[0].gate, + hdev->init_data.gates[0].pipe); if (r < 0) goto exit; @@ -351,10 +367,6 @@ static int hci_dev_session_init(struct nfc_hci_dev *hdev) if (r < 0) goto exit; - r = hci_dev_connect_gates(hdev, sizeof(hci_gates), hci_gates); - if (r < 0) - goto disconnect_all; - r = hci_dev_connect_gates(hdev, hdev->init_data.gate_count, hdev->init_data.gates); if (r < 0) @@ -717,6 +729,27 @@ void *nfc_hci_get_clientdata(struct nfc_hci_dev *hdev) } EXPORT_SYMBOL(nfc_hci_get_clientdata); +static void nfc_hci_failure(struct nfc_hci_dev *hdev, int err) +{ + mutex_lock(&hdev->msg_tx_mutex); + + if (hdev->cmd_pending_msg == NULL) { + nfc_driver_failure(hdev->ndev, err); + goto exit; + } + + __nfc_hci_cmd_completion(hdev, err, NULL); + +exit: + mutex_unlock(&hdev->msg_tx_mutex); +} + +void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err) +{ + nfc_hci_failure(hdev, err); +} +EXPORT_SYMBOL(nfc_hci_driver_failure); + void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb) { struct hcp_packet *packet; @@ -727,16 +760,6 @@ void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb) struct sk_buff *frag_skb; int msg_len; - if (skb == NULL) { - /* TODO ELa: lower layer had permanent failure, need to - * propagate that up - */ - - skb_queue_purge(&hdev->rx_hcp_frags); - - return; - } - packet = (struct hcp_packet *)skb->data; if ((packet->header & ~NFC_HCI_FRAGMENT) == 0) { skb_queue_tail(&hdev->rx_hcp_frags, skb); @@ -757,9 +780,8 @@ void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb) hcp_skb = nfc_alloc_recv_skb(NFC_HCI_HCP_PACKET_HEADER_LEN + msg_len, GFP_KERNEL); if (hcp_skb == NULL) { - /* TODO ELa: cannot deliver HCP message. How to - * propagate error up? - */ + nfc_hci_failure(hdev, -ENOMEM); + return; } *skb_put(hcp_skb, NFC_HCI_HCP_PACKET_HEADER_LEN) = pipe; diff --git a/net/nfc/hci/hci.h b/net/nfc/hci/hci.h index 45f2fe4fd486..fa9a21e92239 100644 --- a/net/nfc/hci/hci.h +++ b/net/nfc/hci/hci.h @@ -37,10 +37,11 @@ struct hcp_packet { /* * HCI command execution completion callback. - * result will be one of the HCI response codes. - * skb contains the response data and must be disposed. + * result will be a standard linux error (may be converted from HCI response) + * skb contains the response data and must be disposed, or may be NULL if + * an error occured */ -typedef void (*hci_cmd_cb_t) (struct nfc_hci_dev *hdev, u8 result, +typedef void (*hci_cmd_cb_t) (struct nfc_hci_dev *hdev, int result, struct sk_buff *skb, void *cb_data); struct hcp_exec_waiter { @@ -131,9 +132,4 @@ void nfc_hci_hcp_message_rx(struct nfc_hci_dev *hdev, u8 pipe, u8 type, #define NFC_HCI_ANY_E_REG_ACCESS_DENIED 0x0a #define NFC_HCI_ANY_E_PIPE_ACCESS_DENIED 0x0b -/* Pipes */ -#define NFC_HCI_INVALID_PIPE 0x80 -#define NFC_HCI_LINK_MGMT_PIPE 0x00 -#define NFC_HCI_ADMIN_PIPE 0x01 - #endif /* __LOCAL_HCI_H */ diff --git a/net/nfc/hci/shdlc.c b/net/nfc/hci/shdlc.c index 6b836e6242b7..6f840c18c892 100644 --- a/net/nfc/hci/shdlc.c +++ b/net/nfc/hci/shdlc.c @@ -340,15 +340,6 @@ static void nfc_shdlc_connect_complete(struct nfc_shdlc *shdlc, int r) shdlc->state = SHDLC_CONNECTED; } else { shdlc->state = SHDLC_DISCONNECTED; - - /* - * TODO: Could it be possible that there are pending - * executing commands that are waiting for connect to complete - * before they can be carried? As connect is a blocking - * operation, it would require that the userspace process can - * send commands on the same device from a second thread before - * the device is up. I don't think that is possible, is it? - */ } shdlc->connect_result = r; @@ -413,12 +404,12 @@ static void nfc_shdlc_rcv_u_frame(struct nfc_shdlc *shdlc, r = nfc_shdlc_connect_send_ua(shdlc); nfc_shdlc_connect_complete(shdlc, r); } - } else if (shdlc->state > SHDLC_NEGOCIATING) { + } else if (shdlc->state == SHDLC_CONNECTED) { /* - * TODO: Chip wants to reset link - * send ua, empty skb lists, reset counters - * propagate info to HCI layer + * Chip wants to reset link. This is unexpected and + * unsupported. */ + shdlc->hard_fault = -ECONNRESET; } break; case U_FRAME_UA: @@ -523,10 +514,6 @@ static void nfc_shdlc_handle_send_queue(struct nfc_shdlc *shdlc) r = shdlc->ops->xmit(shdlc, skb); if (r < 0) { - /* - * TODO: Cannot send, shdlc machine is dead, we - * must propagate the information up to HCI. - */ shdlc->hard_fault = r; break; } @@ -590,6 +577,11 @@ static void nfc_shdlc_sm_work(struct work_struct *work) skb_queue_purge(&shdlc->ack_pending_q); break; case SHDLC_CONNECTING: + if (shdlc->hard_fault) { + nfc_shdlc_connect_complete(shdlc, shdlc->hard_fault); + break; + } + if (shdlc->connect_tries++ < 5) r = nfc_shdlc_connect_initiate(shdlc); else @@ -610,6 +602,11 @@ static void nfc_shdlc_sm_work(struct work_struct *work) } nfc_shdlc_handle_rcv_queue(shdlc); + + if (shdlc->hard_fault) { + nfc_shdlc_connect_complete(shdlc, shdlc->hard_fault); + break; + } break; case SHDLC_CONNECTED: nfc_shdlc_handle_rcv_queue(shdlc); @@ -637,10 +634,7 @@ static void nfc_shdlc_sm_work(struct work_struct *work) } if (shdlc->hard_fault) { - /* - * TODO: Handle hard_fault that occured during - * this invocation of the shdlc worker - */ + nfc_hci_driver_failure(shdlc->hdev, shdlc->hard_fault); } break; default: @@ -923,8 +917,6 @@ void nfc_shdlc_free(struct nfc_shdlc *shdlc) { pr_debug("\n"); - /* TODO: Check that this cannot be called while still in use */ - nfc_hci_unregister_device(shdlc->hdev); nfc_hci_free_device(shdlc->hdev); diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c index 5d503eeb15a1..82f0f7588b46 100644 --- a/net/nfc/llcp/llcp.c +++ b/net/nfc/llcp/llcp.c @@ -45,7 +45,7 @@ void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk) write_unlock(&l->lock); } -static void nfc_llcp_socket_release(struct nfc_llcp_local *local) +static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen) { struct sock *sk; struct hlist_node *node, *tmp; @@ -78,6 +78,11 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local) sock_orphan(accept_sk); } + + if (listen == true) { + release_sock(sk); + continue; + } } sk->sk_state = LLCP_CLOSED; @@ -106,7 +111,7 @@ static void local_release(struct kref *ref) local = container_of(ref, struct nfc_llcp_local, ref); list_del(&local->list); - nfc_llcp_socket_release(local); + nfc_llcp_socket_release(local, false); del_timer_sync(&local->link_timer); skb_queue_purge(&local->tx_queue); destroy_workqueue(local->tx_wq); @@ -118,23 +123,48 @@ static void local_release(struct kref *ref) int nfc_llcp_local_put(struct nfc_llcp_local *local) { - WARN_ON(local == NULL); - if (local == NULL) return 0; return kref_put(&local->ref, local_release); } -static void nfc_llcp_clear_sdp(struct nfc_llcp_local *local) +static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, + u8 ssap, u8 dsap) { - mutex_lock(&local->sdp_lock); + struct sock *sk; + struct hlist_node *node; + struct nfc_llcp_sock *llcp_sock; - local->local_wks = 0; - local->local_sdp = 0; - local->local_sap = 0; + pr_debug("ssap dsap %d %d\n", ssap, dsap); - mutex_unlock(&local->sdp_lock); + if (ssap == 0 && dsap == 0) + return NULL; + + read_lock(&local->sockets.lock); + + llcp_sock = NULL; + + sk_for_each(sk, node, &local->sockets.head) { + llcp_sock = nfc_llcp_sock(sk); + + if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap) + 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) @@ -197,6 +227,51 @@ static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len) 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 hlist_node *node; + 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, node, &local->sockets.head) { + tmp_sock = nfc_llcp_sock(sk); + + pr_debug("llcp sock %p\n", tmp_sock); + + if (tmp_sock->sk.sk_state != LLCP_LISTEN) + 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) { @@ -223,41 +298,26 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local, } /* - * This is not a well known service, - * we should try to find a local SDP free spot + * Check if there already is a non WKS socket bound + * to this service name. */ - ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP); - if (ssap == LLCP_SDP_NUM_SAP) { + if (nfc_llcp_sock_from_sn(local, sock->service_name, + sock->service_name_len) != NULL) { 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; - - } else if (sock->ssap != 0) { - if (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; - } + return LLCP_SDP_UNBOUND; - } else if (sock->ssap < LLCP_SDP_NUM_SAP) { - if (!test_bit(sock->ssap - LLCP_WKS_NUM_SAP, - &local->local_sdp)) { - set_bit(sock->ssap - LLCP_WKS_NUM_SAP, - &local->local_sdp); - mutex_unlock(&local->sdp_lock); + } 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; - } + return sock->ssap; } } @@ -294,8 +354,34 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) 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; @@ -310,19 +396,26 @@ void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap) mutex_unlock(&local->sdp_lock); } -u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len) +static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local) { - struct nfc_llcp_local *local; + u8 ssap; - local = nfc_llcp_find_local(dev); - if (local == NULL) { - *general_bytes_len = 0; - return NULL; + 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; } - *general_bytes_len = local->gb_len; + pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap); - return local->gb; + 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) @@ -386,6 +479,23 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local) return 0; } +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); @@ -509,74 +619,12 @@ out: return llcp_sock; } -static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, - u8 ssap, u8 dsap) -{ - struct sock *sk; - struct hlist_node *node; - struct nfc_llcp_sock *llcp_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, node, &local->sockets.head) { - llcp_sock = nfc_llcp_sock(sk); - - if (llcp_sock->ssap == ssap && - llcp_sock->dsap == dsap) - break; - } - - read_unlock(&local->sockets.lock); - - if (llcp_sock == NULL) - return NULL; - - sock_hold(&llcp_sock->sk); - - 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 sock *sk; - struct hlist_node *node; struct nfc_llcp_sock *llcp_sock; - pr_debug("sn %zd\n", sn_len); - - if (sn == NULL || sn_len == 0) - return NULL; - - read_lock(&local->sockets.lock); - - llcp_sock = NULL; - - sk_for_each(sk, node, &local->sockets.head) { - llcp_sock = nfc_llcp_sock(sk); - - if (llcp_sock->sk.sk_state != LLCP_LISTEN) - continue; - - if (llcp_sock->service_name == NULL || - llcp_sock->service_name_len == 0) - continue; - - if (llcp_sock->service_name_len != sn_len) - continue; - - if (memcmp(sn, llcp_sock->service_name, sn_len) == 0) - break; - } - - read_unlock(&local->sockets.lock); + llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len); if (llcp_sock == NULL) return NULL; @@ -586,11 +634,6 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local, return llcp_sock; } -static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) -{ - sock_put(&sock->sk); -} - static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len) { u8 *tlv = &skb->data[2], type, length; @@ -662,6 +705,21 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, 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; @@ -675,9 +733,21 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local, new_sock->local = nfc_llcp_local_get(local); new_sock->miu = local->remote_miu; new_sock->nfc_protocol = sock->nfc_protocol; - new_sock->ssap = sock->ssap; 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); @@ -886,6 +956,45 @@ static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb) 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_err("Invalid DM\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); + + return; +} + static void nfc_llcp_rx_work(struct work_struct *work) { struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, @@ -929,6 +1038,11 @@ static void nfc_llcp_rx_work(struct work_struct *work) 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_I: case LLCP_PDU_RR: case LLCP_PDU_RNR: @@ -985,10 +1099,8 @@ void nfc_llcp_mac_is_down(struct nfc_dev *dev) if (local == NULL) return; - nfc_llcp_clear_sdp(local); - /* Close and purge all existing sockets */ - nfc_llcp_socket_release(local); + nfc_llcp_socket_release(local, true); } void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx, diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h index 7286c86982ff..83b8bba5a280 100644 --- a/net/nfc/llcp/llcp.h +++ b/net/nfc/llcp/llcp.h @@ -37,6 +37,7 @@ enum llcp_state { #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; @@ -69,6 +70,7 @@ struct nfc_llcp_local { 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]; @@ -113,6 +115,9 @@ struct nfc_llcp_sock { /* 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 sk_buff_head tx_backlog_queue; diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c index 05ca5a680071..ddeb9aa398f0 100644 --- a/net/nfc/llcp/sock.c +++ b/net/nfc/llcp/sock.c @@ -78,11 +78,11 @@ static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) struct sockaddr_nfc_llcp llcp_addr; int len, ret = 0; - pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); - if (!addr || addr->sa_family != AF_NFC) return -EINVAL; + 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); @@ -121,8 +121,12 @@ static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) GFP_KERNEL); llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock); - if (llcp_sock->ssap == LLCP_MAX_SAP) + 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); @@ -283,22 +287,28 @@ error: return ret; } -static int llcp_sock_getname(struct socket *sock, struct sockaddr *addr, +static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr, int *len, int peer) { - struct sockaddr_nfc_llcp *llcp_addr = (struct sockaddr_nfc_llcp *)addr; struct sock *sk = sock->sk; struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr); - pr_debug("%p\n", sk); + 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); if (llcp_sock == NULL || llcp_sock->dev == NULL) return -EBADFD; - addr->sa_family = AF_NFC; + 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; @@ -406,7 +416,8 @@ static int llcp_sock_release(struct socket *sock) } } - nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); + if (llcp_sock->reserved_ssap < LLCP_SAP_MAX) + nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); release_sock(sk); @@ -486,6 +497,9 @@ static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr, ret = -ENOMEM; goto put_dev; } + + llcp_sock->reserved_ssap = llcp_sock->ssap; + if (addr->service_name_len == 0) llcp_sock->dsap = addr->dsap; else @@ -687,6 +701,7 @@ struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp) 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; skb_queue_head_init(&llcp_sock->tx_queue); skb_queue_head_init(&llcp_sock->tx_pending_queue); skb_queue_head_init(&llcp_sock->tx_backlog_queue); diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c index 766a02b1dfa1..5bb4da680427 100644 --- a/net/nfc/nci/core.c +++ b/net/nfc/nci/core.c @@ -194,7 +194,7 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt) } if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) && - (protocols & NFC_PROTO_ISO14443_MASK)) { + (protocols & NFC_PROTO_ISO14443_B_MASK)) { cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode = NCI_NFC_B_PASSIVE_POLL_MODE; cmd.disc_configs[cmd.num_disc_configs].frequency = 1; @@ -486,7 +486,8 @@ static int nci_activate_target(struct nfc_dev *nfc_dev, param.rf_protocol = NCI_RF_PROTOCOL_T2T; else if (protocol == NFC_PROTO_FELICA) param.rf_protocol = NCI_RF_PROTOCOL_T3T; - else if (protocol == NFC_PROTO_ISO14443) + else if (protocol == NFC_PROTO_ISO14443 || + protocol == NFC_PROTO_ISO14443_B) param.rf_protocol = NCI_RF_PROTOCOL_ISO_DEP; else param.rf_protocol = NCI_RF_PROTOCOL_NFC_DEP; diff --git a/net/nfc/nci/ntf.c b/net/nfc/nci/ntf.c index 2ab196a9f228..af7a93b04393 100644 --- a/net/nfc/nci/ntf.c +++ b/net/nfc/nci/ntf.c @@ -170,7 +170,10 @@ static int nci_add_new_protocol(struct nci_dev *ndev, if (rf_protocol == NCI_RF_PROTOCOL_T2T) protocol = NFC_PROTO_MIFARE_MASK; else if (rf_protocol == NCI_RF_PROTOCOL_ISO_DEP) - protocol = NFC_PROTO_ISO14443_MASK; + if (rf_tech_and_mode == NCI_NFC_A_PASSIVE_POLL_MODE) + protocol = NFC_PROTO_ISO14443_MASK; + else + protocol = NFC_PROTO_ISO14443_B_MASK; else if (rf_protocol == NCI_RF_PROTOCOL_T3T) protocol = NFC_PROTO_FELICA_MASK; else diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index f4f07f9b61c0..4c51714ee741 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -634,6 +634,15 @@ static int nfc_genl_stop_poll(struct sk_buff *skb, struct genl_info *info) if (!dev) return -ENODEV; + device_lock(&dev->dev); + + if (!dev->polling) { + device_unlock(&dev->dev); + return -EINVAL; + } + + device_unlock(&dev->dev); + mutex_lock(&dev->genl_data.genl_data_mutex); if (dev->genl_data.poll_req_pid != info->snd_pid) { |