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