net/nfc/llcp/sock.c

   1 /*
   2  * Copyright (C) 2011  Intel Corporation. All rights reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program; if not, write to the
  16  * Free Software Foundation, Inc.,
  17  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18  */
  19
  20 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
  21
  22 #include <linux/init.h>
  23 #include <linux/kernel.h>
  24 #include <linux/module.h>
  25 #include <linux/nfc.h>
  26
  27 #include "../nfc.h"
  28 #include "llcp.h"
  29
  30 static struct proto llcp_sock_proto = {
  31         .name     = "NFC_LLCP",
  32         .owner    = THIS_MODULE,
  33         .obj_size = sizeof(struct nfc_llcp_sock),
  34 };
  35
  36 static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
  37 {
  38         struct sock *sk = sock->sk;
  39         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  40         struct nfc_llcp_local *local;
  41         struct nfc_dev *dev;
  42         struct sockaddr_nfc_llcp llcp_addr;
  43         int len, ret = 0;
  44
  45         pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
  46
  47         if (!addr || addr->sa_family != AF_NFC)
  48                 return -EINVAL;
  49
  50         memset(&llcp_addr, 0, sizeof(llcp_addr));
  51         len = min_t(unsigned int, sizeof(llcp_addr), alen);
  52         memcpy(&llcp_addr, addr, len);
  53
  54         /* This is going to be a listening socket, dsap must be 0 */
  55         if (llcp_addr.dsap != 0)
  56                 return -EINVAL;
  57
  58         lock_sock(sk);
  59
  60         if (sk->sk_state != LLCP_CLOSED) {
  61                 ret = -EBADFD;
  62                 goto error;
  63         }
  64
  65         dev = nfc_get_device(llcp_addr.dev_idx);
  66         if (dev == NULL) {
  67                 ret = -ENODEV;
  68                 goto error;
  69         }
  70
  71         local = nfc_llcp_find_local(dev);
  72         if (local == NULL) {
  73                 ret = -ENODEV;
  74                 goto put_dev;
  75         }
  76
  77         llcp_sock->dev = dev;
  78         llcp_sock->local = local;
  79         llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
  80         llcp_sock->service_name_len = min_t(unsigned int,
  81                         llcp_addr.service_name_len, NFC_LLCP_MAX_SERVICE_NAME);
  82         llcp_sock->service_name = kmemdup(llcp_addr.service_name,
  83                                 llcp_sock->service_name_len, GFP_KERNEL);
  84
  85         llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
  86         if (llcp_sock->ssap == LLCP_MAX_SAP)
  87                 goto put_dev;
  88
  89         local->sockets[llcp_sock->ssap] = llcp_sock;
  90
  91         pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
  92
  93         sk->sk_state = LLCP_BOUND;
  94
  95 put_dev:
  96         nfc_put_device(dev);
  97
  98 error:
  99         release_sock(sk);
 100         return ret;
 101 }
 102
 103 static int llcp_sock_listen(struct socket *sock, int backlog)
 104 {
 105         struct sock *sk = sock->sk;
 106         int ret = 0;
 107
 108         pr_debug("sk %p backlog %d\n", sk, backlog);
 109
 110         lock_sock(sk);
 111
 112         if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
 113                         || sk->sk_state != LLCP_BOUND) {
 114                 ret = -EBADFD;
 115                 goto error;
 116         }
 117
 118         sk->sk_max_ack_backlog = backlog;
 119         sk->sk_ack_backlog = 0;
 120
 121         pr_debug("Socket listening\n");
 122         sk->sk_state = LLCP_LISTEN;
 123
 124 error:
 125         release_sock(sk);
 126
 127         return ret;
 128 }
 129
 130 void nfc_llcp_accept_unlink(struct sock *sk)
 131 {
 132         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 133
 134         pr_debug("state %d\n", sk->sk_state);
 135
 136         list_del_init(&llcp_sock->accept_queue);
 137         sk_acceptq_removed(llcp_sock->parent);
 138         llcp_sock->parent = NULL;
 139
 140         sock_put(sk);
 141 }
 142
 143 void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
 144 {
 145         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 146         struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
 147
 148         /* Lock will be free from unlink */
 149         sock_hold(sk);
 150
 151         list_add_tail(&llcp_sock->accept_queue,
 152                         &llcp_sock_parent->accept_queue);
 153         llcp_sock->parent = parent;
 154         sk_acceptq_added(parent);
 155 }
 156
 157 struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
 158                                         struct socket *newsock)
 159 {
 160         struct nfc_llcp_sock *lsk, *n, *llcp_parent;
 161         struct sock *sk;
 162
 163         llcp_parent = nfc_llcp_sock(parent);
 164
 165         list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
 166                                                         accept_queue) {
 167                 sk = &lsk->sk;
 168                 lock_sock(sk);
 169
 170                 if (sk->sk_state == LLCP_CLOSED) {
 171                         release_sock(sk);
 172                         nfc_llcp_accept_unlink(sk);
 173                         continue;
 174                 }
 175
 176                 if (sk->sk_state == LLCP_CONNECTED || !newsock) {
 177                         nfc_llcp_accept_unlink(sk);
 178                         if (newsock)
 179                                 sock_graft(sk, newsock);
 180
 181                         release_sock(sk);
 182
 183                         pr_debug("Returning sk state %d\n", sk->sk_state);
 184
 185                         return sk;
 186                 }
 187
 188                 release_sock(sk);
 189         }
 190
 191         return NULL;
 192 }
 193
 194 static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
 195                                                                 int flags)
 196 {
 197         DECLARE_WAITQUEUE(wait, current);
 198         struct sock *sk = sock->sk, *new_sk;
 199         long timeo;
 200         int ret = 0;
 201
 202         pr_debug("parent %p\n", sk);
 203
 204         lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
 205
 206         if (sk->sk_state != LLCP_LISTEN) {
 207                 ret = -EBADFD;
 208                 goto error;
 209         }
 210
 211         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
 212
 213         /* Wait for an incoming connection. */
 214         add_wait_queue_exclusive(sk_sleep(sk), &wait);
 215         while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
 216                 set_current_state(TASK_INTERRUPTIBLE);
 217
 218                 if (!timeo) {
 219                         ret = -EAGAIN;
 220                         break;
 221                 }
 222
 223                 if (signal_pending(current)) {
 224                         ret = sock_intr_errno(timeo);
 225                         break;
 226                 }
 227
 228                 release_sock(sk);
 229                 timeo = schedule_timeout(timeo);
 230                 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
 231         }
 232         __set_current_state(TASK_RUNNING);
 233         remove_wait_queue(sk_sleep(sk), &wait);
 234
 235         if (ret)
 236                 goto error;
 237
 238         newsock->state = SS_CONNECTED;
 239
 240         pr_debug("new socket %p\n", new_sk);
 241
 242 error:
 243         release_sock(sk);
 244
 245         return ret;
 246 }
 247
 248 static int llcp_sock_getname(struct socket *sock, struct sockaddr *addr,
 249                              int *len, int peer)
 250 {
 251         struct sockaddr_nfc_llcp *llcp_addr = (struct sockaddr_nfc_llcp *) addr;
 252         struct sock *sk = sock->sk;
 253         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 254
 255         pr_debug("%p\n", sk);
 256
 257         addr->sa_family = AF_NFC;
 258         *len = sizeof(struct sockaddr_nfc_llcp);
 259
 260         llcp_addr->dev_idx = llcp_sock->dev->idx;
 261         llcp_addr->dsap = llcp_sock->dsap;
 262         llcp_addr->ssap = llcp_sock->ssap;
 263         llcp_addr->service_name_len = llcp_sock->service_name_len;
 264         memcpy(llcp_addr->service_name, llcp_sock->service_name,
 265                                         llcp_addr->service_name_len);
 266
 267         return 0;
 268 }
 269
 270 static inline unsigned int llcp_accept_poll(struct sock *parent)
 271 {
 272         struct nfc_llcp_sock *llcp_sock, *n, *parent_sock;
 273         struct sock *sk;
 274
 275         parent_sock = nfc_llcp_sock(parent);
 276
 277         list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue,
 278                                                                 accept_queue) {
 279                 sk = &llcp_sock->sk;
 280
 281                 if (sk->sk_state == LLCP_CONNECTED)
 282                         return POLLIN | POLLRDNORM;
 283         }
 284
 285         return 0;
 286 }
 287
 288 static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
 289                                                         poll_table *wait)
 290 {
 291         struct sock *sk = sock->sk;
 292         unsigned int mask = 0;
 293
 294         pr_debug("%p\n", sk);
 295
 296         sock_poll_wait(file, sk_sleep(sk), wait);
 297
 298         if (sk->sk_state == LLCP_LISTEN)
 299                 return llcp_accept_poll(sk);
 300
 301         if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
 302                 mask |= POLLERR;
 303
 304         if (!skb_queue_empty(&sk->sk_receive_queue))
 305                 mask |= POLLIN;
 306
 307         if (sk->sk_state == LLCP_CLOSED)
 308                 mask |= POLLHUP;
 309
 310         return mask;
 311 }
 312
 313 static int llcp_sock_release(struct socket *sock)
 314 {
 315         struct sock *sk = sock->sk;
 316         struct nfc_llcp_local *local;
 317         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 318
 319         if (!sk)
 320                 return 0;
 321
 322         pr_debug("%p\n", sk);
 323
 324         local = llcp_sock->local;
 325         if (local == NULL)
 326                 return -ENODEV;
 327
 328         mutex_lock(&local->socket_lock);
 329
 330         if (llcp_sock == local->sockets[llcp_sock->ssap]) {
 331                 local->sockets[llcp_sock->ssap] = NULL;
 332         } else {
 333                 struct nfc_llcp_sock *parent, *s, *n;
 334
 335                 parent = local->sockets[llcp_sock->ssap];
 336
 337                 list_for_each_entry_safe(s, n, &parent->list, list)
 338                         if (llcp_sock == s) {
 339                                 list_del(&s->list);
 340                                 break;
 341                         }
 342
 343         }
 344
 345         mutex_unlock(&local->socket_lock);
 346
 347         lock_sock(sk);
 348
 349         /* Send a DISC */
 350         if (sk->sk_state == LLCP_CONNECTED)
 351                 nfc_llcp_disconnect(llcp_sock);
 352
 353         if (sk->sk_state == LLCP_LISTEN) {
 354                 struct nfc_llcp_sock *lsk, *n;
 355                 struct sock *accept_sk;
 356
 357                 list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
 358                                                                 accept_queue) {
 359                         accept_sk = &lsk->sk;
 360                         lock_sock(accept_sk);
 361
 362                         nfc_llcp_disconnect(lsk);
 363                         nfc_llcp_accept_unlink(accept_sk);
 364
 365                         release_sock(accept_sk);
 366
 367                         sock_set_flag(sk, SOCK_DEAD);
 368                         sock_orphan(accept_sk);
 369                         sock_put(accept_sk);
 370                 }
 371         }
 372
 373         /* Freeing the SAP */
 374         if ((sk->sk_state == LLCP_CONNECTED
 375                         && llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) ||
 376             sk->sk_state == LLCP_BOUND ||
 377             sk->sk_state == LLCP_LISTEN)
 378                 nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
 379
 380         sock_set_flag(sk, SOCK_DEAD);
 381
 382         release_sock(sk);
 383
 384         sock_orphan(sk);
 385         sock_put(sk);
 386
 387         return 0;
 388 }
 389
 390 static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
 391                                                         int len, int flags)
 392 {
 393         struct sock *sk = sock->sk;
 394         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 395         struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
 396         struct nfc_dev *dev;
 397         struct nfc_llcp_local *local;
 398         int ret = 0;
 399
 400         pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
 401
 402         if (!addr || len < sizeof(struct sockaddr_nfc) ||
 403                         addr->sa_family != AF_NFC) {
 404                 pr_err("Invalid socket\n");
 405                 return -EINVAL;
 406         }
 407
 408         if (addr->service_name_len == 0 && addr->dsap == 0) {
 409                 pr_err("Missing service name or dsap\n");
 410                 return -EINVAL;
 411         }
 412
 413         pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
 414                                         addr->target_idx, addr->nfc_protocol);
 415
 416         lock_sock(sk);
 417
 418         if (sk->sk_state == LLCP_CONNECTED) {
 419                 ret = -EISCONN;
 420                 goto error;
 421         }
 422
 423         dev = nfc_get_device(addr->dev_idx);
 424         if (dev == NULL) {
 425                 ret = -ENODEV;
 426                 goto error;
 427         }
 428
 429         local = nfc_llcp_find_local(dev);
 430         if (local == NULL) {
 431                 ret = -ENODEV;
 432                 goto put_dev;
 433         }
 434
 435         device_lock(&dev->dev);
 436         if (dev->dep_link_up == false) {
 437                 ret = -ENOLINK;
 438                 device_unlock(&dev->dev);
 439                 goto put_dev;
 440         }
 441         device_unlock(&dev->dev);
 442
 443         if (local->rf_mode == NFC_RF_INITIATOR &&
 444                         addr->target_idx != local->target_idx) {
 445                 ret = -ENOLINK;
 446                 goto put_dev;
 447         }
 448
 449         llcp_sock->dev = dev;
 450         llcp_sock->local = local;
 451         llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
 452         if (llcp_sock->ssap == LLCP_SAP_MAX) {
 453                 ret = -ENOMEM;
 454                 goto put_dev;
 455         }
 456         if (addr->service_name_len == 0)
 457                 llcp_sock->dsap = addr->dsap;
 458         else
 459                 llcp_sock->dsap = LLCP_SAP_SDP;
 460         llcp_sock->nfc_protocol = addr->nfc_protocol;
 461         llcp_sock->service_name_len = min_t(unsigned int,
 462                         addr->service_name_len, NFC_LLCP_MAX_SERVICE_NAME);
 463         llcp_sock->service_name = kmemdup(addr->service_name,
 464                                  llcp_sock->service_name_len, GFP_KERNEL);
 465
 466         local->sockets[llcp_sock->ssap] = llcp_sock;
 467
 468         ret = nfc_llcp_send_connect(llcp_sock);
 469         if (ret)
 470                 goto put_dev;
 471
 472         sk->sk_state = LLCP_CONNECTED;
 473
 474         release_sock(sk);
 475         return 0;
 476
 477 put_dev:
 478         nfc_put_device(dev);
 479
 480 error:
 481         release_sock(sk);
 482         return ret;
 483 }
 484
 485 static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 486                              struct msghdr *msg, size_t len, int flags)
 487 {
 488         int noblock = flags & MSG_DONTWAIT;
 489         struct sock *sk = sock->sk;
 490         unsigned int copied, rlen;
 491         struct sk_buff *skb, *cskb;
 492         int err = 0;
 493
 494         pr_debug("%p %zu\n", sk, len);
 495
 496         lock_sock(sk);
 497
 498         if (sk->sk_state == LLCP_CLOSED &&
 499                         skb_queue_empty(&sk->sk_receive_queue)) {
 500                 release_sock(sk);
 501                 return 0;
 502         }
 503
 504         release_sock(sk);
 505
 506         if (flags & (MSG_OOB))
 507                 return -EOPNOTSUPP;
 508
 509         skb = skb_recv_datagram(sk, flags, noblock, &err);
 510         if (!skb) {
 511                 pr_err("Recv datagram failed state %d %d %d",
 512                                 sk->sk_state, err, sock_error(sk));
 513
 514                 if (sk->sk_shutdown & RCV_SHUTDOWN)
 515                         return 0;
 516
 517                 return err;
 518         }
 519
 520         rlen   = skb->len;              /* real length of skb */
 521         copied = min_t(unsigned int, rlen, len);
 522
 523         cskb = skb;
 524         if (memcpy_toiovec(msg->msg_iov, cskb->data, copied)) {
 525                 if (!(flags & MSG_PEEK))
 526                         skb_queue_head(&sk->sk_receive_queue, skb);
 527                 return -EFAULT;
 528         }
 529
 530         /* Mark read part of skb as used */
 531         if (!(flags & MSG_PEEK)) {
 532
 533                 /* SOCK_STREAM: re-queue skb if it contains unreceived data */
 534                 if (sk->sk_type == SOCK_STREAM) {
 535                         skb_pull(skb, copied);
 536                         if (skb->len) {
 537                                 skb_queue_head(&sk->sk_receive_queue, skb);
 538                                 goto done;
 539                         }
 540                 }
 541
 542                 kfree_skb(skb);
 543         }
 544
 545         /* XXX Queue backlogged skbs */
 546
 547 done:
 548         /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
 549         if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
 550                 copied = rlen;
 551
 552         return copied;
 553 }
 554
 555 static const struct proto_ops llcp_sock_ops = {
 556         .family         = PF_NFC,
 557         .owner          = THIS_MODULE,
 558         .bind           = llcp_sock_bind,
 559         .connect        = llcp_sock_connect,
 560         .release        = llcp_sock_release,
 561         .socketpair     = sock_no_socketpair,
 562         .accept         = llcp_sock_accept,
 563         .getname        = llcp_sock_getname,
 564         .poll           = llcp_sock_poll,
 565         .ioctl          = sock_no_ioctl,
 566         .listen         = llcp_sock_listen,
 567         .shutdown       = sock_no_shutdown,
 568         .setsockopt     = sock_no_setsockopt,
 569         .getsockopt     = sock_no_getsockopt,
 570         .sendmsg        = sock_no_sendmsg,
 571         .recvmsg        = llcp_sock_recvmsg,
 572         .mmap           = sock_no_mmap,
 573 };
 574
 575 static void llcp_sock_destruct(struct sock *sk)
 576 {
 577         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 578
 579         pr_debug("%p\n", sk);
 580
 581         if (sk->sk_state == LLCP_CONNECTED)
 582                 nfc_put_device(llcp_sock->dev);
 583
 584         skb_queue_purge(&sk->sk_receive_queue);
 585
 586         nfc_llcp_sock_free(llcp_sock);
 587
 588         if (!sock_flag(sk, SOCK_DEAD)) {
 589                 pr_err("Freeing alive NFC LLCP socket %p\n", sk);
 590                 return;
 591         }
 592 }
 593
 594 struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
 595 {
 596         struct sock *sk;
 597         struct nfc_llcp_sock *llcp_sock;
 598
 599         sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto);
 600         if (!sk)
 601                 return NULL;
 602
 603         llcp_sock = nfc_llcp_sock(sk);
 604
 605         sock_init_data(sock, sk);
 606         sk->sk_state = LLCP_CLOSED;
 607         sk->sk_protocol = NFC_SOCKPROTO_LLCP;
 608         sk->sk_type = type;
 609         sk->sk_destruct = llcp_sock_destruct;
 610
 611         llcp_sock->ssap = 0;
 612         llcp_sock->dsap = LLCP_SAP_SDP;
 613         llcp_sock->send_n = llcp_sock->send_ack_n = 0;
 614         llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
 615         llcp_sock->remote_ready = 1;
 616         skb_queue_head_init(&llcp_sock->tx_queue);
 617         skb_queue_head_init(&llcp_sock->tx_pending_queue);
 618         skb_queue_head_init(&llcp_sock->tx_backlog_queue);
 619         INIT_LIST_HEAD(&llcp_sock->list);
 620         INIT_LIST_HEAD(&llcp_sock->accept_queue);
 621
 622         if (sock != NULL)
 623                 sock->state = SS_UNCONNECTED;
 624
 625         return sk;
 626 }
 627
 628 void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
 629 {
 630         kfree(sock->service_name);
 631
 632         skb_queue_purge(&sock->tx_queue);
 633         skb_queue_purge(&sock->tx_pending_queue);
 634         skb_queue_purge(&sock->tx_backlog_queue);
 635
 636         list_del_init(&sock->accept_queue);
 637
 638         sock->parent = NULL;
 639 }
 640
 641 static int llcp_sock_create(struct net *net, struct socket *sock,
 642                                 const struct nfc_protocol *nfc_proto)
 643 {
 644         struct sock *sk;
 645
 646         pr_debug("%p\n", sock);
 647
 648         if (sock->type != SOCK_STREAM && sock->type != SOCK_DGRAM)
 649                 return -ESOCKTNOSUPPORT;
 650
 651         sock->ops = &llcp_sock_ops;
 652
 653         sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
 654         if (sk == NULL)
 655                 return -ENOMEM;
 656
 657         return 0;
 658 }
 659
 660 static const struct nfc_protocol llcp_nfc_proto = {
 661         .id       = NFC_SOCKPROTO_LLCP,
 662         .proto    = &llcp_sock_proto,
 663         .owner    = THIS_MODULE,
 664         .create   = llcp_sock_create
 665 };
 666
 667 int __init nfc_llcp_sock_init(void)
 668 {
 669         return nfc_proto_register(&llcp_nfc_proto);
 670 }
 671
 672 void nfc_llcp_sock_exit(void)
 673 {
 674         nfc_proto_unregister(&llcp_nfc_proto);
 675 }