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->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 error;
 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_irq());
 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         int noblock = flags & MSG_DONTWAIT;
 242         struct sock *sk = sock->sk;
 243         struct sk_buff *skb;
 244         int copied;
 245         int rc;
 246
 247         pr_debug("sock=%p sk=%p len=%zu flags=%d\n", sock, sk, len, flags);
 248
 249         skb = skb_recv_datagram(sk, flags, noblock, &rc);
 250         if (!skb)
 251                 return rc;
 252
 253         copied = skb->len;
 254         if (len < copied) {
 255                 msg->msg_flags |= MSG_TRUNC;
 256                 copied = len;
 257         }
 258
 259         rc = skb_copy_datagram_msg(skb, 0, msg, copied);
 260
 261         skb_free_datagram(sk, skb);
 262
 263         return rc ? : copied;
 264 }
 265
 266 static const struct proto_ops rawsock_ops = {
 267         .family         = PF_NFC,
 268         .owner          = THIS_MODULE,
 269         .release        = rawsock_release,
 270         .bind           = sock_no_bind,
 271         .connect        = rawsock_connect,
 272         .socketpair     = sock_no_socketpair,
 273         .accept         = sock_no_accept,
 274         .getname        = sock_no_getname,
 275         .poll           = datagram_poll,
 276         .ioctl          = sock_no_ioctl,
 277         .listen         = sock_no_listen,
 278         .shutdown       = sock_no_shutdown,
 279         .sendmsg        = rawsock_sendmsg,
 280         .recvmsg        = rawsock_recvmsg,
 281         .mmap           = sock_no_mmap,
 282 };
 283
 284 static const struct proto_ops rawsock_raw_ops = {
 285         .family         = PF_NFC,
 286         .owner          = THIS_MODULE,
 287         .release        = rawsock_release,
 288         .bind           = sock_no_bind,
 289         .connect        = sock_no_connect,
 290         .socketpair     = sock_no_socketpair,
 291         .accept         = sock_no_accept,
 292         .getname        = sock_no_getname,
 293         .poll           = datagram_poll,
 294         .ioctl          = sock_no_ioctl,
 295         .listen         = sock_no_listen,
 296         .shutdown       = sock_no_shutdown,
 297         .sendmsg        = sock_no_sendmsg,
 298         .recvmsg        = rawsock_recvmsg,
 299         .mmap           = sock_no_mmap,
 300 };
 301
 302 static void rawsock_destruct(struct sock *sk)
 303 {
 304         pr_debug("sk=%p\n", sk);
 305
 306         if (sk->sk_state == TCP_ESTABLISHED) {
 307                 nfc_deactivate_target(nfc_rawsock(sk)->dev,
 308                                       nfc_rawsock(sk)->target_idx,
 309                                       NFC_TARGET_MODE_IDLE);
 310                 nfc_put_device(nfc_rawsock(sk)->dev);
 311         }
 312
 313         skb_queue_purge(&sk->sk_receive_queue);
 314
 315         if (!sock_flag(sk, SOCK_DEAD)) {
 316                 pr_err("Freeing alive NFC raw socket %p\n", sk);
 317                 return;
 318         }
 319 }
 320
 321 static int rawsock_create(struct net *net, struct socket *sock,
 322                           const struct nfc_protocol *nfc_proto, int kern)
 323 {
 324         struct sock *sk;
 325
 326         pr_debug("sock=%p\n", sock);
 327
 328         if ((sock->type != SOCK_SEQPACKET) && (sock->type != SOCK_RAW))
 329                 return -ESOCKTNOSUPPORT;
 330
 331         if (sock->type == SOCK_RAW)
 332                 sock->ops = &rawsock_raw_ops;
 333         else
 334                 sock->ops = &rawsock_ops;
 335
 336         sk = sk_alloc(net, PF_NFC, GFP_ATOMIC, nfc_proto->proto, kern);
 337         if (!sk)
 338                 return -ENOMEM;
 339
 340         sock_init_data(sock, sk);
 341         sk->sk_protocol = nfc_proto->id;
 342         sk->sk_destruct = rawsock_destruct;
 343         sock->state = SS_UNCONNECTED;
 344         if (sock->type == SOCK_RAW)
 345                 nfc_sock_link(&raw_sk_list, sk);
 346         else {
 347                 INIT_WORK(&nfc_rawsock(sk)->tx_work, rawsock_tx_work);
 348                 nfc_rawsock(sk)->tx_work_scheduled = false;
 349         }
 350
 351         return 0;
 352 }
 353
 354 void nfc_send_to_raw_sock(struct nfc_dev *dev, struct sk_buff *skb,
 355                           u8 payload_type, u8 direction)
 356 {
 357         struct sk_buff *skb_copy = NULL, *nskb;
 358         struct sock *sk;
 359         u8 *data;
 360
 361         read_lock(&raw_sk_list.lock);
 362
 363         sk_for_each(sk, &raw_sk_list.head) {
 364                 if (!skb_copy) {
 365                         skb_copy = __pskb_copy_fclone(skb, NFC_RAW_HEADER_SIZE,
 366                                                       GFP_ATOMIC, true);
 367                         if (!skb_copy)
 368                                 continue;
 369
 370                         data = skb_push(skb_copy, NFC_RAW_HEADER_SIZE);
 371
 372                         data[0] = dev ? dev->idx : 0xFF;
 373                         data[1] = direction & 0x01;
 374                         data[1] |= (payload_type << 1);
 375                 }
 376
 377                 nskb = skb_clone(skb_copy, GFP_ATOMIC);
 378                 if (!nskb)
 379                         continue;
 380
 381                 if (sock_queue_rcv_skb(sk, nskb))
 382                         kfree_skb(nskb);
 383         }
 384
 385         read_unlock(&raw_sk_list.lock);
 386
 387         kfree_skb(skb_copy);
 388 }
 389 EXPORT_SYMBOL(nfc_send_to_raw_sock);
 390
 391 static struct proto rawsock_proto = {
 392         .name     = "NFC_RAW",
 393         .owner    = THIS_MODULE,
 394         .obj_size = sizeof(struct nfc_rawsock),
 395 };
 396
 397 static const struct nfc_protocol rawsock_nfc_proto = {
 398         .id       = NFC_SOCKPROTO_RAW,
 399         .proto    = &rawsock_proto,
 400         .owner    = THIS_MODULE,
 401         .create   = rawsock_create
 402 };
 403
 404 int __init rawsock_init(void)
 405 {
 406         int rc;
 407
 408         rc = nfc_proto_register(&rawsock_nfc_proto);
 409
 410         return rc;
 411 }
 412
 413 void rawsock_exit(void)
 414 {
 415         nfc_proto_unregister(&rawsock_nfc_proto);
 416 }