net/nfc/rawsock.c

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