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