net/nfc/rawsock.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Copyright (C) 2011 Instituto Nokia de Tecnologia
0004  *
0005  * Authors:
0006  *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
0007  *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
0008  */
0009
0010 #define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
0011
0012 #include <net/tcp_states.h>
0013 #include <linux/nfc.h>
0014 #include <linux/export.h>
0015
0016 #include "nfc.h"
0017
0018 static struct nfc_sock_list raw_sk_list = {
0019     .lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock)
0020 };
0021
0022 static void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk)
0023 {
0024     write_lock(&l->lock);
0025     sk_add_node(sk, &l->head);
0026     write_unlock(&l->lock);
0027 }
0028
0029 static void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk)
0030 {
0031     write_lock(&l->lock);
0032     sk_del_node_init(sk);
0033     write_unlock(&l->lock);
0034 }
0035
0036 static void rawsock_write_queue_purge(struct sock *sk)
0037 {
0038     pr_debug("sk=%p\n", sk);
0039
0040     spin_lock_bh(&sk->sk_write_queue.lock);
0041     __skb_queue_purge(&sk->sk_write_queue);
0042     nfc_rawsock(sk)->tx_work_scheduled = false;
0043     spin_unlock_bh(&sk->sk_write_queue.lock);
0044 }
0045
0046 static void rawsock_report_error(struct sock *sk, int err)
0047 {
0048     pr_debug("sk=%p err=%d\n", sk, err);
0049
0050     sk->sk_shutdown = SHUTDOWN_MASK;
0051     sk->sk_err = -err;
0052     sk_error_report(sk);
0053
0054     rawsock_write_queue_purge(sk);
0055 }
0056
0057 static int rawsock_release(struct socket *sock)
0058 {
0059     struct sock *sk = sock->sk;
0060
0061     pr_debug("sock=%p sk=%p\n", sock, sk);
0062
0063     if (!sk)
0064         return 0;
0065
0066     if (sock->type == SOCK_RAW)
0067         nfc_sock_unlink(&raw_sk_list, sk);
0068
0069     sock_orphan(sk);
0070     sock_put(sk);
0071
0072     return 0;
0073 }
0074
0075 static int rawsock_connect(struct socket *sock, struct sockaddr *_addr,
0076                int len, int flags)
0077 {
0078     struct sock *sk = sock->sk;
0079     struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr;
0080     struct nfc_dev *dev;
0081     int rc = 0;
0082
0083     pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags);
0084
0085     if (!addr || len < sizeof(struct sockaddr_nfc) ||
0086         addr->sa_family != AF_NFC)
0087         return -EINVAL;
0088
0089     pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n",
0090          addr->dev_idx, addr->target_idx, addr->nfc_protocol);
0091
0092     lock_sock(sk);
0093
0094     if (sock->state == SS_CONNECTED) {
0095         rc = -EISCONN;
0096         goto error;
0097     }
0098
0099     dev = nfc_get_device(addr->dev_idx);
0100     if (!dev) {
0101         rc = -ENODEV;
0102         goto error;
0103     }
0104
0105     if (addr->target_idx > dev->target_next_idx - 1 ||
0106         addr->target_idx < dev->target_next_idx - dev->n_targets) {
0107         rc = -EINVAL;
0108         goto put_dev;
0109     }
0110
0111     rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol);
0112     if (rc)
0113         goto put_dev;
0114
0115     nfc_rawsock(sk)->dev = dev;
0116     nfc_rawsock(sk)->target_idx = addr->target_idx;
0117     sock->state = SS_CONNECTED;
0118     sk->sk_state = TCP_ESTABLISHED;
0119     sk->sk_state_change(sk);
0120
0121     release_sock(sk);
0122     return 0;
0123
0124 put_dev:
0125     nfc_put_device(dev);
0126 error:
0127     release_sock(sk);
0128     return rc;
0129 }
0130
0131 static int rawsock_add_header(struct sk_buff *skb)
0132 {
0133     *(u8 *)skb_push(skb, NFC_HEADER_SIZE) = 0;
0134
0135     return 0;
0136 }
0137
0138 static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
0139                        int err)
0140 {
0141     struct sock *sk = (struct sock *) context;
0142
0143     BUG_ON(in_hardirq());
0144
0145     pr_debug("sk=%p err=%d\n", sk, err);
0146
0147     if (err)
0148         goto error;
0149
0150     err = rawsock_add_header(skb);
0151     if (err)
0152         goto error_skb;
0153
0154     err = sock_queue_rcv_skb(sk, skb);
0155     if (err)
0156         goto error_skb;
0157
0158     spin_lock_bh(&sk->sk_write_queue.lock);
0159     if (!skb_queue_empty(&sk->sk_write_queue))
0160         schedule_work(&nfc_rawsock(sk)->tx_work);
0161     else
0162         nfc_rawsock(sk)->tx_work_scheduled = false;
0163     spin_unlock_bh(&sk->sk_write_queue.lock);
0164
0165     sock_put(sk);
0166     return;
0167
0168 error_skb:
0169     kfree_skb(skb);
0170
0171 error:
0172     rawsock_report_error(sk, err);
0173     sock_put(sk);
0174 }
0175
0176 static void rawsock_tx_work(struct work_struct *work)
0177 {
0178     struct sock *sk = to_rawsock_sk(work);
0179     struct nfc_dev *dev = nfc_rawsock(sk)->dev;
0180     u32 target_idx = nfc_rawsock(sk)->target_idx;
0181     struct sk_buff *skb;
0182     int rc;
0183
0184     pr_debug("sk=%p target_idx=%u\n", sk, target_idx);
0185
0186     if (sk->sk_shutdown & SEND_SHUTDOWN) {
0187         rawsock_write_queue_purge(sk);
0188         return;
0189     }
0190
0191     skb = skb_dequeue(&sk->sk_write_queue);
0192
0193     sock_hold(sk);
0194     rc = nfc_data_exchange(dev, target_idx, skb,
0195                    rawsock_data_exchange_complete, sk);
0196     if (rc) {
0197         rawsock_report_error(sk, rc);
0198         sock_put(sk);
0199     }
0200 }
0201
0202 static int rawsock_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
0203 {
0204     struct sock *sk = sock->sk;
0205     struct nfc_dev *dev = nfc_rawsock(sk)->dev;
0206     struct sk_buff *skb;
0207     int rc;
0208
0209     pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len);
0210
0211     if (msg->msg_namelen)
0212         return -EOPNOTSUPP;
0213
0214     if (sock->state != SS_CONNECTED)
0215         return -ENOTCONN;
0216
0217     skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc);
0218     if (skb == NULL)
0219         return rc;
0220
0221     rc = memcpy_from_msg(skb_put(skb, len), msg, len);
0222     if (rc < 0) {
0223         kfree_skb(skb);
0224         return rc;
0225     }
0226
0227     spin_lock_bh(&sk->sk_write_queue.lock);
0228     __skb_queue_tail(&sk->sk_write_queue, skb);
0229     if (!nfc_rawsock(sk)->tx_work_scheduled) {
0230         schedule_work(&nfc_rawsock(sk)->tx_work);
0231         nfc_rawsock(sk)->tx_work_scheduled = true;
0232     }
0233     spin_unlock_bh(&sk->sk_write_queue.lock);
0234
0235     return len;
0236 }
0237
0238 static int rawsock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
0239                int flags)
0240 {
0241     struct sock *sk = sock->sk;
0242     struct sk_buff *skb;
0243     int copied;
0244     int rc;
0245
0246     pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
0247
0248     skb = skb_recv_datagram(sk, flags, &rc);
0249     if (!skb)
0250         return rc;
0251
0252     copied = skb->len;
0253     if (len < copied) {
0254         msg->msg_flags |= MSG_TRUNC;
0255         copied = len;
0256     }
0257
0258     rc = skb_copy_datagram_msg(skb, 0, msg, copied);
0259
0260     skb_free_datagram(sk, skb);
0261
0262     return rc ? : copied;
0263 }
0264
0265 static const struct proto_ops rawsock_ops = {
0266     .family         = PF_NFC,
0267     .owner          = THIS_MODULE,
0268     .release        = rawsock_release,
0269     .bind           = sock_no_bind,
0270     .connect        = rawsock_connect,
0271     .socketpair     = sock_no_socketpair,
0272     .accept         = sock_no_accept,
0273     .getname        = sock_no_getname,
0274     .poll           = datagram_poll,
0275     .ioctl          = sock_no_ioctl,
0276     .listen         = sock_no_listen,
0277     .shutdown       = sock_no_shutdown,
0278     .sendmsg        = rawsock_sendmsg,
0279     .recvmsg        = rawsock_recvmsg,
0280     .mmap           = sock_no_mmap,
0281 };
0282
0283 static const struct proto_ops rawsock_raw_ops = {
0284     .family         = PF_NFC,
0285     .owner          = THIS_MODULE,
0286     .release        = rawsock_release,
0287     .bind           = sock_no_bind,
0288     .connect        = sock_no_connect,
0289     .socketpair     = sock_no_socketpair,
0290     .accept         = sock_no_accept,
0291     .getname        = sock_no_getname,
0292     .poll           = datagram_poll,
0293     .ioctl          = sock_no_ioctl,
0294     .listen         = sock_no_listen,
0295     .shutdown       = sock_no_shutdown,
0296     .sendmsg        = sock_no_sendmsg,
0297     .recvmsg        = rawsock_recvmsg,
0298     .mmap           = sock_no_mmap,
0299 };
0300
0301 static void rawsock_destruct(struct sock *sk)
0302 {
0303     pr_debug("sk=%p\n", sk);
0304
0305     if (sk->sk_state == TCP_ESTABLISHED) {
0306         nfc_deactivate_target(nfc_rawsock(sk)->dev,
0307                       nfc_rawsock(sk)->target_idx,
0308                       NFC_TARGET_MODE_IDLE);
0309         nfc_put_device(nfc_rawsock(sk)->dev);
0310     }
0311
0312     skb_queue_purge(&sk->sk_receive_queue);
0313
0314     if (!sock_flag(sk, SOCK_DEAD)) {
0315         pr_err("Freeing alive NFC raw socket %p\n", sk);
0316         return;
0317     }
0318 }
0319
0320 static int rawsock_create(struct net *net, struct socket *sock,
0321               const struct nfc_protocol *nfc_proto, int kern)
0322 {
0323     struct sock *sk;
0324
0325     pr_debug("sock=%p\n", sock);
0326
0327     if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
0328         return -ESOCKTNOSUPPORT;
0329
0330     if (sock->type == SOCK_RAW) {
0331         if (!ns_capable(net->user_ns, CAP_NET_RAW))
0332             return -EPERM;
0333         sock->ops = &rawsock_raw_ops;
0334     } else {
0335         sock->ops = &rawsock_ops;
0336     }
0337
0338     sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto, kern);
0339     if (!sk)
0340         return -ENOMEM;
0341
0342     sock_init_data(sock, sk);
0343     sk->sk_protocol = nfc_proto->id;
0344     sk->sk_destruct = rawsock_destruct;
0345     sock->state = SS_UNCONNECTED;
0346     if (sock->type == SOCK_RAW)
0347         nfc_sock_link(&raw_sk_list, sk);
0348     else {
0349         INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
0350         nfc_rawsock(sk)->tx_work_scheduled = false;
0351     }
0352
0353     return 0;
0354 }
0355
0356 void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
0357               u8 payload_type, u8 direction)
0358 {
0359     struct sk_buff *skb_copy = NULL, *nskb;
0360     struct sock *sk;
0361     u8 *data;
0362
0363     read_lock(&raw_sk_list.lock);
0364
0365     sk_for_each(sk, &raw_sk_list.head) {
0366         if (!skb_copy) {
0367             skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
0368                               GFP_ATOMIC, true);
0369             if (!skb_copy)
0370                 continue;
0371
0372             data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
0373
0374             data[0] = dev ? dev->idx : 0xFF;
0375             data[1] = direction & 0x01;
0376             data[1] |= (payload_type << 1);
0377         }
0378
0379         nskb = skb_clone(skb_copy, GFP_ATOMIC);
0380         if (!nskb)
0381             continue;
0382
0383         if (sock_queue_rcv_skb(sk, nskb))
0384             kfree_skb(nskb);
0385     }
0386
0387     read_unlock(&raw_sk_list.lock);
0388
0389     kfree_skb(skb_copy);
0390 }
0391 EXPORT_SYMBOL(nfc_send_to_raw_sock);
0392
0393 static struct proto rawsock_proto = {
0394     .name     = "NFC_RAW",
0395     .owner    = THIS_MODULE,
0396     .obj_size = sizeof(struct nfc_rawsock),
0397 };
0398
0399 static const struct nfc_protocol rawsock_nfc_proto = {
0400     .id   = NFC_SOCKPROTO_RAW,
0401     .proto    = &rawsock_proto,
0402     .owner    = THIS_MODULE,
0403     .create   = rawsock_create
0404 };
0405
0406 int __init rawsock_init(void)
0407 {
0408     int rc;
0409
0410     rc = nfc_proto_register(&rawsock_nfc_proto);
0411
0412     return rc;
0413 }
0414
0415 void rawsock_exit(void)
0416 {
0417     nfc_proto_unregister(&rawsock_nfc_proto);
0418 }