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         llcp_addr->sa_family = AF_NFC;
 513         llcp_addr->dev_idx = llcp_sock->dev->idx;
 514         llcp_addr->target_idx = llcp_sock->target_idx;
 515         llcp_addr->nfc_protocol = llcp_sock->nfc_protocol;
 516         llcp_addr->dsap = llcp_sock->dsap;
 517         llcp_addr->ssap = llcp_sock->ssap;
 518         llcp_addr->service_name_len = llcp_sock->service_name_len;
 519         memcpy(llcp_addr->service_name, llcp_sock->service_name,
 520                llcp_addr->service_name_len);
 521
 522         return 0;
 523 }
 524
 525 static inline unsigned int llcp_accept_poll(struct sock *parent)
 526 {
 527         struct nfc_llcp_sock *llcp_sock, *parent_sock;
 528         struct sock *sk;
 529
 530         parent_sock = nfc_llcp_sock(parent);
 531
 532         list_for_each_entry(llcp_sock, &parent_sock->accept_queue,
 533                             accept_queue) {
 534                 sk = &llcp_sock->sk;
 535
 536                 if (sk->sk_state == LLCP_CONNECTED)
 537                         return POLLIN | POLLRDNORM;
 538         }
 539
 540         return 0;
 541 }
 542
 543 static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
 544                                    poll_table *wait)
 545 {
 546         struct sock *sk = sock->sk;
 547         unsigned int mask = 0;
 548
 549         pr_debug("%p\n", sk);
 550
 551         sock_poll_wait(file, sk_sleep(sk), wait);
 552
 553         if (sk->sk_state == LLCP_LISTEN)
 554                 return llcp_accept_poll(sk);
 555
 556         if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
 557                 mask |= POLLERR |
 558                         (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0);
 559
 560         if (!skb_queue_empty(&sk->sk_receive_queue))
 561                 mask |= POLLIN | POLLRDNORM;
 562
 563         if (sk->sk_state == LLCP_CLOSED)
 564                 mask |= POLLHUP;
 565
 566         if (sk->sk_shutdown & RCV_SHUTDOWN)
 567                 mask |= POLLRDHUP | POLLIN | POLLRDNORM;
 568
 569         if (sk->sk_shutdown == SHUTDOWN_MASK)
 570                 mask |= POLLHUP;
 571
 572         if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED)
 573                 mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
 574         else
 575                 set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
 576
 577         pr_debug("mask 0x%x\n", mask);
 578
 579         return mask;
 580 }
 581
 582 static int llcp_sock_release(struct socket *sock)
 583 {
 584         struct sock *sk = sock->sk;
 585         struct nfc_llcp_local *local;
 586         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 587         int err = 0;
 588
 589         if (!sk)
 590                 return 0;
 591
 592         pr_debug("%p\n", sk);
 593
 594         local = llcp_sock->local;
 595         if (local == NULL) {
 596                 err = -ENODEV;
 597                 goto out;
 598         }
 599
 600         lock_sock(sk);
 601
 602         /* Send a DISC */
 603         if (sk->sk_state == LLCP_CONNECTED)
 604                 nfc_llcp_send_disconnect(llcp_sock);
 605
 606         if (sk->sk_state == LLCP_LISTEN) {
 607                 struct nfc_llcp_sock *lsk, *n;
 608                 struct sock *accept_sk;
 609
 610                 list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
 611                                          accept_queue) {
 612                         accept_sk = &lsk->sk;
 613                         lock_sock(accept_sk);
 614
 615                         nfc_llcp_send_disconnect(lsk);
 616                         nfc_llcp_accept_unlink(accept_sk);
 617
 618                         release_sock(accept_sk);
 619                 }
 620         }
 621
 622         if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
 623                 nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
 624
 625         release_sock(sk);
 626
 627         /* Keep this sock alive and therefore do not remove it from the sockets
 628          * list until the DISC PDU has been actually sent. Otherwise we would
 629          * reply with DM PDUs before sending the DISC one.
 630          */
 631         if (sk->sk_state == LLCP_DISCONNECTING)
 632                 return err;
 633
 634         if (sock->type == SOCK_RAW)
 635                 nfc_llcp_sock_unlink(&local->raw_sockets, sk);
 636         else
 637                 nfc_llcp_sock_unlink(&local->sockets, sk);
 638
 639 out:
 640         sock_orphan(sk);
 641         sock_put(sk);
 642
 643         return err;
 644 }
 645
 646 static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
 647                              int len, int flags)
 648 {
 649         struct sock *sk = sock->sk;
 650         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 651         struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
 652         struct nfc_dev *dev;
 653         struct nfc_llcp_local *local;
 654         int ret = 0;
 655
 656         pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
 657
 658         if (!addr || len < sizeof(struct sockaddr_nfc) ||
 659             addr->sa_family != AF_NFC)
 660                 return -EINVAL;
 661
 662         if (addr->service_name_len == 0 && addr->dsap == 0)
 663                 return -EINVAL;
 664
 665         pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
 666                  addr->target_idx, addr->nfc_protocol);
 667
 668         lock_sock(sk);
 669
 670         if (sk->sk_state == LLCP_CONNECTED) {
 671                 ret = -EISCONN;
 672                 goto error;
 673         }
 674
 675         dev = nfc_get_device(addr->dev_idx);
 676         if (dev == NULL) {
 677                 ret = -ENODEV;
 678                 goto error;
 679         }
 680
 681         local = nfc_llcp_find_local(dev);
 682         if (local == NULL) {
 683                 ret = -ENODEV;
 684                 goto put_dev;
 685         }
 686
 687         device_lock(&dev->dev);
 688         if (dev->dep_link_up == false) {
 689                 ret = -ENOLINK;
 690                 device_unlock(&dev->dev);
 691                 goto put_dev;
 692         }
 693         device_unlock(&dev->dev);
 694
 695         if (local->rf_mode == NFC_RF_INITIATOR &&
 696             addr->target_idx != local->target_idx) {
 697                 ret = -ENOLINK;
 698                 goto put_dev;
 699         }
 700
 701         llcp_sock->dev = dev;
 702         llcp_sock->local = nfc_llcp_local_get(local);
 703         llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
 704         if (llcp_sock->ssap == LLCP_SAP_MAX) {
 705                 ret = -ENOMEM;
 706                 goto put_dev;
 707         }
 708
 709         llcp_sock->reserved_ssap = llcp_sock->ssap;
 710
 711         if (addr->service_name_len == 0)
 712                 llcp_sock->dsap = addr->dsap;
 713         else
 714                 llcp_sock->dsap = LLCP_SAP_SDP;
 715         llcp_sock->nfc_protocol = addr->nfc_protocol;
 716         llcp_sock->service_name_len = min_t(unsigned int,
 717                                             addr->service_name_len,
 718                                             NFC_LLCP_MAX_SERVICE_NAME);
 719         llcp_sock->service_name = kmemdup(addr->service_name,
 720                                           llcp_sock->service_name_len,
 721                                           GFP_KERNEL);
 722
 723         nfc_llcp_sock_link(&local->connecting_sockets, sk);
 724
 725         ret = nfc_llcp_send_connect(llcp_sock);
 726         if (ret)
 727                 goto sock_unlink;
 728
 729         sk->sk_state = LLCP_CONNECTING;
 730
 731         ret = sock_wait_state(sk, LLCP_CONNECTED,
 732                               sock_sndtimeo(sk, flags & O_NONBLOCK));
 733         if (ret && ret != -EINPROGRESS)
 734                 goto sock_unlink;
 735
 736         release_sock(sk);
 737
 738         return ret;
 739
 740 sock_unlink:
 741         nfc_llcp_put_ssap(local, llcp_sock->ssap);
 742
 743         nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
 744
 745 put_dev:
 746         nfc_put_device(dev);
 747
 748 error:
 749         release_sock(sk);
 750         return ret;
 751 }
 752
 753 static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
 754                              struct msghdr *msg, size_t len)
 755 {
 756         struct sock *sk = sock->sk;
 757         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 758         int ret;
 759
 760         pr_debug("sock %p sk %p", sock, sk);
 761
 762         ret = sock_error(sk);
 763         if (ret)
 764                 return ret;
 765
 766         if (msg->msg_flags & MSG_OOB)
 767                 return -EOPNOTSUPP;
 768
 769         lock_sock(sk);
 770
 771         if (sk->sk_type == SOCK_DGRAM) {
 772                 DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, addr,
 773                                  msg->msg_name);
 774
 775                 if (msg->msg_namelen < sizeof(*addr)) {
 776                         release_sock(sk);
 777                         return -EINVAL;
 778                 }
 779
 780                 release_sock(sk);
 781
 782                 return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
 783                                               msg, len);
 784         }
 785
 786         if (sk->sk_state != LLCP_CONNECTED) {
 787                 release_sock(sk);
 788                 return -ENOTCONN;
 789         }
 790
 791         release_sock(sk);
 792
 793         return nfc_llcp_send_i_frame(llcp_sock, msg, len);
 794 }
 795
 796 static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 797                              struct msghdr *msg, size_t len, int flags)
 798 {
 799         int noblock = flags & MSG_DONTWAIT;
 800         struct sock *sk = sock->sk;
 801         unsigned int copied, rlen;
 802         struct sk_buff *skb, *cskb;
 803         int err = 0;
 804
 805         pr_debug("%p %zu\n", sk, len);
 806
 807         lock_sock(sk);
 808
 809         if (sk->sk_state == LLCP_CLOSED &&
 810             skb_queue_empty(&sk->sk_receive_queue)) {
 811                 release_sock(sk);
 812                 return 0;
 813         }
 814
 815         release_sock(sk);
 816
 817         if (flags & (MSG_OOB))
 818                 return -EOPNOTSUPP;
 819
 820         skb = skb_recv_datagram(sk, flags, noblock, &err);
 821         if (!skb) {
 822                 pr_err("Recv datagram failed state %d %d %d",
 823                        sk->sk_state, err, sock_error(sk));
 824
 825                 if (sk->sk_shutdown & RCV_SHUTDOWN)
 826                         return 0;
 827
 828                 return err;
 829         }
 830
 831         rlen = skb->len;                /* real length of skb */
 832         copied = min_t(unsigned int, rlen, len);
 833
 834         cskb = skb;
 835         if (skb_copy_datagram_msg(cskb, 0, msg, copied)) {
 836                 if (!(flags & MSG_PEEK))
 837                         skb_queue_head(&sk->sk_receive_queue, skb);
 838                 return -EFAULT;
 839         }
 840
 841         sock_recv_timestamp(msg, sk, skb);
 842
 843         if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
 844                 struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
 845                 DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, sockaddr,
 846                                  msg->msg_name);
 847
 848                 msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
 849
 850                 pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
 851
 852                 memset(sockaddr, 0, sizeof(*sockaddr));
 853                 sockaddr->sa_family = AF_NFC;
 854                 sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
 855                 sockaddr->dsap = ui_cb->dsap;
 856                 sockaddr->ssap = ui_cb->ssap;
 857         }
 858
 859         /* Mark read part of skb as used */
 860         if (!(flags & MSG_PEEK)) {
 861
 862                 /* SOCK_STREAM: re-queue skb if it contains unreceived data */
 863                 if (sk->sk_type == SOCK_STREAM ||
 864                     sk->sk_type == SOCK_DGRAM ||
 865                     sk->sk_type == SOCK_RAW) {
 866                         skb_pull(skb, copied);
 867                         if (skb->len) {
 868                                 skb_queue_head(&sk->sk_receive_queue, skb);
 869                                 goto done;
 870                         }
 871                 }
 872
 873                 kfree_skb(skb);
 874         }
 875
 876         /* XXX Queue backlogged skbs */
 877
 878 done:
 879         /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
 880         if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
 881                 copied = rlen;
 882
 883         return copied;
 884 }
 885
 886 static const struct proto_ops llcp_sock_ops = {
 887         .family         = PF_NFC,
 888         .owner          = THIS_MODULE,
 889         .bind           = llcp_sock_bind,
 890         .connect        = llcp_sock_connect,
 891         .release        = llcp_sock_release,
 892         .socketpair     = sock_no_socketpair,
 893         .accept         = llcp_sock_accept,
 894         .getname        = llcp_sock_getname,
 895         .poll           = llcp_sock_poll,
 896         .ioctl          = sock_no_ioctl,
 897         .listen         = llcp_sock_listen,
 898         .shutdown       = sock_no_shutdown,
 899         .setsockopt     = nfc_llcp_setsockopt,
 900         .getsockopt     = nfc_llcp_getsockopt,
 901         .sendmsg        = llcp_sock_sendmsg,
 902         .recvmsg        = llcp_sock_recvmsg,
 903         .mmap           = sock_no_mmap,
 904 };
 905
 906 static const struct proto_ops llcp_rawsock_ops = {
 907         .family         = PF_NFC,
 908         .owner          = THIS_MODULE,
 909         .bind           = llcp_raw_sock_bind,
 910         .connect        = sock_no_connect,
 911         .release        = llcp_sock_release,
 912         .socketpair     = sock_no_socketpair,
 913         .accept         = sock_no_accept,
 914         .getname        = llcp_sock_getname,
 915         .poll           = llcp_sock_poll,
 916         .ioctl          = sock_no_ioctl,
 917         .listen         = sock_no_listen,
 918         .shutdown       = sock_no_shutdown,
 919         .setsockopt     = sock_no_setsockopt,
 920         .getsockopt     = sock_no_getsockopt,
 921         .sendmsg        = sock_no_sendmsg,
 922         .recvmsg        = llcp_sock_recvmsg,
 923         .mmap           = sock_no_mmap,
 924 };
 925
 926 static void llcp_sock_destruct(struct sock *sk)
 927 {
 928         struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
 929
 930         pr_debug("%p\n", sk);
 931
 932         if (sk->sk_state == LLCP_CONNECTED)
 933                 nfc_put_device(llcp_sock->dev);
 934
 935         skb_queue_purge(&sk->sk_receive_queue);
 936
 937         nfc_llcp_sock_free(llcp_sock);
 938
 939         if (!sock_flag(sk, SOCK_DEAD)) {
 940                 pr_err("Freeing alive NFC LLCP socket %p\n", sk);
 941                 return;
 942         }
 943 }
 944
 945 struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
 946 {
 947         struct sock *sk;
 948         struct nfc_llcp_sock *llcp_sock;
 949
 950         sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto);
 951         if (!sk)
 952                 return NULL;
 953
 954         llcp_sock = nfc_llcp_sock(sk);
 955
 956         sock_init_data(sock, sk);
 957         sk->sk_state = LLCP_CLOSED;
 958         sk->sk_protocol = NFC_SOCKPROTO_LLCP;
 959         sk->sk_type = type;
 960         sk->sk_destruct = llcp_sock_destruct;
 961
 962         llcp_sock->ssap = 0;
 963         llcp_sock->dsap = LLCP_SAP_SDP;
 964         llcp_sock->rw = LLCP_MAX_RW + 1;
 965         llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
 966         llcp_sock->send_n = llcp_sock->send_ack_n = 0;
 967         llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
 968         llcp_sock->remote_ready = 1;
 969         llcp_sock->reserved_ssap = LLCP_SAP_MAX;
 970         nfc_llcp_socket_remote_param_init(llcp_sock);
 971         skb_queue_head_init(&llcp_sock->tx_queue);
 972         skb_queue_head_init(&llcp_sock->tx_pending_queue);
 973         INIT_LIST_HEAD(&llcp_sock->accept_queue);
 974
 975         if (sock != NULL)
 976                 sock->state = SS_UNCONNECTED;
 977
 978         return sk;
 979 }
 980
 981 void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
 982 {
 983         kfree(sock->service_name);
 984
 985         skb_queue_purge(&sock->tx_queue);
 986         skb_queue_purge(&sock->tx_pending_queue);
 987
 988         list_del_init(&sock->accept_queue);
 989
 990         sock->parent = NULL;
 991
 992         nfc_llcp_local_put(sock->local);
 993 }
 994
 995 static int llcp_sock_create(struct net *net, struct socket *sock,
 996                             const struct nfc_protocol *nfc_proto)
 997 {
 998         struct sock *sk;
 999
1000         pr_debug("%p\n", sock);
1001
1002         if (sock->type != SOCK_STREAM &&
1003             sock->type != SOCK_DGRAM &&
1004             sock->type != SOCK_RAW)
1005                 return -ESOCKTNOSUPPORT;
1006
1007         if (sock->type == SOCK_RAW)
1008                 sock->ops = &llcp_rawsock_ops;
1009         else
1010                 sock->ops = &llcp_sock_ops;
1011
1012         sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
1013         if (sk == NULL)
1014                 return -ENOMEM;
1015
1016         return 0;
1017 }
1018
1019 static const struct nfc_protocol llcp_nfc_proto = {
1020         .id       = NFC_SOCKPROTO_LLCP,
1021         .proto    = &llcp_sock_proto,
1022         .owner    = THIS_MODULE,
1023         .create   = llcp_sock_create
1024 };
1025
1026 int __init nfc_llcp_sock_init(void)
1027 {
1028         return nfc_proto_register(&llcp_nfc_proto);
1029 }
1030
1031 void nfc_llcp_sock_exit(void)
1032 {
1033         nfc_proto_unregister(&llcp_nfc_proto);
1034 }