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(struct file *file, struct socket *sock, 552 poll_table *wait) 553 { 554 struct sock *sk = sock->sk; 555 __poll_t mask = 0; 556 557 pr_debug("%p\n", sk); 558 559 sock_poll_wait(file, sock, wait); 560 561 if (sk->sk_state == LLCP_LISTEN) 562 return llcp_accept_poll(sk); 563 564 if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue)) 565 mask |= EPOLLERR | 566 (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? EPOLLPRI : 0); 567 568 if (!skb_queue_empty(&sk->sk_receive_queue)) 569 mask |= EPOLLIN | EPOLLRDNORM; 570 571 if (sk->sk_state == LLCP_CLOSED) 572 mask |= EPOLLHUP; 573 574 if (sk->sk_shutdown & RCV_SHUTDOWN) 575 mask |= EPOLLRDHUP | EPOLLIN | EPOLLRDNORM; 576 577 if (sk->sk_shutdown == SHUTDOWN_MASK) 578 mask |= EPOLLHUP; 579 580 if (sock_writeable(sk) && sk->sk_state == LLCP_CONNECTED) 581 mask |= EPOLLOUT | EPOLLWRNORM | EPOLLWRBAND; 582 else 583 sk_set_bit(SOCKWQ_ASYNC_NOSPACE, sk); 584 585 pr_debug("mask 0x%x\n", mask); 586 587 return mask; 588 } 589 590 static int llcp_sock_release(struct socket *sock) 591 { 592 struct sock *sk = sock->sk; 593 struct nfc_llcp_local *local; 594 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); 595 int err = 0; 596 597 if (!sk) 598 return 0; 599 600 pr_debug("%p\n", sk); 601 602 local = llcp_sock->local; 603 if (local == NULL) { 604 err = -ENODEV; 605 goto out; 606 } 607 608 lock_sock(sk); 609 610 /* Send a DISC */ 611 if (sk->sk_state == LLCP_CONNECTED) 612 nfc_llcp_send_disconnect(llcp_sock); 613 614 if (sk->sk_state == LLCP_LISTEN) { 615 struct nfc_llcp_sock *lsk, *n; 616 struct sock *accept_sk; 617 618 list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue, 619 accept_queue) { 620 accept_sk = &lsk->sk; 621 lock_sock(accept_sk); 622 623 nfc_llcp_send_disconnect(lsk); 624 nfc_llcp_accept_unlink(accept_sk); 625 626 release_sock(accept_sk); 627 } 628 } 629 630 if (llcp_sock->reserved_ssap < LLCP_SAP_MAX) 631 nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); 632 633 release_sock(sk); 634 635 /* Keep this sock alive and therefore do not remove it from the sockets 636 * list until the DISC PDU has been actually sent. Otherwise we would 637 * reply with DM PDUs before sending the DISC one. 638 */ 639 if (sk->sk_state == LLCP_DISCONNECTING) 640 return err; 641 642 if (sock->type == SOCK_RAW) 643 nfc_llcp_sock_unlink(&local->raw_sockets, sk); 644 else 645 nfc_llcp_sock_unlink(&local->sockets, sk); 646 647 out: 648 sock_orphan(sk); 649 sock_put(sk); 650 651 return err; 652 } 653 654 static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr, 655 int len, int flags) 656 { 657 struct sock *sk = sock->sk; 658 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); 659 struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr; 660 struct nfc_dev *dev; 661 struct nfc_llcp_local *local; 662 int ret = 0; 663 664 pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags); 665 666 if (!addr || len < sizeof(*addr) || addr->sa_family != AF_NFC) 667 return -EINVAL; 668 669 if (addr->service_name_len == 0 && addr->dsap == 0) 670 return -EINVAL; 671 672 pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx, 673 addr->target_idx, addr->nfc_protocol); 674 675 lock_sock(sk); 676 677 if (sk->sk_state == LLCP_CONNECTED) { 678 ret = -EISCONN; 679 goto error; 680 } 681 682 dev = nfc_get_device(addr->dev_idx); 683 if (dev == NULL) { 684 ret = -ENODEV; 685 goto error; 686 } 687 688 local = nfc_llcp_find_local(dev); 689 if (local == NULL) { 690 ret = -ENODEV; 691 goto put_dev; 692 } 693 694 device_lock(&dev->dev); 695 if (dev->dep_link_up == false) { 696 ret = -ENOLINK; 697 device_unlock(&dev->dev); 698 goto put_dev; 699 } 700 device_unlock(&dev->dev); 701 702 if (local->rf_mode == NFC_RF_INITIATOR && 703 addr->target_idx != local->target_idx) { 704 ret = -ENOLINK; 705 goto put_dev; 706 } 707 708 llcp_sock->dev = dev; 709 llcp_sock->local = nfc_llcp_local_get(local); 710 llcp_sock->ssap = nfc_llcp_get_local_ssap(local); 711 if (llcp_sock->ssap == LLCP_SAP_MAX) { 712 ret = -ENOMEM; 713 goto put_dev; 714 } 715 716 llcp_sock->reserved_ssap = llcp_sock->ssap; 717 718 if (addr->service_name_len == 0) 719 llcp_sock->dsap = addr->dsap; 720 else 721 llcp_sock->dsap = LLCP_SAP_SDP; 722 llcp_sock->nfc_protocol = addr->nfc_protocol; 723 llcp_sock->service_name_len = min_t(unsigned int, 724 addr->service_name_len, 725 NFC_LLCP_MAX_SERVICE_NAME); 726 llcp_sock->service_name = kmemdup(addr->service_name, 727 llcp_sock->service_name_len, 728 GFP_KERNEL); 729 730 nfc_llcp_sock_link(&local->connecting_sockets, sk); 731 732 ret = nfc_llcp_send_connect(llcp_sock); 733 if (ret) 734 goto sock_unlink; 735 736 sk->sk_state = LLCP_CONNECTING; 737 738 ret = sock_wait_state(sk, LLCP_CONNECTED, 739 sock_sndtimeo(sk, flags & O_NONBLOCK)); 740 if (ret && ret != -EINPROGRESS) 741 goto sock_unlink; 742 743 release_sock(sk); 744 745 return ret; 746 747 sock_unlink: 748 nfc_llcp_put_ssap(local, llcp_sock->ssap); 749 750 nfc_llcp_sock_unlink(&local->connecting_sockets, sk); 751 752 put_dev: 753 nfc_put_device(dev); 754 755 error: 756 release_sock(sk); 757 return ret; 758 } 759 760 static int llcp_sock_sendmsg(struct socket *sock, struct msghdr *msg, 761 size_t len) 762 { 763 struct sock *sk = sock->sk; 764 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); 765 int ret; 766 767 pr_debug("sock %p sk %p", sock, sk); 768 769 ret = sock_error(sk); 770 if (ret) 771 return ret; 772 773 if (msg->msg_flags & MSG_OOB) 774 return -EOPNOTSUPP; 775 776 lock_sock(sk); 777 778 if (sk->sk_type == SOCK_DGRAM) { 779 DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, addr, 780 msg->msg_name); 781 782 if (msg->msg_namelen < sizeof(*addr)) { 783 release_sock(sk); 784 return -EINVAL; 785 } 786 787 release_sock(sk); 788 789 return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap, 790 msg, len); 791 } 792 793 if (sk->sk_state != LLCP_CONNECTED) { 794 release_sock(sk); 795 return -ENOTCONN; 796 } 797 798 release_sock(sk); 799 800 return nfc_llcp_send_i_frame(llcp_sock, msg, len); 801 } 802 803 static int llcp_sock_recvmsg(struct socket *sock, struct msghdr *msg, 804 size_t len, int flags) 805 { 806 int noblock = flags & MSG_DONTWAIT; 807 struct sock *sk = sock->sk; 808 unsigned int copied, rlen; 809 struct sk_buff *skb, *cskb; 810 int err = 0; 811 812 pr_debug("%p %zu\n", sk, len); 813 814 lock_sock(sk); 815 816 if (sk->sk_state == LLCP_CLOSED && 817 skb_queue_empty(&sk->sk_receive_queue)) { 818 release_sock(sk); 819 return 0; 820 } 821 822 release_sock(sk); 823 824 if (flags & (MSG_OOB)) 825 return -EOPNOTSUPP; 826 827 skb = skb_recv_datagram(sk, flags, noblock, &err); 828 if (!skb) { 829 pr_err("Recv datagram failed state %d %d %d", 830 sk->sk_state, err, sock_error(sk)); 831 832 if (sk->sk_shutdown & RCV_SHUTDOWN) 833 return 0; 834 835 return err; 836 } 837 838 rlen = skb->len; /* real length of skb */ 839 copied = min_t(unsigned int, rlen, len); 840 841 cskb = skb; 842 if (skb_copy_datagram_msg(cskb, 0, msg, copied)) { 843 if (!(flags & MSG_PEEK)) 844 skb_queue_head(&sk->sk_receive_queue, skb); 845 return -EFAULT; 846 } 847 848 sock_recv_timestamp(msg, sk, skb); 849 850 if (sk->sk_type == SOCK_DGRAM && msg->msg_name) { 851 struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb); 852 DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, sockaddr, 853 msg->msg_name); 854 855 msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp); 856 857 pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap); 858 859 memset(sockaddr, 0, sizeof(*sockaddr)); 860 sockaddr->sa_family = AF_NFC; 861 sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP; 862 sockaddr->dsap = ui_cb->dsap; 863 sockaddr->ssap = ui_cb->ssap; 864 } 865 866 /* Mark read part of skb as used */ 867 if (!(flags & MSG_PEEK)) { 868 869 /* SOCK_STREAM: re-queue skb if it contains unreceived data */ 870 if (sk->sk_type == SOCK_STREAM || 871 sk->sk_type == SOCK_DGRAM || 872 sk->sk_type == SOCK_RAW) { 873 skb_pull(skb, copied); 874 if (skb->len) { 875 skb_queue_head(&sk->sk_receive_queue, skb); 876 goto done; 877 } 878 } 879 880 kfree_skb(skb); 881 } 882 883 /* XXX Queue backlogged skbs */ 884 885 done: 886 /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */ 887 if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC)) 888 copied = rlen; 889 890 return copied; 891 } 892 893 static const struct proto_ops llcp_sock_ops = { 894 .family = PF_NFC, 895 .owner = THIS_MODULE, 896 .bind = llcp_sock_bind, 897 .connect = llcp_sock_connect, 898 .release = llcp_sock_release, 899 .socketpair = sock_no_socketpair, 900 .accept = llcp_sock_accept, 901 .getname = llcp_sock_getname, 902 .poll = llcp_sock_poll, 903 .ioctl = sock_no_ioctl, 904 .listen = llcp_sock_listen, 905 .shutdown = sock_no_shutdown, 906 .setsockopt = nfc_llcp_setsockopt, 907 .getsockopt = nfc_llcp_getsockopt, 908 .sendmsg = llcp_sock_sendmsg, 909 .recvmsg = llcp_sock_recvmsg, 910 .mmap = sock_no_mmap, 911 }; 912 913 static const struct proto_ops llcp_rawsock_ops = { 914 .family = PF_NFC, 915 .owner = THIS_MODULE, 916 .bind = llcp_raw_sock_bind, 917 .connect = sock_no_connect, 918 .release = llcp_sock_release, 919 .socketpair = sock_no_socketpair, 920 .accept = sock_no_accept, 921 .getname = llcp_sock_getname, 922 .poll = llcp_sock_poll, 923 .ioctl = sock_no_ioctl, 924 .listen = sock_no_listen, 925 .shutdown = sock_no_shutdown, 926 .setsockopt = sock_no_setsockopt, 927 .getsockopt = sock_no_getsockopt, 928 .sendmsg = sock_no_sendmsg, 929 .recvmsg = llcp_sock_recvmsg, 930 .mmap = sock_no_mmap, 931 }; 932 933 static void llcp_sock_destruct(struct sock *sk) 934 { 935 struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); 936 937 pr_debug("%p\n", sk); 938 939 if (sk->sk_state == LLCP_CONNECTED) 940 nfc_put_device(llcp_sock->dev); 941 942 skb_queue_purge(&sk->sk_receive_queue); 943 944 nfc_llcp_sock_free(llcp_sock); 945 946 if (!sock_flag(sk, SOCK_DEAD)) { 947 pr_err("Freeing alive NFC LLCP socket %p\n", sk); 948 return; 949 } 950 } 951 952 struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp, int kern) 953 { 954 struct sock *sk; 955 struct nfc_llcp_sock *llcp_sock; 956 957 sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto, kern); 958 if (!sk) 959 return NULL; 960 961 llcp_sock = nfc_llcp_sock(sk); 962 963 sock_init_data(sock, sk); 964 sk->sk_state = LLCP_CLOSED; 965 sk->sk_protocol = NFC_SOCKPROTO_LLCP; 966 sk->sk_type = type; 967 sk->sk_destruct = llcp_sock_destruct; 968 969 llcp_sock->ssap = 0; 970 llcp_sock->dsap = LLCP_SAP_SDP; 971 llcp_sock->rw = LLCP_MAX_RW + 1; 972 llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1); 973 llcp_sock->send_n = llcp_sock->send_ack_n = 0; 974 llcp_sock->recv_n = llcp_sock->recv_ack_n = 0; 975 llcp_sock->remote_ready = 1; 976 llcp_sock->reserved_ssap = LLCP_SAP_MAX; 977 nfc_llcp_socket_remote_param_init(llcp_sock); 978 skb_queue_head_init(&llcp_sock->tx_queue); 979 skb_queue_head_init(&llcp_sock->tx_pending_queue); 980 INIT_LIST_HEAD(&llcp_sock->accept_queue); 981 982 if (sock != NULL) 983 sock->state = SS_UNCONNECTED; 984 985 return sk; 986 } 987 988 void nfc_llcp_sock_free(struct nfc_llcp_sock *sock) 989 { 990 kfree(sock->service_name); 991 992 skb_queue_purge(&sock->tx_queue); 993 skb_queue_purge(&sock->tx_pending_queue); 994 995 list_del_init(&sock->accept_queue); 996 997 sock->parent = NULL; 998 999 nfc_llcp_local_put(sock->local); 1000 } 1001 1002 static int llcp_sock_create(struct net *net, struct socket *sock, 1003 const struct nfc_protocol *nfc_proto, int kern) 1004 { 1005 struct sock *sk; 1006 1007 pr_debug("%p\n", sock); 1008 1009 if (sock->type != SOCK_STREAM && 1010 sock->type != SOCK_DGRAM && 1011 sock->type != SOCK_RAW) 1012 return -ESOCKTNOSUPPORT; 1013 1014 if (sock->type == SOCK_RAW) 1015 sock->ops = &llcp_rawsock_ops; 1016 else 1017 sock->ops = &llcp_sock_ops; 1018 1019 sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC, kern); 1020 if (sk == NULL) 1021 return -ENOMEM; 1022 1023 return 0; 1024 } 1025 1026 static const struct nfc_protocol llcp_nfc_proto = { 1027 .id = NFC_SOCKPROTO_LLCP, 1028 .proto = &llcp_sock_proto, 1029 .owner = THIS_MODULE, 1030 .create = llcp_sock_create 1031 }; 1032 1033 int __init nfc_llcp_sock_init(void) 1034 { 1035 return nfc_proto_register(&llcp_nfc_proto); 1036 } 1037 1038 void nfc_llcp_sock_exit(void) 1039 { 1040 nfc_proto_unregister(&llcp_nfc_proto); 1041 } 1042