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