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