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