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