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, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #define pr_fmt(fmt) "llcp: %s: " fmt, __func__
  19
  20 #include <linux/init.h>
  21 #include <linux/kernel.h>
  22 #include <linux/module.h>
  23 #include <linux/nfc.h>
  24
  25 #include "nfc.h"
  26 #include "llcp.h"
  27
  28 static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
  29 {
  30         DECLARE_WAITQUEUE(wait, current);
  31         int err = 0;
  32
  33         pr_debug("sk %p", sk);
  34
  35         add_wait_queue(sk_sleep(sk), &wait);
  36         set_current_state(TASK_INTERRUPTIBLE);
  37
  38         while (sk->sk_state != state) {
  39                 if (!timeo) {
  40                         err = -EINPROGRESS;
  41                         break;
  42                 }
  43
  44                 if (signal_pending(current)) {
  45                         err = sock_intr_errno(timeo);
  46                         break;
  47                 }
  48
  49                 release_sock(sk);
  50                 timeo = schedule_timeout(timeo);
  51                 lock_sock(sk);
  52                 set_current_state(TASK_INTERRUPTIBLE);
  53
  54                 err = sock_error(sk);
  55                 if (err)
  56                         break;
  57         }
  58
  59         __set_current_state(TASK_RUNNING);
  60         remove_wait_queue(sk_sleep(sk), &wait);
  61         return err;
  62 }
  63
  64 static struct proto llcp_sock_proto = {
  65         .name     = "NFC_LLCP",
  66         .owner    = THIS_MODULE,
  67         .obj_size = sizeof(struct nfc_llcp_sock),
  68 };
  69
  70 static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
  71 {
  72         struct sock *sk = sock->sk;
  73         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
  74         struct nfc_llcp_local *local;
  75         struct nfc_dev *dev;
  76         struct sockaddr_nfc_llcp llcp_addr;
  77         int len, ret = 0;
  78
  79         if (!addr || addr->sa_family != AF_NFC)
  80                 return -EINVAL;
  81
  82         pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
  83
  84         memset(&llcp_addr, 0, sizeof(llcp_addr));
  85         len = min_t(unsigned int, sizeof(llcp_addr), alen);
  86         memcpy(&llcp_addr, addr, len);
  87
  88         /* This is going to be a listening socket, dsap must be 0 */
  89         if (llcp_addr.dsap != 0)
  90                 return -EINVAL;
  91
  92         lock_sock(sk);
  93
  94         if (sk->sk_state != LLCP_CLOSED) {
  95                 ret = -EBADFD;
  96                 goto error;
  97         }
  98
  99         dev = nfc_get_device(llcp_addr.dev_idx);
 100         if (dev == NULL) {
 101                 ret = -ENODEV;
 102                 goto error;
 103         }
 104
 105         local = nfc_llcp_find_local(dev);
 106         if (local == NULL) {
 107                 ret = -ENODEV;
 108                 goto put_dev;
 109         }
 110
 111         llcp_sock->dev = dev;
 112         llcp_sock->local = nfc_llcp_local_get(local);
 113         llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
 114         llcp_sock->service_name_len = min_t(unsigned int,
 115                                             llcp_addr.service_name_len,
 116                                             NFC_LLCP_MAX_SERVICE_NAME);
 117         llcp_sock->service_name = kmemdup(llcp_addr.service_name,
 118                                           llcp_sock->service_name_len,
 119                                           GFP_KERNEL);
 120
 121         llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
 122         if (llcp_sock->ssap == LLCP_SAP_MAX) {
 123                 ret = -EADDRINUSE;
 124                 goto put_dev;
 125         }
 126
 127         llcp_sock->reserved_ssap = llcp_sock->ssap;
 128
 129         nfc_llcp_sock_link(&local->sockets, sk);
 130
 131         pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
 132
 133         sk->sk_state = LLCP_BOUND;
 134
 135 put_dev:
 136         nfc_put_device(dev);
 137
 138 error:
 139         release_sock(sk);
 140         return ret;
 141 }
 142
 143 static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
 144                               int alen)
 145 {
 146         struct sock *sk = sock->sk;
 147         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 148         struct nfc_llcp_local *local;
 149         struct nfc_dev *dev;
 150         struct sockaddr_nfc_llcp llcp_addr;
 151         int len, ret = 0;
 152
 153         if (!addr || addr->sa_family != AF_NFC)
 154                 return -EINVAL;
 155
 156         pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
 157
 158         memset(&llcp_addr, 0, sizeof(llcp_addr));
 159         len = min_t(unsigned int, sizeof(llcp_addr), alen);
 160         memcpy(&llcp_addr, addr, len);
 161
 162         lock_sock(sk);
 163
 164         if (sk->sk_state != LLCP_CLOSED) {
 165                 ret = -EBADFD;
 166                 goto error;
 167         }
 168
 169         dev = nfc_get_device(llcp_addr.dev_idx);
 170         if (dev == NULL) {
 171                 ret = -ENODEV;
 172                 goto error;
 173         }
 174
 175         local = nfc_llcp_find_local(dev);
 176         if (local == NULL) {
 177                 ret = -ENODEV;
 178                 goto put_dev;
 179         }
 180
 181         llcp_sock->dev = dev;
 182         llcp_sock->local = nfc_llcp_local_get(local);
 183         llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
 184
 185         nfc_llcp_sock_link(&local->raw_sockets, sk);
 186
 187         sk->sk_state = LLCP_BOUND;
 188
 189 put_dev:
 190         nfc_put_device(dev);
 191
 192 error:
 193         release_sock(sk);
 194         return ret;
 195 }
 196
 197 static int llcp_sock_listen(struct socket *sock, int backlog)
 198 {
 199         struct sock *sk = sock->sk;
 200         int ret = 0;
 201
 202         pr_debug("sk %p backlog %d\n", sk, backlog);
 203
 204         lock_sock(sk);
 205
 206         if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
 207             sk->sk_state != LLCP_BOUND) {
 208                 ret = -EBADFD;
 209                 goto error;
 210         }
 211
 212         sk->sk_max_ack_backlog = backlog;
 213         sk->sk_ack_backlog = 0;
 214
 215         pr_debug("Socket listening\n");
 216         sk->sk_state = LLCP_LISTEN;
 217
 218 error:
 219         release_sock(sk);
 220
 221         return ret;
 222 }
 223
 224 static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
 225                                char __user *optval, unsigned int optlen)
 226 {
 227         struct sock *sk = sock->sk;
 228         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 229         u32 opt;
 230         int err = 0;
 231
 232         pr_debug("%p optname %d\n", sk, optname);
 233
 234         if (level != SOL_NFC)
 235                 return -ENOPROTOOPT;
 236
 237         lock_sock(sk);
 238
 239         switch (optname) {
 240         case NFC_LLCP_RW:
 241                 if (sk->sk_state == LLCP_CONNECTED ||
 242                     sk->sk_state == LLCP_BOUND ||
 243                     sk->sk_state == LLCP_LISTEN) {
 244                         err = -EINVAL;
 245                         break;
 246                 }
 247
 248                 if (get_user(opt, (u32 __user *) optval)) {
 249                         err = -EFAULT;
 250                         break;
 251                 }
 252
 253                 if (opt > LLCP_MAX_RW) {
 254                         err = -EINVAL;
 255                         break;
 256                 }
 257
 258                 llcp_sock->rw = (u8) opt;
 259
 260                 break;
 261
 262         case NFC_LLCP_MIUX:
 263                 if (sk->sk_state == LLCP_CONNECTED ||
 264                     sk->sk_state == LLCP_BOUND ||
 265                     sk->sk_state == LLCP_LISTEN) {
 266                         err = -EINVAL;
 267                         break;
 268                 }
 269
 270                 if (get_user(opt, (u32 __user *) optval)) {
 271                         err = -EFAULT;
 272                         break;
 273                 }
 274
 275                 if (opt > LLCP_MAX_MIUX) {
 276                         err = -EINVAL;
 277                         break;
 278                 }
 279
 280                 llcp_sock->miux = cpu_to_be16((u16) opt);
 281
 282                 break;
 283
 284         default:
 285                 err = -ENOPROTOOPT;
 286                 break;
 287         }
 288
 289         release_sock(sk);
 290
 291         pr_debug("%p rw %d miux %d\n", llcp_sock,
 292                  llcp_sock->rw, llcp_sock->miux);
 293
 294         return err;
 295 }
 296
 297 static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
 298                                char __user *optval, int __user *optlen)
 299 {
 300         struct nfc_llcp_local *local;
 301         struct sock *sk = sock->sk;
 302         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 303         int len, err = 0;
 304         u16 miux, remote_miu;
 305         u8 rw;
 306
 307         pr_debug("%p optname %d\n", sk, optname);
 308
 309         if (level != SOL_NFC)
 310                 return -ENOPROTOOPT;
 311
 312         if (get_user(len, optlen))
 313                 return -EFAULT;
 314
 315         local = llcp_sock->local;
 316         if (!local)
 317                 return -ENODEV;
 318
 319         len = min_t(u32, len, sizeof(u32));
 320
 321         lock_sock(sk);
 322
 323         switch (optname) {
 324         case NFC_LLCP_RW:
 325                 rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
 326                 if (put_user(rw, (u32 __user *) optval))
 327                         err = -EFAULT;
 328
 329                 break;
 330
 331         case NFC_LLCP_MIUX:
 332                 miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
 333                         be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
 334
 335                 if (put_user(miux, (u32 __user *) optval))
 336                         err = -EFAULT;
 337
 338                 break;
 339
 340         case NFC_LLCP_REMOTE_MIU:
 341                 remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
 342                                 local->remote_miu : llcp_sock->remote_miu;
 343
 344                 if (put_user(remote_miu, (u32 __user *) optval))
 345                         err = -EFAULT;
 346
 347                 break;
 348
 349         case NFC_LLCP_REMOTE_LTO:
 350                 if (put_user(local->remote_lto / 10, (u32 __user *) optval))
 351                         err = -EFAULT;
 352
 353                 break;
 354
 355         case NFC_LLCP_REMOTE_RW:
 356                 if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
 357                         err = -EFAULT;
 358
 359                 break;
 360
 361         default:
 362                 err = -ENOPROTOOPT;
 363                 break;
 364         }
 365
 366         release_sock(sk);
 367
 368         if (put_user(len, optlen))
 369                 return -EFAULT;
 370
 371         return err;
 372 }
 373
 374 void nfc_llcp_accept_unlink(struct sock *sk)
 375 {
 376         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 377
 378         pr_debug("state %d\n", sk->sk_state);
 379
 380         list_del_init(&llcp_sock->accept_queue);
 381         sk_acceptq_removed(llcp_sock->parent);
 382         llcp_sock->parent = NULL;
 383
 384         sock_put(sk);
 385 }
 386
 387 void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
 388 {
 389         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 390         struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
 391
 392         /* Lock will be free from unlink */
 393         sock_hold(sk);
 394
 395         list_add_tail(&llcp_sock->accept_queue,
 396                       &llcp_sock_parent->accept_queue);
 397         llcp_sock->parent = parent;
 398         sk_acceptq_added(parent);
 399 }
 400
 401 struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
 402                                      struct socket *newsock)
 403 {
 404         struct nfc_llcp_sock *lsk, *n, *llcp_parent;
 405         struct sock *sk;
 406
 407         llcp_parent = nfc_llcp_sock(parent);
 408
 409         list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
 410                                  accept_queue) {
 411                 sk = &lsk->sk;
 412                 lock_sock(sk);
 413
 414                 if (sk->sk_state == LLCP_CLOSED) {
 415                         release_sock(sk);
 416                         nfc_llcp_accept_unlink(sk);
 417                         continue;
 418                 }
 419
 420                 if (sk->sk_state == LLCP_CONNECTED || !newsock) {
 421                         list_del_init(&lsk->accept_queue);
 422                         sock_put(sk);
 423
 424                         if (newsock)
 425                                 sock_graft(sk, newsock);
 426
 427                         release_sock(sk);
 428
 429                         pr_debug("Returning sk state %d\n", sk->sk_state);
 430
 431                         sk_acceptq_removed(parent);
 432
 433                         return sk;
 434                 }
 435
 436                 release_sock(sk);
 437         }
 438
 439         return NULL;
 440 }
 441
 442 static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
 443                             int flags)
 444 {
 445         DECLARE_WAITQUEUE(wait, current);
 446         struct sock *sk = sock->sk, *new_sk;
 447         long timeo;
 448         int ret = 0;
 449
 450         pr_debug("parent %p\n", sk);
 451
 452         lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
 453
 454         if (sk->sk_state != LLCP_LISTEN) {
 455                 ret = -EBADFD;
 456                 goto error;
 457         }
 458
 459         timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
 460
 461         /* Wait for an incoming connection. */
 462         add_wait_queue_exclusive(sk_sleep(sk), &wait);
 463         while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
 464                 set_current_state(TASK_INTERRUPTIBLE);
 465
 466                 if (!timeo) {
 467                         ret = -EAGAIN;
 468                         break;
 469                 }
 470
 471                 if (signal_pending(current)) {
 472                         ret = sock_intr_errno(timeo);
 473                         break;
 474                 }
 475
 476                 release_sock(sk);
 477                 timeo = schedule_timeout(timeo);
 478                 lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
 479         }
 480         __set_current_state(TASK_RUNNING);
 481         remove_wait_queue(sk_sleep(sk), &wait);
 482
 483         if (ret)
 484                 goto error;
 485
 486         newsock->state = SS_CONNECTED;
 487
 488         pr_debug("new socket %p\n", new_sk);
 489
 490 error:
 491         release_sock(sk);
 492
 493         return ret;
 494 }
 495
 496 static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr,
 497                              int *len, int peer)
 498 {
 499         struct sock *sk = sock->sk;
 500         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 501         DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr);
 502
 503         if (llcp_sock == NULL || llcp_sock->dev == NULL)
 504                 return -EBADFD;
 505
 506         pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx,
 507                  llcp_sock->dsap, llcp_sock->ssap);
 508
 509         memset(llcp_addr, 0, sizeof(*llcp_addr));
 510         *len = sizeof(struct sockaddr_nfc_llcp);
 511
 512         lock_sock(sk);
 513         if (!llcp_sock->dev) {
 514                 release_sock(sk);
 515                 return -EBADFD;
 516         }
 517         llcp_addr->sa_family = AF_NFC;
 518         llcp_addr->dev_idx = llcp_sock->dev->idx;
 519         llcp_addr->target_idx = llcp_sock->target_idx;
 520         llcp_addr->nfc_protocol = llcp_sock->nfc_protocol;
 521         llcp_addr->dsap = llcp_sock->dsap;
 522         llcp_addr->ssap = llcp_sock->ssap;
 523         llcp_addr->service_name_len = llcp_sock->service_name_len;
 524         memcpy(llcp_addr->service_name, llcp_sock->service_name,
 525                llcp_addr->service_name_len);
 526         release_sock(sk);
 527
 528         return 0;
 529 }
 530
 531 static inline unsigned int llcp_accept_poll(struct sock *parent)
 532 {
 533         struct nfc_llcp_sock *llcp_sock, *parent_sock;
 534         struct sock *sk;
 535
 536         parent_sock = nfc_llcp_sock(parent);
 537
 538         list_for_each_entry(llcp_sock, &parent_sock->accept_queue,
 539                             accept_queue) {
 540                 sk = &llcp_sock->sk;
 541
 542                 if (sk->sk_state == LLCP_CONNECTED)
 543                         return POLLIN | POLLRDNORM;
 544         }
 545
 546         return 0;
 547 }
 548
 549 static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
 550                                    poll_table *wait)
 551 {
 552         struct sock *sk = sock->sk;
 553         unsigned int mask = 0;
 554
 555         pr_debug("%p\n", sk);
 556
 557         sock_poll_wait(file, sk_sleep(sk), wait);
 558
 559         if (sk->sk_state == LLCP_LISTEN)
 560                 return llcp_accept_poll(sk);
 561
 562         if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
 563                 mask |= POLLERR |
 564                         (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0);
 565
 566         if (!skb_queue_empty(&sk->sk_receive_queue))
 567                 mask |= POLLIN | POLLRDNORM;
 568
 569         if (sk->sk_state == LLCP_CLOSED)
 570                 mask |= POLLHUP;
 571
 572         if (sk->sk_shutdown & RCV_SHUTDOWN)
 573                 mask |= POLLRDHUP | POLLIN | POLLRDNORM;
 574
 575         if (sk->sk_shutdown == SHUTDOWN_MASK)
 576                 mask |= POLLHUP;
 577
 578         if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED)
 579                 mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
 580         else
 581                 sk_set_bit(SOCKWQ_ASYNC_NOSPACE, sk);
 582
 583         pr_debug("mask 0x%x\n", mask);
 584
 585         return mask;
 586 }
 587
 588 static int llcp_sock_release(struct socket *sock)
 589 {
 590         struct sock *sk = sock->sk;
 591         struct nfc_llcp_local *local;
 592         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 593         int err = 0;
 594
 595         if (!sk)
 596                 return 0;
 597
 598         pr_debug("%p\n", sk);
 599
 600         local = llcp_sock->local;
 601         if (local == NULL) {
 602                 err = -ENODEV;
 603                 goto out;
 604         }
 605
 606         lock_sock(sk);
 607
 608         /* Send a DISC */
 609         if (sk->sk_state == LLCP_CONNECTED)
 610                 nfc_llcp_send_disconnect(llcp_sock);
 611
 612         if (sk->sk_state == LLCP_LISTEN) {
 613                 struct nfc_llcp_sock *lsk, *n;
 614                 struct sock *accept_sk;
 615
 616                 list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
 617                                          accept_queue) {
 618                         accept_sk = &lsk->sk;
 619                         lock_sock(accept_sk);
 620
 621                         nfc_llcp_send_disconnect(lsk);
 622                         nfc_llcp_accept_unlink(accept_sk);
 623
 624                         release_sock(accept_sk);
 625                 }
 626         }
 627
 628         if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
 629                 nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
 630
 631         release_sock(sk);
 632
 633         /* Keep this sock alive and therefore do not remove it from the sockets
 634          * list until the DISC PDU has been actually sent. Otherwise we would
 635          * reply with DM PDUs before sending the DISC one.
 636          */
 637         if (sk->sk_state == LLCP_DISCONNECTING)
 638                 return err;
 639
 640         if (sock->type == SOCK_RAW)
 641                 nfc_llcp_sock_unlink(&local->raw_sockets, sk);
 642         else
 643                 nfc_llcp_sock_unlink(&local->sockets, sk);
 644
 645 out:
 646         sock_orphan(sk);
 647         sock_put(sk);
 648
 649         return err;
 650 }
 651
 652 static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
 653                              int len, int flags)
 654 {
 655         struct sock *sk = sock->sk;
 656         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 657         struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
 658         struct nfc_dev *dev;
 659         struct nfc_llcp_local *local;
 660         int ret = 0;
 661
 662         pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
 663
 664         if (!addr || len < sizeof(struct sockaddr_nfc) ||
 665             addr->sa_family != AF_NFC)
 666                 return -EINVAL;
 667
 668         if (addr->service_name_len == 0 && addr->dsap == 0)
 669                 return -EINVAL;
 670
 671         pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
 672                  addr->target_idx, addr->nfc_protocol);
 673
 674         lock_sock(sk);
 675
 676         if (sk->sk_state == LLCP_CONNECTED) {
 677                 ret = -EISCONN;
 678                 goto error;
 679         }
 680
 681         dev = nfc_get_device(addr->dev_idx);
 682         if (dev == NULL) {
 683                 ret = -ENODEV;
 684                 goto error;
 685         }
 686
 687         local = nfc_llcp_find_local(dev);
 688         if (local == NULL) {
 689                 ret = -ENODEV;
 690                 goto put_dev;
 691         }
 692
 693         device_lock(&dev->dev);
 694         if (dev->dep_link_up == false) {
 695                 ret = -ENOLINK;
 696                 device_unlock(&dev->dev);
 697                 goto put_dev;
 698         }
 699         device_unlock(&dev->dev);
 700
 701         if (local->rf_mode == NFC_RF_INITIATOR &&
 702             addr->target_idx != local->target_idx) {
 703                 ret = -ENOLINK;
 704                 goto put_dev;
 705         }
 706
 707         llcp_sock->dev = dev;
 708         llcp_sock->local = nfc_llcp_local_get(local);
 709         llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
 710         if (llcp_sock->ssap == LLCP_SAP_MAX) {
 711                 ret = -ENOMEM;
 712                 goto put_dev;
 713         }
 714
 715         llcp_sock->reserved_ssap = llcp_sock->ssap;
 716
 717         if (addr->service_name_len == 0)
 718                 llcp_sock->dsap = addr->dsap;
 719         else
 720                 llcp_sock->dsap = LLCP_SAP_SDP;
 721         llcp_sock->nfc_protocol = addr->nfc_protocol;
 722         llcp_sock->service_name_len = min_t(unsigned int,
 723                                             addr->service_name_len,
 724                                             NFC_LLCP_MAX_SERVICE_NAME);
 725         llcp_sock->service_name = kmemdup(addr->service_name,
 726                                           llcp_sock->service_name_len,
 727                                           GFP_KERNEL);
 728
 729         nfc_llcp_sock_link(&local->connecting_sockets, sk);
 730
 731         ret = nfc_llcp_send_connect(llcp_sock);
 732         if (ret)
 733                 goto sock_unlink;
 734
 735         sk->sk_state = LLCP_CONNECTING;
 736
 737         ret = sock_wait_state(sk, LLCP_CONNECTED,
 738                               sock_sndtimeo(sk, flags & O_NONBLOCK));
 739         if (ret && ret != -EINPROGRESS)
 740                 goto sock_unlink;
 741
 742         release_sock(sk);
 743
 744         return ret;
 745
 746 sock_unlink:
 747         nfc_llcp_put_ssap(local, llcp_sock->ssap);
 748
 749         nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
 750
 751 put_dev:
 752         nfc_put_device(dev);
 753
 754 error:
 755         release_sock(sk);
 756         return ret;
 757 }
 758
 759 static int llcp_sock_sendmsg(struct socket *sock, struct msghdr *msg,
 760                              size_t len)
 761 {
 762         struct sock *sk = sock->sk;
 763         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 764         int ret;
 765
 766         pr_debug("sock %p sk %p", sock, sk);
 767
 768         ret = sock_error(sk);
 769         if (ret)
 770                 return ret;
 771
 772         if (msg->msg_flags & MSG_OOB)
 773                 return -EOPNOTSUPP;
 774
 775         lock_sock(sk);
 776
 777         if (sk->sk_type == SOCK_DGRAM) {
 778                 DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, addr,
 779                                  msg->msg_name);
 780
 781                 if (msg->msg_namelen < sizeof(*addr)) {
 782                         release_sock(sk);
 783                         return -EINVAL;
 784                 }
 785
 786                 release_sock(sk);
 787
 788                 return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
 789                                               msg, len);
 790         }
 791
 792         if (sk->sk_state != LLCP_CONNECTED) {
 793                 release_sock(sk);
 794                 return -ENOTCONN;
 795         }
 796
 797         release_sock(sk);
 798
 799         return nfc_llcp_send_i_frame(llcp_sock, msg, len);
 800 }
 801
 802 static int llcp_sock_recvmsg(struct socket *sock, struct msghdr *msg,
 803                              size_t len, int flags)
 804 {
 805         int noblock = flags & MSG_DONTWAIT;
 806         struct sock *sk = sock->sk;
 807         unsigned int copied, rlen;
 808         struct sk_buff *skb, *cskb;
 809         int err = 0;
 810
 811         pr_debug("%p %zu\n", sk, len);
 812
 813         lock_sock(sk);
 814
 815         if (sk->sk_state == LLCP_CLOSED &&
 816             skb_queue_empty(&sk->sk_receive_queue)) {
 817                 release_sock(sk);
 818                 return 0;
 819         }
 820
 821         release_sock(sk);
 822
 823         if (flags & (MSG_OOB))
 824                 return -EOPNOTSUPP;
 825
 826         skb = skb_recv_datagram(sk, flags, noblock, &err);
 827         if (!skb) {
 828                 pr_err("Recv datagram failed state %d %d %d",
 829                        sk->sk_state, err, sock_error(sk));
 830
 831                 if (sk->sk_shutdown & RCV_SHUTDOWN)
 832                         return 0;
 833
 834                 return err;
 835         }
 836
 837         rlen = skb->len;                /* real length of skb */
 838         copied = min_t(unsigned int, rlen, len);
 839
 840         cskb = skb;
 841         if (skb_copy_datagram_msg(cskb, 0, msg, copied)) {
 842                 if (!(flags & MSG_PEEK))
 843                         skb_queue_head(&sk->sk_receive_queue, skb);
 844                 return -EFAULT;
 845         }
 846
 847         sock_recv_timestamp(msg, sk, skb);
 848
 849         if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
 850                 struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
 851                 DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, sockaddr,
 852                                  msg->msg_name);
 853
 854                 msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
 855
 856                 pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
 857
 858                 memset(sockaddr, 0, sizeof(*sockaddr));
 859                 sockaddr->sa_family = AF_NFC;
 860                 sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
 861                 sockaddr->dsap = ui_cb->dsap;
 862                 sockaddr->ssap = ui_cb->ssap;
 863         }
 864
 865         /* Mark read part of skb as used */
 866         if (!(flags & MSG_PEEK)) {
 867
 868                 /* SOCK_STREAM: re-queue skb if it contains unreceived data */
 869                 if (sk->sk_type == SOCK_STREAM ||
 870                     sk->sk_type == SOCK_DGRAM ||
 871                     sk->sk_type == SOCK_RAW) {
 872                         skb_pull(skb, copied);
 873                         if (skb->len) {
 874                                 skb_queue_head(&sk->sk_receive_queue, skb);
 875                                 goto done;
 876                         }
 877                 }
 878
 879                 kfree_skb(skb);
 880         }
 881
 882         /* XXX Queue backlogged skbs */
 883
 884 done:
 885         /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
 886         if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
 887                 copied = rlen;
 888
 889         return copied;
 890 }
 891
 892 static const struct proto_ops llcp_sock_ops = {
 893         .family         = PF_NFC,
 894         .owner          = THIS_MODULE,
 895         .bind           = llcp_sock_bind,
 896         .connect        = llcp_sock_connect,
 897         .release        = llcp_sock_release,
 898         .socketpair     = sock_no_socketpair,
 899         .accept         = llcp_sock_accept,
 900         .getname        = llcp_sock_getname,
 901         .poll           = llcp_sock_poll,
 902         .ioctl          = sock_no_ioctl,
 903         .listen         = llcp_sock_listen,
 904         .shutdown       = sock_no_shutdown,
 905         .setsockopt     = nfc_llcp_setsockopt,
 906         .getsockopt     = nfc_llcp_getsockopt,
 907         .sendmsg        = llcp_sock_sendmsg,
 908         .recvmsg        = llcp_sock_recvmsg,
 909         .mmap           = sock_no_mmap,
 910 };
 911
 912 static const struct proto_ops llcp_rawsock_ops = {
 913         .family         = PF_NFC,
 914         .owner          = THIS_MODULE,
 915         .bind           = llcp_raw_sock_bind,
 916         .connect        = sock_no_connect,
 917         .release        = llcp_sock_release,
 918         .socketpair     = sock_no_socketpair,
 919         .accept         = sock_no_accept,
 920         .getname        = llcp_sock_getname,
 921         .poll           = llcp_sock_poll,
 922         .ioctl          = sock_no_ioctl,
 923         .listen         = sock_no_listen,
 924         .shutdown       = sock_no_shutdown,
 925         .setsockopt     = sock_no_setsockopt,
 926         .getsockopt     = sock_no_getsockopt,
 927         .sendmsg        = sock_no_sendmsg,
 928         .recvmsg        = llcp_sock_recvmsg,
 929         .mmap           = sock_no_mmap,
 930 };
 931
 932 static void llcp_sock_destruct(struct sock *sk)
 933 {
 934         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 935
 936         pr_debug("%p\n", sk);
 937
 938         if (sk->sk_state == LLCP_CONNECTED)
 939                 nfc_put_device(llcp_sock->dev);
 940
 941         skb_queue_purge(&sk->sk_receive_queue);
 942
 943         nfc_llcp_sock_free(llcp_sock);
 944
 945         if (!sock_flag(sk, SOCK_DEAD)) {
 946                 pr_err("Freeing alive NFC LLCP socket %p\n", sk);
 947                 return;
 948         }
 949 }
 950
 951 struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp, int kern)
 952 {
 953         struct sock *sk;
 954         struct nfc_llcp_sock *llcp_sock;
 955
 956         sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto, kern);
 957         if (!sk)
 958                 return NULL;
 959
 960         llcp_sock = nfc_llcp_sock(sk);
 961
 962         sock_init_data(sock, sk);
 963         sk->sk_state = LLCP_CLOSED;
 964         sk->sk_protocol = NFC_SOCKPROTO_LLCP;
 965         sk->sk_type = type;
 966         sk->sk_destruct = llcp_sock_destruct;
 967
 968         llcp_sock->ssap = 0;
 969         llcp_sock->dsap = LLCP_SAP_SDP;
 970         llcp_sock->rw = LLCP_MAX_RW + 1;
 971         llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
 972         llcp_sock->send_n = llcp_sock->send_ack_n = 0;
 973         llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
 974         llcp_sock->remote_ready = 1;
 975         llcp_sock->reserved_ssap = LLCP_SAP_MAX;
 976         nfc_llcp_socket_remote_param_init(llcp_sock);
 977         skb_queue_head_init(&llcp_sock->tx_queue);
 978         skb_queue_head_init(&llcp_sock->tx_pending_queue);
 979         INIT_LIST_HEAD(&llcp_sock->accept_queue);
 980
 981         if (sock != NULL)
 982                 sock->state = SS_UNCONNECTED;
 983
 984         return sk;
 985 }
 986
 987 void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
 988 {
 989         kfree(sock->service_name);
 990
 991         skb_queue_purge(&sock->tx_queue);
 992         skb_queue_purge(&sock->tx_pending_queue);
 993
 994         list_del_init(&sock->accept_queue);
 995
 996         sock->parent = NULL;
 997
 998         nfc_llcp_local_put(sock->local);
 999 }
1000
1001 static int llcp_sock_create(struct net *net, struct socket *sock,
1002                             const struct nfc_protocol *nfc_proto, int kern)
1003 {
1004         struct sock *sk;
1005
1006         pr_debug("%p\n", sock);
1007
1008         if (sock->type != SOCK_STREAM &&
1009             sock->type != SOCK_DGRAM &&
1010             sock->type != SOCK_RAW)
1011                 return -ESOCKTNOSUPPORT;
1012
1013         if (sock->type == SOCK_RAW)
1014                 sock->ops = &llcp_rawsock_ops;
1015         else
1016                 sock->ops = &llcp_sock_ops;
1017
1018         sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC, kern);
1019         if (sk == NULL)
1020                 return -ENOMEM;
1021
1022         return 0;
1023 }
1024
1025 static const struct nfc_protocol llcp_nfc_proto = {
1026         .id       = NFC_SOCKPROTO_LLCP,
1027         .proto    = &llcp_sock_proto,
1028         .owner    = THIS_MODULE,
1029         .create   = llcp_sock_create
1030 };
1031
1032 int __init nfc_llcp_sock_init(void)
1033 {
1034         return nfc_proto_register(&llcp_nfc_proto);
1035 }
1036
1037 void nfc_llcp_sock_exit(void)
1038 {
1039         nfc_proto_unregister(&llcp_nfc_proto);
1040 }