summaryrefslogtreecommitdiff
path: root/net/nfc/llcp
diff options
context:
space:
mode:
Diffstat (limited to 'net/nfc/llcp')
-rw-r--r--net/nfc/llcp/commands.c2
-rw-r--r--net/nfc/llcp/llcp.c131
-rw-r--r--net/nfc/llcp/llcp.h6
-rw-r--r--net/nfc/llcp/sock.c93
4 files changed, 161 insertions, 71 deletions
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index b982b5b890d7..c45ccd6c094c 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -312,6 +312,8 @@ int nfc_llcp_send_symm(struct nfc_dev *dev)
skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
+ 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);
}
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 82f0f7588b46..c12c5ef3d036 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -56,7 +56,7 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
llcp_sock = nfc_llcp_sock(sk);
- lock_sock(sk);
+ bh_lock_sock(sk);
if (sk->sk_state == LLCP_CONNECTED)
nfc_put_device(llcp_sock->dev);
@@ -68,26 +68,26 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
accept_queue) {
accept_sk = &lsk->sk;
- lock_sock(accept_sk);
+ bh_lock_sock(accept_sk);
nfc_llcp_accept_unlink(accept_sk);
accept_sk->sk_state = LLCP_CLOSED;
- release_sock(accept_sk);
+ bh_unlock_sock(accept_sk);
sock_orphan(accept_sk);
}
if (listen == true) {
- release_sock(sk);
+ bh_unlock_sock(sk);
continue;
}
}
sk->sk_state = LLCP_CLOSED;
- release_sock(sk);
+ bh_unlock_sock(sk);
sock_orphan(sk);
@@ -114,9 +114,9 @@ static void local_release(struct kref *ref)
nfc_llcp_socket_release(local, false);
del_timer_sync(&local->link_timer);
skb_queue_purge(&local->tx_queue);
- destroy_workqueue(local->tx_wq);
- destroy_workqueue(local->rx_wq);
- destroy_workqueue(local->timeout_wq);
+ cancel_work_sync(&local->tx_work);
+ cancel_work_sync(&local->rx_work);
+ cancel_work_sync(&local->timeout_work);
kfree_skb(local->rx_pending);
kfree(local);
}
@@ -181,7 +181,7 @@ static void nfc_llcp_symm_timer(unsigned long data)
pr_err("SYMM timeout\n");
- queue_work(local->timeout_wq, &local->timeout_work);
+ queue_work(system_nrt_wq, &local->timeout_work);
}
struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
@@ -426,6 +426,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
u8 *miux_tlv, miux_length;
__be16 miux;
u8 gb_len = 0;
+ int ret = 0;
version = LLCP_VERSION_11;
version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
@@ -450,8 +451,8 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
gb_len += ARRAY_SIZE(llcp_magic);
if (gb_len > NFC_MAX_GT_LEN) {
- kfree(version_tlv);
- return -EINVAL;
+ ret = -EINVAL;
+ goto out;
}
gb_cur = local->gb;
@@ -471,12 +472,15 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
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);
- local->gb_len = gb_len;
-
- return 0;
+ return ret;
}
u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
@@ -554,6 +558,46 @@ static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
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 hlist_node *node;
+ struct sk_buff *skb_copy = NULL, *nskb;
+ struct sock *sk;
+ u8 *data;
+
+ read_lock(&local->raw_sockets.lock);
+
+ sk_for_each(sk, node, &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,
@@ -574,6 +618,9 @@ static void nfc_llcp_tx_work(struct work_struct *work)
DUMP_PREFIX_OFFSET, 16, 1,
skb->data, skb->len, true);
+ 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);
@@ -1018,6 +1065,8 @@ static void nfc_llcp_rx_work(struct work_struct *work)
print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
16, 1, skb->data, skb->len, true);
+ nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
switch (ptype) {
case LLCP_PDU_SYMM:
pr_debug("SYMM\n");
@@ -1052,7 +1101,7 @@ static void nfc_llcp_rx_work(struct work_struct *work)
}
- queue_work(local->tx_wq, &local->tx_work);
+ queue_work(system_nrt_wq, &local->tx_work);
kfree_skb(local->rx_pending);
local->rx_pending = NULL;
@@ -1071,7 +1120,7 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
local->rx_pending = skb_get(skb);
del_timer(&local->link_timer);
- queue_work(local->rx_wq, &local->rx_work);
+ queue_work(system_nrt_wq, &local->rx_work);
return;
}
@@ -1086,7 +1135,7 @@ int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
local->rx_pending = skb_get(skb);
del_timer(&local->link_timer);
- queue_work(local->rx_wq, &local->rx_work);
+ queue_work(system_nrt_wq, &local->rx_work);
return 0;
}
@@ -1121,7 +1170,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
if (rf_mode == NFC_RF_INITIATOR) {
pr_debug("Queueing Tx work\n");
- queue_work(local->tx_wq, &local->tx_work);
+ queue_work(system_nrt_wq, &local->tx_work);
} else {
mod_timer(&local->link_timer,
jiffies + msecs_to_jiffies(local->remote_lto));
@@ -1130,10 +1179,7 @@ void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
int nfc_llcp_register_device(struct nfc_dev *ndev)
{
- struct device *dev = &ndev->dev;
struct nfc_llcp_local *local;
- char name[32];
- int err;
local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
if (local == NULL)
@@ -1149,41 +1195,15 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
skb_queue_head_init(&local->tx_queue);
INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
- snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev));
- local->tx_wq =
- alloc_workqueue(name,
- WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
- 1);
- if (local->tx_wq == NULL) {
- err = -ENOMEM;
- goto err_local;
- }
local->rx_pending = NULL;
INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
- snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev));
- local->rx_wq =
- alloc_workqueue(name,
- WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
- 1);
- if (local->rx_wq == NULL) {
- err = -ENOMEM;
- goto err_tx_wq;
- }
INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
- snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev));
- local->timeout_wq =
- alloc_workqueue(name,
- WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM,
- 1);
- if (local->timeout_wq == NULL) {
- err = -ENOMEM;
- goto err_rx_wq;
- }
- local->sockets.lock = __RW_LOCK_UNLOCKED(local->sockets.lock);
- local->connecting_sockets.lock = __RW_LOCK_UNLOCKED(local->connecting_sockets.lock);
+ rwlock_init(&local->sockets.lock);
+ rwlock_init(&local->connecting_sockets.lock);
+ rwlock_init(&local->raw_sockets.lock);
nfc_llcp_build_gb(local);
@@ -1193,17 +1213,6 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
list_add(&llcp_devices, &local->list);
return 0;
-
-err_rx_wq:
- destroy_workqueue(local->rx_wq);
-
-err_tx_wq:
- destroy_workqueue(local->tx_wq);
-
-err_local:
- kfree(local);
-
- return 0;
}
void nfc_llcp_unregister_device(struct nfc_dev *dev)
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index 83b8bba5a280..fdb2d24e60bd 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -56,12 +56,9 @@ struct nfc_llcp_local {
struct timer_list link_timer;
struct sk_buff_head tx_queue;
- struct workqueue_struct *tx_wq;
struct work_struct tx_work;
- struct workqueue_struct *rx_wq;
struct work_struct rx_work;
struct sk_buff *rx_pending;
- struct workqueue_struct *timeout_wq;
struct work_struct timeout_work;
u32 target_idx;
@@ -89,6 +86,7 @@ struct nfc_llcp_local {
/* sockets array */
struct llcp_sock_list sockets;
struct llcp_sock_list connecting_sockets;
+ struct llcp_sock_list raw_sockets;
};
struct nfc_llcp_sock {
@@ -187,6 +185,8 @@ u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
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);
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index ddeb9aa398f0..40f056debf9a 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -142,6 +142,60 @@ error:
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;
@@ -300,9 +354,6 @@ static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr,
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;
-
uaddr->sa_family = AF_NFC;
*len = sizeof(struct sockaddr_nfc_llcp);
@@ -421,7 +472,10 @@ static int llcp_sock_release(struct socket *sock)
release_sock(sk);
- nfc_llcp_sock_unlink(&local->sockets, 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);
@@ -617,7 +671,7 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
if (!(flags & MSG_PEEK)) {
/* SOCK_STREAM: re-queue skb if it contains unreceived data */
- if (sk->sk_type == SOCK_STREAM) {
+ if (sk->sk_type == SOCK_STREAM || sk->sk_type == SOCK_RAW) {
skb_pull(skb, copied);
if (skb->len) {
skb_queue_head(&sk->sk_receive_queue, skb);
@@ -658,6 +712,26 @@ static const struct proto_ops llcp_sock_ops = {
.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);
@@ -735,10 +809,15 @@ static int llcp_sock_create(struct net *net, struct socket *sock,
pr_debug("%p\n", sock);
- if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM)
+ if (sock->type != SOCK_STREAM &&
+ sock->type != SOCK_DGRAM &&
+ sock->type != SOCK_RAW)
return -ESOCKTNOSUPPORT;
- sock->ops = &llcp_sock_ops;
+ 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)
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-17 03:02:57 +0000