net/nfc/rawsock.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2011 Instituto Nokia de Tecnologia
   4  *
   5  * Authors:
   6  *    Aloisio Almeida Jr <aloisio.almeida@openbossa.org>
   7  *    Lauro Ramos Venancio <lauro.venancio@openbossa.org>
   8  */
   9
  10 #define pr_fmt(fmt) KBUILD_MODNAME ": %s: " fmt, __func__
  11
  12 #include <net/tcp_states.h>
  13 #include <linux/nfc.h>
  14 #include <linux/export.h>
  15 #include <linux/kcov.h>
  16
  17 #include "nfc.h"
  18
  19 static struct nfc_sock_list raw_sk_list = {
  20         .lock = __RW_LOCK_UNLOCKED(raw_sk_list.lock)
  21 };
  22
  23 static void nfc_sock_link(struct nfc_sock_list *l, struct sock *sk)
  24 {
  25         write_lock(&l->lock);
  26         sk_add_node(sk, &l->head);
  27         write_unlock(&l->lock);
  28 }
  29
  30 static void nfc_sock_unlink(struct nfc_sock_list *l, struct sock *sk)
  31 {
  32         write_lock(&l->lock);
  33         sk_del_node_init(sk);
  34         write_unlock(&l->lock);
  35 }
  36
  37 static void rawsock_write_queue_purge(struct sock *sk)
  38 {
  39         pr_debug("sk=%p\n", sk);
  40
  41         spin_lock_bh(&sk->sk_write_queue.lock);
  42         __skb_queue_purge(&sk->sk_write_queue);
  43         nfc_rawsock(sk)->tx_work_scheduled = false;
  44         spin_unlock_bh(&sk->sk_write_queue.lock);
  45 }
  46
  47 static void rawsock_report_error(struct sock *sk, int err)
  48 {
  49         pr_debug("sk=%p err=%d\n", sk, err);
  50
  51         sk->sk_shutdown = SHUTDOWN_MASK;
  52         sk->sk_err = -err;
  53         sk_error_report(sk);
  54
  55         rawsock_write_queue_purge(sk);
  56 }
  57
  58 static int rawsock_release(struct socket *sock)
  59 {
  60         struct sock *sk = sock->sk;
  61
  62         pr_debug("sock=%p sk=%p\n", sock, sk);
  63
  64         if (!sk)
  65                 return 0;
  66
  67         if (sock->type == SOCK_RAW)
  68                 nfc_sock_unlink(&raw_sk_list, sk);
  69
  70         sock_orphan(sk);
  71         sock_put(sk);
  72
  73         return 0;
  74 }
  75
  76 static int rawsock_connect(struct socket *sock, struct sockaddr *_addr,
  77                            int len, int flags)
  78 {
  79         struct sock *sk = sock->sk;
  80         struct sockaddr_nfc *addr = (struct sockaddr_nfc *)_addr;
  81         struct nfc_dev *dev;
  82         int rc = 0;
  83
  84         pr_debug("sock=%p sk=%p flags=%d\n", sock, sk, flags);
  85
  86         if (!addr || len < sizeof(struct sockaddr_nfc) ||
  87             addr->sa_family != AF_NFC)
  88                 return -EINVAL;
  89
  90         pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n",
  91                  addr->dev_idx, addr->target_idx, addr->nfc_protocol);
  92
  93         lock_sock(sk);
  94
  95         if (sock->state == SS_CONNECTED) {
  96                 rc = -EISCONN;
  97                 goto error;
  98         }
  99
 100         dev = nfc_get_device(addr->dev_idx);
 101         if (!dev) {
 102                 rc = -ENODEV;
 103                 goto error;
 104         }
 105
 106         if (addr->target_idx > dev->target_next_idx - 1 ||
 107             addr->target_idx < dev->target_next_idx - dev->n_targets) {
 108                 rc = -EINVAL;
 109                 goto put_dev;
 110         }
 111
 112         rc = nfc_activate_target(dev, addr->target_idx, addr->nfc_protocol);
 113         if (rc)
 114                 goto put_dev;
 115
 116         nfc_rawsock(sk)->dev = dev;
 117         nfc_rawsock(sk)->target_idx = addr->target_idx;
 118         sock->state = SS_CONNECTED;
 119         sk->sk_state = TCP_ESTABLISHED;
 120         sk->sk_state_change(sk);
 121
 122         release_sock(sk);
 123         return 0;
 124
 125 put_dev:
 126         nfc_put_device(dev);
 127 error:
 128         release_sock(sk);
 129         return rc;
 130 }
 131
 132 static int rawsock_add_header(struct sk_buff *skb)
 133 {
 134         *(u8 *)skb_push(skb, NFC_HEADER_SIZE) = 0;
 135
 136         return 0;
 137 }
 138
 139 static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
 140                                            int err)
 141 {
 142         struct sock *sk = (struct sock *) context;
 143
 144         BUG_ON(in_hardirq());
 145
 146         pr_debug("sk=%p err=%d\n", sk, err);
 147
 148         if (err)
 149                 goto error;
 150
 151         err = rawsock_add_header(skb);
 152         if (err)
 153                 goto error_skb;
 154
 155         err = sock_queue_rcv_skb(sk, skb);
 156         if (err)
 157                 goto error_skb;
 158
 159         spin_lock_bh(&sk->sk_write_queue.lock);
 160         if (!skb_queue_empty(&sk->sk_write_queue))
 161                 schedule_work(&nfc_rawsock(sk)->tx_work);
 162         else
 163                 nfc_rawsock(sk)->tx_work_scheduled = false;
 164         spin_unlock_bh(&sk->sk_write_queue.lock);
 165
 166         sock_put(sk);
 167         return;
 168
 169 error_skb:
 170         kfree_skb(skb);
 171
 172 error:
 173         rawsock_report_error(sk, err);
 174         sock_put(sk);
 175 }
 176
 177 static void rawsock_tx_work(struct work_struct *work)
 178 {
 179         struct sock *sk = to_rawsock_sk(work);
 180         struct nfc_dev *dev = nfc_rawsock(sk)->dev;
 181         u32 target_idx = nfc_rawsock(sk)->target_idx;
 182         struct sk_buff *skb;
 183         int rc;
 184
 185         pr_debug("sk=%p target_idx=%u\n", sk, target_idx);
 186
 187         if (sk->sk_shutdown & SEND_SHUTDOWN) {
 188                 rawsock_write_queue_purge(sk);
 189                 return;
 190         }
 191
 192         skb = skb_dequeue(&sk->sk_write_queue);
 193         kcov_remote_start_common(skb_get_kcov_handle(skb));
 194
 195         sock_hold(sk);
 196         rc = nfc_data_exchange(dev, target_idx, skb,
 197                                rawsock_data_exchange_complete, sk);
 198         if (rc) {
 199                 rawsock_report_error(sk, rc);
 200                 sock_put(sk);
 201         }
 202         kcov_remote_stop();
 203 }
 204
 205 static int rawsock_sendmsg(struct socket *sock, struct msghdr *msg, size_t len)
 206 {
 207         struct sock *sk = sock->sk;
 208         struct nfc_dev *dev = nfc_rawsock(sk)->dev;
 209         struct sk_buff *skb;
 210         int rc;
 211
 212         pr_debug("sock=%p sk=%p len=%zu\n", sock, sk, len);
 213
 214         if (msg->msg_namelen)
 215                 return -EOPNOTSUPP;
 216
 217         if (sock->state != SS_CONNECTED)
 218                 return -ENOTCONN;
 219
 220         skb = nfc_alloc_send_skb(dev, sk, msg->msg_flags, len, &rc);
 221         if (skb == NULL)
 222                 return rc;
 223
 224         rc = memcpy_from_msg(skb_put(skb, len), msg, len);
 225         if (rc < 0) {
 226                 kfree_skb(skb);
 227                 return rc;
 228         }
 229
 230         spin_lock_bh(&sk->sk_write_queue.lock);
 231         __skb_queue_tail(&sk->sk_write_queue, skb);
 232         if (!nfc_rawsock(sk)->tx_work_scheduled) {
 233                 schedule_work(&nfc_rawsock(sk)->tx_work);
 234                 nfc_rawsock(sk)->tx_work_scheduled = true;
 235         }
 236         spin_unlock_bh(&sk->sk_write_queue.lock);
 237
 238         return len;
 239 }
 240
 241 static int rawsock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len,
 242                            int flags)
 243 {
 244         struct sock *sk = sock->sk;
 245         struct sk_buff *skb;
 246         int copied;
 247         int rc;
 248
 249         pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
 250
 251         skb = skb_recv_datagram(sk, flags, &rc);
 252         if (!skb)
 253                 return rc;
 254
 255         copied = skb->len;
 256         if (len < copied) {
 257                 msg->msg_flags |= MSG_TRUNC;
 258                 copied = len;
 259         }
 260
 261         rc = skb_copy_datagram_msg(skb, 0, msg, copied);
 262
 263         skb_free_datagram(sk, skb);
 264
 265         return rc ? : copied;
 266 }
 267
 268 static const struct proto_ops rawsock_ops = {
 269         .family         = PF_NFC,
 270         .owner          = THIS_MODULE,
 271         .release        = rawsock_release,
 272         .bind           = sock_no_bind,
 273         .connect        = rawsock_connect,
 274         .socketpair     = sock_no_socketpair,
 275         .accept         = sock_no_accept,
 276         .getname        = sock_no_getname,
 277         .poll           = datagram_poll,
 278         .ioctl          = sock_no_ioctl,
 279         .listen         = sock_no_listen,
 280         .shutdown       = sock_no_shutdown,
 281         .sendmsg        = rawsock_sendmsg,
 282         .recvmsg        = rawsock_recvmsg,
 283         .mmap           = sock_no_mmap,
 284 };
 285
 286 static const struct proto_ops rawsock_raw_ops = {
 287         .family         = PF_NFC,
 288         .owner          = THIS_MODULE,
 289         .release        = rawsock_release,
 290         .bind           = sock_no_bind,
 291         .connect        = sock_no_connect,
 292         .socketpair     = sock_no_socketpair,
 293         .accept         = sock_no_accept,
 294         .getname        = sock_no_getname,
 295         .poll           = datagram_poll,
 296         .ioctl          = sock_no_ioctl,
 297         .listen         = sock_no_listen,
 298         .shutdown       = sock_no_shutdown,
 299         .sendmsg        = sock_no_sendmsg,
 300         .recvmsg        = rawsock_recvmsg,
 301         .mmap           = sock_no_mmap,
 302 };
 303
 304 static void rawsock_destruct(struct sock *sk)
 305 {
 306         pr_debug("sk=%p\n", sk);
 307
 308         if (sk->sk_state == TCP_ESTABLISHED) {
 309                 nfc_deactivate_target(nfc_rawsock(sk)->dev,
 310                                       nfc_rawsock(sk)->target_idx,
 311                                       NFC_TARGET_MODE_IDLE);
 312                 nfc_put_device(nfc_rawsock(sk)->dev);
 313         }
 314
 315         skb_queue_purge(&sk->sk_receive_queue);
 316
 317         if (!sock_flag(sk, SOCK_DEAD)) {
 318                 pr_err("Freeing alive NFC raw socket %p\n", sk);
 319                 return;
 320         }
 321 }
 322
 323 static int rawsock_create(struct net *net, struct socket *sock,
 324                           const struct nfc_protocol *nfc_proto, int kern)
 325 {
 326         struct sock *sk;
 327
 328         pr_debug("sock=%p\n", sock);
 329
 330         if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
 331                 return -ESOCKTNOSUPPORT;
 332
 333         if (sock->type == SOCK_RAW) {
 334                 if (!ns_capable(net->user_ns, CAP_NET_RAW))
 335                         return -EPERM;
 336                 sock->ops = &rawsock_raw_ops;
 337         } else {
 338                 sock->ops = &rawsock_ops;
 339         }
 340
 341         sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto, kern);
 342         if (!sk)
 343                 return -ENOMEM;
 344
 345         sock_init_data(sock, sk);
 346         sk->sk_protocol = nfc_proto->id;
 347         sk->sk_destruct = rawsock_destruct;
 348         sock->state = SS_UNCONNECTED;
 349         if (sock->type == SOCK_RAW)
 350                 nfc_sock_link(&raw_sk_list, sk);
 351         else {
 352                 INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
 353                 nfc_rawsock(sk)->tx_work_scheduled = false;
 354         }
 355
 356         return 0;
 357 }
 358
 359 void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
 360                           u8 payload_type, u8 direction)
 361 {
 362         struct sk_buff *skb_copy = NULL, *nskb;
 363         struct sock *sk;
 364         u8 *data;
 365
 366         read_lock(&raw_sk_list.lock);
 367
 368         sk_for_each(sk, &raw_sk_list.head) {
 369                 if (!skb_copy) {
 370                         skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
 371                                                       GFP_ATOMIC, true);
 372                         if (!skb_copy)
 373                                 continue;
 374
 375                         data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
 376
 377                         data[0] = dev ? dev->idx : 0xFF;
 378                         data[1] = direction & 0x01;
 379                         data[1] |= (payload_type << 1);
 380                 }
 381
 382                 nskb = skb_clone(skb_copy, GFP_ATOMIC);
 383                 if (!nskb)
 384                         continue;
 385
 386                 if (sock_queue_rcv_skb(sk, nskb))
 387                         kfree_skb(nskb);
 388         }
 389
 390         read_unlock(&raw_sk_list.lock);
 391
 392         kfree_skb(skb_copy);
 393 }
 394 EXPORT_SYMBOL(nfc_send_to_raw_sock);
 395
 396 static struct proto rawsock_proto = {
 397         .name     = "NFC_RAW",
 398         .owner    = THIS_MODULE,
 399         .obj_size = sizeof(struct nfc_rawsock),
 400 };
 401
 402 static const struct nfc_protocol rawsock_nfc_proto = {
 403         .id       = NFC_SOCKPROTO_RAW,
 404         .proto    = &rawsock_proto,
 405         .owner    = THIS_MODULE,
 406         .create   = rawsock_create
 407 };
 408
 409 int __init rawsock_init(void)
 410 {
 411         int rc;
 412
 413         rc = nfc_proto_register(&rawsock_nfc_proto);
 414
 415         return rc;
 416 }
 417
 418 void rawsock_exit(void)
 419 {
 420         nfc_proto_unregister(&rawsock_nfc_proto);
 421 }